Commit 4a4d48e6 authored by Dominique Piche's avatar Dominique Piche

add water level safety check

parent 88348f5a
......@@ -8,12 +8,13 @@ from NI_DAQ.DAQ_Volt_Tracking_module import DAQ_Volt_Tracking
class MecaRobotForDosimetry(MecaRobot):
def __init__(self,
ip: str,
port: int,
ip: str = "192.168.0.100",
port: int = 10000,
simulation_mode: bool = False,
enable_log: bool = False,
scintillators_positions: np.ndarray = np.array([0]),
source_position: np.ndarray = np.array([0,0,0]),
water_level: float = -np.inf,
measure_with_nidaq: bool = False,
daq_path_number: int = None,
analog_input_channels: tuple = None,
......@@ -29,6 +30,8 @@ class MecaRobotForDosimetry(MecaRobot):
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.
:param measure_with_nidaq : enables tracking with NI data acquisition board
The next parameters are used when tracking with a NI data acquisition board.
:param : daq_path_number : number of the daq board path position (i.e. 1 for Dev1, 2 for Dev2)
......@@ -64,7 +67,13 @@ class MecaRobotForDosimetry(MecaRobot):
self.source_offset = np.array([0, 0, 0])
# 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.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])
def start_tracking(self):
......@@ -179,6 +188,7 @@ class MecaRobotForDosimetry(MecaRobot):
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]
......@@ -187,9 +197,12 @@ class MecaRobotForDosimetry(MecaRobot):
# We rotate back the positions in their originial orientation.
inv_rotation_matrix = R.from_euler('xyz', TRF_orientation, degrees=True)
scintillators_positions = np.matmul(inv_rotation_matrix.as_dcm(), np.transpose(scintillators_positions))
scintillators_positions = np.transpose(np.matmul(inv_rotation_matrix.as_dcm(), np.transpose(scintillators_positions)))
# Updating the heigth of the robot end, which is used to make sure the robot does not go below water level.
self.last_robot_end_height = scintillators_positions[0][2] + self.TRF_z_offset
return np.transpose(scintillators_positions)
return scintillators_positions
def get_scintillators_positions_relative_to_source(self) -> np.ndarray:
"""
......@@ -197,6 +210,7 @@ class MecaRobotForDosimetry(MecaRobot):
"""
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
......@@ -276,6 +290,44 @@ class MecaRobotForDosimetry(MecaRobot):
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):
"""
Moves the tool reference frame linearly along an axis by steps. In its default position,
the robot is in a singularity, which means it has to move elsewhere using the MovePose or
Movejoints command prior to a linear sequence.
The program waits for an end of movement response from the robot before tracking the data.
The buffer is cleared to ensure the EOM responses are not from earlier movements. This step
can be skipped to save 2 seconds by setting do_clear_buffer to False.
"""
if do_clear_buffer and self.is_tracking:
self.clear_buffer()
direction_vector = direction_vector / np.linalg.norm(direction_vector)
for i in range(abs(int(total_distance/step))):
# Cheking if the next step will send the robot lower than the water level
print(self.last_robot_end_height, step * direction_vector[2], self.water_level, self.water_level_safety_buffer)
if (self.last_robot_end_height + step * direction_vector[2]) <= (self.water_level + self.water_level_safety_buffer):
if self.enable_log:
with open('log.txt', 'a+') as log:
log.write('The robot is getting too close to the water level, stopping the sequence.\n')
break
# Writing the movement in the form [x, y, z, alpha, beta, gamma] used by the robot.
lin_pose = list(step * direction_vector) + [0, 0, 0]
self.run('MoveLinRelWRF', lin_pose)
if delay != 0:
self.run('Delay', delay)
if self.is_tracking:
self.wait_for('3004', 'Did not receive EOM')
self._track_data()
def move_position_relative_to_source(self, position: np.ndarray):
"""
Moves the lowest scintillator (TRF) to a given position in the form (x,y,z) relative to the source position.
......
......@@ -90,6 +90,7 @@ class DAQ_Volt_Tracking(threading.Thread):
self.last_read_val = val
try:
self.values_file.write(str(read_time-self.start_time) + ', ')
self.values_file.write(np.array2string(val, separator=',')[1:-1] + '\n')
except Exception as e:
print(e)
......
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