Renan · Thiago Franco de Moraes · Olli-Pekka Kahilakoski
Showing 16 changed files 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/gui/dialogs.py
invesalius/gui/task_navigator.py
invesalius/navigation/icp.py
invesalius/navigation/navigation.py
invesalius/navigation/robot.py
invesalius/navigation/tracker.py
invesalius/net/neuronavigation_api.py
invesalius/net/remote_control.py
requirements_m1.txt
@@ -832,7 +832,6 @@ SEED_RADIUS = 1.5
 # Increased the default sleep parameter from 0.1 to 0.15 to decrease CPU load during navigation.
 SLEEP_NAVIGATION = 0.2
 SLEEP_COORDINATES = 0.05
-SLEEP_ROBOT = 0.01
 BRAIN_OPACITY = 0.6
 N_CPU = psutil.cpu_count()
@@ -864,14 +863,3 @@ 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 #mm/s
-ROBOT_ARC_THRESHOLD_DISTANCE = 100 #mm
-ROBOT_VERSOR_SCALE_FACTOR = 70
-#Robot Working Space is defined as 800mm in Elfin manual. For safety, the value is reduced by 5%.
-ROBOT_WORKING_SPACE = 760 #mm
-ROBOT_MOVE_STATE = {"free to move": 0,
-                    "in motion": 1009,
-                    "waiting for execution": 1013,
-                    "error": 1025}
@@ -163,6 +163,12 @@ def transform_icp(m_img, m_icp):
     return m_img
+def inverse_transform_icp(m_img, m_icp):
+    coord_img = [m_img[0, -1], -m_img[1, -1], m_img[2, -1], 1]
+    m_img[0, -1], m_img[1, -1], m_img[2, -1], _ = np.linalg.inv(m_icp) @ coord_img
+    m_img[0, -1], m_img[1, -1], m_img[2, -1] = m_img[0, -1], -m_img[1, -1], m_img[2, -1]
+
+    return m_img
 def object_registration(fiducials, orients, coord_raw, m_change):
     """
@@ -244,47 +250,3 @@ 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
-
-        translation, angles_as_deg = dco.transformation_matrix_to_coordinates(M_tracker_in_robot, axes='rzyx')
-        tracker_in_robot = list(translation) + list(angles_as_deg)
-
-        return tracker_in_robot
@@ -22,7 +22,6 @@ import numpy as np
 import threading
 import wx
-import invesalius.data.bases as db
 import invesalius.data.transformations as tr
 import invesalius.constants as const
@@ -47,14 +46,20 @@ class TrackerCoordinates():
     def SetCoordinates(self, coord, markers_flag):
         self.coord = coord
         self.markers_flag = markers_flag
-        if self.previous_markers_flag != self.markers_flag and not self.nav_status:
-            wx.CallAfter(Publisher.sendMessage, 'Sensors ID', markers_flag=self.markers_flag)
-            self.previous_markers_flag = self.markers_flag
+        if not self.nav_status:
+            wx.CallAfter(Publisher.sendMessage, 'Update tracker coordinates',
+                         coord=self.coord.tolist(), markers_flag=self.markers_flag)
+            if self.previous_markers_flag != self.markers_flag:
+                wx.CallAfter(Publisher.sendMessage, 'Sensors ID', markers_flag=self.markers_flag)
+                self.previous_markers_flag = self.markers_flag
     def GetCoordinates(self):
-        if self.previous_markers_flag != self.markers_flag and self.nav_status:
-            wx.CallAfter(Publisher.sendMessage, 'Sensors ID', markers_flag=self.markers_flag)
-            self.previous_markers_flag = self.markers_flag
+        if self.nav_status:
+            wx.CallAfter(Publisher.sendMessage, 'Update tracker coordinates',
+                         coord=self.coord.tolist(), markers_flag=self.markers_flag)
+            if self.previous_markers_flag != self.markers_flag:
+                wx.CallAfter(Publisher.sendMessage, 'Sensors ID', markers_flag=self.markers_flag)
+                self.previous_markers_flag = self.markers_flag
         return self.coord, self.markers_flag
@@ -195,16 +200,6 @@ def PolarisCoord(trck_init, trck_id, ref_mode):
     return coord, [trck.probeID, trck.refID, trck.objID]
-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:
-        coord = np.array([0, 0, 0, 0, 0, 0])
-
-    return coord
 def CameraCoord(trck_init, trck_id, ref_mode):
     trck = trck_init[0]
@@ -346,6 +341,13 @@ def PolhemusSerialCoord(trck_init, trck_id, ref_mode):
     return coord
+def RobotCoord(trk_init, trck_id, ref_mode):
+    #trck_id is the tracker related to the robot ID. To get the tracker ID, combined with the robot,
+    # it is required to get trk_init[1]
+    tracker_id = trk_init[1]
+    coord_tracker, markers_flag = GetCoordinatesForThread(trk_init[0], tracker_id, ref_mode)
+
+    return np.vstack([coord_tracker[0], coord_tracker[1], coord_tracker[2]]), markers_flag
 def DebugCoordRandom(trk_init, trck_id, ref_mode):
     """
