Download as
Browse Code »

Commit c3a918a16d36457b9c0f67f84f75dfcbd2af55c6

Authored by Victor Hugo Souza
Committed by GitHub
2 parents 72952604 be0caa51
Exists in master

Merge pull request #396 from rmatsuda/define_max_robot_working_space 

Robot must work only inside working space
Showing 3 changed files with 57 additions and 44 deletions   Show diff stats
invesalius/constants.py
  Diff comments   View file @c3a918a
... ... @@ -853,6 +853,8 @@ PULSE_DURATION_IN_MILLISECONDS = 0.2
853 853 ROBOT_ElFIN_IP = ['143.107.220.251', '169.254.153.251', '127.0.0.1']
854 854 ROBOT_ElFIN_PORT = 10003
855 855 ROBOT_MOTIONS = {"normal": 0, "linear out": 1, "arc": 2}
856   -ROBOT_HEAD_VELOCITY_THRESHOLD = 10
857   -ROBOT_ARC_THRESHOLD_DISTANCE = 100
  856 +ROBOT_HEAD_VELOCITY_THRESHOLD = 10 #mm/s
  857 +ROBOT_ARC_THRESHOLD_DISTANCE = 100 #mm
858 858 ROBOT_VERSOR_SCALE_FACTOR = 70
  859 +#Robot Working Space is defined as 800mm in Elfin manual. For safety, the value is reduced by 5%.
  860 +ROBOT_WORKING_SPACE = 760 #mm
... ...
invesalius/data/elfin_processing.py
  Diff comments   View file @c3a918a
... ... @@ -197,11 +197,17 @@ class TrackerProcessing:
197 197 """
198 198 Estimates the Euclidean distance between the actual position and the target
199 199 """
200   - square_sum = (coord_inv[0]-actual_point[0]) ** 2 +\
201   - (coord_inv[1]-actual_point[1]) ** 2 +\
202   - (coord_inv[2]-actual_point[2]) ** 2
203   - correction_distance_compensation = pow(square_sum, 0.5)
  200 + correction_distance_compensation = np.sqrt((coord_inv[0]-actual_point[0]) ** 2 +
  201 + (coord_inv[1]-actual_point[1]) ** 2 +
  202 + (coord_inv[2]-actual_point[2]) ** 2)
204 203  
205 204 return correction_distance_compensation
206 205  
  206 + def estimate_robot_target_length(self, robot_target):
  207 + """
  208 + Estimates the length of the 3D vector of the robot target
  209 + """
  210 + robot_target_length = np.sqrt(robot_target[0] ** 2 + robot_target[1] ** 2 + robot_target[2] ** 2)
  211 +
  212 + return robot_target_length
207 213  
... ...
invesalius/navigation/robot.py
  Diff comments   View file @c3a918a
