implement inverse kinematics

takayuki5168 · takayuki5168 · commit c49f982923cb · 2019-01-25T21:39:32.000+09:00
diff --git a/ArmNavigation/n_joint_arm_3d/NLinkArm.py b/ArmNavigation/n_joint_arm_3d/NLinkArm.py
@@ -7,7 +7,7 @@ class Link:
     def __init__(self, dh_params):
         self.dh_params_ = dh_params
 
-    def calc_transformation_matrix(self):
+    def transformation_matrix(self):
         theta = self.dh_params_[0]
         alpha = self.dh_params_[1]
         a = self.dh_params_[2]
@@ -21,6 +21,14 @@ def calc_transformation_matrix(self):
 
         return trans
 
+    def basic_jacobian(self, trans_prev, ee_pose):
+        pos_prev = np.array([trans_prev[0, 3], trans_prev[1, 3], trans_prev[2, 3]])
+        z_axis_prev = np.array([trans_prev[0, 2], trans_prev[1, 2], trans_prev[2, 2]])
+
+        basic_jacobian = np.hstack((np.cross(z_axis_prev, ee_pose - pos_prev), z_axis_prev))
+        return basic_jacobian
+        
+
 class NLinkArm:
     def __init__(self, dh_params_list):
         self.link_list = []
@@ -30,18 +38,14 @@ def __init__(self, dh_params_list):
         self.fig = plt.figure()
         self.ax = Axes3D(self.fig)
 
-    def calc_transformation_matrix(self):
-        trans = np.array([[1, 0, 0, 0],
-                             [0, 1, 0, 0],
-                             [0, 0, 1, 0],
-                             [0, 0, 0, 1]])
+    def transformation_matrix(self):
+        trans = np.identity(4)
         for i in range(len(self.link_list)):
-            trans = np.dot(trans, self.link_list[i].calc_transformation_matrix())
-            
+            trans = np.dot(trans, self.link_list[i].transformation_matrix())
         return trans
     
     def forward_kinematics(self):
-        trans = self.calc_transformation_matrix()
+        trans = self.transformation_matrix()
 
         x = trans[0, 3]
         y = trans[1, 3]
@@ -52,9 +56,50 @@ def forward_kinematics(self):
 
         return [x, y, z, alpha, beta, gamma]
 
+    def basic_jacobian(self, ref_ee_pose):
+        basic_jacobian_mat = []
+        
+        trans = np.identity(4)        
+        for i in range(len(self.link_list)):
+            trans = np.dot(trans, self.link_list[i].transformation_matrix())
+            basic_jacobian_mat.append(self.link_list[i].basic_jacobian(trans, ref_ee_pose[0:3]))
+            
+        #print(np.array(basic_jacobian_mat).T)
+        return np.array(basic_jacobian_mat).T
+
+    def inverse_kinematics(self, ref_ee_pose):
+        ee_pose = self.forward_kinematics()
+        diff_pose = ee_pose - np.array(ref_ee_pose)
+        
+        for cnt in range(1000):
+            basic_jacobian_mat = self.basic_jacobian(ref_ee_pose)
+            alpha, beta, gamma = self.calc_euler_angle()
+            
+            K_zyz = np.array([[0, -math.sin(alpha), math.cos(alpha) * math.sin(beta)],
+                              [0, math.cos(alpha), math.sin(alpha) * math.sin(beta)],
+                              [1, 0, math.cos(beta)]])
+            K_alpha = np.identity(6)
+            K_alpha[3:, 3:] = K_zyz
+
+            theta_dot = np.dot(np.dot(np.linalg.pinv(basic_jacobian_mat), K_alpha), np.array(diff_pose))
+            self.update_joint_angles(theta_dot)
+
+    def calc_euler_angle(self):
+        trans = self.transformation_matrix()
+        
+        alpha = math.atan2(trans[1][2], trans[0][2])
+        beta = math.atan2(trans[0][2] * math.cos(alpha) + trans[1][2] * math.sin(alpha), trans[2][2])
+        gamma = math.atan2(-trans[0][0] * math.sin(alpha) + trans[1][0] * math.cos(alpha), -trans[0][1] * math.sin(alpha) + trans[1][1] * math.cos(alpha))
+        
+        return alpha, beta, gamma
+    
     def set_joint_angles(self, joint_angle_list):
         for i in range(len(self.link_list)):
             self.link_list[i].dh_params_[0] = joint_angle_list[i]
+
+    def update_joint_angles(self, diff_joint_angle_list):
+        for i in range(len(self.link_list)):
+            self.link_list[i].dh_params_[0] += diff_joint_angle_list[i]
         
     def plot(self):
         x_list = []
@@ -67,7 +112,7 @@ def plot(self):
         y_list.append(trans[1, 3])
         z_list.append(trans[2, 3])
         for i in range(len(self.link_list)):
-            trans = np.dot(trans, self.link_list[i].calc_transformation_matrix())
+            trans = np.dot(trans, self.link_list[i].transformation_matrix())
             x_list.append(trans[0, 3])
             y_list.append(trans[1, 3])
             z_list.append(trans[2, 3])
diff --git a/ArmNavigation/n_joint_arm_3d/random_forward_kinematics.py b/ArmNavigation/n_joint_arm_3d/random_forward_kinematics.py
diff --git a/ArmNavigation/n_joint_arm_3d/random_inverse_kinematics.py b/ArmNavigation/n_joint_arm_3d/random_inverse_kinematics.py
@@ -0,0 +1,17 @@
+import math
+from NLinkArm import NLinkArm
+import random
+import time
+
+
+n_link_arm = NLinkArm([[0., -math.pi/2, .1, 0.],
+                       [math.pi/2, math.pi/2, 0., 0.],
+                       [0., -math.pi/2, 0., .4],
+                       [0., math.pi/2, 0., 0.],
+                       [0., -math.pi/2, 0., .321],
+                       [0., math.pi/2, 0., 0.],
+                       [0., 0., 0., 0.]])
+
+#n_link_arm.inverse_kinematics([-0.621, 0., 0., 0., 0., math.pi / 2])
+n_link_arm.inverse_kinematics([-0.5, 0., 0.1, 0., 0., math.pi / 2])
+n_link_arm.plot()
