Merge branch 'invesalius:master' into 278_add_visualization_for_the_direction_of_the_marker

sotodela · GitHub
2 parents 52a9c203 e9f5883b
Showing 23 changed files with 1600 additions and 308 deletions Show diff stats
app.py
invesalius/constants.py
invesalius/control.py
invesalius/data/bases.py
invesalius/data/coordinates.py
invesalius/data/coregistration.py
invesalius/data/elfin.py
invesalius/data/elfin_processing.py
invesalius/data/slice_.py
invesalius/data/trackers.py
invesalius/data/tractography.py
invesalius/data/viewer_volume.py
invesalius/gui/dialogs.py
invesalius/gui/task_navigator.py
invesalius/i18n.py
invesalius/navigation/navigation.py
invesalius/navigation/robot.py
invesalius/navigation/tracker.py
navigation/mtc_files/Markers/2Ref
navigation/mtc_files/Markers/3Coil_big
@@ -87,11 +87,11 @@ session = ses.Session()
 if session.ReadSession():
     lang = session.GetLanguage()
     if lang:
-        LANG = lang
         try:
             _ = i18n.InstallLanguage(lang)
+            LANG = lang
         except FileNotFoundError:
-            LANG = None
+            pass
 class InVesalius(wx.App):
@@ -134,7 +134,7 @@ class Inv3SplashScreen(SplashScreen):
     Splash screen to be shown in InVesalius initialization.
     """
     def __init__(self):
-        # Splash screen image will depend on currently language
+        # Splash screen image will depend on the current language
         lang = LANG
         self.locale = wx.Locale(wx.LANGUAGE_ENGLISH)
@@ -143,22 +143,20 @@ class Inv3SplashScreen(SplashScreen):
         # should be created
         create_session = LANG is None
-        install_lang = 0
+        install_lang = False
         if lang:
             _ = i18n.InstallLanguage(lang)
-            install_lang = 1
-        else:
-            install_lang = 0
+            install_lang = True
         # If no language is set into session file, show dialog so
         # user can select language
-        if install_lang == 0:
+        if not install_lang:
             dialog = lang_dlg.LanguageDialog()
             # FIXME: This works ok in linux2, darwin and win32,
             # except on win64, due to wxWidgets bug
             try:
-                ok = (dialog.ShowModal() == wx.ID_OK)
+                ok = dialog.ShowModal() == wx.ID_OK
             except wx._core.PyAssertionError:
                 ok = True
             finally:
@@ -174,9 +172,8 @@ class Inv3SplashScreen(SplashScreen):
             dialog.Destroy()
-        # Session file should be created... So we set the recent
-        # choosen language
-        if (create_session):
+        # Session file should be created... So we set the recently chosen language.
+        if create_session:
             session.CreateItens()
             session.SetLanguage(lang)
             session.WriteSessionFile()
@@ -184,8 +181,7 @@ class Inv3SplashScreen(SplashScreen):
         #  session.SaveConfigFileBackup()
-        # Only after language was defined, splash screen will be
-        # shown
+        # Only after language was defined, splash screen will be shown.
         if lang:
             #import locale
@@ -206,9 +202,7 @@ class Inv3SplashScreen(SplashScreen):
                 abs_file_path = os.path.abspath(".." + os.sep)
                 path = abs_file_path
                 path = os.path.join(path, 'icons', icon_file)
-
             else:
-
                 path = os.path.join(inv_paths.ICON_DIR, icon_file)
                 if not os.path.exists(path):
                     path = os.path.join(inv_paths.ICON_DIR, "splash_en.png")
@@ -241,7 +235,7 @@ class Inv3SplashScreen(SplashScreen):
         self.control = Controller(self.main)
         self.fc = wx.CallLater(200, self.ShowMain)
-        options, args = parse_comand_line()
+        options, args = parse_command_line()
         wx.CallLater(1, use_cmd_optargs, options, args)
         # Check for updates
@@ -292,7 +286,7 @@ def non_gui_startup(options, args):
 # ------------------------------------------------------------------
-def parse_comand_line():
+def parse_command_line():
     """
     Handle command line arguments.
     """
@@ -302,7 +296,7 @@ def parse_comand_line():
     # Parse command line arguments
     parser = op.OptionParser()
-    # -d or --debug: print all pubsub messagessent
+    # -d or --debug: print all pubsub messages sent
     parser.add_option("-d", "--debug",
                       action="store_true",
                       dest="debug")
@@ -353,12 +347,6 @@ def parse_comand_line():
 def use_cmd_optargs(options, args):
-    # If debug argument...
-    if options.debug:
-        Publisher.subscribe(print_events, Publisher.ALL_TOPICS)
-        session = ses.Session()
-        session.debug = 1
-
     # If import DICOM argument...
     if options.dicom_dir:
         import_dir = options.dicom_dir
@@ -494,11 +482,18 @@ def print_events(topic=Publisher.AUTO_TOPIC, **msg_data):
     """
     utils.debug("%s\n\tParameters: %s" % (topic, msg_data))
+
 def main():
     """
     Initialize InVesalius GUI
     """
-    options, args = parse_comand_line()
+    options, args = parse_command_line()
+
+    session = ses.Session()
+    session.debug = options.debug
+
+    if options.debug:
+        Publisher.subscribe(print_events, Publisher.ALL_TOPICS)
     if options.remote_host is not None:
         from invesalius.net.remote_control import RemoteControl
@@ -517,6 +512,7 @@ def main():
         application = InVesalius(0)
         application.MainLoop()
+
 if __name__ == '__main__':
     #Is needed because of pyinstaller
     multiprocessing.freeze_support()
@@ -655,6 +655,18 @@ BOOLEAN_DIFF = 2
 BOOLEAN_AND = 3
 BOOLEAN_XOR = 4
+# -------------- User interface ---------------------
+
+# The column order in the marker panel
+#
+ID_COLUMN = 0
+SESSION_COLUMN = 1
+LABEL_COLUMN = 2
+TARGET_COLUMN = 3
+X_COLUMN = 4
+Y_COLUMN = 5
+Z_COLUMN = 6
+
 #------------ Navigation defaults -------------------
 MARKER_COLOUR = (1.0, 1.0, 0.)
@@ -672,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
@@ -835,3 +848,13 @@ 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 #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
@@ -965,7 +965,7 @@ class Controller():
                 re_dialog = dialog.ResizeImageDialog()
                 re_dialog.SetValue(int(resolution_percentage*100))
                 re_dialog_value = re_dialog.ShowModal()
