Decouple robot control (#404)

* INIT testing.. * REM: Decouple robot * FIX: trck_init gets the first value * INIT testing.. * REM: Decouple robot * FIX: trck_init gets the first value * FIX: append tracker init robot * FIX: transform robot coordinates to tracker coordinates system * ENH: robot matrix estimation outside invesalius * ENH: remove robot coordinates and target matrix from invesalius * FIX: disconnected * FIX: tracker connection * ENH: Cleaning * CLN: remove unused imports * ENH: checks with target angle is zero/none to enable send target to robot * more cleaning * FIX: destroys robot dialog if not possible to connect to the robot * ENH: deal with robot resetting * ADD: function to estimate transformation matrix -navigation starts after ICP box * FIX: deals with tracker devices not connected * FIX: transformation matrix to list * ADD: send target to robot from image -need tests * FIX: image_to_tracker transformation * ENH: apply icp matrix to target * ENH: Show robot menu item only with tracker is robot * FIX: typo and comments * FIX: exception for BadNamespaceError * ADD: set metaclass to navigation and icp -allows access for plugins * CLEAN: remove unused imports

Decouple robot control (#404)
* INIT testing.. * REM: Decouple robot * FIX: trck_init gets the first value * INIT testing.. * REM: Decouple robot * FIX: trck_init gets the first value * FIX: append tracker init robot * FIX: transform robot coordinates to tracker coordinates system * ENH: robot matrix estimation outside invesalius * ENH: remove robot coordinates and target matrix from invesalius * FIX: disconnected * FIX: tracker connection * ENH: Cleaning * CLN: remove unused imports * ENH: checks with target angle is zero/none to enable send target to robot * more cleaning * FIX: destroys robot dialog if not possible to connect to the robot * ENH: deal with robot resetting * ADD: function to estimate transformation matrix -navigation starts after ICP box * FIX: deals with tracker devices not connected * FIX: transformation matrix to list * ADD: send target to robot from image -need tests * FIX: image_to_tracker transformation * ENH: apply icp matrix to target * ENH: Show robot menu item only with tracker is robot * FIX: typo and comments * FIX: exception for BadNamespaceError * ADD: set metaclass to navigation and icp -allows access for plugins * CLEAN: remove unused imports
Renan · GitHub
1 parent f9bfc475
Showing 14 changed files with 197 additions and 1041 deletions Show diff stats
invesalius/constants.py
invesalius/data/bases.py
invesalius/data/coordinates.py
invesalius/data/coregistration.py
invesalius/data/elfin.py
invesalius/data/elfin_processing.py
invesalius/data/trackers.py
invesalius/gui/dialogs.py
invesalius/gui/task_navigator.py
invesalius/navigation/icp.py
invesalius/navigation/navigation.py
invesalius/navigation/robot.py
invesalius/navigation/tracker.py
invesalius/net/remote_control.py
@@ -832,7 +832,6 @@ SEED_RADIUS = 1.5
 # Increased the default sleep parameter from 0.1 to 0.15 to decrease CPU load during navigation.
 SLEEP_NAVIGATION = 0.2
 SLEEP_COORDINATES = 0.05
-SLEEP_ROBOT = 0.01
 BRAIN_OPACITY = 0.6
 N_CPU = psutil.cpu_count()
@@ -864,14 +863,3 @@ PULSE_DURATION_IN_MILLISECONDS = 0.2
 #Robot
 ROBOT_ElFIN_IP = ['143.107.220.251', '169.254.153.251', '127.0.0.1']
-ROBOT_ElFIN_PORT = 10003
-ROBOT_MOTIONS = {"normal": 0, "linear out": 1, "arc": 2}
-ROBOT_HEAD_VELOCITY_THRESHOLD = 10 #mm/s
-ROBOT_ARC_THRESHOLD_DISTANCE = 100 #mm
-ROBOT_VERSOR_SCALE_FACTOR = 70
-#Robot Working Space is defined as 800mm in Elfin manual. For safety, the value is reduced by 5%.
-ROBOT_WORKING_SPACE = 760 #mm
-ROBOT_MOVE_STATE = {"free to move": 0,
-                    "in motion": 1009,
-                    "waiting for execution": 1013,
-                    "error": 1025}
@@ -163,6 +163,12 @@ def transform_icp(m_img, m_icp):
     return m_img
+def inverse_transform_icp(m_img, m_icp):
+    coord_img = [m_img[0, -1], -m_img[1, -1], m_img[2, -1], 1]
+    m_img[0, -1], m_img[1, -1], m_img[2, -1], _ = np.linalg.inv(m_icp) @ coord_img
+    m_img[0, -1], m_img[1, -1], m_img[2, -1] = m_img[0, -1], -m_img[1, -1], m_img[2, -1]
+
+    return m_img
 def object_registration(fiducials, orients, coord_raw, m_change):
     """
@@ -244,47 +250,3 @@ def object_registration(fiducials, orients, coord_raw, m_change):
     )
     return t_obj_raw, s0_raw, r_s0_raw, s0_dyn, m_obj_raw, r_obj_img
-
-def compute_robot_to_head_matrix(raw_target_robot):
-    """
-    :param head: nx6 array of head coordinates from tracking device in robot space
-    :param robot: nx6 array of robot coordinates
-
-    :return: target_robot_matrix: 3x3 array representing change of basis from robot to head in robot system
-    """
-    head, robot = raw_target_robot
-    # compute head target matrix
-    m_head_target = dco.coordinates_to_transformation_matrix(
-        position=head[:3],
-        orientation=head[3:],
-        axes='rzyx',
-    )
-
-    # compute robot target matrix
-    m_robot_target = dco.coordinates_to_transformation_matrix(
-        position=robot[:3],
-        orientation=robot[3:],
-        axes='rzyx',
-    )
-    robot_to_head_matrix = np.linalg.inv(m_head_target) @ m_robot_target
-
-    return robot_to_head_matrix
-
-
-class transform_tracker_to_robot(object):
-    M_tracker_to_robot = np.array([])
-    def transformation_tracker_to_robot(self, tracker_coord):
-        if not transform_tracker_to_robot.M_tracker_to_robot.any():
-            return None
-
-        M_tracker = dco.coordinates_to_transformation_matrix(
-            position=tracker_coord[:3],
-            orientation=tracker_coord[3:6],
-            axes='rzyx',
-        )
-        M_tracker_in_robot = transform_tracker_to_robot.M_tracker_to_robot @ M_tracker
-
-        translation, angles_as_deg = dco.transformation_matrix_to_coordinates(M_tracker_in_robot, axes='rzyx')
-        tracker_in_robot = list(translation) + list(angles_as_deg)
-
-        return tracker_in_robot
@@ -22,7 +22,6 @@ import numpy as np
 import threading
 import wx
-import invesalius.data.bases as db
 import invesalius.data.transformations as tr
 import invesalius.constants as const
@@ -47,14 +46,20 @@ class TrackerCoordinates():
     def SetCoordinates(self, coord, markers_flag):
         self.coord = coord
         self.markers_flag = markers_flag
-        if self.previous_markers_flag != self.markers_flag and not self.nav_status:
-            wx.CallAfter(Publisher.sendMessage, 'Sensors ID', markers_flag=self.markers_flag)
-            self.previous_markers_flag = self.markers_flag
+        if not self.nav_status:
+            wx.CallAfter(Publisher.sendMessage, 'Update tracker coordinates',
+                         coord=self.coord.tolist(), markers_flag=self.markers_flag)
+            if self.previous_markers_flag != self.markers_flag:
+                wx.CallAfter(Publisher.sendMessage, 'Sensors ID', markers_flag=self.markers_flag)
+                self.previous_markers_flag = self.markers_flag
     def GetCoordinates(self):
-        if self.previous_markers_flag != self.markers_flag and self.nav_status:
-            wx.CallAfter(Publisher.sendMessage, 'Sensors ID', markers_flag=self.markers_flag)
-            self.previous_markers_flag = self.markers_flag
+        if self.nav_status:
+            wx.CallAfter(Publisher.sendMessage, 'Update tracker coordinates',
+                         coord=self.coord.tolist(), markers_flag=self.markers_flag)
+            if self.previous_markers_flag != self.markers_flag:
+                wx.CallAfter(Publisher.sendMessage, 'Sensors ID', markers_flag=self.markers_flag)
+                self.previous_markers_flag = self.markers_flag
         return self.coord, self.markers_flag
@@ -195,16 +200,6 @@ def PolarisCoord(trck_init, trck_id, ref_mode):
     return coord, [trck.probeID, trck.refID, trck.objID]
-def ElfinCoord(trck_init):
-    if len(trck_init) > 2:
-        robotcoordinates = trck_init[-1]
-        coord = robotcoordinates.GetRobotCoordinates()
-        if coord is None:
-            coord = np.array([0, 0, 0, 0, 0, 0])
-    else:
-        coord = np.array([0, 0, 0, 0, 0, 0])
-
-    return coord
 def CameraCoord(trck_init, trck_id, ref_mode):
     trck = trck_init[0]
@@ -346,6 +341,13 @@ def PolhemusSerialCoord(trck_init, trck_id, ref_mode):
     return coord
+def RobotCoord(trk_init, trck_id, ref_mode):
+    #trck_id is the tracker related to the robot ID. To get the tracker ID, combined with the robot,
+    # it is required to get trk_init[1]
+    tracker_id = trk_init[1]
+    coord_tracker, markers_flag = GetCoordinatesForThread(trk_init[0], tracker_id, ref_mode)
+
+    return np.vstack([coord_tracker[0], coord_tracker[1], coord_tracker[2]]), markers_flag
 def DebugCoordRandom(trk_init, trck_id, ref_mode):
     """
@@ -497,22 +499,6 @@ def dynamic_reference_m(probe, reference):
     return coord_rot
-def RobotCoord(trk_init, trck_id, ref_mode):
-    coord_tracker, markers_flag = GetCoordinatesForThread(trk_init[0][0], trk_init[1], ref_mode)
-    coord_robot = ElfinCoord([trk_init[0][1]]+trk_init[1:])
-
-    probe_tracker_in_robot = db.transform_tracker_to_robot().transformation_tracker_to_robot(coord_tracker[0])
-    ref_tracker_in_robot = db.transform_tracker_to_robot().transformation_tracker_to_robot(coord_tracker[1])
-    obj_tracker_in_robot = db.transform_tracker_to_robot().transformation_tracker_to_robot(coord_tracker[2])
-
-    if probe_tracker_in_robot is None:
-        probe_tracker_in_robot = coord_tracker[0]
-        ref_tracker_in_robot = coord_tracker[1]
-        obj_tracker_in_robot = coord_tracker[2]
-
-    return np.vstack([probe_tracker_in_robot, ref_tracker_in_robot, coord_robot, obj_tracker_in_robot]), markers_flag
-
-
 def dynamic_reference_m2(probe, reference):
     """
     Apply dynamic reference correction to probe coordinates. Uses the alpha, beta and gama
@@ -590,4 +576,4 @@ class ReceiveCoordinates(threading.Thread):
         while not self.event.is_set():
             coord_raw, markers_flag = GetCoordinatesForThread(self.trck_init, self.trck_id, const.DEFAULT_REF_MODE)
             self.TrackerCoordinates.SetCoordinates(coord_raw, markers_flag)
-            sleep(const.SLEEP_COORDINATES)
 \ No newline at end of file
+            sleep(const.SLEEP_COORDINATES)
@@ -101,6 +101,32 @@ def tracker_to_image(m_change, m_probe_ref, r_obj_img, m_obj_raw, s0_dyn):
     m_img[:3, :3] = r_obj[:3, :3]
     return m_img
+def image_to_tracker(m_change, target, icp):
+    """Compute the transformation matrix to the tracker coordinate system
+
+    :param m_change: Corregistration transformation obtained from fiducials
+    :type m_change: numpy.ndarray
+    :param target: Target in invesalius coordinate system
+    :type target: numpy.ndarray
+    :param icp: ICP transformation matrix
+    :type icp: numpy.ndarray
+
+    :return: 4 x 4 numpy double array
+    :rtype: numpy.ndarray
+    """
+    m_target_in_image = dco.coordinates_to_transformation_matrix(
+        position=target[:3],
+        orientation=[0, 0, 0],
+        axes='sxyz',
+    )
+    if icp.use_icp:
+        m_target_in_image = bases.inverse_transform_icp(m_target_in_image, icp.m_icp)
+    m_target_in_tracker = np.linalg.inv(m_change) @ m_target_in_image
+
+    # invert y coordinate
+    m_target_in_tracker[2, -1] = -m_target_in_tracker[2, -1]
+
+    return m_target_in_tracker
 def corregistrate_object_dynamic(inp, coord_raw, ref_mode_id, icp):
@@ -193,7 +219,6 @@ class CoordinateCorregistrate(threading.Thread):
         self.object_at_target_queue = queues[3]
         self.icp = None
         self.m_icp = None
-        self.robot_tracker_flag = None
         self.last_coord = None
         self.tracker_id = tracker_id
         self.target = target
@@ -254,7 +279,7 @@ class CoordinateCorregistrate(threading.Thread):
                 if self.icp:
                     m_img = bases.transform_icp(m_img, self.m_icp)
-                self.coord_queue.put_nowait([coord, [coord_raw, markers_flag], m_img, view_obj])
+                self.coord_queue.put_nowait([coord, markers_flag, m_img, view_obj])
                 # print('CoordCoreg: put {}'.format(count))
                 # count += 1
@@ -307,7 +332,7 @@ class CoordinateCorregistrateNoObject(threading.Thread):
                 if self.icp:
                     m_img = bases.transform_icp(m_img, self.m_icp)
-                self.coord_queue.put_nowait([coord, [coord_raw, markers_flag], m_img, view_obj])
+                self.coord_queue.put_nowait([coord, markers_flag, m_img, view_obj])
                 if self.view_tracts:
                     self.coord_tracts_queue.put_nowait(m_img_flip)
@@ -1,252 +0,0 @@
-#!/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, motion_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 == const.ROBOT_MOVE_STATE["free to move"]:
-            if motion_type == const.ROBOT_MOTIONS["normal"] or motion_type == const.ROBOT_MOTIONS["linear out"]:
-                self.cobot.MoveL(target)
-            elif motion_type == const.ROBOT_MOTIONS["arc"]:
-                self.cobot.MoveC(target)
-        elif status == const.ROBOT_MOVE_STATE["error"]:
-            self.StopRobot()
-
-    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.StopRobot()
-        #TODO: robot function to close? 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)
@@ -1,212 +0,0 @@
-#--------------------------------------------------------------------------
-# 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
-
@@ -18,6 +18,7 @@
 #--------------------------------------------------------------------------
 import invesalius.constants as const
 import invesalius.gui.dialogs as dlg
+from invesalius.pubsub import pub as Publisher
 # TODO: Disconnect tracker when a new one is connected
 # TODO: Test if there are too many prints when connection fails
 # TODO: Redesign error messages. No point in having "Could not connect to default tracker" in all trackers
@@ -223,42 +224,23 @@ def OptitrackTracker(tracker_id):
         print('#####')
     return trck_init, lib_mode
-def ElfinRobot(robot_IP):
-    trck_init = None
-    try:
-        import invesalius.data.elfin as elfin
-        print("Trying to connect Robot via: ", robot_IP)
-        trck_init = elfin.Elfin_Server(robot_IP, const.ROBOT_ElFIN_PORT)
-        trck_init.Initialize()
-        lib_mode = 'wrapper'
-        print('Connect to elfin robot tracking device.')
-
-    except:
-        lib_mode = 'error'
-        trck_init = None
-        print('Could not connect to elfin robot tracker.')
-
-    # return tracker initialization variable and type of connection
-    return trck_init, lib_mode
-
 def RobotTracker(tracker_id):
     from wx import ID_OK
-    trck_init = None
-    trck_init_robot = None
-    tracker_id = None
+
     dlg_device = dlg.SetTrackerDeviceToRobot()
     if dlg_device.ShowModal() == ID_OK:
         tracker_id = dlg_device.GetValue()
         if tracker_id:
-            trck_connection = TrackerConnection(tracker_id, None, 'connect')
-            if trck_connection[0]:
+            trck_init = TrackerConnection(tracker_id, None, 'connect')
+            if trck_init[0]:
                 dlg_ip = dlg.SetRobotIP()
                 if dlg_ip.ShowModal() == ID_OK:
                     robot_IP = dlg_ip.GetValue()
-                    trck_init = trck_connection
-                    trck_init_robot = ElfinRobot(robot_IP)
+                    Publisher.sendMessage('Connect to robot', robot_IP=robot_IP)
+
+                    return trck_init, tracker_id
-    return [(trck_init, trck_init_robot), tracker_id]
+    return None, None
 def DebugTrackerRandom(tracker_id):
     trck_init = True
@@ -392,8 +374,9 @@ def DisconnectTracker(tracker_id, trck_init):
                 lib_mode = 'serial'
                 print('Tracker disconnected.')
             elif tracker_id == const.ROBOT:
-                trck_init[0][0].Close()
-                trck_init[1][0].Close()
+                Publisher.sendMessage('Reset robot', data=None)
+                if trck_init[1] == const.DEBUGTRACKRANDOM or trck_init[1] == const.DEBUGTRACKAPPROACH:
+                    trck_init[0].Close()
                 trck_init = False
                 lib_mode = 'wrapper'
                 print('Tracker disconnected.')
@@ -57,8 +57,6 @@ except ImportError:
     from wx import AboutDialogInfo, AboutBox
 import invesalius.constants as const
-import invesalius.data.coordinates as dco
-import invesalius.data.transformations as tr
 import invesalius.gui.widgets.gradient as grad
 import invesalius.session as ses
 import invesalius.utils as utils
@@ -3597,11 +3595,7 @@ class ObjectCalibrationDialog(wx.Dialog):
         #      and not change the function to the point of potentially breaking it.)
         #
         if self.obj_ref_id and fiducial_index == 4:
-            if self.tracker_id == const.ROBOT:
-                trck_init_robot = self.trk_init[1][0]
-                coord = trck_init_robot.Run()
-            else:
-                coord = coord_raw[self.obj_ref_id, :]
+            coord = coord_raw[self.obj_ref_id, :]
         else:
             coord = coord_raw[0, :]
@@ -4323,7 +4317,7 @@ class SetTrackerDeviceToRobot(wx.Dialog):
     def _init_gui(self):
         # ComboBox for spatial tracker device selection
         tooltip = wx.ToolTip(_("Choose the tracking device"))
-        trackers = const.TRACKERS
+        trackers = const.TRACKERS.copy()
         if not ses.Session().debug:
             del trackers[-3:]
         tracker_options = [_("Select tracker:")] + trackers
@@ -4425,10 +4419,8 @@ class CreateTransformationMatrixRobot(wx.Dialog):
         M_robot_2_tracker is created by an affine transformation. Robot TCP should be calibrated to the center of the tracker marker
         '''
         #TODO: make aboutbox
-        self.tracker_coord = []
-        self.tracker_angles = []
-        self.robot_coord = []
-        self.robot_angles = []
+        self.matrix_tracker_to_robot = []
+        self.robot_status = False
         self.tracker = tracker
@@ -4470,6 +4462,8 @@ class CreateTransformationMatrixRobot(wx.Dialog):
         btn_load = wx.Button(self, -1, label=_('Load'), size=wx.Size(65, 23))
         btn_load.Bind(wx.EVT_BUTTON, self.OnLoadReg)
+        btn_load.Enable(False)
+        self.btn_load = btn_load
         # Create a horizontal sizers
         border = 1
@@ -4524,16 +4518,17 @@ class CreateTransformationMatrixRobot(wx.Dialog):
         main_sizer.Fit(self)
         self.CenterOnParent()
+        self.__bind_events()
-    def affine_correg(self, tracker, robot):
-        m_change = tr.affine_matrix_from_points(robot[:].T, tracker[:].T,
-                                                shear=False, scale=False, usesvd=False)
-        return m_change
+    def __bind_events(self):
+        Publisher.subscribe(self.OnUpdateTransformationMatrix, 'Update robot transformation matrix')
+        Publisher.subscribe(self.OnCoordinatesAdquired, 'Coordinates for the robot transformation matrix collected')
+        Publisher.subscribe(self.OnRobotConnectionStatus, 'Robot connection status')
     def OnContinuousAcquisition(self, evt=None, btn=None):
         value = btn.GetValue()
         if value:
-            self.timer.Start(500)
+            self.timer.Start(100)
         else:
             self.timer.Stop()
@@ -4541,56 +4536,50 @@ class CreateTransformationMatrixRobot(wx.Dialog):
         self.OnCreatePoint(evt=None)
     def OnCreatePoint(self, evt):
-        coord_raw, markers_flag = self.tracker.TrackerCoordinates.GetCoordinates()
-        #robot thread is not initialized yet
-        coord_raw_robot = self.tracker.trk_init[0][1][0].Run()
-        coord_raw_tracker_obj = coord_raw[3]
-
-        if markers_flag[2]:
-            self.tracker_coord.append(coord_raw_tracker_obj[:3])
-            self.tracker_angles.append(coord_raw_tracker_obj[3:])
-            self.robot_coord.append(coord_raw_robot[:3])
-            self.robot_angles.append(coord_raw_robot[3:])
-            self.txt_number.SetLabel(str(int(self.txt_number.GetLabel())+1))
-        else:
-            print('Cannot detect the coil markers, pls try again')
+        Publisher.sendMessage('Collect coordinates for the robot transformation matrix', data=None)
+
+    def OnCoordinatesAdquired(self):
+        self.txt_number.SetLabel(str(int(self.txt_number.GetLabel())+1))
-        if len(self.tracker_coord) >= 3:
+        if self.robot_status and int(self.txt_number.GetLabel()) >= 3:
             self.btn_apply_reg.Enable(True)
+    def OnRobotConnectionStatus(self, data):
+        self.robot_status = data
+        if self.robot_status:
+            self.btn_load.Enable(True)
+            if int(self.txt_number.GetLabel()) >= 3:
+                self.btn_apply_reg.Enable(True)
+
     def OnReset(self, evt):
+        Publisher.sendMessage('Reset coordinates collection for the robot transformation matrix', data=None)
         if self.btn_cont_point:
             self.btn_cont_point.SetValue(False)
             self.OnContinuousAcquisition(evt=None, btn=self.btn_cont_point)
-        self.tracker_coord = []
-        self.tracker_angles = []
-        self.robot_coord = []
-        self.robot_angles = []
-        self.M_tracker_2_robot = []
         self.txt_number.SetLabel('0')
         self.btn_apply_reg.Enable(False)
         self.btn_save.Enable(False)
         self.btn_ok.Enable(False)
+        self.matrix_tracker_to_robot = []
+
     def OnApply(self, evt):
         if self.btn_cont_point:
             self.btn_cont_point.SetValue(False)
             self.OnContinuousAcquisition(evt=None, btn=self.btn_cont_point)
-        tracker_coord = np.array(self.tracker_coord)
-        robot_coord = np.array(self.robot_coord)
-
-        M_robot_2_tracker = self.affine_correg(tracker_coord, robot_coord)
-        M_tracker_2_robot = tr.inverse_matrix(M_robot_2_tracker)
-        self.M_tracker_2_robot = M_tracker_2_robot
+        Publisher.sendMessage('Robot transformation matrix estimation', data=None)
         self.btn_save.Enable(True)
         self.btn_ok.Enable(True)
         #TODO: make a colored circle to sinalize that the transformation was made (green) (red if not)
+    def OnUpdateTransformationMatrix(self, data):
+        self.matrix_tracker_to_robot = np.array(data)
+
     def OnSaveReg(self, evt):
         filename = ShowLoadSaveDialog(message=_(u"Save robot transformation file as..."),
                                           wildcard=_("Robot transformation files (*.rbtf)|*.rbtf"),
@@ -4598,10 +4587,10 @@ class CreateTransformationMatrixRobot(wx.Dialog):
                                           default_filename="robottransform.rbtf", save_ext="rbtf")
         if filename:
-            if self.M_tracker_2_robot is not None:
+            if self.matrix_tracker_to_robot is not None:
                 with open(filename, 'w', newline='') as file:
                     writer = csv.writer(file, delimiter='\t')
-                    writer.writerows(self.M_tracker_2_robot)
+                    writer.writerows(np.vstack(self.matrix_tracker_to_robot).tolist())
     def OnLoadReg(self, evt):
         filename = ShowLoadSaveDialog(message=_(u"Load robot transformation"),
@@ -4611,12 +4600,12 @@ class CreateTransformationMatrixRobot(wx.Dialog):
                 reader = csv.reader(file, delimiter='\t')
                 content = [row for row in reader]
-            self.M_tracker_2_robot = np.vstack(list(np.float_(content)))
-            print("Matrix tracker to robot:", self.M_tracker_2_robot)
-            self.btn_ok.Enable(True)
+            self.matrix_tracker_to_robot = np.vstack(list(np.float_(content)))
+            print("Matrix tracker to robot:", self.matrix_tracker_to_robot)
+            Publisher.sendMessage('Load robot transformation matrix', data=self.matrix_tracker_to_robot.tolist())
+            if self.robot_status:
+                self.btn_ok.Enable(True)
-    def GetValue(self):
-        return self.M_tracker_2_robot
 class SetNDIconfigs(wx.Dialog):
     def __init__(self, title=_("Setting NDI polaris configs:")):
@@ -20,10 +20,7 @@
 import dataclasses
 from functools import partial
 import itertools
-import csv
-import queue
 import time
-import threading
 import nibabel as nb
 import numpy as np
@@ -41,7 +38,6 @@ except ImportError:
     has_robot = False
 import wx
-import vtk
 try:
     import wx.lib.agw.foldpanelbar as fpb
@@ -51,7 +47,6 @@ except ImportError:
 import wx.lib.colourselect as csel
 import wx.lib.masked.numctrl
 from invesalius.pubsub import pub as Publisher
-from time import sleep
 import invesalius.constants as const
@@ -64,6 +59,7 @@ import invesalius.data.tractography as dti
 import invesalius.data.record_coords as rec
 import invesalius.data.vtk_utils as vtk_utils
 import invesalius.data.bases as db
+import invesalius.data.coregistration as dcr
 import invesalius.gui.dialogs as dlg
 import invesalius.project as prj
 import invesalius.session as ses
@@ -73,7 +69,6 @@ from invesalius.gui import utils as gui_utils
 from invesalius.navigation.icp import ICP
 from invesalius.navigation.navigation import Navigation
 from invesalius.navigation.tracker import Tracker
-from invesalius.navigation.robot import Robot
 from invesalius.data.converters import to_vtk
 from invesalius.net.neuronavigation_api import NeuronavigationApi
@@ -176,7 +171,7 @@ class InnerFoldPanel(wx.Panel):
         #
         tracker = Tracker()
         pedal_connection = PedalConnection() if HAS_PEDAL_CONNECTION else None
-
+        icp = ICP()
         neuronavigation_api = NeuronavigationApi()
         navigation = Navigation(
             pedal_connection=pedal_connection,
@@ -194,6 +189,7 @@ class InnerFoldPanel(wx.Panel):
             parent=item,
             navigation=navigation,
             tracker=tracker,
+            icp=icp,
             pedal_connection=pedal_connection,
             neuronavigation_api=neuronavigation_api,
         )
@@ -213,7 +209,7 @@ class InnerFoldPanel(wx.Panel):
         # Fold 3 - Markers panel
         item = fold_panel.AddFoldPanel(_("Markers"), collapsed=True)
-        mtw = MarkersPanel(item, navigation, tracker)
+        mtw = MarkersPanel(item, navigation, tracker, icp)
         fold_panel.ApplyCaptionStyle(item, style)
         fold_panel.AddFoldPanelWindow(item, mtw, spacing= 0,
@@ -352,7 +348,7 @@ class InnerFoldPanel(wx.Panel):
 class NeuronavigationPanel(wx.Panel):
-    def __init__(self, parent, navigation, tracker, pedal_connection, neuronavigation_api):
+    def __init__(self, parent, navigation, tracker, icp, pedal_connection, neuronavigation_api):
         wx.Panel.__init__(self, parent)
         try:
             default_colour = wx.SystemSettings.GetColour(wx.SYS_COLOUR_MENUBAR)
@@ -369,9 +365,8 @@ class NeuronavigationPanel(wx.Panel):
         self.neuronavigation_api = neuronavigation_api
         self.navigation = navigation
-        self.icp = ICP()
+        self.icp = icp
         self.tracker = tracker
-        self.robot = Robot(tracker)
         self.nav_status = False
         self.tracker_fiducial_being_set = None
@@ -558,6 +553,7 @@ class NeuronavigationPanel(wx.Panel):
         Publisher.subscribe(self.UpdateTarget, 'Update target')
         Publisher.subscribe(self.OnStartNavigation, 'Start navigation')
         Publisher.subscribe(self.OnStopNavigation, 'Stop navigation')
+        Publisher.subscribe(self.OnDialogRobotDestroy, 'Dialog robot destroy')
     def LoadImageFiducials(self, label, coord):
         fiducial = self.GetFiducialByAttribute(const.IMAGE_FIDUCIALS, 'label', label)
@@ -671,8 +667,6 @@ class NeuronavigationPanel(wx.Panel):
         self.checkbox_icp.SetValue(False)
     def OnDisconnectTracker(self):
-        if self.tracker.tracker_id == const.ROBOT:
-            self.robot.StopRobotThreadNavigation()
         self.tracker.DisconnectTracker()
         self.ResetICP()
         self.tracker.UpdateUI(self.select_tracker_elem, self.numctrls_fiducial[3:6], self.txtctrl_fre)
@@ -691,7 +685,12 @@ class NeuronavigationPanel(wx.Panel):
         self.tracker.SetTracker(choice)
         if self.tracker.tracker_id == const.ROBOT:
-            self.tracker.ConnectToRobot(self.navigation, self.tracker, self.robot)
+            self.dlg_correg_robot = dlg.CreateTransformationMatrixRobot(self.tracker)
+            if self.dlg_correg_robot.ShowModal() == wx.ID_OK:
+                Publisher.sendMessage('Robot navigation mode', robot_mode=True)
+            else:
+                Publisher.sendMessage('Disconnect tracker')
+                wx.MessageBox(_("Not possible to connect to the robot."), _("InVesalius 3"))
         self.ResetICP()
         self.tracker.UpdateUI(ctrl, self.numctrls_fiducial[3:6], self.txtctrl_fre)
@@ -776,8 +775,8 @@ class NeuronavigationPanel(wx.Panel):
         self.navigation.StopNavigation()
         if self.tracker.tracker_id == const.ROBOT:
-            Publisher.sendMessage('Robot target matrix', robot_tracker_flag=False,
-                                  m_change_robot_to_head=None)
+            Publisher.sendMessage('Update robot target', robot_tracker_flag=False,
+                                  target_index=None, target=None)
         # Enable all navigation buttons
         choice_ref.Enable(True)
@@ -786,6 +785,10 @@ class NeuronavigationPanel(wx.Panel):
         for btn_c in self.btns_set_fiducial:
             btn_c.Enable(True)
+    def OnDialogRobotDestroy(self):
+        if self.dlg_correg_robot:
+            self.dlg_correg_robot.Destroy()
+
     def CheckFiducialRegistrationError(self):
         self.navigation.UpdateFiducialRegistrationError(self.tracker)
         fre, fre_ok = self.navigation.GetFiducialRegistrationError(self.icp)
@@ -820,7 +823,7 @@ class NeuronavigationPanel(wx.Panel):
             for btn_c in self.btns_set_fiducial:
                 btn_c.Enable(False)
-            self.navigation.StartNavigation(self.tracker)
+            self.navigation.EstimateTrackerToInVTransformationMatrix(self.tracker)
             if not self.CheckFiducialRegistrationError():
                 # TODO: Exhibit FRE in a warning dialog and only starts navigation after user clicks ok
@@ -830,9 +833,11 @@ class NeuronavigationPanel(wx.Panel):
             if self.icp.use_icp:
                 self.checkbox_icp.Enable(True)
                 self.checkbox_icp.SetValue(True)
-            # Update FRE once more after starting the navigation, due to the optional use of ICP,
-            # which improves FRE.
-            self.CheckFiducialRegistrationError()
+                # Update FRE once more after starting the navigation, due to the optional use of ICP,
+                # which improves FRE.
+                self.CheckFiducialRegistrationError()
+
+            self.navigation.StartNavigation(self.tracker)
     def OnNavigate(self, evt, btn_nav):
         select_tracker_elem = self.select_tracker_elem
@@ -881,7 +886,6 @@ class NeuronavigationPanel(wx.Panel):
         )
         self.tracker.__init__()
         self.icp.__init__()
-        self.robot.__init__(self.tracker)
 class ObjectRegistrationPanel(wx.Panel):
@@ -1248,20 +1252,7 @@ class MarkersPanel(wx.Panel):
                 if field.type is bool:
                     setattr(self, field.name, str_val=='True')
-    @dataclasses.dataclass
-    class Robot_Marker:
-        """Class for storing robot target."""
-        m_robot_target : list = None
-
-        @property
-        def robot_target_matrix(self):
-            return self.m_robot_target
-
-        @robot_target_matrix.setter
-        def robot_target_matrix(self, new_m_robot_target):
-            self.m_robot_target = new_m_robot_target
-
-    def __init__(self, parent, navigation, tracker):
+    def __init__(self, parent, navigation, tracker, icp):
         wx.Panel.__init__(self, parent)
         try:
             default_colour = wx.SystemSettings.GetColour(wx.SYS_COLOUR_MENUBAR)
@@ -1273,6 +1264,7 @@ class MarkersPanel(wx.Panel):
         self.navigation = navigation
         self.tracker = tracker
+        self.icp = icp
         self.__bind_events()
@@ -1280,11 +1272,9 @@ class MarkersPanel(wx.Panel):
         self.current_coord = [0, 0, 0, None, None, None]
         self.current_seed = 0, 0, 0
-        self.current_robot_target_matrix = [None] * 9
+
         self.markers = []
-        self.robot_markers = []
         self.nav_status = False
-        self.raw_target_robot = None, None
         self.marker_colour = const.MARKER_COLOUR
         self.marker_size = const.MARKER_SIZE
@@ -1383,7 +1373,6 @@ class MarkersPanel(wx.Panel):
         Publisher.subscribe(self.UpdateNavigationStatus, 'Navigation status')
         Publisher.subscribe(self.UpdateSeedCoordinates, 'Update tracts')
         Publisher.subscribe(self.OnChangeCurrentSession, 'Current session changed')
-        Publisher.subscribe(self.UpdateRobotCoordinates, 'Update raw coordinates')
     def __find_target_marker(self):
         """
@@ -1417,7 +1406,6 @@ class MarkersPanel(wx.Panel):
         """
         for i in reversed(index):
             del self.markers[i]
-            del self.robot_markers[i]
             self.lc.DeleteItem(i)
             for n in range(0, self.lc.GetItemCount()):
                 self.lc.SetItem(n, 0, str(n + 1))
@@ -1470,34 +1458,37 @@ class MarkersPanel(wx.Panel):
     def UpdateSeedCoordinates(self, root=None, affine_vtk=None, coord_offset=(0, 0, 0), coord_offset_w=(0, 0, 0)):
         self.current_seed = coord_offset_w
-    def UpdateRobotCoordinates(self, coordinates_raw, markers_flag):
-        self.raw_target_robot = coordinates_raw[1], coordinates_raw[2]
-
     def OnMouseRightDown(self, evt):
         # TODO: Enable the "Set as target" only when target is created with registered object
         menu_id = wx.Menu()
         edit_id = menu_id.Append(0, _('Edit label'))
         menu_id.Bind(wx.EVT_MENU, self.OnMenuEditMarkerLabel, edit_id)
-        color_id = menu_id.Append(2, _('Edit color'))
+        color_id = menu_id.Append(1, _('Edit color'))
         menu_id.Bind(wx.EVT_MENU, self.OnMenuSetColor, color_id)
         menu_id.AppendSeparator()
-        target_menu = menu_id.Append(1, _('Set as target'))
+        target_menu = menu_id.Append(2, _('Set as target'))
         menu_id.Bind(wx.EVT_MENU, self.OnMenuSetTarget, target_menu)
         menu_id.AppendSeparator()
-        send_target_to_robot = menu_id.Append(3, _('Send target to robot'))
-        menu_id.Bind(wx.EVT_MENU, self.OnMenuSendTargetToRobot, send_target_to_robot)
-        if all([elem is not None for elem in self.markers[self.lc.GetFocusedItem()].coord[3:]]):
+        check_target_angles = all([elem is not None for elem in self.markers[self.lc.GetFocusedItem()].coord[3:]])
+        # Enable "Send target to robot" button only if tracker is robot, if navigation is on and if target is not none
+        if self.tracker.tracker_id == const.ROBOT:
+            send_target_to_robot_compensation = menu_id.Append(3, _('Sets target to robot head move compensation'))
+            menu_id.Bind(wx.EVT_MENU, self.OnMenuSetRobotCompensation, send_target_to_robot_compensation)
+            send_target_to_robot = menu_id.Append(4, _('Send target from InVesalius to robot'))
+            menu_id.Bind(wx.EVT_MENU, self.OnMenuSendTargetToRobot, send_target_to_robot)
+            if self.nav_status and check_target_angles:
+                send_target_to_robot_compensation.Enable(True)
+                send_target_to_robot.Enable(True)
+            else:
+                send_target_to_robot_compensation.Enable(False)
+                send_target_to_robot.Enable(False)
+
+        if check_target_angles:
             target_menu.Enable(True)
         else:
             target_menu.Enable(False)
-        # Enable "Send target to robot" button only if tracker is robot, if navigation is on and if target is not none
-        m_target_robot = np.array([self.robot_markers[self.lc.GetFocusedItem()].robot_target_matrix])
-        if self.tracker.tracker_id == const.ROBOT and self.nav_status and m_target_robot.any():
-            send_target_to_robot.Enable(True)
-        else:
-            send_target_to_robot.Enable(False)
         # TODO: Create the remove target option so the user can disable the target without removing the marker
         # target_menu_rem = menu_id.Append(3, _('Remove target'))
         # menu_id.Bind(wx.EVT_MENU, self.OnMenuRemoveTarget, target_menu_rem)
@@ -1547,14 +1538,33 @@ class MarkersPanel(wx.Panel):
             Publisher.sendMessage('Set new color', index=index, color=color_new)
+    def OnMenuSetRobotCompensation(self, evt):
+        if isinstance(evt, int):
+           self.lc.Focus(evt)
+
+        Publisher.sendMessage('Reset robot process', data=None)
+        matrix_tracker_fiducials = self.tracker.GetMatrixTrackerFiducials()
+        Publisher.sendMessage('Update tracker fiducials matrix',
+                              matrix_tracker_fiducials=matrix_tracker_fiducials)
+        Publisher.sendMessage('Update robot target', robot_tracker_flag=True, target_index=self.lc.GetFocusedItem(), target=None)
+
     def OnMenuSendTargetToRobot(self, evt):
         if isinstance(evt, int):
            self.lc.Focus(evt)
+        index = self.lc.GetFocusedItem()
+        if index == -1:
+            wx.MessageBox(_("No data selected."), _("InVesalius 3"))
+            return
+
+        Publisher.sendMessage('Reset robot process', data=None)
+        matrix_tracker_fiducials = self.tracker.GetMatrixTrackerFiducials()
+        Publisher.sendMessage('Update tracker fiducials matrix',
+                              matrix_tracker_fiducials=matrix_tracker_fiducials)
-        m_target_robot = self.robot_markers[self.lc.GetFocusedItem()].robot_target_matrix
+        target_coord = self.markers[index].coord[:3]
+        target = dcr.image_to_tracker(self.navigation.m_change, target_coord, self.icp)
-        Publisher.sendMessage('Reset robot process')
-        Publisher.sendMessage('Robot target matrix', robot_tracker_flag=True, m_change_robot_to_head=m_target_robot)
+        Publisher.sendMessage('Update robot target', robot_tracker_flag=True, target_index=self.lc.GetFocusedItem(), target=target.tolist())
     def OnDeleteAllMarkers(self, evt=None):
         if evt is not None:
@@ -1566,9 +1576,11 @@ class MarkersPanel(wx.Panel):
             Publisher.sendMessage('Disable or enable coil tracker', status=False)
             if evt is not None:
                 wx.MessageBox(_("Target deleted."), _("InVesalius 3"))
+            if self.tracker.tracker_id == const.ROBOT:
+                Publisher.sendMessage('Update robot target', robot_tracker_flag=False,
+                                      target_index=None, target=None)
         self.markers = []
-        self.robot_markers = []
         Publisher.sendMessage('Remove all markers', indexes=self.lc.GetItemCount())
         self.lc.DeleteAllItems()
         Publisher.sendMessage('Stop Blink Marker', index='DeleteAll')
@@ -1596,8 +1608,8 @@ class MarkersPanel(wx.Panel):
             if self.__find_target_marker() in index:
                 Publisher.sendMessage('Disable or enable coil tracker', status=False)
                 if self.tracker.tracker_id == const.ROBOT:
-                    Publisher.sendMessage('Robot target matrix', robot_tracker_flag=False,
-                                          m_change_robot_to_head=[])
+                    Publisher.sendMessage('Update robot target', robot_tracker_flag=False,
+                                          target_index=None, target=None)
                 wx.MessageBox(_("Target deleted."), _("InVesalius 3"))
             self.__delete_multiple_markers(index)
@@ -1702,12 +1714,11 @@ class MarkersPanel(wx.Panel):
         new_marker.session_id = session_id or self.current_session
         if self.tracker.tracker_id == const.ROBOT and self.nav_status:
-            self.current_robot_target_matrix = db.compute_robot_to_head_matrix(self.raw_target_robot)
+            current_head_robot_target_status = True
         else:
-            self.current_robot_target_matrix = [None] * 9
+            current_head_robot_target_status = False
-        new_robot_marker = self.Robot_Marker()
-        new_robot_marker.robot_target_matrix = self.current_robot_target_matrix
+        Publisher.sendMessage('Add marker to robot control', data=current_head_robot_target_status)
         # Note that ball_id is zero-based, so we assign it len(self.markers) before the new marker is added
         if all([elem is not None for elem in new_marker.coord[3:]]):
@@ -1723,7 +1734,6 @@ class MarkersPanel(wx.Panel):
         self.markers.append(new_marker)
-        self.robot_markers.append(new_robot_marker)
         # Add item to list control in panel
         num_items = self.lc.GetItemCount()
@@ -21,10 +21,10 @@ import wx
 import invesalius.data.bases as db
 import invesalius.gui.dialogs as dlg
-from invesalius.pubsub import pub as Publisher
+from invesalius.utils import Singleton
-class ICP():
+class ICP(metaclass=Singleton):
     def __init__(self):
         self.use_icp = False
         self.m_icp = None
@@ -35,7 +35,6 @@ class ICP():
         if not self.use_icp:
             if dlg.ICPcorregistration(navigation.fre):
-                Publisher.sendMessage('Stop navigation')
                 use_icp, self.m_icp = self.OnICP(navigation, tracker, navigation.m_change)
                 if use_icp:
                     self.icp_fre = db.calculate_fre(tracker.tracker_fiducials_raw, navigation.all_fiducials,
@@ -43,7 +42,6 @@ class ICP():
                     self.SetICP(navigation, use_icp)
                 else:
                     print("ICP canceled")
-                Publisher.sendMessage('Start navigation')
     def OnICP(self, navigation, tracker, m_change):
         ref_mode_id = navigation.GetReferenceMode()
@@ -34,6 +34,7 @@ import invesalius.data.tractography as dti
 import invesalius.data.transformations as tr
 import invesalius.data.vtk_utils as vtk_utils
 from invesalius.pubsub import pub as Publisher
+from invesalius.utils import Singleton
 class QueueCustom(queue.Queue):
@@ -83,7 +84,7 @@ class UpdateNavigationScene(threading.Thread):
         threading.Thread.__init__(self, name='UpdateScene')
         self.serial_port_enabled, self.view_tracts, self.peel_loaded = vis_components
-        self.coord_queue, self.serial_port_queue, self.tracts_queue, self.icp_queue, self.robot_target_queue = vis_queues
+        self.coord_queue, self.serial_port_queue, self.tracts_queue, self.icp_queue = vis_queues
         self.sle = sle
         self.event = event
         self.neuronavigation_api = neuronavigation_api
@@ -93,7 +94,7 @@ class UpdateNavigationScene(threading.Thread):
         while not self.event.is_set():
             got_coords = False
             try:
-                coord, [coordinates_raw, markers_flag], m_img, view_obj = self.coord_queue.get_nowait()
+                coord, markers_flag, m_img, view_obj = self.coord_queue.get_nowait()
                 got_coords = True
                 # print('UpdateScene: get {}'.format(count))
@@ -117,7 +118,6 @@ class UpdateNavigationScene(threading.Thread):
                 # see the red cross in the position of the offset marker
                 wx.CallAfter(Publisher.sendMessage, 'Update slices position', position=coord[:3])
                 wx.CallAfter(Publisher.sendMessage, 'Set cross focal point', position=coord)
-                wx.CallAfter(Publisher.sendMessage, 'Update raw coordinates', coordinates_raw=coordinates_raw, markers_flag=markers_flag)
                 wx.CallAfter(Publisher.sendMessage, 'Update slice viewer')
                 wx.CallAfter(Publisher.sendMessage, 'Sensor ID', markers_flag=markers_flag)
@@ -140,7 +140,7 @@ class UpdateNavigationScene(threading.Thread):
                     self.coord_queue.task_done()
-class Navigation():
+class Navigation(metaclass=Singleton):
     def __init__(self, pedal_connection, neuronavigation_api):
         self.pedal_connection = pedal_connection
         self.neuronavigation_api = neuronavigation_api
@@ -157,7 +157,6 @@ class Navigation():
         self.coord_queue = QueueCustom(maxsize=1)
         self.icp_queue = QueueCustom(maxsize=1)
         self.object_at_target_queue = QueueCustom(maxsize=1)
-        self.robot_target_queue = QueueCustom(maxsize=1)
         # self.visualization_queue = QueueCustom(maxsize=1)
         self.serial_port_queue = QueueCustom(maxsize=1)
         self.coord_tracts_queue = QueueCustom(maxsize=1)
@@ -245,9 +244,14 @@ class Navigation():
         if state and permission_to_stimulate:
             self.serial_port_connection.SendPulse()
-    def StartNavigation(self, tracker):
+    def EstimateTrackerToInVTransformationMatrix(self, tracker):
         tracker_fiducials, tracker_fiducials_raw = tracker.GetTrackerFiducials()
+        self.all_fiducials = np.vstack([self.image_fiducials, tracker_fiducials])
+        self.m_change = tr.affine_matrix_from_points(self.all_fiducials[3:, :].T, self.all_fiducials[:3, :].T,
+                                                shear=False, scale=False)
+
+    def StartNavigation(self, tracker):
         # initialize jobs list
         jobs_list = []
@@ -255,17 +259,9 @@ class Navigation():
             self.event.clear()
         vis_components = [self.serial_port_in_use, self.view_tracts, self.peel_loaded]
-        vis_queues = [self.coord_queue, self.serial_port_queue, self.tracts_queue, self.icp_queue, self.robot_target_queue]
+        vis_queues = [self.coord_queue, self.serial_port_queue, self.tracts_queue, self.icp_queue]
         Publisher.sendMessage("Navigation status", nav_status=True, vis_status=vis_components)
-
-        self.all_fiducials = np.vstack([self.image_fiducials, tracker_fiducials])
-
-        # fiducials matrix
-        m_change = tr.affine_matrix_from_points(self.all_fiducials[3:, :].T, self.all_fiducials[:3, :].T,
-                                                shear=False, scale=False)
-        self.m_change = m_change
-
         errors = False
         if self.track_obj:
@@ -279,14 +275,14 @@ class Navigation():
                 # obj_reg[0] is object 3x3 fiducial matrix and obj_reg[1] is 3x3 orientation matrix
                 obj_fiducials, obj_orients, obj_ref_mode, obj_name = self.obj_reg
-                coreg_data = [m_change, obj_ref_mode]
+                coreg_data = [self.m_change, obj_ref_mode]
                 if self.ref_mode_id:
                     coord_raw, markers_flag = tracker.TrackerCoordinates.GetCoordinates()
                 else:
                     coord_raw = np.array([None])
-                obj_data = db.object_registration(obj_fiducials, obj_orients, coord_raw, m_change)
+                obj_data = db.object_registration(obj_fiducials, obj_orients, coord_raw, self.m_change)
                 coreg_data.extend(obj_data)
                 queues = [self.coord_queue, self.coord_tracts_queue, self.icp_queue, self.object_at_target_queue]
@@ -295,7 +291,7 @@ class Navigation():
                                                                 self.event, self.sleep_nav, tracker.tracker_id,
                                                                 self.target))
         else:
-            coreg_data = (m_change, 0)
+            coreg_data = (self.m_change, 0)
             queues = [self.coord_queue, self.coord_tracts_queue, self.icp_queue]
             jobs_list.append(dcr.CoordinateCorregistrateNoObject(self.ref_mode_id, tracker, coreg_data,
                                                                     self.view_tracts, queues,
@@ -1,308 +0,0 @@
-#--------------------------------------------------------------------------
-# 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
-from time import sleep
-
-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][0] or not self.tracker.trk_init[0][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[0][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[0][1][0]
-        self.trck_init_tracker = trck_init[0][0][0]
-        self.trk_id = trck_init[1]
-        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
-        self.previous_robot_status = False
-
-    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_motion_reset(self):
-        self.trck_init_robot.StopRobot()
-        self.arc_motion_flag = False
-        self.arc_motion_step_flag = const.ROBOT_MOTIONS["normal"]
-
-    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)
-            robot_status = True
-        else:
-            print("Head is too far from the robot basis")
-            robot_status = False
-
-        return robot_status
-
-    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]
-        robot_status = False
-
-        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)
-                    robot_status = True
-                    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)
-                        robot_status = 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()
-
-        return robot_status
-
-    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_motion_reset()
-                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()
-
-            robot_status = self.robot_control(current_tracker_coordinates_in_robot, current_robot_coordinates, markers_flag)
-
-            if self.previous_robot_status != robot_status:
-                wx.CallAfter(Publisher.sendMessage, 'Update robot status', robot_status=robot_status)
-                self.previous_robot_status = robot_status
-
-            sleep(const.SLEEP_ROBOT)
@@ -42,7 +42,6 @@ class Tracker():
         self.thread_coord = None
         self.event_coord = threading.Event()
-        self.event_robot = threading.Event()
         self.TrackerCoordinates = dco.TrackerCoordinates()
@@ -78,11 +77,8 @@ class Tracker():
                 if self.thread_coord:
                     self.event_coord.set()
-                    self.event_robot.set()
                     self.thread_coord.join()
                     self.event_coord.clear()
-                    self.event_robot.clear()
-
                 Publisher.sendMessage('Update status text in GUI',
                                         label=_("Tracker disconnected"))
@@ -92,13 +88,6 @@ class Tracker():
                                         label=_("Tracker still connected"))
                 print("Tracker still connected!")
-    def ConnectToRobot(self, navigation, tracker, robot):
-        robot.SetRobotQueues([navigation.robot_target_queue, navigation.object_at_target_queue])
-        robot.OnRobotConnection()
-        trk_init_robot = self.trk_init[0][1][0]
-        if trk_init_robot:
-            robot.StartRobotThreadNavigation(tracker, navigation.coord_queue)
-            Publisher.sendMessage('Robot navigation mode', robot_mode=True)
     def IsTrackerInitialized(self):
         return self.trk_init and self.tracker_id and self.tracker_connected
@@ -158,7 +147,7 @@ class Tracker():
         m_probe_ref_right = np.linalg.inv(self.m_tracker_fiducials_raw[3]) @ self.m_tracker_fiducials_raw[2]
         m_probe_ref_nasion = np.linalg.inv(self.m_tracker_fiducials_raw[5]) @ self.m_tracker_fiducials_raw[4]
-        return [m_probe_ref_left, m_probe_ref_right, m_probe_ref_nasion]
+        return [m_probe_ref_left.tolist(), m_probe_ref_right.tolist(), m_probe_ref_nasion.tolist()]
     def GetTrackerInfo(self):
         return self.trk_init, self.tracker_id
@@ -79,5 +79,7 @@ class RemoteControl:
                     })
             except TypeError:
                 pass
+            except socketio.exceptions.BadNamespaceError:
+                pass
         Publisher.add_sendMessage_hook(_emit)
	@@ -832,7 +832,6 @@ SEED_RADIUS = 1.5		@@ -832,7 +832,6 @@ SEED_RADIUS = 1.5
832	# Increased the default sleep parameter from 0.1 to 0.15 to decrease CPU load during navigation.	832	# Increased the default sleep parameter from 0.1 to 0.15 to decrease CPU load during navigation.
833	SLEEP_NAVIGATION = 0.2	833	SLEEP_NAVIGATION = 0.2
834	SLEEP_COORDINATES = 0.05	834	SLEEP_COORDINATES = 0.05
835	-SLEEP_ROBOT = 0.01
836		835
837	BRAIN_OPACITY = 0.6	836	BRAIN_OPACITY = 0.6
838	N_CPU = psutil.cpu_count()	837	N_CPU = psutil.cpu_count()
	@@ -864,14 +863,3 @@ PULSE_DURATION_IN_MILLISECONDS = 0.2		@@ -864,14 +863,3 @@ PULSE_DURATION_IN_MILLISECONDS = 0.2
864		863
865	#Robot	864	#Robot
866	ROBOT_ElFIN_IP = ['143.107.220.251', '169.254.153.251', '127.0.0.1']	865	ROBOT_ElFIN_IP = ['143.107.220.251', '169.254.153.251', '127.0.0.1']
867	-ROBOT_ElFIN_PORT = 10003
868	-ROBOT_MOTIONS = {"normal": 0, "linear out": 1, "arc": 2}
869	-ROBOT_HEAD_VELOCITY_THRESHOLD = 10 #mm/s
870	-ROBOT_ARC_THRESHOLD_DISTANCE = 100 #mm
871	-ROBOT_VERSOR_SCALE_FACTOR = 70
872	-#Robot Working Space is defined as 800mm in Elfin manual. For safety, the value is reduced by 5%.
873	-ROBOT_WORKING_SPACE = 760 #mm
874	-ROBOT_MOVE_STATE = {"free to move": 0,
875	- "in motion": 1009,
876	- "waiting for execution": 1013,
877	- "error": 1025}
	@@ -163,6 +163,12 @@ def transform_icp(m_img, m_icp):		@@ -163,6 +163,12 @@ def transform_icp(m_img, m_icp):
163		163
164	return m_img	164	return m_img
165		165
		166	+def inverse_transform_icp(m_img, m_icp):
		167	+ coord_img = [m_img[0, -1], -m_img[1, -1], m_img[2, -1], 1]
		168	+ m_img[0, -1], m_img[1, -1], m_img[2, -1], _ = np.linalg.inv(m_icp) @ coord_img
		169	+ m_img[0, -1], m_img[1, -1], m_img[2, -1] = m_img[0, -1], -m_img[1, -1], m_img[2, -1]
		170	+
		171	+ return m_img
166		172
167	def object_registration(fiducials, orients, coord_raw, m_change):	173	def object_registration(fiducials, orients, coord_raw, m_change):
168	"""	174	"""
	@@ -244,47 +250,3 @@ def object_registration(fiducials, orients, coord_raw, m_change):		@@ -244,47 +250,3 @@ def object_registration(fiducials, orients, coord_raw, m_change):
244	)	250	)
245		251
246	return t_obj_raw, s0_raw, r_s0_raw, s0_dyn, m_obj_raw, r_obj_img	252	return t_obj_raw, s0_raw, r_s0_raw, s0_dyn, m_obj_raw, r_obj_img
247	-
248	-def compute_robot_to_head_matrix(raw_target_robot):
249	- """
250	- :param head: nx6 array of head coordinates from tracking device in robot space
251	- :param robot: nx6 array of robot coordinates
252	-
253	- :return: target_robot_matrix: 3x3 array representing change of basis from robot to head in robot system
254	- """
255	- head, robot = raw_target_robot
256	- # compute head target matrix
257	- m_head_target = dco.coordinates_to_transformation_matrix(
258	- position=head[:3],
259	- orientation=head[3:],
260	- axes='rzyx',
261	- )
262	-
263	- # compute robot target matrix
264	- m_robot_target = dco.coordinates_to_transformation_matrix(
265	- position=robot[:3],
266	- orientation=robot[3:],
267	- axes='rzyx',
268	- )
269	- robot_to_head_matrix = np.linalg.inv(m_head_target) @ m_robot_target
270	-
271	- return robot_to_head_matrix
272	-
273	-
274	-class transform_tracker_to_robot(object):
275	- M_tracker_to_robot = np.array([])
276	- def transformation_tracker_to_robot(self, tracker_coord):
277	- if not transform_tracker_to_robot.M_tracker_to_robot.any():
278	- return None
279	-
280	- M_tracker = dco.coordinates_to_transformation_matrix(
281	- position=tracker_coord[:3],
282	- orientation=tracker_coord[3:6],
283	- axes='rzyx',
284	- )
285	- M_tracker_in_robot = transform_tracker_to_robot.M_tracker_to_robot @ M_tracker
286	-
287	- translation, angles_as_deg = dco.transformation_matrix_to_coordinates(M_tracker_in_robot, axes='rzyx')
288	- tracker_in_robot = list(translation) + list(angles_as_deg)
289	-
290	- return tracker_in_robot
	@@ -101,6 +101,32 @@ def tracker_to_image(m_change, m_probe_ref, r_obj_img, m_obj_raw, s0_dyn):		@@ -101,6 +101,32 @@ def tracker_to_image(m_change, m_probe_ref, r_obj_img, m_obj_raw, s0_dyn):
101	m_img[:3, :3] = r_obj[:3, :3]	101	m_img[:3, :3] = r_obj[:3, :3]
102	return m_img	102	return m_img
103		103
		104	+def image_to_tracker(m_change, target, icp):
		105	+ """Compute the transformation matrix to the tracker coordinate system
		106	+
		107	+ :param m_change: Corregistration transformation obtained from fiducials
		108	+ :type m_change: numpy.ndarray
		109	+ :param target: Target in invesalius coordinate system
		110	+ :type target: numpy.ndarray
		111	+ :param icp: ICP transformation matrix
		112	+ :type icp: numpy.ndarray
		113	+
		114	+ :return: 4 x 4 numpy double array
		115	+ :rtype: numpy.ndarray
		116	+ """
		117	+ m_target_in_image = dco.coordinates_to_transformation_matrix(
		118	+ position=target[:3],
		119	+ orientation=[0, 0, 0],
		120	+ axes='sxyz',
		121	+ )
		122	+ if icp.use_icp:
		123	+ m_target_in_image = bases.inverse_transform_icp(m_target_in_image, icp.m_icp)
		124	+ m_target_in_tracker = np.linalg.inv(m_change) @ m_target_in_image
		125	+
		126	+ # invert y coordinate
		127	+ m_target_in_tracker[2, -1] = -m_target_in_tracker[2, -1]
		128	+
		129	+ return m_target_in_tracker
104		130
105	def corregistrate_object_dynamic(inp, coord_raw, ref_mode_id, icp):	131	def corregistrate_object_dynamic(inp, coord_raw, ref_mode_id, icp):
106		132
	@@ -193,7 +219,6 @@ class CoordinateCorregistrate(threading.Thread):		@@ -193,7 +219,6 @@ class CoordinateCorregistrate(threading.Thread):
193	self.object_at_target_queue = queues[3]	219	self.object_at_target_queue = queues[3]
194	self.icp = None	220	self.icp = None
195	self.m_icp = None	221	self.m_icp = None
196	- self.robot_tracker_flag = None
197	self.last_coord = None	222	self.last_coord = None
198	self.tracker_id = tracker_id	223	self.tracker_id = tracker_id
199	self.target = target	224	self.target = target
	@@ -254,7 +279,7 @@ class CoordinateCorregistrate(threading.Thread):		@@ -254,7 +279,7 @@ class CoordinateCorregistrate(threading.Thread):
254	if self.icp:	279	if self.icp:
255	m_img = bases.transform_icp(m_img, self.m_icp)	280	m_img = bases.transform_icp(m_img, self.m_icp)
256		281
257	- self.coord_queue.put_nowait([coord, [coord_raw, markers_flag], m_img, view_obj])	282	+ self.coord_queue.put_nowait([coord, markers_flag, m_img, view_obj])
258	# print('CoordCoreg: put {}'.format(count))	283	# print('CoordCoreg: put {}'.format(count))
259	# count += 1	284	# count += 1
260		285
	@@ -307,7 +332,7 @@ class CoordinateCorregistrateNoObject(threading.Thread):		@@ -307,7 +332,7 @@ class CoordinateCorregistrateNoObject(threading.Thread):
307	if self.icp:	332	if self.icp:
308	m_img = bases.transform_icp(m_img, self.m_icp)	333	m_img = bases.transform_icp(m_img, self.m_icp)
309		334
310	- self.coord_queue.put_nowait([coord, [coord_raw, markers_flag], m_img, view_obj])	335	+ self.coord_queue.put_nowait([coord, markers_flag, m_img, view_obj])
311		336
312	if self.view_tracts:	337	if self.view_tracts:
313	self.coord_tracts_queue.put_nowait(m_img_flip)	338	self.coord_tracts_queue.put_nowait(m_img_flip)
	@@ -1,252 +0,0 @@	@@ -1,252 +0,0 @@
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, motion_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 == const.ROBOT_MOVE_STATE["free to move"]:
34	- if motion_type == const.ROBOT_MOTIONS["normal"] or motion_type == const.ROBOT_MOTIONS["linear out"]:
35	- self.cobot.MoveL(target)
36	- elif motion_type == const.ROBOT_MOTIONS["arc"]:
37	- self.cobot.MoveC(target)
38	- elif status == const.ROBOT_MOVE_STATE["error"]:
39	- self.StopRobot()
40	-
41	- def StopRobot(self):
42	- # Takes some microseconds to the robot actual stops after the command.
43	- # The sleep time is required to guarantee the stop
44	- self.cobot.GrpStop()
45	- sleep(0.1)
46	-
47	- def Close(self):
48	- self.StopRobot()
49	- #TODO: robot function to close? self.cobot.close()
50	-
51	-class Elfin:
52	- def __init__(self):
53	- """
54	- Class to communicate with elfin robot. This class follows "HansRobot Communication Protocol Interface".
55	- """
56	- self.end_msg = ",;"
57	-
58	- def connect(self, server_ip, port_number, message_size, robot_id):
59	- mySocket = socket(AF_INET, SOCK_STREAM)
60	- mySocket.connect((server_ip, port_number))
61	-
62	- self.message_size = message_size
63	- self.robot_id = str(robot_id)
64	- self.mySocket = mySocket
65	-
66	- def send(self, message):
67	- self.mySocket.sendall(message.encode('utf-8'))
68	- data = self.mySocket.recv(self.message_size).decode('utf-8').split(',')
69	- status = self.check_status(data)
70	- if status and type(data) != bool:
71	- if len(data) > 3:
72	- return data[2:-1]
73	- return status
74	-
75	- def check_status(self, recv_message):
76	- status = recv_message[1]
77	- if status == 'OK':
78	- return True
79	-
80	- elif status == 'Fail':
81	- print("Error code: ", recv_message[2])
82	- return False
83	-
84	- def Electrify(self):
85	- """
86	- Function: Power the robot
87	- Notes: successful completion of power up before returning, power up time is
88	- about 44s.
89	- :return:
90	- if Error Return False
91	- if not Error Return True
92	- """
93	- message = "Electrify" + self.end_msg
94	- status = self.send(message)
95	- return status
96	-
97	- def BlackOut(self):
98	- """
99	- Function: Robot blackout
100	- Notes: successful power outage will only return, power failure time is 3s.
101	- :return:
102	- if Error Return False
103	- if not Error Return True
104	- """
105	- message = "BlackOut" + self.end_msg
106	- status = self.send(message)
107	- return status
108	-
109	- def StartMaster(self):
110	- """
111	- Function: Start master station
112	- Notes: the master station will not be returned until successfully started, startup
113	- master time is about 4s.
114	- :return:
115	- if Error Return False
116	- if not Error Return True
117	- """
118	- message = "StartMaster" + self.end_msg
119	- status = self.send(message)
120	- return status
121	-
122	- def CloseMaster(self):
123	- """
124	- Function: Close master station
125	- Notes: the master station will not be returned until successfully closed, shut
126	- down the master station time is about 2s.
127	- :return:
128	- if Error Return False
129	- if not Error Return True
130	- """
131	- message = "CloseMaster" + self.end_msg
132	- status = self.send(message)
133	- return status
134	-
135	- def GrpPowerOn(self):
136	- """
137	- Function: Robot servo on
138	- :return:
139	- if Error Return False
140	- if not Error Return True
141	- """
142	- message = "GrpPowerOn," + self.robot_id + self.end_msg
143	- status = self.send(message)
144	- return status
145	-
146	- def GrpPowerOff(self):
147	- """
148	- Function: Robot servo off
149	- :return:
150	- if Error Return False
151	- if not Error Return True
152	- """
153	- message = "GrpPowerOff," + self.robot_id + self.end_msg
154	- status = self.send(message)
155	- return status
156	-
157	- def GrpStop(self):
158	- """
159	- Function: Stop robot
160	- :return:
161	- if Error Return False
162	- if not Error Return True
163	- """
164	- message = "GrpStop," + self.robot_id + self.end_msg
165	- status = self.send(message)
166	- return status
167	-
168	- def SetOverride(self, override):
169	- """
170	- function: Set speed ratio
171	- :param override:
172	- double: set speed ratio, range of 0.01~1
173	- :return:
174	- if Error Return False
175	- if not Error Return True
176	- """
177	-
178	- message = "SetOverride," + self.robot_id + ',' + str(override) + self.end_msg
179	- status = self.send(message)
180	- return status
181	-
182	- def ReadPcsActualPos(self):
183	- """Function: Get the actual position of the space coordinate
184	- :return:
185	- if True Return x,y,z,a,b,c
186	- if Error Return False
187	- """
188	- message = "ReadPcsActualPos," + self.robot_id + self.end_msg
189	- coord = self.send(message)
190	- if coord:
191	- return [float(s) for s in coord]
192	-
193	- return coord
194	-
195	- def MoveL(self, target):
196	- """
197	- function: Robot moves straight to the specified space coordinates
198	- :param: target:[X,Y,Z,RX,RY,RZ]
199	- :return:
200	- """
201	- target = [str(s) for s in target]
202	- target = (",".join(target))
203	- message = "MoveL," + self.robot_id + ',' + target + self.end_msg
204	- return self.send(message)
205	-
206	- def SetToolCoordinateMotion(self, status):
207	- """
208	- function: Function: Set tool coordinate motion
209	- :param: int Switch 0=close 1=open
210	- :return:
211	- if Error Return False
212	- if not Error Return True
213	- """
214	- message = "SetToolCoordinateMotion," + self.robot_id + ',' + str(status) + self.end_msg
215	- status = self.send(message)
216	- return status
217	-
218	- def ReadMoveState(self):
219	- """
220	- Function: Get the motion state of the robot
221	- :return:
222	- Current state of motion of robot:
223	- 0=motion completion;
224	- 1009=in motion;
225	- 1013=waiting for execution;
226	- 1025 =Error reporting
227	- """
228	- message = "ReadMoveState," + self.robot_id + self.end_msg
229	- status = int(self.send(message)[0])
230	- return status
231	-
232	- def MoveHoming(self):
233	- """
234	- Function: Robot returns to the origin
235	- :return:
236	- if Error Return False
237	- if not Error Return True
238	- """
239	- message = "MoveHoming," + self.robot_id + self.end_msg
240	- status = self.send(message)
241	- return status
242	-
243	- def MoveC(self, target):
244	- """
245	- function: Arc motion
246	- :param: Through position[X,Y,Z],GoalCoord[X,Y,Z,RX,RY,RZ],Type[0 or 1],;
247	- :return:
248	- """
249	- target = [str(s) for s in target]
250	- target = (",".join(target))
251	- message = "MoveC," + self.robot_id + ',' + target + self.end_msg
252	- return self.send(message)
	@@ -1,212 +0,0 @@	@@ -1,212 +0,0 @@
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	-
	@@ -1,308 +0,0 @@	@@ -1,308 +0,0 @@
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	-from time import sleep
24	-
25	-import invesalius.data.bases as db
26	-import invesalius.gui.dialogs as dlg
27	-import invesalius.constants as const
28	-from invesalius.pubsub import pub as Publisher
29	-
30	-try:
31	- import invesalius.data.elfin as elfin
32	- import invesalius.data.elfin_processing as elfin_process
33	- has_robot = True
34	-except ImportError:
35	- has_robot = False
36	-
37	-class Robot():
38	- def __init__(self, tracker):
39	- """
40	- Class to establish the connection between the robot and InVesalius
41	- :param tracker: tracker.py instance
42	- """
43	- self.tracker = tracker
44	- self.trk_init = None
45	- self.robot_target_queue = None
46	- self.object_at_target_queue = None
47	- self.process_tracker = None
48	-
49	- self.thread_robot = None
50	-
51	- self.robotcoordinates = RobotCoordinates()
52	-
53	- self.__bind_events()
54	-
55	- def __bind_events(self):
56	- Publisher.subscribe(self.OnSendCoordinates, 'Send coord to robot')
57	- Publisher.subscribe(self.OnUpdateRobotTargetMatrix, 'Robot target matrix')
58	- Publisher.subscribe(self.OnObjectTarget, 'Coil at target')
59	- Publisher.subscribe(self.OnResetProcessTracker, 'Reset robot process')
60	-
61	- def OnRobotConnection(self):
62	- if not self.tracker.trk_init[0][0][0] or not self.tracker.trk_init[0][1][0]:
63	- dlg.ShowNavigationTrackerWarning(self.tracker.tracker_id, self.tracker.trk_init[1])
64	- self.tracker.tracker_id = 0
65	- self.tracker.tracker_connected = False
66	- else:
67	- self.tracker.trk_init.append(self.robotcoordinates)
68	- self.process_tracker = elfin_process.TrackerProcessing()
69	- dlg_correg_robot = dlg.CreateTransformationMatrixRobot(self.tracker)
70	- if dlg_correg_robot.ShowModal() == wx.ID_OK:
71	- M_tracker_to_robot = dlg_correg_robot.GetValue()
72	- db.transform_tracker_to_robot.M_tracker_to_robot = M_tracker_to_robot
73	- self.robot_server = self.tracker.trk_init[0][1][0]
74	- self.trk_init = self.tracker.trk_init
75	- else:
76	- dlg.ShowNavigationTrackerWarning(self.tracker.tracker_id, 'disconnect')
77	- self.tracker.trk_init = None
78	- self.tracker.tracker_id = 0
79	- self.tracker.tracker_connected = False
80	-
81	- def StartRobotThreadNavigation(self, tracker, coord_queue):
82	- if tracker.event_robot.is_set():
83	- tracker.event_robot.clear()
84	- self.thread_robot = ControlRobot(self.trk_init, tracker, self.robotcoordinates,
85	- [coord_queue, self.robot_target_queue, self.object_at_target_queue],
86	- self.process_tracker, tracker.event_robot)
87	- self.thread_robot.start()
88	-
89	- def StopRobotThreadNavigation(self):
90	- self.thread_robot.join()
91	- self.OnResetProcessTracker()
92	-
93	- def OnResetProcessTracker(self):
94	- self.process_tracker.__init__()
95	-
96	- def OnSendCoordinates(self, coord):
97	- self.robot_server.SendCoordinates(coord)
98	-
99	- def OnUpdateRobotTargetMatrix(self, robot_tracker_flag, m_change_robot_to_head):
100	- try:
101	- self.robot_target_queue.put_nowait([robot_tracker_flag, m_change_robot_to_head])
102	- except queue.Full:
103	- pass
104	-
105	- def OnObjectTarget(self, state):
106	- try:
107	- if self.object_at_target_queue:
108	- self.object_at_target_queue.put_nowait(state)
109	- except queue.Full:
110	- pass
111	-
112	- def SetRobotQueues(self, queues):
113	- self.robot_target_queue, self.object_at_target_queue = queues
114	-
115	-
116	-class RobotCoordinates():
117	- """
118	- Class to set/get robot coordinates. Robot coordinates are acquired in ControlRobot thread.
119	- The class is required to avoid acquisition conflict with different threads (coordinates and navigation)
120	- """
121	- def __init__(self):
122	- self.coord = None
123	-
124	- def SetRobotCoordinates(self, coord):
125	- self.coord = coord
126	-
127	- def GetRobotCoordinates(self):
128	- return self.coord
129	-
130	-
131	-class ControlRobot(threading.Thread):
132	- def __init__(self, trck_init, tracker, robotcoordinates, queues, process_tracker, event_robot):
133	- """Class (threading) to perform the robot control.
134	- A distinguish thread is required to increase perform and separate robot control from navigation thread
135	- (safetly layer for robot positining).
136	-
137	- There is no GUI involved, them no Sleep is required
138	-
139	- :param trck_init: Initialization variable of tracking device and connection type. See tracker.py.
140	- :param tracker: tracker.py instance
141	- :param robotcoordinates: RobotCoordinates() instance
142	- :param queues: Queue instance that manage coordinates and robot target
143	- :param process_tracker: TrackerProcessing() instance from elfin_process
144	- :param event_robot: Threading event to ControlRobot when tasks is done
145	- """
146	- threading.Thread.__init__(self, name='ControlRobot')
147	-
148	- self.trck_init_robot = trck_init[0][1][0]
149	- self.trck_init_tracker = trck_init[0][0][0]
150	- self.trk_id = trck_init[1]
151	- self.tracker = tracker
152	- self.robotcoordinates = robotcoordinates
153	- self.robot_tracker_flag = False
154	- self.objattarget_flag = False
155	- self.target_flag = False
156	- self.m_change_robot_to_head = None
157	- self.coord_inv_old = None
158	- self.coord_queue = queues[0]
159	- self.robot_target_queue = queues[1]
160	- self.object_at_target_queue = queues[2]
161	- self.process_tracker = process_tracker
162	- self.event_robot = event_robot
163	- self.arc_motion_flag = False
164	- self.arc_motion_step_flag = None
165	- self.target_linear_out = None
166	- self.target_linear_in = None
167	- self.target_arc = None
168	- self.previous_robot_status = False
169	-
170	- def get_coordinates_from_tracker_devices(self):
171	- coord_robot_raw = self.trck_init_robot.Run()
172	- coord_robot = np.array(coord_robot_raw)
173	- coord_robot[3], coord_robot[5] = coord_robot[5], coord_robot[3]
174	- self.robotcoordinates.SetRobotCoordinates(coord_robot)
175	-
176	- coord_raw, markers_flag = self.tracker.TrackerCoordinates.GetCoordinates()
177	-
178	- return coord_raw, coord_robot_raw, markers_flag
179	-
180	- def robot_motion_reset(self):
181	- self.trck_init_robot.StopRobot()
182	- self.arc_motion_flag = False
183	- self.arc_motion_step_flag = const.ROBOT_MOTIONS["normal"]
184	-
185	- def robot_move_decision(self, distance_target, new_robot_coordinates, current_robot_coordinates, current_head_filtered):
186	- """
187	- There are two types of robot movements.
188	- We can imagine in two concentric spheres of different sizes. The inside sphere is to compensate for small head movements.
189	- It was named "normal" moves.
190	- The outside sphere is for the arc motion. The arc motion is a safety feature for long robot movements.
191	- Even for a new target or a sudden huge head movement.
192	- 1) normal:
193	- A linear move from the actual position until the target position.
194	- This movement just happens when move distance is below a threshold (const.ROBOT_ARC_THRESHOLD_DISTANCE)
195	- 2) arc motion:
196	- It can be divided into three parts.
197	- The first one represents the movement from the inner sphere to the outer sphere.
198	- The robot moves back using a radial move (it use the center of the head as a reference).
199	- The second step is the actual arc motion (along the outer sphere).
200	- A middle point, between the actual position and the target, is required.
201	- The last step is to make a linear move until the target (goes to the inner sphere)
202	-
203	- """
204	- #Check if the target is inside the working space
205	- if self.process_tracker.estimate_robot_target_length(new_robot_coordinates) < const.ROBOT_WORKING_SPACE:
206	- #Check the target distance to define the motion mode
207	- if distance_target < const.ROBOT_ARC_THRESHOLD_DISTANCE and not self.arc_motion_flag:
208	- self.trck_init_robot.SendCoordinates(new_robot_coordinates, const.ROBOT_MOTIONS["normal"])
209	-
210	- elif distance_target >= const.ROBOT_ARC_THRESHOLD_DISTANCE or self.arc_motion_flag:
211	- actual_point = current_robot_coordinates
212	- if not self.arc_motion_flag:
213	- head_center_coordinates = self.process_tracker.estimate_head_center(self.tracker,
214	- current_head_filtered).tolist()
215	-
216	- self.target_linear_out, self.target_arc = self.process_tracker.compute_arc_motion(current_robot_coordinates, head_center_coordinates,
217	- new_robot_coordinates)
218	- self.arc_motion_flag = True
219	- self.arc_motion_step_flag = const.ROBOT_MOTIONS["linear out"]
220	-
221	- if self.arc_motion_flag and self.arc_motion_step_flag == const.ROBOT_MOTIONS["linear out"]:
222	- coord = self.target_linear_out
223	- if np.allclose(np.array(actual_point), np.array(self.target_linear_out), 0, 1):
224	- self.arc_motion_step_flag = const.ROBOT_MOTIONS["arc"]
225	- coord = self.target_arc
226	-
227	- elif self.arc_motion_flag and self.arc_motion_step_flag == const.ROBOT_MOTIONS["arc"]:
228	- head_center_coordinates = self.process_tracker.estimate_head_center(self.tracker,
229	- current_head_filtered).tolist()
230	-
231	- _, new_target_arc = self.process_tracker.compute_arc_motion(current_robot_coordinates, head_center_coordinates,
232	- new_robot_coordinates)
233	- if np.allclose(np.array(new_target_arc[3:-1]), np.array(self.target_arc[3:-1]), 0, 1):
234	- None
235	- else:
236	- if self.process_tracker.correction_distance_calculation_target(new_robot_coordinates, current_robot_coordinates) >= \
237	- const.ROBOT_ARC_THRESHOLD_DISTANCE*0.8:
238	- self.target_arc = new_target_arc
239	-
240	- coord = self.target_arc
241	-
242	- if np.allclose(np.array(actual_point), np.array(self.target_arc[3:-1]), 0, 10):
243	- self.arc_motion_flag = False
244	- self.arc_motion_step_flag = const.ROBOT_MOTIONS["normal"]
245	- coord = new_robot_coordinates
246	-
247	- self.trck_init_robot.SendCoordinates(coord, self.arc_motion_step_flag)
248	- robot_status = True
249	- else:
250	- print("Head is too far from the robot basis")
251	- robot_status = False
252	-
253	- return robot_status
254	-
255	- def robot_control(self, current_tracker_coordinates_in_robot, current_robot_coordinates, markers_flag):
256	- coord_head_tracker_in_robot = current_tracker_coordinates_in_robot[1]
257	- marker_head_flag = markers_flag[1]
258	- coord_obj_tracker_in_robot = current_tracker_coordinates_in_robot[2]
259	- marker_obj_flag = markers_flag[2]
260	- robot_status = False
261	-
262	- if self.robot_tracker_flag:
263	- current_head = coord_head_tracker_in_robot
264	- if current_head is not None and marker_head_flag:
265	- current_head_filtered = self.process_tracker.kalman_filter(current_head)
266	- if self.process_tracker.compute_head_move_threshold(current_head_filtered):
267	- new_robot_coordinates = self.process_tracker.compute_head_move_compensation(current_head_filtered,
268	- self.m_change_robot_to_head)
269	- robot_status = True
270	- if self.coord_inv_old is None:
271	- self.coord_inv_old = new_robot_coordinates
272	-
273	- if np.allclose(np.array(new_robot_coordinates), np.array(current_robot_coordinates), 0, 0.01):
274	- #avoid small movements (0.01 mm)
275	- pass
276	- elif not np.allclose(np.array(new_robot_coordinates), np.array(self.coord_inv_old), 0, 5):
277	- #if the head moves (>5mm) before the robot reach the target
278	- self.trck_init_robot.StopRobot()
279	- self.coord_inv_old = new_robot_coordinates
280	- else:
281	- distance_target = self.process_tracker.correction_distance_calculation_target(new_robot_coordinates, current_robot_coordinates)
282	- robot_status = self.robot_move_decision(distance_target, new_robot_coordinates, current_robot_coordinates, current_head_filtered)
283	- self.coord_inv_old = new_robot_coordinates
284	- else:
285	- self.trck_init_robot.StopRobot()
286	-
287	- return robot_status
288	-
289	- def run(self):
290	- while not self.event_robot.is_set():
291	- current_tracker_coordinates_in_robot, current_robot_coordinates, markers_flag = self.get_coordinates_from_tracker_devices()
292	-
293	- if not self.robot_target_queue.empty():
294	- self.robot_tracker_flag, self.m_change_robot_to_head = self.robot_target_queue.get_nowait()
295	- self.robot_motion_reset()
296	- self.robot_target_queue.task_done()
297	-
298	- if not self.object_at_target_queue.empty():
299	- self.target_flag = self.object_at_target_queue.get_nowait()
300	- self.object_at_target_queue.task_done()
301	-
302	- robot_status = self.robot_control(current_tracker_coordinates_in_robot, current_robot_coordinates, markers_flag)
303	-
304	- if self.previous_robot_status != robot_status:
305	- wx.CallAfter(Publisher.sendMessage, 'Update robot status', robot_status=robot_status)
306	- self.previous_robot_status = robot_status
307	-
308	- sleep(const.SLEEP_ROBOT)
	@@ -79,5 +79,7 @@ class RemoteControl:		@@ -79,5 +79,7 @@ class RemoteControl:
79	})	79	})
80	except TypeError:	80	except TypeError:
81	pass	81	pass
		82	+ except socketio.exceptions.BadNamespaceError:
		83	+ pass
82		84
83	Publisher.add_sendMessage_hook(_emit)	85	Publisher.add_sendMessage_hook(_emit)