├── .gitignore ├── Adafruit_I2C.py ├── Adafruit_PWM_Servo_Driver.py ├── DemoIK.py ├── LICENSE ├── README.md ├── kinematics.py └── meArm.py /.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | 5 | # C extensions 6 | *.so 7 | 8 | # Distribution / packaging 9 | .Python 10 | env/ 11 | bin/ 12 | build/ 13 | develop-eggs/ 14 | dist/ 15 | eggs/ 16 | lib/ 17 | lib64/ 18 | parts/ 19 | sdist/ 20 | var/ 21 | *.egg-info/ 22 | .installed.cfg 23 | *.egg 24 | 25 | # Installer logs 26 | pip-log.txt 27 | pip-delete-this-directory.txt 28 | 29 | # Unit test / coverage reports 30 | htmlcov/ 31 | .tox/ 32 | .coverage 33 | .cache 34 | nosetests.xml 35 | coverage.xml 36 | 37 | # Translations 38 | *.mo 39 | 40 | # Mr Developer 41 | .mr.developer.cfg 42 | .project 43 | .pydevproject 44 | 45 | # Rope 46 | .ropeproject 47 | 48 | # Django stuff: 49 | *.log 50 | *.pot 51 | 52 | # Sphinx documentation 53 | docs/_build/ 54 | 55 | -------------------------------------------------------------------------------- /Adafruit_I2C.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | import smbus 4 | 5 | # =========================================================================== 6 | # Adafruit_I2C Class 7 | # =========================================================================== 8 | 9 | class Adafruit_I2C : 10 | 11 | @staticmethod 12 | def getPiRevision(): 13 | "Gets the version number of the Raspberry Pi board" 14 | # Courtesy quick2wire-python-api 15 | # https://github.com/quick2wire/quick2wire-python-api 16 | try: 17 | with open('/proc/cpuinfo','r') as f: 18 | for line in f: 19 | if line.startswith('Revision'): 20 | return 1 if int(line.rstrip()[-4:]) < 3 else 2 21 | except: 22 | return 0 23 | 24 | @staticmethod 25 | def getPiI2CBusNumber(): 26 | # Gets the I2C bus number /dev/i2c# 27 | return 1 if Adafruit_I2C.getPiRevision() > 1 else 0 28 | 29 | def __init__(self, address, busnum=-1, debug=False): 30 | self.address = address 31 | # By default, the correct I2C bus is auto-detected using /proc/cpuinfo 32 | # Alternatively, you can hard-code the bus version below: 33 | # self.bus = smbus.SMBus(0); # Force I2C0 (early 256MB Pi's) 34 | # self.bus = smbus.SMBus(1); # Force I2C1 (512MB Pi's) 35 | self.bus = smbus.SMBus( 36 | busnum if busnum >= 0 else Adafruit_I2C.getPiI2CBusNumber()) 37 | self.debug = debug 38 | 39 | def reverseByteOrder(self, data): 40 | "Reverses the byte order of an int (16-bit) or long (32-bit) value" 41 | # Courtesy Vishal Sapre 42 | byteCount = len(hex(data)[2:].replace('L','')[::2]) 43 | val = 0 44 | for i in range(byteCount): 45 | val = (val << 8) | (data & 0xff) 46 | data >>= 8 47 | return val 48 | 49 | def errMsg(self): 50 | print "Error accessing 0x%02X: Check your I2C address" % self.address 51 | return -1 52 | 53 | def write8(self, reg, value): 54 | "Writes an 8-bit value to the specified register/address" 55 | try: 56 | self.bus.write_byte_data(self.address, reg, value) 57 | if self.debug: 58 | print "I2C: Wrote 0x%02X to register 0x%02X" % (value, reg) 59 | except IOError, err: 60 | return self.errMsg() 61 | 62 | def write16(self, reg, value): 63 | "Writes a 16-bit value to the specified register/address pair" 64 | try: 65 | self.bus.write_word_data(self.address, reg, value) 66 | if self.debug: 67 | print ("I2C: Wrote 0x%02X to register pair 0x%02X,0x%02X" % 68 | (value, reg, reg+1)) 69 | except IOError, err: 70 | return self.errMsg() 71 | 72 | def writeList(self, reg, list): 73 | "Writes an array of bytes using I2C format" 74 | try: 75 | if self.debug: 76 | print "I2C: Writing list to register 0x%02X:" % reg 77 | print list 78 | self.bus.write_i2c_block_data(self.address, reg, list) 79 | except IOError, err: 80 | return self.errMsg() 81 | 82 | def readList(self, reg, length): 83 | "Read a list of bytes from the I2C device" 84 | try: 85 | results = self.bus.read_i2c_block_data(self.address, reg, length) 86 | if self.debug: 87 | print ("I2C: Device 0x%02X returned the following from reg 0x%02X" % 88 | (self.address, reg)) 89 | print results 90 | return results 91 | except IOError, err: 92 | return self.errMsg() 93 | 94 | def readU8(self, reg): 95 | "Read an unsigned byte from the I2C device" 96 | try: 97 | result = self.bus.read_byte_data(self.address, reg) 98 | if self.debug: 99 | print ("I2C: Device 0x%02X returned 0x%02X from reg 0x%02X" % 100 | (self.address, result & 0xFF, reg)) 101 | return result 102 | except IOError, err: 103 | return self.errMsg() 104 | 105 | def readS8(self, reg): 106 | "Reads a signed byte from the I2C device" 107 | try: 108 | result = self.bus.read_byte_data(self.address, reg) 109 | if result > 127: result -= 256 110 | if self.debug: 111 | print ("I2C: Device 0x%02X returned 0x%02X from reg 0x%02X" % 112 | (self.address, result & 0xFF, reg)) 113 | return result 114 | except IOError, err: 115 | return self.errMsg() 116 | 117 | def readU16(self, reg): 118 | "Reads an unsigned 16-bit value from the I2C device" 119 | try: 120 | result = self.bus.read_word_data(self.address,reg) 121 | if (self.debug): 122 | print "I2C: Device 0x%02X returned 0x%04X from reg 0x%02X" % (self.address, result & 0xFFFF, reg) 123 | return result 124 | except IOError, err: 125 | return self.errMsg() 126 | 127 | def readS16(self, reg): 128 | "Reads a signed 16-bit value from the I2C device" 129 | try: 130 | result = self.bus.read_word_data(self.address,reg) 131 | if (self.debug): 132 | print "I2C: Device 0x%02X returned 0x%04X from reg 0x%02X" % (self.address, result & 0xFFFF, reg) 133 | return result 134 | except IOError, err: 135 | return self.errMsg() 136 | 137 | if __name__ == '__main__': 138 | try: 139 | bus = Adafruit_I2C(address=0) 140 | print "Default I2C bus is accessible" 141 | except: 142 | print "Error accessing default I2C bus" 143 | -------------------------------------------------------------------------------- /Adafruit_PWM_Servo_Driver.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | import time 4 | import math 5 | from Adafruit_I2C import Adafruit_I2C 6 | 7 | # ============================================================================ 8 | # Adafruit PCA9685 16-Channel PWM Servo Driver 9 | # ============================================================================ 10 | 11 | class PWM : 12 | i2c = None 13 | 14 | # Registers/etc. 15 | __SUBADR1 = 0x02 16 | __SUBADR2 = 0x03 17 | __SUBADR3 = 0x04 18 | __MODE1 = 0x00 19 | __PRESCALE = 0xFE 20 | __LED0_ON_L = 0x06 21 | __LED0_ON_H = 0x07 22 | __LED0_OFF_L = 0x08 23 | __LED0_OFF_H = 0x09 24 | __ALLLED_ON_L = 0xFA 25 | __ALLLED_ON_H = 0xFB 26 | __ALLLED_OFF_L = 0xFC 27 | __ALLLED_OFF_H = 0xFD 28 | 29 | def __init__(self, address=0x40, debug=False): 30 | self.i2c = Adafruit_I2C(address) 31 | self.address = address 32 | self.debug = debug 33 | if (self.debug): 34 | print "Reseting PCA9685" 35 | self.i2c.write8(self.__MODE1, 0x00) 36 | 37 | def setPWMFreq(self, freq): 38 | "Sets the PWM frequency" 39 | prescaleval = 25000000.0 # 25MHz 40 | prescaleval /= 4096.0 # 12-bit 41 | prescaleval /= float(freq) 42 | prescaleval -= 1.0 43 | if (self.debug): 44 | print "Setting PWM frequency to %d Hz" % freq 45 | print "Estimated pre-scale: %d" % prescaleval 46 | prescale = math.floor(prescaleval + 0.5) 47 | if (self.debug): 48 | print "Final pre-scale: %d" % prescale 49 | 50 | oldmode = self.i2c.readU8(self.__MODE1); 51 | newmode = (oldmode & 0x7F) | 0x10 # sleep 52 | self.i2c.write8(self.__MODE1, newmode) # go to sleep 53 | self.i2c.write8(self.__PRESCALE, int(math.floor(prescale))) 54 | self.i2c.write8(self.__MODE1, oldmode) 55 | time.sleep(0.005) 56 | self.i2c.write8(self.__MODE1, oldmode | 0x80) 57 | 58 | def setPWM(self, channel, on, off): 59 | "Sets a single PWM channel" 60 | self.i2c.write8(self.__LED0_ON_L+4*channel, on & 0xFF) 61 | self.i2c.write8(self.__LED0_ON_H+4*channel, on >> 8) 62 | self.i2c.write8(self.__LED0_OFF_L+4*channel, off & 0xFF) 63 | self.i2c.write8(self.__LED0_OFF_H+4*channel, off >> 8) 64 | 65 | 66 | 67 | 68 | -------------------------------------------------------------------------------- /DemoIK.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | # 4 | # DemoIK.py - York Hack Space May 2014 5 | # Simple demo of meArm library to walk through some points defined in Cartesian coordinates 6 | 7 | import meArm 8 | 9 | def main(): 10 | arm = meArm.meArm() 11 | arm.begin() 12 | 13 | while True: 14 | arm.openGripper() 15 | arm.closeGripper() 16 | arm.openGripper() 17 | arm.closeGripper() 18 | arm.openGripper() 19 | 20 | arm.gotoPoint(0, 150, 50) 21 | arm.gotoPoint(0, 150, 0) 22 | arm.gotoPoint(0, 150, 150) 23 | arm.gotoPoint(0, 150, 50) 24 | arm.gotoPoint(-150, 150, 50) 25 | arm.gotoPoint(150, 150, 50) 26 | arm.gotoPoint(0, 150, 50) 27 | arm.gotoPoint(0, 100, 50) 28 | 29 | if __name__ == '__main__': 30 | main() 31 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | The MIT License (MIT) 2 | 3 | Copyright (c) 2014 RorschachUK 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | meArmPi 2 | ======= 3 | 4 | Inverse Kinematics movement control library in Python for Phenoptix meArm on Raspberry Pi via Adafruit PWM Servo driver. 5 | 6 | The meArm has four mini servos - one for the gripper, and one each to rotate the base, shoulder joint and elbow joint. But it's not terribly convenient to be specifying things in terms of servo angles when you're much more interested in where you would like to place the gripper, in normal Cartesian (x, y, z) coordinates. 7 | 8 | This library solves the angles required to send to the servos in order to meet a given position, allowing for much simpler coding. 9 | 10 | Coordinates are (approximately) measured in mm from the base rotation centre. Initial 'home' position is at (0, 100, 50), i.e. 100mm forward of the base and 50mm off the ground. 11 | 12 | Various other versions of this library exist: 13 | * [Arduino](https://github.com/yorkhackspace/meArm) 14 | * [Arduino with Adafruit PWM driver board](https://github.com/RorschachUK/meArm_Adafruit) 15 | * [Beaglebone Black](https://github.com/RorschachUK/meArmBBB) 16 | 17 | [![meArm moving with Inverse Kinematics](http://img.youtube.com/vi/HbxhVs3UmuE/0.jpg)](http://www.youtube.com/watch?v=HbxhVs3UmuE) 18 | 19 | Wiring 20 | ------ 21 | 22 | This uses an Adafruit 16-channel PWM servo driver board to connect the servos to the Raspberry Pi. Use the first block of four servo connectors, and connect yellow wire to the top, brown wire to the bottom. 23 | * Servo 0: meArm rotating base 24 | * Servo 1: meArm shoulder (right hand side servo) 25 | * Servo 2: meArm elbow (left hand side servo) 26 | * Servo 3: meArm gripper 27 | 28 | Connect the Adafruit PWM Servo driver to the Pi as follows (I used Adafruit Pi Cobbler to help breadboard it): 29 | * Adafruit GND to RPi GND 30 | * Adafruit SCL to RPi SCL0 31 | * Adafruit SDA to RPi SDA0 32 | * Adafruit VCC to RPi 3.3V 33 | * Adafruit V+ to RPi 5V 34 | 35 | Usage 36 | ----- 37 | 38 | ``` 39 | import meArm 40 | 41 | def main(): 42 | arm = meArm.meArm() 43 | arm.begin() 44 | 45 | while True: 46 | arm.openGripper() 47 | arm.closeGripper() 48 | arm.openGripper() 49 | arm.closeGripper() 50 | arm.openGripper() 51 | 52 | #Go up and left to grab something 53 | arm.gotoPoint(-80,100,140) 54 | arm.closeGripper() 55 | #Go down, forward and right to drop it 56 | arm.gotoPoint(70,200,10) 57 | arm.openGripper() 58 | #Back to start position 59 | arm.gotoPoint(0,100,50) 60 | return 0 61 | 62 | if __name__ == '__main__': 63 | main() 64 | ``` 65 | 66 | One usage examples is included: 67 | * DemoIK follows a pre-programmed path defined in Cartesian coordinates 68 | 69 | Installation 70 | ------------ 71 | * Clone this repository to your local machine 72 | * Run with sudo, i.e. 'sudo python DemoIK.py' 73 | 74 | Class methods of meArm object 75 | ----------------------------- 76 | * begin(block=0, address=0x40) - determines which block of four servo connections to use (0 to 3) and which I2C address the Adafruit can be found on - defaults to 0x40. Begin must be called before any other calls to the meArm instance are made. 77 | * openGripper() - opens the gripper, letting go of anything it was holding 78 | * closeGripper() - closes the gripper, perhaps grabbing and holding something as it does so 79 | * gotoPoint(x, y, z) - move in a straight line from the current point to the requested position 80 | * goDirectlyTo(x, y, z) - set the servo angles to immediately go to the requested point without caring what path the arm swings through to get there - faster but less predictable than gotoPoint 81 | * isReachable() - returns true if the point can theoretically be reached by the arm 82 | * getPos() - current [x, y, z] coordinates 83 | -------------------------------------------------------------------------------- /kinematics.py: -------------------------------------------------------------------------------- 1 | # Inverse kinetics, adapted for Python by Bob Stone from C++ original by Nick Moriarty May 2014 2 | # Original is here: https://github.com/aquila12/me-arm-ik 3 | # 4 | # This code is provided under the terms of the MIT license. 5 | # 6 | # The MIT License (MIT) 7 | # 8 | # Copyright (c) 2014 Nick Moriarty 9 | # 10 | # Permission is hereby granted, free of charge, to any person obtaining a copy 11 | # of this software and associated documentation files (the "Software"), to deal 12 | # in the Software without restriction, including without limitation the rights 13 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 14 | # copies of the Software, and to permit persons to whom the Software is 15 | # furnished to do so, subject to the following conditions: 16 | # 17 | # The above copyright notice and this permission notice shall be included in 18 | # all copies or substantial portions of the Software. 19 | # 20 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 21 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 22 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 23 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 24 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 25 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 26 | # SOFTWARE. 27 | 28 | import math 29 | 30 | L1 = 80 # Shoulder to elbow length 31 | L2 = 80 # Elbow to wrist length 32 | L3 = 68 # Wrist to hand plus base centre to shoulder 33 | 34 | def cart2polar(x, y): 35 | # Determine magnitude of cartesian coordinates 36 | r = math.hypot(x, y) 37 | # Don't try to calculate zero magnitude vectors angles 38 | if r == 0: 39 | return 40 | 41 | c = x / r 42 | s = y / r 43 | 44 | # Safety! 45 | if s > 1: s = 1 46 | if c > 1: c = 1 47 | if s < -1: s = -1 48 | if c < -1: c = -1 49 | 50 | # Calculate angle in 0..PI 51 | theta = math.acos(c) 52 | 53 | # Convert to full range 54 | if s < 0: theta = -theta 55 | 56 | return r, theta 57 | 58 | # Get angle from triangle using cosine rule 59 | def cosangle(opp, adj1, adj2, theta): 60 | # Cosine rule: 61 | # C^2 = A^2 + B^2 - 2*A*B*cos(Angle_AB) 62 | # cos(Angle_AB) = (A^2 + B^2 - C^2)/(2*A*B) 63 | # C is opposite 64 | # A, B are adjacent 65 | 66 | den = 2 * adj1 * adj2 67 | 68 | if den == 0: 69 | return False 70 | c = (adj1 * adj1 + adj2 * adj2 - opp * opp)/den 71 | if c > 1 or c < -1: 72 | return False 73 | theta[0] = math.acos(c) 74 | return True 75 | 76 | # Solve angles 77 | def solve(x, y, z, angles): 78 | # Solve top-down view 79 | r, th0 = cart2polar(y, x) 80 | r -= L3 # Account for the wrist length 81 | 82 | # In arm plane, convert to polar 83 | R, ang_P = cart2polar(r, z) 84 | 85 | parmB = [0] 86 | parmC = [0] 87 | 88 | # Solve arm inner angles as required 89 | if not cosangle(L2, L1, R, parmB): return False 90 | if not cosangle(R, L1, L2, parmC): return False 91 | B = parmB[0] 92 | C = parmC[0] 93 | 94 | # Solve for servo angles from horizontal 95 | a0 = th0 96 | a1 = ang_P + B 97 | a2 = C + a1 - math.pi 98 | 99 | angles[0] = a0 100 | angles[1] = a1 101 | angles[2] = a2 102 | 103 | return True 104 | 105 | # Forward kinematics - for unsolving! 106 | 107 | def polar2cart(r, theta): 108 | a = r * math.cos(theta) 109 | b = r * sin(theta) 110 | return a,b 111 | 112 | def distance(x1, y1, z1, x2, y2, z2): 113 | dx = x2 - x1 114 | dy = y2 - y1 115 | dz = z2 - z1 116 | return math.sqrt(dx*dx + dy*dy + dz*dz) 117 | 118 | def unsolve(a0, a1, a2): 119 | # Calculate u,v coordinates for arm 120 | u01, v01 = polar2cart(L1, a1) 121 | u12, v12 = polar2cart(L2, a2) 122 | 123 | # Add vectors 124 | u = u01 + u12 + L3 125 | v = v01 + v12 126 | 127 | # Calculate in 3D space - note x/y reversal! 128 | y, x = polar2cart(u, a0) 129 | z = v 130 | return x, y, z 131 | -------------------------------------------------------------------------------- /meArm.py: -------------------------------------------------------------------------------- 1 | # meArm.py - York Hack Space May 2014 2 | # A motion control library for Phenoptix meArm using Adafruit 16-channel PWM servo driver 3 | 4 | from Adafruit_PWM_Servo_Driver import PWM 5 | import kinematics 6 | import time 7 | from math import pi 8 | 9 | class meArm(): 10 | def __init__(self, sweepMinBase = 145, sweepMaxBase = 49, angleMinBase = -pi/4, angleMaxBase = pi/4, 11 | sweepMinShoulder = 118, sweepMaxShoulder = 22, angleMinShoulder = pi/4, angleMaxShoulder = 3*pi/4, 12 | sweepMinElbow = 144, sweepMaxElbow = 36, angleMinElbow = pi/4, angleMaxElbow = -pi/4, 13 | sweepMinGripper = 75, sweepMaxGripper = 115, angleMinGripper = pi/2, angleMaxGripper = 0): 14 | """Constructor for meArm - can use as default arm=meArm(), or supply calibration data for servos.""" 15 | self.servoInfo = {} 16 | self.servoInfo["base"] = self.setupServo(sweepMinBase, sweepMaxBase, angleMinBase, angleMaxBase) 17 | self.servoInfo["shoulder"] = self.setupServo(sweepMinShoulder, sweepMaxShoulder, angleMinShoulder, angleMaxShoulder) 18 | self.servoInfo["elbow"] = self.setupServo(sweepMinElbow, sweepMaxElbow, angleMinElbow, angleMaxElbow) 19 | self.servoInfo["gripper"] = self.setupServo(sweepMinGripper, sweepMaxGripper, angleMinGripper, angleMaxGripper) 20 | 21 | # Adafruit servo driver has four 'blocks' of four servo connectors, 0, 1, 2 or 3. 22 | def begin(self, block = 0, address = 0x40): 23 | """Call begin() before any other meArm calls. Optional parameters to select a different block of servo connectors or different I2C address.""" 24 | self.pwm = PWM(address) # Address of Adafruit PWM servo driver 25 | self.base = block * 4 26 | self.shoulder = block * 4 + 1 27 | self.elbow = block * 4 + 2 28 | self.gripper = block * 4 + 3 29 | self.pwm.setPWMFreq(60) 30 | self.openGripper() 31 | self.goDirectlyTo(0, 100, 50) 32 | 33 | def setupServo(self, n_min, n_max, a_min, a_max): 34 | """Calculate servo calibration record to place in self.servoInfo""" 35 | rec = {} 36 | n_range = n_max - n_min 37 | a_range = a_max - a_min 38 | if a_range == 0: return 39 | gain = n_range / a_range 40 | zero = n_min - gain * a_min 41 | rec["gain"] = gain 42 | rec["zero"] = zero 43 | rec["min"] = n_min 44 | rec["max"] = n_max 45 | return rec 46 | 47 | def angle2pwm(self, servo, angle): 48 | """Work out pulse length to use to achieve a given requested angle taking into account stored calibration data""" 49 | ret = 150 + int(0.5 + (self.servoInfo[servo]["zero"] + self.servoInfo[servo]["gain"] * angle) * 450 / 180) 50 | return ret 51 | 52 | def goDirectlyTo(self, x, y, z): 53 | """Set servo angles so as to place the gripper at a given Cartesian point as quickly as possible, without caring what path it takes to get there""" 54 | angles = [0,0,0] 55 | if kinematics.solve(x, y, z, angles): 56 | radBase = angles[0] 57 | radShoulder = angles[1] 58 | radElbow = angles[2] 59 | self.pwm.setPWM(self.base, 0, self.angle2pwm("base", radBase)) 60 | self.pwm.setPWM(self.shoulder, 0, self.angle2pwm("shoulder", radShoulder)) 61 | self.pwm.setPWM(self.elbow, 0, self.angle2pwm("elbow", radElbow)) 62 | self.x = x 63 | self.y = y 64 | self.z = z 65 | print "goto %s" % ([x,y,z]) 66 | 67 | def gotoPoint(self, x, y, z): 68 | """Travel in a straight line from current position to a requested position""" 69 | x0 = self.x 70 | y0 = self.y 71 | z0 = self.z 72 | dist = kinematics.distance(x0, y0, z0, x, y, z) 73 | step = 10 74 | i = 0 75 | while i < dist: 76 | self.goDirectlyTo(x0 + (x - x0) * i / dist, y0 + (y - y0) * i / dist, z0 + (z - z0) * i / dist) 77 | time.sleep(0.05) 78 | i += step 79 | self.goDirectlyTo(x, y, z) 80 | time.sleep(0.05) 81 | 82 | def openGripper(self): 83 | """Open the gripper, dropping whatever is being carried""" 84 | self.pwm.setPWM(self.gripper, 0, self.angle2pwm("gripper", pi/4.0)) 85 | time.sleep(0.3) 86 | 87 | def closeGripper(self): 88 | """Close the gripper, grabbing onto anything that might be there""" 89 | self.pwm.setPWM(self.gripper, 0, self.angle2pwm("gripper", -pi/4.0)) 90 | time.sleep(0.3) 91 | 92 | def isReachable(self, x, y, z): 93 | """Returns True if the point is (theoretically) reachable by the gripper""" 94 | radBase = 0 95 | radShoulder = 0 96 | radElbow = 0 97 | return kinematics.solve(x, y, z, radBase, radShoulder, radElbow) 98 | 99 | def getPos(self): 100 | """Returns the current position of the gripper""" 101 | return [self.x, self.y, self.z] 102 | --------------------------------------------------------------------------------