-                re_dialog.Close() 
+                re_dialog.Close()
                 if re_dialog_value == wx.ID_OK:
                     percentage = re_dialog.GetValue()
@@ -1143,7 +1143,7 @@ class Controller():
     def show_mask_preview(self, index, flag=True):
         proj = prj.Project()
         mask = proj.mask_dict[index]
-        slc = self.Slice.do_threshold_to_all_slices(mask)
+        self.Slice.do_threshold_to_all_slices(mask)
         mask.create_3d_preview()
         Publisher.sendMessage("Load mask preview", mask_3d_actor=mask.volume._actor, flag=flag)
         Publisher.sendMessage("Reload actual slice")
@@ -244,3 +244,47 @@ 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
@@ -19,7 +19,6 @@
 from math import sin, cos
 import numpy as np
-import queue
 import threading
 import wx
@@ -35,13 +34,28 @@ class TrackerCoordinates():
     def __init__(self):
         self.coord = None
         self.markers_flag = [False, False, False]
+        self.previous_markers_flag = self.markers_flag
+        self.nav_status = False
+        self.__bind_events()
+
+    def __bind_events(self):
+        Publisher.subscribe(self.OnUpdateNavigationStatus, 'Navigation status')
+
+    def OnUpdateNavigationStatus(self, nav_status, vis_status):
+        self.nav_status = nav_status
     def SetCoordinates(self, coord, markers_flag):
         self.coord = coord
         self.markers_flag = markers_flag
-        wx.CallAfter(Publisher.sendMessage, 'Sensors ID', markers_flag=self.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
     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
+
         return self.coord, self.markers_flag
@@ -65,15 +79,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 +195,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]
@@ -223,11 +246,10 @@ def PolhemusCoord(trck, trck_id, ref_mode):
     elif trck[1] == 'wrapper':
         coord = PolhemusWrapperCoord(trck[0], trck_id, ref_mode)
-    return coord, None
+    return coord, [False, False, False]
 def PolhemusWrapperCoord(trck, trck_id, ref_mode):
-
     trck.Run()
     scale = 10.0 * np.array([1., 1., 1.])
@@ -475,6 +497,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,212 @@
+#--------------------------------------------------------------------------
+# 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.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
+
+        translation, angles_as_deg = dco.transformation_matrix_to_coordinates(m_robot_new, axes='sxyz')
+        new_robot_position = list(translation) + list(angles_as_deg)
+
+        return new_robot_position
+
+    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
+        """
+        correction_distance_compensation = np.sqrt((coord_inv[0]-actual_point[0]) ** 2 +
+                                                   (coord_inv[1]-actual_point[1]) ** 2 +
+                                                   (coord_inv[2]-actual_point[2]) ** 2)
+
+        return correction_distance_compensation
+
+    def estimate_robot_target_length(self, robot_target):
+        """
+        Estimates the length of the 3D vector of the robot target
+        """
+        robot_target_length = np.sqrt(robot_target[0] ** 2 + robot_target[1] ** 2 + robot_target[2] ** 2)
+
+        return robot_target_length
+
@@ -264,8 +264,6 @@ class Slice(metaclass=utils.Singleton):
     def OnRemoveMasks(self, mask_indexes):
         proj = Project()
         for item in mask_indexes:
-            proj.RemoveMask(item)
-
             # if the deleted mask is the current mask, cleans the current mask
             # and discard from buffer all datas related to mask.
             if self.current_mask is not None and item == self.current_mask.index:
@@ -277,6 +275,7 @@ class Slice(metaclass=utils.Singleton):
                 Publisher.sendMessage("Show mask", index=item, value=False)
                 Publisher.sendMessage("Reload actual slice")
+            proj.RemoveMask(item)
     def OnDuplicateMasks(self, mask_indexes):
         proj = Project()
@@ -1185,6 +1184,7 @@ class Slice(metaclass=utils.Singleton):
         future_mask = proj.GetMask(index)
         future_mask.is_shown = True
         self.current_mask = future_mask
+        self.current_mask.on_show()
         colour = future_mask.colour
         self.SetMaskColour(index, colour, update=False)
@@ -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]
@@ -987,10 +987,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,9 @@ class SetNDIconfigs(wx.Dialog):
         btn_ok = wx.Button(self, wx.ID_OK)
         btn_ok.SetHelpText("")
         btn_ok.SetDefault()
+        if not port_selec:
+            btn_ok.Enable(False)
+        self.btn_ok = btn_ok
         btn_cancel = wx.Button(self, wx.ID_CANCEL)
         btn_cancel.SetHelpText("")
@@ -4402,6 +4722,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,13 +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:
@@ -183,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,
@@ -341,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
@@ -640,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)
@@ -657,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)
@@ -739,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)
@@ -841,6 +864,7 @@ class NeuronavigationPanel(wx.Panel):
         )
         self.tracker.__init__()
         self.icp.__init__()
+        self.robot.__init__(self.tracker)
 class ObjectRegistrationPanel(wx.Panel):
@@ -899,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))
@@ -1200,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)
@@ -1210,13 +1247,20 @@ class MarkersPanel(wx.Panel):
         self.SetAutoLayout(1)
+        self.tracker = tracker
+
         self.__bind_events()
+        self.session = ses.Session()
+
         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
@@ -1269,21 +1313,27 @@ class MarkersPanel(wx.Panel):
         # List of markers
         self.lc = wx.ListCtrl(self, -1, style=wx.LC_REPORT, size=wx.Size(0,120))
-        self.lc.InsertColumn(0, '#')
-        self.lc.InsertColumn(1, 'X')
-        self.lc.InsertColumn(2, 'Y')
-        self.lc.InsertColumn(3, 'Z')
-        self.lc.InsertColumn(4, 'Label')
-        self.lc.InsertColumn(5, 'Target')
-        self.lc.InsertColumn(6, 'Session')
-
-        self.lc.SetColumnWidth(0, 28)
-        self.lc.SetColumnWidth(1, 50)
-        self.lc.SetColumnWidth(2, 50)
-        self.lc.SetColumnWidth(3, 50)
-        self.lc.SetColumnWidth(4, 60)
-        self.lc.SetColumnWidth(5, 60)
-        self.lc.SetColumnWidth(5, 50)
+        self.lc.InsertColumn(const.ID_COLUMN, '#')
+        self.lc.SetColumnWidth(const.ID_COLUMN, 28)
+
+        self.lc.InsertColumn(const.SESSION_COLUMN, 'Session')
+        self.lc.SetColumnWidth(const.SESSION_COLUMN, 52)
+
+        self.lc.InsertColumn(const.LABEL_COLUMN, 'Label')
+        self.lc.SetColumnWidth(const.LABEL_COLUMN, 118)
+
+        self.lc.InsertColumn(const.TARGET_COLUMN, 'Target')
+        self.lc.SetColumnWidth(const.TARGET_COLUMN, 45)
+
+        if self.session.debug:
+            self.lc.InsertColumn(const.X_COLUMN, 'X')
+            self.lc.SetColumnWidth(const.X_COLUMN, 45)
+
+            self.lc.InsertColumn(const.Y_COLUMN, 'Y')
+            self.lc.SetColumnWidth(const.Y_COLUMN, 45)
+
+            self.lc.InsertColumn(const.Z_COLUMN, 'Z')
+            self.lc.SetColumnWidth(const.Z_COLUMN, 45)
         self.lc.Bind(wx.EVT_LIST_ITEM_RIGHT_CLICK, self.OnMouseRightDown)
         self.lc.Bind(wx.EVT_LIST_ITEM_ACTIVATED, self.OnItemBlink)
@@ -1301,7 +1351,6 @@ class MarkersPanel(wx.Panel):
         self.Update()
     def __bind_events(self):
-        # Publisher.subscribe(self.UpdateCurrentCoord, 'Co-registered points')
         Publisher.subscribe(self.UpdateCurrentCoord, 'Set cross focal point')
         Publisher.subscribe(self.OnDeleteMultipleMarkers, 'Delete fiducial marker')
         Publisher.subscribe(self.OnDeleteAllMarkers, 'Delete all markers')
@@ -1309,15 +1358,18 @@ 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):
-        """Return the index of the marker currently selected as target (there
-        should be at most one). If there is no such marker, return -1."""
+        """
+        Return the index of the marker currently selected as target (there
+        should be at most one). If there is no such marker, return None.
+        """
         for i in range(len(self.markers)):
             if self.markers[i].is_target:
                 return i
-        return -1
+        return None
     def __get_selected_items(self):
         """    
