From c49f982923cb415e80291ae7d743fd42532713d8 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Fri, 25 Jan 2019 21:39:32 +0900 Subject: [PATCH] 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()