Support to multiple robot model

.gitignore

@@ -132,3 +132,7 @@ dmypy.json
 
 # Pyre type checker
 .pyre/
+
+*.patch
+
+tmp

display.py

@@ -0,0 +1,64 @@
+import os
+import numpy as np
+import matplotlib
+import matplotlib.pyplot as plt
+import atexit
+
+from matplotlib.patches import FancyBboxPatch
+import matplotlib.animation as animation
+import robot.constants as const
+
+matplotlib.use('TkAgg')
+
+def delete_file(file):
+    if os.path.exists(file):
+      os.remove(file)
+    else:
+      print(file)
+
+
+class PointOfApp:
+
+    def __init__(self):
+        # create plots for point of application
+        self.fig = plt.figure()
+        self.ax = self.fig.add_subplot(1, 1, 1)
+        self.square = FancyBboxPatch((-5, -5), 10, 10, alpha=0.5, boxstyle="round,pad=3")
+        self.ax.add_patch(self.square) 
+        self.ax.set_xlim(-12, 12)
+        self.ax.set_ylim(-12, 12)
+        self.point, = self.ax.plot(0, 0, 'ro', markersize=10)
+
+        ani = animation.FuncAnimation(self.fig, self.animate, fargs=(), interval=1)
+        plt.show()
+
+        atexit.register(delete_file, const.TEMP_FILE)
+
+    def animate(self, i):
+        '''
+        Loop to read live sensor data and perform relevant operations. 
+
+        '''
+        if os.path.exists(const.TEMP_FILE) and os.path.getsize(const.TEMP_FILE) > 0:
+            with open(const.TEMP_FILE, 'rb') as file:
+                try:  # catch OSError in case of a one line file 
+                    file.seek(-2, os.SEEK_END)
+                    while file.read(1) != b'\n':
+                        file.seek(-2, os.SEEK_CUR)
+                except OSError:
+                    file.seek(0)
+                data = file.readline().decode()
+
+            data = data[data.find("[")+1:data.find("]")]
+            data  = np.array(data.split(", "), dtype = 'float64')
+
+            self.point.set_data(data[0], data[1])
+
+            # Redraw the plot
+            self.fig.canvas.draw()
+        else:
+            pass
+
+if __name__ == '__main__' :
+    PointOfApp()
+

environment.yml

@@ -9,4 +9,5 @@ dependencies:
 - websockets
 - numpy
 - opencv
-- wxPython
+- wxPython
+- pynput

main_loop.py

@@ -107,5 +107,4 @@ def reset_robot(data):
                 rc.send_message(topic, data)
                 previous_robot_status = robot_status
 
-        #time.sleep(const.SLEEP_ROBOT)
-
+        time.sleep(const.SLEEP_ROBOT)

robot/constants.py

@@ -1,10 +1,16 @@
-#Robot
+# Please set up the site or coil, choose the robot model, and decide if you want to use force sensing
 SITE = "usp_coil"
+ROBOT = "dobot" #elfin, dobot, or ur
+FORCE_TORQUE_SENSOR = True
+DISPLAY_POA = True
+
+TEMP_FILE = 'tmp'
+
 # Define the required offset to align the coil and robot end effector
 if SITE == "usp_coil":
     ROBOT_RX_OFFSET = 0
     ROBOT_RY_OFFSET = 0
-    ROBOT_RZ_OFFSET = -90
+    ROBOT_RZ_OFFSET = 0
 elif SITE == "usp_neurosoft":
     ROBOT_RX_OFFSET = -90
     ROBOT_RY_OFFSET = 0
@@ -13,37 +19,78 @@
     ROBOT_RX_OFFSET = 0
     ROBOT_RY_OFFSET = 0
     ROBOT_RZ_OFFSET = 157.5
+elif SITE == "tubingen":
+    ROBOT_RX_OFFSET = 0
+    ROBOT_RY_OFFSET = 0
+    ROBOT_RZ_OFFSET = 157.5
 else:
     ROBOT_RX_OFFSET = 0
     ROBOT_RY_OFFSET = 0
     ROBOT_RZ_OFFSET = 0
 
