From 3988e21051e912ec7da5995026dbb0c699f27c40 Mon Sep 17 00:00:00 2001
From: Daniel Ingram <ingramds@appstate.edu>
Date: Mon, 13 Aug 2018 21:42:17 -0400
Subject: [PATCH] two_joint_arm.py

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

diff --git a/RoboticArm/two_joint_arm.py b/RoboticArm/two_joint_arm.py
new file mode 100644
index 00000000..60fb5eff
--- /dev/null
+++ b/RoboticArm/two_joint_arm.py
@@ -0,0 +1,77 @@
+"""
+Inverse kinematics of a two-joint arm
+Left-click the plot to set the goal position of the end effector
+
+Author: Daniel Ingram (daniel-s-ingram)
+"""
+from __future__ import print_function, division
+
+import matplotlib.pyplot as plt
+import numpy as np
+
+from math import sin, cos, atan2, acos, pi
+
+Kp = 15
+dt = 0.01
+
+#Link lengths
+l1 = l2 = 1
+
+shoulder = np.array([0, 0])
+
+#Set initial goal position to the initial end-effector position
+x = 2
+y = 0
+
+plt.ion()
+
+def two_joint_arm():
+    """
+    Computes the inverse kinematics for a planar 2DOF arm
+    """
+    theta1 = theta2 = 0
+    while True:
+        try:
+            theta2_goal = acos((x**2 + y**2 - l1**2 -l2**2)/(2*l1*l2))
+            theta1_goal = atan2(y, x) - atan2(l2*sin(theta2_goal), (l1 + l2*cos(theta2_goal)))
+
+            if theta1_goal < 0:
+                theta2_goal = -theta2_goal
+                theta1_goal = atan2(y, x) - atan2(l2*sin(theta2_goal), (l1 + l2*cos(theta2_goal)))
+
+            theta1 = theta1 + Kp*ang_diff(theta1_goal, theta1)*dt
+            theta2 = theta2 + Kp*ang_diff(theta2_goal, theta2)*dt
+        except ValueError as e:
+            print("Unreachable goal")
+
+        plot_arm(theta1, theta2, x, y)
+
+def plot_arm(theta1, theta2, x, y):
+    elbow = shoulder + np.array([l1*cos(theta1), l1*sin(theta1)])
+    wrist = elbow + np.array([l2*cos(theta1+theta2), l2*sin(theta1+theta2)])
+    plt.cla()
+    plt.plot([shoulder[0], elbow[0]], [shoulder[1], elbow[1]], 'k-')
+    plt.plot([elbow[0], wrist[0]], [elbow[1], wrist[1]], 'k-')
+    plt.plot(shoulder[0], shoulder[1], 'ro')
+    plt.plot(elbow[0], elbow[1], 'ro')
+    plt.plot(wrist[0], wrist[1], 'ro')
+    plt.plot([wrist[0], x], [wrist[1], y], 'g--')
+    plt.plot(x, y, 'g*')
+    plt.xlim(-2, 2)
+    plt.ylim(-2, 2)
+    plt.show()
+    plt.pause(dt)
+
+def ang_diff(theta1, theta2):
+    #Returns the difference between two angles in the range -pi to +pi
+    return (theta1-theta2+pi)%(2*pi)-pi
+
+def click(event):
+    global x, y
+    x = event.xdata
+    y = event.ydata
+
+if __name__ == "__main__":
+    fig = plt.figure()
+    fig.canvas.mpl_connect("button_press_event", click)
+    two_joint_arm()