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,6 +853,8 @@ PULSE_DURATION_IN_MILLISECONDS = 0.2
853 ROBOT_ElFIN_IP = ['143.107.220.251', '169.254.153.251', '127.0.0.1'] 853 ROBOT_ElFIN_IP = ['143.107.220.251', '169.254.153.251', '127.0.0.1']
854 ROBOT_ElFIN_PORT = 10003 854 ROBOT_ElFIN_PORT = 10003
855 ROBOT_MOTIONS = {"normal": 0, "linear out": 1, "arc": 2} 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 ROBOT_VERSOR_SCALE_FACTOR = 70 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,11 +197,17 @@ class TrackerProcessing:
197 """ 197 """
198 Estimates the Euclidean distance between the actual position and the target 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 return correction_distance_compensation 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,47 +194,52 @@ class ControlRobot(threading.Thread):
194 The last step is to make a linear move until the target (goes to the inner sphere) 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 def robot_control(self, current_tracker_coordinates_in_robot, current_robot_coordinates, markers_flag): 244 def robot_control(self, current_tracker_coordinates_in_robot, current_robot_coordinates, markers_flag):
240 coord_head_tracker_in_robot = current_tracker_coordinates_in_robot[1] 245 coord_head_tracker_in_robot = current_tracker_coordinates_in_robot[1]