Download as
Browse Code »

Commit 06dd9de7a4cbbad4db7ccd989dec85fc73b7ff1a

Authored by Renan
1 parent e33eeb5d
Exists in master

ENH: code cleaning and standardization

Showing 8 changed files with 32 additions and 36 deletions   Show diff stats
invesalius/constants.py
  Diff comments   View file @06dd9de
... ... @@ -837,7 +837,7 @@ BAUD_RATES = [300, 1200, 2400, 4800, 9600, 19200, 38400, 57600, 115200]
837 837 BAUD_RATE_DEFAULT_SELECTION = 4
838 838  
839 839 #Robot
840   -ROBOT_ElFIN_IP = ['Select robot IP:', '143.107.220.251', '169.254.153.251', '127.0.0.1']
  840 +ROBOT_ElFIN_IP = ['143.107.220.251', '169.254.153.251', '127.0.0.1']
841 841 ROBOT_ElFIN_PORT = 10003
842 842 ROBOT_MOTIONS = {"normal": 0, "linear out": 1, "arc": 2}
843 843 ROBOT_HEAD_VELOCITY_THRESHOLD = 10
... ...
invesalius/data/bases.py
  Diff comments   View file @06dd9de
... ... @@ -245,7 +245,7 @@ def object_registration(fiducials, orients, coord_raw, m_change):
245 245  
246 246 return t_obj_raw, s0_raw, r_s0_raw, s0_dyn, m_obj_raw, r_obj_img
247 247  
248   -def compute_robot_target_matrix(raw_target_robot):
  248 +def compute_robot_to_head_matrix(raw_target_robot):
