From 06dd9de7a4cbbad4db7ccd989dec85fc73b7ff1a Mon Sep 17 00:00:00 2001 From: Renan Date: Thu, 14 Oct 2021 09:57:08 +0300 Subject: [PATCH] ENH: code cleaning and standardization --- invesalius/constants.py | 2 +- invesalius/data/bases.py | 28 ++++++++++++++-------------- invesalius/data/coordinates.py | 4 ++-- invesalius/data/coregistration.py | 8 ++------ invesalius/data/elfin.py | 17 ++++++++--------- invesalius/data/trackers.py | 2 +- invesalius/gui/dialogs.py | 5 +++-- invesalius/gui/task_navigator.py | 2 +- 8 files changed, 32 insertions(+), 36 deletions(-) diff --git a/invesalius/constants.py b/invesalius/constants.py index d519aa6..6b55d68 100644 --- a/invesalius/constants.py +++ b/invesalius/constants.py @@ -837,7 +837,7 @@ BAUD_RATES = [300, 1200, 2400, 4800, 9600, 19200, 38400, 57600, 115200] BAUD_RATE_DEFAULT_SELECTION = 4 #Robot -ROBOT_ElFIN_IP = ['Select robot IP:', '143.107.220.251', '169.254.153.251', '127.0.0.1'] +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 diff --git a/invesalius/data/bases.py b/invesalius/data/bases.py index 91e8f4a..f212328 100644 --- a/invesalius/data/bases.py +++ b/invesalius/data/bases.py @@ -245,7 +245,7 @@ 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_target_matrix(raw_target_robot): +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 @@ -266,26 +266,26 @@ def compute_robot_target_matrix(raw_target_robot): orientation=robot[3:], axes='rzyx', ) - target_robot_matrix = np.linalg.inv(m_head_target) @ m_robot_target + robot_to_head_matrix = np.linalg.inv(m_head_target) @ m_robot_target - return target_robot_matrix + 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(): - #print("matrix tracker2robot is not define") return None - else: - trans = tr.translation_matrix(tracker_coord[:3]) - a, b, g = np.radians(tracker_coord[3:6]) - rot = tr.euler_matrix(a, b, g, 'rzyx') - M_tracker = tr.concatenate_matrices(trans, rot) - M_tracker_in_robot = transform_tracker_to_robot.M_tracker_to_robot @ M_tracker - _, _, angles, translate, _ = tr.decompose_matrix(M_tracker_in_robot) - tracker_in_robot = [translate[0], translate[1], translate[2], \ - np.degrees(angles[2]), np.degrees(angles[1]), np.degrees(angles[0])] + M_tracker = dco.coordinates_to_transformation_matrix( + position=tracker_coord[:3], + orientation= tracker_coord[3:6], + axes='rzyx', + ) + M_tracker_in_robot = transform_tracker_to_robot.M_tracker_to_robot @ M_tracker + + angles_as_deg, translate = dco.transformation_matrix_to_coordinates(M_tracker_in_robot, axes='rxyz') + #TODO: check this with robot + tracker_in_robot = list(translate) + list(angles_as_deg) - return tracker_in_robot + return tracker_in_robot diff --git a/invesalius/data/coordinates.py b/invesalius/data/coordinates.py index e44b2c6..71d2aeb 100644 --- a/invesalius/data/coordinates.py +++ b/invesalius/data/coordinates.py @@ -190,7 +190,7 @@ def ElfinCoord(trck_init): else: coord = np.array([0, 0, 0, 0, 0, 0]) - return coord, None + return coord def CameraCoord(trck_init, trck_id, ref_mode): trck = trck_init[0] @@ -486,7 +486,7 @@ def dynamic_reference_m(probe, reference): def RobotCoord(trk_init, trck_id, ref_mode): coord_tracker, markers_flag = GetCoordinatesForThread(trk_init[0], trk_init[2], ref_mode) - coord_robot, _ = ElfinCoord(trk_init[1:]) + coord_robot = ElfinCoord(trk_init[1:]) probe_tracker_in_robot = db.transform_tracker_to_robot().transformation_tracker_to_robot(coord_tracker[0]) ref_tracker_in_robot = db.transform_tracker_to_robot().transformation_tracker_to_robot(coord_tracker[1]) diff --git a/invesalius/data/coregistration.py b/invesalius/data/coregistration.py index 4dfcbb6..b9948d0 100644 --- a/invesalius/data/coregistration.py +++ b/invesalius/data/coregistration.py @@ -217,14 +217,10 @@ class CoordinateCorregistrate(threading.Thread): # print('CoordCoreg: event {}'.format(self.event.is_set())) while not self.event.is_set(): try: - if self.icp_queue.empty(): - None - else: + if not self.icp_queue.empty(): self.icp, self.m_icp = self.icp_queue.get_nowait() - if self.object_at_target_queue.empty(): - None - else: + if not self.object_at_target_queue.empty(): self.target_flag = self.object_at_target_queue.get_nowait() # print(f"Set the coordinate") diff --git a/invesalius/data/elfin.py b/invesalius/data/elfin.py index ad0f3e5..da4b135 100644 --- a/invesalius/data/elfin.py +++ b/invesalius/data/elfin.py @@ -2,10 +2,10 @@ import sys from time import sleep -from socket import socket, AF_INET, SOCK_DGRAM, SOCK_STREAM +from socket import socket, AF_INET, SOCK_STREAM import invesalius.constants as const -class elfin_server(): +class Elfin_Server(): def __init__(self, server_ip, port_number): self.server_ip = server_ip self.port_number = port_number @@ -13,20 +13,19 @@ class elfin_server(): def Initialize(self): message_size = 1024 robot_id = 0 - self.cobot = elfin() + self.cobot = Elfin() self.cobot.connect(self.server_ip, self.port_number, message_size, robot_id) - print("conected!") + print("connected!") def Run(self): return self.cobot.ReadPcsActualPos() def SendCoordinates(self, target, type=const.ROBOT_MOTIONS["normal"]): status = self.cobot.ReadMoveState() - if type == const.ROBOT_MOTIONS["normal"] or type == const.ROBOT_MOTIONS["linear out"]: - if status != 1009: + if status != 1009: + if type == const.ROBOT_MOTIONS["normal"] or type == const.ROBOT_MOTIONS["linear out"]: self.cobot.MoveL(target) - elif type == const.ROBOT_MOTIONS["arc"]: - if status != 1009: + elif type == const.ROBOT_MOTIONS["arc"]: self.cobot.MoveC(target) def StopRobot(self): @@ -36,7 +35,7 @@ class elfin_server(): def Close(self): self.cobot.close() -class elfin: +class Elfin: def __init__(self): self.end_msg = ",;" diff --git a/invesalius/data/trackers.py b/invesalius/data/trackers.py index 86eefd5..f0a1688 100644 --- a/invesalius/data/trackers.py +++ b/invesalius/data/trackers.py @@ -224,7 +224,7 @@ def ElfinRobot(robot_IP): 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 = elfin.Elfin_Server(robot_IP, const.ROBOT_ElFIN_PORT) trck_init.Initialize() lib_mode = 'wrapper' print('Connect to elfin robot tracking device.') diff --git a/invesalius/gui/dialogs.py b/invesalius/gui/dialogs.py index 685394d..2e373f0 100644 --- a/invesalius/gui/dialogs.py +++ b/invesalius/gui/dialogs.py @@ -4383,8 +4383,9 @@ class SetRobotIP(wx.Dialog): def _init_gui(self): # ComboBox for spatial tracker device selection tooltip = wx.ToolTip(_("Choose or type the robot IP")) + robot_ip_options = [_("Select robot IP:")] + const.ROBOT_ElFIN_IP choice_IP = wx.ComboBox(self, -1, "", - choices=const.ROBOT_ElFIN_IP, style=wx.CB_DROPDOWN | wx.TE_PROCESS_ENTER) + choices=robot_ip_options, style=wx.CB_DROPDOWN | wx.TE_PROCESS_ENTER) choice_IP.SetToolTip(tooltip) choice_IP.SetSelection(const.DEFAULT_TRACKER) choice_IP.Bind(wx.EVT_COMBOBOX, partial(self.OnChoiceIP, ctrl=choice_IP)) @@ -4591,8 +4592,8 @@ class CreateTransformationMatrixRobot(wx.Dialog): M_robot_2_tracker = self.affine_correg(tracker_coord, robot_coord) M_tracker_2_robot = tr.inverse_matrix(M_robot_2_tracker) - #db.transform_tracker_2_robot.M_tracker_2_robot = M_tracker_2_robot self.M_tracker_2_robot = M_tracker_2_robot + self.btn_save.Enable(True) self.btn_ok.Enable(True) diff --git a/invesalius/gui/task_navigator.py b/invesalius/gui/task_navigator.py index 0ac13a3..28a686b 100644 --- a/invesalius/gui/task_navigator.py +++ b/invesalius/gui/task_navigator.py @@ -1641,7 +1641,7 @@ 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_target_matrix(self.raw_target_robot) + self.current_robot_target_matrix = db.compute_robot_to_head_matrix(self.raw_target_robot) else: self.current_robot_target_matrix = [None] * 9 -- libgit2 0.21.2