ENH: robot documentation

Renan
1 parent c21b81c5
Showing 4 changed files with 75 additions and 15 deletions Show diff stats
invesalius/data/coordinates.py
invesalius/data/elfin.py
invesalius/data/elfin_processing.py
invesalius/navigation/robot.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.])
@@ -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):
@@ -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
@@ -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()
	@@ -236,7 +236,6 @@ def PolhemusCoord(trck, trck_id, ref_mode):		@@ -236,7 +236,6 @@ def PolhemusCoord(trck, trck_id, ref_mode):
236		236
237		237
238	def PolhemusWrapperCoord(trck, trck_id, ref_mode):	238	def PolhemusWrapperCoord(trck, trck_id, ref_mode):
239	-
240	trck.Run()	239	trck.Run()
241	scale = 10.0 * np.array([1., 1., 1.])	240	scale = 10.0 * np.array([1., 1., 1.])
242		241