@@ -1334,18 +1386,22 @@ class MarkersPanel(wx.Panel):
         return selection
     def __delete_multiple_markers(self, index):
-        """ Delete multiple markers indexed by index. index must be sorted in
+        """
+        Delete multiple markers indexed by index. index must be sorted in
         the ascending order.
         """
         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))
+                self.lc.SetItem(n, 0, str(n + 1))
         Publisher.sendMessage('Remove multiple markers', index=index)
     def __set_marker_as_target(self, idx):
-        """Set marker indexed by idx as the new target. idx must be a valid index."""
+        """
+        Set marker indexed by idx as the new target. idx must be a valid index.
+        """
         # Find the previous target
         prev_idx = self.__find_target_marker()
@@ -1354,16 +1410,16 @@ class MarkersPanel(wx.Panel):
             return
         # Unset the previous target
-        if prev_idx != -1:
+        if prev_idx is not None:
             self.markers[prev_idx].is_target = False
             self.lc.SetItemBackgroundColour(prev_idx, 'white')
             Publisher.sendMessage('Set target transparency', status=False, index=prev_idx)
-            self.lc.SetItem(prev_idx, 5, "")
+            self.lc.SetItem(prev_idx, const.TARGET_COLUMN, "")
         # Set the new target
         self.markers[idx].is_target = True
         self.lc.SetItemBackgroundColour(idx, 'RED')
-        self.lc.SetItem(idx, 5, _("Yes"))
+        self.lc.SetItem(idx, const.TARGET_COLUMN, _("Yes"))
         Publisher.sendMessage('Update target', coord=self.markers[idx].coord)
         Publisher.sendMessage('Set target transparency', status=True, index=idx)
@@ -1389,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()
@@ -1399,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)
@@ -1416,9 +1485,9 @@ class MarkersPanel(wx.Panel):
     def OnMenuEditMarkerLabel(self, evt):
         list_index = self.lc.GetFocusedItem()
         if list_index != -1:
-            new_label = dlg.ShowEnterMarkerID(self.lc.GetItemText(list_index, 4))
+            new_label = dlg.ShowEnterMarkerID(self.lc.GetItemText(list_index, const.LABEL_COLUMN))
             self.markers[list_index].label = str(new_label)
-            self.lc.SetItem(list_index, 4, new_label)
+            self.lc.SetItem(list_index, const.LABEL_COLUMN, new_label)
         else:
             wx.MessageBox(_("No data selected."), _("InVesalius 3"))
@@ -1445,25 +1514,32 @@ class MarkersPanel(wx.Panel):
             # XXX: Seems like a slightly too early point for rounding; better to round only when the value
             #      is printed to the screen or file.
             #
-            self.markers[index].colour = [round(s/255.0, 3) for s in color_new]
+            self.markers[index].colour = [round(s / 255.0, 3) for s in color_new]
             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 None:
-            result = wx.ID_OK
-        else:
+        if evt is not None:
             result = dlg.ShowConfirmationDialog(msg=_("Remove all markers? Cannot be undone."))
+            if result != wx.ID_OK:
+                return
-        if result != wx.ID_OK:
-            return
-
-        if self.__find_target_marker() != -1:
+        if self.__find_target_marker() is not None:
             Publisher.sendMessage('Disable or enable coil tracker', status=False)
             if evt is not None:
                 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')
@@ -1472,22 +1548,27 @@ class MarkersPanel(wx.Panel):
         # OnDeleteMultipleMarkers is used for both pubsub and button click events
         # Pubsub is used for fiducial handle and button click for all others
-        if not evt: # called through pubsub
+        # called through pubsub
+        if not evt:
             index = []
             if label and (label in self.__list_fiducial_labels()):
                 for id_n in range(self.lc.GetItemCount()):
-                    item = self.lc.GetItem(id_n, 4)
+                    item = self.lc.GetItem(id_n, const.LABEL_COLUMN)
                     if item.GetText() == label:
                         self.lc.Focus(item.GetId())
                         index = [self.lc.GetFocusedItem()]
-        else:       # called from button click
+        # called from button click
+        else:
             index = self.__get_selected_items()
         if index:
             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=[])
                 wx.MessageBox(_("Target deleted."), _("InVesalius 3"))
             self.__delete_multiple_markers(index)
@@ -1532,13 +1613,12 @@ class MarkersPanel(wx.Panel):
                     # If the new marker has is_target=True, we first create
                     # a marker with is_target=False, and then call __set_marker_as_target
                     if marker.is_target:
-                        self.__set_marker_as_target(len(self.markers)-1)
+                        self.__set_marker_as_target(len(self.markers) - 1)
         except:
             wx.MessageBox(_("Invalid markers file."), _("InVesalius 3"))     
     def OnMarkersVisibility(self, evt, ctrl):
-
         if ctrl.GetValue():
             Publisher.sendMessage('Hide all markers',  indexes=self.lc.GetItemCount())
             ctrl.SetLabel('Show')
@@ -1573,8 +1653,8 @@ class MarkersPanel(wx.Panel):
             wx.MessageBox(_("Error writing markers file."), _("InVesalius 3"))  
     def OnSelectColour(self, evt, ctrl):
-        #TODO: Make sure GetValue returns 3 numbers (without alpha)
-        self.marker_colour = [colour/255.0 for colour in ctrl.GetValue()][:3]
+        # TODO: Make sure GetValue returns 3 numbers (without alpha)
+        self.marker_colour = [colour / 255.0 for colour in ctrl.GetValue()][:3]
     def OnSelectSize(self, evt, ctrl):
         self.marker_size = ctrl.GetValue()
@@ -1592,6 +1672,14 @@ 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,
@@ -1603,15 +1691,19 @@ class MarkersPanel(wx.Panel):
         #                       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()
         self.lc.InsertItem(num_items, str(num_items + 1))
-        self.lc.SetItem(num_items, 1, str(round(new_marker.x, 2)))
-        self.lc.SetItem(num_items, 2, str(round(new_marker.y, 2)))
-        self.lc.SetItem(num_items, 3, str(round(new_marker.z, 2)))
-        self.lc.SetItem(num_items, 4, new_marker.label)
-        self.lc.SetItem(num_items, 6, str(new_marker.session_id))
+        self.lc.SetItem(num_items, const.SESSION_COLUMN, str(new_marker.session_id))
+        self.lc.SetItem(num_items, const.LABEL_COLUMN, new_marker.label)
+
+        if self.session.debug:
+            self.lc.SetItem(num_items, const.X_COLUMN, str(round(new_marker.x, 1)))
+            self.lc.SetItem(num_items, const.Y_COLUMN, str(round(new_marker.y, 1)))
+            self.lc.SetItem(num_items, const.Z_COLUMN, str(round(new_marker.z, 1)))
+
         self.lc.EnsureVisible(num_items)
 class DbsPanel(wx.Panel):
@@ -21,7 +21,7 @@
 try:
     import configparser as ConfigParser
-except(ImportError):
+except ImportError:
     import ConfigParser
 import locale
@@ -66,22 +66,21 @@ def GetLocaleOS():
 def InstallLanguage(language):
     file_path = os.path.split(__file__)[0]
-    if hasattr(sys,"frozen") and (sys.frozen == "windows_exe"\
-                            or sys.frozen == "console_exe"):
+    abs_file_path = os.path.abspath(file_path + os.sep + "..")
-        abs_file_path = os.path.abspath(file_path + os.sep + ".." + os.sep + ".." + os.sep + "..")
-        language_dir = os.path.join(abs_file_path, 'locale')
-    else:
-        abs_file_path = os.path.abspath(file_path + os.sep + "..")
-        language_dir = os.path.join(abs_file_path, 'locale')
+    if hasattr(sys, "frozen") and (sys.frozen == "windows_exe" or sys.frozen == "console_exe"):
+        abs_file_path = os.path.abspath(abs_file_path + os.sep + ".." + os.sep + "..")
-    # MAC App
+    language_dir = os.path.join(abs_file_path, 'locale')
+
+    # MAC app
     if not os.path.exists(language_dir):
         abs_file_path = os.path.abspath(os.path.join(file_path, '..', '..',  '..', '..'))
         language_dir = os.path.join(abs_file_path, 'locale')
-    lang = gettext.translation('invesalius', language_dir,\
-                                   languages=[language], codeset='utf8') 
+    lang = gettext.translation('invesalius', language_dir,
+                               languages=[language], codeset='utf8')
+
     # Using unicode
     try:
         lang.install(unicode=1)
@@ -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,286 @@
+#--------------------------------------------------------------------------
+# 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)
+
+        """
+        #Check if the target is inside the working space
+        if self.process_tracker.estimate_robot_target_length(new_robot_coordinates) < const.ROBOT_WORKING_SPACE:
+            #Check the target distance to define the motion mode
+            if distance_target < const.ROBOT_ARC_THRESHOLD_DISTANCE and not self.arc_motion_flag:
+                self.trck_init_robot.SendCoordinates(new_robot_coordinates, const.ROBOT_MOTIONS["normal"])
+
+            elif distance_target >= const.ROBOT_ARC_THRESHOLD_DISTANCE or self.arc_motion_flag:
+                actual_point = current_robot_coordinates
+                if not self.arc_motion_flag:
+                    head_center_coordinates = self.process_tracker.estimate_head_center(self.tracker,
+                                                                           current_head_filtered).tolist()
+
+                    self.target_linear_out, self.target_arc = self.process_tracker.compute_arc_motion(current_robot_coordinates, head_center_coordinates,
+                                                                                                      new_robot_coordinates)
+                    self.arc_motion_flag = True
+                    self.arc_motion_step_flag = const.ROBOT_MOTIONS["linear out"]
+
+                if self.arc_motion_flag and self.arc_motion_step_flag == const.ROBOT_MOTIONS["linear out"]:
+                    coord = self.target_linear_out
+                    if np.allclose(np.array(actual_point), np.array(self.target_linear_out), 0, 1):
+                        self.arc_motion_step_flag = const.ROBOT_MOTIONS["arc"]
+                        coord = self.target_arc
+
+                elif self.arc_motion_flag and self.arc_motion_step_flag == const.ROBOT_MOTIONS["arc"]:
+                    head_center_coordinates = self.process_tracker.estimate_head_center(self.tracker,
+                                                                           current_head_filtered).tolist()
+
+                    _, new_target_arc = self.process_tracker.compute_arc_motion(current_robot_coordinates, head_center_coordinates,
+                                                                                new_robot_coordinates)
+                    if np.allclose(np.array(new_target_arc[3:-1]), np.array(self.target_arc[3:-1]), 0, 1):
+                        None
+                    else:
+                        if self.process_tracker.correction_distance_calculation_target(new_robot_coordinates, current_robot_coordinates) >= \
+                                const.ROBOT_ARC_THRESHOLD_DISTANCE*0.8:
+                            self.target_arc = new_target_arc
+
+                    coord = self.target_arc
+
+                    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)
+        else:
+            print("Head is too far from the robot basis")
+
+    def robot_control(self, current_tracker_coordinates_in_robot, current_robot_coordinates, markers_flag):
+        coord_head_tracker_in_robot = current_tracker_coordinates_in_robot[1]
+        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
	@@ -655,6 +655,18 @@ BOOLEAN_DIFF = 2		@@ -655,6 +655,18 @@ BOOLEAN_DIFF = 2