@@ -497,22 +499,6 @@ 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][0], trk_init[1], ref_mode)
-    coord_robot = ElfinCoord([trk_init[0][1]]+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
@@ -590,4 +576,4 @@ class ReceiveCoordinates(threading.Thread):
         while not self.event.is_set():
             coord_raw, markers_flag = GetCoordinatesForThread(self.trck_init, self.trck_id, const.DEFAULT_REF_MODE)
             self.TrackerCoordinates.SetCoordinates(coord_raw, markers_flag)
-            sleep(const.SLEEP_COORDINATES)
 \ No newline at end of file
+            sleep(const.SLEEP_COORDINATES)
@@ -101,6 +101,32 @@ def tracker_to_image(m_change, m_probe_ref, r_obj_img, m_obj_raw, s0_dyn):
     m_img[:3, :3] = r_obj[:3, :3]
     return m_img
+def image_to_tracker(m_change, target, icp):
+    """Compute the transformation matrix to the tracker coordinate system
+
+    :param m_change: Corregistration transformation obtained from fiducials
+    :type m_change: numpy.ndarray
+    :param target: Target in invesalius coordinate system
+    :type target: numpy.ndarray
+    :param icp: ICP transformation matrix
+    :type icp: numpy.ndarray
+
+    :return: 4 x 4 numpy double array
+    :rtype: numpy.ndarray
+    """
+    m_target_in_image = dco.coordinates_to_transformation_matrix(
+        position=target[:3],
+        orientation=[0, 0, 0],
+        axes='sxyz',
+    )
+    if icp.use_icp:
+        m_target_in_image = bases.inverse_transform_icp(m_target_in_image, icp.m_icp)
+    m_target_in_tracker = np.linalg.inv(m_change) @ m_target_in_image
+
+    # invert y coordinate
+    m_target_in_tracker[2, -1] = -m_target_in_tracker[2, -1]
+
+    return m_target_in_tracker
 def corregistrate_object_dynamic(inp, coord_raw, ref_mode_id, icp):
@@ -193,7 +219,6 @@ class CoordinateCorregistrate(threading.Thread):
         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
@@ -254,7 +279,7 @@ class CoordinateCorregistrate(threading.Thread):
                 if self.icp:
                     m_img = bases.transform_icp(m_img, self.m_icp)
-                self.coord_queue.put_nowait([coord, [coord_raw, markers_flag], m_img, view_obj])
+                self.coord_queue.put_nowait([coord, markers_flag, m_img, view_obj])
                 # print('CoordCoreg: put {}'.format(count))
                 # count += 1
@@ -307,7 +332,7 @@ class CoordinateCorregistrateNoObject(threading.Thread):
                 if self.icp:
                     m_img = bases.transform_icp(m_img, self.m_icp)
-                self.coord_queue.put_nowait([coord, [coord_raw, markers_flag], m_img, view_obj])
+                self.coord_queue.put_nowait([coord, markers_flag, m_img, view_obj])
                 if self.view_tracts:
                     self.coord_tracts_queue.put_nowait(m_img_flip)
@@ -18,6 +18,7 @@
 #--------------------------------------------------------------------------
 import invesalius.constants as const
 import invesalius.gui.dialogs as dlg
+from invesalius.pubsub import pub as Publisher
 # TODO: Disconnect tracker when a new one is connected
 # TODO: Test if there are too many prints when connection fails
 # TODO: Redesign error messages. No point in having "Could not connect to default tracker" in all trackers
@@ -223,42 +224,23 @@ def OptitrackTracker(tracker_id):
         print('#####')
     return trck_init, lib_mode
-def ElfinRobot(robot_IP):
-    trck_init = None
-    try:
-        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()
-        lib_mode = 'wrapper'
-        print('Connect to elfin robot tracking device.')
-
-    except:
-        lib_mode = 'error'
-        trck_init = None
-        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]:
+            trck_init = TrackerConnection(tracker_id, None, 'connect')
+            if trck_init[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)
+                    Publisher.sendMessage('Connect to robot', robot_IP=robot_IP)
+
+                    return trck_init, tracker_id
-    return [(trck_init, trck_init_robot), tracker_id]
+    return None, None
 def DebugTrackerRandom(tracker_id):
     trck_init = True
@@ -392,8 +374,9 @@ def DisconnectTracker(tracker_id, trck_init):
                 lib_mode = 'serial'
                 print('Tracker disconnected.')
             elif tracker_id == const.ROBOT:
-                trck_init[0][0].Close()
-                trck_init[1][0].Close()
+                Publisher.sendMessage('Reset robot', data=None)
+                if trck_init[1] == const.DEBUGTRACKRANDOM or trck_init[1] == const.DEBUGTRACKAPPROACH:
+                    trck_init[0].Close()
                 trck_init = False
                 lib_mode = 'wrapper'
                 print('Tracker disconnected.')
@@ -57,8 +57,6 @@ except ImportError:
     from wx import AboutDialogInfo, AboutBox
 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
@@ -3597,11 +3595,7 @@ 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:
-            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, :]
+            coord = coord_raw[self.obj_ref_id, :]
         else:
             coord = coord_raw[0, :]
@@ -4323,7 +4317,7 @@ class SetTrackerDeviceToRobot(wx.Dialog):
     def _init_gui(self):
         # ComboBox for spatial tracker device selection
         tooltip = wx.ToolTip(_("Choose the tracking device"))
-        trackers = const.TRACKERS
+        trackers = const.TRACKERS.copy()
         if not ses.Session().debug:
             del trackers[-3:]
         tracker_options = [_("Select tracker:")] + trackers
@@ -4425,10 +4419,8 @@ class CreateTransformationMatrixRobot(wx.Dialog):
         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.matrix_tracker_to_robot = []
+        self.robot_status = False
         self.tracker = tracker
@@ -4470,6 +4462,8 @@ class CreateTransformationMatrixRobot(wx.Dialog):
         btn_load = wx.Button(self, -1, label=_('Load'), size=wx.Size(65, 23))
         btn_load.Bind(wx.EVT_BUTTON, self.OnLoadReg)
+        btn_load.Enable(False)
+        self.btn_load = btn_load
         # Create a horizontal sizers
         border = 1
@@ -4524,16 +4518,17 @@ class CreateTransformationMatrixRobot(wx.Dialog):
         main_sizer.Fit(self)
         self.CenterOnParent()
+        self.__bind_events()
-    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 __bind_events(self):
+        Publisher.subscribe(self.OnUpdateTransformationMatrix, 'Update robot transformation matrix')
+        Publisher.subscribe(self.OnCoordinatesAdquired, 'Coordinates for the robot transformation matrix collected')
+        Publisher.subscribe(self.OnRobotConnectionStatus, 'Robot connection status')
     def OnContinuousAcquisition(self, evt=None, btn=None):
         value = btn.GetValue()
         if value:
-            self.timer.Start(500)
+            self.timer.Start(100)
         else:
             self.timer.Stop()
@@ -4541,56 +4536,50 @@ class CreateTransformationMatrixRobot(wx.Dialog):
         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[0][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')
+        Publisher.sendMessage('Collect coordinates for the robot transformation matrix', data=None)
+
+    def OnCoordinatesAdquired(self):
+        self.txt_number.SetLabel(str(int(self.txt_number.GetLabel())+1))
-        if len(self.tracker_coord) >= 3:
+        if self.robot_status and int(self.txt_number.GetLabel()) >= 3:
             self.btn_apply_reg.Enable(True)
+    def OnRobotConnectionStatus(self, data):
+        self.robot_status = data
+        if self.robot_status:
+            self.btn_load.Enable(True)
+            if int(self.txt_number.GetLabel()) >= 3:
+                self.btn_apply_reg.Enable(True)
+
     def OnReset(self, evt):
+        Publisher.sendMessage('Reset coordinates collection for the robot transformation matrix', data=None)
         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)