... ... @@ -194,47 +194,52 @@ class ControlRobot(threading.Thread):
194 194 The last step is to make a linear move until the target (goes to the inner sphere)
195 195  
196 196 """
197   - if distance_target < const.ROBOT_ARC_THRESHOLD_DISTANCE and not self.arc_motion_flag:
198   - self.trck_init_robot.SendCoordinates(new_robot_coordinates, const.ROBOT_MOTIONS["normal"])
199   -
200   - elif distance_target >= const.ROBOT_ARC_THRESHOLD_DISTANCE or self.arc_motion_flag:
201   - actual_point = current_robot_coordinates
202   - if not self.arc_motion_flag:
203   - head_center_coordinates = self.process_tracker.estimate_head_center(self.tracker,
204   - current_head_filtered).tolist()
205   -
206   - self.target_linear_out, self.target_arc = self.process_tracker.compute_arc_motion(current_robot_coordinates, head_center_coordinates,
207   - new_robot_coordinates)
208   - self.arc_motion_flag = True
209   - self.arc_motion_step_flag = const.ROBOT_MOTIONS["linear out"]
210   -
211   - if self.arc_motion_flag and self.arc_motion_step_flag == const.ROBOT_MOTIONS["linear out"]:
212   - coord = self.target_linear_out
213   - if np.allclose(np.array(actual_point), np.array(self.target_linear_out), 0, 1):
214   - self.arc_motion_step_flag = const.ROBOT_MOTIONS["arc"]
215   - coord = self.target_arc
216   -
217   - elif self.arc_motion_flag and self.arc_motion_step_flag == const.ROBOT_MOTIONS["arc"]:
218   - head_center_coordinates = self.process_tracker.estimate_head_center(self.tracker,
219   - current_head_filtered).tolist()
220   -
221   - _, new_target_arc = self.process_tracker.compute_arc_motion(current_robot_coordinates, head_center_coordinates,
222   - new_robot_coordinates)
223   - if np.allclose(np.array(new_target_arc[3:-1]), np.array(self.target_arc[3:-1]), 0, 1):
224   - None
225   - else:
226   - if self.process_tracker.correction_distance_calculation_target(new_robot_coordinates, current_robot_coordinates) >= \
227   - const.ROBOT_ARC_THRESHOLD_DISTANCE*0.8:
228   - self.target_arc = new_target_arc
  197 + #Check if the target is inside the working space
  198 + if self.process_tracker.estimate_robot_target_length(new_robot_coordinates) < const.ROBOT_WORKING_SPACE:
  199 + #Check the target distance to define the motion mode
  200 + if distance_target < const.ROBOT_ARC_THRESHOLD_DISTANCE and not self.arc_motion_flag:
  201 + self.trck_init_robot.SendCoordinates(new_robot_coordinates, const.ROBOT_MOTIONS["normal"])
  202 +
  203 + elif distance_target >= const.ROBOT_ARC_THRESHOLD_DISTANCE or self.arc_motion_flag:
  204 + actual_point = current_robot_coordinates
  205 + if not self.arc_motion_flag:
  206 + head_center_coordinates = self.process_tracker.estimate_head_center(self.tracker,
  207 + current_head_filtered).tolist()
  208 +
  209 + self.target_linear_out, self.target_arc = self.process_tracker.compute_arc_motion(current_robot_coordinates, head_center_coordinates,
  210 + new_robot_coordinates)
  211 + self.arc_motion_flag = True
  212 + self.arc_motion_step_flag = const.ROBOT_MOTIONS["linear out"]
  213 +
  214 + if self.arc_motion_flag and self.arc_motion_step_flag == const.ROBOT_MOTIONS["linear out"]:
  215 + coord = self.target_linear_out
  216 + if np.allclose(np.array(actual_point), np.array(self.target_linear_out), 0, 1):
  217 + self.arc_motion_step_flag = const.ROBOT_MOTIONS["arc"]
  218 + coord = self.target_arc
  219 +
  220 + elif self.arc_motion_flag and self.arc_motion_step_flag == const.ROBOT_MOTIONS["arc"]:
  221 + head_center_coordinates = self.process_tracker.estimate_head_center(self.tracker,
  222 + current_head_filtered).tolist()
  223 +
  224 + _, new_target_arc = self.process_tracker.compute_arc_motion(current_robot_coordinates, head_center_coordinates,
  225 + new_robot_coordinates)
  226 + if np.allclose(np.array(new_target_arc[3:-1]), np.array(self.target_arc[3:-1]), 0, 1):
  227 + None
  228 + else:
  229 + if self.process_tracker.correction_distance_calculation_target(new_robot_coordinates, current_robot_coordinates) >= \
  230 + const.ROBOT_ARC_THRESHOLD_DISTANCE*0.8:
  231 + self.target_arc = new_target_arc
229 232  
230   - coord = self.target_arc
  233 + coord = self.target_arc
231 234  
232   - if np.allclose(np.array(actual_point), np.array(self.target_arc[3:-1]), 0, 10):
233   - self.arc_motion_flag = False
234   - self.arc_motion_step_flag = const.ROBOT_MOTIONS["normal"]
235   - coord = new_robot_coordinates
  235 + if np.allclose(np.array(actual_point), np.array(self.target_arc[3:-1]), 0, 10):
  236 + self.arc_motion_flag = False
  237 + self.arc_motion_step_flag = const.ROBOT_MOTIONS["normal"]
  238 + coord = new_robot_coordinates
236 239  
237   - self.trck_init_robot.SendCoordinates(coord, self.arc_motion_step_flag)
  240 + self.trck_init_robot.SendCoordinates(coord, self.arc_motion_step_flag)
  241 + else:
  242 + print("Head is too far from the robot basis")
238 243  
239 244 def robot_control(self, current_tracker_coordinates_in_robot, current_robot_coordinates, markers_flag):
240 245 coord_head_tracker_in_robot = current_tracker_coordinates_in_robot[1]
... ...