-SLEEP_ROBOT = 0.01
-
-ROBOT_ElFIN_PORT = 10003
-
-ROBOT_MOTIONS = {"normal": 0, "linear out": 1, "arc": 2, 'force linear out': 3}
-ROBOT_HEAD_VELOCITY_THRESHOLD = 20 #mm/s
-ROBOT_ARC_THRESHOLD_DISTANCE = 80 #mm
-ROBOT_ARC_THRESHOLD_DISTANCE_ANGLE = 30 #°
-ROBOT_THRESHOLD_TO_UPDATE_TARGET_ARC = 40 #mm
-ROBOT_VERSOR_SCALE_FACTOR = 50
-ROBOT_TRANSFORMATION_MATRIX_THRESHOLD = 1 #mm
-ROBOT_FORCE_SENSOR_THRESHOLD = 10 #N
-ROBOT_FORCE_SENSOR_SCALE_THRESHOLD = 30 #% of init force
-ROBOT_TARGET_TUNING_THRESHOLD_DISTANCE = 30
-ROBOT_TARGET_TUNING_THRESHOLD_ANGLE = 5
+if ROBOT == "dobot":
+    # Robot settings
+    ROBOT_WORKING_SPACE = 1500  # mm
+    ROBOT_DOBOT_DASHBOARD_PORT = 29999
+    ROBOT_DOBOT_MOVE_PORT = 30003
+    ROBOT_DOBOT_FEED_PORT = 30004
+    ROBOT_DOBOT_TOOL_ID = 0
+    ROBOT_DOBOT_TIMEOUT_START_MOTION = 10
+    ROBOT_DOBOT_TIMEOUT_MOTION = 45
+    SLEEP_ROBOT = 0.005
+    # Filtering head motion
+    ROBOT_HEAD_VELOCITY_THRESHOLD = 30 #mm/s
+    # Tunning motion
+    ROBOT_TARGET_TUNING_THRESHOLD_DISTANCE = 50
+    ROBOT_TARGET_TUNING_THRESHOLD_ANGLE = 15
+    # Arc motion
+    ROBOT_ARC_THRESHOLD_DISTANCE = 100 #mm
+    ROBOT_ARC_THRESHOLD_DISTANCE_ANGLE = 45 #°
+    ROBOT_ARC_BEZIER_CURVE_STEP = 0.4 # step size between 0 and 1. 0.1 generates 10 points; 0.01 generates 100 points along the curve path
+    ROBOT_THRESHOLD_TO_UPDATE_TARGET_ARC = 40 #mm
+    ROBOT_VERSOR_SCALE_FACTOR = 50
+    ROBOT_MIDDLE_ARC_SCALE_FACTOR = 4
+    # Check robot tracker registration
+    ROBOT_TRANSFORMATION_MATRIX_THRESHOLD = 1 #mm
+    # Force and torque sensor
+    ROBOT_FORCE_SENSOR_THRESHOLD = 10 #N
+    ROBOT_FORCE_SENSOR_SCALE_THRESHOLD = 100 #% of init force
+    # Robot States
+    ROBOT_DOBOT_MOVE_STATE = {"error": 9}
+elif ROBOT == "elfin":
+    # Robot settings
+    # Robot Working Space is defined as 800mm in Elfin 5 manual. For safety, the value is reduced by 5%.
+    # For debug, feel free to use 1000 mm
+    ROBOT_WORKING_SPACE = 1000 #mm
+    ROBOT_ElFIN_PORT = 10003
+    SLEEP_ROBOT = 0.0
+    # Filtering head motion
+    ROBOT_HEAD_VELOCITY_THRESHOLD = 20  # mm/s
+    # Tunning motion
+    ROBOT_TARGET_TUNING_THRESHOLD_DISTANCE = 30
+    ROBOT_TARGET_TUNING_THRESHOLD_ANGLE = 5
+    # Arc motion
+    ROBOT_ARC_THRESHOLD_DISTANCE = 80  # mm
+    ROBOT_ARC_THRESHOLD_DISTANCE_ANGLE = 30  # °
+    ROBOT_MIDDLE_ARC_SCALE_FACTOR = 1.5
+    ROBOT_THRESHOLD_TO_UPDATE_TARGET_ARC = 40  # mm
+    ROBOT_VERSOR_SCALE_FACTOR = 50
+    # Check robot tracker registration
+    ROBOT_TRANSFORMATION_MATRIX_THRESHOLD = 1  # mm
+    # Force and torque sensor
+    ROBOT_FORCE_SENSOR_THRESHOLD = 10  # N
+    ROBOT_FORCE_SENSOR_SCALE_THRESHOLD = 30  # % of init force
+    # Robot States
+    ROBOT_ELFIN_MOVE_STATE = {"free to move": 0,
+                              "in motion": 1009,
+                              "waiting for execution": 1013,
+                              "error": 1025}
 
