Implemented MTM IK + Cleaned up test IK logic for both PSM and MTM

ambf_controller/dvrk/scripts/dh.py

@@ -0,0 +1,50 @@
+import numpy as np
+
+
+class DH:
+    def __init__(self, alpha, a, theta, d, offset, joint_type, convention='STANDARD'):
+        self.alpha = alpha
+        self.a = a
+        self.theta = theta
+        self.d = d
+        self.offset = offset
+        self.joint_type = joint_type
+        if convention in ['STANDARD', 'MODIFIED']:
+            self.convention = convention
+        else:
+            raise ('ERROR, DH CONVENTION NOT UNDERSTOOD')
+
+    def mat_from_dh(self, alpha, a, theta, d, offset):
+        ca = np.cos(alpha)
+        sa = np.sin(alpha)
+        if self.joint_type == 'R':
+            theta = theta + offset
+        elif self.joint_type == 'P':
+            d = d + offset
+        else:
+            assert type == 'P' and type == 'R'
+
+        ct = np.cos(theta)
+        st = np.sin(theta)
+
+        if self.convention == 'STANDARD':
+            mat = np.mat([
+                [ct, -st * ca,  st * sa,  a * ct],
+                [st,  ct * ca, -ct * sa,  a * st],
+                [0,        sa,       ca,       d],
+                [0,         0,        0,       1]
+            ])
+        elif self.convention == 'MODIFIED':
+            mat = np.mat([
+                [ct, -st, 0, a],
+                [st * ca, ct * ca, -sa, -d * sa],
+                [st * sa, ct * sa, ca, d * ca],
+                [0, 0, 0, 1]
+            ])
+        else:
+            raise ('ERROR, DH CONVENTION NOT UNDERSTOOD')
+
+        return mat
+
+    def get_trans(self):
+        return self.mat_from_dh(self.alpha, self.a, self.theta, self.d, self.offset)

ambf_controller/dvrk/scripts/mtmFK.py

@@ -0,0 +1,83 @@
+import numpy as np
+from utilities import *
+from dh import DH
+
+# THIS IS THE FK FOR THE PSM MOUNTED WITH THE LARGE NEEDLE DRIVER TOOL. THIS IS THE
+# SAME KINEMATIC CONFIGURATION FOUND IN THE DVRK MANUAL. NOTE, JUST LIKE A FAULT IN THE
+# MTM's DH PARAMETERS IN THE MANUAL, THERE IS A FAULT IN THE PSM's DH AS WELL. BASED ON
+# THE FRAME ATTACHMENT IN THE DVRK MANUAL THE CORRECT DH CAN FOUND IN THIS FILE
+
+# ALSO, NOTICE THAT AT HOME CONFIGURATION THE TIP OF THE PSM HAS THE FOLLOWING
+# ROTATION OFFSET W.R.T THE BASE. THIS IS IMPORTANT FOR IK PURPOSES.
+# R_7_0 = [ 0,  1,  0 ]
+#       = [ 1,  0,  0 ]
+#       = [ 0,  0, -1 ]
+# Basically, x_7 is along y_0, y_7 is along x_0 and z_7 is along -z_0.
+
+# You need to provide a list of joint positions. If the list is less that the number of joint
+# i.e. the robot has 6 joints, but only provide 3 joints. The FK till the 3+1 link will be provided
+def compute_FK(joint_pos):
+    j = [0, 0, 0, 0, 0, 0, 0]
+    for i in range(len(joint_pos)):
+        j[i] = joint_pos[i]
+
+    # The last frame is fixed
+
+    L_arm = 0.278828
+    L_forearm = 0.363867
+    L_h = 0.147733
+
+    # PSM DH Params
+    link1 = DH(alpha=PI_2, a=0, theta=j[0], d=0, offset=-PI_2, joint_type='R', convention='STANDARD')
+    link2 = DH(alpha=0, a=L_arm, theta=j[1], d=0, offset=-PI_2, joint_type='R', convention='STANDARD')
+    link3 = DH(alpha=-PI_2, a=L_forearm, theta=j[2], d=0, offset=PI_2, joint_type='R', convention='STANDARD')
+    link4 = DH(alpha=PI_2, a=0, theta=j[3], d=L_h, offset=0, joint_type='R', convention='STANDARD')
+    link5 = DH(alpha=-PI_2, a=0, theta=j[4], d=0, offset=0, joint_type='R', convention='STANDARD')
+    link6 = DH(alpha=PI_2, a=0, theta=j[5], d=0, offset=-PI_2, joint_type='R', convention='STANDARD')
+    link7 = DH(alpha=0, a=0, theta=j[6], d=0, offset=PI_2, joint_type='R', convention='STANDARD')
+
+    T_1_0 = link1.get_trans()
+    T_2_1 = link2.get_trans()
+    T_3_2 = link3.get_trans()
+    T_4_3 = link4.get_trans()
+    T_5_4 = link5.get_trans()
+    T_6_5 = link6.get_trans()
+    T_7_6 = link7.get_trans()
+
+    T_2_0 = np.matmul(T_1_0, T_2_1)
+    T_3_0 = np.matmul(T_2_0, T_3_2)
+    T_4_0 = np.matmul(T_3_0, T_4_3)
+    T_5_0 = np.matmul(T_4_0, T_5_4)
+    T_6_0 = np.matmul(T_5_0, T_6_5)
+    T_7_0 = np.matmul(T_6_0, T_7_6)
+
+    # print("RETURNING FK FOR LINK ", len(joint_pos))
+
+    if len(joint_pos) == 1:
+        return T_1_0
+
+    elif len(joint_pos) == 2:
+        return T_2_0
+
+    elif len(joint_pos) == 3:
+        return T_3_0
+
+    elif len(joint_pos) == 4:
+        return T_4_0
+
+    elif len(joint_pos) == 5:
+        return T_5_0
+
+    elif len(joint_pos) == 6:
+        return T_6_0
+
+    elif len(joint_pos) == 7:
+        return T_7_0
+
+
+# T_7_0 = compute_FK([-0.5, 0, 0.2, 0, 0, 0])
+#
+# print T_7_0
+# print "\n AFTER ROUNDING \n"
+# print(round_mat(T_7_0, 4, 4, 3))
+