+        self.matrix_tracker_to_robot = []
+
     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
+        Publisher.sendMessage('Robot transformation matrix estimation', data=None)
         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 OnUpdateTransformationMatrix(self, data):
+        self.matrix_tracker_to_robot = np.array(data)
+
     def OnSaveReg(self, evt):
         filename = ShowLoadSaveDialog(message=_(u"Save robot transformation file as..."),
                                           wildcard=_("Robot transformation files (*.rbtf)|*.rbtf"),
@@ -4598,10 +4587,10 @@ class CreateTransformationMatrixRobot(wx.Dialog):
                                           default_filename="robottransform.rbtf", save_ext="rbtf")
         if filename:
-            if self.M_tracker_2_robot is not None:
+            if self.matrix_tracker_to_robot is not None:
                 with open(filename, 'w', newline='') as file:
                     writer = csv.writer(file, delimiter='\t')
-                    writer.writerows(self.M_tracker_2_robot)
+                    writer.writerows(np.vstack(self.matrix_tracker_to_robot).tolist())
     def OnLoadReg(self, evt):
         filename = ShowLoadSaveDialog(message=_(u"Load robot transformation"),
@@ -4611,12 +4600,12 @@ class CreateTransformationMatrixRobot(wx.Dialog):
                 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)
+            self.matrix_tracker_to_robot = np.vstack(list(np.float_(content)))
+            print("Matrix tracker to robot:", self.matrix_tracker_to_robot)
+            Publisher.sendMessage('Load robot transformation matrix', data=self.matrix_tracker_to_robot.tolist())
+            if self.robot_status:
+                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:")):
@@ -20,10 +20,7 @@
 import dataclasses
 from functools import partial
 import itertools
-import csv
-import queue
 import time
-import threading
 import nibabel as nb
 import numpy as np
@@ -41,7 +38,6 @@ except ImportError:
     has_robot = False
 import wx
-import vtk
 try:
     import wx.lib.agw.foldpanelbar as fpb
@@ -51,7 +47,6 @@ except ImportError:
 import wx.lib.colourselect as csel
 import wx.lib.masked.numctrl
 from invesalius.pubsub import pub as Publisher
-from time import sleep
 import invesalius.constants as const
@@ -64,6 +59,7 @@ 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.data.coregistration as dcr
 import invesalius.gui.dialogs as dlg
 import invesalius.project as prj
 import invesalius.session as ses
@@ -73,7 +69,6 @@ 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
 from invesalius.data.converters import to_vtk
 from invesalius.net.neuronavigation_api import NeuronavigationApi
@@ -176,7 +171,7 @@ class InnerFoldPanel(wx.Panel):
         #
         tracker = Tracker()
         pedal_connection = PedalConnection() if HAS_PEDAL_CONNECTION else None
-
+        icp = ICP()
         neuronavigation_api = NeuronavigationApi()
         navigation = Navigation(
             pedal_connection=pedal_connection,
@@ -194,6 +189,7 @@ class InnerFoldPanel(wx.Panel):
             parent=item,
             navigation=navigation,
             tracker=tracker,
+            icp=icp,
             pedal_connection=pedal_connection,
             neuronavigation_api=neuronavigation_api,
         )
@@ -205,7 +201,12 @@ class InnerFoldPanel(wx.Panel):
         # Fold 2 - Object registration panel
         item = fold_panel.AddFoldPanel(_("Object registration"), collapsed=True)
-        otw = ObjectRegistrationPanel(item, tracker, pedal_connection)
+        otw = ObjectRegistrationPanel(
+            parent=item,
+            tracker=tracker,
+            pedal_connection=pedal_connection,
+            neuronavigation_api=neuronavigation_api,
+        )
         fold_panel.ApplyCaptionStyle(item, style)
         fold_panel.AddFoldPanelWindow(item, otw, spacing=0,
@@ -213,7 +214,7 @@ class InnerFoldPanel(wx.Panel):
         # Fold 3 - Markers panel
         item = fold_panel.AddFoldPanel(_("Markers"), collapsed=True)
-        mtw = MarkersPanel(item, navigation, tracker)
+        mtw = MarkersPanel(item, navigation, tracker, icp)
         fold_panel.ApplyCaptionStyle(item, style)
         fold_panel.AddFoldPanelWindow(item, mtw, spacing= 0,
@@ -352,7 +353,7 @@ class InnerFoldPanel(wx.Panel):
 class NeuronavigationPanel(wx.Panel):
-    def __init__(self, parent, navigation, tracker, pedal_connection, neuronavigation_api):
+    def __init__(self, parent, navigation, tracker, icp, pedal_connection, neuronavigation_api):
         wx.Panel.__init__(self, parent)
         try:
             default_colour = wx.SystemSettings.GetColour(wx.SYS_COLOUR_MENUBAR)
@@ -369,9 +370,8 @@ class NeuronavigationPanel(wx.Panel):
         self.neuronavigation_api = neuronavigation_api
         self.navigation = navigation
-        self.icp = ICP()
+        self.icp = icp
         self.tracker = tracker
-        self.robot = Robot(tracker)
         self.nav_status = False
         self.tracker_fiducial_being_set = None
@@ -558,6 +558,7 @@ class NeuronavigationPanel(wx.Panel):
         Publisher.subscribe(self.UpdateTarget, 'Update target')
         Publisher.subscribe(self.OnStartNavigation, 'Start navigation')
         Publisher.subscribe(self.OnStopNavigation, 'Stop navigation')
+        Publisher.subscribe(self.OnDialogRobotDestroy, 'Dialog robot destroy')
     def LoadImageFiducials(self, label, coord):
         fiducial = self.GetFiducialByAttribute(const.IMAGE_FIDUCIALS, 'label', label)
@@ -671,8 +672,6 @@ 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)
@@ -691,7 +690,12 @@ class NeuronavigationPanel(wx.Panel):
         self.tracker.SetTracker(choice)
         if self.tracker.tracker_id == const.ROBOT:
-            self.tracker.ConnectToRobot(self.navigation, self.tracker, self.robot)
+            self.dlg_correg_robot = dlg.CreateTransformationMatrixRobot(self.tracker)
+            if self.dlg_correg_robot.ShowModal() == wx.ID_OK:
+                Publisher.sendMessage('Robot navigation mode', robot_mode=True)
+            else:
+                Publisher.sendMessage('Disconnect tracker')
+                wx.MessageBox(_("Not possible to connect to the robot."), _("InVesalius 3"))
         self.ResetICP()
         self.tracker.UpdateUI(ctrl, self.numctrls_fiducial[3:6], self.txtctrl_fre)
@@ -776,8 +780,8 @@ class NeuronavigationPanel(wx.Panel):
         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)
