Merge pull request AtsushiSakai#95 from daniel-s-ingram/master

Inverse kinematics for n-link arm

Author: AtsushiSakai

RoboticArm/NLinkArm.py

@@ -0,0 +1,62 @@
+"""
+Class for controlling and plotting an arm with an arbitrary number of links.
+
+Author: Daniel Ingram
+"""
+
+import numpy as np
+import matplotlib.pyplot as plt
+
+class NLinkArm(object):
+    def __init__(self, link_lengths, joint_angles, goal):
+        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.lim = sum(link_lengths)
+        self.goal = np.array(goal).T
+
+        self.fig = plt.figure()
+        self.fig.canvas.mpl_connect('button_press_event', self.click)
+
+        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]))
+
+        self.end_effector = np.array(self.points[self.n_links]).T
+        self.plot()
+
+    def plot(self):
+        plt.cla()
+
+        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], '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.xlim([-self.lim, self.lim])
+        plt.ylim([-self.lim, self.lim])
+        plt.draw()
+        plt.pause(0.0001)
+
+    def click(self, event):
+        self.goal = np.array([event.xdata, event.ydata]).T
+        self.plot()

RoboticArm/n_link_arm_ik.py

@@ -0,0 +1,102 @@
+"""
+Inverse kinematics for an n-link arm using the Jacobian inverse method
+
+Author: Daniel Ingram (daniel-s-ingram)
+"""
+import matplotlib.pyplot as plt
+import numpy as np
+
+from NLinkArm import NLinkArm
+
+#Simulation parameters
+Kp = 2
+dt = 0.1
+N_LINKS = 10
+N_ITERATIONS = 10000
+
+#States
+WAIT_FOR_NEW_GOAL = 1
+MOVING_TO_GOAL = 2
+
+def n_link_arm_ik():
+    """
+    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)
+    goal_pos = [N_LINKS, 0]
+    arm = NLinkArm(link_lengths, joint_angles, goal_pos)
+    state = WAIT_FOR_NEW_GOAL
+    solution_found = False
+    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 = end_effector
+                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.update_joints(joint_angles)
+            
+
+def inverse_kinematics(link_lengths, joint_angles, goal_pos):
+    """
+    Calculates the inverse kinematics using the Jacobian inverse method.
+    """
+    for iteration in range(N_ITERATIONS):
+        current_pos = forward_kinematics(link_lengths, joint_angles)
+        errors, distance = distance_to_goal(current_pos, goal_pos)
+        if distance < 0.1:
+            print("Solution found in %d iterations." % iteration)
+            return joint_angles, True
+        J = jacobian_inverse(link_lengths, joint_angles)
+        joint_angles = joint_angles + np.matmul(J, errors)
+    return joint_angles, False
+
+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]))
+    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]))
+
+    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
+
+if __name__ == '__main__':
+    n_link_arm_ik()