655	BOOLEAN_AND = 3	655	BOOLEAN_AND = 3
656	BOOLEAN_XOR = 4	656	BOOLEAN_XOR = 4
657		657
		658	+# -------------- User interface ---------------------
		659	+
		660	+# The column order in the marker panel
		661	+#
		662	+ID_COLUMN = 0
		663	+SESSION_COLUMN = 1
		664	+LABEL_COLUMN = 2
		665	+TARGET_COLUMN = 3
		666	+X_COLUMN = 4
		667	+Y_COLUMN = 5
		668	+Z_COLUMN = 6
		669	+
658	#------------ Navigation defaults -------------------	670	#------------ Navigation defaults -------------------
659		671
660	MARKER_COLOUR = (1.0, 1.0, 0.)	672	MARKER_COLOUR = (1.0, 1.0, 0.)
	@@ -672,18 +684,19 @@ CAMERA = 5		@@ -672,18 +684,19 @@ CAMERA = 5
672	POLARIS = 6	684	POLARIS = 6
673	POLARISP4 = 7	685	POLARISP4 = 7
674	OPTITRACK = 8	686	OPTITRACK = 8
675	-DEBUGTRACKRANDOM = 9
676	-DEBUGTRACKAPPROACH = 10	687	+ROBOT = 9
		688	+DEBUGTRACKRANDOM = 10
		689	+DEBUGTRACKAPPROACH = 11
