diff --git a/ArmNavigation/n_joint_to_point_control/NLinkArm.py b/ArmNavigation/n_joint_to_point_control/NLinkArm.py index 4ab56054..71cab16c 100644 --- a/ArmNavigation/n_joint_to_point_control/NLinkArm.py +++ b/ArmNavigation/n_joint_to_point_control/NLinkArm.py @@ -7,50 +7,61 @@ Author: Daniel Ingram import numpy as np import matplotlib.pyplot as plt + class NLinkArm(object): - def __init__(self, link_lengths, joint_angles, goal): + def __init__(self, link_lengths, joint_angles, goal, show_animation): + self.show_animation = show_animation self.n_links = len(link_lengths) if self.n_links is not len(joint_angles): raise ValueError() self.link_lengths = np.array(link_lengths) self.joint_angles = np.array(joint_angles) - self.points = [[0, 0] for _ in range(self.n_links+1)] + self.points = [[0, 0] for _ in range(self.n_links + 1)] self.lim = sum(link_lengths) self.goal = np.array(goal).T - self.fig = plt.figure() - self.fig.canvas.mpl_connect('button_press_event', self.click) + if show_animation: + self.fig = plt.figure() + self.fig.canvas.mpl_connect('button_press_event', self.click) - plt.ion() - plt.show() + plt.ion() + plt.show() self.update_points() def update_joints(self, joint_angles): self.joint_angles = joint_angles + self.update_points() def update_points(self): - for i in range(1, self.n_links+1): - self.points[i][0] = self.points[i-1][0] + self.link_lengths[i-1]*np.cos(np.sum(self.joint_angles[:i])) - self.points[i][1] = self.points[i-1][1] + self.link_lengths[i-1]*np.sin(np.sum(self.joint_angles[:i])) + for i in range(1, self.n_links + 1): + self.points[i][0] = self.points[i - 1][0] + \ + self.link_lengths[i - 1] * \ + np.cos(np.sum(self.joint_angles[:i])) + self.points[i][1] = self.points[i - 1][1] + \ + self.link_lengths[i - 1] * \ + np.sin(np.sum(self.joint_angles[:i])) self.end_effector = np.array(self.points[self.n_links]).T - self.plot() + if self.show_animation: + self.plot() def plot(self): plt.cla() - for i in range(self.n_links+1): + for i in range(self.n_links + 1): if i is not self.n_links: - plt.plot([self.points[i][0], self.points[i+1][0]], [self.points[i][1], self.points[i+1][1]], 'r-') + plt.plot([self.points[i][0], self.points[i + 1][0]], + [self.points[i][1], self.points[i + 1][1]], 'r-') plt.plot(self.points[i][0], self.points[i][1], 'ko') plt.plot(self.goal[0], self.goal[1], 'gx') - plt.plot([self.end_effector[0], self.goal[0]], [self.end_effector[1], self.goal[1]], 'g--') + plt.plot([self.end_effector[0], self.goal[0]], [ + self.end_effector[1], self.goal[1]], 'g--') plt.xlim([-self.lim, self.lim]) plt.ylim([-self.lim, self.lim]) diff --git a/ArmNavigation/n_joint_to_point_control/animation.gif b/ArmNavigation/n_joint_to_point_control/animation.gif new file mode 100644 index 00000000..a5de81fd Binary files /dev/null and b/ArmNavigation/n_joint_to_point_control/animation.gif differ diff --git a/ArmNavigation/n_joint_to_point_control/n_joint_to_point_control.py b/ArmNavigation/n_joint_to_point_control/n_joint_to_point_control.py index 5089d111..33fdf8c8 100644 --- a/ArmNavigation/n_joint_to_point_control/n_joint_to_point_control.py +++ b/ArmNavigation/n_joint_to_point_control/n_joint_to_point_control.py @@ -2,31 +2,35 @@ Inverse kinematics for an n-link arm using the Jacobian inverse method Author: Daniel Ingram (daniel-s-ingram) + Atsushi Sakai (@Atsushi_twi) """ import matplotlib.pyplot as plt import numpy as np from NLinkArm import NLinkArm -#Simulation parameters +# Simulation parameters Kp = 2 dt = 0.1 N_LINKS = 10 N_ITERATIONS = 10000 -#States +# States WAIT_FOR_NEW_GOAL = 1 MOVING_TO_GOAL = 2 -def n_link_arm_ik(): +show_animation = True + + +def main(): """ Creates an arm using the NLinkArm class and uses its inverse kinematics to move it to the desired position. """ - link_lengths = [1]*N_LINKS - joint_angles = np.array([0]*N_LINKS) + link_lengths = [1] * N_LINKS + joint_angles = np.array([0] * N_LINKS) goal_pos = [N_LINKS, 0] - arm = NLinkArm(link_lengths, joint_angles, goal_pos) + arm = NLinkArm(link_lengths, joint_angles, goal_pos, show_animation) state = WAIT_FOR_NEW_GOAL solution_found = False while True: @@ -35,10 +39,11 @@ def n_link_arm_ik(): end_effector = arm.end_effector errors, distance = distance_to_goal(end_effector, goal_pos) - #State machine to allow changing of goal before current goal has been reached + # State machine to allow changing of goal before current goal has been reached if state is WAIT_FOR_NEW_GOAL: if distance > 0.1 and not solution_found: - joint_goal_angles, solution_found = inverse_kinematics(link_lengths, joint_angles, goal_pos) + joint_goal_angles, solution_found = inverse_kinematics( + link_lengths, joint_angles, goal_pos) if not solution_found: print("Solution could not be found.") state = WAIT_FOR_NEW_GOAL @@ -47,13 +52,14 @@ def n_link_arm_ik(): state = MOVING_TO_GOAL elif state is MOVING_TO_GOAL: if distance > 0.1 and (old_goal is goal_pos): - joint_angles = joint_angles + Kp*ang_diff(joint_goal_angles, joint_angles)*dt + joint_angles = joint_angles + Kp * \ + ang_diff(joint_goal_angles, joint_angles) * dt else: state = WAIT_FOR_NEW_GOAL solution_found = False arm.update_joints(joint_angles) - + def inverse_kinematics(link_lengths, joint_angles, goal_pos): """ @@ -69,34 +75,90 @@ def inverse_kinematics(link_lengths, joint_angles, goal_pos): joint_angles = joint_angles + np.matmul(J, errors) return joint_angles, False + +def get_random_goal(): + from random import random + SAREA = 15.0 + return [SAREA * random() - SAREA / 2.0, + SAREA * random() - SAREA / 2.0] + + +def animation(): + link_lengths = [1] * N_LINKS + joint_angles = np.array([0] * N_LINKS) + goal_pos = get_random_goal() + arm = NLinkArm(link_lengths, joint_angles, goal_pos, show_animation) + state = WAIT_FOR_NEW_GOAL + solution_found = False + + i_goal = 0 + while True: + old_goal = goal_pos + goal_pos = arm.goal + end_effector = arm.end_effector + errors, distance = distance_to_goal(end_effector, goal_pos) + + # State machine to allow changing of goal before current goal has been reached + if state is WAIT_FOR_NEW_GOAL: + + if distance > 0.1 and not solution_found: + joint_goal_angles, solution_found = inverse_kinematics( + link_lengths, joint_angles, goal_pos) + if not solution_found: + print("Solution could not be found.") + state = WAIT_FOR_NEW_GOAL + arm.goal = get_random_goal() + elif solution_found: + state = MOVING_TO_GOAL + elif state is MOVING_TO_GOAL: + if distance > 0.1 and (old_goal is goal_pos): + joint_angles = joint_angles + Kp * \ + ang_diff(joint_goal_angles, joint_angles) * dt + else: + state = WAIT_FOR_NEW_GOAL + solution_found = False + arm.goal = get_random_goal() + i_goal += 1 + + if i_goal >= 5: + break + + arm.update_joints(joint_angles) + + def forward_kinematics(link_lengths, joint_angles): x = y = 0 - for i in range(1, N_LINKS+1): - x += link_lengths[i-1]*np.cos(np.sum(joint_angles[:i])) - y += link_lengths[i-1]*np.sin(np.sum(joint_angles[:i])) + for i in range(1, N_LINKS + 1): + x += link_lengths[i - 1] * np.cos(np.sum(joint_angles[:i])) + y += link_lengths[i - 1] * np.sin(np.sum(joint_angles[:i])) return np.array([x, y]).T + def jacobian_inverse(link_lengths, joint_angles): J = np.zeros((2, N_LINKS)) for i in range(N_LINKS): J[0, i] = 0 J[1, i] = 0 for j in range(i, N_LINKS): - J[0, i] -= link_lengths[j]*np.sin(np.sum(joint_angles[:j])) - J[1, i] += link_lengths[j]*np.cos(np.sum(joint_angles[:j])) + J[0, i] -= link_lengths[j] * np.sin(np.sum(joint_angles[:j])) + J[1, i] += link_lengths[j] * np.cos(np.sum(joint_angles[:j])) return np.linalg.pinv(J) + def distance_to_goal(current_pos, goal_pos): x_diff = goal_pos[0] - current_pos[0] y_diff = goal_pos[1] - current_pos[1] return np.array([x_diff, y_diff]).T, np.math.sqrt(x_diff**2 + y_diff**2) + def ang_diff(theta1, theta2): """ Returns the difference between two angles in the range -pi to +pi """ - return (theta1 - theta2 + np.pi)%(2*np.pi) - np.pi + return (theta1 - theta2 + np.pi) % (2 * np.pi) - np.pi + if __name__ == '__main__': - n_link_arm_ik() \ No newline at end of file + main() + # animation() \ No newline at end of file