minor changes

takayuki5168 · takayuki5168 · commit 33e7b67db013 · 2019-01-26T03:06:27.000+09:00
diff --git a/ArmNavigation/n_joint_arm_3d/NLinkArm.py b/ArmNavigation/n_joint_arm_3d/NLinkArm.py
@@ -24,11 +24,11 @@ def transformation_matrix(self):
 
         return trans
 
-    def basic_jacobian(self, trans_prev, ee_pose):
+    def basic_jacobian(self, trans_prev, ee_pos):
         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))
+        basic_jacobian = np.hstack((np.cross(z_axis_prev, ee_pos - pos_prev), z_axis_prev))
         return basic_jacobian
         
 
@@ -50,7 +50,7 @@ def forward_kinematics(self, plot=False):
         x = trans[0, 3]
         y = trans[1, 3]
         z = trans[2, 3]
-        alpha, beta, gamma = self.calc_euler_angle()
+        alpha, beta, gamma = self.euler_angle()
 
         if plot:
             self.fig = plt.figure()
@@ -82,12 +82,13 @@ def forward_kinematics(self, plot=False):
 
         return [x, y, z, alpha, beta, gamma]
 
-    def basic_jacobian(self, ee_pose):
+    def basic_jacobian(self):
+        ee_pos = self.forward_kinematics()[0:3]
         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]))
+            basic_jacobian_mat.append(self.link_list[i].basic_jacobian(trans, ee_pos))
             trans = np.dot(trans, self.link_list[i].transformation_matrix())
             
         return np.array(basic_jacobian_mat).T
@@ -97,8 +98,8 @@ def inverse_kinematics(self, ref_ee_pose, plot=False):
             ee_pose = self.forward_kinematics()
             diff_pose = np.array(ref_ee_pose) - ee_pose
         
-            basic_jacobian_mat = self.basic_jacobian(ee_pose)
-            alpha, beta, gamma = self.calc_euler_angle()
+            basic_jacobian_mat = self.basic_jacobian()
+            alpha, beta, gamma = self.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)],
@@ -138,13 +139,13 @@ def inverse_kinematics(self, ref_ee_pose, plot=False):
             self.ax.plot([ref_ee_pose[0]], [ref_ee_pose[1]], [ref_ee_pose[2]], "o")
             plt.show()
                 
-    def calc_euler_angle(self):
+    def 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:
+        if not (-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:
+        if not (-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))
diff --git a/ArmNavigation/n_joint_arm_3d/random_forward_kinematics.py b/ArmNavigation/n_joint_arm_3d/random_forward_kinematics.py
@@ -22,5 +22,4 @@ def random_val(min_val, max_val):
         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)
 

Original file line number	Diff line number	Diff line change
`@@ -22,5 +22,4 @@ def random_val(min_val, max_val):`
`22`	`22`	`n_link_arm.set_joint_angles([random_val(-1, 1) for j in range(len(n_link_arm.link_list))])`
`23`	`23`
`24`	`24`	`ee_pose = n_link_arm.forward_kinematics(plot=True)`
`25`		`- print(ee_pose)`
`26`	`25`