677	DEFAULT_TRACKER = SELECT	690	DEFAULT_TRACKER = SELECT
678		691
679	NDICOMPORT = b'COM1'	692	NDICOMPORT = b'COM1'
680		693
681	TRACKERS = [_("Claron MicronTracker"),	694	TRACKERS = [_("Claron MicronTracker"),
682	- _("Polhemus FASTRAK"), _("Polhemus ISOTRAK II"),
683	- _("Polhemus PATRIOT"), _("Camera tracker"),
684	- _("NDI Polaris"), _("NDI Polaris P4"),
685	- _("Optitrack"), _("Debug tracker (random)"),
686	- _("Debug tracker (approach)")]	695	+ _("Polhemus FASTRAK"), _("Polhemus ISOTRAK II"),
		696	+ _("Polhemus PATRIOT"), _("Camera tracker"),
		697	+ _("NDI Polaris"), _("NDI Polaris P4"),
		698	+ _("Optitrack"), _("Robot tracker"),
		699	+ _("Debug tracker (random)"), _("Debug tracker (approach)")]
687		700
688	STATIC_REF = 0	701	STATIC_REF = 0
689	DYNAMIC_REF = 1	702	DYNAMIC_REF = 1
	@@ -835,3 +848,13 @@ WILDCARD_MARKER_FILES = _("Marker scanner coord files (.mkss)\|.mkss")		@@ -835,3 +848,13 @@ WILDCARD_MARKER_FILES = _("Marker scanner coord files (.mkss)\|.mkss")
835	BAUD_RATES = [300, 1200, 2400, 4800, 9600, 19200, 38400, 57600, 115200]	848	BAUD_RATES = [300, 1200, 2400, 4800, 9600, 19200, 38400, 57600, 115200]
836	BAUD_RATE_DEFAULT_SELECTION = 4	849	BAUD_RATE_DEFAULT_SELECTION = 4
837	PULSE_DURATION_IN_MILLISECONDS = 0.2	850	PULSE_DURATION_IN_MILLISECONDS = 0.2
		851	+
		852	+#Robot
		853	+ROBOT_ElFIN_IP = ['143.107.220.251', '169.254.153.251', '127.0.0.1']
		854	+ROBOT_ElFIN_PORT = 10003
		855	+ROBOT_MOTIONS = {"normal": 0, "linear out": 1, "arc": 2}
		856	+ROBOT_HEAD_VELOCITY_THRESHOLD = 10 #mm/s
		857	+ROBOT_ARC_THRESHOLD_DISTANCE = 100 #mm
		858	+ROBOT_VERSOR_SCALE_FACTOR = 70
		859	+#Robot Working Space is defined as 800mm in Elfin manual. For safety, the value is reduced by 5%.
		860	+ROBOT_WORKING_SPACE = 760 #mm
	@@ -965,7 +965,7 @@ class Controller():		@@ -965,7 +965,7 @@ class Controller():
965	re_dialog = dialog.ResizeImageDialog()	965	re_dialog = dialog.ResizeImageDialog()
966	re_dialog.SetValue(int(resolution_percentage*100))	966	re_dialog.SetValue(int(resolution_percentage*100))
967	re_dialog_value = re_dialog.ShowModal()	967	re_dialog_value = re_dialog.ShowModal()
968	- re_dialog.Close()	968	+ re_dialog.Close()
969		969
970	if re_dialog_value == wx.ID_OK:	970	if re_dialog_value == wx.ID_OK:
971	percentage = re_dialog.GetValue()	971	percentage = re_dialog.GetValue()
	@@ -1143,7 +1143,7 @@ class Controller():		@@ -1143,7 +1143,7 @@ class Controller():
