Refactor

.atom-build.yml

@@ -0,0 +1 @@
+cmd: python PyIK

PyIK/evoarm.yaml

@@ -0,0 +1,37 @@
+name: EvoArm
+description: EvoArm configured with 2-axis upward-oriented "wrist"
+config:
+    # General configuration
+    elevator_length: 148.4
+    forearm_length: 160
+    linkage_length: 155
+    lower_actuator_length: 65
+    upper_actuator_length: 54.4
+    wrist_length: 90.52
+    shoulder_offset: [-9.7, 18.71]
+
+    servos:
+        -   name: swing
+            ratio: -1.0
+            offset: 150
+            limits: [60, 240]
+
+        -   name: elevator
+            ratio: -1.0
+            offset: 178.21
+            limits: [60, 210]
+
+        -   name: actuator
+            ratio: 1.0
+            offset: -204.78
+            limits: [100, 250]
+
+        -   name: wrist_x
+            ratio: -1.0
+            offset: 150
+            limits: [60, 240]
+
+        -   name: wrist_y
+            ratio: 1.0
+            offset: 147
+            limits: [60, 160]

PyIK/quick-ik.py

@@ -0,0 +1,47 @@
+import numpy as np
+from matplotlib import pyplot as plt
+
+from src.util import *
+from src.solvers import Circle
+from src.litearm import ArmController
+
+width = 300
+height = 600
+goal = np.zeros((width,height,2))
+for j in xrange(height):
+    for i in xrange(width):
+        goal[i,j]=[i+40, -j+height/2]
+
+origin=np.array([18.71,0])
+
+elevator_length = 148.4
+forearm_length = 160.0
+
+valid=np.ones((width,height), dtype=bool)
+centers = np.array(goal)
+diff = centers - origin
+dists = np.linalg.norm(diff, axis=2)
+
+# close enough for intersection
+valid &= dists < forearm_length + elevator_length
+valid &= dists > 0
+# intersect
+a = (forearm_length**2 - elevator_length**2 + dists**2) / (dists*2)
+h = np.sqrt(forearm_length**2 - a**2)
+p2 = centers + (np.dstack([a,a])*(origin - centers)) / np.dstack([dists,dists])
+i1 = np.array(p2)
+# [:, :, ::-1] flips x and y coords
+# dstack is a lazy way to get the scalar h/dists to multiply across the vectors
+i1 += [1,-1] * np.dstack([h,h]) * (origin - centers)[:,:,::-1] / np.dstack([dists,dists])
+i2 = np.array(p2)
+i2 += [-1,1] * np.dstack([h,h]) * (origin - centers)[:,:,::-1] / np.dstack([dists,dists])
+
+print('my dist={0}'.format(dists))
+print('my a={0}'.format(a))
+print('my h={0}'.format(h[0,0]))
+print('my p2={0}'.format(p2[0,0]))
+print('my i1={0}'.format(i1[0,0]))
+print(origin-centers)
+c1 = Circle(origin, elevator_length)
+c2 = Circle([250,40], forearm_length)
+c2.intersect(c1)

PyIK/src/PyIK.py

@@ -6,6 +6,7 @@
 import socket
 import select
 import struct
+import os
 
 import numpy as np
 import pygame as pyg
@@ -124,14 +125,17 @@ def __init__(self):
         pyg.display.set_caption("IK Control Test")
 
         # The arm's controller - encapsulates IK and arm control
+        dir_path = os.path.dirname(os.path.realpath(__file__))
+        arm_config = litearm.ArmConfig()
+        arm_config.loadConfigFile(dir_path + '/../evoarm.yaml')
         self.arm = litearm.ArmController(
             servo_swing = self.servos['swing'],
             servo_shoulder = self.servos['shoulder'],
             servo_elbow = self.servos['actuator'],
             servo_wrist_x = self.servos['wrist_x'],
             servo_wrist_y = self.servos['wrist_y'],
             # use the default config
-            arm_config = litearm.ArmConfig())
+            arm_config = arm_config)
 
         # Capacitive sensor on connected UNO
         try:
@@ -149,7 +153,7 @@ def __init__(self):
         if realPose is not None:
             self.curGoal = self.arm.getRealPose().effector
         else:
-            self.curGoal = np.array([0., 50., 200.])
+            self.curGoal = np.array([0., 40., 250.])
         self.ikTarget = np.array(self.curGoal)
         self.lastValidGoal = np.array(self.curGoal)
         self.curDir = [0.0, 0.0]
@@ -297,6 +301,12 @@ def tick(self):
                         self.arm.enableMovement(False)
                     else:
                         self.arm.enableMovement(True)
+                elif event.key == pyg.K_r:
+                    # Reset position
+                    pose = self.arm.getIKPose()
+                    pose.setServoActuator(150)
+                    pose.setServoElevator(150)
+                    #self.resetIKTarget()
 
         # Clear the render canvas
         self.r.surf.fill(white)
@@ -390,11 +400,11 @@ def drawViews(self, pose):
         text = "Elbow differential {0:.3f} deg".format(pose.armDiffAngle())
         self.r.drawText(text, blue if pose.checkDiff() else red, [40, 520])
 
-        text = "Elevator servo target {0:.3f} deg".format(pose.getServoElevator())
-        self.r.drawText(text, blue if pose.checkElevator() else red, [40, 540])
+        text = "Elevator servo target {0:.3f} deg".format(pose.getServoAngle('elevator'))
+        self.r.drawText(text, blue if pose.checkServoAngle('elevator') else red, [40, 540])
 
-        text = "Actuator servo target {0:.3f} deg".format(pose.getServoActuator())
-        self.r.drawText(text, blue if pose.checkActuator() else red, [40, 560])
+        text = "Actuator servo target {0:.3f} deg".format(pose.getServoAngle('actuator'))
+        self.r.drawText(text, blue if pose.checkServoAngle('actuator') else red, [40, 560])
 
         if pose.checkPositioning():
             self.r.drawText("Pose OK", blue, [40, 580])