@@ -24,11 +24,11 @@ def transformation_matrix(self):
24
24
25
25
return trans
26
26
27
- def basic_jacobian (self , trans_prev , ee_pose ):
27
+ def basic_jacobian (self , trans_prev , ee_pos ):
28
28
pos_prev = np .array ([trans_prev [0 , 3 ], trans_prev [1 , 3 ], trans_prev [2 , 3 ]])
29
29
z_axis_prev = np .array ([trans_prev [0 , 2 ], trans_prev [1 , 2 ], trans_prev [2 , 2 ]])
30
30
31
- basic_jacobian = np .hstack ((np .cross (z_axis_prev , ee_pose - pos_prev ), z_axis_prev ))
31
+ basic_jacobian = np .hstack ((np .cross (z_axis_prev , ee_pos - pos_prev ), z_axis_prev ))
32
32
return basic_jacobian
33
33
34
34
@@ -50,7 +50,7 @@ def forward_kinematics(self, plot=False):
50
50
x = trans [0 , 3 ]
51
51
y = trans [1 , 3 ]
52
52
z = trans [2 , 3 ]
53
- alpha , beta , gamma = self .calc_euler_angle ()
53
+ alpha , beta , gamma = self .euler_angle ()
54
54
55
55
if plot :
56
56
self .fig = plt .figure ()
@@ -82,12 +82,13 @@ def forward_kinematics(self, plot=False):
82
82
83
83
return [x , y , z , alpha , beta , gamma ]
84
84
85
- def basic_jacobian (self , ee_pose ):
85
+ def basic_jacobian (self ):
86
+ ee_pos = self .forward_kinematics ()[0 :3 ]
86
87
basic_jacobian_mat = []
87
88
88
89
trans = np .identity (4 )
89
90
for i in range (len (self .link_list )):
90
- basic_jacobian_mat .append (self .link_list [i ].basic_jacobian (trans , ee_pose [ 0 : 3 ] ))
91
+ basic_jacobian_mat .append (self .link_list [i ].basic_jacobian (trans , ee_pos ))
91
92
trans = np .dot (trans , self .link_list [i ].transformation_matrix ())
92
93
93
94
return np .array (basic_jacobian_mat ).T
@@ -97,8 +98,8 @@ def inverse_kinematics(self, ref_ee_pose, plot=False):
97
98
ee_pose = self .forward_kinematics ()
98
99
diff_pose = np .array (ref_ee_pose ) - ee_pose
99
100
100
- basic_jacobian_mat = self .basic_jacobian (ee_pose )
101
- alpha , beta , gamma = self .calc_euler_angle ()
101
+ basic_jacobian_mat = self .basic_jacobian ()
102
+ alpha , beta , gamma = self .euler_angle ()
102
103
103
104
K_zyz = np .array ([[0 , - math .sin (alpha ), math .cos (alpha ) * math .sin (beta )],
104
105
[0 , math .cos (alpha ), math .sin (alpha ) * math .sin (beta )],
@@ -138,13 +139,13 @@ def inverse_kinematics(self, ref_ee_pose, plot=False):
138
139
self .ax .plot ([ref_ee_pose [0 ]], [ref_ee_pose [1 ]], [ref_ee_pose [2 ]], "o" )
139
140
plt .show ()
140
141
141
- def calc_euler_angle (self ):
142
+ def euler_angle (self ):
142
143
trans = self .transformation_matrix ()
143
144
144
145
alpha = math .atan2 (trans [1 ][2 ], trans [0 ][2 ])
145
- if - math .pi / 2 <= alpha and alpha <= math .pi / 2 :
146
+ if not ( - math .pi / 2 <= alpha and alpha <= math .pi / 2 ) :
146
147
alpha = math .atan2 (trans [1 ][2 ], trans [0 ][2 ]) + math .pi
147
- if - math .pi / 2 <= alpha and alpha <= math .pi / 2 :
148
+ if not ( - math .pi / 2 <= alpha and alpha <= math .pi / 2 ) :
148
149
alpha = math .atan2 (trans [1 ][2 ], trans [0 ][2 ]) - math .pi
149
150
beta = math .atan2 (trans [0 ][2 ] * math .cos (alpha ) + trans [1 ][2 ] * math .sin (alpha ), trans [2 ][2 ])
150
151
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 ))
0 commit comments