1143	def show_mask_preview(self, index, flag=True):	1143	def show_mask_preview(self, index, flag=True):
1144	proj = prj.Project()	1144	proj = prj.Project()
1145	mask = proj.mask_dict[index]	1145	mask = proj.mask_dict[index]
1146	- slc = self.Slice.do_threshold_to_all_slices(mask)	1146	+ self.Slice.do_threshold_to_all_slices(mask)
1147	mask.create_3d_preview()	1147	mask.create_3d_preview()
1148	Publisher.sendMessage("Load mask preview", mask_3d_actor=mask.volume._actor, flag=flag)	1148	Publisher.sendMessage("Load mask preview", mask_3d_actor=mask.volume._actor, flag=flag)
1149	Publisher.sendMessage("Reload actual slice")	1149	Publisher.sendMessage("Reload actual slice")
	@@ -244,3 +244,47 @@ def object_registration(fiducials, orients, coord_raw, m_change):		@@ -244,3 +244,47 @@ 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	+ 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
@@ -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,212 @@		@@ -0,0 +1,212 @@
	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.coregistration as dcr
	24	+import invesalius.data.coordinates as dco
	25	+import invesalius.constants as const
	26	+
	27	+
	28	+class KalmanTracker:
	29	+ """
	30	+ Kalman filter to avoid sudden fluctuation from tracker device.
	31	+ The filter strength can be set by the cov_process, and cov_measure parameter
	32	+ It is required to create one instance for each variable (x, y, z, a, b, g)
	33	+ """
	34	+ def __init__(self,
	35	+ state_num=2,
	36	+ covariance_process=0.001,
	37	+ covariance_measure=0.1):
	38	+
	39	+ self.state_num = state_num
	40	+ measure_num = 1
	41	+
	42	+ # The filter itself.
	43	+ self.filter = cv2.KalmanFilter(state_num, measure_num, 0)
	44	+
	45	+ self.state = np.zeros((state_num, 1), dtype=np.float32)
	46	+ self.measurement = np.array((measure_num, 1), np.float32)
	47	+ self.prediction = np.zeros((state_num, 1), np.float32)
	48	+
	49	+
	50	+ self.filter.transitionMatrix = np.array([[1, 1],
	51	+ [0, 1]], np.float32)
	52	+ self.filter.measurementMatrix = np.array([[1, 1]], np.float32)
	53	+ self.filter.processNoiseCov = np.array([[1, 0],
	54	+ [0, 1]], np.float32) * covariance_process
	55	+ self.filter.measurementNoiseCov = np.array([[1]], np.float32) * covariance_measure
	56	+
	57	+ def update_kalman(self, measurement):
	58	+ self.prediction = self.filter.predict()
	59	+ self.measurement = np.array([[np.float32(measurement[0])]])
	60	+
	61	+ self.filter.correct(self.measurement)
	62	+ self.state = self.filter.statePost
	63	+
	64	+
	65	+class TrackerProcessing:
	66	+ def __init__(self):
	67	+ self.coord_vel = []
	68	+ self.timestamp = []
	69	+ self.velocity_vector = []
	70	+ self.kalman_coord_vector = []
	71	+ self.velocity_std = 0
	72	+
	73	+ self.tracker_stabilizers = [KalmanTracker(
	74	+ state_num=2,
	75	+ covariance_process=0.001,
	76	+ covariance_measure=0.1) for _ in range(6)]
	77	+
	78	+ def kalman_filter(self, coord_tracker):
	79	+ kalman_array = []
	80	+ pose_np = np.array((coord_tracker[:3], coord_tracker[3:])).flatten()
	81	+ for value, ps_stb in zip(pose_np, self.tracker_stabilizers):
	82	+ ps_stb.update_kalman([value])
	83	+ kalman_array.append(ps_stb.state[0])
	84	+ coord_kalman = np.hstack(kalman_array)
	85	+
	86	+ self.kalman_coord_vector.append(coord_kalman[:3])
	87	+ if len(self.kalman_coord_vector) < 20: #avoid initial fluctuations
	88	+ coord_kalman = coord_tracker
	89	+ print('initializing filter')
	90	+ else:
	91	+ del self.kalman_coord_vector[0]
	92	+
	93	+ return coord_kalman
	94	+
	95	+ def estimate_head_velocity(self, coord_vel, timestamp):
	96	+ coord_vel = np.vstack(np.array(coord_vel))
	97	+ coord_init = coord_vel[:int(len(coord_vel) / 2)].mean(axis=0)
	98	+ coord_final = coord_vel[int(len(coord_vel) / 2):].mean(axis=0)
	99	+ velocity = (coord_final - coord_init)/(timestamp[-1] - timestamp[0])
	100	+ distance = (coord_final - coord_init)
	101	+
	102	+ return velocity, distance
	103	+
	104	+ def compute_versors(self, init_point, final_point):
	105	+ init_point = np.array(init_point)
	106	+ final_point = np.array(final_point)
	107	+ norm = (sum((final_point - init_point) 2)) 0.5
	108	+ versor_factor = (((final_point-init_point) / norm) * const.ROBOT_VERSOR_SCALE_FACTOR).tolist()
	109	+
	110	+ return versor_factor
	111	+
	112	+
	113	+ def compute_arc_motion(self, current_robot_coordinates, head_center_coordinates, new_robot_coordinates):
	114	+ head_center = head_center_coordinates[0], head_center_coordinates[1], head_center_coordinates[2], \
	115	+ new_robot_coordinates[3], new_robot_coordinates[4], new_robot_coordinates[5]
	116	+
	117	+ versor_factor_move_out = self.compute_versors(head_center, current_robot_coordinates)
	118	+ init_move_out_point = current_robot_coordinates[0] + versor_factor_move_out[0], \
	119	+ current_robot_coordinates[1] + versor_factor_move_out[1], \
	120	+ current_robot_coordinates[2] + versor_factor_move_out[2], \
	121	+ current_robot_coordinates[3], current_robot_coordinates[4], current_robot_coordinates[5]
	122	+
	123	+ middle_point = ((new_robot_coordinates[0] + current_robot_coordinates[0]) / 2,
	124	+ (new_robot_coordinates[1] + current_robot_coordinates[1]) / 2,
	125	+ (new_robot_coordinates[2] + current_robot_coordinates[2]) / 2,
	126	+ 0, 0, 0)
	127	+ versor_factor_middle_arc = (np.array(self.compute_versors(head_center, middle_point))) * 2
	128	+ middle_arc_point = middle_point[0] + versor_factor_middle_arc[0], \
	129	+ middle_point[1] + versor_factor_middle_arc[1], \
	130	+ middle_point[2] + versor_factor_middle_arc[2]
	131	+
	132	+ versor_factor_arc = self.compute_versors(head_center, new_robot_coordinates)
	133	+ final_ext_arc_point = new_robot_coordinates[0] + versor_factor_arc[0], \
	134	+ new_robot_coordinates[1] + versor_factor_arc[1], \
	135	+ new_robot_coordinates[2] + versor_factor_arc[2], \
	136	+ new_robot_coordinates[3], new_robot_coordinates[4], new_robot_coordinates[5], 0
	137	+
	138	+ target_arc = middle_arc_point + final_ext_arc_point
	139	+
	140	+ return init_move_out_point, target_arc
	141	+
	142	+ def compute_head_move_threshold(self, current_ref):
	143	+ """
	144	+ Checks if the head velocity is bellow the threshold
	145	+ """
	146	+ self.coord_vel.append(current_ref)
	147	+ self.timestamp.append(time())
	148	+ if len(self.coord_vel) >= 10:
	149	+ head_velocity, head_distance = self.estimate_head_velocity(self.coord_vel, self.timestamp)
	150	+ self.velocity_vector.append(head_velocity)
	151	+
	152	+ del self.coord_vel[0]
	153	+ del self.timestamp[0]
	154	+
	155	+ if len(self.velocity_vector) >= 30:
	156	+ self.velocity_std = np.std(self.velocity_vector)
	157	+ del self.velocity_vector[0]
	158	+
	159	+ if self.velocity_std > const.ROBOT_HEAD_VELOCITY_THRESHOLD:
	160	+ print('Velocity threshold activated')
	161	+ return False
	162	+ else:
	163	+ return True
	164	+
	165	+ return False
	166	+
	167	+ def compute_head_move_compensation(self, current_head, m_change_robot_to_head):
	168	+ """
	169	+ Estimates the new robot position to reach the target
	170	+ """
	171	+ M_current_head = dco.coordinates_to_transformation_matrix(
	172	+ position=current_head[:3],
	173	+ orientation=current_head[3:6],
	174	+ axes='rzyx',
	175	+ )
	176	+ m_robot_new = M_current_head @ m_change_robot_to_head
	177	+
	178	+ translation, angles_as_deg = dco.transformation_matrix_to_coordinates(m_robot_new, axes='sxyz')
	179	+ new_robot_position = list(translation) + list(angles_as_deg)
	180	+
	181	+ return new_robot_position
	182	+
	183	+ def estimate_head_center(self, tracker, current_head):
	184	+ """
	185	+ Estimates the actual head center position using fiducials
	186	+ """
	187	+ m_probe_head_left, m_probe_head_right, m_probe_head_nasion = tracker.GetMatrixTrackerFiducials()
	188	+ m_current_head = dcr.compute_marker_transformation(np.array([current_head]), 0)
	189	+
	190	+ m_ear_left_new = m_current_head @ m_probe_head_left
	191	+ m_ear_right_new = m_current_head @ m_probe_head_right
	192	+
	193	+ return (m_ear_left_new[:3, -1] + m_ear_right_new[:3, -1])/2
	194	+
	195	+ def correction_distance_calculation_target(self, coord_inv, actual_point):
	196	+ """
	197	+ Estimates the Euclidean distance between the actual position and the target
	198	+ """
	199	+ correction_distance_compensation = np.sqrt((coord_inv[0]-actual_point[0]) ** 2 +
	200	+ (coord_inv[1]-actual_point[1]) ** 2 +
	201	+ (coord_inv[2]-actual_point[2]) ** 2)
	202	+
	203	+ return correction_distance_compensation
	204	+
	205	+ def estimate_robot_target_length(self, robot_target):
	206	+ """
	207	+ Estimates the length of the 3D vector of the robot target
	208	+ """
	209	+ robot_target_length = np.sqrt(robot_target[0] 2 + robot_target[1] 2 + robot_target[2] ** 2)
	210	+
	211	+ return robot_target_length
	212	+
	@@ -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]
	@@ -987,10 +987,10 @@ class Viewer(wx.Panel):		@@ -987,10 +987,10 @@ class Viewer(wx.Panel):
