From 1d5ae475d87b2aa27a9c1d7ff5d55a51eb918240 Mon Sep 17 00:00:00 2001 From: Alistair Date: Sun, 5 Feb 2017 23:10:26 -0800 Subject: [PATCH 1/2] Move config to file ... ... and encapsulated the angle to servo position calculations as well as limit checks for each servo. Configuration for these settings is now in the evoarm.yaml config file. In theory this allows for use of externally geared servos - the Dynamixels are fast but not very powerful or accurate, so a possible plan is to reduce the main servos by 2:1 to use their full range of motion with twice the positional accuracy and power. --- PyIK/evoarm.yaml | 37 +++++++++ PyIK/src/PyIK.py | 20 +++-- PyIK/src/litearm.py | 191 +++++++++++++++++++++----------------------- PyIK/src/views.py | 14 ++-- 4 files changed, 149 insertions(+), 113 deletions(-) create mode 100644 PyIK/evoarm.yaml diff --git a/PyIK/evoarm.yaml b/PyIK/evoarm.yaml new file mode 100644 index 0000000..cdaf57d --- /dev/null +++ b/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] diff --git a/PyIK/src/PyIK.py b/PyIK/src/PyIK.py index 84b9c6a..56596c3 100644 --- a/PyIK/src/PyIK.py +++ b/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,6 +125,9 @@ 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'], @@ -131,7 +135,7 @@ def __init__(self): 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: @@ -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]) diff --git a/PyIK/src/litearm.py b/PyIK/src/litearm.py index 603e1db..2cd33c3 100644 --- a/PyIK/src/litearm.py +++ b/PyIK/src/litearm.py @@ -3,6 +3,8 @@ import numpy as np import struct +import yaml + import solvers import pid from util import * @@ -13,28 +15,50 @@ ERRORLIM = 5.0 +class ServoMapping: + """Given a directional ratio and offset, maps between a servo position and an arm pose angle.""" + def __init__(self, ratio = 1.0, offset = 0.0, limits = [0, 300]): + self.ratio = ratio + self.offset = offset + self.limits = limits + + def fromServo(self, servo_angle): + return servo_angle * self.ratio + self.offset + + def toServo(self, arm_angle): + return (arm_angle - self.offset) / self.ratio + class ArmConfig: - """Holds an arm's proportions, limits and other configuration data""" - def __init__(self, - main_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]): - self.main_length = main_length - self.forearm_length = forearm_length - self.linkage_length = linkage_length - self.lower_actuator_length = lower_actuator_length - self.upper_actuator_length = upper_actuator_length - self.wrist_length = wrist_length; - self.shoulder_offset = shoulder_offset + """Loads and stores an arm's proportions, limits and other configuration data""" + def __init__(self): + pass + + def loadConfigFile(self, config_file): + file = open(config_file) + config = yaml.load(file.read()) + file.close() + self.loadConfigData(config) + + def loadConfigData(self, config): + self.name = config['name'] + + arm = config['config'] + self.main_length = arm['elevator_length'] + self.forearm_length = arm['forearm_length'] + self.linkage_length = arm['linkage_length'] + self.lower_actuator_length = arm['lower_actuator_length'] + self.upper_actuator_length = arm['upper_actuator_length'] + self.wrist_length = arm['wrist_length'] + self.shoulder_offset = arm['shoulder_offset'] + self.servos = {} + for s in arm['servos']: + mapping = ServoMapping(s['ratio'], s['offset'], s['limits']) + self.servos[s['name']] = mapping class ArmPose: """ - Defines a physical configuration of a LiteArm robot arm. + Defines a physical configuration of a LiteArm-style robot arm. Internal angles are relative to vertical (elevator/actuator) or straight forward (swing), and are stored in radians. Extracted servo angles range 0-300 and are measured in degrees. @@ -45,30 +69,10 @@ class ArmPose: """ structFormat = 'fffff' - @staticmethod - def calcElevatorAngle(servoAngle): - return radians(178.21 - servoAngle) - - @staticmethod - def calcSwingAngle(servoAngle): - return radians(150.0 - servoAngle) - - @staticmethod - def calcActuatorAngle(servoAngle): - return radians(servoAngle - 204.78) - - @staticmethod - def calcWristXAngle(servoAngle): - return radians(150.0 - servoAngle) - - @staticmethod - def calcWristYAngle(servoAngle): - return radians(servoAngle - 147.0) - def __init__(self, arm_config, swing_angle, - shoulder_angle, + elevator_angle, actuator_angle, elbow_angle, elbow2D, @@ -78,9 +82,10 @@ def __init__(self, wrist_x, wrist_y): self.cfg = arm_config - self.swing_angle = swing_angle - self.shoulder_angle = shoulder_angle - self.actuator_angle = actuator_angle + self.angles = {} + self.angles['swing'] = swing_angle + self.angles['elevator'] = elevator_angle + self.angles['actuator'] = actuator_angle self.elbow_angle = elbow_angle # Joints in the arm shoulder = rotate(self.cfg.shoulder_offset, swing_angle) @@ -98,55 +103,43 @@ def __init__(self, self.elbow[1] = elbow2D[1] self.wrist = self.effector - normalize(arm_vec)*arm_config.wrist_length # Wrist pose - self.wristXAngle = wrist_x - self.wristYAngle = wrist_y - - def getServoElevator(self): - return 178.21 - degrees(self.shoulder_angle) - - def getServoActuator(self): - return degrees(self.actuator_angle) + 204.78 + self.angles['wrist_x'] = wrist_x + self.angles['wrist_y'] = wrist_y - def getServoSwing(self): - return 150 - degrees(self.swing_angle) + def getServoAngle(self, servo_name): + angle = degrees(self.angles[servo_name]) + return self.cfg.servos[servo_name].toServo(angle) - def getServoWristX(self): - return 150 - degrees(self.wristXAngle) - - def getServoWristY(self): - return 147 + degrees(self.wristYAngle) + def setServoAngle(self, servo_name, angle): + adjusted = self.cfg.servos[servo_name].fromServo(angle) + self.angles[servo_name] = radians(adjusted) def armDiffAngle(self): - return degrees(self.shoulder_angle - self.actuator_angle) - - def checkActuator(self): - angle = self.getServoActuator() - return angle >= 95 and angle <= 250 + return degrees(self.angles['elevator'] - self.angles['actuator']) + + def checkServoAngle(self, servo_name): + servo = self.cfg.servos[servo_name] + angle = self.getServoAngle(servo_name) + return angle >= servo.limits[0] and angle <= servo.limits[1] + + def checkServoAngles(self): + """Checks that the servo angles for the pose are within defined limits; SKIPS WRIST SERVOS!""" + valid = True + for name in self.cfg.servos.keys(): + if name in ('wrist_x', 'wrist_y'): + continue + valid = valid and self.checkServoAngle(name) + return valid + #return True def checkDiff(self): angle = self.armDiffAngle() return angle >= 44 and angle <= 175 - def checkElevator(self): - angle = self.getServoElevator() - return angle >= 60 and angle <= 210 - def checkForearm(self): - angle = degrees(self.elbow_angle + self.shoulder_angle) + angle = degrees(self.elbow_angle + self.angles['elevator']) return angle < 200 and angle > 80 - def checkSwing(self): - angle = self.getServoSwing() - return angle >= 60 and angle <= 240 - - def checkWristX(self): - angle = self.getServoWristX() - return angle >= 60 and angle <= 240 - - def checkWristY(self): - angle = self.getServoWristY() - return angle >= 60 and angle <= 160 - def checkPositioning(self): # When Y>0 Forearm always faces outwards if self.wrist2D[1] > 0 and self.wrist2D[0] < self.elbow2D[0]: @@ -160,20 +153,18 @@ def checkPositioning(self): return True def checkClearance(self): - return (self.checkDiff() and self.checkActuator() and - self.checkElevator() and self.checkSwing() and - self.checkWristX() and self.checkWristY() and + return (self.checkDiff() and self.checkServoAngles() and self.checkPositioning() and self.checkForearm()) def serialize(self): """Returns a packed struct holding the pose information""" return struct.pack( ArmPose.structFormat, - self.swing_angle, - self.shoulder_angle, + self.angles['swing'], + self.angles['elevator'], self.elbow_angle, - self.wristXAngle, - self.wristYAngle + self.angles['wrist_x'], + self.angles['wrist_y'] ) class ArmController: @@ -246,16 +237,9 @@ def enableMovement(self, enable): changed = True if changed: # Set servos on/off - if self.servos['swing'] is not None: - self.servos['swing'].setTorqueEnable(self.motion_enable) - if self.servos['shoulder'] is not None: - self.servos['shoulder'].setTorqueEnable(self.motion_enable) - if self.servos['elbow'] is not None: - self.servos['elbow'].setTorqueEnable(self.motion_enable) - if self.servos['wrist_x'] is not None: - self.servos['wrist_x'].setTorqueEnable(self.motion_enable) - if self.servos['wrist_y'] is not None: - self.servos['wrist_y'].setTorqueEnable(self.motion_enable) + for name, servo in self.servos.items(): + if servo is not None: + servo.setTorqueEnable(self.motion_enable) def setWristGoalPosition(self, pos): self.ik.setGoal(pos) @@ -281,7 +265,7 @@ def getIKPose(self): self.cfg, swing_angle = self.ik.swing, # angles from vertical - shoulder_angle = arm_vert_angle, + elevator_angle = arm_vert_angle, actuator_angle = actuator_angle, # angle between the main arm and forearm elbow_angle = elbow_angle, @@ -324,11 +308,16 @@ def getRealPose(self): wrist_y_servo = self.servos['wrist_y'].data['pos'] # Find the internal arm-pose angles for the given servo positions - swing_angle = ArmPose.calcSwingAngle(swing_servo) - elevator_angle = ArmPose.calcElevatorAngle(elevator_servo) - actuator_angle = ArmPose.calcActuatorAngle(actuator_servo) - wrist_x_angle = ArmPose.calcWristXAngle(wrist_x_servo) - wrist_y_angle = ArmPose.calcWristYAngle(wrist_y_servo) + swing_angle = self.cfg.servos['swing'].fromServo(swing_servo) + swing_angle = radians(swing_angle) + elevator_angle = self.cfg.servos['elevator'].fromServo(elevator_servo) + elevator_angle = radians(elevator_angle) + actuator_angle = self.cfg.servos['actuator'].fromServo(actuator_servo) + actuator_angle = radians(actuator_angle) + wrist_x_angle = self.cfg.servos['wrist_x'].fromServo(wrist_x_servo) + wrist_x_angle = radians(wrist_x_angle) + wrist_y_angle = self.cfg.servos['wrist_y'].fromServo(wrist_y_servo) + wrist_y_angle = radians(wrist_y_angle) # Solve elbow angle for given actuator and elevator angles # (this is the angle from the elevator arm's direction to the forearm's) elbow_angle = self.physsolver.solve_forearm(elevator_angle, actuator_angle) diff --git a/PyIK/src/views.py b/PyIK/src/views.py index 5bb483f..877859b 100644 --- a/PyIK/src/views.py +++ b/PyIK/src/views.py @@ -73,11 +73,11 @@ def draw(self, pose, r): r.drawLine(pt_l([0,0]), pt_l([0,150]), green) r.drawText('y', green, pt_l([-10,170])) # Z axis from side - zvec = rotate([0,1], pose.swing_angle) + zvec = rotate([0,1], pose.angles['swing']) r.drawLine(pt_l([0,0]), pt_l([zvec[1]*150,0]), blue) r.drawText('z', blue, pt_l([zvec[1]*150,0])) # X axis from side - xvec = rotate([-1,0], pose.swing_angle) + xvec = rotate([-1,0], pose.angles['swing']) r.drawLine(pt_l([0,0]), pt_l([xvec[1]*150,0]), red) r.drawText('x', red, pt_l([xvec[1]*150,0])) @@ -89,7 +89,7 @@ def draw(self, pose, r): fore_arm = pose.wrist2D - pose.elbow2D effector = pose.effector2D - pose.wrist2D - vec = rotate(vertical, pose.actuator_angle) + vec = rotate(vertical, pose.angles['actuator']) actuator = vec*pose.cfg.lower_actuator_length upper_actuator = -normalize(fore_arm)*pose.cfg.upper_actuator_length @@ -121,7 +121,7 @@ def draw(self, pose, r): r.drawCircle(pt_l(pose.effector2D), self.line_width, col) # Wrist - wrist_vec = rotate(horizontal, pose.wristYAngle) * 20 + wrist_vec = rotate(horizontal, pose.angles['wrist_y']) * 20 r.drawLine(pt_l(pose.effector2D), pt_l(pose.effector2D + wrist_vec), col) # show 2D effector position @@ -136,7 +136,7 @@ def draw(self, pose, r): x = 50 else: x = 200 - vert_angle = degrees(pose.shoulder_angle) + vert_angle = degrees(pose.angles['elevator']) text = "Main arm {0:.2f} deg".format(vert_angle) r.drawText(text, black, [x, 40]) @@ -176,7 +176,7 @@ def draw(self, pose, r): offset = 600 else: offset = 750 - swing = degrees(pose.swing_angle) + swing = degrees(pose.angles['swing']) text = "Swing {0:.2f} deg".format(swing) r.drawText(text, self.color, [offset, 20]) @@ -208,7 +208,7 @@ def draw(self, pose, r): r.drawCircle(pt_r(elbow), self.line_width, gray) # Wrist joint - wrist_vec = rotate(vertical, pose.swing_angle + pose.wristXAngle) * 20 + wrist_vec = rotate(vertical, pose.angles['swing'] + pose.angles['wrist_x']) * 20 r.drawLine(pt_r(effector), pt_r(effector + wrist_vec), col) # show top-down effector position From 2ec114a0cfeb9cdc08edfb52c18bc45f6f3c8129 Mon Sep 17 00:00:00 2001 From: Alistair Date: Mon, 13 Mar 2017 17:48:28 -0700 Subject: [PATCH 2/2] Testing fast IK --- .atom-build.yml | 1 + PyIK/quick-ik.py | 47 +++++++++++++++++++++++++++++++++++++++++++++++ PyIK/src/PyIK.py | 2 +- 3 files changed, 49 insertions(+), 1 deletion(-) create mode 100644 .atom-build.yml create mode 100644 PyIK/quick-ik.py diff --git a/.atom-build.yml b/.atom-build.yml new file mode 100644 index 0000000..5376c4e --- /dev/null +++ b/.atom-build.yml @@ -0,0 +1 @@ +cmd: python PyIK diff --git a/PyIK/quick-ik.py b/PyIK/quick-ik.py new file mode 100644 index 0000000..77d26ce --- /dev/null +++ b/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) diff --git a/PyIK/src/PyIK.py b/PyIK/src/PyIK.py index 56596c3..d08327b 100644 --- a/PyIK/src/PyIK.py +++ b/PyIK/src/PyIK.py @@ -153,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]