+            Publisher.sendMessage('Update robot target', robot_tracker_flag=False,
+                                  target_index=None, target=None)
         # Enable all navigation buttons
         choice_ref.Enable(True)
@@ -786,6 +790,10 @@ class NeuronavigationPanel(wx.Panel):
         for btn_c in self.btns_set_fiducial:
             btn_c.Enable(True)
+    def OnDialogRobotDestroy(self):
+        if self.dlg_correg_robot:
+            self.dlg_correg_robot.Destroy()
+
     def CheckFiducialRegistrationError(self):
         self.navigation.UpdateFiducialRegistrationError(self.tracker)
         fre, fre_ok = self.navigation.GetFiducialRegistrationError(self.icp)
@@ -820,7 +828,7 @@ class NeuronavigationPanel(wx.Panel):
             for btn_c in self.btns_set_fiducial:
                 btn_c.Enable(False)
-            self.navigation.StartNavigation(self.tracker)
+            self.navigation.EstimateTrackerToInVTransformationMatrix(self.tracker)
             if not self.CheckFiducialRegistrationError():
                 # TODO: Exhibit FRE in a warning dialog and only starts navigation after user clicks ok
@@ -830,9 +838,11 @@ class NeuronavigationPanel(wx.Panel):
             if self.icp.use_icp:
                 self.checkbox_icp.Enable(True)
                 self.checkbox_icp.SetValue(True)
-            # Update FRE once more after starting the navigation, due to the optional use of ICP,
-            # which improves FRE.
-            self.CheckFiducialRegistrationError()
+                # Update FRE once more after starting the navigation, due to the optional use of ICP,
+                # which improves FRE.
+                self.CheckFiducialRegistrationError()
+
+            self.navigation.StartNavigation(self.tracker)
     def OnNavigate(self, evt, btn_nav):
         select_tracker_elem = self.select_tracker_elem
@@ -881,11 +891,10 @@ class NeuronavigationPanel(wx.Panel):
         )
         self.tracker.__init__()
         self.icp.__init__()
-        self.robot.__init__(self.tracker)
 class ObjectRegistrationPanel(wx.Panel):
-    def __init__(self, parent, tracker, pedal_connection):
+    def __init__(self, parent, tracker, pedal_connection, neuronavigation_api):
         wx.Panel.__init__(self, parent)
         try:
             default_colour = wx.SystemSettings.GetColour(wx.SYS_COLOUR_MENUBAR)
@@ -897,6 +906,7 @@ class ObjectRegistrationPanel(wx.Panel):
         self.tracker = tracker
         self.pedal_connection = pedal_connection
+        self.neuronavigation_api = neuronavigation_api
         self.nav_prop = None
         self.obj_fiducials = None
@@ -913,7 +923,7 @@ class ObjectRegistrationPanel(wx.Panel):
         btn_new = wx.Button(self, -1, _("New"), size=wx.Size(65, 23))
         btn_new.SetToolTip(tooltip)
         btn_new.Enable(1)
-        btn_new.Bind(wx.EVT_BUTTON, self.OnLinkCreate)
+        btn_new.Bind(wx.EVT_BUTTON, self.OnCreateNewCoil)
         self.btn_new = btn_new
         # Button for import config coil file
@@ -1056,13 +1066,16 @@ class ObjectRegistrationPanel(wx.Panel):
         coil_index = evt.GetSelection()
         Publisher.sendMessage('Change selected coil', self.coil_list[coil_index][1])
-    def OnLinkCreate(self, event=None):
+    def OnCreateNewCoil(self, event=None):
         if self.tracker.IsTrackerInitialized():
             dialog = dlg.ObjectCalibrationDialog(self.tracker, self.pedal_connection)
             try:
                 if dialog.ShowModal() == wx.ID_OK:
                     self.obj_fiducials, self.obj_orients, self.obj_ref_mode, self.obj_name, polydata, use_default_object = dialog.GetValue()
+
+                    self.neuronavigation_api.update_coil_mesh(polydata)
+
                     if np.isfinite(self.obj_fiducials).all() and np.isfinite(self.obj_orients).all():
                         self.checktrack.Enable(1)
                         Publisher.sendMessage('Update object registration',
@@ -1248,20 +1261,7 @@ class MarkersPanel(wx.Panel):
                 if field.type is bool:
                     setattr(self, field.name, str_val=='True')
-    @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, navigation, tracker):
+    def __init__(self, parent, navigation, tracker, icp):
         wx.Panel.__init__(self, parent)
         try:
             default_colour = wx.SystemSettings.GetColour(wx.SYS_COLOUR_MENUBAR)
@@ -1273,6 +1273,7 @@ class MarkersPanel(wx.Panel):
         self.navigation = navigation
         self.tracker = tracker
+        self.icp = icp
         self.__bind_events()
@@ -1280,11 +1281,9 @@ class MarkersPanel(wx.Panel):
         self.current_coord = [0, 0, 0, None, None, None]
         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
