github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-02-16 14:05:30 -05:00
Code Issues Packages Projects Releases Wiki Activity

fix bug of NLinkArm.py and modify random exapmle scripts

Browse Source
This commit is contained in:
Takayuki Murooka
2019-01-26 02:23:53 +09:00
parent c49f982923
commit 3504833748
3 changed files with 121 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files

106
ArmNavigation/n_joint_arm_3d/NLinkArm.py
View File

@@ -13,11 +13,21 @@ class Link:
a = self.dh_params_[2] a = self.dh_params_[2]
d = self.dh_params_[3] d = self.dh_params_[3]
'''
trans = np.array( trans = np.array(
[[math.cos(theta), -math.sin(theta), 0, a], [[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.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)], [math.sin(alpha) * math.sin(theta), math.sin(alpha) * math.cos(theta), math.cos(alpha), d * math.cos(alpha)],
[0, 0, 0, 1]]) [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 return trans
@@ -35,44 +45,66 @@ class NLinkArm:
for i in range(len(dh_params_list)): for i in range(len(dh_params_list)):
self.link_list.append(Link(dh_params_list[i])) self.link_list.append(Link(dh_params_list[i]))
self.fig = plt.figure()
self.ax = Axes3D(self.fig)
def transformation_matrix(self): def transformation_matrix(self):
trans = np.identity(4) trans = np.identity(4)
for i in range(len(self.link_list)): for i in range(len(self.link_list)):
trans = np.dot(trans, self.link_list[i].transformation_matrix()) trans = np.dot(trans, self.link_list[i].transformation_matrix())
return trans return trans
def forward_kinematics(self): def forward_kinematics(self, plot=False):
trans = self.transformation_matrix() trans = self.transformation_matrix()
x = trans[0, 3] x = trans[0, 3]
y = trans[1, 3] y = trans[1, 3]
z = trans[2, 3] z = trans[2, 3]
alpha = math.atan2(trans[1, 2], trans[1, 3]) alpha, beta, gamma = self.calc_euler_angle()
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)) 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] return [x, y, z, alpha, beta, gamma]
def basic_jacobian(self, ref_ee_pose): def basic_jacobian(self, ee_pose):
basic_jacobian_mat = [] basic_jacobian_mat = []
trans = np.identity(4) trans = np.identity(4)
for i in range(len(self.link_list)): 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()) 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 return np.array(basic_jacobian_mat).T
def inverse_kinematics(self, ref_ee_pose): def inverse_kinematics(self, ref_ee_pose, plot=False):
ee_pose = self.forward_kinematics() for cnt in range(500):
diff_pose = ee_pose - np.array(ref_ee_pose) 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(ee_pose)
basic_jacobian_mat = self.basic_jacobian(ref_ee_pose)
alpha, beta, gamma = self.calc_euler_angle() alpha, beta, gamma = self.calc_euler_angle()
K_zyz = np.array([[0, -math.sin(alpha), math.cos(alpha) * math.sin(beta)], K_zyz = np.array([[0, -math.sin(alpha), math.cos(alpha) * math.sin(beta)],
@@ -82,12 +114,45 @@ class NLinkArm:
K_alpha[3:, 3:] = K_zyz K_alpha[3:, 3:] = K_zyz
theta_dot = np.dot(np.dot(np.linalg.pinv(basic_jacobian_mat), K_alpha), np.array(diff_pose)) 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): def calc_euler_angle(self):
trans = self.transformation_matrix() trans = self.transformation_matrix()
alpha = math.atan2(trans[1][2], trans[0][2]) 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]) 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)) 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 @@ class NLinkArm:
self.link_list[i].dh_params_[0] += diff_joint_angle_list[i] self.link_list[i].dh_params_[0] += diff_joint_angle_list[i]
def plot(self): def plot(self):
self.fig = plt.figure()
self.ax = Axes3D(self.fig)
x_list = [] x_list = []
y_list = [] y_list = []
z_list = [] z_list = []
@@ -119,6 +187,10 @@ class NLinkArm:
self.ax.plot(x_list, y_list, z_list, "o-", color="#00aa00", ms=4, mew=0.5) 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.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_xlim(-1, 1)
self.ax.set_ylim(-1, 1) self.ax.set_ylim(-1, 1)

16
ArmNavigation/n_joint_arm_3d/random_forward_kinematics.py
View File

@@ -1,12 +1,15 @@
import math import math
from NLinkArm import NLinkArm from NLinkArm import NLinkArm
import random import random
import time
def random_val(min_val, max_val): def random_val(min_val, max_val):
return min_val + random.random() * (max_val - min_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.], n_link_arm = NLinkArm([[0., -math.pi/2, .1, 0.],
[math.pi/2, math.pi/2, 0., 0.], [math.pi/2, math.pi/2, 0., 0.],
[0., -math.pi/2, 0., .4], [0., -math.pi/2, 0., .4],
@@ -14,7 +17,10 @@ for i in range(10):
[0., -math.pi/2, 0., .321], [0., -math.pi/2, 0., .321],
[0., math.pi/2, 0., 0.], [0., math.pi/2, 0., 0.],
[0., 0., 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))]) for i in range(10):
n_link_arm.plot() 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)

32
ArmNavigation/n_joint_arm_3d/random_inverse_kinematics.py
View File

@@ -1,17 +1,27 @@
import math import math
from NLinkArm import NLinkArm from NLinkArm import NLinkArm
import random import random
import time
n_link_arm = NLinkArm([[0., -math.pi/2, .1, 0.], def random_val(min_val, max_val):
[math.pi/2, math.pi/2, 0., 0.], return min_val + random.random() * (max_val - min_val)
[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]) if __name__ == "__main__":
n_link_arm.inverse_kinematics([-0.5, 0., 0.1, 0., 0., math.pi / 2]) print("Start solving Inverse Kinematics 10 times")
n_link_arm.plot()
# 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)