-FORCE_TORQUE_SENSOR = False
+# Please, do not change the following constants.
 
-#Robot Working Space is defined as 800mm in Elfin manual. For safety, the value is reduced by 5%.
-ROBOT_WORKING_SPACE = 1000 #mm
-ROBOT_MOVE_STATE = {"free to move": 0,
-                    "in motion": 1009,
-                    "waiting for execution": 1013,
-                    "error": 1025}
+ROBOT_MOTIONS = {"normal": 0, "linear out": 1, "arc": 2, 'force linear out': 3, 'tunning': 4}
 
-#Publisher messages
+# Publisher messages from invesalius
 PUB_MESSAGES = ['Connect to robot',
                 'Robot navigation mode',
                 'Update robot target',

robot/control/coordinates.py

@@ -1,8 +1,5 @@
-
 import numpy as np
 
-import robot.control.elfin_processing as elfin_process
-
 
 class RobotCoordinates:
     """

robot/control/ft.py

@@ -0,0 +1,59 @@
+import numpy as np
+import matplotlib.pyplot as plt
+from scipy.optimize import minimize
+
+
+# Rotation and Translation matrices to find pos on mTMS head
+R = np.load('robot/resources/Rot.npy')
+T = np.load('robot/resources/Tr.npy')
+# Set tool origin and size bounds
+# ..... based on size of mTMS
+
+ORIG = np.array([0.0, 0.0, 0.06])
+BOUNDS_X = (-0.08, 0.08)
+BOUNDS_Y = (-0.08, 0.08)
+BOUNDS_Z = (0, 0.06)
+
+def _func(r, F, M, orig):
+    '''
+    Function to minimise objective function
+
+    Returns: Norm of r x F - M (Np Array)  
+    '''
+    # Check if r is outside the box
+    return np.linalg.norm(np.cross(r - orig, F) - M)
+
+
+def find_r(F, M):
+    '''
+    Find point of application of force
+
+    Returns: transformed point of application (list)
+    '''
+
+    # initial guess
+    r0 = np.array([0.0, 0.0, 0.06])
+
+    # find r that minimizes the objective function
+    res = minimize(_func, r0, 
+                   args=(F, M, ORIG), 
+                   method='Nelder-Mead', 
+                   bounds=(BOUNDS_X, BOUNDS_Y, BOUNDS_Z))
+
+    r_min = res.x * 100  # multiply by 100 to get value in cm
+    r_tran = R @ r_min + T
+
+    # conditions to identify p.o.a
+    if not (-8 <= r_tran[0] <= 8 and
+            -8 <= r_tran[1] <= 8):
+        r_tran[0], r_tran[1] = 0, 0
+
+    else:
+        if F[2] < 1:
+            #print(r_tran)
+            pass
+        else:
+            r_tran[0], r_tran[1] = 0, 0
+
+    return [round(r_tran[i], 1) for i in range(0, len(r_tran))]
+