Commit 1d902a7e authored by Dominique Piche's avatar Dominique Piche

more intuitive scintillators position definition

parent 4774f8e1
......@@ -253,6 +253,7 @@ class MecaRobot:
step_2 = -step_2
# Writing the movement in the form [x, y, z, alpha, beta, gamma] used by the robot.
lin_pose = list(step_1 * direction_vec_1) + [0, 0, 0]
self.run('MoveLinRelWRF', lin_pose)
......@@ -289,6 +290,7 @@ class MecaRobot:
if total_distance_along_direction_vec_1/step_1 % 2 != 0:
step_2 = -step_2
# Writing the movement in the form [x, y, z, alpha, beta, gamma] used by the robot.
lin_pose = list(step_3 * direction_vec_3) + [0, 0, 0]
self.run('MoveLinRelWRF', lin_pose)
......
......@@ -12,7 +12,7 @@ class MecaRobotForDosimetry(MecaRobot):
port: int = 10000,
simulation_mode: bool = False,
enable_log: bool = False,
scintillators_positions: np.ndarray = np.array([0]),
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):
......@@ -23,12 +23,12 @@ class MecaRobotForDosimetry(MecaRobot):
:param simulation_mode: enables simulation mode, in which the robot does not move but it is simulated
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 z-position relative to
: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.
: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
the robot never touches the water during sequences.
the robot never touches the water during linear, square or cube sequences.
:param scintillators_signal_queue : if specified, the robot will track the signal from this queue in
the scintilators_signal.txt file.
......@@ -41,7 +41,7 @@ class MecaRobotForDosimetry(MecaRobot):
simulation_mode = simulation_mode,
enable_log = enable_log)
self.scintillators_relative_positions = scintillators_positions - scintillators_positions[0]
self.scintillators_relative_positions = scintillators_positions - scintillators_positions[-1]
self.number_of_scintillators = len(scintillators_positions)
self.source_position = source_position
......@@ -51,12 +51,13 @@ class MecaRobotForDosimetry(MecaRobot):
# Setting the parameters used to make sure that the robot does go below the water level.
self.water_level = water_level
self.water_level_safety_buffer = 40
self.water_level_safety_buffer = 40
self.last_robot_end_height = 250
# Setting the last scintillator as the tool reference frame (TRF).
self.TRF_z_offset = scintillators_positions[-1]
self.run('SetTRF', [scintillators_positions[-1], 0, 3.5, 0, -90, 0])
last_scintillator_position = scintillators_positions[-1]
self.TRF_z_offset = last_scintillator_position[-1]
self.run('SetTRF', [-last_scintillator_position[2], last_scintillator_position[1], last_scintillator_position[0], 0, -90, 0])
def start_tracking(self):
"""
......@@ -152,25 +153,20 @@ class MecaRobotForDosimetry(MecaRobot):
"""
# We get the pose of the robot, which is is the form [x, y, z, alpha, beta, gamma], where alpha beta and
# gamma are euler angles.
# gamma are euler angles. This returns the position of the lowest scintillator, since it is the TRF.
pose = self.get_TRF_pose()
# In case the orientation of the mPSD holder has changed, we rotate the tool reference frame so that
# it is in the same orientation as the world reference frame.
# We know the scintillators relative positions in the original orientation. In case the orientation of the
# mPSD holder has changed, we rotate the tool reference frame so that it is in the same orientation as the
# world reference frame.
TRF_position = pose[0:3]
TRF_orientation = pose[3:6]
rotation_matrix = R.from_euler('xyz', -TRF_orientation, degrees=True)
rotated_position = np.matmul(rotation_matrix.as_dcm(), np.transpose(TRF_position))
# In the world reference frame, the scintillators are along the z-axis. We calculate their position.
# TODO : vectorize this.
scintillators_positions = np.zeros([self.number_of_scintillators, 3])
for i in range(self.number_of_scintillators):
scintillators_positions[i][0] = rotated_position[0]
scintillators_positions[i][1] = rotated_position[1]
scintillators_positions[i][2] = rotated_position[2] + self.scintillators_relative_positions[i]
scintillators_positions = rotated_position + self.scintillators_relative_positions
# We rotate back the positions in their originial orientation.
# We rotate back the positions in their actual orientation.
inv_rotation_matrix = R.from_euler('xyz', TRF_orientation, degrees=True)
scintillators_positions = np.transpose(np.matmul(inv_rotation_matrix.as_dcm(), np.transpose(scintillators_positions)))
......@@ -184,10 +180,7 @@ class MecaRobotForDosimetry(MecaRobot):
Returns an array of the position of each scintillators in relation to the source
"""
scintillators_positions = self.get_scintillators_positions()
scintillators_positions_relative_to_source = np.zeros([self.number_of_scintillators, 3])
# TODO : vectorize this.
for i in range(self.number_of_scintillators):
scintillators_positions_relative_to_source[i] = scintillators_positions[i]-self.source_position
scintillators_positions_relative_to_source = scintillators_positions-self.source_position
return scintillators_positions_relative_to_source
......
......@@ -222,7 +222,7 @@ Parameters:
Enables the log, in which case every communication with the robot is written in the *log.txt* file.
- **scintillators_positions : *np.ndarray***
Array of each scintillator's z-position (in mm) relative to the end of the robot (flange reference frame), starting with the closest. For instance, for the 3 point PSD depicted [here](https://drive.google.com/file/d/1_yfqkUp4w1Qwi7k_yBzmAPf0obf2jAUw/view?usp=sharing), this parameter would be np.array([230, 240, 250]).
Array of each scintillator's position in the form (x, y, z) (in mm) relative to the end of the robot (flange reference frame), starting with the closest. The x, y and z axis are defined as [such](https://photos.app.goo.gl/VJsigoDeULbp3z447). For instance, with the scintillators setup depicted [here](https://photos.app.goo.gl/hMZSctgg5Qzhubjn6), this parameter would be *np.array([[3, 0, -10], [3, 0, -15], [3, 0 -20]])*.
- **source_position : *np.ndarray***
Position (x, y, z) of the source (in mm) in relation to the base of the robot (world reference frame).
......
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