ambf_controller/dvrk/scripts/mtmIK.py

@@ -0,0 +1,133 @@
+from PyKDL import Vector, Rotation, Frame, dot
+import numpy as np
+import math
+from mtmFK import *
+import rospy
+
+# THIS IS THE IK FOR THE PSM MOUNTED WITH THE LARGE NEEDLE DRIVER TOOL. THIS IS THE
+# SAME KINEMATIC CONFIGURATION FOUND IN THE DVRK MANUAL. NOTE, JUST LIKE A FAULT IN THE
+# MTM's DH PARAMETERS IN THE MANUAL, THERE IS A FAULT IN THE PSM's DH AS WELL. CHECK THE FK
+# FILE TO FIND THE CORRECT DH PARAMS BASED ON THE FRAME ATTACHMENT IN THE DVRK MANUAL
+
+# ALSO, NOTICE THAT AT HOME CONFIGURATION THE TIP OF THE PSM HAS THE FOLLOWING
+# ROTATION OFFSET W.R.T THE BASE. THIS IS IMPORTANT FOR IK PURPOSES.
+# R_7_0 = [ 0,  1,  0 ]
+#       = [ 1,  0,  0 ]
+#       = [ 0,  0, -1 ]
+# Basically, x_7 is along y_0, y_7 is along x_0 and z_7 is along -z_0.
+
+# Read the frames, positions and rotation as follows, T_A_B, means that this
+# is a Transfrom of frame A with respect to frame B. Similarly P_A_B is the
+# Position Vector of frame A's origin with respect to frame B's origin. And finally
+# R_A_B is the rotation matrix representing the orientation of frame B with respect to
+# frame A.
+
+# If you are confused by the above description, consider the equations below to make sense of it
+# all. Suppose we have three frames A, B and C. The following equations will give you the L.H.S
+
+# 1) T_C_A = T_B_A * T_C_B
+# 2) R_A_C = inv(R_B_A * R_C_B)
+# 3) P_C_A = R_B_A * R_C_B * P_C
+
+# For Postions, the missing second underscore separated quantity means that it is expressed in local
+# coodinates. Rotations, and Transforms are always to defined a frame w.r.t to some
+# other frame so this is a special case for only positions. Consider the example
+
+# P_B indiciates a point expressed in B frame.
+
+# Now there are two special cases that are identified by letter D and N. The first characeter D indiciates a
+# difference (vector) of between two points, specified by the first and second underscore separater (_) strings,
+# expressed in the third underscore separated reference. I.e.
+
+# D_A_B_C
+# This means the difference between Point A and  B expressed in C. On the other hand the letter N indicates
+# the direction, and not specifically the actually
+# measurement. So:
+#
+# N_A_B_C
+#
+# is the direction between the difference of A and B expressed in C.
+
+T_PalmJoint_0 = None
+
+
+def enforce_limits(j_raw):
+    # Min to Max Limits
+    num_joints = 7
+    j1_lims = [np.deg2rad(-75), np.deg2rad(44.98)]
+    j2_lims = [np.deg2rad(-44.98), np.deg2rad(44.98)]
+    j3_lims = [np.deg2rad(-44.98), np.deg2rad(44.98)]
+    j4_lims = [np.deg2rad(-59), np.deg2rad(245)]
+    j5_lims = [np.deg2rad(-90), np.deg2rad(180)]
+    j6_lims = [np.deg2rad(-44.98), np.deg2rad(44.98)]
+    j7_lims = [np.deg2rad(-175), np.deg2rad(175)]
+
+    j_limited = [0.0]*num_joints
+    j_lims = [j1_lims, j2_lims, j3_lims, j4_lims, j5_lims, j6_lims, j7_lims]
+
+    for idx in range(num_joints):
+        min_lim = j_lims[idx][0]
+        max_lim = j_lims[idx][1]
+        j_limited[idx] = max(min_lim, min(j_raw[idx], max_lim))
+
+    return [j_limited[0], j_limited[1], j_limited[2], j_limited[3], j_limited[4], j_limited[5], j_limited[6]]
+
+
+def get_T_PalmJoint_0():
+    global T_PalmJoint_0
+    return T_PalmJoint_0
+
+
+def compute_IK(T_7_0):
+    global T_PalmJoint_0
+    L_arm = 0.278828
+    L_forearm = 0.363867
+    L_h = 0.147733
+
+    j1 = math.atan2(T_7_0.p[0], -T_7_0.p[1])
+
+    l1 = L_arm
+    l2 = math.sqrt(L_forearm ** 2 + L_h ** 2)
+    angle_offset = math.asin(L_h/l2)
+
+    x = -T_7_0.p[2]
+    y = math.sqrt(T_7_0.p[0] ** 2 + T_7_0.p[1] ** 2)
+
+    d = math.sqrt(x ** 2 + y ** 2)
+    a1 = math.atan2(y, x)
+    a2 = math.acos((l1 ** 2 - l2 ** 2 + d ** 2) / (2.0 * l1 * d))
+    q2 = -math.acos((l1 ** 2 + l2 ** 2 - d ** 2) / (2.0 * l1 * l2))
+
+    j2 = a1 - a2
+    j3 = q2 - angle_offset + PI_2
+
+    T_7_0_FK = convert_mat_to_frame(compute_FK([j1, j2, j3, 0, 0, 0, 0]))
+
+    R_IK_in_FK = T_7_0_FK.M.Inverse() * T_7_0.M
+
+    # https://www.geometrictools.com/Documentation/EulerAngles.pdf
+    r = R_IK_in_FK
+
+    theta_y = 0
+    theta_x = 0
+    theta_z = 0
+    if r[0, 2] < 1.0:
+        if r[0, 2] > -1.0:
+            theta_y = math.asin(r[0, 2])
+            theta_x = math.atan2(-r[1, 2], r[2, 2])
+            theta_z = math.atan2(-r[0, 1], r[0, 0])
+        else:
+            theta_y = -PI_2
+            theta_x = math.atan2(r[1, 0], r[1, 1])
+            theta_z = 0
+    else:
+        theta_y = PI_2
+        theta_x = math.atan2(r[1, 0], r[1, 1])
+        theta_z = 0
+
+    j7 = theta_z
+    j5 = -theta_y
+    j4 = theta_x
+    j6 = 0
+
+    return [j1, j2, j3, j4, j5, j6, j7]