987		987
988	if thrdist and thrcoordx and thrcoordy and thrcoordz:	988	if thrdist and thrcoordx and thrcoordy and thrcoordz:
989	self.dummy_coil_actor.GetProperty().SetDiffuseColor(vtk_colors.GetColor3d('Green'))	989	self.dummy_coil_actor.GetProperty().SetDiffuseColor(vtk_colors.GetColor3d('Green'))
990	- Publisher.sendMessage('Coil at target', state=True)	990	+ wx.CallAfter(Publisher.sendMessage, 'Coil at target', state=True)
991	else:	991	else:
992	self.dummy_coil_actor.GetProperty().SetDiffuseColor(vtk_colors.GetColor3d('DarkOrange'))	992	self.dummy_coil_actor.GetProperty().SetDiffuseColor(vtk_colors.GetColor3d('DarkOrange'))
993	- Publisher.sendMessage('Coil at target', state=False)	993	+ wx.CallAfter(Publisher.sendMessage, 'Coil at target', state=False)
994		994
995	self.arrow_actor_list = arrow_roll_x1, arrow_roll_x2, arrow_yaw_z1, arrow_yaw_z2, \	995	self.arrow_actor_list = arrow_roll_x1, arrow_roll_x2, arrow_yaw_z1, arrow_yaw_z2, \
996	arrow_pitch_y1, arrow_pitch_y2	996	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,9 @@ class SetNDIconfigs(wx.Dialog):		@@ -4374,6 +4691,9 @@ 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	+ if not port_selec:
		4695	+ btn_ok.Enable(False)
		4696	+ self.btn_ok = btn_ok
4377		4697
4378	btn_cancel = wx.Button(self, wx.ID_CANCEL)	4698	btn_cancel = wx.Button(self, wx.ID_CANCEL)
4379	btn_cancel.SetHelpText("")	4699	btn_cancel.SetHelpText("")
	@@ -4402,6 +4722,9 @@ class SetNDIconfigs(wx.Dialog):		@@ -4402,6 +4722,9 @@ class SetNDIconfigs(wx.Dialog):
