diff --git a/invesalius/constants.py b/invesalius/constants.py index 0873cd8..9278b6a 100644 --- a/invesalius/constants.py +++ b/invesalius/constants.py @@ -853,6 +853,8 @@ PULSE_DURATION_IN_MILLISECONDS = 0.2 ROBOT_ElFIN_IP = ['143.107.220.251', '169.254.153.251', '127.0.0.1'] ROBOT_ElFIN_PORT = 10003 ROBOT_MOTIONS = {"normal": 0, "linear out": 1, "arc": 2} -ROBOT_HEAD_VELOCITY_THRESHOLD = 10 -ROBOT_ARC_THRESHOLD_DISTANCE = 100 +ROBOT_HEAD_VELOCITY_THRESHOLD = 10 #mm/s +ROBOT_ARC_THRESHOLD_DISTANCE = 100 #mm ROBOT_VERSOR_SCALE_FACTOR = 70 +#Robot Working Space is defined as 800mm in Elfin manual. For safety, the value is reduced by 5%. +ROBOT_WORKING_SPACE = 760 #mm diff --git a/invesalius/data/elfin_processing.py b/invesalius/data/elfin_processing.py index c6d9182..dd34705 100644 --- a/invesalius/data/elfin_processing.py +++ b/invesalius/data/elfin_processing.py @@ -197,11 +197,17 @@ class TrackerProcessing: """ Estimates the Euclidean distance between the actual position and the target """ - square_sum = (coord_inv[0]-actual_point[0]) ** 2 +\ - (coord_inv[1]-actual_point[1]) ** 2 +\ - (coord_inv[2]-actual_point[2]) ** 2 - correction_distance_compensation = pow(square_sum, 0.5) + correction_distance_compensation = np.sqrt((coord_inv[0]-actual_point[0]) ** 2 + + (coord_inv[1]-actual_point[1]) ** 2 + + (coord_inv[2]-actual_point[2]) ** 2) return correction_distance_compensation + def estimate_robot_target_length(self, robot_target): + """ + Estimates the length of the 3D vector of the robot target + """ + robot_target_length = np.sqrt(robot_target[0] ** 2 + robot_target[1] ** 2 + robot_target[2] ** 2) + + return robot_target_length diff --git a/invesalius/navigation/robot.py b/invesalius/navigation/robot.py index cb856c2..33196a2 100644 --- a/invesalius/navigation/robot.py +++ b/invesalius/navigation/robot.py @@ -194,47 +194,52 @@ class ControlRobot(threading.Thread): The last step is to make a linear move until the target (goes to the inner sphere) """ - if distance_target < const.ROBOT_ARC_THRESHOLD_DISTANCE and not self.arc_motion_flag: - self.trck_init_robot.SendCoordinates(new_robot_coordinates, const.ROBOT_MOTIONS["normal"]) - - elif distance_target >= const.ROBOT_ARC_THRESHOLD_DISTANCE or self.arc_motion_flag: - actual_point = current_robot_coordinates - if not self.arc_motion_flag: - head_center_coordinates = self.process_tracker.estimate_head_center(self.tracker, - current_head_filtered).tolist() - - self.target_linear_out, self.target_arc = self.process_tracker.compute_arc_motion(current_robot_coordinates, head_center_coordinates, - new_robot_coordinates) - self.arc_motion_flag = True - self.arc_motion_step_flag = const.ROBOT_MOTIONS["linear out"] - - if self.arc_motion_flag and self.arc_motion_step_flag == const.ROBOT_MOTIONS["linear out"]: - coord = self.target_linear_out - if np.allclose(np.array(actual_point), np.array(self.target_linear_out), 0, 1): - self.arc_motion_step_flag = const.ROBOT_MOTIONS["arc"] - coord = self.target_arc - - elif self.arc_motion_flag and self.arc_motion_step_flag == const.ROBOT_MOTIONS["arc"]: - head_center_coordinates = self.process_tracker.estimate_head_center(self.tracker, - current_head_filtered).tolist() - - _, new_target_arc = self.process_tracker.compute_arc_motion(current_robot_coordinates, head_center_coordinates, - new_robot_coordinates) - if np.allclose(np.array(new_target_arc[3:-1]), np.array(self.target_arc[3:-1]), 0, 1): - None - else: - if self.process_tracker.correction_distance_calculation_target(new_robot_coordinates, current_robot_coordinates) >= \ - const.ROBOT_ARC_THRESHOLD_DISTANCE*0.8: - self.target_arc = new_target_arc + #Check if the target is inside the working space + if self.process_tracker.estimate_robot_target_length(new_robot_coordinates) < const.ROBOT_WORKING_SPACE: + #Check the target distance to define the motion mode + if distance_target < const.ROBOT_ARC_THRESHOLD_DISTANCE and not self.arc_motion_flag: + self.trck_init_robot.SendCoordinates(new_robot_coordinates, const.ROBOT_MOTIONS["normal"]) + + elif distance_target >= const.ROBOT_ARC_THRESHOLD_DISTANCE or self.arc_motion_flag: + actual_point = current_robot_coordinates + if not self.arc_motion_flag: + head_center_coordinates = self.process_tracker.estimate_head_center(self.tracker, + current_head_filtered).tolist() + + self.target_linear_out, self.target_arc = self.process_tracker.compute_arc_motion(current_robot_coordinates, head_center_coordinates, + new_robot_coordinates) + self.arc_motion_flag = True + self.arc_motion_step_flag = const.ROBOT_MOTIONS["linear out"] + + if self.arc_motion_flag and self.arc_motion_step_flag == const.ROBOT_MOTIONS["linear out"]: + coord = self.target_linear_out + if np.allclose(np.array(actual_point), np.array(self.target_linear_out), 0, 1): + self.arc_motion_step_flag = const.ROBOT_MOTIONS["arc"] + coord = self.target_arc + + elif self.arc_motion_flag and self.arc_motion_step_flag == const.ROBOT_MOTIONS["arc"]: + head_center_coordinates = self.process_tracker.estimate_head_center(self.tracker, + current_head_filtered).tolist() + + _, new_target_arc = self.process_tracker.compute_arc_motion(current_robot_coordinates, head_center_coordinates, + new_robot_coordinates) + if np.allclose(np.array(new_target_arc[3:-1]), np.array(self.target_arc[3:-1]), 0, 1): + None + else: + if self.process_tracker.correction_distance_calculation_target(new_robot_coordinates, current_robot_coordinates) >= \ + const.ROBOT_ARC_THRESHOLD_DISTANCE*0.8: + self.target_arc = new_target_arc - coord = self.target_arc + coord = self.target_arc - if np.allclose(np.array(actual_point), np.array(self.target_arc[3:-1]), 0, 10): - self.arc_motion_flag = False - self.arc_motion_step_flag = const.ROBOT_MOTIONS["normal"] - coord = new_robot_coordinates + if np.allclose(np.array(actual_point), np.array(self.target_arc[3:-1]), 0, 10): + self.arc_motion_flag = False + self.arc_motion_step_flag = const.ROBOT_MOTIONS["normal"] + coord = new_robot_coordinates - self.trck_init_robot.SendCoordinates(coord, self.arc_motion_step_flag) + self.trck_init_robot.SendCoordinates(coord, self.arc_motion_step_flag) + else: + print("Head is too far from the robot basis") def robot_control(self, current_tracker_coordinates_in_robot, current_robot_coordinates, markers_flag): coord_head_tracker_in_robot = current_tracker_coordinates_in_robot[1] -- libgit2 0.21.2