ambf_controller/dvrk/scripts/psmFK.py

@@ -1,6 +1,6 @@
 import numpy as np
 from utilities import *
-
+from dh import DH
 
 # THIS IS THE FK FOR THE PSM MOUNTED WITH THE LARGE NEEDLE DRIVER TOOL. THIS IS THE
 # SAME KINEMATIC CONFIGURATION FOUND IN THE DVRK MANUAL. NOTE, JUST LIKE A FAULT IN THE
@@ -24,13 +24,13 @@ def compute_FK(joint_pos):
     # The last frame is fixed
 
     # PSM DH Params
-    link1 = DH(alpha=PI_2, a=0, theta=j[0], d=0, offset=PI_2, joint_type='R')
-    link2 = DH(alpha=-PI_2, a=0, theta=j[1], d=0, offset=-PI_2, joint_type='R')
-    link3 = DH(alpha=PI_2, a=0, theta=0, d=j[2], offset=-0.4389, joint_type='P')
-    link4 = DH(alpha=0, a=0, theta=j[3], d=0.416, offset=0, joint_type='R')
-    link5 = DH(alpha=-PI_2, a=0, theta=j[4], d=0, offset=-PI_2, joint_type='R')
-    link6 = DH(alpha=-PI_2, a=0.009, theta=j[5], d=0, offset=-PI_2, joint_type='R')
-    link7 = DH(alpha=-PI_2, a=0, theta=0, d=0.0106, offset=PI_2, joint_type='R')
+    link1 = DH(alpha=PI_2, a=0, theta=j[0], d=0, offset=PI_2, joint_type='R', convention='MODIFIED')
+    link2 = DH(alpha=-PI_2, a=0, theta=j[1], d=0, offset=-PI_2, joint_type='R', convention='MODIFIED')
+    link3 = DH(alpha=PI_2, a=0, theta=0, d=j[2], offset=-0.4389, joint_type='P', convention='MODIFIED')
+    link4 = DH(alpha=0, a=0, theta=j[3], d=0.416, offset=0, joint_type='R', convention='MODIFIED')
+    link5 = DH(alpha=-PI_2, a=0, theta=j[4], d=0, offset=-PI_2, joint_type='R', convention='MODIFIED')
+    link6 = DH(alpha=-PI_2, a=0.009, theta=j[5], d=0, offset=-PI_2, joint_type='R', convention='MODIFIED')
+    link7 = DH(alpha=-PI_2, a=0, theta=0, d=0.0106, offset=PI_2, joint_type='R', convention='MODIFIED')
 
     T_1_0 = link1.get_trans()
     T_2_1 = link2.get_trans()
