add animation

Author: AtsushiSakai

ArmNavigation/n_joint_to_point_control/NLinkArm.py

@@ -7,50 +7,61 @@
 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])

ArmNavigation/n_joint_to_point_control/animation.gif

@@ -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()
+    main()
+    # animation()