Skip to content

Commit 33e7b67

Browse files
committed
minor changes
1 parent b543623 commit 33e7b67

File tree

2 files changed

+11
-11
lines changed

2 files changed

+11
-11
lines changed

ArmNavigation/n_joint_arm_3d/NLinkArm.py

Lines changed: 11 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -24,11 +24,11 @@ def transformation_matrix(self):
2424

2525
return trans
2626

27-
def basic_jacobian(self, trans_prev, ee_pose):
27+
def basic_jacobian(self, trans_prev, ee_pos):
2828
pos_prev = np.array([trans_prev[0, 3], trans_prev[1, 3], trans_prev[2, 3]])
2929
z_axis_prev = np.array([trans_prev[0, 2], trans_prev[1, 2], trans_prev[2, 2]])
3030

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))
3232
return basic_jacobian
3333

3434

@@ -50,7 +50,7 @@ def forward_kinematics(self, plot=False):
5050
x = trans[0, 3]
5151
y = trans[1, 3]
5252
z = trans[2, 3]
53-
alpha, beta, gamma = self.calc_euler_angle()
53+
alpha, beta, gamma = self.euler_angle()
5454

5555
if plot:
5656
self.fig = plt.figure()
@@ -82,12 +82,13 @@ def forward_kinematics(self, plot=False):
8282

8383
return [x, y, z, alpha, beta, gamma]
8484

85-
def basic_jacobian(self, ee_pose):
85+
def basic_jacobian(self):
86+
ee_pos = self.forward_kinematics()[0:3]
8687
basic_jacobian_mat = []
8788

8889
trans = np.identity(4)
8990
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))
9192
trans = np.dot(trans, self.link_list[i].transformation_matrix())
9293

9394
return np.array(basic_jacobian_mat).T
@@ -97,8 +98,8 @@ def inverse_kinematics(self, ref_ee_pose, plot=False):
9798
ee_pose = self.forward_kinematics()
9899
diff_pose = np.array(ref_ee_pose) - ee_pose
99100

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()
102103

103104
K_zyz = np.array([[0, -math.sin(alpha), math.cos(alpha) * math.sin(beta)],
104105
[0, math.cos(alpha), math.sin(alpha) * math.sin(beta)],
@@ -138,13 +139,13 @@ def inverse_kinematics(self, ref_ee_pose, plot=False):
138139
self.ax.plot([ref_ee_pose[0]], [ref_ee_pose[1]], [ref_ee_pose[2]], "o")
139140
plt.show()
140141

141-
def calc_euler_angle(self):
142+
def euler_angle(self):
142143
trans = self.transformation_matrix()
143144

144145
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):
146147
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):
148149
alpha = math.atan2(trans[1][2], trans[0][2]) - math.pi
149150
beta = math.atan2(trans[0][2] * math.cos(alpha) + trans[1][2] * math.sin(alpha), trans[2][2])
150151
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))

ArmNavigation/n_joint_arm_3d/random_forward_kinematics.py

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -22,5 +22,4 @@ def random_val(min_val, max_val):
2222
n_link_arm.set_joint_angles([random_val(-1, 1) for j in range(len(n_link_arm.link_list))])
2323

2424
ee_pose = n_link_arm.forward_kinematics(plot=True)
25-
print(ee_pose)
2625

0 commit comments

Comments
 (0)