diff --git a/invesalius/data/coordinates.py b/invesalius/data/coordinates.py index 71d2aeb..4eb2a61 100644 --- a/invesalius/data/coordinates.py +++ b/invesalius/data/coordinates.py @@ -236,7 +236,6 @@ def PolhemusCoord(trck, trck_id, ref_mode): def PolhemusWrapperCoord(trck, trck_id, ref_mode): - trck.Run() scale = 10.0 * np.array([1., 1., 1.]) diff --git a/invesalius/data/elfin.py b/invesalius/data/elfin.py index da4b135..8fe34f4 100644 --- a/invesalius/data/elfin.py +++ b/invesalius/data/elfin.py @@ -6,6 +6,10 @@ from socket import socket, AF_INET, SOCK_STREAM import invesalius.constants as const class Elfin_Server(): + """ + This class is similar to tracker devices wrappers. + It follows the same functions as the others (Initialize, Run and Close) + """ def __init__(self, server_ip, port_number): self.server_ip = server_ip self.port_number = port_number @@ -21,6 +25,10 @@ class Elfin_Server(): return self.cobot.ReadPcsActualPos() def SendCoordinates(self, target, type=const.ROBOT_MOTIONS["normal"]): + """ + It's not possible to send a move command to elfin if the robot is during a move. + Status 1009 means robot in motion. + """ status = self.cobot.ReadMoveState() if status != 1009: if type == const.ROBOT_MOTIONS["normal"] or type == const.ROBOT_MOTIONS["linear out"]: @@ -29,6 +37,8 @@ class Elfin_Server(): self.cobot.MoveC(target) def StopRobot(self): + # Takes some microseconds to the robot actual stops after the command. + # The sleep time is required to guarantee the stop self.cobot.GrpStop() sleep(0.1) @@ -37,6 +47,9 @@ class Elfin_Server(): class Elfin: def __init__(self): + """ + Class to communicate with elfin robot. This class follows "HansRobot Communication Protocol Interface". + """ self.end_msg = ",;" def connect(self, server_ip, port_number, message_size, robot_id): diff --git a/invesalius/data/elfin_processing.py b/invesalius/data/elfin_processing.py index 5443514..41b0202 100644 --- a/invesalius/data/elfin_processing.py +++ b/invesalius/data/elfin_processing.py @@ -140,6 +140,9 @@ class TrackerProcessing: return init_move_out_point, target_arc def compute_head_move_threshold(self, current_ref): + """ + Checks if the head velocity is bellow the threshold + """ self.coord_vel.append(current_ref) self.timestamp.append(time()) if len(self.coord_vel) >= 10: @@ -161,18 +164,24 @@ class TrackerProcessing: return False - def compute_head_move_compensation(self, current_head, M_change_robot_to_head): + def compute_head_move_compensation(self, current_head, m_change_robot_to_head): + """ + Estimates the new robot position to reach the target + """ M_current_head = dco.coordinates_to_transformation_matrix( position=current_head[:3], orientation=current_head[3:6], axes='rzyx', ) - M_robot_new = M_current_head @ M_change_robot_to_head - angles_as_deg, translate = dco.transformation_matrix_to_coordinates(M_robot_new, axes='rzyx') + m_robot_new = M_current_head @ m_change_robot_to_head + angles_as_deg, translate = dco.transformation_matrix_to_coordinates(m_robot_new, axes='rzyx') #TODO: check this with robot return list(translate) + list(angles_as_deg) def estimate_head_center(self, tracker, current_head): + """ + Estimates the actual head center position using fiducials + """ m_probe_head_left, m_probe_head_right, m_probe_head_nasion = tracker.GetMatrixTrackerFiducials() m_current_head = dcr.compute_marker_transformation(np.array([current_head]), 0) @@ -182,10 +191,13 @@ class TrackerProcessing: return (m_ear_left_new[:3, -1] + m_ear_right_new[:3, -1])/2 def correction_distance_calculation_target(self, coord_inv, actual_point): - 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(sum, 0.5) + """ + 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) return correction_distance_compensation diff --git a/invesalius/navigation/robot.py b/invesalius/navigation/robot.py index ba0c1bb..cb856c2 100644 --- a/invesalius/navigation/robot.py +++ b/invesalius/navigation/robot.py @@ -35,6 +35,10 @@ except ImportError: class Robot(): def __init__(self, tracker): + """ + Class to establish the connection between the robot and InVesalius + :param tracker: tracker.py instance + """ self.tracker = tracker self.trk_init = None self.robot_target_queue = None @@ -109,6 +113,10 @@ class Robot(): class RobotCoordinates(): + """ + Class to set/get robot coordinates. Robot coordinates are acquired in ControlRobot thread. + The class is required to avoid acquisition conflict with different threads (coordinates and navigation) + """ def __init__(self): self.coord = None @@ -121,6 +129,19 @@ class RobotCoordinates(): class ControlRobot(threading.Thread): def __init__(self, trck_init, tracker, robotcoordinates, queues, process_tracker, event_robot): + """Class (threading) to perform the robot control. + A distinguish thread is required to increase perform and separate robot control from navigation thread + (safetly layer for robot positining). + + There is no GUI involved, them no Sleep is required + + :param trck_init: Initialization variable of tracking device and connection type. See tracker.py. + :param tracker: tracker.py instance + :param robotcoordinates: RobotCoordinates() instance + :param queues: Queue instance that manage coordinates and robot target + :param process_tracker: TrackerProcessing() instance from elfin_process + :param event_robot: Threading event to ControlRobot when tasks is done + """ threading.Thread.__init__(self, name='ControlRobot') self.trck_init_robot = trck_init[1][0] @@ -155,6 +176,24 @@ class ControlRobot(threading.Thread): return coord_raw, coord_robot_raw, markers_flag def robot_move_decision(self, distance_target, new_robot_coordinates, current_robot_coordinates, current_head_filtered): + """ + There are two types of robot movements. + We can imagine in two concentric spheres of different sizes. The inside sphere is to compensate for small head movements. + It was named "normal" moves. + The outside sphere is for the arc motion. The arc motion is a safety feature for long robot movements. + Even for a new target or a sudden huge head movement. + 1) normal: + A linear move from the actual position until the target position. + This movement just happens when move distance is below a threshold (const.ROBOT_ARC_THRESHOLD_DISTANCE) + 2) arc motion: + It can be divided into three parts. + The first one represents the movement from the inner sphere to the outer sphere. + The robot moves back using a radial move (it use the center of the head as a reference). + The second step is the actual arc motion (along the outer sphere). + A middle point, between the actual position and the target, is required. + 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"]) @@ -214,9 +253,10 @@ class ControlRobot(threading.Thread): self.coord_inv_old = new_robot_coordinates if np.allclose(np.array(new_robot_coordinates), np.array(current_robot_coordinates), 0, 0.01): - # print("At target within range 1") + #avoid small movements (0.01 mm) pass elif not np.allclose(np.array(new_robot_coordinates), np.array(self.coord_inv_old), 0, 5): + #if the head moves (>5mm) before the robot reach the target self.trck_init_robot.StopRobot() self.coord_inv_old = new_robot_coordinates else: @@ -230,15 +270,11 @@ class ControlRobot(threading.Thread): while not self.event_robot.is_set(): current_tracker_coordinates_in_robot, current_robot_coordinates, markers_flag = self.get_coordinates_from_tracker_devices() - if self.robot_target_queue.empty(): - None - else: + if not self.robot_target_queue.empty(): self.robot_tracker_flag, self.m_change_robot_to_head = self.robot_target_queue.get_nowait() self.robot_target_queue.task_done() - if self.object_at_target_queue.empty(): - None - else: + if not self.object_at_target_queue.empty(): self.target_flag = self.object_at_target_queue.get_nowait() self.object_at_target_queue.task_done() -- libgit2 0.21.2