4402		4722
4403	self.CenterOnParent()	4723	self.CenterOnParent()
4404		4724
		4725	+ def OnChoicePort(self, evt, ctrl):
		4726	+ self.btn_ok.Enable(True)
		4727	+
4405	def GetValue(self):	4728	def GetValue(self):
4406	fn_probe = self.dir_probe.GetPath().encode(const.FS_ENCODE)	4729	fn_probe = self.dir_probe.GetPath().encode(const.FS_ENCODE)
4407	fn_ref = self.dir_ref.GetPath().encode(const.FS_ENCODE)	4730	fn_ref = self.dir_ref.GetPath().encode(const.FS_ENCODE)
@@ -0,0 +1,286 @@		@@ -0,0 +1,286 @@
	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	+ #Check if the target is inside the working space
	198	+ if self.process_tracker.estimate_robot_target_length(new_robot_coordinates) < const.ROBOT_WORKING_SPACE:
	199	+ #Check the target distance to define the motion mode
	200	+ if distance_target < const.ROBOT_ARC_THRESHOLD_DISTANCE and not self.arc_motion_flag:
	201	+ self.trck_init_robot.SendCoordinates(new_robot_coordinates, const.ROBOT_MOTIONS["normal"])
	202	+
	203	+ elif distance_target >= const.ROBOT_ARC_THRESHOLD_DISTANCE or self.arc_motion_flag:
	204	+ actual_point = current_robot_coordinates
	205	+ if not self.arc_motion_flag:
	206	+ head_center_coordinates = self.process_tracker.estimate_head_center(self.tracker,
	207	+ current_head_filtered).tolist()
	208	+
	209	+ self.target_linear_out, self.target_arc = self.process_tracker.compute_arc_motion(current_robot_coordinates, head_center_coordinates,
	210	+ new_robot_coordinates)
	211	+ self.arc_motion_flag = True
	212	+ self.arc_motion_step_flag = const.ROBOT_MOTIONS["linear out"]
	213	+
	214	+ if self.arc_motion_flag and self.arc_motion_step_flag == const.ROBOT_MOTIONS["linear out"]:
	215	+ coord = self.target_linear_out
	216	+ if np.allclose(np.array(actual_point), np.array(self.target_linear_out), 0, 1):
	217	+ self.arc_motion_step_flag = const.ROBOT_MOTIONS["arc"]
	218	+ coord = self.target_arc
	219	+
	220	+ elif self.arc_motion_flag and self.arc_motion_step_flag == const.ROBOT_MOTIONS["arc"]:
	221	+ head_center_coordinates = self.process_tracker.estimate_head_center(self.tracker,
	222	+ current_head_filtered).tolist()
	223	+
	224	+ _, new_target_arc = self.process_tracker.compute_arc_motion(current_robot_coordinates, head_center_coordinates,
	225	+ new_robot_coordinates)
	226	+ if np.allclose(np.array(new_target_arc[3:-1]), np.array(self.target_arc[3:-1]), 0, 1):
	227	+ None
	228	+ else:
	229	+ if self.process_tracker.correction_distance_calculation_target(new_robot_coordinates, current_robot_coordinates) >= \
	230	+ const.ROBOT_ARC_THRESHOLD_DISTANCE*0.8:
	231	+ self.target_arc = new_target_arc
	232	+
	233	+ coord = self.target_arc
	234	+
	235	+ if np.allclose(np.array(actual_point), np.array(self.target_arc[3:-1]), 0, 10):
	236	+ self.arc_motion_flag = False
	237	+ self.arc_motion_step_flag = const.ROBOT_MOTIONS["normal"]
	238	+ coord = new_robot_coordinates
	239	+
	240	+ self.trck_init_robot.SendCoordinates(coord, self.arc_motion_step_flag)
	241	+ else:
	242	+ print("Head is too far from the robot basis")
	243	+
	244	+ def robot_control(self, current_tracker_coordinates_in_robot, current_robot_coordinates, markers_flag):
	245	+ coord_head_tracker_in_robot = current_tracker_coordinates_in_robot[1]
	246	+ marker_head_flag = markers_flag[1]
	247	+ coord_obj_tracker_in_robot = current_tracker_coordinates_in_robot[2]
	248	+ marker_obj_flag = markers_flag[2]
	249	+
	250	+ if self.robot_tracker_flag:
	251	+ current_head = coord_head_tracker_in_robot
	252	+ if current_head is not None and marker_head_flag:
	253	+ current_head_filtered = self.process_tracker.kalman_filter(current_head)
	254	+ if self.process_tracker.compute_head_move_threshold(current_head_filtered):
	255	+ new_robot_coordinates = self.process_tracker.compute_head_move_compensation(current_head_filtered,
	256	+ self.m_change_robot_to_head)
	257	+ if self.coord_inv_old is None:
	258	+ self.coord_inv_old = new_robot_coordinates
	259	+
	260	+ if np.allclose(np.array(new_robot_coordinates), np.array(current_robot_coordinates), 0, 0.01):
	261	+ #avoid small movements (0.01 mm)
	262	+ pass
	263	+ elif not np.allclose(np.array(new_robot_coordinates), np.array(self.coord_inv_old), 0, 5):
	264	+ #if the head moves (>5mm) before the robot reach the target
	265	+ self.trck_init_robot.StopRobot()
	266	+ self.coord_inv_old = new_robot_coordinates
	267	+ else:
	268	+ distance_target = self.process_tracker.correction_distance_calculation_target(new_robot_coordinates, current_robot_coordinates)
	269	+ self.robot_move_decision(distance_target, new_robot_coordinates, current_robot_coordinates, current_head_filtered)
	270	+ self.coord_inv_old = new_robot_coordinates
	271	+ else:
	272	+ self.trck_init_robot.StopRobot()
	273	+
	274	+ def run(self):
	275	+ while not self.event_robot.is_set():
	276	+ current_tracker_coordinates_in_robot, current_robot_coordinates, markers_flag = self.get_coordinates_from_tracker_devices()
	277	+
	278	+ if not self.robot_target_queue.empty():
	279	+ self.robot_tracker_flag, self.m_change_robot_to_head = self.robot_target_queue.get_nowait()
	280	+ self.robot_target_queue.task_done()
	281	+
	282	+ if not self.object_at_target_queue.empty():
	283	+ self.target_flag = self.object_at_target_queue.get_nowait()
	284	+ self.object_at_target_queue.task_done()
	285	+
	286	+ 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.