fix bug of NLinkArm.py and modify random exapmle scripts

Author: takayuki5168

ArmNavigation/n_joint_arm_3d/NLinkArm.py

@@ -13,11 +13,21 @@ def transformation_matrix(self):
         a = self.dh_params_[2]
         d = self.dh_params_[3]
 
+        '''
         trans = np.array(
             [[math.cos(theta), -math.sin(theta), 0, a],
              [math.cos(alpha) * math.sin(theta), math.cos(alpha) * math.cos(theta), -math.sin(alpha), -d * math.sin(alpha)],
              [math.sin(alpha) * math.sin(theta), math.sin(alpha) * math.cos(theta), math.cos(alpha), d * math.cos(alpha)],
              [0, 0, 0, 1]])
+        '''
+        st = math.sin(theta)
+        ct = math.cos(theta)
+        sa = math.sin(alpha)
+        ca = math.cos(alpha)
+        trans = np.array([[ct, -st * ca, st * sa, a * ct],
+                          [st, ct * ca, -ct * sa, a * st],
+                          [0, sa, ca, d],
+                          [0, 0, 0, 1]])
 
         return trans
 
@@ -35,44 +45,66 @@ def __init__(self, dh_params_list):
         for i in range(len(dh_params_list)):
             self.link_list.append(Link(dh_params_list[i]))
 
-        self.fig = plt.figure()
-        self.ax = Axes3D(self.fig)
-
     def transformation_matrix(self):
         trans = np.identity(4)
         for i in range(len(self.link_list)):
             trans = np.dot(trans, self.link_list[i].transformation_matrix())
         return trans
 
-    def forward_kinematics(self):
+    def forward_kinematics(self, plot=False):
         trans = self.transformation_matrix()
 
         x = trans[0, 3]
         y = trans[1, 3]
         z = trans[2, 3]
-        alpha = math.atan2(trans[1, 2], trans[1, 3])
-        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))
+        alpha, beta, gamma = self.calc_euler_angle()
+
+        if plot:
+            self.fig = plt.figure()
+            self.ax = Axes3D(self.fig)
+            
+            x_list = []
+            y_list = []
+            z_list = []
+    
+            trans = np.identity(4)
+            
+            x_list.append(trans[0, 3])
+            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].transformation_matrix())
+                x_list.append(trans[0, 3])
+                y_list.append(trans[1, 3])
+                z_list.append(trans[2, 3])
+                
+            self.ax.plot(x_list, y_list, z_list, "o-", color="#00aa00", ms=4, mew=0.5)
+            self.ax.plot([0], [0], [0], "o")
+            
+            self.ax.set_xlim(-1, 1)
+            self.ax.set_ylim(-1, 1)
+            self.ax.set_zlim(-1, 1)
+  
+            plt.show()
 
         return [x, y, z, alpha, beta, gamma]
 
-    def basic_jacobian(self, ref_ee_pose):
+    def basic_jacobian(self, ee_pose):
         basic_jacobian_mat = []
 
         trans = np.identity(4)        
         for i in range(len(self.link_list)):
+            basic_jacobian_mat.append(self.link_list[i].basic_jacobian(trans, ee_pose[0:3]))
             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)
+    def inverse_kinematics(self, ref_ee_pose, plot=False):
+        for cnt in range(500):
+            ee_pose = self.forward_kinematics()
+            diff_pose = np.array(ref_ee_pose) - ee_pose
 
