From ce36e2c91becfe9fdadb60d684767bb867c660ce Mon Sep 17 00:00:00 2001
From: Daniel Ingram <ingramds@appstate.edu>
Date: Wed, 15 Aug 2018 21:40:54 -0400
Subject: [PATCH] Inverse kinematics for n-link arm using the inverse Jacobian
 method

---
 RoboticArm/n_link_arm_ik.py | 102 ++++++++++++++++++++++++++++++++++++
 1 file changed, 102 insertions(+)
 create mode 100644 RoboticArm/n_link_arm_ik.py

diff --git a/RoboticArm/n_link_arm_ik.py b/RoboticArm/n_link_arm_ik.py
new file mode 100644
index 00000000..5089d111
--- /dev/null
+++ b/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()
\ No newline at end of file