Commit 7a268d79 authored by Dominique Piche's avatar Dominique Piche

add sequences for aug 13 measurements

parent 1d902a7e
import numpy as np
import time
from scipy.spatial.transform import Rotation as R
import queue
import multiprocessing
import os
from MecaRobot.meca_robot import MecaRobot
......@@ -15,7 +15,7 @@ class MecaRobotForDosimetry(MecaRobot):
scintillators_positions: np.ndarray = np.array([0,0,0]),
source_position: np.ndarray = np.array([0,0,0]),
water_level: float = -np.inf,
scintillators_signal_queue: queue.Queue = None):
scintillators_signal_value: multiprocessing.Array = None):
"""
:param ip : robot ip
......@@ -24,7 +24,8 @@ class MecaRobotForDosimetry(MecaRobot):
on the web interace.
:param enable_log : writes in the log.txt file every communication with the robot.
:param scintillators_positions : array of each scintillator's position (x,y,z) relative to
the end of the robot (flange reference frame), starting with the closest.
the end of the robot (flange reference frame), starting with the closest. This x, y and z axes
are defined as such https://photos.app.goo.gl/VJsigoDeULbp3z447.
:param source_position : position of the source (x,y,z) relative to the base of the robot (world
reference frame).
:param water_level : height of the water in relation to the WRF. If specified, this will make sure
......@@ -45,7 +46,7 @@ class MecaRobotForDosimetry(MecaRobot):
self.number_of_scintillators = len(scintillators_positions)
self.source_position = source_position
self.scintillators_signal_queue = scintillators_signal_queue
self.scintillators_signal_value = scintillators_signal_value
self.source_offset = np.array([0, 0, 0])
......@@ -93,7 +94,7 @@ class MecaRobotForDosimetry(MecaRobot):
self.scintillators_distance_to_source_files.append(open(file_path, 'w+'))
# Creating scintillators' signal file
if self.scintillators_signal_queue is not None:
if self.scintillators_signal_value is not None:
self.scintillators_signal_file = open(os.path.join(current_path, 'Data/scintillators_signal.txt'), 'w+')
self.start_time = time.time()
......@@ -116,7 +117,7 @@ class MecaRobotForDosimetry(MecaRobot):
scintillator_distance_file.write(str(scintillators_distance_to_source[i]) + '\n')
# Writing the scintillators' signal in the signal file
if self.scintillators_signal_queue is not None:
if self.scintillators_signal_value is not None:
scintillators_signal = self.get_scintillators_signal()
self.scintillators_signal_file.write(np.array2string(scintillators_signal, separator=',')[1:-1] + '\n')
......@@ -135,7 +136,7 @@ class MecaRobotForDosimetry(MecaRobot):
distance_to_source_file.close()
# Closing signals files
if self.scintillators_signal_queue is not None:
if self.scintillators_signal is not None:
self.scintillators_signal_file.close()
def get_scintillators_signal(self) -> np.ndarray:
......@@ -143,7 +144,7 @@ class MecaRobotForDosimetry(MecaRobot):
Reads the scintillators signal from the signal queue.
"""
time.sleep(0.1)
scintillators_signal = self.scintillators_signal_queue.get()
scintillators_signal = np.array(self.scintillators_signal_value[:])
return scintillators_signal
def get_scintillators_positions(self) -> np.ndarray:
......@@ -187,7 +188,7 @@ class MecaRobotForDosimetry(MecaRobot):
def find_source_position(self, distance_to_source=20):
"""
Automatically finds the source's position by moving along every axis near the source until we get the maximum signal.
This assumes that an estimate of the source position as already been set, as it will move around this poin tto find
This assumes that an estimate of the source position as already been set, as it will move around this point to find
the maximum.
:param distance_to_source: how far from the source in mm will the the dosimeter be placed. If the user is unsure of the
......@@ -197,7 +198,6 @@ class MecaRobotForDosimetry(MecaRobot):
x_axis_signal = []
y_axis_signal = []
z_axis_signal = []
# Moving the robot near the source
self.move_position_relative_to_source(np.array([-5,50,0]))
......@@ -222,35 +222,22 @@ class MecaRobotForDosimetry(MecaRobot):
self.wait_for('3012', 'Did not receive EOB.')
y_axis_signal.append(self.get_scintillators_signal())
# Moving the robot near the source
self.move_position_relative_to_source(np.array([-50,-5,0]))
self.move_position_relative_to_source(np.array([0,50,-10]))
self.move_position_relative_to_source(np.array([0,distance_to_source,-10]))
self.wait_for('3012', 'Did not receive EOB.')
# Moving along the z-axis
for i in range(200):
self.run('MoveLinRelTRF', [0,0,0.1,0,0,0])
self.wait_for('3012', 'Did not receive EOB.')
z_axis_signal.append(self.get_scintillators_signal())
self.move_position_relative_to_source(np.array([-50,0,0]))
self.move_position_relative_to_source(np.array([0,50,-10]))
self.zero_joints()
self.wait_for('3012', 'Did not receive EOB.')
x_axis_signal, y_axis_signal, z_axis_signal = np.array(x_axis_signal), np.array(y_axis_signal), np.array(z_axis_signal)
x_axis_signal, y_axis_signal= np.array(x_axis_signal), np.array(y_axis_signal)
x_max = np.argmax(x_axis_signal[:,1])*0.1-4.9
y_max = np.argmax(y_axis_signal[:,1])*0.1-4.9
z_max = np.argmax(z_axis_signal[:,1])*0.1-9.9
x_max = np.argmax(x_axis_signal[:,0])*0.1-4.9
y_max = np.argmax(y_axis_signal[:,0])*0.1-4.9
np.savetxt('Data/x_calibration_data.txt', x_axis_signal, delimiter = ',')
np.savetxt('Data/y_calibration_data.txt', y_axis_signal, delimiter = ',')
np.savetxt('Data/z_calibration_data.txt', z_axis_signal, delimiter = ',')
self.source_offset = np.array([x_max, y_max, z_max])
self.source_offset = np.array([x_max, y_max, 0])
self.source_position = self.source_position + self.source_offset
#self.source_position = self.source_position + self.source_offset
def linear_sequence(self, direction_vector: np.ndarray, total_distance: float, step: float, delay: float=0, do_clear_buffer : bool=True):
"""
......@@ -279,7 +266,8 @@ class MecaRobotForDosimetry(MecaRobot):
if self.is_tracking:
self.wait_for('3004', 'Did not receive EOM')
self._track_data()
for i in range(10):
self._track_data()
# Cheking if the next step will send the robot lower than the water level
if (self.last_robot_end_height + step * direction_vector[2]) <= (self.water_level + self.water_level_safety_buffer):
......
import sys
import numpy as np
import time
from MecaRobot.meca_robot_for_dosimetry import MecaRobotForDosimetry
from NI_DAQ.DAQ_Volt_module import *
robot_ip = "192.168.0.100"
robot_port = 10000
simulation_mode = True
enable_log = True
tool_length = 400
source_depth = 170
scintillators_positions = np.array([[5.55, 0, -tool_length+67], [5.55, 0, -tool_length+42], [5.55, 0, -tool_length+13]])
source_position = np.array([264.522,-0.400,128.342])+np.array([5.55+10, 0, 270-tool_length])+np.array([0,0,-source_depth])
robot = MecaRobotForDosimetry(ip = robot_ip,
port = robot_port,
simulation_mode = simulation_mode,
enable_log = enable_log,
scintillators_positions = scintillators_positions,
source_position = source_position,
scintillators_signal_queue=signal_queue)
# Calibrating the source position
robot.run('SetConf', [1,1,-1])
robot.find_source_position(distance_to_source=10)
robot.start_tracking()
robot.move_position_relative_to_source(np.array([-50,0,-64]))
robot.move_position_relative_to_source(np.array([-10,0,-64]))
robot.linear_sequence(np.array([0,0,1]), 74, 0.2)
time.sleep(2)
robot.stop_tracking()
robot.deactivate()
import sys
import numpy as np
import time
from MecaRobot.meca_robot_for_dosimetry import MecaRobotForDosimetry
robot_ip = "192.168.0.100"
robot_port = 10000
simulation_mode = True
enable_log = True
source_position = np.array([277.617,18.972,290.005])+np.array([0,0,-100])+np.array([-0.214,1.45,1.945])
robot = MecaRobotForDosimetry(ip = robot_ip,
port = robot_port,
simulation_mode = simulation_mode,
enable_log = enable_log,
source_position = source_position)
robot.run('SetTRF', [0,0,0,0,-90,0])
robot.start_tracking()
robot.move_position_relative_to_source(np.array([-5,50,0]))
robot.wait_for('3004', 'Did not receive EOM')
robot.move_position_relative_to_source(np.array([-5,10,0]))
robot.wait_for('3004', 'Did not receive EOM')
robot.run('Delay', 5)
robot.linear_sequence(np.array([1,0,0]), 5, 0.2, 0.5)
robot.linear_sequence(np.array([1,0,0]), 5, 0.2, 0.5)
robot.run('Delay', 5)
robot.move_position_relative_to_source(np.array([-10,50,0]))
robot.wait_for('3004', 'Did not receive EOM')
robot.move_position_relative_to_source(np.array([-50,-5,0]))
robot.wait_for('3004', 'Did not receive EOM')
robot.move_position_relative_to_source(np.array([-10,-5,0]))
robot.wait_for('3004', 'Did not receive EOM')
robot.run('Delay', 5)
robot.linear_sequence(np.array([0,1,0]), 5, 0.2, 0.5)
robot.linear_sequence(np.array([0,1,0]), 5, 0.2, 0.5)
robot.run('Delay', 5)
robot.move_position_relative_to_source(np.array([-50,-5,0]))
robot.wait_for('3004', 'Did not receive EOM')
robot.move_position_relative_to_source(np.array([0,50,-5]))
robot.wait_for('3004', 'Did not receive EOM')
robot.move_position_relative_to_source(np.array([0,10,-5]))
robot.wait_for('3004', 'Did not receive EOM')
robot.run('Delay', 5)
robot.linear_sequence(np.array([0,0,1]), 5, 0.2, 0.5)
robot.linear_sequence(np.array([0,0,1]), 5, 0.2, 0.5)
robot.run('Delay', 5)
robot.move_position_relative_to_source(np.array([0,50,-10]))
robot.wait_for('3012', 'Did not receive EOB')
robot.stop_tracking()
time.sleep(5)
robot.deactivate()
\ No newline at end of file
import sys
import numpy as np
import queue
import threading
from MecaRobot.meca_robot_for_dosimetry import MecaRobotForDosimetry
from NI_DAQ.DAQ_Volt_module import *
# Moves the robot in straight lines of 150 mm towards the source in a water tank.
def robot_thread(signal_queue):
robot_ip = "192.168.0.100"
robot_port = 10000
simulation_mode = True # Only run in simulation, the robot is at full speed!
enable_log = True
tool_length = 399.6
scintillators_positions = np.array([[5.55, 0, -tool_length+67], [5.55, 0, -tool_length+42], [5.55, 0, -tool_length+13]])
source_position = np.array([264.522,-0.400,128.342])+np.array([5.55+10, 0, 270-399.6])+np.array([0,0,-170])
robot = MecaRobotForDosimetry(ip = robot_ip,
port = robot_port,
simulation_mode = simulation_mode,
enable_log = enable_log,
scintillators_positions = scintillators_positions,
source_position = source_position,
scintillators_signal_queue=signal_queue)
# Calibrating the source position
robot.run('SetConf', [1,1,-1])
robot.run('SetCartLinVel', 500)
robot.run('SetJointVel', 100)
# Movements
height_source_start = -64
number_of_straight_lines = 74
lines_resolution = 1
for i in range(number_of_straight_lines):
robot.move_position_relative_to_source(np.array([0,50,lines_resolution*i+height_source_start]))
robot.move_position_relative_to_source(np.array([0,30,lines_resolution*i+height_source_start]))
robot.run('MoveLinRelWRF', [0,-10,0,0,0,0])
robot.move_position_relative_to_source(np.array([0,50,lines_resolution*i+height_source_start]))
for i in range(number_of_straight_lines):
robot.move_position_relative_to_source(np.array([-100,0,lines_resolution*i+height_source_start]))
robot.run('MoveLinRelWRF', [95,0,0,0,0,0])
for i in range(number_of_straight_lines):
robot.move_position_relative_to_source(np.array([0,-50,lines_resolution*i+height_source_start]))
robot.move_position_relative_to_source(np.array([0,-30,lines_resolution*i+height_source_start]))
robot.run('MoveLinRelWRF', [0,10,0,0,0,0])
robot.move_position_relative_to_source(np.array([0,-50,lines_resolution*i+height_source_start]))
robot.run('Delay', 1)
robot.run('MoveLinRelTRF', [-75,0,0,0,0,0])
# Deactivating robot
robot.deactivate()
print('Main program done')
sys.stdout.flush()
with open('log.txt') as log:
content = log.read()
if content.find('singularity') != -1:
print('A singularity was encountered during simulation.')
if __name__ == '__main__':
signal_queue = queue.LifoQueue()
t = threading.Thread(target=robot_thread, args=(signal_queue,))
app = QtWidgets.QApplication([])
window = DAQ_MainWin(app, signal_queue)
t.start()
window.show()
window.startAcquisition()
app.exec_()
\ No newline at end of file
import sys
import numpy as np
import time
from MecaRobot.meca_robot_for_dosimetry import MecaRobotForDosimetry
from NI_DAQ.DAQ_Volt_module import *
# Moves the robot in straight lines of 150 mm towards the source in a water tank.
robot_ip = "192.168.0.100"
robot_port = 10000
simulation_mode = True
enable_log = True
tool_length = 400
scintillators_positions = np.array([[5.55, 0, -tool_length+67], [5.55, 0, -tool_length+42], [5.55, 0, -tool_length+13]])
source_position = np.array([264.522,-0.400,128.342])+np.array([5.55+10, 0, 270-399.6])+np.array([0,0,-170])
robot = MecaRobotForDosimetry(ip = robot_ip,
port = robot_port,
simulation_mode = simulation_mode,
enable_log = enable_log,
scintillators_positions = scintillators_positions,
source_position = source_position,
scintillators_signal_value=val)
# Calibrating the source position
robot.run('SetConf', [1,1,-1])
# Movements
robot.start_tracking()
height_source_start = -64
number_of_straight_lines = 3
lines_resolution = 1
robot.move_position_relative_to_source(np.array([0,50,height_source_start]))
robot.wait_for('3004', 'Did not receive EOM.')
robot.move_position_relative_to_source(np.array([0,20,height_source_start]))
robot.wait_for('3004', 'Did not receive EOM.')
robot.linear_sequence(direction_vector=np.array([0, -1, 0]), total_distance=10, step=0.5)
robot.move_position_relative_to_source(np.array([0,50,height_source_start]))
for i in range(number_of_straight_lines):
robot.move_position_relative_to_source(np.array([-100,0,lines_resolution*i+height_source_start]))
robot.wait_for('3004', 'Did not receive EOM')
robot.linear_sequence(direction_vector=np.array([1,0,0]), total_distance=60, step=5)
robot.linear_sequence(direction_vector=np.array([1,0,0]), total_distance=20, step=1, do_clear_buffer=False)
robot.linear_sequence(direction_vector=np.array([1,0,0]), total_distance=10, step=0.5, do_clear_buffer=False)
robot.linear_sequence(direction_vector=np.array([1,0,0]), total_distance=5, step=0.1, do_clear_buffer=False)
robot.move_position_relative_to_source(np.array([-50,0,height_source_start]))
robot.wait_for('3004', 'Did not receive EOM.')
robot.move_position_relative_to_source(np.array([0,-20,height_source_start]))
robot.wait_for('3004', 'Did not receive EOM.')
robot.linear_sequence(direction_vector=np.array([0, 1, 0]), total_distance=10, step=0.5)
robot.move_position_relative_to_source(np.array([0,-50, height_source_start]))
robot.stop_tracking()
robot.run('Delay', 1)
robot.run('MoveLinRelTRF', [-75,0,0,0,0,0])
# Deactivating robot
robot.deactivate()
print('Main program done')
sys.stdout.flush()
\ No newline at end of file
import sys
import numpy as np
import time
from threading import Timer
from MecaRobot.meca_robot_for_dosimetry import MecaRobotForDosimetry
def stop_robot():
robot.stop_tracking()
time.sleep(5)
robot.deactivate()
t = Timer(500, stop_robot)
t.start()
robot_ip = "192.168.0.100"
robot_port = 10000
simulation_mode = True
enable_log = True
source_position = np.array([277.617,18.972,290.005])+np.array([0,0,-100])+np.array([-0.214,1.45,1.945])+np.array([-0.52,-0.62,0.57])
water_level = -8.415
polar_angles_list = np.array([160, 135, 90, 45, 20]) * np.pi / 180
dwell_time = 0.6
robot = MecaRobotForDosimetry(ip = robot_ip,
port = robot_port,
simulation_mode = simulation_mode,
enable_log = enable_log,
source_position = source_position,
water_level = water_level)
robot.run('SetTRF', [0,0,0,0,-90,0])
robot.start_tracking()
robot.move_position_relative_to_source(np.array([0,50,0]))
robot.wait_for('3004', 'Did not receive EOM.')
robot.move_position_relative_to_source(np.array([0,30,0]))
robot.wait_for('3004', 'Did not receive EOM.')
robot.run('Delay', 5)
robot.linear_sequence(direction_vector=np.array([0, -1, 0]), total_distance=15, step=1, delay=dwell_time)
robot.linear_sequence(direction_vector=np.array([0, -1, 0]), total_distance=7, step=0.5, delay=dwell_time, do_clear_buffer = False)
robot.run('Delay', 5)
robot.move_position_relative_to_source(np.array([-10,50,0]))
robot.wait_for('3004', 'Did not receive EOM.')
robot.move_position_relative_to_source(np.array([-50,0,0]))
robot.wait_for('3004', 'Did not receive EOM.')
robot.move_position_relative_to_source(np.array([-35,0,0]))
robot.wait_for('3004', 'Did not receive EOM.')
robot.run('Delay', 5)
for angle in polar_angles_list:
robot.move_position_relative_to_source(np.array([-4.9, 0, 0]))
robot.wait_for('3004', 'Did not receive EOM.')
direction_vector = np.array([-np.sin(angle), 0, np.cos(angle)])
robot.linear_sequence(direction_vector=direction_vector, total_distance=10, step=0.5, delay=dwell_time)
robot.linear_sequence(direction_vector=direction_vector, total_distance=60, step=1, delay=dwell_time, do_clear_buffer=False)
robot.run('Delay', 5)
robot.move_position_relative_to_source(np.array([-50,0,0]))
robot.wait_for('3004', 'Did not receive EOM.')
robot.move_position_relative_to_source(np.array([0,-30,0]))
robot.wait_for('3004', 'Did not receive EOM.')
robot.run('Delay', 5)
robot.linear_sequence(direction_vector=np.array([0, 1, 0]), total_distance=15, step=1, delay=dwell_time)
robot.linear_sequence(direction_vector=np.array([0, 1, 0]), total_distance=7, step=0.5, delay=dwell_time, do_clear_buffer = False)
robot.run('Delay', 5)
robot.move_position_relative_to_source(np.array([0, -50, 0]))
robot.move_position_relative_to_source(np.array([-50, 0, 0]))
robot.stop_tracking()
time.sleep(5)
robot.deactivate()
\ No newline at end of file
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment