Merge pull request #377 from rmatsuda/multimodal_tracking

Add robot control for TMS applications

Merge pull request #377 from rmatsuda/multimodal_tracking
Add robot control for TMS applications
Victor Hugo Souza · GitHub
2 parents 0acb6ee2 99884224
Showing 19 changed files with 1461 additions and 217 deletions Show diff stats
invesalius/constants.py
invesalius/data/bases.py
invesalius/data/coordinates.py
invesalius/data/coregistration.py
invesalius/data/elfin.py
invesalius/data/elfin_processing.py
invesalius/data/trackers.py
invesalius/data/tractography.py
invesalius/data/viewer_volume.py
invesalius/gui/dialogs.py
invesalius/gui/task_navigator.py
invesalius/navigation/navigation.py
invesalius/navigation/robot.py
invesalius/navigation/tracker.py
navigation/mtc_files/Markers/2Ref
navigation/mtc_files/Markers/3Coil_big
navigation/ndi_files/Markers/objP4.rom
navigation/ndi_files/Markers/refP4.rom
optional-requirements.txt
@@ -684,18 +684,19 @@ CAMERA = 5
 POLARIS = 6
 POLARISP4 = 7
 OPTITRACK = 8
-DEBUGTRACKRANDOM = 9
-DEBUGTRACKAPPROACH = 10
+ROBOT = 9
+DEBUGTRACKRANDOM = 10
+DEBUGTRACKAPPROACH = 11
 DEFAULT_TRACKER = SELECT
 NDICOMPORT = b'COM1'
 TRACKERS = [_("Claron MicronTracker"),
-            _("Polhemus FASTRAK"), _("Polhemus ISOTRAK II"),
-            _("Polhemus PATRIOT"), _("Camera tracker"),
-            _("NDI Polaris"), _("NDI Polaris P4"),
-            _("Optitrack"), _("Debug tracker (random)"),
-            _("Debug tracker (approach)")]
+           _("Polhemus FASTRAK"), _("Polhemus ISOTRAK II"),
+           _("Polhemus PATRIOT"), _("Camera tracker"),
+           _("NDI Polaris"), _("NDI Polaris P4"),
+           _("Optitrack"), _("Robot tracker"),
+           _("Debug tracker (random)"), _("Debug tracker (approach)")]
 STATIC_REF = 0
 DYNAMIC_REF = 1
@@ -847,3 +848,11 @@ WILDCARD_MARKER_FILES = _(&quot;Marker scanner coord files (*.mkss)|*.mkss&quot;)
 BAUD_RATES = [300, 1200, 2400, 4800, 9600, 19200, 38400, 57600, 115200]
 BAUD_RATE_DEFAULT_SELECTION = 4
 PULSE_DURATION_IN_MILLISECONDS = 0.2
+
+#Robot
+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_VERSOR_SCALE_FACTOR = 70
@@ -244,3 +244,48 @@ def object_registration(fiducials, orients, coord_raw, m_change):
     )
     return t_obj_raw, s0_raw, r_s0_raw, s0_dyn, m_obj_raw, r_obj_img
+
+def compute_robot_to_head_matrix(raw_target_robot):
+    """
+    :param head: nx6 array of head coordinates from tracking device in robot space
+    :param robot: nx6 array of robot coordinates
+
+    :return: target_robot_matrix: 3x3 array representing change of basis from robot to head in robot system
+    """
+    head, robot = raw_target_robot
+    # compute head target matrix
+    m_head_target = dco.coordinates_to_transformation_matrix(
+        position=head[:3],
+        orientation=head[3:],
+        axes='rzyx',
+    )
+
+    # compute robot target matrix
+    m_robot_target = dco.coordinates_to_transformation_matrix(
+        position=robot[:3],
+        orientation=robot[3:],
+        axes='rzyx',
+    )
+    robot_to_head_matrix = np.linalg.inv(m_head_target) @ m_robot_target
+
+    return robot_to_head_matrix
+
+
+class transform_tracker_to_robot(object):
+    M_tracker_to_robot = np.array([])
+    def transformation_tracker_to_robot(self, tracker_coord):
+        if not transform_tracker_to_robot.M_tracker_to_robot.any():
+            return None
+
+        M_tracker = dco.coordinates_to_transformation_matrix(
+            position=tracker_coord[:3],
+            orientation=tracker_coord[3:6],
+            axes='rzyx',
+        )
+        M_tracker_in_robot = transform_tracker_to_robot.M_tracker_to_robot @ M_tracker
+
+        _, _, angles, translate, _ = tr.decompose_matrix(M_tracker_in_robot)
+        tracker_in_robot = [translate[0], translate[1], translate[2], \
+                            np.degrees(angles[2]), np.degrees(angles[1]), np.degrees(angles[0])]
+
+        return tracker_in_robot
@@ -65,15 +65,57 @@ def GetCoordinatesForThread(trck_init, trck_id, ref_mode):
                     const.POLARIS: PolarisCoord,
                     const.POLARISP4: PolarisP4Coord,
                     const.OPTITRACK: OptitrackCoord,
+                    const.ROBOT: RobotCoord,
                     const.DEBUGTRACKRANDOM: DebugCoordRandom,
                     const.DEBUGTRACKAPPROACH: DebugCoordRandom}
-
         coord, markers_flag = getcoord[trck_id](trck_init, trck_id, ref_mode)
     else:
         print("Select Tracker")
     return coord, markers_flag
+def PolarisP4Coord(trck_init, trck_id, ref_mode):
+    trck = trck_init[0]
+    trck.Run()
+
+    probe = trck.probe.decode(const.FS_ENCODE)
+    ref = trck.ref.decode(const.FS_ENCODE)
+    obj = trck.obj.decode(const.FS_ENCODE)
+
+    probe = probe[2:]
+    ref = ref[2:]
+    obj = obj[2:]
+
+    if probe[:7] == "MISSING":
+        coord1 = np.hstack(([0, 0, 0], [0, 0, 0]))
+    else:
+        q = [int(probe[i:i + 6]) * 0.0001 for i in range(0, 24, 6)]
+        t = [int(probe[i:i + 7]) * 0.01 for i in range(24, 45, 7)]
+        angles_probe = np.degrees(tr.euler_from_quaternion(q, axes='rzyx'))
+        trans_probe = np.array(t).astype(float)
+        coord1 = np.hstack((trans_probe, angles_probe))
+
+    if ref[:7] == "MISSING":
+        coord2 = np.hstack(([0, 0, 0], [0, 0, 0]))
+    else:
+        q = [int(ref[i:i + 6]) * 0.0001 for i in range(0, 24, 6)]
+        t = [int(ref[i:i + 7]) * 0.01 for i in range(24, 45, 7)]
+        angles_ref = np.degrees(tr.euler_from_quaternion(q, axes='rzyx'))
+        trans_ref = np.array(t).astype(float)
+        coord2 = np.hstack((trans_ref, angles_ref))
+
+    if obj[:7] == "MISSING":
+        coord3 = np.hstack(([0, 0, 0], [0, 0, 0]))
+    else:
+        q = [int(obj[i:i + 6]) * 0.0001 for i in range(0, 24, 6)]
+        t = [int(obj[i:i + 7]) * 0.01 for i in range(24, 45, 7)]
+        angles_obj = np.degrees(tr.euler_from_quaternion(q, axes='rzyx'))
+        trans_obj = np.array(t).astype(float)
+        coord3 = np.hstack((trans_obj, angles_obj))
+
+    coord = np.vstack([coord1, coord2, coord3])
+
+    return coord, [trck.probeID, trck.refID, trck.objID]
 def OptitrackCoord(trck_init, trck_id, ref_mode):
     """
@@ -139,49 +181,16 @@ def PolarisCoord(trck_init, trck_id, ref_mode):
     return coord, [trck.probeID, trck.refID, trck.objID]
-
-def PolarisP4Coord(trck_init, trck_id, ref_mode):
-    trck = trck_init[0]
-    trck.Run()
-
-    probe = trck.probe.decode(const.FS_ENCODE)
-    ref = trck.ref.decode(const.FS_ENCODE)
-    obj = trck.obj.decode(const.FS_ENCODE)
-
-    probe = probe[2:]
-    ref = ref[2:]
-    obj = obj[2:]
-
-    if probe[:7] == "MISSING":
-        coord1 = np.hstack(([0, 0, 0], [0, 0, 0]))
+def ElfinCoord(trck_init):
+    if len(trck_init) > 2:
+        robotcoordinates = trck_init[-1]
+        coord = robotcoordinates.GetRobotCoordinates()
+        if coord is None:
+            coord = np.array([0, 0, 0, 0, 0, 0])
     else:
-        q = [int(probe[i:i + 6]) * 0.0001 for i in range(0, 24, 6)]
-        t = [int(probe[i:i + 7]) * 0.01 for i in range(24, 45, 7)]
-        angles_probe = np.degrees(tr.euler_from_quaternion(q, axes='rzyx'))
-        trans_probe = np.array(t).astype(float)
-        coord1 = np.hstack((trans_probe, angles_probe))
-        if ref[:7] == "MISSING":
-            coord2 = np.hstack(([0, 0, 0], [0, 0, 0]))
-        else:
-            q = [int(ref[i:i + 6]) * 0.0001 for i in range(0, 24, 6)]
-            t = [int(ref[i:i + 7]) * 0.01 for i in range(24, 45, 7)]
-            angles_ref = np.degrees(tr.euler_from_quaternion(q, axes='rzyx'))
-            trans_ref = np.array(t).astype(float)
-            coord2 = np.hstack((trans_ref, angles_ref))
-
-        if obj[:7] == "MISSING":
-            coord3 = np.hstack(([0, 0, 0], [0, 0, 0]))
-        else:
-            q = [int(obj[i:i + 6]) * 0.0001 for i in range(0, 24, 6)]
-            t = [int(obj[i:i + 7]) * 0.01 for i in range(24, 45, 7)]
-            angles_obj = np.degrees(tr.euler_from_quaternion(q, axes='rzyx'))
-            trans_obj = np.array(t).astype(float)
-            coord3 = np.hstack((trans_obj, angles_obj))
-
-        coord = np.vstack([coord1, coord2, coord3])
-
-        return coord, [trck.probeID, trck.refID, trck.objID]
+        coord = np.array([0, 0, 0, 0, 0, 0])
+    return coord
 def CameraCoord(trck_init, trck_id, ref_mode):
     trck = trck_init[0]
@@ -227,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.])
@@ -475,6 +483,22 @@ def dynamic_reference_m(probe, reference):
     return coord_rot
+def RobotCoord(trk_init, trck_id, ref_mode):
+    coord_tracker, markers_flag = GetCoordinatesForThread(trk_init[0], trk_init[2], ref_mode)
+    coord_robot = ElfinCoord(trk_init[1:])
+
+    probe_tracker_in_robot = db.transform_tracker_to_robot().transformation_tracker_to_robot(coord_tracker[0])
+    ref_tracker_in_robot = db.transform_tracker_to_robot().transformation_tracker_to_robot(coord_tracker[1])
+    obj_tracker_in_robot = db.transform_tracker_to_robot().transformation_tracker_to_robot(coord_tracker[2])
+
+    if probe_tracker_in_robot is None:
+        probe_tracker_in_robot = coord_tracker[0]
+        ref_tracker_in_robot = coord_tracker[1]
+        obj_tracker_in_robot = coord_tracker[2]
+
+    return np.vstack([probe_tracker_in_robot, ref_tracker_in_robot, coord_robot, obj_tracker_in_robot]), markers_flag
+
+
 def dynamic_reference_m2(probe, reference):
     """
     Apply dynamic reference correction to probe coordinates. Uses the alpha, beta and gama
@@ -23,9 +23,9 @@ import threading
 from time import sleep
 import invesalius.constants as const
-import invesalius.data.coordinates as dco
 import invesalius.data.transformations as tr
 import invesalius.data.bases as bases
+import invesalius.data.coordinates as dco
 # TODO: Replace the use of degrees by radians in every part of the navigation pipeline
@@ -88,9 +88,9 @@ def tracker_to_image(m_change, m_probe_ref, r_obj_img, m_obj_raw, s0_dyn):
     :type m_probe_ref: numpy.ndarray
     :param r_obj_img: Object coordinate system in image space (3d model)
     :type r_obj_img: numpy.ndarray
-    :param m_obj_raw: Object basis in raw coordinates from tacker
+    :param m_obj_raw: Object basis in raw coordinates from tracker
     :type m_obj_raw: numpy.ndarray
-    :param s0_dyn:
+    :param s0_dyn: Initial alignment of probe fixed in the object in reference (or static) frame
     :type s0_dyn: numpy.ndarray
     :return: 4 x 4 numpy double array
     :rtype: numpy.ndarray
@@ -190,11 +190,14 @@ class CoordinateCorregistrate(threading.Thread):
         self.event = event
         self.sle = sle
         self.icp_queue = queues[2]
+        self.object_at_target_queue = queues[3]
         self.icp = None
         self.m_icp = None
+        self.robot_tracker_flag = None
         self.last_coord = None
         self.tracker_id = tracker_id
         self.target = target
+        self.target_flag = False
         if self.target is not None:
             self.target = np.array(self.target)
@@ -214,10 +217,12 @@ class CoordinateCorregistrate(threading.Thread):
         # print('CoordCoreg: event {}'.format(self.event.is_set()))
         while not self.event.is_set():
             try:
-                if self.icp_queue.empty():
-                    None
-                else:
+                if not self.icp_queue.empty():
                     self.icp, self.m_icp = self.icp_queue.get_nowait()
+
+                if not self.object_at_target_queue.empty():
+                    self.target_flag = self.object_at_target_queue.get_nowait()
+
                 # print(f"Set the coordinate")
                 coord_raw, markers_flag = self.tracker.TrackerCoordinates.GetCoordinates()
                 coord, m_img = corregistrate_object_dynamic(coreg_data, coord_raw, self.ref_mode_id, [self.icp, self.m_icp])
@@ -249,7 +254,7 @@ class CoordinateCorregistrate(threading.Thread):
                 if self.icp:
                     m_img = bases.transform_icp(m_img, self.m_icp)
-                self.coord_queue.put_nowait([coord, m_img, view_obj])
+                self.coord_queue.put_nowait([coord, [coord_raw, markers_flag], m_img, view_obj])
                 # print('CoordCoreg: put {}'.format(count))
                 # count += 1
@@ -258,7 +263,6 @@ class CoordinateCorregistrate(threading.Thread):
                 if not self.icp_queue.empty():
                     self.icp_queue.task_done()
-
                 # The sleep has to be in both threads
                 sleep(self.sle)
             except queue.Full:
@@ -303,7 +307,7 @@ class CoordinateCorregistrateNoObject(threading.Thread):
                 if self.icp:
                     m_img = bases.transform_icp(m_img, self.m_icp)
-                self.coord_queue.put_nowait([coord, m_img, view_obj])
+                self.coord_queue.put_nowait([coord, [coord_raw, markers_flag], m_img, view_obj])
                 if self.view_tracts:
                     self.coord_tracts_queue.put_nowait(m_img_flip)
@@ -0,0 +1,249 @@
+#!/usr/bin/env python3
+
+import sys
+from time import sleep
+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
+
+    def Initialize(self):
+        message_size = 1024
+        robot_id = 0
+        self.cobot = Elfin()
+        self.cobot.connect(self.server_ip, self.port_number, message_size, robot_id)
+        print("connected!")
+
+    def Run(self):
+        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"]:
+                self.cobot.MoveL(target)
+            elif type == const.ROBOT_MOTIONS["arc"]:
+                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)
+
+    def Close(self):
+        self.cobot.close()
+
+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):
+        mySocket = socket(AF_INET, SOCK_STREAM)
+        mySocket.connect((server_ip, port_number))
+
+        self.message_size = message_size
+        self.robot_id = str(robot_id)
+        self.mySocket = mySocket
+
+    def send(self, message):
+        self.mySocket.sendall(message.encode('utf-8'))
+        data = self.mySocket.recv(self.message_size).decode('utf-8').split(',')
+        status = self.check_status(data)
+        if status and type(data) != bool:
+            if len(data) > 3:
+                return data[2:-1]
+        return status
+
+    def check_status(self, recv_message):
+        status = recv_message[1]
+        if status == 'OK':
+            return True
+
+        elif status == 'Fail':
+            print("Error code: ", recv_message[2])
+            return False
+
+    def Electrify(self):
+        """
+        Function: Power the robot
+        Notes: successful completion of power up before returning, power up time is
+        about 44s.
+        :return:
+            if Error Return False
+            if not Error Return True
+        """
+        message = "Electrify" + self.end_msg
+        status = self.send(message)
+        return status
+
+    def BlackOut(self):
+        """
+        Function: Robot blackout
+        Notes: successful power outage will only return, power failure time is 3s.
+        :return:
+            if Error Return False
+            if not Error Return True
+        """
+        message = "BlackOut" + self.end_msg
+        status = self.send(message)
+        return status
+
+    def StartMaster(self):
+        """
+        Function: Start master station
+        Notes: the master station will not be returned until successfully started, startup
+        master time is about 4s.
+        :return:
+            if Error Return False
+            if not Error Return True
+        """
+        message = "StartMaster" + self.end_msg
+        status = self.send(message)
+        return status
+
+    def CloseMaster(self):
+        """
+        Function: Close master station
+        Notes: the master station will not be returned until successfully closed, shut
+        down the master station time is about 2s.
+        :return:
+            if Error Return False
+            if not Error Return True
+        """
+        message = "CloseMaster" + self.end_msg
+        status = self.send(message)
+        return status
+
+    def GrpPowerOn(self):
+        """
+        Function: Robot servo on
+        :return:
+            if Error Return False
+            if not Error Return True
+        """
+        message = "GrpPowerOn," + self.robot_id + self.end_msg
+        status = self.send(message)
+        return status
+
+    def GrpPowerOff(self):
+        """
+        Function: Robot servo off
+        :return:
+            if Error Return False
+            if not Error Return True
+        """
+        message = "GrpPowerOff," + self.robot_id + self.end_msg
+        status = self.send(message)
+        return status
+
+    def GrpStop(self):
+        """
+        Function: Stop robot
+        :return:
+            if Error Return False
+            if not Error Return True
+        """
+        message = "GrpStop," + self.robot_id + self.end_msg
+        status = self.send(message)
+        return status
+
+    def SetOverride(self, override):
+        """
+        function: Set speed ratio
+        :param override:
+            double: set speed ratio, range of 0.01~1
+        :return:
+        if Error Return False
+            if not Error Return True
+        """
+
+        message = "SetOverride," + self.robot_id + ',' + str(override) + self.end_msg
+        status = self.send(message)
+        return status
+
+    def ReadPcsActualPos(self):
+        """Function: Get the actual position of the space coordinate
+        :return:
+            if True Return x,y,z,a,b,c
+            if Error Return False
+        """
+        message = "ReadPcsActualPos," + self.robot_id + self.end_msg
+        coord = self.send(message)
+        if coord:
+            return [float(s) for s in coord]
+
+        return coord
+
+    def MoveL(self, target):
+        """
+        function: Robot moves straight to the specified space coordinates
+        :param: target:[X,Y,Z,RX,RY,RZ]
+        :return:
+        """
+        target = [str(s) for s in target]
+        target = (",".join(target))
+        message = "MoveL," + self.robot_id + ',' + target + self.end_msg
+        return self.send(message)
+
+    def SetToolCoordinateMotion(self, status):
+        """
+        function: Function: Set tool coordinate motion
+        :param: int Switch 0=close 1=open
+        :return:
+            if Error Return False
+            if not Error Return True
+        """
+        message = "SetToolCoordinateMotion," + self.robot_id + ',' + str(status) + self.end_msg
+        status = self.send(message)
+        return status
+
+    def ReadMoveState(self):
+        """
+        Function: Get the motion state of the robot
+        :return:
+            Current state of motion of robot:
+            0=motion completion;
+            1009=in motion;
+            1013=waiting for execution;
+            1025 =Error reporting
+        """
+        message = "ReadMoveState," + self.robot_id + self.end_msg
+        status = int(self.send(message)[0])
+        return status
+
+    def MoveHoming(self):
+        """
+        Function: Robot returns to the origin
+        :return:
+            if Error Return False
+            if not Error Return True
+        """
+        message = "MoveHoming," + self.robot_id + self.end_msg
+        status = self.send(message)
+        return status
+
+    def MoveC(self, target):
+        """
+        function: Arc motion
+        :param: Through position[X,Y,Z],GoalCoord[X,Y,Z,RX,RY,RZ],Type[0 or 1],;
+        :return:
+        """
+        target = [str(s) for s in target]
+        target = (",".join(target))
+        message = "MoveC," + self.robot_id + ',' + target + self.end_msg
+        return self.send(message)
@@ -0,0 +1,207 @@
+#--------------------------------------------------------------------------
+# Software:     InVesalius - Software de Reconstrucao 3D de Imagens Medicas
+# Copyright:    (C) 2001  Centro de Pesquisas Renato Archer
+# Homepage:     http://www.softwarepublico.gov.br
+# Contact:      invesalius@cti.gov.br
+# License:      GNU - GPL 2 (LICENSE.txt/LICENCA.txt)
+#--------------------------------------------------------------------------
+#    Este programa e software livre; voce pode redistribui-lo e/ou
+#    modifica-lo sob os termos da Licenca Publica Geral GNU, conforme
+#    publicada pela Free Software Foundation; de acordo com a versao 2
+#    da Licenca.
+#
+#    Este programa eh distribuido na expectativa de ser util, mas SEM
+#    QUALQUER GARANTIA; sem mesmo a garantia implicita de
+#    COMERCIALIZACAO ou de ADEQUACAO A QUALQUER PROPOSITO EM
+#    PARTICULAR. Consulte a Licenca Publica Geral GNU para obter mais
+#    detalhes.
+#--------------------------------------------------------------------------
+import numpy as np
+import cv2
+from time import time
+
+import invesalius.data.transformations as tr
+import invesalius.data.coregistration as dcr
+import invesalius.data.coordinates as dco
+import invesalius.constants as const
+
+
+class KalmanTracker:
+    """
+    Kalman filter to avoid sudden fluctuation from tracker device.
+    The filter strength can be set by the cov_process, and cov_measure parameter
+    It is required to create one instance for each variable (x, y, z, a, b, g)
+    """
+    def __init__(self,
+                 state_num=2,
+                 covariance_process=0.001,
+                 covariance_measure=0.1):
+
+        self.state_num = state_num
+        measure_num = 1
+
+        # The filter itself.
+        self.filter = cv2.KalmanFilter(state_num, measure_num, 0)
+
+        self.state = np.zeros((state_num, 1), dtype=np.float32)
+        self.measurement = np.array((measure_num, 1), np.float32)
+        self.prediction = np.zeros((state_num, 1), np.float32)
+
+
+        self.filter.transitionMatrix = np.array([[1, 1],
+                                                 [0, 1]], np.float32)
+        self.filter.measurementMatrix = np.array([[1, 1]], np.float32)
+        self.filter.processNoiseCov = np.array([[1, 0],
+                                                [0, 1]], np.float32) * covariance_process
+        self.filter.measurementNoiseCov = np.array([[1]], np.float32) * covariance_measure
+
+    def update_kalman(self, measurement):
+        self.prediction = self.filter.predict()
+        self.measurement = np.array([[np.float32(measurement[0])]])
+
+        self.filter.correct(self.measurement)
+        self.state = self.filter.statePost
+
+
+class TrackerProcessing:
+    def __init__(self):
+        self.coord_vel = []
+        self.timestamp = []
+        self.velocity_vector = []
+        self.kalman_coord_vector = []
+        self.velocity_std = 0
+
+        self.tracker_stabilizers = [KalmanTracker(
+            state_num=2,
+            covariance_process=0.001,
+            covariance_measure=0.1) for _ in range(6)]
+
+    def kalman_filter(self, coord_tracker):
+        kalman_array = []
+        pose_np = np.array((coord_tracker[:3], coord_tracker[3:])).flatten()
+        for value, ps_stb in zip(pose_np, self.tracker_stabilizers):
+            ps_stb.update_kalman([value])
+            kalman_array.append(ps_stb.state[0])
+        coord_kalman = np.hstack(kalman_array)
+
+        self.kalman_coord_vector.append(coord_kalman[:3])
+        if len(self.kalman_coord_vector) < 20: #avoid initial fluctuations
+            coord_kalman = coord_tracker
+            print('initializing filter')
+        else:
+            del self.kalman_coord_vector[0]
+
+        return coord_kalman
+
+    def estimate_head_velocity(self, coord_vel, timestamp):
+        coord_vel = np.vstack(np.array(coord_vel))
+        coord_init = coord_vel[:int(len(coord_vel) / 2)].mean(axis=0)
+        coord_final = coord_vel[int(len(coord_vel) / 2):].mean(axis=0)
+        velocity = (coord_final - coord_init)/(timestamp[-1] - timestamp[0])
+        distance = (coord_final - coord_init)
+
+        return velocity, distance
+
+    def compute_versors(self, init_point, final_point):
+        init_point = np.array(init_point)
+        final_point = np.array(final_point)
+        norm = (sum((final_point - init_point) ** 2)) ** 0.5
+        versor_factor = (((final_point-init_point) / norm) * const.ROBOT_VERSOR_SCALE_FACTOR).tolist()
+
+        return versor_factor
+
+
+    def compute_arc_motion(self, current_robot_coordinates, head_center_coordinates, new_robot_coordinates):
+        head_center = head_center_coordinates[0], head_center_coordinates[1], head_center_coordinates[2], \
+                      new_robot_coordinates[3], new_robot_coordinates[4], new_robot_coordinates[5]
+
+        versor_factor_move_out = self.compute_versors(head_center, current_robot_coordinates)
+        init_move_out_point = current_robot_coordinates[0] + versor_factor_move_out[0], \
+                              current_robot_coordinates[1] + versor_factor_move_out[1], \
+                              current_robot_coordinates[2] + versor_factor_move_out[2], \
+                              current_robot_coordinates[3], current_robot_coordinates[4], current_robot_coordinates[5]
+
+        middle_point = ((new_robot_coordinates[0] + current_robot_coordinates[0]) / 2,
+                        (new_robot_coordinates[1] + current_robot_coordinates[1]) / 2,
+                        (new_robot_coordinates[2] + current_robot_coordinates[2]) / 2,
+                        0, 0, 0)
+        versor_factor_middle_arc = (np.array(self.compute_versors(head_center, middle_point))) * 2
+        middle_arc_point = middle_point[0] + versor_factor_middle_arc[0], \
+                           middle_point[1] + versor_factor_middle_arc[1], \
+                           middle_point[2] + versor_factor_middle_arc[2]
+
+        versor_factor_arc = self.compute_versors(head_center, new_robot_coordinates)
+        final_ext_arc_point = new_robot_coordinates[0] + versor_factor_arc[0], \
+                              new_robot_coordinates[1] + versor_factor_arc[1], \
+                              new_robot_coordinates[2] + versor_factor_arc[2], \
+                              new_robot_coordinates[3], new_robot_coordinates[4], new_robot_coordinates[5], 0
+
+        target_arc = middle_arc_point + final_ext_arc_point
+
+        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:
+            head_velocity, head_distance = self.estimate_head_velocity(self.coord_vel, self.timestamp)
+            self.velocity_vector.append(head_velocity)
+
+            del self.coord_vel[0]
+            del self.timestamp[0]
+
+            if len(self.velocity_vector) >= 30:
+                self.velocity_std = np.std(self.velocity_vector)
+                del self.velocity_vector[0]
+
+            if self.velocity_std > const.ROBOT_HEAD_VELOCITY_THRESHOLD:
+                print('Velocity threshold activated')
+                return False
+            else:
+                return True
+
+        return False
+
+    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, translate, _ = tr.decompose_matrix(m_robot_new)
+        angles = np.degrees(angles)
+
+        return m_robot_new[0, -1], m_robot_new[1, -1], m_robot_new[2, -1], angles[0], angles[1], \
+                    angles[2]
+
+    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)
+
+        m_ear_left_new = m_current_head @ m_probe_head_left
+        m_ear_right_new = m_current_head @ m_probe_head_right
+
+        return (m_ear_left_new[:3, -1] + m_ear_right_new[:3, -1])/2
+
+    def correction_distance_calculation_target(self, coord_inv, actual_point):
+        """
+        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
+
+
@@ -41,7 +41,8 @@ def TrackerConnection(tracker_id, trck_init, action):
                     const.CAMERA: CameraTracker,      # CAMERA
                     const.POLARIS: PolarisTracker,      # POLARIS
                     const.POLARISP4: PolarisP4Tracker,  # POLARISP4
-                    const.OPTITRACK: OptitrackTracker,   #Optitrack
+                    const.OPTITRACK: OptitrackTracker, #Optitrack
+                    const.ROBOT: RobotTracker,   #Robot
                     const.DEBUGTRACKRANDOM: DebugTrackerRandom,
                     const.DEBUGTRACKAPPROACH: DebugTrackerApproach}
@@ -65,36 +66,68 @@ def DefaultTracker(tracker_id):
     # return tracker initialization variable and type of connection
     return trck_init, 'wrapper'
-def OptitrackTracker(tracker_id):
-    """
-    Imports optitrack wrapper from Motive 2.2. Initialize cameras, attach listener, loads Calibration, loads User Profile
-    (Rigid bodies information).
+def ClaronTracker(tracker_id):
+    import invesalius.constants as const
+    from invesalius import inv_paths
-    Parameters
-    ----------
-    tracker_id : Optitrack ID
+    trck_init = None
+    try:
+        import pyclaron
-    Returns
-    -------
-    trck_init : local name for Optitrack module
-    """
-    from wx import ID_OK
+        lib_mode = 'wrapper'
+        trck_init = pyclaron.pyclaron()
+        trck_init.CalibrationDir = inv_paths.MTC_CAL_DIR.encode(const.FS_ENCODE)
+        trck_init.MarkerDir = inv_paths.MTC_MAR_DIR.encode(const.FS_ENCODE)
+        trck_init.NumberFramesProcessed = 1
+        trck_init.FramesExtrapolated = 0
+        trck_init.PROBE_NAME = const.MTC_PROBE_NAME.encode(const.FS_ENCODE)
+        trck_init.REF_NAME = const.MTC_REF_NAME.encode(const.FS_ENCODE)
+        trck_init.OBJ_NAME = const.MTC_OBJ_NAME.encode(const.FS_ENCODE)
+        trck_init.Initialize()
+
+        if trck_init.GetIdentifyingCamera():
+            trck_init.Run()
+            print("MicronTracker camera identified.")
+        else:
+            trck_init = None
+
+    except ImportError:
+        lib_mode = 'error'
+        print('The ClaronTracker library is not installed.')
+
+    return trck_init, lib_mode
+
+def PolhemusTracker(tracker_id):
+    try:
+        trck_init = PlhWrapperConnection(tracker_id)
+        lib_mode = 'wrapper'
+        if not trck_init:
+            print('Could not connect with Polhemus wrapper, trying USB connection...')
+            trck_init = PlhUSBConnection(tracker_id)
+            lib_mode = 'usb'
+            if not trck_init:
+                print('Could not connect with Polhemus USB, trying serial connection...')
+                trck_init = PlhSerialConnection(tracker_id)
+                lib_mode = 'serial'
+    except:
+        trck_init = None
+        lib_mode = 'error'
+        print('Could not connect to Polhemus by any method.')
+
+    return trck_init, lib_mode
+
+def CameraTracker(tracker_id):
     trck_init = None
-    dlg_port = dlg.SetOptitrackconfigs()
-    if dlg_port.ShowModal() == ID_OK:
-        Cal_optitrack, User_profile_optitrack = dlg_port.GetValue()
-        try:
-            import optitrack
-            trck_init = optitrack.optr()
+    try:
+        import invesalius.data.camera_tracker as cam
+        trck_init = cam.camera()
+        trck_init.Initialize()
+        print('Connect to camera tracking device.')
-            if trck_init.Initialize(Cal_optitrack, User_profile_optitrack)==0:
-                trck_init.Run() #Runs once Run function, to update cameras.
-            else:
-                trck_init = None
-        except ImportError:
-            print('Error')
-    else:
-        print('#####')
+    except:
+        print('Could not connect to camera tracker.')
+
+    # return tracker initialization variable and type of connection
     return trck_init, 'wrapper'
 def PolarisTracker(tracker_id):
@@ -111,22 +144,21 @@ def PolarisTracker(tracker_id):
             if trck_init.Initialize(com_port, PROBE_DIR, REF_DIR, OBJ_DIR) != 0:
                 trck_init = None
                 lib_mode = None
-                print('Could not connect to default tracker.')
+                print('Could not connect to polaris tracker.')
             else:
                 print('Connect to polaris tracking device.')
         except:
             lib_mode = 'error'
             trck_init = None
-            print('Could not connect to default tracker.')
+            print('Could not connect to polaris tracker.')
     else:
         lib_mode = None
-        print('Could not connect to default tracker.')
+        print('Could not connect to polaris tracker.')
     # return tracker initialization variable and type of connection
     return trck_init, lib_mode
-
 def PolarisP4Tracker(tracker_id):
     from wx import ID_OK
     trck_init = None
@@ -155,73 +187,74 @@ def PolarisP4Tracker(tracker_id):
     # return tracker initialization variable and type of connection
     return trck_init, lib_mode
+def OptitrackTracker(tracker_id):
+    """
+    Imports optitrack wrapper from Motive 2.2. Initialize cameras, attach listener, loads Calibration, loads User Profile
+    (Rigid bodies information).
+
+    Parameters
+    ----------
+    tracker_id : Optitrack ID
-def CameraTracker(tracker_id):
+    Returns
+    -------
+    trck_init : local name for Optitrack module
+    """
+    from wx import ID_OK
     trck_init = None
-    try:
-        import invesalius.data.camera_tracker as cam
-        trck_init = cam.camera()
-        trck_init.Initialize()
-        print('Connect to camera tracking device.')
-
-    except:
-        print('Could not connect to default tracker.')
+    dlg_port = dlg.SetOptitrackconfigs()
+    if dlg_port.ShowModal() == ID_OK:
+        Cal_optitrack, User_profile_optitrack = dlg_port.GetValue()
+        try:
+            import optitrack
+            trck_init = optitrack.optr()
-    # return tracker initialization variable and type of connection
+            if trck_init.Initialize(Cal_optitrack, User_profile_optitrack)==0:
+                trck_init.Run() #Runs once Run function, to update cameras.
+            else:
+                trck_init = None
+        except ImportError:
+            print('Error')
+    else:
+        print('#####')
     return trck_init, 'wrapper'
-
-def ClaronTracker(tracker_id):
-    import invesalius.constants as const
-    from invesalius import inv_paths
-
+def ElfinRobot(robot_IP):
     trck_init = None
     try:
-        import pyclaron
-
-        lib_mode = 'wrapper'
-        trck_init = pyclaron.pyclaron()
-        trck_init.CalibrationDir = inv_paths.MTC_CAL_DIR.encode(const.FS_ENCODE)
-        trck_init.MarkerDir = inv_paths.MTC_MAR_DIR.encode(const.FS_ENCODE)
-        trck_init.NumberFramesProcessed = 1
-        trck_init.FramesExtrapolated = 0
-        trck_init.PROBE_NAME = const.MTC_PROBE_NAME.encode(const.FS_ENCODE)
-        trck_init.REF_NAME = const.MTC_REF_NAME.encode(const.FS_ENCODE)
-        trck_init.OBJ_NAME = const.MTC_OBJ_NAME.encode(const.FS_ENCODE)
+        import invesalius.data.elfin as elfin
+        print("Trying to connect Robot via: ", robot_IP)
+        trck_init = elfin.Elfin_Server(robot_IP, const.ROBOT_ElFIN_PORT)
         trck_init.Initialize()
-
-        if trck_init.GetIdentifyingCamera():
-            trck_init.Run()
-            print("MicronTracker camera identified.")
-        else:
-            trck_init = None
-
-    except ImportError:
-        lib_mode = 'error'
-        print('The ClaronTracker library is not installed.')
-
-    return trck_init, lib_mode
-
-
-def PolhemusTracker(tracker_id):
-    try:
-        trck_init = PlhWrapperConnection(tracker_id)
         lib_mode = 'wrapper'
-        if not trck_init:
-            print('Could not connect with Polhemus wrapper, trying USB connection...')
-            trck_init = PlhUSBConnection(tracker_id)
-            lib_mode = 'usb'
-            if not trck_init:
-                print('Could not connect with Polhemus USB, trying serial connection...')
-                trck_init = PlhSerialConnection(tracker_id)
-                lib_mode = 'serial'
+        print('Connect to elfin robot tracking device.')
+
     except:
+        lib_mode = 'disconnect'
         trck_init = None
-        lib_mode = 'error'
-        print('Could not connect to Polhemus by any method.')
+        print('Could not connect to elfin robot tracker.')
+    # return tracker initialization variable and type of connection
     return trck_init, lib_mode
+def RobotTracker(tracker_id):
+    from wx import ID_OK
+    trck_init = None
+    trck_init_robot = None
+    tracker_id = None
+    dlg_device = dlg.SetTrackerDeviceToRobot()
+    if dlg_device.ShowModal() == ID_OK:
+        tracker_id = dlg_device.GetValue()
+        if tracker_id:
+            trck_connection = TrackerConnection(tracker_id, None, 'connect')
+            if trck_connection[0]:
+                dlg_ip = dlg.SetRobotIP()
+                if dlg_ip.ShowModal() == ID_OK:
+                    robot_IP = dlg_ip.GetValue()
+                    trck_init = trck_connection
+                    trck_init_robot = ElfinRobot(robot_IP)
+
+    return [trck_init, trck_init_robot, tracker_id]
 def DebugTrackerRandom(tracker_id):
     trck_init = True
@@ -294,10 +327,10 @@ def PlhSerialConnection(tracker_id):
         except:
             lib_mode = 'error'
             trck_init = None
-            print('Could not connect to default tracker.')
+            print('Could not connect to Polhemus tracker.')
     else:
         lib_mode = None
-        print('Could not connect to default tracker.')
+        print('Could not connect to Polhemus tracker.')
     return trck_init
@@ -335,7 +368,6 @@ def PlhUSBConnection(tracker_id):
     return trck_init
-
 def DisconnectTracker(tracker_id, trck_init):
     """
     Disconnect current spatial tracker
@@ -355,6 +387,11 @@ def DisconnectTracker(tracker_id, trck_init):
                 trck_init = False
                 lib_mode = 'serial'
                 print('Tracker disconnected.')
+            elif tracker_id == const.ROBOT:
+                trck_init[0].Close()
+                trck_init = False
+                lib_mode = 'wrapper'
+                print('Tracker disconnected.')
             else:
                 trck_init.Close()
                 trck_init = False
@@ -633,7 +633,7 @@ class ComputeTractsThreadSingleBlock(threading.Thread):
         # print('ComputeTractsThread: event {}'.format(self.event.is_set()))
         while not self.event.is_set():
             try:
-                coord, m_img, m_img_flip = self.coord_queue.get_nowait()
+                coord, [coord_raw, markers_flag], m_img, m_img_flip = self.coord_queue.get_nowait()
                 # translate the coordinate along the normal vector of the object/coil
                 coord_offset = m_img_flip[:3, -1] - offset * m_img_flip[:3, 2]
@@ -956,10 +956,10 @@ class Viewer(wx.Panel):
             if thrdist and thrcoordx and thrcoordy and thrcoordz:
                 self.dummy_coil_actor.GetProperty().SetDiffuseColor(vtk_colors.GetColor3d('Green'))
-                Publisher.sendMessage('Coil at target', state=True)
+                wx.CallAfter(Publisher.sendMessage, 'Coil at target', state=True)
             else:
                 self.dummy_coil_actor.GetProperty().SetDiffuseColor(vtk_colors.GetColor3d('DarkOrange'))
-                Publisher.sendMessage('Coil at target', state=False)
+                wx.CallAfter(Publisher.sendMessage, 'Coil at target', state=False)
             self.arrow_actor_list = arrow_roll_x1, arrow_roll_x2, arrow_yaw_z1, arrow_yaw_z2, \
                                     arrow_pitch_y1, arrow_pitch_y2
@@ -48,6 +48,7 @@ from wx.lib import masked
 from wx.lib.agw import floatspin
 from wx.lib.wordwrap import wordwrap
 from invesalius.pubsub import pub as Publisher
+import csv
 try:
     from wx.adv import AboutDialogInfo, AboutBox
@@ -56,6 +57,7 @@ except ImportError:
 import invesalius.constants as const
 import invesalius.data.coordinates as dco
+import invesalius.data.transformations as tr
 import invesalius.gui.widgets.gradient as grad
 import invesalius.session as ses
 import invesalius.utils as utils
@@ -873,6 +875,7 @@ def ShowNavigationTrackerWarning(trck_id, lib_mode):
             const.POLARIS: 'NDI Polaris',
             const.POLARISP4: 'NDI Polaris P4',
             const.OPTITRACK: 'Optitrack',
+            const.ROBOT: 'Robotic navigation',
             const.DEBUGTRACKRANDOM: 'Debug tracker device (random)',
             const.DEBUGTRACKAPPROACH: 'Debug tracker device (approach)'}
@@ -3309,7 +3312,6 @@ class ObjectCalibrationDialog(wx.Dialog):
         self.pedal_connection = pedal_connection
         self.trk_init, self.tracker_id = tracker.GetTrackerInfo()
-
         self.obj_ref_id = 2
         self.obj_name = None
         self.polydata = None
@@ -3594,8 +3596,14 @@ class ObjectCalibrationDialog(wx.Dialog):
         #      and not change the function to the point of potentially breaking it.)
         #
         if self.obj_ref_id and fiducial_index == 4:
-            coord = coord_raw[self.obj_ref_id, :]
-            coord[2] = -coord[2]
+            if self.tracker_id == const.ROBOT:
+                trck_init_robot = self.trk_init[1][0]
+                coord = trck_init_robot.Run()
+            else:
+                coord = coord_raw[self.obj_ref_id, :]
+        else:
+            coord = coord_raw[0, :]
+        coord[2] = -coord[2]
         if fiducial_index == 3:
             coord = np.zeros([6,])
@@ -4301,6 +4309,312 @@ class SetOptitrackconfigs(wx.Dialog):
         return fn_cal, fn_userprofile
+class SetTrackerDeviceToRobot(wx.Dialog):
+    """
+    Robot navigation requires a tracker device to tracker the head position and the object (coil) position.
+    A dialog pops up showing a combobox with all trackers but debugs and the robot itself (const.TRACKERS[:-3])
+    """
+    def __init__(self, title=_("Setting tracker device:")):
+        wx.Dialog.__init__(self, wx.GetApp().GetTopWindow(), -1, title, size=wx.Size(1000, 200),
+                           style=wx.DEFAULT_DIALOG_STYLE|wx.FRAME_FLOAT_ON_PARENT|wx.STAY_ON_TOP|wx.RESIZE_BORDER)
+        self.tracker_id = const.DEFAULT_TRACKER
+        self._init_gui()
+
+    def _init_gui(self):
+        # ComboBox for spatial tracker device selection
+        tooltip = wx.ToolTip(_("Choose the tracking device"))
+        tracker_options = [_("Select tracker:")] + const.TRACKERS[:-3]
+        choice_trck = wx.ComboBox(self, -1, "",
+                                  choices=tracker_options, style=wx.CB_DROPDOWN | wx.CB_READONLY)
+        choice_trck.SetToolTip(tooltip)
+        choice_trck.SetSelection(const.DEFAULT_TRACKER)
+        choice_trck.Bind(wx.EVT_COMBOBOX, partial(self.OnChoiceTracker, ctrl=choice_trck))
+
+        btn_ok = wx.Button(self, wx.ID_OK)
+        btn_ok.SetHelpText("")
+        btn_ok.SetDefault()
+
+        btn_cancel = wx.Button(self, wx.ID_CANCEL)
+        btn_cancel.SetHelpText("")
+
+        btnsizer = wx.StdDialogButtonSizer()
+        btnsizer.AddButton(btn_ok)
+        btnsizer.AddButton(btn_cancel)
+        btnsizer.Realize()
+
+        main_sizer = wx.BoxSizer(wx.VERTICAL)
+
+        main_sizer.Add((5, 5))
+        main_sizer.Add(choice_trck, 1, wx.EXPAND|wx.LEFT|wx.RIGHT, 5)
+        main_sizer.Add((15, 15))
+        main_sizer.Add(btnsizer, 0, wx.EXPAND)
+        main_sizer.Add((5, 5))
+
+        self.SetSizer(main_sizer)
+        main_sizer.Fit(self)
+
+        self.CenterOnParent()
+
+    def OnChoiceTracker(self, evt, ctrl):
+        choice = evt.GetSelection()
+        self.tracker_id = choice
+
+    def GetValue(self):
+        return self.tracker_id
+
+class SetRobotIP(wx.Dialog):
+    def __init__(self, title=_("Setting Robot IP")):
+        wx.Dialog.__init__(self, wx.GetApp().GetTopWindow(), -1, title, size=wx.Size(1000, 200),
+                           style=wx.DEFAULT_DIALOG_STYLE|wx.FRAME_FLOAT_ON_PARENT|wx.STAY_ON_TOP|wx.RESIZE_BORDER)
+        self.robot_ip = None
+        self._init_gui()
+
+    def _init_gui(self):
+        # ComboBox for spatial tracker device selection
+        tooltip = wx.ToolTip(_("Choose or type the robot IP"))
+        robot_ip_options = [_("Select robot IP:")] + const.ROBOT_ElFIN_IP
+        choice_IP = wx.ComboBox(self, -1, "",
+                                  choices=robot_ip_options, style=wx.CB_DROPDOWN | wx.TE_PROCESS_ENTER)
+        choice_IP.SetToolTip(tooltip)
+        choice_IP.SetSelection(const.DEFAULT_TRACKER)
+        choice_IP.Bind(wx.EVT_COMBOBOX, partial(self.OnChoiceIP, ctrl=choice_IP))
+        choice_IP.Bind(wx.EVT_TEXT, partial(self.OnTxt_Ent, ctrl=choice_IP))
+
+        btn_ok = wx.Button(self, wx.ID_OK)
+        btn_ok.SetHelpText("")
+        btn_ok.SetDefault()
+
+        btn_cancel = wx.Button(self, wx.ID_CANCEL)
+        btn_cancel.SetHelpText("")
+
+        btnsizer = wx.StdDialogButtonSizer()
+        btnsizer.AddButton(btn_ok)
+        btnsizer.AddButton(btn_cancel)
+        btnsizer.Realize()
+
+        main_sizer = wx.BoxSizer(wx.VERTICAL)
+
+        main_sizer.Add((5, 5))
+        main_sizer.Add(choice_IP, 1, wx.EXPAND|wx.LEFT|wx.RIGHT, 5)
+        main_sizer.Add((15, 15))
+        main_sizer.Add(btnsizer, 0, wx.EXPAND)
+        main_sizer.Add((5, 5))
+
+        self.SetSizer(main_sizer)
+        main_sizer.Fit(self)
+
+        self.CenterOnParent()
+
+    def OnTxt_Ent(self, evt, ctrl):
+        self.robot_ip = str(ctrl.GetValue())
+
+    def OnChoiceIP(self, evt, ctrl):
+        self.robot_ip = ctrl.GetStringSelection()
+
+    def GetValue(self):
+        return self.robot_ip
+
+class CreateTransformationMatrixRobot(wx.Dialog):
+    def __init__(self, tracker, title=_("Create transformation matrix to robot space")):
+        wx.Dialog.__init__(self, wx.GetApp().GetTopWindow(), -1, title, #size=wx.Size(1000, 200),
+                           style=wx.DEFAULT_DIALOG_STYLE|wx.FRAME_FLOAT_ON_PARENT|wx.STAY_ON_TOP|wx.RESIZE_BORDER)
+        '''
+        M_robot_2_tracker is created by an affine transformation. Robot TCP should be calibrated to the center of the tracker marker
+        '''
+        #TODO: make aboutbox
+        self.tracker_coord = []
+        self.tracker_angles = []
+        self.robot_coord = []
+        self.robot_angles = []
+
+        self.tracker = tracker
+
+        self.timer = wx.Timer(self)
+        self.Bind(wx.EVT_TIMER, self.OnUpdate, self.timer)
+
+        self._init_gui()
+
+    def _init_gui(self):
+        # Buttons to acquire and remove points
+        txt_acquisition = wx.StaticText(self, -1, _('Poses acquisition for robot registration:'))
+
+        btn_create_point = wx.Button(self, -1, label=_('Single'))
+        btn_create_point.Bind(wx.EVT_BUTTON, self.OnCreatePoint)
+
+        btn_cont_point = wx.ToggleButton(self, -1, label=_('Continuous'))
+        btn_cont_point.Bind(wx.EVT_TOGGLEBUTTON, partial(self.OnContinuousAcquisition, btn=btn_cont_point))
+        self.btn_cont_point = btn_cont_point
+
+        txt_number = wx.StaticText(self, -1, _('0'))
+        txt_recorded = wx.StaticText(self, -1, _('Poses recorded'))
+        self.txt_number = txt_number
+
+        btn_reset = wx.Button(self, -1, label=_('Reset'))
+        btn_reset.Bind(wx.EVT_BUTTON, self.OnReset)
+
+        btn_apply_reg = wx.Button(self, -1, label=_('Apply'))
+        btn_apply_reg.Bind(wx.EVT_BUTTON, self.OnApply)
+        btn_apply_reg.Enable(False)
+        self.btn_apply_reg = btn_apply_reg
+
+        # Buttons to save and load
+        txt_file = wx.StaticText(self, -1, _('Registration file'))
+
+        btn_save = wx.Button(self, -1, label=_('Save'), size=wx.Size(65, 23))
+        btn_save.Bind(wx.EVT_BUTTON, self.OnSaveReg)
+        btn_save.Enable(False)
+        self.btn_save = btn_save
+
+        btn_load = wx.Button(self, -1, label=_('Load'), size=wx.Size(65, 23))
+        btn_load.Bind(wx.EVT_BUTTON, self.OnLoadReg)
+
+        # Create a horizontal sizers
+        border = 1
+        acquisition = wx.BoxSizer(wx.HORIZONTAL)
+        acquisition.AddMany([(btn_create_point, 1, wx.EXPAND | wx.GROW | wx.TOP | wx.RIGHT | wx.LEFT, border),
+                             (btn_cont_point, 1, wx.ALL | wx.EXPAND | wx.GROW, border)])
+
+        txt_pose = wx.BoxSizer(wx.HORIZONTAL)
+        txt_pose.AddMany([(txt_number, 1,  wx.LEFT, 50),
+                          (txt_recorded, 1, wx.LEFT, border)])
+
+        apply_reset = wx.BoxSizer(wx.HORIZONTAL)
+        apply_reset.AddMany([(btn_reset, 1, wx.EXPAND | wx.GROW | wx.TOP | wx.RIGHT | wx.LEFT, border),
+                             (btn_apply_reg, 1, wx.ALL | wx.EXPAND | wx.GROW, border)])
+
+        save_load = wx.BoxSizer(wx.HORIZONTAL)
+        save_load.AddMany([(btn_save, 1, wx.EXPAND | wx.GROW | wx.TOP | wx.RIGHT | wx.LEFT, border),
+                           (btn_load, 1, wx.ALL | wx.EXPAND | wx.GROW, border)])
+
+        btn_ok = wx.Button(self, wx.ID_OK)
+        btn_ok.SetHelpText("")
+        btn_ok.SetDefault()
+        btn_ok.Enable(False)
+        self.btn_ok = btn_ok
+
+        btn_cancel = wx.Button(self, wx.ID_CANCEL)
+        btn_cancel.SetHelpText("")
+
+        btnsizer = wx.StdDialogButtonSizer()
+        btnsizer.AddButton(btn_ok)
+        btnsizer.AddButton(btn_cancel)
+        btnsizer.Realize()
+
+        # Add line sizers into main sizer
+        border = 10
+        border_last = 10
+        main_sizer = wx.BoxSizer(wx.VERTICAL)
+        main_sizer.Add(wx.StaticLine(self, -1), 0, wx.EXPAND | wx.TOP | wx.BOTTOM, border)
+        main_sizer.Add(txt_acquisition, 0, wx.BOTTOM |  wx.LEFT | wx.RIGHT | wx.ALIGN_CENTER_HORIZONTAL , border)
+        main_sizer.Add(acquisition, 0, wx.GROW | wx.EXPAND | wx.LEFT | wx.RIGHT | wx.BOTTOM, border)
+        main_sizer.Add(txt_pose, 0,  wx.ALIGN_CENTER_HORIZONTAL | wx.TOP | wx.BOTTOM, border)
+        main_sizer.Add(apply_reset, 0, wx.GROW | wx.EXPAND | wx.LEFT | wx.RIGHT , border_last)
+        main_sizer.Add(wx.StaticLine(self, -1), 0, wx.EXPAND | wx.TOP | wx.BOTTOM, border)
+        main_sizer.Add(txt_file, 0, wx.GROW | wx.EXPAND | wx.LEFT | wx.RIGHT | wx.BOTTOM, border/2)
+        main_sizer.Add(save_load, 0, wx.GROW | wx.EXPAND | wx.LEFT | wx.RIGHT | wx.BOTTOM, border)
+        main_sizer.Add(wx.StaticLine(self, -1), 0, wx.EXPAND | wx.TOP | wx.BOTTOM, border)
+        main_sizer.Add(btnsizer, 0, wx.GROW | wx.EXPAND | wx.LEFT | wx.RIGHT | wx.BOTTOM, border)
+        main_sizer.Fit(self)
+
+        self.SetSizer(main_sizer)
+        self.Update()
+        main_sizer.Fit(self)
+
+        self.CenterOnParent()
+
+    def affine_correg(self, tracker, robot):
+        m_change = tr.affine_matrix_from_points(robot[:].T, tracker[:].T,
+                                                shear=False, scale=False, usesvd=False)
+        return m_change
+
+    def OnContinuousAcquisition(self, evt=None, btn=None):
+        value = btn.GetValue()
+        if value:
+            self.timer.Start(500)
+        else:
+            self.timer.Stop()
+
+    def OnUpdate(self, evt):
+        self.OnCreatePoint(evt=None)
+
+    def OnCreatePoint(self, evt):
+        coord_raw, markers_flag = self.tracker.TrackerCoordinates.GetCoordinates()
+        #robot thread is not initialized yet
+        coord_raw_robot = self.tracker.trk_init[1][0].Run()
+        coord_raw_tracker_obj = coord_raw[3]
+
+        if markers_flag[2]:
+            self.tracker_coord.append(coord_raw_tracker_obj[:3])
+            self.tracker_angles.append(coord_raw_tracker_obj[3:])
+            self.robot_coord.append(coord_raw_robot[:3])
+            self.robot_angles.append(coord_raw_robot[3:])
+            self.txt_number.SetLabel(str(int(self.txt_number.GetLabel())+1))
+        else:
+            print('Cannot detect the coil markers, pls try again')
+
+        if len(self.tracker_coord) >= 3:
+            self.btn_apply_reg.Enable(True)
+
+    def OnReset(self, evt):
+        if self.btn_cont_point:
+            self.btn_cont_point.SetValue(False)
+            self.OnContinuousAcquisition(evt=None, btn=self.btn_cont_point)
+
+        self.tracker_coord = []
+        self.tracker_angles = []
+        self.robot_coord = []
+        self.robot_angles = []
+        self.M_tracker_2_robot = []
+        self.txt_number.SetLabel('0')
+
+        self.btn_apply_reg.Enable(False)
+        self.btn_save.Enable(False)
+        self.btn_ok.Enable(False)
+
+    def OnApply(self, evt):
+        if self.btn_cont_point:
+            self.btn_cont_point.SetValue(False)
+            self.OnContinuousAcquisition(evt=None, btn=self.btn_cont_point)
+
+        tracker_coord = np.array(self.tracker_coord)
+        robot_coord = np.array(self.robot_coord)
+
+        M_robot_2_tracker = self.affine_correg(tracker_coord, robot_coord)
+        M_tracker_2_robot = tr.inverse_matrix(M_robot_2_tracker)
+        self.M_tracker_2_robot = M_tracker_2_robot
+
+        self.btn_save.Enable(True)
+        self.btn_ok.Enable(True)
+
+        #TODO: make a colored circle to sinalize that the transformation was made (green) (red if not)
+
+    def OnSaveReg(self, evt):
+        filename = ShowLoadSaveDialog(message=_(u"Save robot transformation file as..."),
+                                          wildcard=_("Robot transformation files (*.rbtf)|*.rbtf"),
+                                          style=wx.FD_SAVE | wx.FD_OVERWRITE_PROMPT,
+                                          default_filename="robottransform.rbtf", save_ext="rbtf")
+
+        if filename:
+            if self.M_tracker_2_robot is not None:
+                with open(filename, 'w', newline='') as file:
+                    writer = csv.writer(file, delimiter='\t')
+                    writer.writerows(self.M_tracker_2_robot)
+
+    def OnLoadReg(self, evt):
+        filename = ShowLoadSaveDialog(message=_(u"Load robot transformation"),
+                                          wildcard=_("Robot transformation files (*.rbtf)|*.rbtf"))
+        if filename:
+            with open(filename, 'r') as file:
+                reader = csv.reader(file, delimiter='\t')
+                content = [row for row in reader]
+
+            self.M_tracker_2_robot = np.vstack(list(np.float_(content)))
+            print("Matrix tracker to robot:", self.M_tracker_2_robot)
+            self.btn_ok.Enable(True)
+
+    def GetValue(self):
+        return self.M_tracker_2_robot
+
 class SetNDIconfigs(wx.Dialog):
     def __init__(self, title=_("Setting NDI polaris configs:")):
         wx.Dialog.__init__(self, wx.GetApp().GetTopWindow(), -1, title, size=wx.Size(1000, 200),
@@ -4329,16 +4643,19 @@ class SetNDIconfigs(wx.Dialog):
         return port_list, port_selec
     def _init_gui(self):
-        self.com_ports = wx.ComboBox(self, -1, style=wx.CB_DROPDOWN|wx.CB_READONLY)
+        com_ports = wx.ComboBox(self, -1, style=wx.CB_DROPDOWN|wx.CB_READONLY)
+        com_ports.Bind(wx.EVT_COMBOBOX, partial(self.OnChoicePort, ctrl=com_ports))
         row_com = wx.BoxSizer(wx.VERTICAL)
         row_com.Add(wx.StaticText(self, wx.ID_ANY, "Select the COM port"), 0, wx.TOP|wx.RIGHT,5)
-        row_com.Add(self.com_ports, 0, wx.EXPAND)
+        row_com.Add(com_ports, 0, wx.EXPAND)
         port_list, port_selec = self.serial_ports()
-        self.com_ports.Append(port_list)
+        com_ports.Append(port_list)
         if port_selec:
-            self.com_ports.SetSelection(port_selec[0])
+            com_ports.SetSelection(port_selec[0])
+
+        self.com_ports = com_ports
         session = ses.Session()
         last_ndi_probe_marker = session.get('paths', 'last_ndi_probe_marker', '')
@@ -4374,6 +4691,8 @@ class SetNDIconfigs(wx.Dialog):
         btn_ok = wx.Button(self, wx.ID_OK)
         btn_ok.SetHelpText("")
         btn_ok.SetDefault()
+        btn_ok.Enable(False)
+        self.btn_ok = btn_ok
         btn_cancel = wx.Button(self, wx.ID_CANCEL)
         btn_cancel.SetHelpText("")
@@ -4402,6 +4721,9 @@ class SetNDIconfigs(wx.Dialog):
         self.CenterOnParent()
+    def OnChoicePort(self, evt, ctrl):
+        self.btn_ok.Enable(True)
+
     def GetValue(self):
         fn_probe = self.dir_probe.GetPath().encode(const.FS_ENCODE)
         fn_ref = self.dir_ref.GetPath().encode(const.FS_ENCODE)
@@ -20,7 +20,10 @@
 import dataclasses
 from functools import partial
 import itertools
+import csv
+import queue
 import time
+import threading
 import nibabel as nb
 import numpy as np
@@ -29,6 +32,13 @@ try:
     has_trekker = True
 except ImportError:
     has_trekker = False
+try:
+    import invesalius.data.elfin as elfin
+    import invesalius.data.elfin_processing as elfin_process
+    has_robot = True
+except ImportError:
+    has_robot = False
+
 import wx
 try:
@@ -51,14 +61,17 @@ import invesalius.data.slice_ as sl
 import invesalius.data.tractography as dti
 import invesalius.data.record_coords as rec
 import invesalius.data.vtk_utils as vtk_utils
+import invesalius.data.bases as db
 import invesalius.gui.dialogs as dlg
 import invesalius.project as prj
 import invesalius.session as ses
+
 from invesalius import utils
 from invesalius.gui import utils as gui_utils
 from invesalius.navigation.icp import ICP
 from invesalius.navigation.navigation import Navigation
 from invesalius.navigation.tracker import Tracker
+from invesalius.navigation.robot import Robot
 HAS_PEDAL_CONNECTION = True
 try:
@@ -184,7 +197,7 @@ class InnerFoldPanel(wx.Panel):
         # Fold 3 - Markers panel
         item = fold_panel.AddFoldPanel(_("Markers"), collapsed=True)
-        mtw = MarkersPanel(item)
+        mtw = MarkersPanel(item, tracker)
         fold_panel.ApplyCaptionStyle(item, style)
         fold_panel.AddFoldPanelWindow(item, mtw, spacing= 0,
@@ -342,6 +355,7 @@ class NeuronavigationPanel(wx.Panel):
         )
         self.icp = ICP()
         self.tracker = tracker
+        self.robot = Robot(tracker)
         self.nav_status = False
         self.tracker_fiducial_being_set = None
@@ -641,6 +655,8 @@ class NeuronavigationPanel(wx.Panel):
         self.checkbox_icp.SetValue(False)
     def OnDisconnectTracker(self):
+        if self.tracker.tracker_id == const.ROBOT:
+            self.robot.StopRobotThreadNavigation()
         self.tracker.DisconnectTracker()
         self.ResetICP()
         self.tracker.UpdateUI(self.select_tracker_elem, self.numctrls_fiducial[3:6], self.txtctrl_fre)
@@ -658,6 +674,9 @@ class NeuronavigationPanel(wx.Panel):
             choice = None
         self.tracker.SetTracker(choice)
+        if self.tracker.tracker_id == const.ROBOT:
+            self.tracker.ConnectToRobot(self.navigation, self.tracker, self.robot)
+
         self.ResetICP()
         self.tracker.UpdateUI(ctrl, self.numctrls_fiducial[3:6], self.txtctrl_fre)
@@ -740,6 +759,9 @@ class NeuronavigationPanel(wx.Panel):
         choice_ref = self.choice_ref
         self.navigation.StopNavigation()
+        if self.tracker.tracker_id == const.ROBOT:
+            Publisher.sendMessage('Robot target matrix', robot_tracker_flag=False,
+                                  m_change_robot_to_head=None)
         # Enable all navigation buttons
         choice_ref.Enable(True)
@@ -842,6 +864,7 @@ class NeuronavigationPanel(wx.Panel):
         )
         self.tracker.__init__()
         self.icp.__init__()
+        self.robot.__init__(self.tracker)
 class ObjectRegistrationPanel(wx.Panel):
@@ -900,16 +923,16 @@ class ObjectRegistrationPanel(wx.Panel):
         # Change angles threshold
         text_angles = wx.StaticText(self, -1, _("Angle threshold [degrees]:"))
-        spin_size_angles = wx.SpinCtrl(self, -1, "", size=wx.Size(50, 23))
-        spin_size_angles.SetRange(1, 99)
+        spin_size_angles = wx.SpinCtrlDouble(self, -1, "", size=wx.Size(50, 23))
+        spin_size_angles.SetRange(0.1, 99)
         spin_size_angles.SetValue(const.COIL_ANGLES_THRESHOLD)
         spin_size_angles.Bind(wx.EVT_TEXT, partial(self.OnSelectAngleThreshold, ctrl=spin_size_angles))
         spin_size_angles.Bind(wx.EVT_SPINCTRL, partial(self.OnSelectAngleThreshold, ctrl=spin_size_angles))
         # Change dist threshold
         text_dist = wx.StaticText(self, -1, _("Distance threshold [mm]:"))
-        spin_size_dist = wx.SpinCtrl(self, -1, "", size=wx.Size(50, 23))
-        spin_size_dist.SetRange(1, 99)
+        spin_size_dist = wx.SpinCtrlDouble(self, -1, "", size=wx.Size(50, 23))
+        spin_size_dist.SetRange(0.1, 99)
         spin_size_dist.SetValue(const.COIL_ANGLES_THRESHOLD)
         spin_size_dist.Bind(wx.EVT_TEXT, partial(self.OnSelectDistThreshold, ctrl=spin_size_dist))
         spin_size_dist.Bind(wx.EVT_SPINCTRL, partial(self.OnSelectDistThreshold, ctrl=spin_size_dist))
@@ -1201,7 +1224,20 @@ class MarkersPanel(wx.Panel):
                 if field.type is bool:
                     setattr(self, field.name, str_val=='True')
-    def __init__(self, parent):
+    @dataclasses.dataclass
+    class Robot_Marker:
+        """Class for storing robot target."""
+        m_robot_target : list = None
+
+        @property
+        def robot_target_matrix(self):
+            return self.m_robot_target
+
+        @robot_target_matrix.setter
+        def robot_target_matrix(self, new_m_robot_target):
+            self.m_robot_target = new_m_robot_target
+
+    def __init__(self, parent, tracker):
         wx.Panel.__init__(self, parent)
         try:
             default_colour = wx.SystemSettings.GetColour(wx.SYS_COLOUR_MENUBAR)
@@ -1211,6 +1247,8 @@ class MarkersPanel(wx.Panel):
         self.SetAutoLayout(1)
+        self.tracker = tracker
+
         self.__bind_events()
         self.session = ses.Session()
@@ -1218,8 +1256,11 @@ class MarkersPanel(wx.Panel):
         self.current_coord = 0, 0, 0, 0, 0, 0
         self.current_angle = 0, 0, 0
         self.current_seed = 0, 0, 0
+        self.current_robot_target_matrix = [None] * 9
         self.markers = []
+        self.robot_markers = []
         self.nav_status = False
+        self.raw_target_robot = None, None
         self.marker_colour = const.MARKER_COLOUR
         self.marker_size = const.MARKER_SIZE
@@ -1317,6 +1358,7 @@ class MarkersPanel(wx.Panel):
         Publisher.subscribe(self.UpdateNavigationStatus, 'Navigation status')
         Publisher.subscribe(self.UpdateSeedCoordinates, 'Update tracts')
         Publisher.subscribe(self.OnChangeCurrentSession, 'Current session changed')
+        Publisher.subscribe(self.UpdateRobotCoordinates, 'Update raw coordinates')
     def __find_target_marker(self):
         """
@@ -1350,6 +1392,7 @@ class MarkersPanel(wx.Panel):
         """
         for i in reversed(index):
             del self.markers[i]
+            del self.robot_markers[i]
             self.lc.DeleteItem(i)
             for n in range(0, self.lc.GetItemCount()):
                 self.lc.SetItem(n, 0, str(n + 1))
@@ -1402,6 +1445,9 @@ class MarkersPanel(wx.Panel):
     def UpdateSeedCoordinates(self, root=None, affine_vtk=None, coord_offset=(0, 0, 0)):
         self.current_seed = coord_offset
+    def UpdateRobotCoordinates(self, coordinates_raw, markers_flag):
+        self.raw_target_robot = coordinates_raw[1], coordinates_raw[2]
+
     def OnMouseRightDown(self, evt):
         # TODO: Enable the "Set as target" only when target is created with registered object
         menu_id = wx.Menu()
@@ -1412,6 +1458,16 @@ class MarkersPanel(wx.Panel):
         menu_id.AppendSeparator()
         target_menu = menu_id.Append(1, _('Set as target'))
         menu_id.Bind(wx.EVT_MENU, self.OnMenuSetTarget, target_menu)
+        menu_id.AppendSeparator()
+        send_target_to_robot = menu_id.Append(3, _('Send target to robot'))
+        menu_id.Bind(wx.EVT_MENU, self.OnMenuSendTargetToRobot, send_target_to_robot)
+
+        # Enable "Send target to robot" button only if tracker is robot, if navigation is on and if target is not none
+        m_target_robot = np.array([self.robot_markers[self.lc.GetFocusedItem()].robot_target_matrix])
+        if self.tracker.tracker_id == const.ROBOT and self.nav_status and m_target_robot.any():
+            send_target_to_robot.Enable(True)
+        else:
+            send_target_to_robot.Enable(False)
         # TODO: Create the remove target option so the user can disable the target without removing the marker
         # target_menu_rem = menu_id.Append(3, _('Remove target'))
         # menu_id.Bind(wx.EVT_MENU, self.OnMenuRemoveTarget, target_menu_rem)
@@ -1462,6 +1518,15 @@ class MarkersPanel(wx.Panel):
             Publisher.sendMessage('Set new color', index=index, color=color_new)
+    def OnMenuSendTargetToRobot(self, evt):
+        if isinstance(evt, int):
+           self.lc.Focus(evt)
+
+        m_target_robot = self.robot_markers[self.lc.GetFocusedItem()].robot_target_matrix
+
+        Publisher.sendMessage('Reset robot process')
+        Publisher.sendMessage('Robot target matrix', robot_tracker_flag=True, m_change_robot_to_head=m_target_robot)
+
     def OnDeleteAllMarkers(self, evt=None):
         if evt is not None:
             result = dlg.ShowConfirmationDialog(msg=_("Remove all markers? Cannot be undone."))
@@ -1474,6 +1539,7 @@ class MarkersPanel(wx.Panel):
                 wx.MessageBox(_("Target deleted."), _("InVesalius 3"))
         self.markers = []
+        self.robot_markers = []
         Publisher.sendMessage('Remove all markers', indexes=self.lc.GetItemCount())
         self.lc.DeleteAllItems()
         Publisher.sendMessage('Stop Blink Marker', index='DeleteAll')
@@ -1500,6 +1566,8 @@ class MarkersPanel(wx.Panel):
         if index:
             if self.__find_target_marker() in index:
                 Publisher.sendMessage('Disable or enable coil tracker', status=False)
+                Publisher.sendMessage('Robot target matrix', robot_tracker_flag=False,
+                                      m_change_robot_to_head=None)
                 wx.MessageBox(_("Target deleted."), _("InVesalius 3"))
             self.__delete_multiple_markers(index)
@@ -1603,12 +1671,21 @@ class MarkersPanel(wx.Panel):
         new_marker.seed = seed or self.current_seed
         new_marker.session_id = session_id or self.current_session
+        if self.tracker.tracker_id == const.ROBOT and self.nav_status:
+            self.current_robot_target_matrix = db.compute_robot_to_head_matrix(self.raw_target_robot)
+        else:
+            self.current_robot_target_matrix = [None] * 9
+
+        new_robot_marker = self.Robot_Marker()
+        new_robot_marker.robot_target_matrix = self.current_robot_target_matrix
+
         # Note that ball_id is zero-based, so we assign it len(self.markers) before the new marker is added
         Publisher.sendMessage('Add marker', ball_id=len(self.markers),
                                             size=new_marker.size,
                                             colour=new_marker.colour,
                                             coord=new_marker.coord[:3])
         self.markers.append(new_marker)
+        self.robot_markers.append(new_robot_marker)
         # Add item to list control in panel
         num_items = self.lc.GetItemCount()
@@ -82,7 +82,7 @@ class UpdateNavigationScene(threading.Thread):
         threading.Thread.__init__(self, name='UpdateScene')
         self.serial_port_enabled, self.view_tracts, self.peel_loaded = vis_components
-        self.coord_queue, self.serial_port_queue, self.tracts_queue, self.icp_queue = vis_queues
+        self.coord_queue, self.serial_port_queue, self.tracts_queue, self.icp_queue, self.robot_target_queue = vis_queues
         self.sle = sle
         self.event = event
@@ -91,7 +91,7 @@ class UpdateNavigationScene(threading.Thread):
         while not self.event.is_set():
             got_coords = False
             try:
-                coord, m_img, view_obj = self.coord_queue.get_nowait()
+                coord, [coordinates_raw, markers_flag], m_img, view_obj = self.coord_queue.get_nowait()
                 got_coords = True
                 # print('UpdateScene: get {}'.format(count))
@@ -116,6 +116,7 @@ class UpdateNavigationScene(threading.Thread):
                 # see the red cross in the position of the offset marker
                 wx.CallAfter(Publisher.sendMessage, 'Update slices position', position=coord[:3])
                 wx.CallAfter(Publisher.sendMessage, 'Set cross focal point', position=coord)
+                wx.CallAfter(Publisher.sendMessage, 'Update raw coordinates', coordinates_raw=coordinates_raw, markers_flag=markers_flag)
                 wx.CallAfter(Publisher.sendMessage, 'Update slice viewer')
                 if view_obj:
@@ -146,6 +147,8 @@ class Navigation():
         self.event = threading.Event()
         self.coord_queue = QueueCustom(maxsize=1)
         self.icp_queue = QueueCustom(maxsize=1)
+        self.object_at_target_queue = QueueCustom(maxsize=1)
+        self.robot_target_queue = QueueCustom(maxsize=1)
         # self.visualization_queue = QueueCustom(maxsize=1)
         self.serial_port_queue = QueueCustom(maxsize=1)
         self.coord_tracts_queue = QueueCustom(maxsize=1)
@@ -243,7 +246,7 @@ class Navigation():
             self.event.clear()
         vis_components = [self.serial_port_in_use, self.view_tracts, self.peel_loaded]
-        vis_queues = [self.coord_queue, self.serial_port_queue, self.tracts_queue, self.icp_queue]
+        vis_queues = [self.coord_queue, self.serial_port_queue, self.tracts_queue, self.icp_queue, self.robot_target_queue]
         Publisher.sendMessage("Navigation status", nav_status=True, vis_status=vis_components)
@@ -277,7 +280,7 @@ class Navigation():
                 obj_data = db.object_registration(obj_fiducials, obj_orients, coord_raw, m_change)
                 coreg_data.extend(obj_data)
-                queues = [self.coord_queue, self.coord_tracts_queue, self.icp_queue]
+                queues = [self.coord_queue, self.coord_tracts_queue, self.icp_queue, self.object_at_target_queue]
                 jobs_list.append(dcr.CoordinateCorregistrate(self.ref_mode_id, tracker, coreg_data,
                                                                 self.view_tracts, queues,
                                                                 self.event, self.sleep_nav, tracker.tracker_id,
@@ -0,0 +1,281 @@
+#--------------------------------------------------------------------------
+# Software:     InVesalius - Software de Reconstrucao 3D de Imagens Medicas
+# Copyright:    (C) 2001  Centro de Pesquisas Renato Archer
+# Homepage:     http://www.softwarepublico.gov.br
+# Contact:      invesalius@cti.gov.br
+# License:      GNU - GPL 2 (LICENSE.txt/LICENCA.txt)
+#--------------------------------------------------------------------------
+#    Este programa e software livre; voce pode redistribui-lo e/ou
+#    modifica-lo sob os termos da Licenca Publica Geral GNU, conforme
+#    publicada pela Free Software Foundation; de acordo com a versao 2
+#    da Licenca.
+#
+#    Este programa eh distribuido na expectativa de ser util, mas SEM
+#    QUALQUER GARANTIA; sem mesmo a garantia implicita de
+#    COMERCIALIZACAO ou de ADEQUACAO A QUALQUER PROPOSITO EM
+#    PARTICULAR. Consulte a Licenca Publica Geral GNU para obter mais
+#    detalhes.
+#--------------------------------------------------------------------------
+import numpy as np
+import wx
+import queue
+import threading
+
+import invesalius.data.bases as db
+import invesalius.gui.dialogs as dlg
+import invesalius.constants as const
+from invesalius.pubsub import pub as Publisher
+
+try:
+    import invesalius.data.elfin as elfin
+    import invesalius.data.elfin_processing as elfin_process
+    has_robot = True
+except ImportError:
+    has_robot = False
+
+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
+        self.object_at_target_queue = None
+        self.process_tracker = None
+
+        self.thread_robot = None
+
+        self.robotcoordinates = RobotCoordinates()
+
+        self.__bind_events()
+
+    def __bind_events(self):
+        Publisher.subscribe(self.OnSendCoordinates, 'Send coord to robot')
+        Publisher.subscribe(self.OnUpdateRobotTargetMatrix, 'Robot target matrix')
+        Publisher.subscribe(self.OnObjectTarget, 'Coil at target')
+        Publisher.subscribe(self.OnResetProcessTracker, 'Reset robot process')
+
+    def OnRobotConnection(self):
+        if not self.tracker.trk_init[0][0] or not self.tracker.trk_init[1][0]:
+            dlg.ShowNavigationTrackerWarning(self.tracker.tracker_id, self.tracker.trk_init[1])
+            self.tracker.tracker_id = 0
+            self.tracker.tracker_connected = False
+        else:
+            self.tracker.trk_init.append(self.robotcoordinates)
+            self.process_tracker = elfin_process.TrackerProcessing()
+            dlg_correg_robot = dlg.CreateTransformationMatrixRobot(self.tracker)
+            if dlg_correg_robot.ShowModal() == wx.ID_OK:
+                M_tracker_to_robot = dlg_correg_robot.GetValue()
+                db.transform_tracker_to_robot.M_tracker_to_robot = M_tracker_to_robot
+                self.robot_server = self.tracker.trk_init[1][0]
+                self.trk_init = self.tracker.trk_init
+            else:
+                dlg.ShowNavigationTrackerWarning(self.tracker.tracker_id, 'disconnect')
+                self.tracker.trk_init = None
+                self.tracker.tracker_id = 0
+                self.tracker.tracker_connected = False
+
+    def StartRobotThreadNavigation(self, tracker, coord_queue):
+        if tracker.event_robot.is_set():
+            tracker.event_robot.clear()
+        self.thread_robot = ControlRobot(self.trk_init, tracker, self.robotcoordinates,
+                                         [coord_queue, self.robot_target_queue, self.object_at_target_queue],
+                                         self.process_tracker, tracker.event_robot)
+        self.thread_robot.start()
+
+    def StopRobotThreadNavigation(self):
+        self.thread_robot.join()
+        self.OnResetProcessTracker()
+
+    def OnResetProcessTracker(self):
+        self.process_tracker.__init__()
+
+    def OnSendCoordinates(self, coord):
+        self.robot_server.SendCoordinates(coord)
+
+    def OnUpdateRobotTargetMatrix(self, robot_tracker_flag, m_change_robot_to_head):
+        try:
+            self.robot_target_queue.put_nowait([robot_tracker_flag, m_change_robot_to_head])
+        except queue.Full:
+            pass
+
+    def OnObjectTarget(self, state):
+        try:
+            if self.object_at_target_queue:
+                self.object_at_target_queue.put_nowait(state)
+        except queue.Full:
+            pass
+
+    def SetRobotQueues(self, queues):
+        self.robot_target_queue, self.object_at_target_queue = queues
+
+
+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
+
+    def SetRobotCoordinates(self, coord):
+        self.coord = coord
+
+    def GetRobotCoordinates(self):
+        return self.coord
+
+
+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]
+        self.trck_init_tracker = trck_init[0]
+        self.trk_id = trck_init[2]
+        self.tracker = tracker
+        self.robotcoordinates = robotcoordinates
+        self.robot_tracker_flag = False
+        self.objattarget_flag = False
+        self.target_flag = False
+        self.m_change_robot_to_head = None
+        self.coord_inv_old = None
+        self.coord_queue = queues[0]
+        self.robot_target_queue = queues[1]
+        self.object_at_target_queue = queues[2]
+        self.process_tracker = process_tracker
+        self.event_robot = event_robot
+        self.arc_motion_flag = False
+        self.arc_motion_step_flag = None
+        self.target_linear_out = None
+        self.target_linear_in = None
+        self.target_arc = None
+
+    def get_coordinates_from_tracker_devices(self):
+        coord_robot_raw = self.trck_init_robot.Run()
+        coord_robot = np.array(coord_robot_raw)
+        coord_robot[3], coord_robot[5] = coord_robot[5], coord_robot[3]
+        self.robotcoordinates.SetRobotCoordinates(coord_robot)
+
+        coord_raw, markers_flag = self.tracker.TrackerCoordinates.GetCoordinates()
+
+        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"])
+
+        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
+
+                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)
+
+    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]
+        marker_head_flag = markers_flag[1]
+        coord_obj_tracker_in_robot = current_tracker_coordinates_in_robot[2]
+        marker_obj_flag = markers_flag[2]
+
+        if self.robot_tracker_flag:
+            current_head = coord_head_tracker_in_robot
+            if current_head is not None and marker_head_flag:
+                current_head_filtered = self.process_tracker.kalman_filter(current_head)
+                if self.process_tracker.compute_head_move_threshold(current_head_filtered):
+                    new_robot_coordinates = self.process_tracker.compute_head_move_compensation(current_head_filtered,
+                                                                                    self.m_change_robot_to_head)
+                    if self.coord_inv_old is None:
+                       self.coord_inv_old = new_robot_coordinates
+
+                    if np.allclose(np.array(new_robot_coordinates), np.array(current_robot_coordinates), 0, 0.01):
+                        #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:
+                        distance_target = self.process_tracker.correction_distance_calculation_target(new_robot_coordinates, current_robot_coordinates)
+                        self.robot_move_decision(distance_target, new_robot_coordinates, current_robot_coordinates, current_head_filtered)
+                        self.coord_inv_old = new_robot_coordinates
+            else:
+                self.trck_init_robot.StopRobot()
+
+    def run(self):
+        while not self.event_robot.is_set():
+            current_tracker_coordinates_in_robot, current_robot_coordinates, markers_flag = self.get_coordinates_from_tracker_devices()
+
+            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 not self.object_at_target_queue.empty():
+                self.target_flag = self.object_at_target_queue.get_nowait()
+                self.object_at_target_queue.task_done()
+
+            self.robot_control(current_tracker_coordinates_in_robot, current_robot_coordinates, markers_flag)
@@ -24,6 +24,7 @@ import invesalius.constants as const
 import invesalius.data.coordinates as dco
 import invesalius.data.trackers as dt
 import invesalius.gui.dialogs as dlg
+import invesalius.data.coregistration as dcr
 from invesalius.pubsub import pub as Publisher
@@ -34,12 +35,14 @@ class Tracker():
         self.tracker_fiducials = np.full([3, 3], np.nan)
         self.tracker_fiducials_raw = np.zeros((6, 6))
+        self.m_tracker_fiducials_raw = np.zeros((6, 4, 4))
         self.tracker_connected = False
         self.thread_coord = None
         self.event_coord = threading.Event()
+        self.event_robot = threading.Event()
         self.TrackerCoordinates = dco.TrackerCoordinates()
@@ -74,8 +77,11 @@ class Tracker():
                 if self.thread_coord:
                     self.event_coord.set()
+                    self.event_robot.set()
                     self.thread_coord.join()
                     self.event_coord.clear()
+                    self.event_robot.clear()
+
                 Publisher.sendMessage('Update status text in GUI',
                                         label=_("Tracker disconnected"))
@@ -85,6 +91,13 @@ class Tracker():
                                         label=_("Tracker still connected"))
                 print("Tracker still connected!")
+    def ConnectToRobot(self, navigation, tracker, robot):
+        robot.SetRobotQueues([navigation.robot_target_queue, navigation.object_at_target_queue])
+        robot.OnRobotConnection()
+        trk_init_robot = self.trk_init[1][0]
+        if trk_init_robot:
+            robot.StartRobotThreadNavigation(tracker, navigation.coord_queue)
+
     def IsTrackerInitialized(self):
         return self.trk_init and self.tracker_id and self.tracker_connected
@@ -126,6 +139,9 @@ class Tracker():
         self.tracker_fiducials_raw[2 * fiducial_index, :] = coord_raw[0, :]
         self.tracker_fiducials_raw[2 * fiducial_index + 1, :] = coord_raw[1, :]
+        self.m_tracker_fiducials_raw[2 * fiducial_index, :] = dcr.compute_marker_transformation(coord_raw, 0)
+        self.m_tracker_fiducials_raw[2 * fiducial_index + 1, :] = dcr.compute_marker_transformation(coord_raw, 1)
+
         print("Set tracker fiducial {} to coordinates {}.".format(fiducial_index, coord[0:3]))
     def ResetTrackerFiducials(self):
@@ -135,6 +151,13 @@ class Tracker():
     def GetTrackerFiducials(self):
         return self.tracker_fiducials, self.tracker_fiducials_raw
+    def GetMatrixTrackerFiducials(self):
+        m_probe_ref_left = np.linalg.inv(self.m_tracker_fiducials_raw[1]) @ self.m_tracker_fiducials_raw[0]
+        m_probe_ref_right = np.linalg.inv(self.m_tracker_fiducials_raw[3]) @ self.m_tracker_fiducials_raw[2]
+        m_probe_ref_nasion = np.linalg.inv(self.m_tracker_fiducials_raw[5]) @ self.m_tracker_fiducials_raw[4]
+
+        return [m_probe_ref_left, m_probe_ref_right, m_probe_ref_nasion]
+
     def GetTrackerInfo(self):
         return self.trk_init, self.tracker_id
+
 [Marker]
 FacetsCount=1
 SliderControlledXpointsCount=0
@@ -7,20 +8,20 @@ Name=2Ref
 VectorsCount=2
 [Facet1V1]
-EndPos(0,0)=-8.9120676333490003
-EndPos(0,1)=2.3980817331903383e-016
-EndPos(0,2)=-1.2505552149377763e-015
-EndPos(1,0)=8.9120676333490003
-EndPos(1,1)=2.6645352591003756e-017
-EndPos(1,2)=0.
+EndPos(0,0)=-32.862195626831138
+EndPos(0,1)=2.2319079841241168e-003
+EndPos(0,2)=7.6730702825049029e-003
+EndPos(1,0)=32.860817892941867
+EndPos(1,1)=1.862130420993991e-003
+EndPos(1,2)=5.6832171433645459e-003
 [Facet1V2]
-EndPos(0,0)=-8.9951734202602971
-EndPos(0,1)=-12.045839883319603
-EndPos(0,2)=-4.5474735088646413e-016
-EndPos(1,0)=-8.9120676333490003
-EndPos(1,1)=2.3980817331903383e-016
-EndPos(1,2)=-1.2505552149377763e-015
+EndPos(0,0)=-4.6454520609455985
+EndPos(0,1)=24.559311056585091
+EndPos(0,2)=-1.6538883706334069
+EndPos(1,0)=-4.8071411059089124
+EndPos(1,1)=-22.341630540181473
+EndPos(1,2)=-1.654808075526687
 [Tooltip2MarkerXf]
 Scale=1.
@@ -1,39 +0,0 @@
-
-[Marker]
-FacetsCount=1
-SliderControlledXpointsCount=0
-Name=3Coil_big
-
-[Facet1]
-VectorsCount=2
-
-[Facet1V1]
-EndPos(0,0)=-12.130921665071337
-EndPos(0,1)=4.771099944171531e-005
-EndPos(0,2)=-1.6777868612989378e-016
-EndPos(1,0)=12.131083622124752
-EndPos(1,1)=1.0800131569805988e-004
-EndPos(1,2)=-2.4831245547224282e-015
-
-[Facet1V2]
-EndPos(0,0)=-12.143021882072084
-EndPos(0,1)=-16.190735690907974
-EndPos(0,2)=-1.3422294890391503e-015
-EndPos(1,0)=-12.130921665071337
-EndPos(1,1)=4.771099944171531e-005
-EndPos(1,2)=-1.6777868612989378e-016
-
-[Tooltip2MarkerXf]
-Scale=1.
-S0=0.
-R0,0=1.
-R1,0=0.
-R2,0=0.
-S1=0.
-R0,1=0.
-R1,1=1.
-R2,1=0.
-S2=0.
-R0,2=0.
-R1,2=0.
-R2,2=1.
@@ -5,3 +5,4 @@ python-rtmidi==1.4.9
 python-socketio[client]==5.3.0
 requests==2.26.0
 uvicorn[standard]==0.15.0
+opencv-python==4.5.3
	@@ -244,3 +244,48 @@ def object_registration(fiducials, orients, coord_raw, m_change):		@@ -244,3 +244,48 @@ def object_registration(fiducials, orients, coord_raw, m_change):
244	)	244	)
245		245
246	return t_obj_raw, s0_raw, r_s0_raw, s0_dyn, m_obj_raw, r_obj_img	246	return t_obj_raw, s0_raw, r_s0_raw, s0_dyn, m_obj_raw, r_obj_img
		247	+
		248	+def compute_robot_to_head_matrix(raw_target_robot):
		249	+ """
		250	+ :param head: nx6 array of head coordinates from tracking device in robot space
		251	+ :param robot: nx6 array of robot coordinates
		252	+
		253	+ :return: target_robot_matrix: 3x3 array representing change of basis from robot to head in robot system
		254	+ """
		255	+ head, robot = raw_target_robot
		256	+ # compute head target matrix
		257	+ m_head_target = dco.coordinates_to_transformation_matrix(
		258	+ position=head[:3],
		259	+ orientation=head[3:],
		260	+ axes='rzyx',
		261	+ )
		262	+
		263	+ # compute robot target matrix
		264	+ m_robot_target = dco.coordinates_to_transformation_matrix(
		265	+ position=robot[:3],
		266	+ orientation=robot[3:],
		267	+ axes='rzyx',
		268	+ )
		269	+ robot_to_head_matrix = np.linalg.inv(m_head_target) @ m_robot_target
		270	+
		271	+ return robot_to_head_matrix
		272	+
		273	+
		274	+class transform_tracker_to_robot(object):
		275	+ M_tracker_to_robot = np.array([])
		276	+ def transformation_tracker_to_robot(self, tracker_coord):
		277	+ if not transform_tracker_to_robot.M_tracker_to_robot.any():
		278	+ return None
		279	+
		280	+ M_tracker = dco.coordinates_to_transformation_matrix(
		281	+ position=tracker_coord[:3],
		282	+ orientation=tracker_coord[3:6],
		283	+ axes='rzyx',
		284	+ )
		285	+ M_tracker_in_robot = transform_tracker_to_robot.M_tracker_to_robot @ M_tracker
		286	+
		287	+ _, _, angles, translate, _ = tr.decompose_matrix(M_tracker_in_robot)
		288	+ tracker_in_robot = [translate[0], translate[1], translate[2], \
		289	+ np.degrees(angles[2]), np.degrees(angles[1]), np.degrees(angles[0])]
		290	+
		291	+ return tracker_in_robot
@@ -0,0 +1,249 @@		@@ -0,0 +1,249 @@
	1	+#!/usr/bin/env python3
	2	+
	3	+import sys
	4	+from time import sleep
	5	+from socket import socket, AF_INET, SOCK_STREAM
	6	+import invesalius.constants as const
	7	+
	8	+class Elfin_Server():
	9	+ """
	10	+ This class is similar to tracker devices wrappers.
	11	+ It follows the same functions as the others (Initialize, Run and Close)
	12	+ """
	13	+ def __init__(self, server_ip, port_number):
	14	+ self.server_ip = server_ip
	15	+ self.port_number = port_number
	16	+
	17	+ def Initialize(self):
	18	+ message_size = 1024
	19	+ robot_id = 0
	20	+ self.cobot = Elfin()
	21	+ self.cobot.connect(self.server_ip, self.port_number, message_size, robot_id)
	22	+ print("connected!")
	23	+
	24	+ def Run(self):
	25	+ return self.cobot.ReadPcsActualPos()
	26	+
	27	+ def SendCoordinates(self, target, type=const.ROBOT_MOTIONS["normal"]):
	28	+ """
	29	+ It's not possible to send a move command to elfin if the robot is during a move.
	30	+ Status 1009 means robot in motion.
	31	+ """
	32	+ status = self.cobot.ReadMoveState()
	33	+ if status != 1009:
	34	+ if type == const.ROBOT_MOTIONS["normal"] or type == const.ROBOT_MOTIONS["linear out"]:
	35	+ self.cobot.MoveL(target)
	36	+ elif type == const.ROBOT_MOTIONS["arc"]:
	37	+ self.cobot.MoveC(target)
	38	+
	39	+ def StopRobot(self):
	40	+ # Takes some microseconds to the robot actual stops after the command.
	41	+ # The sleep time is required to guarantee the stop
	42	+ self.cobot.GrpStop()
	43	+ sleep(0.1)
	44	+
	45	+ def Close(self):
	46	+ self.cobot.close()
	47	+
	48	+class Elfin:
	49	+ def __init__(self):
	50	+ """
	51	+ Class to communicate with elfin robot. This class follows "HansRobot Communication Protocol Interface".
	52	+ """
	53	+ self.end_msg = ",;"
	54	+
	55	+ def connect(self, server_ip, port_number, message_size, robot_id):
	56	+ mySocket = socket(AF_INET, SOCK_STREAM)
	57	+ mySocket.connect((server_ip, port_number))
	58	+
	59	+ self.message_size = message_size
	60	+ self.robot_id = str(robot_id)
	61	+ self.mySocket = mySocket
	62	+
	63	+ def send(self, message):
	64	+ self.mySocket.sendall(message.encode('utf-8'))
	65	+ data = self.mySocket.recv(self.message_size).decode('utf-8').split(',')
	66	+ status = self.check_status(data)
	67	+ if status and type(data) != bool:
	68	+ if len(data) > 3:
	69	+ return data[2:-1]
	70	+ return status
	71	+
	72	+ def check_status(self, recv_message):
	73	+ status = recv_message[1]
	74	+ if status == 'OK':
	75	+ return True
	76	+
	77	+ elif status == 'Fail':
	78	+ print("Error code: ", recv_message[2])
	79	+ return False
	80	+
	81	+ def Electrify(self):
	82	+ """
	83	+ Function: Power the robot
	84	+ Notes: successful completion of power up before returning, power up time is
	85	+ about 44s.
	86	+ :return:
	87	+ if Error Return False
	88	+ if not Error Return True
	89	+ """
	90	+ message = "Electrify" + self.end_msg
	91	+ status = self.send(message)
	92	+ return status
	93	+
	94	+ def BlackOut(self):
	95	+ """
	96	+ Function: Robot blackout
	97	+ Notes: successful power outage will only return, power failure time is 3s.
	98	+ :return:
	99	+ if Error Return False
	100	+ if not Error Return True
	101	+ """
	102	+ message = "BlackOut" + self.end_msg
	103	+ status = self.send(message)
	104	+ return status
	105	+
	106	+ def StartMaster(self):
	107	+ """
	108	+ Function: Start master station
	109	+ Notes: the master station will not be returned until successfully started, startup
	110	+ master time is about 4s.
	111	+ :return:
	112	+ if Error Return False
	113	+ if not Error Return True
	114	+ """
	115	+ message = "StartMaster" + self.end_msg
	116	+ status = self.send(message)
	117	+ return status
	118	+
	119	+ def CloseMaster(self):
	120	+ """
	121	+ Function: Close master station
	122	+ Notes: the master station will not be returned until successfully closed, shut
	123	+ down the master station time is about 2s.
	124	+ :return:
	125	+ if Error Return False
	126	+ if not Error Return True
	127	+ """
	128	+ message = "CloseMaster" + self.end_msg
	129	+ status = self.send(message)
	130	+ return status
	131	+
	132	+ def GrpPowerOn(self):
	133	+ """
	134	+ Function: Robot servo on
	135	+ :return:
	136	+ if Error Return False
	137	+ if not Error Return True
	138	+ """
	139	+ message = "GrpPowerOn," + self.robot_id + self.end_msg
	140	+ status = self.send(message)
	141	+ return status
	142	+
	143	+ def GrpPowerOff(self):
	144	+ """
	145	+ Function: Robot servo off
	146	+ :return:
	147	+ if Error Return False
	148	+ if not Error Return True
	149	+ """
	150	+ message = "GrpPowerOff," + self.robot_id + self.end_msg
	151	+ status = self.send(message)
	152	+ return status
	153	+
	154	+ def GrpStop(self):
	155	+ """
	156	+ Function: Stop robot
	157	+ :return:
	158	+ if Error Return False
	159	+ if not Error Return True
	160	+ """
	161	+ message = "GrpStop," + self.robot_id + self.end_msg
	162	+ status = self.send(message)
	163	+ return status
	164	+
	165	+ def SetOverride(self, override):
	166	+ """
	167	+ function: Set speed ratio
	168	+ :param override:
	169	+ double: set speed ratio, range of 0.01~1
	170	+ :return:
	171	+ if Error Return False
	172	+ if not Error Return True
	173	+ """
	174	+
	175	+ message = "SetOverride," + self.robot_id + ',' + str(override) + self.end_msg
	176	+ status = self.send(message)
	177	+ return status
	178	+
	179	+ def ReadPcsActualPos(self):
	180	+ """Function: Get the actual position of the space coordinate
	181	+ :return:
	182	+ if True Return x,y,z,a,b,c
	183	+ if Error Return False
	184	+ """
	185	+ message = "ReadPcsActualPos," + self.robot_id + self.end_msg
	186	+ coord = self.send(message)
	187	+ if coord:
	188	+ return [float(s) for s in coord]
	189	+
	190	+ return coord
	191	+
	192	+ def MoveL(self, target):
	193	+ """
	194	+ function: Robot moves straight to the specified space coordinates
	195	+ :param: target:[X,Y,Z,RX,RY,RZ]
	196	+ :return:
	197	+ """
	198	+ target = [str(s) for s in target]
	199	+ target = (",".join(target))
	200	+ message = "MoveL," + self.robot_id + ',' + target + self.end_msg
	201	+ return self.send(message)
	202	+
	203	+ def SetToolCoordinateMotion(self, status):
	204	+ """
	205	+ function: Function: Set tool coordinate motion
	206	+ :param: int Switch 0=close 1=open
	207	+ :return:
	208	+ if Error Return False
	209	+ if not Error Return True
	210	+ """
	211	+ message = "SetToolCoordinateMotion," + self.robot_id + ',' + str(status) + self.end_msg
	212	+ status = self.send(message)
	213	+ return status
	214	+
	215	+ def ReadMoveState(self):
	216	+ """
	217	+ Function: Get the motion state of the robot
	218	+ :return:
	219	+ Current state of motion of robot:
	220	+ 0=motion completion;
	221	+ 1009=in motion;
	222	+ 1013=waiting for execution;
	223	+ 1025 =Error reporting
	224	+ """
	225	+ message = "ReadMoveState," + self.robot_id + self.end_msg
	226	+ status = int(self.send(message)[0])
	227	+ return status
	228	+
	229	+ def MoveHoming(self):
	230	+ """
	231	+ Function: Robot returns to the origin
	232	+ :return:
	233	+ if Error Return False
	234	+ if not Error Return True
	235	+ """
	236	+ message = "MoveHoming," + self.robot_id + self.end_msg
	237	+ status = self.send(message)
	238	+ return status
	239	+
	240	+ def MoveC(self, target):
	241	+ """
	242	+ function: Arc motion
	243	+ :param: Through position[X,Y,Z],GoalCoord[X,Y,Z,RX,RY,RZ],Type[0 or 1],;
	244	+ :return:
	245	+ """
	246	+ target = [str(s) for s in target]
	247	+ target = (",".join(target))
	248	+ message = "MoveC," + self.robot_id + ',' + target + self.end_msg
	249	+ return self.send(message)
@@ -0,0 +1,207 @@		@@ -0,0 +1,207 @@
	1	+#--------------------------------------------------------------------------
	2	+# Software: InVesalius - Software de Reconstrucao 3D de Imagens Medicas
	3	+# Copyright: (C) 2001 Centro de Pesquisas Renato Archer
	4	+# Homepage: http://www.softwarepublico.gov.br
	5	+# Contact: invesalius@cti.gov.br
	6	+# License: GNU - GPL 2 (LICENSE.txt/LICENCA.txt)
	7	+#--------------------------------------------------------------------------
	8	+# Este programa e software livre; voce pode redistribui-lo e/ou
	9	+# modifica-lo sob os termos da Licenca Publica Geral GNU, conforme
	10	+# publicada pela Free Software Foundation; de acordo com a versao 2
	11	+# da Licenca.
	12	+#
	13	+# Este programa eh distribuido na expectativa de ser util, mas SEM
	14	+# QUALQUER GARANTIA; sem mesmo a garantia implicita de
	15	+# COMERCIALIZACAO ou de ADEQUACAO A QUALQUER PROPOSITO EM
	16	+# PARTICULAR. Consulte a Licenca Publica Geral GNU para obter mais
	17	+# detalhes.
	18	+#--------------------------------------------------------------------------
	19	+import numpy as np
	20	+import cv2
	21	+from time import time
	22	+
	23	+import invesalius.data.transformations as tr
	24	+import invesalius.data.coregistration as dcr
	25	+import invesalius.data.coordinates as dco
	26	+import invesalius.constants as const
	27	+
	28	+
	29	+class KalmanTracker:
	30	+ """
	31	+ Kalman filter to avoid sudden fluctuation from tracker device.
	32	+ The filter strength can be set by the cov_process, and cov_measure parameter
	33	+ It is required to create one instance for each variable (x, y, z, a, b, g)
	34	+ """
	35	+ def __init__(self,
	36	+ state_num=2,
	37	+ covariance_process=0.001,
	38	+ covariance_measure=0.1):
	39	+
	40	+ self.state_num = state_num
	41	+ measure_num = 1
	42	+
	43	+ # The filter itself.
	44	+ self.filter = cv2.KalmanFilter(state_num, measure_num, 0)
	45	+
	46	+ self.state = np.zeros((state_num, 1), dtype=np.float32)
	47	+ self.measurement = np.array((measure_num, 1), np.float32)
	48	+ self.prediction = np.zeros((state_num, 1), np.float32)
	49	+
	50	+
	51	+ self.filter.transitionMatrix = np.array([[1, 1],
	52	+ [0, 1]], np.float32)
	53	+ self.filter.measurementMatrix = np.array([[1, 1]], np.float32)
	54	+ self.filter.processNoiseCov = np.array([[1, 0],
	55	+ [0, 1]], np.float32) * covariance_process
	56	+ self.filter.measurementNoiseCov = np.array([[1]], np.float32) * covariance_measure
	57	+
	58	+ def update_kalman(self, measurement):
	59	+ self.prediction = self.filter.predict()
	60	+ self.measurement = np.array([[np.float32(measurement[0])]])
	61	+
	62	+ self.filter.correct(self.measurement)
	63	+ self.state = self.filter.statePost
	64	+
	65	+
	66	+class TrackerProcessing:
	67	+ def __init__(self):
	68	+ self.coord_vel = []
	69	+ self.timestamp = []
	70	+ self.velocity_vector = []
	71	+ self.kalman_coord_vector = []
	72	+ self.velocity_std = 0
	73	+
	74	+ self.tracker_stabilizers = [KalmanTracker(
	75	+ state_num=2,
	76	+ covariance_process=0.001,
	77	+ covariance_measure=0.1) for _ in range(6)]
	78	+
	79	+ def kalman_filter(self, coord_tracker):
	80	+ kalman_array = []
	81	+ pose_np = np.array((coord_tracker[:3], coord_tracker[3:])).flatten()
	82	+ for value, ps_stb in zip(pose_np, self.tracker_stabilizers):
	83	+ ps_stb.update_kalman([value])
	84	+ kalman_array.append(ps_stb.state[0])
	85	+ coord_kalman = np.hstack(kalman_array)
	86	+
	87	+ self.kalman_coord_vector.append(coord_kalman[:3])
	88	+ if len(self.kalman_coord_vector) < 20: #avoid initial fluctuations
	89	+ coord_kalman = coord_tracker
	90	+ print('initializing filter')
	91	+ else:
	92	+ del self.kalman_coord_vector[0]
	93	+
	94	+ return coord_kalman
	95	+
	96	+ def estimate_head_velocity(self, coord_vel, timestamp):
	97	+ coord_vel = np.vstack(np.array(coord_vel))
	98	+ coord_init = coord_vel[:int(len(coord_vel) / 2)].mean(axis=0)
	99	+ coord_final = coord_vel[int(len(coord_vel) / 2):].mean(axis=0)
	100	+ velocity = (coord_final - coord_init)/(timestamp[-1] - timestamp[0])
	101	+ distance = (coord_final - coord_init)
	102	+
	103	+ return velocity, distance
	104	+
	105	+ def compute_versors(self, init_point, final_point):
	106	+ init_point = np.array(init_point)
	107	+ final_point = np.array(final_point)
	108	+ norm = (sum((final_point - init_point) 2)) 0.5
	109	+ versor_factor = (((final_point-init_point) / norm) * const.ROBOT_VERSOR_SCALE_FACTOR).tolist()
	110	+
	111	+ return versor_factor
	112	+
	113	+
	114	+ def compute_arc_motion(self, current_robot_coordinates, head_center_coordinates, new_robot_coordinates):
	115	+ head_center = head_center_coordinates[0], head_center_coordinates[1], head_center_coordinates[2], \
	116	+ new_robot_coordinates[3], new_robot_coordinates[4], new_robot_coordinates[5]
	117	+
	118	+ versor_factor_move_out = self.compute_versors(head_center, current_robot_coordinates)
	119	+ init_move_out_point = current_robot_coordinates[0] + versor_factor_move_out[0], \
	120	+ current_robot_coordinates[1] + versor_factor_move_out[1], \
	121	+ current_robot_coordinates[2] + versor_factor_move_out[2], \
	122	+ current_robot_coordinates[3], current_robot_coordinates[4], current_robot_coordinates[5]
	123	+
	124	+ middle_point = ((new_robot_coordinates[0] + current_robot_coordinates[0]) / 2,
	125	+ (new_robot_coordinates[1] + current_robot_coordinates[1]) / 2,
	126	+ (new_robot_coordinates[2] + current_robot_coordinates[2]) / 2,
	127	+ 0, 0, 0)
	128	+ versor_factor_middle_arc = (np.array(self.compute_versors(head_center, middle_point))) * 2
	129	+ middle_arc_point = middle_point[0] + versor_factor_middle_arc[0], \
	130	+ middle_point[1] + versor_factor_middle_arc[1], \
	131	+ middle_point[2] + versor_factor_middle_arc[2]
	132	+
	133	+ versor_factor_arc = self.compute_versors(head_center, new_robot_coordinates)
	134	+ final_ext_arc_point = new_robot_coordinates[0] + versor_factor_arc[0], \
	135	+ new_robot_coordinates[1] + versor_factor_arc[1], \
	136	+ new_robot_coordinates[2] + versor_factor_arc[2], \
	137	+ new_robot_coordinates[3], new_robot_coordinates[4], new_robot_coordinates[5], 0
	138	+
	139	+ target_arc = middle_arc_point + final_ext_arc_point
	140	+
	141	+ return init_move_out_point, target_arc
	142	+
	143	+ def compute_head_move_threshold(self, current_ref):
	144	+ """
	145	+ Checks if the head velocity is bellow the threshold
	146	+ """
	147	+ self.coord_vel.append(current_ref)
	148	+ self.timestamp.append(time())
	149	+ if len(self.coord_vel) >= 10:
	150	+ head_velocity, head_distance = self.estimate_head_velocity(self.coord_vel, self.timestamp)
	151	+ self.velocity_vector.append(head_velocity)
	152	+
	153	+ del self.coord_vel[0]
	154	+ del self.timestamp[0]
	155	+
	156	+ if len(self.velocity_vector) >= 30:
	157	+ self.velocity_std = np.std(self.velocity_vector)
	158	+ del self.velocity_vector[0]
	159	+
	160	+ if self.velocity_std > const.ROBOT_HEAD_VELOCITY_THRESHOLD:
	161	+ print('Velocity threshold activated')
	162	+ return False
	163	+ else:
	164	+ return True
	165	+
	166	+ return False
	167	+
	168	+ def compute_head_move_compensation(self, current_head, m_change_robot_to_head):
	169	+ """
	170	+ Estimates the new robot position to reach the target
	171	+ """
	172	+ M_current_head = dco.coordinates_to_transformation_matrix(
	173	+ position=current_head[:3],
	174	+ orientation=current_head[3:6],
	175	+ axes='rzyx',
	176	+ )
	177	+ m_robot_new = M_current_head @ m_change_robot_to_head
	178	+ _, _, angles, translate, _ = tr.decompose_matrix(m_robot_new)
	179	+ angles = np.degrees(angles)
	180	+
	181	+ return m_robot_new[0, -1], m_robot_new[1, -1], m_robot_new[2, -1], angles[0], angles[1], \
	182	+ angles[2]
	183	+
	184	+ def estimate_head_center(self, tracker, current_head):
	185	+ """
	186	+ Estimates the actual head center position using fiducials
	187	+ """
	188	+ m_probe_head_left, m_probe_head_right, m_probe_head_nasion = tracker.GetMatrixTrackerFiducials()
	189	+ m_current_head = dcr.compute_marker_transformation(np.array([current_head]), 0)
	190	+
	191	+ m_ear_left_new = m_current_head @ m_probe_head_left
	192	+ m_ear_right_new = m_current_head @ m_probe_head_right
	193	+
	194	+ return (m_ear_left_new[:3, -1] + m_ear_right_new[:3, -1])/2
	195	+
	196	+ def correction_distance_calculation_target(self, coord_inv, actual_point):
	197	+ """
	198	+ Estimates the Euclidean distance between the actual position and the target
	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)
	204	+
	205	+ return correction_distance_compensation
	206	+
	207	+
	@@ -633,7 +633,7 @@ class ComputeTractsThreadSingleBlock(threading.Thread):		@@ -633,7 +633,7 @@ class ComputeTractsThreadSingleBlock(threading.Thread):
633	# print('ComputeTractsThread: event {}'.format(self.event.is_set()))	633	# print('ComputeTractsThread: event {}'.format(self.event.is_set()))
634	while not self.event.is_set():	634	while not self.event.is_set():
635	try:	635	try:
636	- coord, m_img, m_img_flip = self.coord_queue.get_nowait()	636	+ coord, [coord_raw, markers_flag], m_img, m_img_flip = self.coord_queue.get_nowait()
637		637
638	# translate the coordinate along the normal vector of the object/coil	638	# translate the coordinate along the normal vector of the object/coil
639	coord_offset = m_img_flip[:3, -1] - offset * m_img_flip[:3, 2]	639	coord_offset = m_img_flip[:3, -1] - offset * m_img_flip[:3, 2]
	@@ -956,10 +956,10 @@ class Viewer(wx.Panel):		@@ -956,10 +956,10 @@ class Viewer(wx.Panel):
956		956
957	if thrdist and thrcoordx and thrcoordy and thrcoordz:	957	if thrdist and thrcoordx and thrcoordy and thrcoordz:
958	self.dummy_coil_actor.GetProperty().SetDiffuseColor(vtk_colors.GetColor3d('Green'))	958	self.dummy_coil_actor.GetProperty().SetDiffuseColor(vtk_colors.GetColor3d('Green'))
959	- Publisher.sendMessage('Coil at target', state=True)	959	+ wx.CallAfter(Publisher.sendMessage, 'Coil at target', state=True)
960	else:	960	else:
961	self.dummy_coil_actor.GetProperty().SetDiffuseColor(vtk_colors.GetColor3d('DarkOrange'))	961	self.dummy_coil_actor.GetProperty().SetDiffuseColor(vtk_colors.GetColor3d('DarkOrange'))
962	- Publisher.sendMessage('Coil at target', state=False)	962	+ wx.CallAfter(Publisher.sendMessage, 'Coil at target', state=False)
963		963
964	self.arrow_actor_list = arrow_roll_x1, arrow_roll_x2, arrow_yaw_z1, arrow_yaw_z2, \	964	self.arrow_actor_list = arrow_roll_x1, arrow_roll_x2, arrow_yaw_z1, arrow_yaw_z2, \
965	arrow_pitch_y1, arrow_pitch_y2	965	arrow_pitch_y1, arrow_pitch_y2
	@@ -48,6 +48,7 @@ from wx.lib import masked		@@ -48,6 +48,7 @@ from wx.lib import masked
48	from wx.lib.agw import floatspin	48	from wx.lib.agw import floatspin
49	from wx.lib.wordwrap import wordwrap	49	from wx.lib.wordwrap import wordwrap
50	from invesalius.pubsub import pub as Publisher	50	from invesalius.pubsub import pub as Publisher
		51	+import csv
51		52
52	try:	53	try:
53	from wx.adv import AboutDialogInfo, AboutBox	54	from wx.adv import AboutDialogInfo, AboutBox
	@@ -56,6 +57,7 @@ except ImportError:		@@ -56,6 +57,7 @@ except ImportError:
56		57
57	import invesalius.constants as const	58	import invesalius.constants as const
58	import invesalius.data.coordinates as dco	59	import invesalius.data.coordinates as dco
		60	+import invesalius.data.transformations as tr
59	import invesalius.gui.widgets.gradient as grad	61	import invesalius.gui.widgets.gradient as grad
60	import invesalius.session as ses	62	import invesalius.session as ses
61	import invesalius.utils as utils	63	import invesalius.utils as utils
	@@ -873,6 +875,7 @@ def ShowNavigationTrackerWarning(trck_id, lib_mode):		@@ -873,6 +875,7 @@ def ShowNavigationTrackerWarning(trck_id, lib_mode):
873	const.POLARIS: 'NDI Polaris',	875	const.POLARIS: 'NDI Polaris',
874	const.POLARISP4: 'NDI Polaris P4',	876	const.POLARISP4: 'NDI Polaris P4',
875	const.OPTITRACK: 'Optitrack',	877	const.OPTITRACK: 'Optitrack',
		878	+ const.ROBOT: 'Robotic navigation',
876	const.DEBUGTRACKRANDOM: 'Debug tracker device (random)',	879	const.DEBUGTRACKRANDOM: 'Debug tracker device (random)',
877	const.DEBUGTRACKAPPROACH: 'Debug tracker device (approach)'}	880	const.DEBUGTRACKAPPROACH: 'Debug tracker device (approach)'}
878		881
	@@ -3309,7 +3312,6 @@ class ObjectCalibrationDialog(wx.Dialog):		@@ -3309,7 +3312,6 @@ class ObjectCalibrationDialog(wx.Dialog):
3309	self.pedal_connection = pedal_connection	3312	self.pedal_connection = pedal_connection
3310		3313
3311	self.trk_init, self.tracker_id = tracker.GetTrackerInfo()	3314	self.trk_init, self.tracker_id = tracker.GetTrackerInfo()
3312	-
3313	self.obj_ref_id = 2	3315	self.obj_ref_id = 2
3314	self.obj_name = None	3316	self.obj_name = None
3315	self.polydata = None	3317	self.polydata = None
	@@ -3594,8 +3596,14 @@ class ObjectCalibrationDialog(wx.Dialog):		@@ -3594,8 +3596,14 @@ class ObjectCalibrationDialog(wx.Dialog):
3594	# and not change the function to the point of potentially breaking it.)	3596	# and not change the function to the point of potentially breaking it.)
3595	#	3597	#
3596	if self.obj_ref_id and fiducial_index == 4:	3598	if self.obj_ref_id and fiducial_index == 4:
3597	- coord = coord_raw[self.obj_ref_id, :]
3598	- coord[2] = -coord[2]	3599	+ if self.tracker_id == const.ROBOT:
		3600	+ trck_init_robot = self.trk_init[1][0]
		3601	+ coord = trck_init_robot.Run()
		3602	+ else:
		3603	+ coord = coord_raw[self.obj_ref_id, :]
		3604	+ else:
		3605	+ coord = coord_raw[0, :]
		3606	+ coord[2] = -coord[2]
3599		3607
3600	if fiducial_index == 3:	3608	if fiducial_index == 3:
3601	coord = np.zeros([6,])	3609	coord = np.zeros([6,])
	@@ -4301,6 +4309,312 @@ class SetOptitrackconfigs(wx.Dialog):		@@ -4301,6 +4309,312 @@ class SetOptitrackconfigs(wx.Dialog):
4301		4309
4302	return fn_cal, fn_userprofile	4310	return fn_cal, fn_userprofile
4303		4311
		4312	+class SetTrackerDeviceToRobot(wx.Dialog):
		4313	+ """
		4314	+ Robot navigation requires a tracker device to tracker the head position and the object (coil) position.
		4315	+ A dialog pops up showing a combobox with all trackers but debugs and the robot itself (const.TRACKERS[:-3])
		4316	+ """
		4317	+ def __init__(self, title=_("Setting tracker device:")):
		4318	+ wx.Dialog.__init__(self, wx.GetApp().GetTopWindow(), -1, title, size=wx.Size(1000, 200),
		4319	+ style=wx.DEFAULT_DIALOG_STYLE\|wx.FRAME_FLOAT_ON_PARENT\|wx.STAY_ON_TOP\|wx.RESIZE_BORDER)
		4320	+ self.tracker_id = const.DEFAULT_TRACKER
		4321	+ self._init_gui()
		4322	+
		4323	+ def _init_gui(self):
		4324	+ # ComboBox for spatial tracker device selection
		4325	+ tooltip = wx.ToolTip(_("Choose the tracking device"))
		4326	+ tracker_options = [_("Select tracker:")] + const.TRACKERS[:-3]
		4327	+ choice_trck = wx.ComboBox(self, -1, "",
		4328	+ choices=tracker_options, style=wx.CB_DROPDOWN \| wx.CB_READONLY)
		4329	+ choice_trck.SetToolTip(tooltip)
		4330	+ choice_trck.SetSelection(const.DEFAULT_TRACKER)
		4331	+ choice_trck.Bind(wx.EVT_COMBOBOX, partial(self.OnChoiceTracker, ctrl=choice_trck))
		4332	+
		4333	+ btn_ok = wx.Button(self, wx.ID_OK)
		4334	+ btn_ok.SetHelpText("")
		4335	+ btn_ok.SetDefault()
		4336	+
		4337	+ btn_cancel = wx.Button(self, wx.ID_CANCEL)
		4338	+ btn_cancel.SetHelpText("")
		4339	+
		4340	+ btnsizer = wx.StdDialogButtonSizer()
		4341	+ btnsizer.AddButton(btn_ok)
		4342	+ btnsizer.AddButton(btn_cancel)
		4343	+ btnsizer.Realize()
		4344	+
		4345	+ main_sizer = wx.BoxSizer(wx.VERTICAL)
		4346	+
		4347	+ main_sizer.Add((5, 5))
		4348	+ main_sizer.Add(choice_trck, 1, wx.EXPAND\|wx.LEFT\|wx.RIGHT, 5)
		4349	+ main_sizer.Add((15, 15))
		4350	+ main_sizer.Add(btnsizer, 0, wx.EXPAND)
		4351	+ main_sizer.Add((5, 5))
		4352	+
		4353	+ self.SetSizer(main_sizer)
		4354	+ main_sizer.Fit(self)
		4355	+
		4356	+ self.CenterOnParent()
		4357	+
		4358	+ def OnChoiceTracker(self, evt, ctrl):
		4359	+ choice = evt.GetSelection()
		4360	+ self.tracker_id = choice
		4361	+
		4362	+ def GetValue(self):
		4363	+ return self.tracker_id
		4364	+
		4365	+class SetRobotIP(wx.Dialog):
		4366	+ def __init__(self, title=_("Setting Robot IP")):
		4367	+ wx.Dialog.__init__(self, wx.GetApp().GetTopWindow(), -1, title, size=wx.Size(1000, 200),
		4368	+ style=wx.DEFAULT_DIALOG_STYLE\|wx.FRAME_FLOAT_ON_PARENT\|wx.STAY_ON_TOP\|wx.RESIZE_BORDER)
		4369	+ self.robot_ip = None
		4370	+ self._init_gui()
		4371	+
		4372	+ def _init_gui(self):
		4373	+ # ComboBox for spatial tracker device selection
		4374	+ tooltip = wx.ToolTip(_("Choose or type the robot IP"))
		4375	+ robot_ip_options = [_("Select robot IP:")] + const.ROBOT_ElFIN_IP
		4376	+ choice_IP = wx.ComboBox(self, -1, "",
		4377	+ choices=robot_ip_options, style=wx.CB_DROPDOWN \| wx.TE_PROCESS_ENTER)
		4378	+ choice_IP.SetToolTip(tooltip)
		4379	+ choice_IP.SetSelection(const.DEFAULT_TRACKER)
		4380	+ choice_IP.Bind(wx.EVT_COMBOBOX, partial(self.OnChoiceIP, ctrl=choice_IP))
		4381	+ choice_IP.Bind(wx.EVT_TEXT, partial(self.OnTxt_Ent, ctrl=choice_IP))
		4382	+
		4383	+ btn_ok = wx.Button(self, wx.ID_OK)
		4384	+ btn_ok.SetHelpText("")
		4385	+ btn_ok.SetDefault()
		4386	+
		4387	+ btn_cancel = wx.Button(self, wx.ID_CANCEL)
		4388	+ btn_cancel.SetHelpText("")
		4389	+
		4390	+ btnsizer = wx.StdDialogButtonSizer()
		4391	+ btnsizer.AddButton(btn_ok)
		4392	+ btnsizer.AddButton(btn_cancel)
		4393	+ btnsizer.Realize()
		4394	+
		4395	+ main_sizer = wx.BoxSizer(wx.VERTICAL)
		4396	+
		4397	+ main_sizer.Add((5, 5))
		4398	+ main_sizer.Add(choice_IP, 1, wx.EXPAND\|wx.LEFT\|wx.RIGHT, 5)
		4399	+ main_sizer.Add((15, 15))
		4400	+ main_sizer.Add(btnsizer, 0, wx.EXPAND)
		4401	+ main_sizer.Add((5, 5))
		4402	+
		4403	+ self.SetSizer(main_sizer)
		4404	+ main_sizer.Fit(self)
		4405	+
		4406	+ self.CenterOnParent()
		4407	+
		4408	+ def OnTxt_Ent(self, evt, ctrl):
		4409	+ self.robot_ip = str(ctrl.GetValue())
		4410	+
		4411	+ def OnChoiceIP(self, evt, ctrl):
		4412	+ self.robot_ip = ctrl.GetStringSelection()
		4413	+
		4414	+ def GetValue(self):
		4415	+ return self.robot_ip
		4416	+
		4417	+class CreateTransformationMatrixRobot(wx.Dialog):
		4418	+ def __init__(self, tracker, title=_("Create transformation matrix to robot space")):
		4419	+ wx.Dialog.__init__(self, wx.GetApp().GetTopWindow(), -1, title, #size=wx.Size(1000, 200),
		4420	+ style=wx.DEFAULT_DIALOG_STYLE\|wx.FRAME_FLOAT_ON_PARENT\|wx.STAY_ON_TOP\|wx.RESIZE_BORDER)
		4421	+ '''
		4422	+ M_robot_2_tracker is created by an affine transformation. Robot TCP should be calibrated to the center of the tracker marker
		4423	+ '''
		4424	+ #TODO: make aboutbox
		4425	+ self.tracker_coord = []
		4426	+ self.tracker_angles = []
		4427	+ self.robot_coord = []
		4428	+ self.robot_angles = []
		4429	+
		4430	+ self.tracker = tracker
		4431	+
		4432	+ self.timer = wx.Timer(self)
		4433	+ self.Bind(wx.EVT_TIMER, self.OnUpdate, self.timer)
		4434	+
		4435	+ self._init_gui()
		4436	+
		4437	+ def _init_gui(self):
		4438	+ # Buttons to acquire and remove points
		4439	+ txt_acquisition = wx.StaticText(self, -1, _('Poses acquisition for robot registration:'))
		4440	+
		4441	+ btn_create_point = wx.Button(self, -1, label=_('Single'))
		4442	+ btn_create_point.Bind(wx.EVT_BUTTON, self.OnCreatePoint)
		4443	+
		4444	+ btn_cont_point = wx.ToggleButton(self, -1, label=_('Continuous'))
		4445	+ btn_cont_point.Bind(wx.EVT_TOGGLEBUTTON, partial(self.OnContinuousAcquisition, btn=btn_cont_point))
		4446	+ self.btn_cont_point = btn_cont_point
		4447	+
		4448	+ txt_number = wx.StaticText(self, -1, _('0'))
		4449	+ txt_recorded = wx.StaticText(self, -1, _('Poses recorded'))
		4450	+ self.txt_number = txt_number
		4451	+
		4452	+ btn_reset = wx.Button(self, -1, label=_('Reset'))
		4453	+ btn_reset.Bind(wx.EVT_BUTTON, self.OnReset)
		4454	+
		4455	+ btn_apply_reg = wx.Button(self, -1, label=_('Apply'))
		4456	+ btn_apply_reg.Bind(wx.EVT_BUTTON, self.OnApply)
		4457	+ btn_apply_reg.Enable(False)
		4458	+ self.btn_apply_reg = btn_apply_reg
		4459	+
		4460	+ # Buttons to save and load
		4461	+ txt_file = wx.StaticText(self, -1, _('Registration file'))
		4462	+
		4463	+ btn_save = wx.Button(self, -1, label=_('Save'), size=wx.Size(65, 23))
		4464	+ btn_save.Bind(wx.EVT_BUTTON, self.OnSaveReg)
		4465	+ btn_save.Enable(False)
		4466	+ self.btn_save = btn_save
		4467	+
		4468	+ btn_load = wx.Button(self, -1, label=_('Load'), size=wx.Size(65, 23))
		4469	+ btn_load.Bind(wx.EVT_BUTTON, self.OnLoadReg)
		4470	+
		4471	+ # Create a horizontal sizers
		4472	+ border = 1
		4473	+ acquisition = wx.BoxSizer(wx.HORIZONTAL)
		4474	+ acquisition.AddMany([(btn_create_point, 1, wx.EXPAND \| wx.GROW \| wx.TOP \| wx.RIGHT \| wx.LEFT, border),
		4475	+ (btn_cont_point, 1, wx.ALL \| wx.EXPAND \| wx.GROW, border)])
		4476	+
		4477	+ txt_pose = wx.BoxSizer(wx.HORIZONTAL)
		4478	+ txt_pose.AddMany([(txt_number, 1, wx.LEFT, 50),
		4479	+ (txt_recorded, 1, wx.LEFT, border)])
		4480	+
		4481	+ apply_reset = wx.BoxSizer(wx.HORIZONTAL)
		4482	+ apply_reset.AddMany([(btn_reset, 1, wx.EXPAND \| wx.GROW \| wx.TOP \| wx.RIGHT \| wx.LEFT, border),
		4483	+ (btn_apply_reg, 1, wx.ALL \| wx.EXPAND \| wx.GROW, border)])
		4484	+
		4485	+ save_load = wx.BoxSizer(wx.HORIZONTAL)
		4486	+ save_load.AddMany([(btn_save, 1, wx.EXPAND \| wx.GROW \| wx.TOP \| wx.RIGHT \| wx.LEFT, border),
		4487	+ (btn_load, 1, wx.ALL \| wx.EXPAND \| wx.GROW, border)])
		4488	+
		4489	+ btn_ok = wx.Button(self, wx.ID_OK)
		4490	+ btn_ok.SetHelpText("")
		4491	+ btn_ok.SetDefault()
		4492	+ btn_ok.Enable(False)
		4493	+ self.btn_ok = btn_ok
		4494	+
		4495	+ btn_cancel = wx.Button(self, wx.ID_CANCEL)
		4496	+ btn_cancel.SetHelpText("")
		4497	+
		4498	+ btnsizer = wx.StdDialogButtonSizer()
		4499	+ btnsizer.AddButton(btn_ok)
		4500	+ btnsizer.AddButton(btn_cancel)
		4501	+ btnsizer.Realize()
		4502	+
		4503	+ # Add line sizers into main sizer
		4504	+ border = 10
		4505	+ border_last = 10
		4506	+ main_sizer = wx.BoxSizer(wx.VERTICAL)
		4507	+ main_sizer.Add(wx.StaticLine(self, -1), 0, wx.EXPAND \| wx.TOP \| wx.BOTTOM, border)
		4508	+ main_sizer.Add(txt_acquisition, 0, wx.BOTTOM \| wx.LEFT \| wx.RIGHT \| wx.ALIGN_CENTER_HORIZONTAL , border)
		4509	+ main_sizer.Add(acquisition, 0, wx.GROW \| wx.EXPAND \| wx.LEFT \| wx.RIGHT \| wx.BOTTOM, border)
		4510	+ main_sizer.Add(txt_pose, 0, wx.ALIGN_CENTER_HORIZONTAL \| wx.TOP \| wx.BOTTOM, border)
		4511	+ main_sizer.Add(apply_reset, 0, wx.GROW \| wx.EXPAND \| wx.LEFT \| wx.RIGHT , border_last)
		4512	+ main_sizer.Add(wx.StaticLine(self, -1), 0, wx.EXPAND \| wx.TOP \| wx.BOTTOM, border)
		4513	+ main_sizer.Add(txt_file, 0, wx.GROW \| wx.EXPAND \| wx.LEFT \| wx.RIGHT \| wx.BOTTOM, border/2)
		4514	+ main_sizer.Add(save_load, 0, wx.GROW \| wx.EXPAND \| wx.LEFT \| wx.RIGHT \| wx.BOTTOM, border)
		4515	+ main_sizer.Add(wx.StaticLine(self, -1), 0, wx.EXPAND \| wx.TOP \| wx.BOTTOM, border)
		4516	+ main_sizer.Add(btnsizer, 0, wx.GROW \| wx.EXPAND \| wx.LEFT \| wx.RIGHT \| wx.BOTTOM, border)
		4517	+ main_sizer.Fit(self)
		4518	+
		4519	+ self.SetSizer(main_sizer)
		4520	+ self.Update()
		4521	+ main_sizer.Fit(self)
		4522	+
		4523	+ self.CenterOnParent()
		4524	+
		4525	+ def affine_correg(self, tracker, robot):
		4526	+ m_change = tr.affine_matrix_from_points(robot[:].T, tracker[:].T,
		4527	+ shear=False, scale=False, usesvd=False)
		4528	+ return m_change
		4529	+
		4530	+ def OnContinuousAcquisition(self, evt=None, btn=None):
		4531	+ value = btn.GetValue()
		4532	+ if value:
		4533	+ self.timer.Start(500)
		4534	+ else:
		4535	+ self.timer.Stop()
		4536	+
		4537	+ def OnUpdate(self, evt):
		4538	+ self.OnCreatePoint(evt=None)
		4539	+
		4540	+ def OnCreatePoint(self, evt):
		4541	+ coord_raw, markers_flag = self.tracker.TrackerCoordinates.GetCoordinates()
		4542	+ #robot thread is not initialized yet
		4543	+ coord_raw_robot = self.tracker.trk_init[1][0].Run()
		4544	+ coord_raw_tracker_obj = coord_raw[3]
		4545	+
		4546	+ if markers_flag[2]:
		4547	+ self.tracker_coord.append(coord_raw_tracker_obj[:3])
		4548	+ self.tracker_angles.append(coord_raw_tracker_obj[3:])
		4549	+ self.robot_coord.append(coord_raw_robot[:3])
		4550	+ self.robot_angles.append(coord_raw_robot[3:])
		4551	+ self.txt_number.SetLabel(str(int(self.txt_number.GetLabel())+1))
		4552	+ else:
		4553	+ print('Cannot detect the coil markers, pls try again')
		4554	+
		4555	+ if len(self.tracker_coord) >= 3:
		4556	+ self.btn_apply_reg.Enable(True)
		4557	+
		4558	+ def OnReset(self, evt):
		4559	+ if self.btn_cont_point:
		4560	+ self.btn_cont_point.SetValue(False)
		4561	+ self.OnContinuousAcquisition(evt=None, btn=self.btn_cont_point)
		4562	+
		4563	+ self.tracker_coord = []
		4564	+ self.tracker_angles = []
		4565	+ self.robot_coord = []
		4566	+ self.robot_angles = []
		4567	+ self.M_tracker_2_robot = []
		4568	+ self.txt_number.SetLabel('0')
		4569	+
		4570	+ self.btn_apply_reg.Enable(False)
		4571	+ self.btn_save.Enable(False)
		4572	+ self.btn_ok.Enable(False)
		4573	+
		4574	+ def OnApply(self, evt):
		4575	+ if self.btn_cont_point:
		4576	+ self.btn_cont_point.SetValue(False)
		4577	+ self.OnContinuousAcquisition(evt=None, btn=self.btn_cont_point)
		4578	+
		4579	+ tracker_coord = np.array(self.tracker_coord)
		4580	+ robot_coord = np.array(self.robot_coord)
		4581	+
		4582	+ M_robot_2_tracker = self.affine_correg(tracker_coord, robot_coord)
		4583	+ M_tracker_2_robot = tr.inverse_matrix(M_robot_2_tracker)
		4584	+ self.M_tracker_2_robot = M_tracker_2_robot
		4585	+
		4586	+ self.btn_save.Enable(True)
		4587	+ self.btn_ok.Enable(True)
		4588	+
		4589	+ #TODO: make a colored circle to sinalize that the transformation was made (green) (red if not)
		4590	+
		4591	+ def OnSaveReg(self, evt):
		4592	+ filename = ShowLoadSaveDialog(message=_(u"Save robot transformation file as..."),
		4593	+ wildcard=_("Robot transformation files (.rbtf)\|.rbtf"),
		4594	+ style=wx.FD_SAVE \| wx.FD_OVERWRITE_PROMPT,
		4595	+ default_filename="robottransform.rbtf", save_ext="rbtf")
		4596	+
		4597	+ if filename:
		4598	+ if self.M_tracker_2_robot is not None:
		4599	+ with open(filename, 'w', newline='') as file:
		4600	+ writer = csv.writer(file, delimiter='\t')
		4601	+ writer.writerows(self.M_tracker_2_robot)
		4602	+
		4603	+ def OnLoadReg(self, evt):
		4604	+ filename = ShowLoadSaveDialog(message=_(u"Load robot transformation"),
		4605	+ wildcard=_("Robot transformation files (.rbtf)\|.rbtf"))
		4606	+ if filename:
		4607	+ with open(filename, 'r') as file:
		4608	+ reader = csv.reader(file, delimiter='\t')
		4609	+ content = [row for row in reader]
		4610	+
		4611	+ self.M_tracker_2_robot = np.vstack(list(np.float_(content)))
		4612	+ print("Matrix tracker to robot:", self.M_tracker_2_robot)
		4613	+ self.btn_ok.Enable(True)
		4614	+
		4615	+ def GetValue(self):
		4616	+ return self.M_tracker_2_robot
		4617	+
4304	class SetNDIconfigs(wx.Dialog):	4618	class SetNDIconfigs(wx.Dialog):
4305	def __init__(self, title=_("Setting NDI polaris configs:")):	4619	def __init__(self, title=_("Setting NDI polaris configs:")):
4306	wx.Dialog.__init__(self, wx.GetApp().GetTopWindow(), -1, title, size=wx.Size(1000, 200),	4620	wx.Dialog.__init__(self, wx.GetApp().GetTopWindow(), -1, title, size=wx.Size(1000, 200),
	@@ -4329,16 +4643,19 @@ class SetNDIconfigs(wx.Dialog):		@@ -4329,16 +4643,19 @@ class SetNDIconfigs(wx.Dialog):
4329	return port_list, port_selec	4643	return port_list, port_selec
4330		4644
4331	def _init_gui(self):	4645	def _init_gui(self):
4332	- self.com_ports = wx.ComboBox(self, -1, style=wx.CB_DROPDOWN\|wx.CB_READONLY)	4646	+ com_ports = wx.ComboBox(self, -1, style=wx.CB_DROPDOWN\|wx.CB_READONLY)
		4647	+ com_ports.Bind(wx.EVT_COMBOBOX, partial(self.OnChoicePort, ctrl=com_ports))
4333	row_com = wx.BoxSizer(wx.VERTICAL)	4648	row_com = wx.BoxSizer(wx.VERTICAL)
4334	row_com.Add(wx.StaticText(self, wx.ID_ANY, "Select the COM port"), 0, wx.TOP\|wx.RIGHT,5)	4649	row_com.Add(wx.StaticText(self, wx.ID_ANY, "Select the COM port"), 0, wx.TOP\|wx.RIGHT,5)
4335	- row_com.Add(self.com_ports, 0, wx.EXPAND)	4650	+ row_com.Add(com_ports, 0, wx.EXPAND)
4336		4651
4337	port_list, port_selec = self.serial_ports()	4652	port_list, port_selec = self.serial_ports()
4338		4653
4339	- self.com_ports.Append(port_list)	4654	+ com_ports.Append(port_list)
4340	if port_selec:	4655	if port_selec:
4341	- self.com_ports.SetSelection(port_selec[0])	4656	+ com_ports.SetSelection(port_selec[0])
		4657	+
		4658	+ self.com_ports = com_ports
4342		4659
4343	session = ses.Session()	4660	session = ses.Session()
4344	last_ndi_probe_marker = session.get('paths', 'last_ndi_probe_marker', '')	4661	last_ndi_probe_marker = session.get('paths', 'last_ndi_probe_marker', '')
	@@ -4374,6 +4691,8 @@ class SetNDIconfigs(wx.Dialog):		@@ -4374,6 +4691,8 @@ class SetNDIconfigs(wx.Dialog):
4374	btn_ok = wx.Button(self, wx.ID_OK)	4691	btn_ok = wx.Button(self, wx.ID_OK)
4375	btn_ok.SetHelpText("")	4692	btn_ok.SetHelpText("")
4376	btn_ok.SetDefault()	4693	btn_ok.SetDefault()
		4694	+ btn_ok.Enable(False)
		4695	+ self.btn_ok = btn_ok
4377		4696
4378	btn_cancel = wx.Button(self, wx.ID_CANCEL)	4697	btn_cancel = wx.Button(self, wx.ID_CANCEL)
4379	btn_cancel.SetHelpText("")	4698	btn_cancel.SetHelpText("")
	@@ -4402,6 +4721,9 @@ class SetNDIconfigs(wx.Dialog):		@@ -4402,6 +4721,9 @@ class SetNDIconfigs(wx.Dialog):
4402		4721
4403	self.CenterOnParent()	4722	self.CenterOnParent()
4404		4723
		4724	+ def OnChoicePort(self, evt, ctrl):
		4725	+ self.btn_ok.Enable(True)
		4726	+
4405	def GetValue(self):	4727	def GetValue(self):
4406	fn_probe = self.dir_probe.GetPath().encode(const.FS_ENCODE)	4728	fn_probe = self.dir_probe.GetPath().encode(const.FS_ENCODE)
4407	fn_ref = self.dir_ref.GetPath().encode(const.FS_ENCODE)	4729	fn_ref = self.dir_ref.GetPath().encode(const.FS_ENCODE)
	@@ -20,7 +20,10 @@		@@ -20,7 +20,10 @@
20	import dataclasses	20	import dataclasses
21	from functools import partial	21	from functools import partial
22	import itertools	22	import itertools
		23	+import csv
		24	+import queue
23	import time	25	import time
		26	+import threading
24		27
25	import nibabel as nb	28	import nibabel as nb
26	import numpy as np	29	import numpy as np
	@@ -29,6 +32,13 @@ try:		@@ -29,6 +32,13 @@ try:
29	has_trekker = True	32	has_trekker = True
30	except ImportError:	33	except ImportError:
31	has_trekker = False	34	has_trekker = False
		35	+try:
		36	+ import invesalius.data.elfin as elfin
		37	+ import invesalius.data.elfin_processing as elfin_process
		38	+ has_robot = True
		39	+except ImportError:
		40	+ has_robot = False
		41	+
32	import wx	42	import wx
33		43
34	try:	44	try:
	@@ -51,14 +61,17 @@ import invesalius.data.slice_ as sl		@@ -51,14 +61,17 @@ import invesalius.data.slice_ as sl
51	import invesalius.data.tractography as dti	61	import invesalius.data.tractography as dti
52	import invesalius.data.record_coords as rec	62	import invesalius.data.record_coords as rec
53	import invesalius.data.vtk_utils as vtk_utils	63	import invesalius.data.vtk_utils as vtk_utils
		64	+import invesalius.data.bases as db
54	import invesalius.gui.dialogs as dlg	65	import invesalius.gui.dialogs as dlg
55	import invesalius.project as prj	66	import invesalius.project as prj
56	import invesalius.session as ses	67	import invesalius.session as ses
		68	+
57	from invesalius import utils	69	from invesalius import utils
58	from invesalius.gui import utils as gui_utils	70	from invesalius.gui import utils as gui_utils
59	from invesalius.navigation.icp import ICP	71	from invesalius.navigation.icp import ICP
60	from invesalius.navigation.navigation import Navigation	72	from invesalius.navigation.navigation import Navigation
61	from invesalius.navigation.tracker import Tracker	73	from invesalius.navigation.tracker import Tracker
		74	+from invesalius.navigation.robot import Robot
62		75
63	HAS_PEDAL_CONNECTION = True	76	HAS_PEDAL_CONNECTION = True
64	try:	77	try:
	@@ -184,7 +197,7 @@ class InnerFoldPanel(wx.Panel):		@@ -184,7 +197,7 @@ class InnerFoldPanel(wx.Panel):
184		197
185	# Fold 3 - Markers panel	198	# Fold 3 - Markers panel
186	item = fold_panel.AddFoldPanel(_("Markers"), collapsed=True)	199	item = fold_panel.AddFoldPanel(_("Markers"), collapsed=True)
187	- mtw = MarkersPanel(item)	200	+ mtw = MarkersPanel(item, tracker)
188		201
189	fold_panel.ApplyCaptionStyle(item, style)	202	fold_panel.ApplyCaptionStyle(item, style)
190	fold_panel.AddFoldPanelWindow(item, mtw, spacing= 0,	203	fold_panel.AddFoldPanelWindow(item, mtw, spacing= 0,
	@@ -342,6 +355,7 @@ class NeuronavigationPanel(wx.Panel):		@@ -342,6 +355,7 @@ class NeuronavigationPanel(wx.Panel):
342	)	355	)
343	self.icp = ICP()	356	self.icp = ICP()
344	self.tracker = tracker	357	self.tracker = tracker
		358	+ self.robot = Robot(tracker)
345		359
346	self.nav_status = False	360	self.nav_status = False
347	self.tracker_fiducial_being_set = None	361	self.tracker_fiducial_being_set = None
	@@ -641,6 +655,8 @@ class NeuronavigationPanel(wx.Panel):		@@ -641,6 +655,8 @@ class NeuronavigationPanel(wx.Panel):
641	self.checkbox_icp.SetValue(False)	655	self.checkbox_icp.SetValue(False)
642		656
643	def OnDisconnectTracker(self):	657	def OnDisconnectTracker(self):
		658	+ if self.tracker.tracker_id == const.ROBOT:
		659	+ self.robot.StopRobotThreadNavigation()
644	self.tracker.DisconnectTracker()	660	self.tracker.DisconnectTracker()
645	self.ResetICP()	661	self.ResetICP()
646	self.tracker.UpdateUI(self.select_tracker_elem, self.numctrls_fiducial[3:6], self.txtctrl_fre)	662	self.tracker.UpdateUI(self.select_tracker_elem, self.numctrls_fiducial[3:6], self.txtctrl_fre)
	@@ -658,6 +674,9 @@ class NeuronavigationPanel(wx.Panel):		@@ -658,6 +674,9 @@ class NeuronavigationPanel(wx.Panel):
658	choice = None	674	choice = None
659		675
660	self.tracker.SetTracker(choice)	676	self.tracker.SetTracker(choice)
		677	+ if self.tracker.tracker_id == const.ROBOT:
		678	+ self.tracker.ConnectToRobot(self.navigation, self.tracker, self.robot)
		679	+
661	self.ResetICP()	680	self.ResetICP()
662	self.tracker.UpdateUI(ctrl, self.numctrls_fiducial[3:6], self.txtctrl_fre)	681	self.tracker.UpdateUI(ctrl, self.numctrls_fiducial[3:6], self.txtctrl_fre)
663		682
	@@ -740,6 +759,9 @@ class NeuronavigationPanel(wx.Panel):		@@ -740,6 +759,9 @@ class NeuronavigationPanel(wx.Panel):
740	choice_ref = self.choice_ref	759	choice_ref = self.choice_ref
741		760
742	self.navigation.StopNavigation()	761	self.navigation.StopNavigation()
		762	+ if self.tracker.tracker_id == const.ROBOT:
		763	+ Publisher.sendMessage('Robot target matrix', robot_tracker_flag=False,
		764	+ m_change_robot_to_head=None)
743		765
744	# Enable all navigation buttons	766	# Enable all navigation buttons
745	choice_ref.Enable(True)	767	choice_ref.Enable(True)
	@@ -842,6 +864,7 @@ class NeuronavigationPanel(wx.Panel):		@@ -842,6 +864,7 @@ class NeuronavigationPanel(wx.Panel):
842	)	864	)
843	self.tracker.__init__()	865	self.tracker.__init__()
844	self.icp.__init__()	866	self.icp.__init__()
		867	+ self.robot.__init__(self.tracker)
845		868
846		869
847	class ObjectRegistrationPanel(wx.Panel):	870	class ObjectRegistrationPanel(wx.Panel):
	@@ -900,16 +923,16 @@ class ObjectRegistrationPanel(wx.Panel):		@@ -900,16 +923,16 @@ class ObjectRegistrationPanel(wx.Panel):
900		923
901	# Change angles threshold	924	# Change angles threshold
902	text_angles = wx.StaticText(self, -1, _("Angle threshold [degrees]:"))	925	text_angles = wx.StaticText(self, -1, _("Angle threshold [degrees]:"))
903	- spin_size_angles = wx.SpinCtrl(self, -1, "", size=wx.Size(50, 23))
904	- spin_size_angles.SetRange(1, 99)	926	+ spin_size_angles = wx.SpinCtrlDouble(self, -1, "", size=wx.Size(50, 23))
		927	+ spin_size_angles.SetRange(0.1, 99)
905	spin_size_angles.SetValue(const.COIL_ANGLES_THRESHOLD)	928	spin_size_angles.SetValue(const.COIL_ANGLES_THRESHOLD)
906	spin_size_angles.Bind(wx.EVT_TEXT, partial(self.OnSelectAngleThreshold, ctrl=spin_size_angles))	929	spin_size_angles.Bind(wx.EVT_TEXT, partial(self.OnSelectAngleThreshold, ctrl=spin_size_angles))
907	spin_size_angles.Bind(wx.EVT_SPINCTRL, partial(self.OnSelectAngleThreshold, ctrl=spin_size_angles))	930	spin_size_angles.Bind(wx.EVT_SPINCTRL, partial(self.OnSelectAngleThreshold, ctrl=spin_size_angles))
908		931
909	# Change dist threshold	932	# Change dist threshold
910	text_dist = wx.StaticText(self, -1, _("Distance threshold [mm]:"))	933	text_dist = wx.StaticText(self, -1, _("Distance threshold [mm]:"))
911	- spin_size_dist = wx.SpinCtrl(self, -1, "", size=wx.Size(50, 23))
912	- spin_size_dist.SetRange(1, 99)	934	+ spin_size_dist = wx.SpinCtrlDouble(self, -1, "", size=wx.Size(50, 23))
		935	+ spin_size_dist.SetRange(0.1, 99)
913	spin_size_dist.SetValue(const.COIL_ANGLES_THRESHOLD)	936	spin_size_dist.SetValue(const.COIL_ANGLES_THRESHOLD)
914	spin_size_dist.Bind(wx.EVT_TEXT, partial(self.OnSelectDistThreshold, ctrl=spin_size_dist))	937	spin_size_dist.Bind(wx.EVT_TEXT, partial(self.OnSelectDistThreshold, ctrl=spin_size_dist))
915	spin_size_dist.Bind(wx.EVT_SPINCTRL, partial(self.OnSelectDistThreshold, ctrl=spin_size_dist))	938	spin_size_dist.Bind(wx.EVT_SPINCTRL, partial(self.OnSelectDistThreshold, ctrl=spin_size_dist))
	@@ -1201,7 +1224,20 @@ class MarkersPanel(wx.Panel):		@@ -1201,7 +1224,20 @@ class MarkersPanel(wx.Panel):
1201	if field.type is bool:	1224	if field.type is bool:
1202	setattr(self, field.name, str_val=='True')	1225	setattr(self, field.name, str_val=='True')
1203		1226
1204	- def __init__(self, parent):	1227	+ @dataclasses.dataclass
		1228	+ class Robot_Marker:
		1229	+ """Class for storing robot target."""
		1230	+ m_robot_target : list = None
		1231	+
		1232	+ @property
		1233	+ def robot_target_matrix(self):
		1234	+ return self.m_robot_target
		1235	+
		1236	+ @robot_target_matrix.setter
		1237	+ def robot_target_matrix(self, new_m_robot_target):
		1238	+ self.m_robot_target = new_m_robot_target
		1239	+
		1240	+ def __init__(self, parent, tracker):
1205	wx.Panel.__init__(self, parent)	1241	wx.Panel.__init__(self, parent)
1206	try:	1242	try:
1207	default_colour = wx.SystemSettings.GetColour(wx.SYS_COLOUR_MENUBAR)	1243	default_colour = wx.SystemSettings.GetColour(wx.SYS_COLOUR_MENUBAR)
	@@ -1211,6 +1247,8 @@ class MarkersPanel(wx.Panel):		@@ -1211,6 +1247,8 @@ class MarkersPanel(wx.Panel):
1211		1247
1212	self.SetAutoLayout(1)	1248	self.SetAutoLayout(1)
1213		1249
		1250	+ self.tracker = tracker
		1251	+
1214	self.__bind_events()	1252	self.__bind_events()
1215		1253
1216	self.session = ses.Session()	1254	self.session = ses.Session()
	@@ -1218,8 +1256,11 @@ class MarkersPanel(wx.Panel):		@@ -1218,8 +1256,11 @@ class MarkersPanel(wx.Panel):
1218	self.current_coord = 0, 0, 0, 0, 0, 0	1256	self.current_coord = 0, 0, 0, 0, 0, 0
1219	self.current_angle = 0, 0, 0	1257	self.current_angle = 0, 0, 0
1220	self.current_seed = 0, 0, 0	1258	self.current_seed = 0, 0, 0
		1259	+ self.current_robot_target_matrix = [None] * 9
1221	self.markers = []	1260	self.markers = []
		1261	+ self.robot_markers = []
1222	self.nav_status = False	1262	self.nav_status = False
		1263	+ self.raw_target_robot = None, None
1223		1264
1224	self.marker_colour = const.MARKER_COLOUR	1265	self.marker_colour = const.MARKER_COLOUR
1225	self.marker_size = const.MARKER_SIZE	1266	self.marker_size = const.MARKER_SIZE
	@@ -1317,6 +1358,7 @@ class MarkersPanel(wx.Panel):		@@ -1317,6 +1358,7 @@ class MarkersPanel(wx.Panel):
1317	Publisher.subscribe(self.UpdateNavigationStatus, 'Navigation status')	1358	Publisher.subscribe(self.UpdateNavigationStatus, 'Navigation status')
1318	Publisher.subscribe(self.UpdateSeedCoordinates, 'Update tracts')	1359	Publisher.subscribe(self.UpdateSeedCoordinates, 'Update tracts')
1319	Publisher.subscribe(self.OnChangeCurrentSession, 'Current session changed')	1360	Publisher.subscribe(self.OnChangeCurrentSession, 'Current session changed')
		1361	+ Publisher.subscribe(self.UpdateRobotCoordinates, 'Update raw coordinates')
1320		1362
1321	def __find_target_marker(self):	1363	def __find_target_marker(self):
1322	"""	1364	"""
	@@ -1350,6 +1392,7 @@ class MarkersPanel(wx.Panel):		@@ -1350,6 +1392,7 @@ class MarkersPanel(wx.Panel):
1350	"""	1392	"""
1351	for i in reversed(index):	1393	for i in reversed(index):
1352	del self.markers[i]	1394	del self.markers[i]
		1395	+ del self.robot_markers[i]
1353	self.lc.DeleteItem(i)	1396	self.lc.DeleteItem(i)
1354	for n in range(0, self.lc.GetItemCount()):	1397	for n in range(0, self.lc.GetItemCount()):
1355	self.lc.SetItem(n, 0, str(n + 1))	1398	self.lc.SetItem(n, 0, str(n + 1))
	@@ -1402,6 +1445,9 @@ class MarkersPanel(wx.Panel):		@@ -1402,6 +1445,9 @@ class MarkersPanel(wx.Panel):
1402	def UpdateSeedCoordinates(self, root=None, affine_vtk=None, coord_offset=(0, 0, 0)):	1445	def UpdateSeedCoordinates(self, root=None, affine_vtk=None, coord_offset=(0, 0, 0)):
1403	self.current_seed = coord_offset	1446	self.current_seed = coord_offset
1404		1447
		1448	+ def UpdateRobotCoordinates(self, coordinates_raw, markers_flag):
		1449	+ self.raw_target_robot = coordinates_raw[1], coordinates_raw[2]
		1450	+
1405	def OnMouseRightDown(self, evt):	1451	def OnMouseRightDown(self, evt):
1406	# TODO: Enable the "Set as target" only when target is created with registered object	1452	# TODO: Enable the "Set as target" only when target is created with registered object
1407	menu_id = wx.Menu()	1453	menu_id = wx.Menu()
	@@ -1412,6 +1458,16 @@ class MarkersPanel(wx.Panel):		@@ -1412,6 +1458,16 @@ class MarkersPanel(wx.Panel):
1412	menu_id.AppendSeparator()	1458	menu_id.AppendSeparator()
1413	target_menu = menu_id.Append(1, _('Set as target'))	1459	target_menu = menu_id.Append(1, _('Set as target'))
1414	menu_id.Bind(wx.EVT_MENU, self.OnMenuSetTarget, target_menu)	1460	menu_id.Bind(wx.EVT_MENU, self.OnMenuSetTarget, target_menu)
		1461	+ menu_id.AppendSeparator()
		1462	+ send_target_to_robot = menu_id.Append(3, _('Send target to robot'))
		1463	+ menu_id.Bind(wx.EVT_MENU, self.OnMenuSendTargetToRobot, send_target_to_robot)
		1464	+
		1465	+ # Enable "Send target to robot" button only if tracker is robot, if navigation is on and if target is not none
		1466	+ m_target_robot = np.array([self.robot_markers[self.lc.GetFocusedItem()].robot_target_matrix])
		1467	+ if self.tracker.tracker_id == const.ROBOT and self.nav_status and m_target_robot.any():
		1468	+ send_target_to_robot.Enable(True)
		1469	+ else:
		1470	+ send_target_to_robot.Enable(False)
1415	# TODO: Create the remove target option so the user can disable the target without removing the marker	1471	# TODO: Create the remove target option so the user can disable the target without removing the marker
1416	# target_menu_rem = menu_id.Append(3, _('Remove target'))	1472	# target_menu_rem = menu_id.Append(3, _('Remove target'))
1417	# menu_id.Bind(wx.EVT_MENU, self.OnMenuRemoveTarget, target_menu_rem)	1473	# menu_id.Bind(wx.EVT_MENU, self.OnMenuRemoveTarget, target_menu_rem)
	@@ -1462,6 +1518,15 @@ class MarkersPanel(wx.Panel):		@@ -1462,6 +1518,15 @@ class MarkersPanel(wx.Panel):
1462		1518
1463	Publisher.sendMessage('Set new color', index=index, color=color_new)	1519	Publisher.sendMessage('Set new color', index=index, color=color_new)
1464		1520
		1521	+ def OnMenuSendTargetToRobot(self, evt):
		1522	+ if isinstance(evt, int):
		1523	+ self.lc.Focus(evt)
		1524	+
		1525	+ m_target_robot = self.robot_markers[self.lc.GetFocusedItem()].robot_target_matrix
		1526	+
		1527	+ Publisher.sendMessage('Reset robot process')
		1528	+ Publisher.sendMessage('Robot target matrix', robot_tracker_flag=True, m_change_robot_to_head=m_target_robot)
		1529	+
1465	def OnDeleteAllMarkers(self, evt=None):	1530	def OnDeleteAllMarkers(self, evt=None):
1466	if evt is not None:	1531	if evt is not None:
1467	result = dlg.ShowConfirmationDialog(msg=_("Remove all markers? Cannot be undone."))	1532	result = dlg.ShowConfirmationDialog(msg=_("Remove all markers? Cannot be undone."))
	@@ -1474,6 +1539,7 @@ class MarkersPanel(wx.Panel):		@@ -1474,6 +1539,7 @@ class MarkersPanel(wx.Panel):
1474	wx.MessageBox(_("Target deleted."), _("InVesalius 3"))	1539	wx.MessageBox(_("Target deleted."), _("InVesalius 3"))
1475		1540
1476	self.markers = []	1541	self.markers = []
		1542	+ self.robot_markers = []
1477	Publisher.sendMessage('Remove all markers', indexes=self.lc.GetItemCount())	1543	Publisher.sendMessage('Remove all markers', indexes=self.lc.GetItemCount())
1478	self.lc.DeleteAllItems()	1544	self.lc.DeleteAllItems()
1479	Publisher.sendMessage('Stop Blink Marker', index='DeleteAll')	1545	Publisher.sendMessage('Stop Blink Marker', index='DeleteAll')
	@@ -1500,6 +1566,8 @@ class MarkersPanel(wx.Panel):		@@ -1500,6 +1566,8 @@ class MarkersPanel(wx.Panel):
1500	if index:	1566	if index:
1501	if self.__find_target_marker() in index:	1567	if self.__find_target_marker() in index:
1502	Publisher.sendMessage('Disable or enable coil tracker', status=False)	1568	Publisher.sendMessage('Disable or enable coil tracker', status=False)
		1569	+ Publisher.sendMessage('Robot target matrix', robot_tracker_flag=False,
		1570	+ m_change_robot_to_head=None)
1503	wx.MessageBox(_("Target deleted."), _("InVesalius 3"))	1571	wx.MessageBox(_("Target deleted."), _("InVesalius 3"))
1504		1572
1505	self.__delete_multiple_markers(index)	1573	self.__delete_multiple_markers(index)
	@@ -1603,12 +1671,21 @@ class MarkersPanel(wx.Panel):		@@ -1603,12 +1671,21 @@ class MarkersPanel(wx.Panel):
1603	new_marker.seed = seed or self.current_seed	1671	new_marker.seed = seed or self.current_seed
1604	new_marker.session_id = session_id or self.current_session	1672	new_marker.session_id = session_id or self.current_session
1605		1673
		1674	+ if self.tracker.tracker_id == const.ROBOT and self.nav_status:
		1675	+ self.current_robot_target_matrix = db.compute_robot_to_head_matrix(self.raw_target_robot)
		1676	+ else:
		1677	+ self.current_robot_target_matrix = [None] * 9
		1678	+
		1679	+ new_robot_marker = self.Robot_Marker()
		1680	+ new_robot_marker.robot_target_matrix = self.current_robot_target_matrix
		1681	+
1606	# Note that ball_id is zero-based, so we assign it len(self.markers) before the new marker is added	1682	# Note that ball_id is zero-based, so we assign it len(self.markers) before the new marker is added
1607	Publisher.sendMessage('Add marker', ball_id=len(self.markers),	1683	Publisher.sendMessage('Add marker', ball_id=len(self.markers),
1608	size=new_marker.size,	1684	size=new_marker.size,
1609	colour=new_marker.colour,	1685	colour=new_marker.colour,
1610	coord=new_marker.coord[:3])	1686	coord=new_marker.coord[:3])
1611	self.markers.append(new_marker)	1687	self.markers.append(new_marker)
		1688	+ self.robot_markers.append(new_robot_marker)
1612		1689
1613	# Add item to list control in panel	1690	# Add item to list control in panel
1614	num_items = self.lc.GetItemCount()	1691	num_items = self.lc.GetItemCount()
@@ -0,0 +1,281 @@		@@ -0,0 +1,281 @@
	1	+#--------------------------------------------------------------------------
	2	+# Software: InVesalius - Software de Reconstrucao 3D de Imagens Medicas
	3	+# Copyright: (C) 2001 Centro de Pesquisas Renato Archer
	4	+# Homepage: http://www.softwarepublico.gov.br
	5	+# Contact: invesalius@cti.gov.br
	6	+# License: GNU - GPL 2 (LICENSE.txt/LICENCA.txt)
	7	+#--------------------------------------------------------------------------
	8	+# Este programa e software livre; voce pode redistribui-lo e/ou
	9	+# modifica-lo sob os termos da Licenca Publica Geral GNU, conforme
	10	+# publicada pela Free Software Foundation; de acordo com a versao 2
	11	+# da Licenca.
	12	+#
	13	+# Este programa eh distribuido na expectativa de ser util, mas SEM
	14	+# QUALQUER GARANTIA; sem mesmo a garantia implicita de
	15	+# COMERCIALIZACAO ou de ADEQUACAO A QUALQUER PROPOSITO EM
	16	+# PARTICULAR. Consulte a Licenca Publica Geral GNU para obter mais
	17	+# detalhes.
	18	+#--------------------------------------------------------------------------
	19	+import numpy as np
	20	+import wx
	21	+import queue
	22	+import threading
	23	+
	24	+import invesalius.data.bases as db
	25	+import invesalius.gui.dialogs as dlg
	26	+import invesalius.constants as const
	27	+from invesalius.pubsub import pub as Publisher
	28	+
	29	+try:
	30	+ import invesalius.data.elfin as elfin
	31	+ import invesalius.data.elfin_processing as elfin_process
	32	+ has_robot = True
	33	+except ImportError:
	34	+ has_robot = False
	35	+
	36	+class Robot():
	37	+ def __init__(self, tracker):
	38	+ """
	39	+ Class to establish the connection between the robot and InVesalius
	40	+ :param tracker: tracker.py instance
	41	+ """
	42	+ self.tracker = tracker
	43	+ self.trk_init = None
	44	+ self.robot_target_queue = None
	45	+ self.object_at_target_queue = None
	46	+ self.process_tracker = None
	47	+
	48	+ self.thread_robot = None
	49	+
	50	+ self.robotcoordinates = RobotCoordinates()
	51	+
	52	+ self.__bind_events()
	53	+
	54	+ def __bind_events(self):
	55	+ Publisher.subscribe(self.OnSendCoordinates, 'Send coord to robot')
	56	+ Publisher.subscribe(self.OnUpdateRobotTargetMatrix, 'Robot target matrix')
	57	+ Publisher.subscribe(self.OnObjectTarget, 'Coil at target')
	58	+ Publisher.subscribe(self.OnResetProcessTracker, 'Reset robot process')
	59	+
	60	+ def OnRobotConnection(self):
	61	+ if not self.tracker.trk_init[0][0] or not self.tracker.trk_init[1][0]:
	62	+ dlg.ShowNavigationTrackerWarning(self.tracker.tracker_id, self.tracker.trk_init[1])
	63	+ self.tracker.tracker_id = 0
	64	+ self.tracker.tracker_connected = False
	65	+ else:
	66	+ self.tracker.trk_init.append(self.robotcoordinates)
	67	+ self.process_tracker = elfin_process.TrackerProcessing()
	68	+ dlg_correg_robot = dlg.CreateTransformationMatrixRobot(self.tracker)
	69	+ if dlg_correg_robot.ShowModal() == wx.ID_OK:
	70	+ M_tracker_to_robot = dlg_correg_robot.GetValue()
	71	+ db.transform_tracker_to_robot.M_tracker_to_robot = M_tracker_to_robot
	72	+ self.robot_server = self.tracker.trk_init[1][0]
	73	+ self.trk_init = self.tracker.trk_init
	74	+ else:
	75	+ dlg.ShowNavigationTrackerWarning(self.tracker.tracker_id, 'disconnect')
	76	+ self.tracker.trk_init = None
	77	+ self.tracker.tracker_id = 0
	78	+ self.tracker.tracker_connected = False
	79	+
	80	+ def StartRobotThreadNavigation(self, tracker, coord_queue):
	81	+ if tracker.event_robot.is_set():
	82	+ tracker.event_robot.clear()
	83	+ self.thread_robot = ControlRobot(self.trk_init, tracker, self.robotcoordinates,
	84	+ [coord_queue, self.robot_target_queue, self.object_at_target_queue],
	85	+ self.process_tracker, tracker.event_robot)
	86	+ self.thread_robot.start()
	87	+
	88	+ def StopRobotThreadNavigation(self):
	89	+ self.thread_robot.join()
	90	+ self.OnResetProcessTracker()
	91	+
	92	+ def OnResetProcessTracker(self):
	93	+ self.process_tracker.__init__()
	94	+
	95	+ def OnSendCoordinates(self, coord):
	96	+ self.robot_server.SendCoordinates(coord)
	97	+
	98	+ def OnUpdateRobotTargetMatrix(self, robot_tracker_flag, m_change_robot_to_head):
	99	+ try:
	100	+ self.robot_target_queue.put_nowait([robot_tracker_flag, m_change_robot_to_head])
	101	+ except queue.Full:
	102	+ pass
	103	+
	104	+ def OnObjectTarget(self, state):
	105	+ try:
	106	+ if self.object_at_target_queue:
	107	+ self.object_at_target_queue.put_nowait(state)
	108	+ except queue.Full:
	109	+ pass
	110	+
	111	+ def SetRobotQueues(self, queues):
	112	+ self.robot_target_queue, self.object_at_target_queue = queues
	113	+
	114	+
	115	+class RobotCoordinates():
	116	+ """
	117	+ Class to set/get robot coordinates. Robot coordinates are acquired in ControlRobot thread.
	118	+ The class is required to avoid acquisition conflict with different threads (coordinates and navigation)
	119	+ """
	120	+ def __init__(self):
	121	+ self.coord = None
	122	+
	123	+ def SetRobotCoordinates(self, coord):
	124	+ self.coord = coord
	125	+
	126	+ def GetRobotCoordinates(self):
	127	+ return self.coord
	128	+
	129	+
	130	+class ControlRobot(threading.Thread):
	131	+ def __init__(self, trck_init, tracker, robotcoordinates, queues, process_tracker, event_robot):
	132	+ """Class (threading) to perform the robot control.
	133	+ A distinguish thread is required to increase perform and separate robot control from navigation thread
	134	+ (safetly layer for robot positining).
	135	+
	136	+ There is no GUI involved, them no Sleep is required
	137	+
	138	+ :param trck_init: Initialization variable of tracking device and connection type. See tracker.py.
	139	+ :param tracker: tracker.py instance
	140	+ :param robotcoordinates: RobotCoordinates() instance
	141	+ :param queues: Queue instance that manage coordinates and robot target
	142	+ :param process_tracker: TrackerProcessing() instance from elfin_process
	143	+ :param event_robot: Threading event to ControlRobot when tasks is done
	144	+ """
	145	+ threading.Thread.__init__(self, name='ControlRobot')
	146	+
	147	+ self.trck_init_robot = trck_init[1][0]
	148	+ self.trck_init_tracker = trck_init[0]
	149	+ self.trk_id = trck_init[2]
	150	+ self.tracker = tracker
	151	+ self.robotcoordinates = robotcoordinates
	152	+ self.robot_tracker_flag = False
	153	+ self.objattarget_flag = False
	154	+ self.target_flag = False
	155	+ self.m_change_robot_to_head = None
	156	+ self.coord_inv_old = None
	157	+ self.coord_queue = queues[0]
	158	+ self.robot_target_queue = queues[1]
	159	+ self.object_at_target_queue = queues[2]
	160	+ self.process_tracker = process_tracker
	161	+ self.event_robot = event_robot
	162	+ self.arc_motion_flag = False
	163	+ self.arc_motion_step_flag = None
	164	+ self.target_linear_out = None
	165	+ self.target_linear_in = None
	166	+ self.target_arc = None
	167	+
	168	+ def get_coordinates_from_tracker_devices(self):
	169	+ coord_robot_raw = self.trck_init_robot.Run()
	170	+ coord_robot = np.array(coord_robot_raw)
	171	+ coord_robot[3], coord_robot[5] = coord_robot[5], coord_robot[3]
	172	+ self.robotcoordinates.SetRobotCoordinates(coord_robot)
	173	+
	174	+ coord_raw, markers_flag = self.tracker.TrackerCoordinates.GetCoordinates()
	175	+
	176	+ return coord_raw, coord_robot_raw, markers_flag
	177	+
	178	+ def robot_move_decision(self, distance_target, new_robot_coordinates, current_robot_coordinates, current_head_filtered):
	179	+ """
	180	+ There are two types of robot movements.
	181	+ We can imagine in two concentric spheres of different sizes. The inside sphere is to compensate for small head movements.
	182	+ It was named "normal" moves.
	183	+ The outside sphere is for the arc motion. The arc motion is a safety feature for long robot movements.
	184	+ Even for a new target or a sudden huge head movement.
	185	+ 1) normal:
	186	+ A linear move from the actual position until the target position.
	187	+ This movement just happens when move distance is below a threshold (const.ROBOT_ARC_THRESHOLD_DISTANCE)
	188	+ 2) arc motion:
	189	+ It can be divided into three parts.
	190	+ The first one represents the movement from the inner sphere to the outer sphere.
	191	+ The robot moves back using a radial move (it use the center of the head as a reference).
	192	+ The second step is the actual arc motion (along the outer sphere).
	193	+ A middle point, between the actual position and the target, is required.
	194	+ The last step is to make a linear move until the target (goes to the inner sphere)
	195	+
	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
	229	+
	230	+ coord = self.target_arc
	231	+
	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
	236	+
	237	+ self.trck_init_robot.SendCoordinates(coord, self.arc_motion_step_flag)
	238	+
	239	+ 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]
	241	+ marker_head_flag = markers_flag[1]
	242	+ coord_obj_tracker_in_robot = current_tracker_coordinates_in_robot[2]
	243	+ marker_obj_flag = markers_flag[2]
	244	+
	245	+ if self.robot_tracker_flag:
	246	+ current_head = coord_head_tracker_in_robot
	247	+ if current_head is not None and marker_head_flag:
	248	+ current_head_filtered = self.process_tracker.kalman_filter(current_head)
	249	+ if self.process_tracker.compute_head_move_threshold(current_head_filtered):
	250	+ new_robot_coordinates = self.process_tracker.compute_head_move_compensation(current_head_filtered,
	251	+ self.m_change_robot_to_head)
	252	+ if self.coord_inv_old is None:
	253	+ self.coord_inv_old = new_robot_coordinates
	254	+
	255	+ if np.allclose(np.array(new_robot_coordinates), np.array(current_robot_coordinates), 0, 0.01):
	256	+ #avoid small movements (0.01 mm)
	257	+ pass
	258	+ elif not np.allclose(np.array(new_robot_coordinates), np.array(self.coord_inv_old), 0, 5):
	259	+ #if the head moves (>5mm) before the robot reach the target
	260	+ self.trck_init_robot.StopRobot()
	261	+ self.coord_inv_old = new_robot_coordinates
	262	+ else:
	263	+ distance_target = self.process_tracker.correction_distance_calculation_target(new_robot_coordinates, current_robot_coordinates)
	264	+ self.robot_move_decision(distance_target, new_robot_coordinates, current_robot_coordinates, current_head_filtered)
	265	+ self.coord_inv_old = new_robot_coordinates
	266	+ else:
	267	+ self.trck_init_robot.StopRobot()
	268	+
	269	+ def run(self):
	270	+ while not self.event_robot.is_set():
	271	+ current_tracker_coordinates_in_robot, current_robot_coordinates, markers_flag = self.get_coordinates_from_tracker_devices()
	272	+
	273	+ if not self.robot_target_queue.empty():
	274	+ self.robot_tracker_flag, self.m_change_robot_to_head = self.robot_target_queue.get_nowait()
	275	+ self.robot_target_queue.task_done()
	276	+
	277	+ if not self.object_at_target_queue.empty():
	278	+ self.target_flag = self.object_at_target_queue.get_nowait()
	279	+ self.object_at_target_queue.task_done()
	280	+
	281	+ self.robot_control(current_tracker_coordinates_in_robot, current_robot_coordinates, markers_flag)
	@@ -1,39 +0,0 @@	@@ -1,39 +0,0 @@
1	-
2	-[Marker]
3	-FacetsCount=1
4	-SliderControlledXpointsCount=0
5	-Name=3Coil_big
6	-
7	-[Facet1]
8	-VectorsCount=2
9	-
10	-[Facet1V1]
11	-EndPos(0,0)=-12.130921665071337
12	-EndPos(0,1)=4.771099944171531e-005
13	-EndPos(0,2)=-1.6777868612989378e-016
14	-EndPos(1,0)=12.131083622124752
15	-EndPos(1,1)=1.0800131569805988e-004
16	-EndPos(1,2)=-2.4831245547224282e-015
17	-
18	-[Facet1V2]
19	-EndPos(0,0)=-12.143021882072084
20	-EndPos(0,1)=-16.190735690907974
21	-EndPos(0,2)=-1.3422294890391503e-015
22	-EndPos(1,0)=-12.130921665071337
23	-EndPos(1,1)=4.771099944171531e-005
24	-EndPos(1,2)=-1.6777868612989378e-016
25	-
26	-[Tooltip2MarkerXf]
27	-Scale=1.
28	-S0=0.
29	-R0,0=1.
30	-R1,0=0.
31	-R2,0=0.
32	-S1=0.
33	-R0,1=0.
34	-R1,1=1.
35	-R2,1=0.
36	-S2=0.
37	-R0,2=0.
38	-R1,2=0.
39	-R2,2=1.
	@@ -5,3 +5,4 @@ python-rtmidi==1.4.9		@@ -5,3 +5,4 @@ python-rtmidi==1.4.9
5	python-socketio[client]==5.3.0	5	python-socketio[client]==5.3.0
6	requests==2.26.0	6	requests==2.26.0
7	uvicorn[standard]==0.15.0	7	uvicorn[standard]==0.15.0
		8	+opencv-python==4.5.3