@@ -1383,7 +1382,6 @@ 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):
         """
@@ -1417,7 +1415,6 @@ 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))
@@ -1470,34 +1467,37 @@ class MarkersPanel(wx.Panel):
     def UpdateSeedCoordinates(self, root=None, affine_vtk=None, coord_offset=(0, 0, 0), coord_offset_w=(0, 0, 0)):
         self.current_seed = coord_offset_w
-    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()
         edit_id = menu_id.Append(0, _('Edit label'))
         menu_id.Bind(wx.EVT_MENU, self.OnMenuEditMarkerLabel, edit_id)
-        color_id = menu_id.Append(2, _('Edit color'))
+        color_id = menu_id.Append(1, _('Edit color'))
         menu_id.Bind(wx.EVT_MENU, self.OnMenuSetColor, color_id)
         menu_id.AppendSeparator()
-        target_menu = menu_id.Append(1, _('Set as target'))
+        target_menu = menu_id.Append(2, _('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)
-        if all([elem is not None for elem in self.markers[self.lc.GetFocusedItem()].coord[3:]]):
+        check_target_angles = all([elem is not None for elem in self.markers[self.lc.GetFocusedItem()].coord[3:]])
+        # Enable "Send target to robot" button only if tracker is robot, if navigation is on and if target is not none
+        if self.tracker.tracker_id == const.ROBOT:
+            send_target_to_robot_compensation = menu_id.Append(3, _('Sets target to robot head move compensation'))
+            menu_id.Bind(wx.EVT_MENU, self.OnMenuSetRobotCompensation, send_target_to_robot_compensation)
+            send_target_to_robot = menu_id.Append(4, _('Send target from InVesalius to robot'))
+            menu_id.Bind(wx.EVT_MENU, self.OnMenuSendTargetToRobot, send_target_to_robot)
+            if self.nav_status and check_target_angles:
+                send_target_to_robot_compensation.Enable(True)
+                send_target_to_robot.Enable(True)
+            else:
+                send_target_to_robot_compensation.Enable(False)
+                send_target_to_robot.Enable(False)
+
+        if check_target_angles:
             target_menu.Enable(True)
         else:
             target_menu.Enable(False)
-        # 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)
@@ -1547,14 +1547,33 @@ class MarkersPanel(wx.Panel):
             Publisher.sendMessage('Set new color', index=index, color=color_new)
+    def OnMenuSetRobotCompensation(self, evt):
+        if isinstance(evt, int):
+           self.lc.Focus(evt)
+
+        Publisher.sendMessage('Reset robot process', data=None)
+        matrix_tracker_fiducials = self.tracker.GetMatrixTrackerFiducials()
+        Publisher.sendMessage('Update tracker fiducials matrix',
+                              matrix_tracker_fiducials=matrix_tracker_fiducials)
+        Publisher.sendMessage('Update robot target', robot_tracker_flag=True, target_index=self.lc.GetFocusedItem(), target=None)
+
     def OnMenuSendTargetToRobot(self, evt):
         if isinstance(evt, int):
            self.lc.Focus(evt)
+        index = self.lc.GetFocusedItem()
+        if index == -1:
+            wx.MessageBox(_("No data selected."), _("InVesalius 3"))
+            return
+
+        Publisher.sendMessage('Reset robot process', data=None)
+        matrix_tracker_fiducials = self.tracker.GetMatrixTrackerFiducials()
+        Publisher.sendMessage('Update tracker fiducials matrix',
+                              matrix_tracker_fiducials=matrix_tracker_fiducials)
-        m_target_robot = self.robot_markers[self.lc.GetFocusedItem()].robot_target_matrix
+        target_coord = self.markers[index].coord[:3]
+        target = dcr.image_to_tracker(self.navigation.m_change, target_coord, self.icp)
-        Publisher.sendMessage('Reset robot process')
-        Publisher.sendMessage('Robot target matrix', robot_tracker_flag=True, m_change_robot_to_head=m_target_robot)
+        Publisher.sendMessage('Update robot target', robot_tracker_flag=True, target_index=self.lc.GetFocusedItem(), target=target.tolist())
     def OnDeleteAllMarkers(self, evt=None):
         if evt is not None:
@@ -1566,9 +1585,11 @@ class MarkersPanel(wx.Panel):
             Publisher.sendMessage('Disable or enable coil tracker', status=False)
             if evt is not None:
                 wx.MessageBox(_("Target deleted."), _("InVesalius 3"))
+            if self.tracker.tracker_id == const.ROBOT:
+                Publisher.sendMessage('Update robot target', robot_tracker_flag=False,
+                                      target_index=None, target=None)
         self.markers = []
-        self.robot_markers = []
         Publisher.sendMessage('Remove all markers', indexes=self.lc.GetItemCount())
         self.lc.DeleteAllItems()
         Publisher.sendMessage('Stop Blink Marker', index='DeleteAll')
@@ -1596,8 +1617,8 @@ class MarkersPanel(wx.Panel):
             if self.__find_target_marker() in index:
                 Publisher.sendMessage('Disable or enable coil tracker', status=False)
                 if self.tracker.tracker_id == const.ROBOT:
-                    Publisher.sendMessage('Robot target matrix', robot_tracker_flag=False,
-                                          m_change_robot_to_head=[])
+                    Publisher.sendMessage('Update robot target', robot_tracker_flag=False,
+                                          target_index=None, target=None)
                 wx.MessageBox(_("Target deleted."), _("InVesalius 3"))
             self.__delete_multiple_markers(index)
@@ -1702,12 +1723,11 @@ class MarkersPanel(wx.Panel):
         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)
+            current_head_robot_target_status = True
         else:
-            self.current_robot_target_matrix = [None] * 9
+            current_head_robot_target_status = False
-        new_robot_marker = self.Robot_Marker()
-        new_robot_marker.robot_target_matrix = self.current_robot_target_matrix
+        Publisher.sendMessage('Add marker to robot control', data=current_head_robot_target_status)
         # Note that ball_id is zero-based, so we assign it len(self.markers) before the new marker is added
         if all([elem is not None for elem in new_marker.coord[3:]]):
@@ -1723,7 +1743,6 @@ class MarkersPanel(wx.Panel):
         self.markers.append(new_marker)
-        self.robot_markers.append(new_robot_marker)
         # Add item to list control in panel
         num_items = self.lc.GetItemCount()
@@ -21,10 +21,10 @@ import wx
 import invesalius.data.bases as db
 import invesalius.gui.dialogs as dlg
-from invesalius.pubsub import pub as Publisher
+from invesalius.utils import Singleton
-class ICP():
+class ICP(metaclass=Singleton):
     def __init__(self):
         self.use_icp = False
         self.m_icp = None
@@ -35,7 +35,6 @@ class ICP():
         if not self.use_icp:
             if dlg.ICPcorregistration(navigation.fre):
-                Publisher.sendMessage('Stop navigation')
                 use_icp, self.m_icp = self.OnICP(navigation, tracker, navigation.m_change)
                 if use_icp:
                     self.icp_fre = db.calculate_fre(tracker.tracker_fiducials_raw, navigation.all_fiducials,
@@ -43,7 +42,6 @@ class ICP():
                     self.SetICP(navigation, use_icp)
                 else:
                     print("ICP canceled")
-                Publisher.sendMessage('Start navigation')
     def OnICP(self, navigation, tracker, m_change):
         ref_mode_id = navigation.GetReferenceMode()
@@ -34,6 +34,7 @@ import invesalius.data.tractography as dti
 import invesalius.data.transformations as tr
 import invesalius.data.vtk_utils as vtk_utils
 from invesalius.pubsub import pub as Publisher
+from invesalius.utils import Singleton
 class QueueCustom(queue.Queue):
@@ -83,7 +84,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, self.robot_target_queue = vis_queues
+        self.coord_queue, self.serial_port_queue, self.tracts_queue, self.icp_queue = vis_queues
         self.sle = sle
         self.event = event
         self.neuronavigation_api = neuronavigation_api
@@ -93,7 +94,7 @@ class UpdateNavigationScene(threading.Thread):
         while not self.event.is_set():
             got_coords = False
             try:
-                coord, [coordinates_raw, markers_flag], m_img, view_obj = self.coord_queue.get_nowait()
+                coord, markers_flag, m_img, view_obj = self.coord_queue.get_nowait()
                 got_coords = True
                 # print('UpdateScene: get {}'.format(count))
@@ -117,7 +118,6 @@ 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')
                 wx.CallAfter(Publisher.sendMessage, 'Sensor ID', markers_flag=markers_flag)
@@ -140,7 +140,7 @@ class UpdateNavigationScene(threading.Thread):
                     self.coord_queue.task_done()
-class Navigation():
+class Navigation(metaclass=Singleton):
     def __init__(self, pedal_connection, neuronavigation_api):
         self.pedal_connection = pedal_connection
         self.neuronavigation_api = neuronavigation_api
@@ -157,7 +157,6 @@ class Navigation():
         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)
@@ -245,9 +244,14 @@ class Navigation():
         if state and permission_to_stimulate:
             self.serial_port_connection.SendPulse()
-    def StartNavigation(self, tracker):
+    def EstimateTrackerToInVTransformationMatrix(self, tracker):
         tracker_fiducials, tracker_fiducials_raw = tracker.GetTrackerFiducials()
+        self.all_fiducials = np.vstack([self.image_fiducials, tracker_fiducials])
+        self.m_change = tr.affine_matrix_from_points(self.all_fiducials[3:, :].T, self.all_fiducials[:3, :].T,
+                                                shear=False, scale=False)
+
+    def StartNavigation(self, tracker):
         # initialize jobs list
         jobs_list = []
@@ -255,17 +259,9 @@ 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, self.robot_target_queue]
+        vis_queues = [self.coord_queue, self.serial_port_queue, self.tracts_queue, self.icp_queue]
         Publisher.sendMessage("Navigation status", nav_status=True, vis_status=vis_components)
-
-        self.all_fiducials = np.vstack([self.image_fiducials, tracker_fiducials])
-
-        # fiducials matrix
-        m_change = tr.affine_matrix_from_points(self.all_fiducials[3:, :].T, self.all_fiducials[:3, :].T,
-                                                shear=False, scale=False)
-        self.m_change = m_change
-
         errors = False
         if self.track_obj:
@@ -279,14 +275,14 @@ class Navigation():
                 # obj_reg[0] is object 3x3 fiducial matrix and obj_reg[1] is 3x3 orientation matrix
                 obj_fiducials, obj_orients, obj_ref_mode, obj_name = self.obj_reg
-                coreg_data = [m_change, obj_ref_mode]
+                coreg_data = [self.m_change, obj_ref_mode]
                 if self.ref_mode_id:
                     coord_raw, markers_flag = tracker.TrackerCoordinates.GetCoordinates()
                 else:
                     coord_raw = np.array([None])
-                obj_data = db.object_registration(obj_fiducials, obj_orients, coord_raw, m_change)
+                obj_data = db.object_registration(obj_fiducials, obj_orients, coord_raw, self.m_change)
                 coreg_data.extend(obj_data)
                 queues = [self.coord_queue, self.coord_tracts_queue, self.icp_queue, self.object_at_target_queue]
@@ -295,7 +291,7 @@ class Navigation():
                                                                 self.event, self.sleep_nav, tracker.tracker_id,
                                                                 self.target))
         else:
-            coreg_data = (m_change, 0)
+            coreg_data = (self.m_change, 0)
             queues = [self.coord_queue, self.coord_tracts_queue, self.icp_queue]
             jobs_list.append(dcr.CoordinateCorregistrateNoObject(self.ref_mode_id, tracker, coreg_data,
                                                                     self.view_tracts, queues,
@@ -42,7 +42,6 @@ class Tracker():
         self.thread_coord = None
         self.event_coord = threading.Event()
-        self.event_robot = threading.Event()
         self.TrackerCoordinates = dco.TrackerCoordinates()
@@ -78,11 +77,8 @@ 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"))
@@ -92,13 +88,6 @@ 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[0][1][0]
-        if trk_init_robot:
-            robot.StartRobotThreadNavigation(tracker, navigation.coord_queue)
-            Publisher.sendMessage('Robot navigation mode', robot_mode=True)
     def IsTrackerInitialized(self):
         return self.trk_init and self.tracker_id and self.tracker_connected
@@ -158,7 +147,7 @@ class Tracker():
         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]
+        return [m_probe_ref_left.tolist(), m_probe_ref_right.tolist(), m_probe_ref_nasion.tolist()]
     def GetTrackerInfo(self):
         return self.trk_init, self.tracker_id
@@ -20,6 +20,9 @@
 from invesalius.pubsub import pub as Publisher
 from invesalius.utils import Singleton
+import numpy as np
+from vtk.numpy_interface import dataset_adapter
+
 class NeuronavigationApi(metaclass=Singleton):
     """
     An API used internally in InVesalius to communicate with the
