From 2d90bd5a52cdd58b635e95ec5d5ef3d59a239caf Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Fri, 25 Jan 2019 02:01:51 +0900 Subject: [PATCH 1/6] add n_joint_arm_3d & implement forward kinematics --- ArmNavigation/n_joint_arm_3d/NLinkArm.py | 94 +++++++++++++++++++ .../n_joint_arm_3d/random_joint_angles.py | 20 ++++ 2 files changed, 114 insertions(+) create mode 100644 ArmNavigation/n_joint_arm_3d/NLinkArm.py create mode 100644 ArmNavigation/n_joint_arm_3d/random_joint_angles.py diff --git a/ArmNavigation/n_joint_arm_3d/NLinkArm.py b/ArmNavigation/n_joint_arm_3d/NLinkArm.py new file mode 100644 index 00000000..d50f9d5c --- /dev/null +++ b/ArmNavigation/n_joint_arm_3d/NLinkArm.py @@ -0,0 +1,94 @@ +import numpy as np +import math +from mpl_toolkits.mplot3d import Axes3D +import matplotlib.pyplot as plt + +class Link: + def __init__(self, dh_params): + self.dh_params_ = dh_params + + def calc_transformation_matrix(self): + theta = self.dh_params_[0] + alpha = self.dh_params_[1] + 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]]) + + return trans + +class NLinkArm: + def __init__(self, dh_params_list): + self.link_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 calc_transformation_matrix(self): + trans = np.array([[1, 0, 0, 0], + [0, 1, 0, 0], + [0, 0, 1, 0], + [0, 0, 0, 1]]) + for i in range(len(self.link_list)): + trans = np.dot(trans, self.link_list[i].calc_transformation_matrix()) + + return trans + + def forward_kinematics(self): + trans = self.calc_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)) + + return [x, y, z, alpha, beta, gamma] + + def set_joint_angles(self, joint_angle_list): + for i in range(len(self.link_list)): + self.link_list[i].dh_params_[0] = joint_angle_list[i] + + def plot(self): + 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].calc_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() + +if __name__ == "__main__": + 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.]]) + + print(n_link_arm.forward_kinematics()) + n_link_arm.set_joint_angles([1, 1, 1, 1, 1, 1, 1]) + n_link_arm.plot() diff --git a/ArmNavigation/n_joint_arm_3d/random_joint_angles.py b/ArmNavigation/n_joint_arm_3d/random_joint_angles.py new file mode 100644 index 00000000..54be155e --- /dev/null +++ b/ArmNavigation/n_joint_arm_3d/random_joint_angles.py @@ -0,0 +1,20 @@ +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): + 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() + From c49f982923cb415e80291ae7d743fd42532713d8 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Fri, 25 Jan 2019 21:39:32 +0900 Subject: [PATCH 2/6] implement inverse kinematics --- ArmNavigation/n_joint_arm_3d/NLinkArm.py | 65 ++++++++++++++++--- ...angles.py => random_forward_kinematics.py} | 0 .../random_inverse_kinematics.py | 17 +++++ 3 files changed, 72 insertions(+), 10 deletions(-) rename ArmNavigation/n_joint_arm_3d/{random_joint_angles.py => random_forward_kinematics.py} (100%) create mode 100644 ArmNavigation/n_joint_arm_3d/random_inverse_kinematics.py diff --git a/ArmNavigation/n_joint_arm_3d/NLinkArm.py b/ArmNavigation/n_joint_arm_3d/NLinkArm.py index d50f9d5c..f6b9c287 100644 --- a/ArmNavigation/n_joint_arm_3d/NLinkArm.py +++ b/ArmNavigation/n_joint_arm_3d/NLinkArm.py @@ -7,7 +7,7 @@ class Link: def __init__(self, dh_params): self.dh_params_ = dh_params - def calc_transformation_matrix(self): + def transformation_matrix(self): theta = self.dh_params_[0] alpha = self.dh_params_[1] a = self.dh_params_[2] @@ -21,6 +21,14 @@ class Link: return trans + def basic_jacobian(self, trans_prev, ee_pose): + 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)) + return basic_jacobian + + class NLinkArm: def __init__(self, dh_params_list): self.link_list = [] @@ -30,18 +38,14 @@ class NLinkArm: self.fig = plt.figure() self.ax = Axes3D(self.fig) - def calc_transformation_matrix(self): - trans = np.array([[1, 0, 0, 0], - [0, 1, 0, 0], - [0, 0, 1, 0], - [0, 0, 0, 1]]) + def transformation_matrix(self): + trans = np.identity(4) for i in range(len(self.link_list)): - trans = np.dot(trans, self.link_list[i].calc_transformation_matrix()) - + trans = np.dot(trans, self.link_list[i].transformation_matrix()) return trans def forward_kinematics(self): - trans = self.calc_transformation_matrix() + trans = self.transformation_matrix() x = trans[0, 3] y = trans[1, 3] @@ -52,9 +56,50 @@ class NLinkArm: return [x, y, z, alpha, beta, gamma] + def basic_jacobian(self, ref_ee_pose): + basic_jacobian_mat = [] + + trans = np.identity(4) + for i in range(len(self.link_list)): + 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) + + for cnt in range(1000): + basic_jacobian_mat = self.basic_jacobian(ref_ee_pose) + alpha, beta, gamma = self.calc_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)], + [1, 0, math.cos(beta)]]) + K_alpha = np.identity(6) + 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) + + def calc_euler_angle(self): + trans = self.transformation_matrix() + + alpha = math.atan2(trans[1][2], trans[0][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)) + + return alpha, beta, gamma + def set_joint_angles(self, joint_angle_list): for i in range(len(self.link_list)): self.link_list[i].dh_params_[0] = joint_angle_list[i] + + def update_joint_angles(self, diff_joint_angle_list): + for i in range(len(self.link_list)): + self.link_list[i].dh_params_[0] += diff_joint_angle_list[i] def plot(self): x_list = [] @@ -67,7 +112,7 @@ class NLinkArm: 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].calc_transformation_matrix()) + 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]) diff --git a/ArmNavigation/n_joint_arm_3d/random_joint_angles.py b/ArmNavigation/n_joint_arm_3d/random_forward_kinematics.py similarity index 100% rename from ArmNavigation/n_joint_arm_3d/random_joint_angles.py rename to ArmNavigation/n_joint_arm_3d/random_forward_kinematics.py diff --git a/ArmNavigation/n_joint_arm_3d/random_inverse_kinematics.py b/ArmNavigation/n_joint_arm_3d/random_inverse_kinematics.py new file mode 100644 index 00000000..a7286475 --- /dev/null +++ b/ArmNavigation/n_joint_arm_3d/random_inverse_kinematics.py @@ -0,0 +1,17 @@ +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.]]) + +#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() From 3504833748d7e5ebb0d9b8ca82b4ab854bd25572 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Sat, 26 Jan 2019 02:23:53 +0900 Subject: [PATCH 3/6] fix bug of NLinkArm.py and modify random exapmle scripts --- ArmNavigation/n_joint_arm_3d/NLinkArm.py | 106 +++++++++++++++--- .../random_forward_kinematics.py | 16 ++- .../random_inverse_kinematics.py | 32 ++++-- 3 files changed, 121 insertions(+), 33 deletions(-) diff --git a/ArmNavigation/n_joint_arm_3d/NLinkArm.py b/ArmNavigation/n_joint_arm_3d/NLinkArm.py index f6b9c287..e1d73697 100644 --- a/ArmNavigation/n_joint_arm_3d/NLinkArm.py +++ b/ArmNavigation/n_joint_arm_3d/NLinkArm.py @@ -13,11 +13,21 @@ class Link: 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 @@ class NLinkArm: 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 @@ class NLinkArm: 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 @@ class NLinkArm: 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 @@ class NLinkArm: 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) diff --git a/ArmNavigation/n_joint_arm_3d/random_forward_kinematics.py b/ArmNavigation/n_joint_arm_3d/random_forward_kinematics.py index 54be155e..df037fb5 100644 --- a/ArmNavigation/n_joint_arm_3d/random_forward_kinematics.py +++ b/ArmNavigation/n_joint_arm_3d/random_forward_kinematics.py @@ -1,12 +1,15 @@ 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], @@ -14,7 +17,10 @@ for i in range(10): [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) diff --git a/ArmNavigation/n_joint_arm_3d/random_inverse_kinematics.py b/ArmNavigation/n_joint_arm_3d/random_inverse_kinematics.py index a7286475..dd513816 100644 --- a/ArmNavigation/n_joint_arm_3d/random_inverse_kinematics.py +++ b/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) From b543623cada7f6f425df6463b44027fdc4ec211d Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Sat, 26 Jan 2019 02:32:49 +0900 Subject: [PATCH 4/6] minor change --- ArmNavigation/n_joint_arm_3d/NLinkArm.py | 20 -------------------- 1 file changed, 20 deletions(-) diff --git a/ArmNavigation/n_joint_arm_3d/NLinkArm.py b/ArmNavigation/n_joint_arm_3d/NLinkArm.py index e1d73697..19b1a32a 100644 --- a/ArmNavigation/n_joint_arm_3d/NLinkArm.py +++ b/ArmNavigation/n_joint_arm_3d/NLinkArm.py @@ -13,13 +13,6 @@ class Link: 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) @@ -196,16 +189,3 @@ class NLinkArm: self.ax.set_ylim(-1, 1) self.ax.set_zlim(-1, 1) plt.show() - -if __name__ == "__main__": - 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.]]) - - print(n_link_arm.forward_kinematics()) - n_link_arm.set_joint_angles([1, 1, 1, 1, 1, 1, 1]) - n_link_arm.plot() From 33e7b67db0130f6ec5277fdf7e630738d13fb2dd Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Sat, 26 Jan 2019 03:06:27 +0900 Subject: [PATCH 5/6] minor changes --- ArmNavigation/n_joint_arm_3d/NLinkArm.py | 21 ++++++++++--------- .../random_forward_kinematics.py | 1 - 2 files changed, 11 insertions(+), 11 deletions(-) diff --git a/ArmNavigation/n_joint_arm_3d/NLinkArm.py b/ArmNavigation/n_joint_arm_3d/NLinkArm.py index 19b1a32a..d0e32a05 100644 --- a/ArmNavigation/n_joint_arm_3d/NLinkArm.py +++ b/ArmNavigation/n_joint_arm_3d/NLinkArm.py @@ -24,11 +24,11 @@ class Link: 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 @@ class NLinkArm: 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 @@ class NLinkArm: 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 @@ class NLinkArm: 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 @@ class NLinkArm: 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 index df037fb5..e9479521 100644 --- a/ArmNavigation/n_joint_arm_3d/random_forward_kinematics.py +++ b/ArmNavigation/n_joint_arm_3d/random_forward_kinematics.py @@ -22,5 +22,4 @@ if __name__ == "__main__": 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) From 47b497019a0bcedc2cd257ea8bd48942f2b616d5 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Sat, 26 Jan 2019 15:28:59 +0900 Subject: [PATCH 6/6] add header comment in each scripts --- ArmNavigation/n_joint_arm_3d/NLinkArm.py | 4 ++++ ArmNavigation/n_joint_arm_3d/random_forward_kinematics.py | 7 ++++++- ArmNavigation/n_joint_arm_3d/random_inverse_kinematics.py | 5 +++++ 3 files changed, 15 insertions(+), 1 deletion(-) diff --git a/ArmNavigation/n_joint_arm_3d/NLinkArm.py b/ArmNavigation/n_joint_arm_3d/NLinkArm.py index d0e32a05..da562d9f 100644 --- a/ArmNavigation/n_joint_arm_3d/NLinkArm.py +++ b/ArmNavigation/n_joint_arm_3d/NLinkArm.py @@ -1,3 +1,7 @@ +""" +Class of n-link arm in 3D +Author: Takayuki Murooka (takayuki5168) +""" import numpy as np import math from mpl_toolkits.mplot3d import Axes3D diff --git a/ArmNavigation/n_joint_arm_3d/random_forward_kinematics.py b/ArmNavigation/n_joint_arm_3d/random_forward_kinematics.py index e9479521..87907f80 100644 --- a/ArmNavigation/n_joint_arm_3d/random_forward_kinematics.py +++ b/ArmNavigation/n_joint_arm_3d/random_forward_kinematics.py @@ -1,3 +1,7 @@ +""" +Forward Kinematics for an n-link arm in 3D +Author: Takayuki Murooka (takayuki5168) +""" import math from NLinkArm import NLinkArm import random @@ -17,7 +21,8 @@ if __name__ == "__main__": [0., -math.pi/2, 0., .321], [0., math.pi/2, 0., 0.], [0., 0., 0., 0.]]) - + + # execute FK 10 times 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))]) diff --git a/ArmNavigation/n_joint_arm_3d/random_inverse_kinematics.py b/ArmNavigation/n_joint_arm_3d/random_inverse_kinematics.py index dd513816..44be2270 100644 --- a/ArmNavigation/n_joint_arm_3d/random_inverse_kinematics.py +++ b/ArmNavigation/n_joint_arm_3d/random_inverse_kinematics.py @@ -1,3 +1,7 @@ +""" +Inverse Kinematics for an n-link arm in 3D +Author: Takayuki Murooka (takayuki5168) +""" import math from NLinkArm import NLinkArm import random @@ -18,6 +22,7 @@ if __name__ == "__main__": [0., math.pi/2, 0., 0.], [0., 0., 0., 0.]]) + # execute IK 10 times for i in range(10): n_link_arm.inverse_kinematics([random_val(-0.5, 0.5), random_val(-0.5, 0.5),