@@ -71,39 +71,6 @@ def compute_FK(joint_pos):
         return T_7_0
 
 
-class DH:
-    def __init__(self, alpha, a, theta, d, offset, joint_type):
-        self.alpha = alpha
-        self.a = a
-        self.theta = theta
-        self.d = d
-        self.offset = offset
-        self.joint_type = joint_type
-
-    def mat_from_dh(self, alpha, a, theta, d, offset):
-        ca = np.cos(alpha)
-        sa = np.sin(alpha)
-        if self.joint_type == 'R':
-            theta = theta + offset
-        elif self.joint_type == 'P':
-            d = d + offset
-        else:
-            assert type == 'P' and type == 'R'
-
-        ct = np.cos(theta)
-        st = np.sin(theta)
-        mat = np.mat([
-            [ct     , -st     ,  0 ,  a],
-            [st * ca,  ct * ca, -sa, -d * sa],
-            [st * sa,  ct * sa,  ca,  d * ca],
-            [0      ,  0      ,  0 ,  1]
-        ])
-        return mat
-
-    def get_trans(self):
-        return self.mat_from_dh(self.alpha, self.a, self.theta, self.d, self.offset)
-
-
 # T_7_0 = compute_FK([-0.5, 0, 0.2, 0, 0, 0])
 #
 # print T_7_0