249 249 """
250 250 :param head: nx6 array of head coordinates from tracking device in robot space
251 251 :param robot: nx6 array of robot coordinates
... ... @@ -266,26 +266,26 @@ def compute_robot_target_matrix(raw_target_robot):
266 266 orientation=robot[3:],
267 267 axes='rzyx',
268 268 )
269   - target_robot_matrix = np.linalg.inv(m_head_target) @ m_robot_target
  269 + robot_to_head_matrix = np.linalg.inv(m_head_target) @ m_robot_target
270 270  
271   - return target_robot_matrix
  271 + return robot_to_head_matrix
272 272  
273 273  
274 274 class transform_tracker_to_robot(object):
275 275 M_tracker_to_robot = np.array([])
276 276 def transformation_tracker_to_robot(self, tracker_coord):
277 277 if not transform_tracker_to_robot.M_tracker_to_robot.any():
278   - #print("matrix tracker2robot is not define")
279 278 return None
280   - else:
281   - trans = tr.translation_matrix(tracker_coord[:3])
282   - a, b, g = np.radians(tracker_coord[3:6])
283   - rot = tr.euler_matrix(a, b, g, 'rzyx')
284   - M_tracker = tr.concatenate_matrices(trans, rot)
285   - M_tracker_in_robot = transform_tracker_to_robot.M_tracker_to_robot @ M_tracker
286 279  
287   - _, _, angles, translate, _ = tr.decompose_matrix(M_tracker_in_robot)
288   - tracker_in_robot = [translate[0], translate[1], translate[2], \
289   - np.degrees(angles[2]), np.degrees(angles[1]), np.degrees(angles[0])]
  280 + M_tracker = dco.coordinates_to_transformation_matrix(
  281 + position=tracker_coord[:3],
  282 + orientation= tracker_coord[3:6],
  283 + axes='rzyx',
  284 + )
  285 + M_tracker_in_robot = transform_tracker_to_robot.M_tracker_to_robot @ M_tracker
  286 +
  287 + angles_as_deg, translate = dco.transformation_matrix_to_coordinates(M_tracker_in_robot, axes='rxyz')
  288 + #TODO: check this with robot
  289 + tracker_in_robot = list(translate) + list(angles_as_deg)
290 290  
291   - return tracker_in_robot
  291 + return tracker_in_robot
... ...
invesalius/data/coordinates.py
  Diff comments   View file @06dd9de
... ... @@ -190,7 +190,7 @@ def ElfinCoord(trck_init):
190 190 else:
191 191 coord = np.array([0, 0, 0, 0, 0, 0])
192 192  
193   - return coord, None
  193 + return coord
194 194  
195 195 def CameraCoord(trck_init, trck_id, ref_mode):
196 196 trck = trck_init[0]
... ... @@ -486,7 +486,7 @@ def dynamic_reference_m(probe, reference):
486 486  
487 487 def RobotCoord(trk_init, trck_id, ref_mode):
488 488 coord_tracker, markers_flag = GetCoordinatesForThread(trk_init[0], trk_init[2], ref_mode)
489   - coord_robot, _ = ElfinCoord(trk_init[1:])
  489 + coord_robot = ElfinCoord(trk_init[1:])
490 490  
491 491 probe_tracker_in_robot = db.transform_tracker_to_robot().transformation_tracker_to_robot(coord_tracker[0])
492 492 ref_tracker_in_robot = db.transform_tracker_to_robot().transformation_tracker_to_robot(coord_tracker[1])
... ...
invesalius/data/coregistration.py
  Diff comments   View file @06dd9de
... ... @@ -217,14 +217,10 @@ class CoordinateCorregistrate(threading.Thread):
217 217 # print('CoordCoreg: event {}'.format(self.event.is_set()))
218 218 while not self.event.is_set():
219 219 try:
220   - if self.icp_queue.empty():
221   - None
222   - else:
  220 + if not self.icp_queue.empty():
223 221 self.icp, self.m_icp = self.icp_queue.get_nowait()
224 222  
225   - if self.object_at_target_queue.empty():
226   - None
227   - else:
  223 + if not self.object_at_target_queue.empty():
228 224 self.target_flag = self.object_at_target_queue.get_nowait()
229 225  
230 226 # print(f"Set the coordinate")
... ...
invesalius/data/elfin.py
  Diff comments   View file @06dd9de
... ... @@ -2,10 +2,10 @@
2 2  
3 3 import sys
4 4 from time import sleep
5   -from socket import socket, AF_INET, SOCK_DGRAM, SOCK_STREAM
  5 +from socket import socket, AF_INET, SOCK_STREAM
6 6 import invesalius.constants as const
7 7  
8   -class elfin_server():
  8 +class Elfin_Server():
9 9 def __init__(self, server_ip, port_number):
10 10 self.server_ip = server_ip
11 11 self.port_number = port_number
... ... @@ -13,20 +13,19 @@ class elfin_server():
13 13 def Initialize(self):
14 14 message_size = 1024
15 15 robot_id = 0
16   - self.cobot = elfin()
  16 + self.cobot = Elfin()
17 17 self.cobot.connect(self.server_ip, self.port_number, message_size, robot_id)
18   - print("conected!")
  18 + print("connected!")
19 19  
20 20 def Run(self):
21 21 return self.cobot.ReadPcsActualPos()
22 22  
23 23 def SendCoordinates(self, target, type=const.ROBOT_MOTIONS["normal"]):
24 24 status = self.cobot.ReadMoveState()
25   - if type == const.ROBOT_MOTIONS["normal"] or type == const.ROBOT_MOTIONS["linear out"]:
26   - if status != 1009:
  25 + if status != 1009:
  26 + if type == const.ROBOT_MOTIONS["normal"] or type == const.ROBOT_MOTIONS["linear out"]:
27 27 self.cobot.MoveL(target)
28   - elif type == const.ROBOT_MOTIONS["arc"]:
29   - if status != 1009:
  28 + elif type == const.ROBOT_MOTIONS["arc"]:
30 29 self.cobot.MoveC(target)
31 30  
32 31 def StopRobot(self):
... ... @@ -36,7 +35,7 @@ class elfin_server():
36 35 def Close(self):
37 36 self.cobot.close()
38 37  
39   -class elfin:
  38 +class Elfin:
40 39 def __init__(self):
41 40 self.end_msg = ",;"
42 41  
... ...
invesalius/data/trackers.py
  Diff comments   View file @06dd9de
... ... @@ -224,7 +224,7 @@ def ElfinRobot(robot_IP):
224 224 try:
225 225 import invesalius.data.elfin as elfin
226 226 print("Trying to connect Robot via: ", robot_IP)
227   - trck_init = elfin.elfin_server(robot_IP, const.ROBOT_ElFIN_PORT)
  227 + trck_init = elfin.Elfin_Server(robot_IP, const.ROBOT_ElFIN_PORT)
228 228 trck_init.Initialize()
229 229 lib_mode = 'wrapper'
230 230 print('Connect to elfin robot tracking device.')
... ...
invesalius/gui/dialogs.py
  Diff comments   View file @06dd9de
... ... @@ -4383,8 +4383,9 @@ class SetRobotIP(wx.Dialog):
4383 4383 def _init_gui(self):
4384 4384 # ComboBox for spatial tracker device selection
4385 4385 tooltip = wx.ToolTip(_("Choose or type the robot IP"))
  4386 + robot_ip_options = [_("Select robot IP:")] + const.ROBOT_ElFIN_IP
4386 4387 choice_IP = wx.ComboBox(self, -1, "",
4387   - choices=const.ROBOT_ElFIN_IP, style=wx.CB_DROPDOWN | wx.TE_PROCESS_ENTER)
  4388 + choices=robot_ip_options, style=wx.CB_DROPDOWN | wx.TE_PROCESS_ENTER)
4388 4389 choice_IP.SetToolTip(tooltip)
4389 4390 choice_IP.SetSelection(const.DEFAULT_TRACKER)
4390 4391 choice_IP.Bind(wx.EVT_COMBOBOX, partial(self.OnChoiceIP, ctrl=choice_IP))
... ... @@ -4591,8 +4592,8 @@ class CreateTransformationMatrixRobot(wx.Dialog):
4591 4592  
4592 4593 M_robot_2_tracker = self.affine_correg(tracker_coord, robot_coord)
4593 4594 M_tracker_2_robot = tr.inverse_matrix(M_robot_2_tracker)
4594   - #db.transform_tracker_2_robot.M_tracker_2_robot = M_tracker_2_robot
4595 4595 self.M_tracker_2_robot = M_tracker_2_robot
  4596 +
4596 4597 self.btn_save.Enable(True)
4597 4598 self.btn_ok.Enable(True)
4598 4599  
... ...
invesalius/gui/task_navigator.py
  Diff comments   View file @06dd9de
... ... @@ -1641,7 +1641,7 @@ class MarkersPanel(wx.Panel):
1641 1641 new_marker.session_id = session_id or self.current_session
1642 1642  
1643 1643 if self.tracker.tracker_id == const.ROBOT and self.nav_status:
1644   - self.current_robot_target_matrix = db.compute_robot_target_matrix(self.raw_target_robot)
  1644 + self.current_robot_target_matrix = db.compute_robot_to_head_matrix(self.raw_target_robot)
1645 1645 else:
1646 1646 self.current_robot_target_matrix = [None] * 9
1647 1647  
... ...