@@ -34,6 +37,8 @@ class NeuronavigationApi(metaclass=Singleton):
     If connection object is not given or it is None, skip doing the updates.
     """
+    N_VERTICES_IN_POLYGON = 3
+
     def __init__(self, connection=None):
         if connection is not None:
             assert self._hasmethod(connection, 'update_coil_pose')
@@ -65,3 +70,28 @@ class NeuronavigationApi(metaclass=Singleton):
                 position=position,
                 orientation=orientation,
             )
+
+    def update_coil_mesh(self, polydata):
+        wrapped = dataset_adapter.WrapDataObject(polydata)
+
+        points = np.asarray(wrapped.Points)
+        polygons_raw = np.asarray(wrapped.Polygons)
+
+        # The polygons are returned as 1d-array of the form
+        #
+        # [n_0, id_0(0), id_0(1), ..., id_0(n_0),
+        #  n_1, id_1(0), id_1(1), ..., id_1(n_1),
+        #  ...]
+        #
+        # where n_i is the number of vertices in polygon i, and id_i's are indices to the vertex list.
+        #
+        # Assert that all polygons have an equal number of vertices, reshape the array, and drop n_i's.
+        #
+        assert np.all(polygons_raw[0::self.N_VERTICES_IN_POLYGON + 1] == self.N_VERTICES_IN_POLYGON)
+
+        polygons = polygons_raw.reshape(-1, self.N_VERTICES_IN_POLYGON + 1)[:, 1:]
+
+        self.connection.update_coil_mesh(
+            points=points,
+            polygons=polygons,
+        )
@@ -79,5 +79,7 @@ class RemoteControl:
                     })
             except TypeError:
                 pass
+            except socketio.exceptions.BadNamespaceError:
+                pass
         Publisher.add_sendMessage_hook(_emit)
@@ -0,0 +1,17 @@
+Cython==0.29.24
+Pillow==9.0.0
+Pypubsub==4.0.3
+configparser==5.0.1
+h5py==3.6.0
+imageio==2.9.0
+nibabel==3.2.1
+numpy==1.22.1
+psutil==5.8.0
+pyserial==3.5
+python-gdcm==3.0.9.1
+scikit-image==0.19.1
+scipy==1.7.3
+vtk==9.0.3
+wxPython==4.1.1
+torch==1.10.0
+pyacvd==0.2.7
	@@ -832,7 +832,6 @@ SEED_RADIUS = 1.5		@@ -832,7 +832,6 @@ SEED_RADIUS = 1.5
832	# Increased the default sleep parameter from 0.1 to 0.15 to decrease CPU load during navigation.	832	# Increased the default sleep parameter from 0.1 to 0.15 to decrease CPU load during navigation.
833	SLEEP_NAVIGATION = 0.2	833	SLEEP_NAVIGATION = 0.2
834	SLEEP_COORDINATES = 0.05	834	SLEEP_COORDINATES = 0.05
835	-SLEEP_ROBOT = 0.01
836		835
837	BRAIN_OPACITY = 0.6	836	BRAIN_OPACITY = 0.6
838	N_CPU = psutil.cpu_count()	837	N_CPU = psutil.cpu_count()
	@@ -864,14 +863,3 @@ PULSE_DURATION_IN_MILLISECONDS = 0.2		@@ -864,14 +863,3 @@ PULSE_DURATION_IN_MILLISECONDS = 0.2
864		863
865	#Robot	864	#Robot
866	ROBOT_ElFIN_IP = ['143.107.220.251', '169.254.153.251', '127.0.0.1']	865	ROBOT_ElFIN_IP = ['143.107.220.251', '169.254.153.251', '127.0.0.1']
867	-ROBOT_ElFIN_PORT = 10003
868	-ROBOT_MOTIONS = {"normal": 0, "linear out": 1, "arc": 2}
869	-ROBOT_HEAD_VELOCITY_THRESHOLD = 10 #mm/s
870	-ROBOT_ARC_THRESHOLD_DISTANCE = 100 #mm
871	-ROBOT_VERSOR_SCALE_FACTOR = 70
872	-#Robot Working Space is defined as 800mm in Elfin manual. For safety, the value is reduced by 5%.
873	-ROBOT_WORKING_SPACE = 760 #mm
874	-ROBOT_MOVE_STATE = {"free to move": 0,
875	- "in motion": 1009,
876	- "waiting for execution": 1013,
877	- "error": 1025}
	@@ -163,6 +163,12 @@ def transform_icp(m_img, m_icp):		@@ -163,6 +163,12 @@ def transform_icp(m_img, m_icp):
163		163
164	return m_img	164	return m_img
165		165
		166	+def inverse_transform_icp(m_img, m_icp):
		167	+ coord_img = [m_img[0, -1], -m_img[1, -1], m_img[2, -1], 1]
		168	+ m_img[0, -1], m_img[1, -1], m_img[2, -1], _ = np.linalg.inv(m_icp) @ coord_img
		169	+ m_img[0, -1], m_img[1, -1], m_img[2, -1] = m_img[0, -1], -m_img[1, -1], m_img[2, -1]
		170	+
		171	+ return m_img
166		172
167	def object_registration(fiducials, orients, coord_raw, m_change):	173	def object_registration(fiducials, orients, coord_raw, m_change):
168	"""	174	"""
	@@ -244,47 +250,3 @@ def object_registration(fiducials, orients, coord_raw, m_change):		@@ -244,47 +250,3 @@ def object_registration(fiducials, orients, coord_raw, m_change):
244	)	250	)
245		251
246	return t_obj_raw, s0_raw, r_s0_raw, s0_dyn, m_obj_raw, r_obj_img	252	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	- translation, angles_as_deg = dco.transformation_matrix_to_coordinates(M_tracker_in_robot, axes='rzyx')
288	- tracker_in_robot = list(translation) + list(angles_as_deg)
289	-
290	- return tracker_in_robot
	@@ -101,6 +101,32 @@ def tracker_to_image(m_change, m_probe_ref, r_obj_img, m_obj_raw, s0_dyn):		@@ -101,6 +101,32 @@ def tracker_to_image(m_change, m_probe_ref, r_obj_img, m_obj_raw, s0_dyn):
101	m_img[:3, :3] = r_obj[:3, :3]	101	m_img[:3, :3] = r_obj[:3, :3]
102	return m_img	102	return m_img
103		103
		104	+def image_to_tracker(m_change, target, icp):
		105	+ """Compute the transformation matrix to the tracker coordinate system
		106	+
		107	+ :param m_change: Corregistration transformation obtained from fiducials
		108	+ :type m_change: numpy.ndarray
		109	+ :param target: Target in invesalius coordinate system
		110	+ :type target: numpy.ndarray
		111	+ :param icp: ICP transformation matrix
		112	+ :type icp: numpy.ndarray
		113	+
		114	+ :return: 4 x 4 numpy double array
		115	+ :rtype: numpy.ndarray
		116	+ """
		117	+ m_target_in_image = dco.coordinates_to_transformation_matrix(
		118	+ position=target[:3],
		119	+ orientation=[0, 0, 0],
		120	+ axes='sxyz',
		121	+ )
		122	+ if icp.use_icp:
		123	+ m_target_in_image = bases.inverse_transform_icp(m_target_in_image, icp.m_icp)
		124	+ m_target_in_tracker = np.linalg.inv(m_change) @ m_target_in_image
		125	+
		126	+ # invert y coordinate
		127	+ m_target_in_tracker[2, -1] = -m_target_in_tracker[2, -1]
		128	+
		129	+ return m_target_in_tracker
104		130
105	def corregistrate_object_dynamic(inp, coord_raw, ref_mode_id, icp):	131	def corregistrate_object_dynamic(inp, coord_raw, ref_mode_id, icp):
106		132
	@@ -193,7 +219,6 @@ class CoordinateCorregistrate(threading.Thread):		@@ -193,7 +219,6 @@ class CoordinateCorregistrate(threading.Thread):
193	self.object_at_target_queue = queues[3]	219	self.object_at_target_queue = queues[3]
194	self.icp = None	220	self.icp = None
195	self.m_icp = None	221	self.m_icp = None
196	- self.robot_tracker_flag = None
197	self.last_coord = None	222	self.last_coord = None
198	self.tracker_id = tracker_id	223	self.tracker_id = tracker_id
199	self.target = target	224	self.target = target
	@@ -254,7 +279,7 @@ class CoordinateCorregistrate(threading.Thread):		@@ -254,7 +279,7 @@ class CoordinateCorregistrate(threading.Thread):
254	if self.icp:	279	if self.icp:
255	m_img = bases.transform_icp(m_img, self.m_icp)	280	m_img = bases.transform_icp(m_img, self.m_icp)
256		281
257	- self.coord_queue.put_nowait([coord, [coord_raw, markers_flag], m_img, view_obj])	282	+ self.coord_queue.put_nowait([coord, markers_flag, m_img, view_obj])
258	# print('CoordCoreg: put {}'.format(count))	283	# print('CoordCoreg: put {}'.format(count))
259	# count += 1	284	# count += 1
260		285
	@@ -307,7 +332,7 @@ class CoordinateCorregistrateNoObject(threading.Thread):		@@ -307,7 +332,7 @@ class CoordinateCorregistrateNoObject(threading.Thread):
307	if self.icp:	332	if self.icp:
308	m_img = bases.transform_icp(m_img, self.m_icp)	333	m_img = bases.transform_icp(m_img, self.m_icp)
309		334
310	- self.coord_queue.put_nowait([coord, [coord_raw, markers_flag], m_img, view_obj])	335	+ self.coord_queue.put_nowait([coord, markers_flag, m_img, view_obj])
311		336
312	if self.view_tracts:	337	if self.view_tracts:
313	self.coord_tracts_queue.put_nowait(m_img_flip)	338	self.coord_tracts_queue.put_nowait(m_img_flip)
	@@ -20,6 +20,9 @@		@@ -20,6 +20,9 @@
20	from invesalius.pubsub import pub as Publisher	20	from invesalius.pubsub import pub as Publisher
21	from invesalius.utils import Singleton	21	from invesalius.utils import Singleton
22		22
		23	+import numpy as np
		24	+from vtk.numpy_interface import dataset_adapter
		25	+
23	class NeuronavigationApi(metaclass=Singleton):	26	class NeuronavigationApi(metaclass=Singleton):
24	"""	27	"""
25	An API used internally in InVesalius to communicate with the	28	An API used internally in InVesalius to communicate with the
	@@ -34,6 +37,8 @@ class NeuronavigationApi(metaclass=Singleton):		@@ -34,6 +37,8 @@ class NeuronavigationApi(metaclass=Singleton):
34		37
35	If connection object is not given or it is None, skip doing the updates.	38	If connection object is not given or it is None, skip doing the updates.
36	"""	39	"""
		40	+ N_VERTICES_IN_POLYGON = 3
		41	+
37	def __init__(self, connection=None):	42	def __init__(self, connection=None):
38	if connection is not None:	43	if connection is not None:
39	assert self._hasmethod(connection, 'update_coil_pose')	44	assert self._hasmethod(connection, 'update_coil_pose')
	@@ -65,3 +70,28 @@ class NeuronavigationApi(metaclass=Singleton):		@@ -65,3 +70,28 @@ class NeuronavigationApi(metaclass=Singleton):
65	position=position,	70	position=position,
66	orientation=orientation,	71	orientation=orientation,
67	)	72	)
		73	+
		74	+ def update_coil_mesh(self, polydata):
		75	+ wrapped = dataset_adapter.WrapDataObject(polydata)
		76	+
		77	+ points = np.asarray(wrapped.Points)
		78	+ polygons_raw = np.asarray(wrapped.Polygons)
		79	+
		80	+ # The polygons are returned as 1d-array of the form
		81	+ #
		82	+ # [n_0, id_0(0), id_0(1), ..., id_0(n_0),
		83	+ # n_1, id_1(0), id_1(1), ..., id_1(n_1),
		84	+ # ...]
		85	+ #
		86	+ # where n_i is the number of vertices in polygon i, and id_i's are indices to the vertex list.
		87	+ #
		88	+ # Assert that all polygons have an equal number of vertices, reshape the array, and drop n_i's.
		89	+ #
		90	+ assert np.all(polygons_raw[0::self.N_VERTICES_IN_POLYGON + 1] == self.N_VERTICES_IN_POLYGON)
		91	+
		92	+ polygons = polygons_raw.reshape(-1, self.N_VERTICES_IN_POLYGON + 1)[:, 1:]
		93	+
		94	+ self.connection.update_coil_mesh(
		95	+ points=points,
		96	+ polygons=polygons,
		97	+ )
	@@ -79,5 +79,7 @@ class RemoteControl:		@@ -79,5 +79,7 @@ class RemoteControl:
79	})	79	})
80	except TypeError:	80	except TypeError:
81	pass	81	pass
		82	+ except socketio.exceptions.BadNamespaceError:
		83	+ pass
82		84
83	Publisher.add_sendMessage_hook(_emit)	85	Publisher.add_sendMessage_hook(_emit)
@@ -0,0 +1,17 @@		@@ -0,0 +1,17 @@
	1	+Cython==0.29.24
	2	+Pillow==9.0.0
	3	+Pypubsub==4.0.3
	4	+configparser==5.0.1
	5	+h5py==3.6.0
	6	+imageio==2.9.0
	7	+nibabel==3.2.1
	8	+numpy==1.22.1
	9	+psutil==5.8.0
	10	+pyserial==3.5
	11	+python-gdcm==3.0.9.1
	12	+scikit-image==0.19.1
	13	+scipy==1.7.3
	14	+vtk==9.0.3
	15	+wxPython==4.1.1
	16	+torch==1.10.0
	17	+pyacvd==0.2.7