-        for cnt in range(1000):
-            basic_jacobian_mat = self.basic_jacobian(ref_ee_pose)
+            basic_jacobian_mat = self.basic_jacobian(ee_pose)
             alpha, beta, gamma = self.calc_euler_angle()
 
             K_zyz = np.array([[0, -math.sin(alpha), math.cos(alpha) * math.sin(beta)],
@@ -82,12 +114,45 @@ def inverse_kinematics(self, ref_ee_pose):
             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)
-
+            self.update_joint_angles(theta_dot / 100.)
+            
+        if plot:
+            self.fig = plt.figure()
+            self.ax = Axes3D(self.fig)
+            
+            x_list = []
+            y_list = []
+            z_list = []
+    
+            trans = np.identity(4)
+            
+            x_list.append(trans[0, 3])
+            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].transformation_matrix())
+                x_list.append(trans[0, 3])
+                y_list.append(trans[1, 3])
+                z_list.append(trans[2, 3])
+                
+            self.ax.plot(x_list, y_list, z_list, "o-", color="#00aa00", ms=4, mew=0.5)
+            self.ax.plot([0], [0], [0], "o")
+            
+            self.ax.set_xlim(-1, 1)
+            self.ax.set_ylim(-1, 1)
+            self.ax.set_zlim(-1, 1)
+  
+            self.ax.plot([ref_ee_pose[0]], [ref_ee_pose[1]], [ref_ee_pose[2]], "o")
+            plt.show()
+                
     def calc_euler_angle(self):
         trans = self.transformation_matrix()
 
         alpha = math.atan2(trans[1][2], trans[0][2])
+        if -math.pi / 2 <= alpha and alpha <= math.pi / 2:
+            alpha = math.atan2(trans[1][2], trans[0][2]) + math.pi
+        if -math.pi / 2 <= alpha and alpha <= math.pi / 2:
+            alpha = math.atan2(trans[1][2], trans[0][2]) - math.pi
         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))
 
@@ -102,6 +167,9 @@ def update_joint_angles(self, diff_joint_angle_list):
             self.link_list[i].dh_params_[0] += diff_joint_angle_list[i]
 
     def plot(self):
+        self.fig = plt.figure()
+        self.ax = Axes3D(self.fig)
+        
         x_list = []
         y_list = []
         z_list = []
@@ -119,6 +187,10 @@ def plot(self):
 
         self.ax.plot(x_list, y_list, z_list, "o-", color="#00aa00", ms=4, mew=0.5)
         self.ax.plot([0], [0], [0], "o")
+
+        self.ax.set_xlabel("x")
+        self.ax.set_ylabel("y")
+        self.ax.set_zlabel("z")        
 
         self.ax.set_xlim(-1, 1)
         self.ax.set_ylim(-1, 1)

ArmNavigation/n_joint_arm_3d/random_forward_kinematics.py

@@ -1,20 +1,26 @@
 import math
 from NLinkArm import NLinkArm
 import random
-import time
 
 def random_val(min_val, max_val):
     return min_val + random.random() * (max_val - min_val)
 
-for i in range(10):
+
+if __name__ == "__main__":
+    print("Start solving Forward Kinematics 10 times")
+
+    # init NLinkArm with Denavit-Hartenberg parameters of PR2    
     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.set_joint_angles([random_val(-1, 1) for j in range(len(n_link_arm.link_list))])
-    n_link_arm.plot()
+    
+    for i in range(10):
+        n_link_arm.set_joint_angles([random_val(-1, 1) for j in range(len(n_link_arm.link_list))])
+        
+        ee_pose = n_link_arm.forward_kinematics(plot=True)
+        print(ee_pose)
 

ArmNavigation/n_joint_arm_3d/random_inverse_kinematics.py

@@ -1,17 +1,27 @@
 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.]])
+def random_val(min_val, max_val):
+    return min_val + random.random() * (max_val - min_val)
 
-#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()
+if __name__ == "__main__":
+    print("Start solving Inverse Kinematics 10 times")
+
+    # init NLinkArm with Denavit-Hartenberg parameters of PR2        
+    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.]])
+
+    for i in range(10):
+        n_link_arm.inverse_kinematics([random_val(-0.5, 0.5),
+                                       random_val(-0.5, 0.5),
+                                       random_val(-0.5, 0.5),
+                                       random_val(-0.5, 0.5),
+                                       random_val(-0.5, 0.5),
+                                       random_val(-0.5, 0.5)], plot=True)