removed joint constraints, left as optional, fixed formatting

InvKin/Arm.py

@@ -15,7 +15,6 @@
 along with this program.  If not, see <http://www.gnu.org/licenses/>.
 '''
 
-import math
 import numpy as np
 import scipy.optimize
 
@@ -34,14 +33,14 @@ def __init__(self, q=None, q0=None, L=None):
             the arm segment lengths
         """
         # initial joint angles
-        self.q = [math.pi/4, math.pi/4, 0] if q is None else q
+        self.q = [.3, .3, 0] if q is None else q
         # some default arm positions
-        self.q0 = np.array([math.pi/4, math.pi/4, 0]) if q0 is None else q0
+        self.q0 = np.array([np.pi/4, np.pi/4, np.pi/4]) if q0 is None else q0
         # arm segment lengths
         self.L = np.array([1, 1, 1]) if L is None else L
 
-        self.max_angles = [math.pi/2.0, math.pi, math.pi/4]
-        self.min_angles = [0, 0, -math.pi/4]
+        self.max_angles = [np.pi, np.pi, np.pi/4]
+        self.min_angles = [0, 0, -np.pi/4]
 
     def get_xy(self, q=None):
         """Returns the corresponding hand xy coordinates for
@@ -170,8 +169,9 @@ def joint_limits_lower_constraint(q, xy):
             x0=self.q,
             eqcons=[x_constraint,
                     y_constraint],
-            ieqcons=[joint_limits_upper_constraint,
-                     joint_limits_lower_constraint],
+            # uncomment to add in min / max angles for the joints
+            # ieqcons=[joint_limits_upper_constraint,
+            #          joint_limits_lower_constraint],
             args=(xy,),
             iprint=0)  # iprint=0 suppresses output
 
@@ -183,7 +183,7 @@ def test():
 
     # set of desired (x,y) hand positions
     x = np.arange(-.75, .75, .05)
-    y = np.arange(0, .75, .05)
+    y = np.arange(.25, .75, .05)
 
     # threshold for printing out information, to find trouble spots
     thresh = .025
@@ -200,9 +200,9 @@ def test():
             # find the (x,y) position of the hand given these angles
             actual_xy = arm.get_xy(q)
             # calculate the root squared error
-            error = np.sqrt((np.array(xy) - np.array(actual_xy))**2)
+            error = np.sqrt(np.sum((np.array(xy) - np.array(actual_xy))**2))
             # total the error
-            total_error += error
+            total_error += np.nan_to_num(error)
 
             # if the error was high, print out more information
             if np.sum(error) > thresh:
@@ -220,3 +220,6 @@ def test():
     print('Total number of trials: ', count)
     print('Total error: ', total_error)
     print('-------------------------')
+
+if __name__ == '__main__':
+    test()

InvKin/ArmPlot.py

@@ -17,61 +17,66 @@
 
 import numpy as np
 import pyglet
-import time
 
 import Arm
 
-def plot(): 
-    """A function for plotting an arm, and having it calculate the 
-    inverse kinematics such that given the mouse (x, y) position it 
+
+def plot():
+    """A function for plotting an arm, and having it calculate the
+    inverse kinematics such that given the mouse (x, y) position it
     finds the appropriate joint angles to reach that point."""
 
     # create an instance of the arm
-    arm = Arm.Arm3Link(L = np.array([300,200,100]))
+    arm = Arm.Arm3Link(L=np.array([300, 200, 100]))
 
     # make our window for drawin'
     window = pyglet.window.Window()
 
-    label = pyglet.text.Label('Mouse (x,y)', font_name='Times New Roman', 
+    label = pyglet.text.Label(
+        'Mouse (x,y)', font_name='Times New Roman',
         font_size=36, x=window.width//2, y=window.height//2,
         anchor_x='center', anchor_y='center')
 
     def get_joint_positions():
         """This method finds the (x,y) coordinates of each joint"""
 
-        x = np.array([ 0, 
+        x = np.array([
+            0,
             arm.L[0]*np.cos(arm.q[0]),
             arm.L[0]*np.cos(arm.q[0]) + arm.L[1]*np.cos(arm.q[0]+arm.q[1]),
-            arm.L[0]*np.cos(arm.q[0]) + arm.L[1]*np.cos(arm.q[0]+arm.q[1]) + 
-                arm.L[2]*np.cos(np.sum(arm.q)) ]) + window.width/2
+            arm.L[0]*np.cos(arm.q[0]) + arm.L[1]*np.cos(arm.q[0]+arm.q[1]) +
+            arm.L[2]*np.cos(np.sum(arm.q))]) + window.width/2
 
-        y = np.array([ 0, 
+        y = np.array([
+            0,
             arm.L[0]*np.sin(arm.q[0]),
             arm.L[0]*np.sin(arm.q[0]) + arm.L[1]*np.sin(arm.q[0]+arm.q[1]),
-            arm.L[0]*np.sin(arm.q[0]) + arm.L[1]*np.sin(arm.q[0]+arm.q[1]) + 
-                arm.L[2]*np.sin(np.sum(arm.q)) ])
+            arm.L[0]*np.sin(arm.q[0]) + arm.L[1]*np.sin(arm.q[0]+arm.q[1]) +
+            arm.L[2]*np.sin(np.sum(arm.q))])
 
         return np.array([x, y]).astype('int')
-    
+
     window.jps = get_joint_positions()
 
     @window.event
     def on_draw():
         window.clear()
         label.draw()
-        for i in range(3): 
-            pyglet.graphics.draw(2, pyglet.gl.GL_LINES, ('v2i', 
-                (window.jps[0][i], window.jps[1][i], 
-                 window.jps[0][i+1], window.jps[1][i+1])))
+        for i in range(3):
+            pyglet.graphics.draw(
+                2,
+                pyglet.gl.GL_LINES,
+                ('v2i', (window.jps[0][i], window.jps[1][i],
+                         window.jps[0][i+1], window.jps[1][i+1])))
 
     @window.event
     def on_mouse_motion(x, y, dx, dy):
         # call the inverse kinematics function of the arm
-        # to find the joint angles optimal for pointing at 
-        # this position of the mouse 
-        label.text = '(x,y) = (%.3f, %.3f)'%(x,y)
-        arm.q = arm.inv_kin([x - window.width/2, y]) # get new arm angles
-        window.jps = get_joint_positions() # get new joint (x,y) positions
+        # to find the joint angles optimal for pointing at
+        # this position of the mouse
+        label.text = '(x,y) = (%.3f, %.3f)' % (x, y)
+        arm.q = arm.inv_kin([x - window.width/2, y])  # get new arm angles
+        window.jps = get_joint_positions()  # get new joint (x,y) positions
 
     pyglet.app.run()