├── .github └── workflows │ └── python.yaml ├── .gitignore ├── LICENSE ├── dev └── hex │ ├── params.py │ ├── robot.py │ └── test.py ├── docs ├── 6360dff3ca1b8f5eafb436bc8855ff49250d.pdf ├── 99d15da9cde98069e24b3dc07218d472dc42.pdf ├── gait │ └── pics │ │ ├── 1-copvyx36cnhwyrvculwn-g1.gif │ │ ├── robotics-05-00013-g009.png │ │ └── walking-2-legs.gif ├── index.html ├── misc │ ├── readme.md │ └── walking-robots-pdfs │ │ ├── Spider robot kinematics.pdf │ │ ├── equations-for-hexapod-robot-control.pdf │ │ ├── gait.pdf │ │ ├── quadruped.pdf │ │ ├── spider-robot-kinematics.pdf │ │ └── spong_kinematics.pdf ├── multiped │ ├── 1-Figure1-1.png │ ├── 2-Figure2-1.png │ ├── 200px-AZIMUTMulti.jpg │ ├── 3-Figure1-1.png │ ├── 9-Figure9-1.png │ ├── AZIMUT-hybrid-robot.png │ ├── img_1182.jpg │ └── robot18.jpg ├── ncomms14494.pdf ├── quad-4-links.png ├── readme.md └── spider.png ├── multiped ├── __init__.py ├── bin │ ├── __init__.py │ ├── get_leg_angles.py │ └── get_leg_info.py ├── engine.py ├── gait.py ├── jsonFile.py ├── kinematics3.py ├── kinematics4.py ├── robot.py └── servo.py ├── pics ├── ax-12a-spider.jpg ├── quad-movement.png ├── rc-spider.jpg └── xl-320-spider.jpg ├── pyproject.toml ├── readme.md └── tests ├── test_all.py ├── test_leg.py └── test_servo.py /.github/workflows/python.yaml: -------------------------------------------------------------------------------- 1 | name: CheckPackage 2 | on: [push] 3 | 4 | jobs: 5 | build: 6 | runs-on: ubuntu-latest 7 | strategy: 8 | max-parallel: 5 9 | matrix: 10 | python-version: [3.6, 3.7, 3.8, 3.9] 11 | steps: 12 | - uses: actions/checkout@master 13 | - name: Setup Python ${{ matrix.python-version }} 14 | uses: actions/setup-python@v1 15 | with: 16 | python-version: ${{ matrix.python-version }} # Version range or exact version of a Python version to use, using semvers version range syntax. 17 | # architecture: 'x64' # (x64 or x86) 18 | # - name: Set up Python ${{ matrix.python-version }} 19 | # uses: actions/setup-python@v1 20 | # with: 21 | # python-version: ${{ matrix.python-version }} 22 | # - name: Install dependencies 23 | # run: | 24 | # python -m pip install --upgrade pip setuptools wheel build_utils 25 | # - name: Lint with flake8 26 | # run: | 27 | # pip install flake8 28 | # # stop the build if there are Python syntax errors or undefined names 29 | # flake8 . --count --select=E9,F63,F7,F82 --show-source --statistics 30 | # # exit-zero treats all errors as warnings. The GitHub editor is 127 chars wide 31 | # flake8 . --count --exit-zero --max-complexity=10 --max-line-length=127 --statistics 32 | - name: Install Poetry 33 | uses: dschep/install-poetry-action@v1.3 34 | # with: 35 | # version: 1.0.0b3 36 | - name: Turn off Virtualenvs 37 | run: poetry config virtualenvs.create false 38 | - name: Install packages 39 | run: poetry install 40 | - name: Run PyTest 41 | run: poetry run pytest 42 | # - name: Run mypy 43 | # run: poetry run mypy pyservos 44 | # - name: nosetests 45 | # run: | 46 | # pip install nose 47 | # cd python 48 | # python -m pip install -e . 49 | # cd tests 50 | # nosetests -vs test.py 51 | # - name: Python Style Checker 52 | # uses: andymckay/pycodestyle-action@0.1.3 53 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | Thumbs.db 2 | *.pyc 3 | old 4 | __pycache__ 5 | .AppleDouble 6 | .DS_Store 7 | ._* 8 | .ipynb_checkpoints 9 | build 10 | dist 11 | *.egg-info 12 | .eggs 13 | .mypy_cache 14 | .pytest_cache 15 | old 16 | poetry.lock 17 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | The MIT License (MIT) 2 | Copyright (c) 2016 Kevin J. Walchko 3 | 4 | Permission is hereby granted, free of charge, to any person obtaining a copy of 5 | this software and associated documentation files (the "Software"), to deal in 6 | the Software without restriction, including without limitation the rights to 7 | use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies 8 | of the Software, and to permit persons to whom the Software is furnished to do 9 | so, subject to the following conditions: 10 | 11 | The above copyright notice and this permission notice shall be included in all 12 | copies or substantial portions of the Software. 13 | 14 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 16 | FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 17 | COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 18 | IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 19 | CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 20 | -------------------------------------------------------------------------------- /dev/hex/params.py: -------------------------------------------------------------------------------- 1 | 2 | from math import pi 3 | 4 | spider = { 5 | "mdh": [ 6 | {"alpha": 0, "a": 0, "theta": 0, "d": 0, "type": 1}, 7 | {"alpha": pi/2, "a": 52, "theta": 0, "d": 0, "type": 1}, 8 | {"alpha": 0, "a": 89, "theta": 0, "d": 0, "type": 1}, 9 | {"alpha": 0, "a": 90, "theta": 0, "d": 0, "type": 1} 10 | ], 11 | # feet are arranged CCW, only spec (x,y) 12 | "neutral": [ 13 | [100, 0], # foot 1 14 | [100, 0], 15 | [100, 0], 16 | [100, 0], 17 | [100, 0], 18 | [100, 0] # foot 6 19 | ], 20 | "forward": [ 21 | [] 22 | ], 23 | "back": [ 24 | [] 25 | ], 26 | "height": 50, # height above the floor, so this will be negative 27 | } 28 | -------------------------------------------------------------------------------- /dev/hex/robot.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | from mdh.link import mdh_params, JointType 4 | from mdh.kinematic_chain import KinematicChain 5 | from numpy import linspace 6 | from math import pi, sin, cos 7 | from collections import deque 8 | 9 | # from pyservos import AX12 10 | 11 | class Tripod: 12 | def __init__(self, neutral, stride, count): 13 | self.gait = self.build_forward(neutral, stride, count) 14 | self.orientations = [-30, -90, -120, 120, 90, 30] 15 | self. 16 | 17 | def build_stride(self, n, dist, steps): 18 | """ 19 | Build a complete stride that starts at the origin (0,0,0) and ends 20 | there. 21 | 22 | Neutral: foot positions, [(x1,y1), (x2,y2), ...] where 1=Leg1, 2=Leg2, ... 23 | Distance [mm]: complete length the foot moves 24 | Steps: how many steps to take 25 | Returns: (phase(0), phase(90)) 26 | """ 27 | if steps % 4 != 0: 28 | raise Exception(f"Invalide number of steps: {steps}") 29 | half = steps//2 # half of teh steps to move to rear position 30 | mid = dist/2 # half of the steps to move forward to start position 31 | stride = [] 32 | step = 2*mid/half # incremental distance of each step [mm] 33 | 34 | for i in range(half): 35 | stride.append([0,mid-step*i, 0]) 36 | for i in range(half): 37 | a = pi - pi*i/half 38 | stride.append([0,(mid)*cos(a), mid*sin(a)]) 39 | 40 | # now shift the step pattern to where we want and produce 41 | # 2 sets starting at the origin, but one is on the ground and the 42 | # other is raised up in the air 43 | s = deque(stride) 44 | s.rotate(half//2) # shift to start at origin (0,0,0) 45 | ss = deque(s) 46 | ss.rotate(half) # shift to start at origin (0,0,range) 47 | 48 | return (s, ss,) 49 | 50 | def build_forward(self, neutral, stride, steps): 51 | s, ss = self.build_stride(neutral, stride, steps) 52 | g = [] 53 | for i, p in enumerate(self.orientations): 54 | st = s if i%2 else ss 55 | g.append(self.rotate_z(st, p, neutral[i])) 56 | return g 57 | 58 | def rotate_z(self, stride, angle, offset): 59 | ret = [] 60 | for s in stride: 61 | # print(s, offset) 62 | x,y,z = s 63 | xx, yy = offset 64 | a = angle*pi/180 65 | r = [ 66 | x*cos(a)-y*sin(a)+xx, 67 | x*sin(a) + y*cos(a)+yy, 68 | z 69 | ] 70 | ret.append(r) 71 | return ret 72 | 73 | def gait_world(self): 74 | ret = [] 75 | for i in range(6): 76 | 77 | return ret 78 | 79 | def __getitem__(self, key): 80 | if isinstance(key, int) and (0 < key < 6): 81 | return self.gait[key] 82 | # elif isinstance(key, tuple): 83 | # return self.gait[] 84 | else: 85 | raise Exception(f"Gait, invalide key: {key}") 86 | 87 | 88 | class Robot: 89 | def __init__(self, params): 90 | self.kinematicChain = KinematicChain.from_parameters(params["mdh"]) 91 | 92 | # interpolate walk, forward/back, using neutral, forward, back steps 93 | # and interpolating between them 94 | n = params["neutral"] 95 | fw = params["forward"] 96 | bk = params["back"] 97 | h = params["height"] 98 | self.gait = Tripod(h, n, fw, bk) 99 | 100 | num_legs = len(n) 101 | num_servos = len(self.kinematicChain) 102 | self.engine = Engine(num_legs, num_servos, AX12) 103 | 104 | def forward(self): 105 | pass 106 | 107 | def backwards(self): 108 | pass 109 | 110 | def turn(self, dir): 111 | pass 112 | 113 | def z(self, altitude): 114 | # altitude: 0 = sit, >0 = stand 115 | pass 116 | -------------------------------------------------------------------------------- /dev/hex/test.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | from robot import Tripod 4 | from params import spider 5 | from pprint import pprint 6 | import matplotlib.pyplot as plt 7 | from mpl_toolkits.mplot3d import Axes3D 8 | import numpy as np 9 | 10 | fig = plt.figure() 11 | ax = fig.add_subplot(111, projection='3d') 12 | 13 | t = Tripod(spider["neutral"], 25, 20) 14 | 15 | # pprint(t.gait) 16 | gait = t.gait 17 | 18 | m = 'h' 19 | for leg, c, i in zip(gait, ['b','g','r','c','m','y'],[1,2,3,4,5,6]): 20 | x = [n[0] for n in leg] + [leg[0][0]] 21 | y = [n[1] for n in leg] + [leg[0][1]] 22 | z = [n[2] for n in leg] + [leg[0][2]] 23 | ax.plot(x[:1],y[:1],z[:1],marker="h",c="k",markersize=15) 24 | ax.plot(x,y,z,"-h",c=c,label=f"Leg {i}: {len(x)} pts") 25 | 26 | ax.set_xlabel('X [mm]') 27 | ax.set_ylabel('Y [mm]') 28 | ax.set_zlabel('Z [mm]') 29 | ax.legend() 30 | # ax.set_aspect('equal', 'box') 31 | 32 | plt.show() 33 | -------------------------------------------------------------------------------- /docs/6360dff3ca1b8f5eafb436bc8855ff49250d.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MultipedRobotics/multiped/17c2023e577cb6593b3f5a5fcbd5a9a57a5da46a/docs/6360dff3ca1b8f5eafb436bc8855ff49250d.pdf -------------------------------------------------------------------------------- /docs/99d15da9cde98069e24b3dc07218d472dc42.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MultipedRobotics/multiped/17c2023e577cb6593b3f5a5fcbd5a9a57a5da46a/docs/99d15da9cde98069e24b3dc07218d472dc42.pdf -------------------------------------------------------------------------------- /docs/gait/pics/1-copvyx36cnhwyrvculwn-g1.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MultipedRobotics/multiped/17c2023e577cb6593b3f5a5fcbd5a9a57a5da46a/docs/gait/pics/1-copvyx36cnhwyrvculwn-g1.gif -------------------------------------------------------------------------------- /docs/gait/pics/robotics-05-00013-g009.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MultipedRobotics/multiped/17c2023e577cb6593b3f5a5fcbd5a9a57a5da46a/docs/gait/pics/robotics-05-00013-g009.png -------------------------------------------------------------------------------- /docs/gait/pics/walking-2-legs.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MultipedRobotics/multiped/17c2023e577cb6593b3f5a5fcbd5a9a57a5da46a/docs/gait/pics/walking-2-legs.gif -------------------------------------------------------------------------------- /docs/index.html: -------------------------------------------------------------------------------- 1 | 2 | 3 |
4 | 5 | 6 |

Hello

7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /docs/misc/readme.md: -------------------------------------------------------------------------------- 1 | # Documentation 2 | 3 | **This is still a work in progress** 4 | 5 | This folder contains most of the documentataion for this robot. Other documents would 6 | include the libraries used: 7 | 8 | - [mote](https://github.com/MomsFriendlyRobotCompany/mote): how the RPi software was setup 9 | - [opencvutils](https://github.com/MomsFriendlyRobotCompany/opencvutils): for the pi camera 10 | - [xl-320](https://github.com/MomsFriendlyRobotCompany/pyxl320): the servos used in the legs 11 | - [ins_nav](https://github.com/MomsFriendlyRobotCompany/ins_nav): navigation software 12 | - [nxp_imu](https://github.com/MomsFriendlyRobotCompany/nxp_imu): inertial measurement unit 13 | - [mcp3208](https://github.com/MomsFriendlyRobotCompany/mcp3208): analog to digital converter 14 | - [fake_rpi](https://github.com/MomsFriendlyRobotCompany/fake_rpi): library to emulate the RPi when testing on different hardware (like my macbook) 15 | 16 | ## Documentation Here 17 | 18 | - Markdown: documentation about this robot that is not already covered above 19 | - ipython: jupyter notebooks for software development 20 | 21 | Other folders contain various information 22 | -------------------------------------------------------------------------------- /docs/misc/walking-robots-pdfs/Spider robot kinematics.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MultipedRobotics/multiped/17c2023e577cb6593b3f5a5fcbd5a9a57a5da46a/docs/misc/walking-robots-pdfs/Spider robot kinematics.pdf -------------------------------------------------------------------------------- /docs/misc/walking-robots-pdfs/equations-for-hexapod-robot-control.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MultipedRobotics/multiped/17c2023e577cb6593b3f5a5fcbd5a9a57a5da46a/docs/misc/walking-robots-pdfs/equations-for-hexapod-robot-control.pdf -------------------------------------------------------------------------------- /docs/misc/walking-robots-pdfs/gait.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MultipedRobotics/multiped/17c2023e577cb6593b3f5a5fcbd5a9a57a5da46a/docs/misc/walking-robots-pdfs/gait.pdf -------------------------------------------------------------------------------- /docs/misc/walking-robots-pdfs/quadruped.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MultipedRobotics/multiped/17c2023e577cb6593b3f5a5fcbd5a9a57a5da46a/docs/misc/walking-robots-pdfs/quadruped.pdf -------------------------------------------------------------------------------- /docs/misc/walking-robots-pdfs/spider-robot-kinematics.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MultipedRobotics/multiped/17c2023e577cb6593b3f5a5fcbd5a9a57a5da46a/docs/misc/walking-robots-pdfs/spider-robot-kinematics.pdf -------------------------------------------------------------------------------- /docs/misc/walking-robots-pdfs/spong_kinematics.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MultipedRobotics/multiped/17c2023e577cb6593b3f5a5fcbd5a9a57a5da46a/docs/misc/walking-robots-pdfs/spong_kinematics.pdf -------------------------------------------------------------------------------- /docs/multiped/1-Figure1-1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MultipedRobotics/multiped/17c2023e577cb6593b3f5a5fcbd5a9a57a5da46a/docs/multiped/1-Figure1-1.png -------------------------------------------------------------------------------- /docs/multiped/2-Figure2-1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MultipedRobotics/multiped/17c2023e577cb6593b3f5a5fcbd5a9a57a5da46a/docs/multiped/2-Figure2-1.png -------------------------------------------------------------------------------- /docs/multiped/200px-AZIMUTMulti.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MultipedRobotics/multiped/17c2023e577cb6593b3f5a5fcbd5a9a57a5da46a/docs/multiped/200px-AZIMUTMulti.jpg -------------------------------------------------------------------------------- /docs/multiped/3-Figure1-1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MultipedRobotics/multiped/17c2023e577cb6593b3f5a5fcbd5a9a57a5da46a/docs/multiped/3-Figure1-1.png -------------------------------------------------------------------------------- /docs/multiped/9-Figure9-1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MultipedRobotics/multiped/17c2023e577cb6593b3f5a5fcbd5a9a57a5da46a/docs/multiped/9-Figure9-1.png -------------------------------------------------------------------------------- /docs/multiped/AZIMUT-hybrid-robot.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MultipedRobotics/multiped/17c2023e577cb6593b3f5a5fcbd5a9a57a5da46a/docs/multiped/AZIMUT-hybrid-robot.png -------------------------------------------------------------------------------- /docs/multiped/img_1182.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MultipedRobotics/multiped/17c2023e577cb6593b3f5a5fcbd5a9a57a5da46a/docs/multiped/img_1182.jpg -------------------------------------------------------------------------------- /docs/multiped/robot18.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MultipedRobotics/multiped/17c2023e577cb6593b3f5a5fcbd5a9a57a5da46a/docs/multiped/robot18.jpg -------------------------------------------------------------------------------- /docs/ncomms14494.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MultipedRobotics/multiped/17c2023e577cb6593b3f5a5fcbd5a9a57a5da46a/docs/ncomms14494.pdf -------------------------------------------------------------------------------- /docs/quad-4-links.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MultipedRobotics/multiped/17c2023e577cb6593b3f5a5fcbd5a9a57a5da46a/docs/quad-4-links.png -------------------------------------------------------------------------------- /docs/readme.md: -------------------------------------------------------------------------------- 1 | # Docs 2 | 3 | **work in progress** 4 | 5 | ## AX-12A Quadruped 6 | 7 | All current efforts is going into this version. 8 | 9 | ![](quad-4-links.png) 10 | 11 | ## XL-320 Quadruped 12 | 13 | **work pretty much abandoned** 14 | 15 | ![](spider.png) 16 | -------------------------------------------------------------------------------- /docs/spider.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MultipedRobotics/multiped/17c2023e577cb6593b3f5a5fcbd5a9a57a5da46a/docs/spider.png -------------------------------------------------------------------------------- /multiped/__init__.py: -------------------------------------------------------------------------------- 1 | # from multiped.Engine import Engine 2 | # from multiped.Gait import DiscreteRippleGait 3 | # # from multiped.kinematics3 import Kinematics3 4 | # from multiped.kinematics4 import Kinematics4 5 | # from multiped.Robot import Robot 6 | 7 | try: 8 | from importlib_metadata import version # type: ignore 9 | except ImportError: 10 | from importlib.metadata import version # type: ignore 11 | 12 | # do I put exceptions here? 13 | 14 | # gait - generates 3d (x,y,z) points for one cycle 15 | # engine - talk to servos/packet/bulk/sync 16 | # servo - individual parameters for each servo 17 | # kinematics - fk/ik 18 | # robot - holds above along with other logic and sensors 19 | 20 | __author__ = 'Kevin J. Walchko' 21 | __license__ = 'MIT' 22 | # __version__ = '0.5.0' 23 | __version__ = version("multiped") 24 | -------------------------------------------------------------------------------- /multiped/bin/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MultipedRobotics/multiped/17c2023e577cb6593b3f5a5fcbd5a9a57a5da46a/multiped/bin/__init__.py -------------------------------------------------------------------------------- /multiped/bin/get_leg_angles.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | ############################################## 4 | # The MIT License (MIT) 5 | # Copyright (c) 2017 Kevin Walchko 6 | # see LICENSE for full details 7 | ############################################## 8 | 9 | from __future__ import print_function, division 10 | from pyxl320 import ServoSerial 11 | from pyxl320.Packet import le, makeReadPacket 12 | import argparse 13 | import simplejson as json 14 | 15 | 16 | def writeToFile(data, filename='data.json'): 17 | with open(filename, 'w') as outfile: 18 | json.dump(data, outfile) 19 | 20 | 21 | DESCRIPTION = """ 22 | Returns the angles of servos 23 | """ 24 | 25 | 26 | def handleArgs(): 27 | parser = argparse.ArgumentParser(description=DESCRIPTION, formatter_class=argparse.RawTextHelpFormatter) 28 | parser.add_argument('port', help='serial port or \'dummy\' for testing', type=str) 29 | parser.add_argument('-j', '--json', help='save info to a json file, you must supply a file name: --json my_file.json', type=str) 30 | 31 | args = vars(parser.parse_args()) 32 | return args 33 | 34 | 35 | def getInfo(pkt): 36 | ID = pkt[4] 37 | angle = float((pkt[10] << 8) + pkt[9])/1023 38 | angle *= 300.0 39 | return ID, angle 40 | 41 | 42 | def getSingle(ID, ser): 43 | pkt = makeReadPacket(ID, 37, le(2)) 44 | # print('made packet:', pkt) 45 | ID = None 46 | angle = None 47 | 48 | ans = ser.sendPkt(pkt) 49 | if ans: 50 | ans = ans[0] 51 | ID, angle = getInfo(ans) 52 | 53 | return angle 54 | 55 | 56 | def main(): 57 | args = handleArgs() 58 | port = args['port'] 59 | 60 | s = ServoSerial(port=port) 61 | s.open() 62 | 63 | ids = range(1, 13) 64 | 65 | resp = {} 66 | for k in ids: 67 | resp[k] = None 68 | 69 | # as more servos add up, I might need to increase the cnt number??? 70 | for i in ids: 71 | angle = getSingle(i, s) 72 | resp[i] = angle 73 | 74 | cnt = 10 75 | while cnt: 76 | cnt = 0 77 | for k, v in resp.items(): 78 | # search through and find servos w/o responses (i.e., None) 79 | if v is None: 80 | cnt += 1 # found a None 81 | angle = getSingle(k, s) 82 | resp[k] = angle 83 | 84 | print('') 85 | print('Servos: 1 - 12') 86 | print('All angles are in {}'.format('degrees')) 87 | print(' {:>13} | {:>13} | {:>13} | {:>13} |'.format('Leg 1', 'Leg 2', 'Leg 3', 'Leg 4')) 88 | print(' {:>4} | {:6} | {:>4} | {:6} | {:>4} | {:6} | {:>4} | {:6} |'.format('ID', 'Angle', 'ID', 'Angle', 'ID', 'Angle', 'ID', 'Angle')) 89 | print('-' * 65) 90 | # for k, v in resp.items(): 91 | # if v is None: 92 | # print('{:4} | {}'.format(k, 'unknown')) 93 | # else: 94 | # print('{:4} | {:6.2f}'.format(k, v)) 95 | for i in range(1, 4): 96 | print(' {:4} | {:6.2f} | {:4} | {:6.2f} | {:4} | {:6.2f} | {:4} | {:6.2f}'.format(i, resp[i], i+3, resp[i+3], i+6, resp[i+6], i+9, resp[i+9])) 97 | print('-' * 65) 98 | print('') 99 | 100 | if args['json']: 101 | print('Saving servo angle info to {}'.format(args['json'])) 102 | writeToFile(resp, args['json']) 103 | 104 | s.close() 105 | 106 | 107 | if __name__ == '__main__': 108 | main() 109 | -------------------------------------------------------------------------------- /multiped/bin/get_leg_info.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | ############################################## 4 | # The MIT License (MIT) 5 | # Copyright (c) 2017 Kevin Walchko 6 | # see LICENSE for full details 7 | ############################################## 8 | 9 | from __future__ import print_function, division 10 | from pyxl320 import ServoSerial 11 | from pyxl320.Packet import le, makeReadPacket 12 | import argparse 13 | import simplejson as json 14 | from serial import SerialException 15 | import sys 16 | from quadruped.packetDecoder import PacketDecoder 17 | 18 | 19 | def writeToFile(data, filename='data.json'): 20 | with open(filename, 'w') as outfile: 21 | json.dump(data, outfile) 22 | 23 | 24 | def pktToDict(p): 25 | # print('pkt', pkt) 26 | # print('len(pkt)', len(pkt)) 27 | ans = {} 28 | ans['ID'] = p.id 29 | ans['Present Position'] = p.angle() 30 | ans['Present Voltage'] = p.voltage() 31 | ans['Present Load'] = '{:>5.1f}% {}'.format(*p.load()) 32 | ans['Present Temperature'] = p.temperature(PacketDecoder.F) 33 | ans['Hardware Error Status'] = p.hw_error() 34 | 35 | return ans 36 | 37 | 38 | DESCRIPTION = """ 39 | Returns limited info for each leg servo. 40 | 41 | ./get_leg_info.py /dev/tty.usbserial-AL034G2K 42 | Opened /dev/tty.usbserial-AL034G2K @ 1000000 43 | 44 | Servos: 1 - 12 45 | -------------------------------------------------- 46 | Servo: 1 HW Error: 0 47 | Position [deg]: 139.6 Load: 0.0% CCW 48 | Voltage [V] 7.0 Temperature [F]: 80.6 49 | -------------------------------------------------- 50 | Servo: 2 HW Error: 0 51 | Position [deg]: 178.9 Load: 4.5% CW 52 | Voltage [V] 7.1 Temperature [F]: 86.0 53 | -------------------------------------------------- 54 | Servo: 3 HW Error: 0 55 | Position [deg]: 119.1 Load: 0.0% CCW 56 | Voltage [V] 7.1 Temperature [F]: 80.6 57 | -------------------------------------------------- 58 | Servo: 4 HW Error: 0 59 | Position [deg]: 146.6 Load: 0.8% CCW 60 | Voltage [V] 7.3 Temperature [F]: 80.6 61 | -------------------------------------------------- 62 | Servo: 5 HW Error: 0 63 | Position [deg]: 275.4 Load: 0.8% CCW 64 | Voltage [V] 7.1 Temperature [F]: 80.6 65 | -------------------------------------------------- 66 | Servo: 6 HW Error: 0 67 | Position [deg]: 104.1 Load: 0.0% CCW 68 | Voltage [V] 7.3 Temperature [F]: 82.4 69 | -------------------------------------------------- 70 | Servo: 7 HW Error: 0 71 | Position [deg]: 163.9 Load: 0.0% CCW 72 | Voltage [V] 7.2 Temperature [F]: 80.6 73 | -------------------------------------------------- 74 | Servo: 8 HW Error: 0 75 | Position [deg]: 279.5 Load: 0.0% CCW 76 | Voltage [V] 7.1 Temperature [F]: 80.6 77 | -------------------------------------------------- 78 | Servo: 9 HW Error: 0 79 | Position [deg]: 100.3 Load: 0.0% CCW 80 | Voltage [V] 7.1 Temperature [F]: 84.2 81 | -------------------------------------------------- 82 | Servo: 10 HW Error: 0 83 | Position [deg]: 156.3 Load: 0.0% CCW 84 | Voltage [V] 7.1 Temperature [F]: 82.4 85 | -------------------------------------------------- 86 | Servo: 11 HW Error: 0 87 | Position [deg]: 280.6 Load: 0.0% CCW 88 | Voltage [V] 7.2 Temperature [F]: 80.6 89 | -------------------------------------------------- 90 | Servo: 12 HW Error: 0 91 | Position [deg]: 97.7 Load: 0.0% CCW 92 | Voltage [V] 7.1 Temperature [F]: 84.2 93 | -------------------------------------------------- 94 | """ 95 | 96 | 97 | def handleArgs(): 98 | parser = argparse.ArgumentParser(description=DESCRIPTION, formatter_class=argparse.RawTextHelpFormatter) 99 | parser.add_argument('port', help='serial port or \'dummy\' for testing', type=str) 100 | parser.add_argument('-j', '--json', metavar='FILENAME', help='save info to a json file: --json my_file.json', type=str) 101 | 102 | args = vars(parser.parse_args()) 103 | return args 104 | 105 | 106 | def getSingle(ID, ser): 107 | pkt = makeReadPacket(ID, 37, le(50-37+1)) 108 | # print('made packet:', pkt) 109 | 110 | ans = ser.sendPkt(pkt) 111 | if ans: 112 | ans = ans[0] 113 | pd = PacketDecoder(ans, 37) # data packet starts at register 37 114 | # pd.printPacket() 115 | if pd.checkError(): 116 | raise Exception('Crap!') 117 | ans = pktToDict(pd) 118 | else: 119 | ans = None 120 | 121 | return ans 122 | 123 | 124 | def printServo(s): 125 | print('-'*50) 126 | print('Servo: {} \t\tHW Error: {}'.format(s['ID'], s['Hardware Error Status'])) 127 | print('Position [deg]: {:5.1f} Load: {}'.format(s['Present Position'], s['Present Load'])) 128 | print('Voltage [V] {:4.1f} Temperature [F]: {:5.1f}'.format(s['Present Voltage'], s['Present Temperature'])) 129 | 130 | 131 | def main(): 132 | args = handleArgs() 133 | port = args['port'] 134 | 135 | s = ServoSerial(port=port) 136 | 137 | # open serial port 138 | try: 139 | s.open() 140 | except SerialException as e: 141 | print('-'*20) 142 | print(sys.argv[0], 'encountered an error') 143 | print(e) 144 | exit(1) 145 | 146 | ids = range(1, 13) 147 | 148 | resp = {} 149 | for k in ids: 150 | resp[k] = None 151 | 152 | # get servo data 153 | try: 154 | for i in ids: 155 | data = getSingle(i, s) 156 | resp[i] = data 157 | except Exception as e: 158 | print(e) 159 | exit(1) 160 | 161 | cnt = 10 162 | while cnt: 163 | cnt = 0 164 | for k, v in resp.items(): 165 | # search through and find servos w/o responses (i.e., None) 166 | if v is None: 167 | cnt += 1 # found a None 168 | ans = getSingle(k, s) 169 | resp[k] = ans 170 | 171 | print('') 172 | print('Servos: 1 - 12') 173 | for i in range(1, 13): 174 | printServo(resp[i]) 175 | print('-' * 50) 176 | print('') 177 | 178 | if args['json']: 179 | print('Saving servo angle info to {}'.format(args['json'])) 180 | writeToFile(resp, args['json']) 181 | 182 | s.close() 183 | 184 | 185 | if __name__ == '__main__': 186 | main() 187 | -------------------------------------------------------------------------------- /multiped/engine.py: -------------------------------------------------------------------------------- 1 | ############################################## 2 | # The MIT License (MIT) 3 | # Copyright (c) 2016 Kevin Walchko 4 | # see LICENSE for full details 5 | ############################################## 6 | 7 | from __future__ import print_function 8 | from __future__ import division 9 | from pyservos import ServoSerial 10 | from pyservos import Packet 11 | # from pyservos import ServoTypes 12 | from pyservos.packet import angle2int 13 | from pyservos.utils import le 14 | from multiped.Servo import Servo 15 | import time 16 | 17 | debug = True 18 | 19 | 20 | def dprint(s): 21 | global debug 22 | if debug: 23 | print(s) 24 | 25 | 26 | def calc_rpm(da, wait): 27 | """ 28 | Given an angular delta and a wait time, calculate the rpm needed to achieve 29 | 30 | [Join Mode] 31 | 0 ~ 1,023(0x3FF) can be used, and the unit is about 0.111rpm. 32 | If it is set to 0, it means the maximum rpm of the motor is used 33 | without controlling the speed. If it is 1023, it is about 114rpm. 34 | For example, if it is set to 300, it is about 33.3 rpm. 35 | 36 | AX12 max rmp is 59 (max speed 532) 37 | rev min 360 deg deg 38 | --- ------- ------- = 6 ---- 39 | min 60 sec rev sec 40 | 41 | rpm = abs(new_angle - old_angle)/(0.111 * wait * 6) 42 | """ 43 | return int(abs(da)/(0.111*wait*6)) 44 | 45 | 46 | def calc_wait(da, speed): 47 | """ 48 | Given an angular delta and the speed (servo counts), calculate the how long 49 | to wait for the servo to complete the movement 50 | 51 | [Join Mode] 52 | 0 ~ 1,023(0x3FF) can be used, and the unit is about 0.111rpm. 53 | If it is set to 0, it means the maximum rpm of the motor is used 54 | without controlling the speed. If it is 1023, it is about 114rpm. 55 | For example, if it is set to 300, it is about 33.3 rpm. 56 | 57 | AX12 max rmp is 59 (max speed 532) 58 | rev min 360 deg deg 59 | --- ------- ------- = 6 ---- 60 | min 60 sec rev sec 61 | 62 | da deg 63 | wait = ---------------------------------------- 64 | rpm 360 deg min 65 | 0.111 --- * speed_cnt * ------- * ------ 66 | cnt rev 60 sec 67 | 68 | wait = abs(new_angle - old_angle)/(0.111 * speed * 6) 69 | """ 70 | try: 71 | w = abs(da)/(0.111*speed*6) 72 | except ZeroDivisionError: 73 | w = 1 74 | print("*** calc_wait() div error: {}".format(speed)) 75 | 76 | return w 77 | 78 | 79 | class Engine(object): 80 | """ 81 | This class holds the serial port and talks to the hardware. Other classes ( 82 | e.g., gait, legs and servos) do the calculations for walking. 83 | """ 84 | 85 | last_move = None 86 | 87 | def __init__(self, data, servoType): 88 | """ 89 | data: serial port to use, if none, then use dummy port 90 | servoType: AX12 or XL320 or other servo type 91 | curr_pos: current leg position 92 | bcm_pin: which gpio pin is used for the comm 93 | { 94 | 0: [(t0,t1,t2,t3,speed), ...] 95 | 1: [...] 96 | ... 97 | 3: [...] 98 | } 99 | """ 100 | if 'bcm_pin' in data: 101 | bcm_pin = data['bcm_pin'] 102 | else: 103 | bcm_pin = None 104 | 105 | # self.wait = wait 106 | # determine serial port 107 | # default to fake serial port 108 | if 'serialPort' in data: 109 | try: 110 | self.serial = ServoSerial(data['serialPort'], pi_pin=bcm_pin) 111 | print('Using servo serial port: {}'.format(data['serialPort'])) 112 | self.serial.open() 113 | 114 | except Exception as e: 115 | print(e) 116 | print('Engine::init(): bye ...') 117 | exit(1) 118 | else: 119 | print('*** Using dummy serial port!!! ***') 120 | self.serial = ServoSerial('dummy') 121 | # raise Exception('No serial port given') 122 | 123 | # handle servos ... how does it know how many servos??? 124 | self.servos_up = {} # check servos are operating 125 | self.servos = [] 126 | for ID, seg in enumerate(['coxa', 'femur', 'tibia', 'tarsus']): 127 | length, offset = data[seg] 128 | self.servos.append(Servo(ID, offset)) 129 | # resp = self.pingServo(ID) # return: (T/F, servo_angle) 130 | # self.servos[ID] = resp[0] 131 | # curr_angle[ID] = 132 | # for s, val in self.servos.items(): 133 | # if val is False: 134 | # print("*** Engine.__init__(): servo[{}] has failed".format(s)) 135 | 136 | self.packet = Packet(servoType) 137 | 138 | # keep track of last location, this is servo angle space 139 | # self.last_move = { 140 | # 0: curr_pos[0][0], 141 | # 1: curr_pos[1][0], 142 | # 2: curr_pos[2][0], 143 | # 3: curr_pos[3][0] 144 | # } 145 | self.last_move = self.getCurrentAngles() 146 | 147 | def getCurrentAngles(self): 148 | """ 149 | Returns the current angles for all servos in DH space as a dictionary. 150 | angles = { 151 | 0: [0.0, 130.26, -115.73, -104.52], 152 | 1: [0.0, 130.26, -115.73, -104.52], 153 | 2: [0.0, 130.26, -115.73, -104.52], 154 | 3: [0.0, 130.26, -115.73, -104.52], 155 | } 156 | 157 | FIXME: actually query the servos and get there angles in DH space 158 | """ 159 | angles = { 160 | 0: [0.0, 130.26, -115.73, -104.52], 161 | 1: [0.0, 130.26, -115.73, -104.52], 162 | 2: [0.0, 130.26, -115.73, -104.52], 163 | 3: [0.0, 130.26, -115.73, -104.52], 164 | } 165 | return angles 166 | 167 | def DH2Servo(self, angle, num): 168 | return self.servos[num].DH2Servo(angle) 169 | 170 | def moveLegsGait4(self, legs): 171 | """ 172 | gait or sequence? 173 | speed = 1 - 1023 (scalar, all servos move at same rate) 174 | { step 0 step 1 ... 175 | 0: [(t1,t2,t3,t4,speed), (t1,t2,t3,t4,speed), ...] # leg0 176 | 2: [(t1,t2,t3,t4,speed), (t1,t2,t3,t4,speed), ...] # leg2 177 | ... 178 | } where t=theta 179 | NOTE: each leg needs the same number of steps and servos per leg 180 | WARNING: these angles are in servo space [0-300 deg] 181 | 182 | [Join Mode] 183 | 0 ~ 1,023(0x3FF) can be used, and the unit is about 0.111rpm. 184 | If it is set to 0, it means the maximum rpm of the motor is used 185 | without controlling the speed. If it is 1023, it is about 114rpm. 186 | For example, if it is set to 300, it is about 33.3 rpm. 187 | 188 | AX12 max rmp is 59 (max speed 532) 189 | rev min 360 deg deg 190 | --- ------- ------- = 6 ---- 191 | min 60 sec rev sec 192 | 193 | sleep time = | (new_angle - old_angle) /(0.111 * min_speed * 6) | 194 | """ 195 | # get the keys and figure out some stuff 196 | keys = list(legs.keys()) # which legs are we moving 197 | numSteps = len(legs[keys[0]]) # how many steps in the cycle 198 | numServos = len(legs[keys[0]][0])-1 # how many servos per leg, -1 because speed there 199 | 200 | # if self.last_move is None: 201 | # # assume we just turned on and was in the sit position 202 | # # need a better solution 203 | # self.last_move = { 204 | # 0: legs[0][0], 205 | # 1: legs[1][0], 206 | # 2: legs[2][0], 207 | # 3: legs[3][0] 208 | # } 209 | 210 | # curr_move = {} 211 | # for each step in legs 212 | for step in range(numSteps): 213 | # dprint("\nStep[{}]===============================================".format(step)) 214 | data = [] 215 | 216 | # find max time we have to wait for all 4 legs to reach their end 217 | # point. 218 | max_wait = 0 219 | for legNum in keys: 220 | angles = legs[legNum][step][:4] 221 | speed = legs[legNum][step][4] 222 | # print(" speed", speed) 223 | for a, oa in zip(angles, self.last_move[legNum][:4]): 224 | da = abs(a-oa) 225 | w = calc_wait(da, speed) 226 | # print(" calc_wait: {:.3f}".format(w)) 227 | # print("calc_wait: {}".format(w)) 228 | max_wait = w if w > max_wait else max_wait 229 | 230 | # print(">> found wait", max_wait, " speed:", speed) 231 | 232 | servo_speeds = [100,125,150,200] 233 | 234 | # for legNum in [0,3,1,2]: 235 | for legNum in keys: 236 | # dprint(" leg[{}]--------------".format(legNum)) 237 | leg_angles_speed = legs[legNum][step] 238 | # print(leg_angles_speed) 239 | angles = leg_angles_speed[:4] # 4 servo angles 240 | speed = leg_angles_speed[4] 241 | # print("Speed:", speed, "wait", max_wait) 242 | 243 | for i, DH_angle in enumerate(angles): 244 | # oldangle = self.last_move[legNum][i] 245 | # due to rounding errors, to ensure the other servers finish 246 | # BEFORE time.sleep(max_wait) ends, the the function it 247 | # has less time 248 | # spd = calc_rpm((angle - oldangle), 0.9*max_wait) 249 | # now that i am scaling the spd parameter above, I sometimes 250 | # exceed the original speed number, so saturate it if 251 | # necessary 252 | # spd = spd if spd <= speed else speed 253 | # sl, sh = le(spd) 254 | sl, sh = le(servo_speeds[i]) 255 | servo_angle = self.DH2Servo(DH_angle, i) 256 | al, ah = angle2int(servo_angle) # angle 257 | data.append([legNum*numServos + i+1, al, ah, sl, sh]) # ID, low angle, high angle, low speed, high speed 258 | # data.append([legNum*numServos + i+1, al, ah]) # ID, low angle, high angle, low speed, high speed 259 | 260 | self.last_move[legNum] = leg_angles_speed 261 | 262 | pkt = self.packet.makeSyncWritePacket(self.packet.base.GOAL_POSITION, data) 263 | self.serial.write(pkt) 264 | dprint("sent serial packet leg: {}".format(legNum)) 265 | data = [] 266 | print('max_wait', max_wait) 267 | time.sleep(max_wait) 268 | 269 | # time.sleep(1) 270 | 271 | 272 | 273 | # def pprint(self, i, step): 274 | # print('***', i, '*'*25) 275 | # for leg in step: 276 | # print(' Servo: [{:.0f} {:.0f} {:.0f} {:.0f}]'.format(*leg)) 277 | 278 | # def setServoSpeed(self, speed): 279 | # """ 280 | # Sets the servo speed for all servos. 281 | # speed - 0-1023, but only up to max speed of servo 282 | # """ 283 | # # make packet function 284 | # pkt = self.packet.makeWritePacket( 285 | # self.packet.base.BROADCAST_ADDR, 286 | # self.packet.base.GOAL_VELOCITY, 287 | # le(speed) 288 | # ) 289 | # self.serial.write(pkt) 290 | 291 | # def moveLegsGait(self, legs): 292 | # """ 293 | # gait or sequence? 294 | # speed = 1 - 1023 (scalar, all servos move at same rate) 295 | # { step 0 step 1 ... 296 | # 0: [[(t1,s1),(t2,s2),(t3,s3),(t4,s4)], [(t1,s1),(t2,s2),(t3,s3),(t4,s4)], ...] # leg0 297 | # 2: [[(t1,s1),(t2,s2),(t3,s3),(t4,s4)], [(t1,s1),(t2,s2),(t3,s3),(t4,s4)], ...] # leg2 298 | # ... 299 | # } where t=theta s=speed 300 | # NOTE: each leg needs the same number of steps and servos per leg 301 | # WARNING: these angles are in servo space [0-300 deg] 302 | # """ 303 | # # get the keys and figure out some stuff 304 | # keys = list(legs.keys()) 305 | # numSteps = len(legs[keys[0]]) 306 | # numServos = len(legs[keys[0]][0]) 307 | # # wait = self.wait # FIXME: wait should be a function of speed 308 | # 309 | # if self.last_move is None: 310 | # # assume we just turned on and was in the sit position 311 | # self.last_move = {} 312 | # # self.last_move = { 313 | # # # 0: [[(t1,s1),(t2,s2),(t3,s3),(t4,s4)] 314 | # # 0: None, # tuple of angles for each leg 315 | # # 1: None, 316 | # # 2: None, 317 | # # 3: None 318 | # # } 319 | # curr_move = {} 320 | # # else: 321 | # # ts = 0 322 | # # for nleg, oleg in zip(legs, self.last_move): 323 | # # for 324 | # 325 | # # for each step in legs 326 | # # print("-------------------") 327 | # for s in range(numSteps): 328 | # dprint("\nStep[{}]===============================================".format(s)) 329 | # min_speed = 10000 330 | # max_angle = 0 331 | # data = [] 332 | # for legNum in keys: 333 | # dprint(" leg[{}]--------------".format(legNum)) 334 | # step = legs[legNum][s] 335 | # # print("step", step) 336 | # curr_move[legNum] = step # save current angle/speed 337 | # for i, (angle, speed) in enumerate(step): 338 | # al, ah = angle2int(angle) # angle 339 | # 340 | # # remember, servo IDs start at 1 not 0, so there is a +1 341 | # min_speed = speed if speed < min_speed else min_speed 342 | # max_angle = angle if angle > max_angle else max_angle 343 | # sl, sh = le(speed) 344 | # dprint(" Servo[{}], angle: {:.2f}, speed: {} time: {:.2f}".format(legNum*numServos + i+1, angle, speed, 0.111*speed*6)) 345 | # data.append([legNum*numServos + i+1, al, ah, sl, sh]) # ID, low angle, high angle, low speed, high speed 346 | # 347 | # pkt = self.packet.makeSyncWritePacket(self.packet.base.GOAL_POSITION, data) 348 | # self.serial.write(pkt) 349 | # dprint("sent serial packet") 350 | # data = [] 351 | # 352 | # """ 353 | # [Join Mode] 354 | # 0 ~ 1,023(0x3FF) can be used, and the unit is about 0.111rpm. 355 | # If it is set to 0, it means the maximum rpm of the motor is used 356 | # without controlling the speed. If it is 1023, it is about 114rpm. 357 | # For example, if it is set to 300, it is about 33.3 rpm. 358 | # 359 | # AX12 max rpm is 59 (max speed 532) 360 | # rev min 360 deg deg 361 | # --- ------- ------- = 6 ---- 362 | # min 60 sec rev sec 363 | # 364 | # sleep time = | (new_angle - old_angle) /(0.111 * min_speed * 6) | 365 | # """ 366 | # 367 | # max_wait = 0 368 | # if self.last_move: 369 | # for legNum in curr_move.keys(): 370 | # for (na, ns), (oa, os) in list(zip(curr_move[legNum], self.last_move[legNum])): 371 | # w = abs((na - oa)/(0.111*ns*6)) 372 | # w *= 1.10 # scaling to make things look better: 1.3 373 | # max_wait = w if w > max_wait else max_wait 374 | # self.last_move[legNum] = curr_move[legNum] 375 | # else: 376 | # # this should only get called after start, when last_move = None 377 | # self.last_move = curr_move 378 | # max_wait = 1.0 379 | # 380 | # # wait = 20 / (0.111*min_speed*6) 381 | # time.sleep(max_wait) 382 | # # print('[{}] max_wait: {:.3f}'.format(s, max_wait)) 383 | # print('max_wait', max_wait) 384 | # # print('max_angle', max_angle) 385 | # 386 | # def moveLegsGait2(self, legs): 387 | # """ 388 | # gait or sequence? 389 | # speed = 1 - 1023 (scalar, all servos move at same rate) 390 | # { step 0 step 1 ... 391 | # 0: [(t1,t2,t3,t4,speed), (t1,t2,t3,t4,speed), ...] # leg0 392 | # 2: [(t1,t2,t3,t4,speed), (t1,t2,t3,t4,speed), ...] # leg2 393 | # ... 394 | # } where t=theta 395 | # NOTE: each leg needs the same number of steps and servos per leg 396 | # WARNING: these angles are in servo space [0-300 deg] 397 | # 398 | # [Join Mode] 399 | # 0 ~ 1,023(0x3FF) can be used, and the unit is about 0.111rpm. 400 | # If it is set to 0, it means the maximum rpm of the motor is used 401 | # without controlling the speed. If it is 1023, it is about 114rpm. 402 | # For example, if it is set to 300, it is about 33.3 rpm. 403 | # 404 | # AX12 max rmp is 59 (max speed 532) 405 | # rev min 360 deg deg 406 | # --- ------- ------- = 6 ---- 407 | # min 60 sec rev sec 408 | # 409 | # sleep time = | (new_angle - old_angle) /(0.111 * min_speed * 6) | 410 | # """ 411 | # # get the keys and figure out some stuff 412 | # keys = list(legs.keys()) # which legs are we moving 413 | # numSteps = len(legs[keys[0]]) # how many steps in the cycle 414 | # numServos = len(legs[keys[0]][0])-1 # how many servos per leg, -1 because speed there 415 | # 416 | # if self.last_move is None: 417 | # # assume we just turned on and was in the sit position 418 | # # need a better solution 419 | # self.last_move = { 420 | # 0: legs[0][0], 421 | # 1: legs[1][0], 422 | # 2: legs[2][0], 423 | # 3: legs[3][0] 424 | # } 425 | # 426 | # # curr_move = {} 427 | # # for each step in legs 428 | # for step in range(numSteps): 429 | # dprint("\nStep[{}]===============================================".format(step)) 430 | # data = [] 431 | # 432 | # # for legNum in [0,3,1,2]: 433 | # for legNum in keys: 434 | # dprint(" leg[{}]--------------".format(legNum)) 435 | # leg_angles_speed = legs[legNum][step] 436 | # # print(leg_angles_speed) 437 | # angles = leg_angles_speed[:4] 438 | # speed = leg_angles_speed[4] 439 | # sl, sh = le(speed) 440 | # 441 | # max_wait = 0 442 | # for i, angle in enumerate(angles): 443 | # al, ah = angle2int(angle) # angle 444 | # dprint(" Servo[{}], angle: {:.2f}, speed: {}".format(legNum*numServos + i+1, angle, speed)) 445 | # data.append([legNum*numServos + i+1, al, ah, sl, sh]) # ID, low angle, high angle, low speed, high speed 446 | # 447 | # # print(self.last_move) 448 | # # print(leg_angles_speed) 449 | # # print(angles) 450 | # # print(i) 451 | # # print(legNum*numServos + i+1) 452 | # 453 | # oldangle = self.last_move[legNum][i] 454 | # # calculate wait time for a leg, take max time 455 | # w = abs((angle - oldangle)/(0.111*speed*6)) 456 | # # print(angle - oldangle) 457 | # # print('w',w) 458 | # w *= 1.10 # scaling to make things look better: 1.3 459 | # max_wait = w if w > max_wait else max_wait 460 | # 461 | # pkt = self.packet.makeSyncWritePacket(self.packet.base.GOAL_POSITION, data) 462 | # self.serial.write(pkt) 463 | # dprint("sent serial packet leg: {}".format(legNum)) 464 | # data = [] 465 | # print('max_wait', max_wait) 466 | # time.sleep(max_wait) 467 | # 468 | # self.last_move[legNum] = leg_angles_speed 469 | # 470 | # def moveLegsGait3(self, legs): 471 | # """ 472 | # gait or sequence? 473 | # speed = 1 - 1023 (scalar, all servos move at same rate) 474 | # { step 0 step 1 ... 475 | # 0: [(t1,t2,t3,t4,speed), (t1,t2,t3,t4,speed), ...] # leg0 476 | # 2: [(t1,t2,t3,t4,speed), (t1,t2,t3,t4,speed), ...] # leg2 477 | # ... 478 | # } where t=theta 479 | # NOTE: each leg needs the same number of steps and servos per leg 480 | # WARNING: these angles are in servo space [0-300 deg] 481 | # 482 | # [Join Mode] 483 | # 0 ~ 1,023(0x3FF) can be used, and the unit is about 0.111rpm. 484 | # If it is set to 0, it means the maximum rpm of the motor is used 485 | # without controlling the speed. If it is 1023, it is about 114rpm. 486 | # For example, if it is set to 300, it is about 33.3 rpm. 487 | # 488 | # AX12 max rmp is 59 (max speed 532) 489 | # rev min 360 deg deg 490 | # --- ------- ------- = 6 ---- 491 | # min 60 sec rev sec 492 | # 493 | # sleep time = | (new_angle - old_angle) /(0.111 * min_speed * 6) | 494 | # """ 495 | # # get the keys and figure out some stuff 496 | # keys = list(legs.keys()) # which legs are we moving 497 | # numSteps = len(legs[keys[0]]) # how many steps in the cycle 498 | # numServos = len(legs[keys[0]][0])-1 # how many servos per leg, -1 because speed there 499 | # 500 | # if self.last_move is None: 501 | # # assume we just turned on and was in the sit position 502 | # # need a better solution 503 | # self.last_move = { 504 | # 0: legs[0][0], 505 | # 1: legs[1][0], 506 | # 2: legs[2][0], 507 | # 3: legs[3][0] 508 | # } 509 | # 510 | # # curr_move = {} 511 | # # for each step in legs 512 | # for step in range(numSteps): 513 | # dprint("\nStep[{}]===============================================".format(step)) 514 | # data = [] 515 | # 516 | # max_wait = 0 517 | # # for legNum in [0,3,1,2]: 518 | # for legNum in keys: 519 | # dprint(" leg[{}]--------------".format(legNum)) 520 | # leg_angles_speed = legs[legNum][step] 521 | # # print(leg_angles_speed) 522 | # angles = leg_angles_speed[:4] 523 | # speed = leg_angles_speed[4] 524 | # sl, sh = le(speed) 525 | # 526 | # # max_wait = 0 527 | # for i, angle in enumerate(angles): 528 | # al, ah = angle2int(angle) # angle 529 | # dprint(" Servo[{}], angle: {:.2f}, speed: {}".format(legNum*numServos + i+1, angle, speed)) 530 | # data.append([legNum*numServos + i+1, al, ah, sl, sh]) # ID, low angle, high angle, low speed, high speed 531 | # 532 | # # print(self.last_move) 533 | # # print(leg_angles_speed) 534 | # # print(angles) 535 | # # print(i) 536 | # # print(legNum*numServos + i+1) 537 | # 538 | # oldangle = self.last_move[legNum][i] 539 | # # calculate wait time for a leg, take max time 540 | # w = abs((angle - oldangle)/(0.111*speed*6)) 541 | # # print(angle - oldangle) 542 | # # print('w',w) 543 | # w *= 1.10 # scaling to make things look better: 1.3 544 | # max_wait = w if w > max_wait else max_wait 545 | # 546 | # self.last_move[legNum] = leg_angles_speed 547 | # 548 | # pkt = self.packet.makeSyncWritePacket(self.packet.base.GOAL_POSITION, data) 549 | # self.serial.write(pkt) 550 | # dprint("sent serial packet leg: {}".format(legNum)) 551 | # data = [] 552 | # print('max_wait', max_wait) 553 | # time.sleep(max_wait) 554 | 555 | # def moveLegsGait4(self, legs): 556 | # """ 557 | # gait or sequence? 558 | # speed = 1 - 1023 (scalar, all servos move at same rate) 559 | # { step 0 step 1 ... 560 | # 0: [(t1,t2,t3,t4,speed), (t1,t2,t3,t4,speed), ...] # leg0 561 | # 2: [(t1,t2,t3,t4,speed), (t1,t2,t3,t4,speed), ...] # leg2 562 | # ... 563 | # } where t=theta 564 | # NOTE: each leg needs the same number of steps and servos per leg 565 | # WARNING: these angles are in servo space [0-300 deg] 566 | # 567 | # [Join Mode] 568 | # 0 ~ 1,023(0x3FF) can be used, and the unit is about 0.111rpm. 569 | # If it is set to 0, it means the maximum rpm of the motor is used 570 | # without controlling the speed. If it is 1023, it is about 114rpm. 571 | # For example, if it is set to 300, it is about 33.3 rpm. 572 | # 573 | # AX12 max rmp is 59 (max speed 532) 574 | # rev min 360 deg deg 575 | # --- ------- ------- = 6 ---- 576 | # min 60 sec rev sec 577 | # 578 | # sleep time = | (new_angle - old_angle) /(0.111 * min_speed * 6) | 579 | # """ 580 | # # get the keys and figure out some stuff 581 | # keys = list(legs.keys()) # which legs are we moving 582 | # numSteps = len(legs[keys[0]]) # how many steps in the cycle 583 | # numServos = len(legs[keys[0]][0])-1 # how many servos per leg, -1 because speed there 584 | # 585 | # # if self.last_move is None: 586 | # # # assume we just turned on and was in the sit position 587 | # # # need a better solution 588 | # # self.last_move = { 589 | # # 0: legs[0][0], 590 | # # 1: legs[1][0], 591 | # # 2: legs[2][0], 592 | # # 3: legs[3][0] 593 | # # } 594 | # 595 | # # curr_move = {} 596 | # # for each step in legs 597 | # for step in range(numSteps): 598 | # # dprint("\nStep[{}]===============================================".format(step)) 599 | # data = [] 600 | # 601 | # # find max time we have to wait for all 4 legs to reach their end 602 | # # point. 603 | # max_wait = 0 604 | # for legNum in keys: 605 | # angles = legs[legNum][step][:4] 606 | # speed = legs[legNum][step][4] 607 | # # print(" speed", speed) 608 | # for a, oa in zip(angles, self.last_move[legNum][:4]): 609 | # da = abs(a-oa) 610 | # w = calc_wait(da, speed) 611 | # # print(" calc_wait: {:.3f}".format(w)) 612 | # # print("calc_wait: {}".format(w)) 613 | # max_wait = w if w > max_wait else max_wait 614 | # 615 | # # print(">> found wait", max_wait, " speed:", speed) 616 | # 617 | # servo_speeds = [100,125,150,200] 618 | # 619 | # # for legNum in [0,3,1,2]: 620 | # for legNum in keys: 621 | # # dprint(" leg[{}]--------------".format(legNum)) 622 | # leg_angles_speed = legs[legNum][step] 623 | # # print(leg_angles_speed) 624 | # angles = leg_angles_speed[:4] # 4 servo angles 625 | # speed = leg_angles_speed[4] 626 | # # print("Speed:", speed, "wait", max_wait) 627 | # 628 | # for i, angle in enumerate(angles): 629 | # # oldangle = self.last_move[legNum][i] 630 | # # due to rounding errors, to ensure the other servers finish 631 | # # BEFORE time.sleep(max_wait) ends, the the function it 632 | # # has less time 633 | # # spd = calc_rpm((angle - oldangle), 0.9*max_wait) 634 | # # now that i am scaling the spd parameter above, I sometimes 635 | # # exceed the original speed number, so saturate it if 636 | # # necessary 637 | # # spd = spd if spd <= speed else speed 638 | # # sl, sh = le(spd) 639 | # sl, sh = le(servo_speeds[i]) 640 | # al, ah = angle2int(angle) # angle 641 | # data.append([legNum*numServos + i+1, al, ah, sl, sh]) # ID, low angle, high angle, low speed, high speed 642 | # # data.append([legNum*numServos + i+1, al, ah]) # ID, low angle, high angle, low speed, high speed 643 | # 644 | # self.last_move[legNum] = leg_angles_speed 645 | # 646 | # pkt = self.packet.makeSyncWritePacket(self.packet.base.GOAL_POSITION, data) 647 | # self.serial.write(pkt) 648 | # dprint("sent serial packet leg: {}".format(legNum)) 649 | # data = [] 650 | # print('max_wait', max_wait) 651 | # time.sleep(max_wait) 652 | # 653 | # # time.sleep(1) 654 | 655 | 656 | # def moveLegsAnglesArray(self, angles, speed=None, degrees=True): 657 | # """ 658 | # The ID number is set by the array position 659 | # speed = 1 - 1023 (scalar, all servos move at same rate) 660 | # angles = [servo1, servo2, servo3 ...] 661 | # WARNING: these angles are in servo space [0-300 deg] 662 | # """ 663 | # # print('legs', legs, '\n\n') 664 | # # build sync message 665 | # data = [] 666 | # if speed: 667 | # sl, sh = le(speed) 668 | # for i, a in enumerate(angles): 669 | # a, b = angle2int(a) 670 | # if speed: 671 | # data.append([i+1, a, b, sl, sh]) 672 | # else: 673 | # data.append([i+1, a, b]) 674 | # print('data', data) 675 | # pkt = self.packet.makeSyncWritePacket(self.packet.base.GOAL_POSITION, data) 676 | # 677 | # # Status Packet will not be returned if Broadcast ID(0xFE) is used 678 | # self.serial.write(pkt) # no point to read 679 | # # self.serial.sendPkt(pkt) 680 | # print('moveLegsAnglesArray', pkt) 681 | 682 | # def stand(self): 683 | # if self.positions['stand']: 684 | # feet = { 685 | # 0: [self.positions['stand']], 686 | # 1: [self.positions['stand']], 687 | # 2: [self.positions['stand']], 688 | # 3: [self.positions['stand']], 689 | # } 690 | # self.moveLegsGait(feet, 100) 691 | # sleep(4) 692 | # 693 | # def sit(self): 694 | # if self.positions['sit']: 695 | # feet = { 696 | # 0: [self.positions['sit']], 697 | # 1: [self.positions['sit']], 698 | # 2: [self.positions['sit']], 699 | # 3: [self.positions['sit']], 700 | # } 701 | # self.moveLegsGait(feet, 100) 702 | # sleep(4) 703 | # def moveLegsPosition2(self, legs): 704 | # """ 705 | # this only does position 706 | # [ step 0 step 1 ... 707 | # [[t1,t2,t3,t4], [t1,t2,t3,t4], ...] # leg0 708 | # [[t1,t2,t3,t4], [t1,t2,t3,t4], ...] # leg1 709 | # ... 710 | # ] 711 | # 712 | # FIXME: don't hard code for 4 legs, might want to just move 1 or 2 or 3 713 | # """ 714 | # # print('legs', legs, '\n\n') 715 | # # build sync message 716 | # # FIXME: hard coded for 4 legs 717 | # for j, step in enumerate(zip(*legs)): # servo angles for each leg at each step 718 | # data = [] # [id, lo, hi, id, lo, hi, ...] 719 | # # leg 0: 0 1 2 3 720 | # # print('step', len(step), step, '\n') 721 | # self.pprint(j, step) 722 | # for i, leg in enumerate(step): # step = [leg0, leg1, leg2, leg3] 723 | # # print('leg', len(leg), leg) 724 | # for j, servo in enumerate(leg): # leg = [servo0, servo1, servo2, servo3] 725 | # s = [] 726 | # s.append(4*i+j) # servo ID 727 | # a, b = angle2int(servo) # angle 728 | # s.append(a) 729 | # s.append(b) 730 | # data.append(s) 731 | # 732 | # # print('\n\ndata', data) 733 | # 734 | # pkt = self.packet.makeSyncWritePacket(self.packet.base.GOAL_POSITION, data) 735 | # self.serial.sendPkt(pkt) 736 | 737 | # def moveLegsPosition(self, legs): 738 | # """ 739 | # [ step 0 step 1 ... 740 | # [[t1,t2,t3,t4], [t1,t2,t3,t4], ...] # leg0 741 | # [[t1,t2,t3,t4], [t1,t2,t3,t4], ...] # leg1 742 | # ... 743 | # ] 744 | # 745 | # FIXME: this moves angles from a gait, not position (x,y,z) ... rename or change interface to take points? 746 | # FIXME: don't hard code for 4 legs, might want to just move 1 or 2 or 3 747 | # FIXME: legs are positional ... can't just move leg 3, try: 748 | # legs = { here legs 1 and 4 are not moved 749 | # 0: [t1,t2, ...], ... move leg 0 750 | # 3: [t1, t2 ...], ... move leg 3 751 | # } 752 | # """ 753 | # # print('legs', legs, '\n\n') 754 | # # build sync message 755 | # # FIXME: hard coded for 4 legs 756 | # for g, step in enumerate(zip(*legs)): # servo angles for each leg at each step 757 | # data = [] # [id, lo, hi, id, lo, hi, ...] 758 | # # leg 0: 0 1 2 3 759 | # # print('step', len(step), step, '\n') 760 | # self.pprint(g, step) 761 | # num_legs = len(step) 762 | # for i, leg in enumerate(step): # step = [leg0, leg1, leg2, leg3] 763 | # # print('leg', len(leg), leg) 764 | # num_joints = len(leg) 765 | # for j, servo in enumerate(leg): # leg = [servo0, servo1, servo2, servo3] 766 | # s = [] 767 | # s.append(num_joints*i+j) # servo ID 768 | # a, b = angle2int(servo) # angle 769 | # s.append(a) 770 | # s.append(b) 771 | # data.append(s) 772 | # 773 | # # print('\n\ndata', data) 774 | # 775 | # pkt = self.packet.makeSyncWritePacket(self.packet.base.GOAL_POSITION, data) 776 | # self.serial.sendPkt(pkt) 777 | 778 | # # FIXME: this also applies to non-gait movement ... moveLegs??? 779 | # def moveLegsGait(self, legs, speed=None): 780 | # """ 781 | # gait or sequence? 782 | # speed = 1 - 1023 (scalar, all servos move at same rate) 783 | # legs = { step 0 step 1 ... 784 | # 0: [[t1,t2,t3,t4], [t1,t2,t3,t4], ...] # leg0 785 | # 1: [[t1,t2,t3,t4], [t1,t2,t3,t4], ...] # leg1 786 | # 3: [[servo1, servo2, ...]] angles in servo space [0-300] deg 787 | # } 788 | # NOTE: each leg needs the same number of steps and servos per leg 789 | # WARNING: these angles are in servo space [0-300 deg] 790 | # """ 791 | # # get the keys and figure out some stuff 792 | # keys = list(legs.keys()) 793 | # numSteps = len(legs[keys[0]]) 794 | # numServos = len(legs[keys[0]][0]) 795 | # wait = self.wait # FIXME: wait should be a function of speed 796 | # 797 | # # print("Speed: {} =====================".format(speed)) 798 | # 799 | # def ramp(val, length): 800 | # """ 801 | # Simple triangle for scaling speed. it always returns 0.5 at the lowest 802 | # and is 1.0 at max in the middle 803 | # 804 | # in: val - step 805 | # length - number of steps 806 | # out: 0.5 - 1.0 807 | # """ 808 | # val = val % length 809 | # # print("ramp: {} {} {}".format(val, length/2, length)) 810 | # slope = 0.5/(length/2) 811 | # if val > length/2: 812 | # # since it is symetric, just mirror the answer 813 | # val = (length - val) 814 | # return slope*val + 0.5 815 | # 816 | # # for each step in legs 817 | # for s in range(numSteps): 818 | # data = [] 819 | # for legNum in keys: 820 | # step = legs[legNum][s] 821 | # for i, a in enumerate(step): 822 | # a, b = angle2int(a) # angle 823 | # 824 | # # remember, servo IDs start at 1 not 0, so there is a +1 825 | # if speed: 826 | # scale = ramp(s, 12) 827 | # sl, sh = le(int(scale*speed)) 828 | # # print("LegNum[{}] Servo[{}] step: {}, scale: {:.2f}, speed: {}".format(legNum, legNum*numServos + i+1, s, scale, int(scale*speed))) 829 | # data.append([legNum*numServos + i+1, a, b, sl, sh]) # ID, low angle, high angle, low speed, high speed 830 | # else: 831 | # dspeed = 100 832 | # sl, sh = le(dspeed) 833 | # # print("LegNum[{}] Servo[{}] step: {}, speed: {}".format(legNum, legNum*numServos + i+1, s, dspeed)) 834 | # data.append([legNum*numServos + i+1, a, b, sl, sh]) # ID, low angle, high angle 835 | # 836 | # # print('\n\ndata', data) 837 | # 838 | # pkt = self.packet.makeSyncWritePacket(self.packet.base.GOAL_POSITION, data) 839 | # # print('raw pkt', pkt) 840 | # self.serial.write(pkt) 841 | # time.sleep(wait) 842 | # # print('moveLegsGait', wait) 843 | -------------------------------------------------------------------------------- /multiped/gait.py: -------------------------------------------------------------------------------- 1 | ############################################## 2 | # The MIT License (MIT) 3 | # Copyright (c) 2016 Kevin Walchko 4 | # see LICENSE for full details 5 | ############################################## 6 | 7 | from __future__ import print_function 8 | from __future__ import division 9 | from math import cos, sin, sqrt, pi 10 | 11 | 12 | debug = False 13 | 14 | 15 | def rot_z_tuple(t, c): 16 | """ 17 | t - theta [radians] 18 | c - [x,y,z] 19 | return - (x,y,z) tuple rotated about z-axis 20 | """ 21 | ans = ( 22 | c[0]*cos(t)-c[1]*sin(t), 23 | c[0]*sin(t)+c[1]*cos(t), 24 | c[2] 25 | ) 26 | 27 | return ans 28 | 29 | 30 | # make a static method in Gait? Nothing else uses it 31 | def rot_z(t, c): 32 | """ 33 | t - theta [radians] 34 | c - [x,y,z] 35 | return - [x,y,z] numpy array rotated about z-axis 36 | """ 37 | ans = [ 38 | c[0]*cos(t)-c[1]*sin(t), 39 | c[0]*sin(t)+c[1]*cos(t), 40 | c[2] 41 | ] 42 | 43 | return ans 44 | 45 | 46 | # class Tripod: 47 | # def __init__(self, neutral, forward, back, count): 48 | # self.build_forward() 49 | # 50 | # def build_forward(self, n,f,b, c): 51 | # 52 | 53 | # @classmethod 54 | # def from_params(cls, neutral, forward, back): 55 | # tri = cls() 56 | # 57 | # return tri 58 | 59 | 60 | class Gait(object): 61 | """ 62 | Base class for gaits. Gait only plan all foot locations for 1 complete cycle 63 | of the gait. 64 | 65 | Like to add center of mass (CM) compensation, so we know the CM is always 66 | inside the stability triangle. 67 | 68 | Gait knows: 69 | - how many legs 70 | - leg stride (neutral position) 71 | """ 72 | # these are the offsets of each leg 73 | legOffset = [0, 6, 3, 9] 74 | # frame rotations for each leg 75 | # cmrot = [pi/4, -pi/4, -3*pi/4, 3*pi/4] 76 | # frame = [-pi/4, pi/4, 3*pi/4, -3*pi/4] # this seem to work better ... wtf? 77 | frame = [pi/4, -pi/4, -3*pi/4, 3*pi/4] 78 | moveFoot = None 79 | rest = None 80 | scale = 50.0 81 | 82 | def __init__(self, rest): 83 | # the resting or idle position/orientation of a leg 84 | self.rest = rest 85 | 86 | def command(self, cmd): 87 | """ 88 | Send a command to the quadruped 89 | cmd: [x, y, rotation about z-axis], the x,y is a unit vector and 90 | rotation is in radians 91 | """ 92 | x, y, rz = cmd 93 | d = sqrt(x**2+y**2) 94 | 95 | # handle no movement command ... do else where? 96 | if d <= 0.1 and abs(rz) < 0.1: 97 | x = y = 0.0 98 | rz = 0.0 99 | return None 100 | # commands should be unit length, oneCyle scales it 101 | elif 0.1 < d: 102 | x /= d 103 | y /= d 104 | else: 105 | return None 106 | 107 | angle_limit = pi/2 108 | if rz > angle_limit: 109 | rz = angle_limit 110 | elif rz < -angle_limit: 111 | rz = -angle_limit 112 | 113 | return self.oneCycle_alt(x, y, rz) 114 | 115 | def oneCycle_alt(self, x, y, rz): 116 | raise NotImplementedError('*** Gait: wrong function, this is base class! ***') 117 | 118 | 119 | class DiscreteRippleGait(Gait): 120 | """ 121 | Discrete 12 step gait 122 | """ 123 | steps = 0 124 | 125 | def __init__(self, height, rest): 126 | """ 127 | height: added to z 128 | rest: the neutral foot position 129 | """ 130 | Gait.__init__(self, rest) 131 | # self.phi = [8/8, 7/8, 1/8, 0/8, 1/8, 2/8, 3/8, 4/8, 5/8, 6/8, 7/8, 8/8] 132 | self.phi = [10/10, 5/10, 0/10, 1/10, 2/10, 3/10, 4/10, 5/10, 6/10, 7/10, 8/10, 9/10] 133 | self.setLegLift(height) 134 | self.steps = len(self.phi) 135 | 136 | def setLegLift(self, lift): 137 | # legs lift in the sequence: 0, 3, 1, 2 138 | # 0 1 2 3 4 5 6 7 8 9 10 11 139 | self.z = [0.0, lift, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # leg height 140 | 141 | def eachLeg(self, index, cmd): 142 | """ 143 | interpolates the foot position of each leg 144 | cmd: 145 | linear (mm) 146 | angle (rads) 147 | """ 148 | rest = self.rest 149 | i = index 150 | phi = self.phi[i] 151 | xx, yy, rzz = cmd 152 | 153 | # rotational commands ----------------------------------------------- 154 | angle = rzz/2-rzz*phi 155 | rest_rot = rot_z(-angle, rest) 156 | 157 | # create new move command 158 | move = [ 159 | xx/2 - phi*xx, 160 | yy/2 - phi*yy, 161 | self.z[i] 162 | ] 163 | 164 | # new foot position: newpos = rot + move ---------------------------- 165 | # newpos = linear_movement + angular_movement 166 | # newpos = move + rest_rot 167 | newpos = [0, 0, 0] 168 | newpos[0] = move[0] + rest_rot[0] 169 | newpos[1] = move[1] + rest_rot[1] 170 | newpos[2] = move[2] + rest_rot[2] 171 | 172 | # print('New [](x,y,z): {:.2f}\t{:.2f}\t{:.2f}'.format(newpos[0], newpos[1], newpos[2])) 173 | return newpos 174 | 175 | def move_cg(self, leg, off, pt, leg_lift): 176 | """ 177 | There is a pattern of which direction to shift based on which leg you 178 | are trying to move and which leg is lifted (not currently providing 179 | support for the robot) 180 | leg - leg number 181 | off - offset in mm to shift cm 182 | leg_lift - which leg is moving through the air currently 183 | 184 | lift 0: x-d 1: y-d 2: x+d 3: y+d 185 | y+d x-d y-d x+d 186 | x+d y+d x-d y-d 187 | y-d x+d y+d x-d 188 | pattern: always x y x y (alternate) 189 | always - + + - 190 | """ 191 | axis = [0, 1, 0, 1] # 0=x, 1=y componets of 3d point 192 | offset = [-off, off, off, -off] # either adding or subtracting offset from point 193 | ia = axis[(leg+leg_lift) % 4] # us mod to rotate through each of these indexes 194 | io = (leg-leg_lift) % 4 195 | pt[ia] += offset[io] # shift point 196 | return pt 197 | # if leg == 0: 198 | # if leg_lift == 0: pt[0] -= offset # x-d 199 | # elif leg_lift == 1: pt[1] += offset # y+d 200 | # elif leg_lift == 2: pt[0] += offset # x+d 201 | # elif leg_lift == 3: pt[1] -= offset # y-d 202 | # elif leg == 0: 203 | # if leg_lift == 0: pt[1] -= offset # x-d 204 | # elif leg_lift == 1: pt[0] -= offset # y+d 205 | # elif leg_lift == 2: pt[1] += offset # x+d 206 | # elif leg_lift == 3: pt[0] += offset # y-d 207 | 208 | 209 | def oneCycle(self, x, y, rz): 210 | """ 211 | direction of travel x, y (2D plane) or rotation about z-axis 212 | Returns 1 complete cycle for all 4 feet (x,y,z) 213 | """ 214 | 215 | # check if x, y, rz is same as last time commanded, if so, return 216 | # the last cycle response, else, calculate a new response 217 | # ??? 218 | 219 | scale = self.scale 220 | cmd = (scale*x, scale*y, rz) 221 | ret = { 222 | 0: [], 223 | 1: [], 224 | 2: [], 225 | 3: [] 226 | } # 4 leg foot positions for the entire 12 count cycle is returned 227 | 228 | # iteration, there are 12 steps in gait cycle, add a 13th so all feet 229 | # are on the ground at the end, otherwise one foot is still in the air 230 | leg_lift = [0,0,0,3,3,3,1,1,1,2,2,2] 231 | for i in range(0, self.steps): 232 | for legNum in [0, 1, 2, 3]: # order them diagonally 233 | rcmd = rot_z_tuple(self.frame[legNum], cmd) 234 | index = (i + self.legOffset[legNum]) % self.steps 235 | pos = self.eachLeg(index, rcmd) # move each leg appropriately 236 | # print('Foot[{}]: {:.2f} {:.2f} {:.2f}'.format(legNum, *(pos))) 237 | 238 | # shift cg 239 | # fist and last shouldn't move cg?? 240 | # pos = self.move_cg(legNum, 40, pos, leg_lift[i]) 241 | 242 | ret[legNum].append(pos) 243 | 244 | if debug: 245 | print("=[Gait.py]===============================") 246 | for legNum in [0, 1, 2, 3]: 247 | print('Leg[{}]---------'.format(legNum)) 248 | for i, pt in enumerate(ret[legNum]): 249 | print(' {:2}: {:7.2f} {:7.2f} {:7.2f}'.format(i, *pt)) 250 | 251 | return ret 252 | 253 | 254 | # def oneCycle2(self, x, y, rz): 255 | # """ 256 | # direction of travel x, y (2D plane) or rotation about z-axis 257 | # Returns 1 complete cycle for all 4 feet (x,y,z) 258 | # """ 259 | # 260 | # # check if x, y, rz is same as last time commanded, if so, return 261 | # # the last cycle response, else, calculate a new response 262 | # # ??? 263 | # 264 | # scale = self.scale 265 | # cmd = (scale*x, scale*y, rz) 266 | # ret = { 267 | # 0: [], 268 | # 1: [], 269 | # 2: [], 270 | # 3: [] 271 | # } # 4 leg foot positions for the entire 12 count cycle is returned 272 | # 273 | # # iteration, there are 12 steps in gait cycle, add a 13th so all feet 274 | # # are on the ground at the end, otherwise one foot is still in the air 275 | # leg_lift = [0,0,0,3,3,3,1,1,1,2,2,2] 276 | # for i in range(0, self.steps)+[0]: 277 | # for legNum in [0, 1, 2, 3]: # order them diagonally 278 | # rcmd = rot_z_tuple(self.frame[legNum], cmd) 279 | # index = (i + self.legOffset[legNum]) % self.steps 280 | # pos = self.eachLeg(index, rcmd) # move each leg appropriately 281 | # # print('Foot[{}]: {:.2f} {:.2f} {:.2f}'.format(legNum, *(pos))) 282 | # 283 | # # shift cg 284 | # # fist and last shouldn't move cg?? 285 | # pos = self.move_cg(legNum, 40, pos, leg_lift[i]) 286 | # 287 | # ret[legNum].append(pos) 288 | # 289 | # if debug: 290 | # for legNum in [0, 1, 2, 3]: 291 | # print('Leg[{}]---------'.format(legNum)) 292 | # for i, pt in enumerate(ret[legNum]): 293 | # print(' {:2}: {:7.2f} {:7.2f} {:7.2f}'.format(i, *pt)) 294 | # 295 | # return ret 296 | 297 | # def oneCycle_alt2(self, x, y, rz): 298 | # """ 299 | # direction of travel x, y (2D plane) or rotation about z-axis 300 | # Returns 1 complete cycle for all 4 feet (x,y,z) 301 | # """ 302 | # 303 | # # check if x, y, rz is same as last time commanded, if so, return 304 | # # the last cycle response, else, calculate a new response 305 | # # ??? 306 | # 307 | # scale = self.scale 308 | # cmd = (scale*x, scale*y, rz) 309 | # ret = { 310 | # 0: [], 311 | # 1: [], 312 | # 2: [], 313 | # 3: [] 314 | # } # 4 leg foot positions for the entire 12 count cycle is returned 315 | # 316 | # speed = 100 317 | # 318 | # # iteration, there are 12 steps in gait cycle, add a 13th so all feet 319 | # # are on the ground at the end, otherwise one foot is still in the air 320 | # for i in range(0, self.steps)+[0]: 321 | # for legNum in [0, 1, 2, 3]: # order them diagonally 322 | # rcmd = rot_z_tuple(self.frame[legNum], cmd) 323 | # index = (i + self.legOffset[legNum]) % self.steps 324 | # pos = self.eachLeg(index, rcmd) # move each leg appropriately 325 | # # print('Foot[{}]: {:.2f} {:.2f} {:.2f}'.format(legNum, *(pos))) 326 | # ret[legNum].append((pos, speed,)) 327 | # 328 | # for legNum in [0, 1, 2, 3]: 329 | # print('Leg[{}]---------'.format(legNum)) 330 | # for i, pt in enumerate(ret[legNum]): 331 | # print(' {}:'.format(i), pt) 332 | # 333 | # return ret 334 | -------------------------------------------------------------------------------- /multiped/jsonFile.py: -------------------------------------------------------------------------------- 1 | ############################################## 2 | # The MIT License (MIT) 3 | # Copyright (c) 2016 Kevin Walchko 4 | # see LICENSE for full details 5 | ############################################## 6 | 7 | from __future__ import division 8 | from __future__ import print_function 9 | import simplejson as json 10 | 11 | 12 | class FileStorageError(Exception): 13 | pass 14 | 15 | 16 | class jsonFile(object): 17 | """ 18 | Store your API keys or other params in a json/yaml file and then import them 19 | with this class. You can also pass a dictionary to this and create a json/yaml 20 | file storage. All key/value pairs are stored in a dictionary called db. 21 | """ 22 | db = None 23 | 24 | def read(self, fname): 25 | """ 26 | Reads a Json file 27 | in: file name 28 | out: length of file, dictionary 29 | """ 30 | try: 31 | with open(fname, 'r') as f: 32 | data = json.load(f) 33 | 34 | self.db = data 35 | return len(self.db), data 36 | except IOError: 37 | raise FileStorageError('Could not open {0!s} for reading'.format((fname))) 38 | 39 | def write(self, fname, data=None): 40 | """ 41 | Writes a Json file 42 | """ 43 | try: 44 | if data is None: 45 | data = self.db 46 | 47 | with open(fname, 'w') as f: 48 | json.dump(data, f) 49 | 50 | except IOError: 51 | raise FileStorageError('Could not open {0!s} for writing'.format((fname))) 52 | 53 | def __getitem__(self, keyName): 54 | if keyName in self.db: 55 | return self.db[keyName] 56 | else: 57 | return None 58 | 59 | def __str__(self): 60 | s = [] 61 | for k, v in db.items(): 62 | s.append(str(k) + ': ' + str(v) + '\n') 63 | return ''.join(s) 64 | 65 | def __repr__(self): 66 | print(self.__str__()) 67 | 68 | def clear(self): 69 | self.db = None 70 | 71 | # 72 | # if __name__ == '__main__': 73 | # fs = jsonFile() 74 | # fs.read('walkingeye.json') 75 | # # db = fs.db 76 | # 77 | # print('serialPort', fs['serialPort']) 78 | # print('') 79 | # 80 | # print(fs) 81 | -------------------------------------------------------------------------------- /multiped/kinematics3.py: -------------------------------------------------------------------------------- 1 | # #!/usr/bin/env python 2 | # ############################################## 3 | # # The MIT License (MIT) 4 | # # Copyright (c) 2016 Kevin Walchko 5 | # # see LICENSE for full details 6 | # ############################################## 7 | # 8 | # from __future__ import print_function 9 | # from __future__ import division 10 | # # import numpy as np 11 | # from math import sin, cos, acos, atan2, sqrt, pi, fabs 12 | # from math import radians as d2r 13 | # from math import degrees as r2d 14 | # # import logging 15 | # from .Servo import Servo 16 | # 17 | # # logging.basicConfig(level=logging.DEBUG) 18 | # # logging.basicConfig(level=logging.ERROR) 19 | # 20 | # 21 | # class LegException(Exception): 22 | # pass 23 | # 24 | # 25 | # class Leg3(object): 26 | # """ 27 | # """ 28 | # # these are fixed by the 3D printing, not changing 29 | # coxaLength = 45.0 30 | # tibiaLength = 55.0 31 | # femurLength = 104.0 32 | # s_limits = [ 33 | # [-90, 90], # set limits 34 | # [-90, 90], 35 | # [-150, 0] 36 | # ] 37 | # s_offsets = [150, 150, 150+90] # angle offsets to line up with fk 38 | # 39 | # sit_raw = (150, 270, 100) 40 | # stand_raw = (150, 175, 172) 41 | # sit_angles = None 42 | # stand_angles = None 43 | # 44 | # def __init__(self, channels): 45 | # """ 46 | # Each leg has 3 servos/channels 47 | # """ 48 | # if not len(channels) == 3: 49 | # raise LegException('len(channels) != 3') 50 | # 51 | # Servo.bulkServoWrite = True 52 | # 53 | # # angle offsets to line up with fk 54 | # self.servos = [] 55 | # for i in range(0, 3): 56 | # self.servos.append(Servo(channels[i])) 57 | # self.servos[i].setServoLimits(self.s_offsets[i], *self.s_limits[i]) 58 | # 59 | # self.sit_angles = self.convertRawAngles(*self.sit_raw) 60 | # initAngles = self.convertRawAngles(*self.stand_raw) 61 | # self.stand_angles = initAngles 62 | # self.foot0 = self.fk(*initAngles) # rest/idle position of the foot/leg 63 | # 64 | # def __del__(self): 65 | # pass 66 | # 67 | # def sit(self): 68 | # self.moveFootAngles(*self.sit_angles) 69 | # 70 | # def stand(self): 71 | # self.moveFootAngles(*self.stand_angles) 72 | # 73 | # def convertRawAngles(self, a, b, c): 74 | # return (a-self.s_offsets[0], b-self.s_offsets[1], c-self.s_offsets[2]) 75 | # 76 | # def fk(self, a, b, g): 77 | # """ 78 | # Forward kinematics of the leg, note, angle are all degrees 79 | # """ 80 | # Lc = self.coxaLength 81 | # Lf = self.femurLength 82 | # Lt = self.tibiaLength 83 | # 84 | # a = d2r(a) 85 | # b = d2r(b) 86 | # g = d2r(g) 87 | # 88 | # foot = [ 89 | # (Lc + Lf*cos(b) + Lt*cos(b + g))*cos(a), 90 | # (Lc + Lf*cos(b) + Lt*cos(b + g))*sin(a), 91 | # Lf*sin(b) + Lt*sin(b + g) 92 | # ] 93 | # 94 | # # return np.array(foot) 95 | # return foot 96 | # 97 | # def ik(self, x, y, z): 98 | # """ 99 | # Calculates the inverse kinematics from a given foot coordinate (x,y,z)[mm] 100 | # and returns the joint angles[degrees] 101 | # 102 | # Reference (there are typos) 103 | # https://tote.readthedocs.io/en/latest/ik.html 104 | # 105 | # If the foot (x,y,z) is in a position that does not produce a result (some 106 | # numeric error or invalid foot location), the this returns None. 107 | # """ 108 | # # try: 109 | # Lc = self.coxaLength 110 | # Lf = self.femurLength 111 | # Lt = self.tibiaLength 112 | # 113 | # if sqrt(x**2 + y**2) < Lc: 114 | # print('too short') 115 | # return None 116 | # # elif z > 0.0: 117 | # # return None 118 | # 119 | # a = atan2(y, x) 120 | # f = sqrt(x**2 + y**2) - Lc 121 | # # b1 = atan2(z, f) # takes into account quadrent, z is neg 122 | # 123 | # # you have different conditions depending if z is pos or neg 124 | # if z < 0.0: 125 | # b1 = atan2(f, fabs(z)) 126 | # else: 127 | # b1 = atan2(z, f) 128 | # 129 | # d = sqrt(f**2 + z**2) # <--- 130 | # 131 | # # print('ik pos: {} {} {}'.format(x,y,z)) 132 | # # print('d: {:.3f} f: {:.3f}'.format(d,f)) 133 | # # print('Lc Lf Lt: {} {} {}'.format(Lc,Lf,Lt)) 134 | # # print('num: {:.3f}'.format(Lf**2 + d**2 - Lt**2)) 135 | # # print('den: {:.3f}'.format(2.0 * Lf * d)) 136 | # # print('acos: {:.2f}'.format((Lf**2 + d**2 - Lt**2) / (2.0 * Lf * d))) 137 | # 138 | # guts = ((Lf**2 + d**2 - Lt**2) / (2.0 * Lf * d)) 139 | # if 1.0 < guts or guts < -1.0: 140 | # print('acos crap!: {:.3f} {:.3f} {:.3f} guts: {:.3f}'.format(x, y, z, guts)) 141 | # return None 142 | # b2 = acos((Lf**2 + d**2 - Lt**2) / (2.0 * Lf * d)) # issues? 143 | # b = b1 + b2 144 | # g = acos((Lf**2 + Lt**2 - d**2) / (2.0 * Lf * Lt)) 145 | # 146 | # # FIXES ################################### 147 | # g -= pi # fix to align fk and ik frames 148 | # 149 | # if z < 0.0: 150 | # b -= pi/2 # 151 | # ############################################## 152 | # # print('b1 b2: {:.2f} {:.2f}'.format(r2d(b1), r2d(b2))) 153 | # # print('ik angles: {:.2f} {:.2f} {:.2f}'.format(r2d(a), r2d(b), r2d(g))) 154 | # return r2d(a), r2d(b), r2d(g) # coxaAngle, femurAngle, tibiaAngle 155 | # 156 | # # except Exception as e: 157 | # # print('ik error:', e) 158 | # # raise e 159 | # 160 | # def moveFoot(self, x, y, z): 161 | # """ 162 | # Attempts to move it's foot to coordinates [x,y,z] 163 | # """ 164 | # try: 165 | # # a, b, c = self.ik(x, y, z) # inverse kinematics 166 | # # angles = [a, b, c] 167 | # angles = self.ik(x, y, z) # inverse kinematics 168 | # # return angles 169 | # if angles is None: 170 | # print('something bad') 171 | # return 172 | # # print('angles: {:.2f} {:.2f} {:.2f}'.format(*angles)) 173 | # for i, servo in enumerate(self.servos): 174 | # # print('i, servo:', i, servo) 175 | # angle = angles[i] 176 | # # print('servo {} angle {}'.format(i, angle)) 177 | # servo.angle = angle 178 | # return angles 179 | # 180 | # except Exception as e: 181 | # print (e) 182 | # raise 183 | # 184 | # def moveFootAngles(self, a, b, c): 185 | # """ 186 | # Attempts to move it's foot to coordinates [x,y,z] 187 | # """ 188 | # try: 189 | # for servo, angle in zip(self.servos, (a, b, c)): 190 | # servo.angle = angle 191 | # 192 | # except Exception as e: 193 | # print('Leg::moveFootAngles() error:', e) 194 | # raise 195 | # 196 | # # def reset(self): 197 | # # # self.angles = self.resting_position 198 | # # self.move(*self.foot0) 199 | -------------------------------------------------------------------------------- /multiped/kinematics4.py: -------------------------------------------------------------------------------- 1 | ############################################## 2 | # The MIT License (MIT) 3 | # Copyright (c) 2016 Kevin Walchko 4 | # see LICENSE for full details 5 | ############################################## 6 | 7 | from __future__ import print_function 8 | from __future__ import division 9 | from math import sin, cos, acos, atan2, sqrt, pi 10 | from math import radians as d2r 11 | # from math import degrees as r2d 12 | # import logging 13 | # from quadruped.Servo import Servo 14 | 15 | # logging.basicConfig(level=logging.DEBUG) 16 | # logging.basicConfig(level=logging.ERROR) 17 | 18 | # from collections import namedtuple 19 | # 20 | # Link = namedtuple('Link', 'length offset') 21 | # Leg4Info = namedtuple('Leg4Info', 'coxa femur tibia fibia tarsus') 22 | 23 | 24 | # def ramp(val, length): 25 | # """ 26 | # Simple triangle for scaling speed. it always returns 0.5 at the lowest 27 | # and is 1.0 at max in the middle 28 | # 29 | # in: val - step 30 | # length - number of steps 31 | # out: 0.5 - 1.0 32 | # """ 33 | # val = val % length 34 | # # print("ramp: {} {} {}".format(val, length/2, length)) 35 | # slope = 0.5/(length/2) 36 | # if val > length/2: 37 | # # since it is symetric, just mirror the answer 38 | # val = (length - val) 39 | # return slope*val + 0.5 40 | 41 | 42 | class KinematicsException(Exception): 43 | pass 44 | 45 | 46 | class Kinematics4(object): 47 | """ 48 | Leg class outputs the servo angles for some requested foot location (x,y,z) 49 | 50 | Leg knows: 51 | - leg dimensions 52 | - number of servos and their parameters/limits 53 | - fk/ik equations 54 | - sit/stand sequence 55 | """ 56 | # these are fixed by the 3D printing, not changing 57 | coxaLength = None 58 | tibiaLength = None 59 | femurLength = None 60 | tarsusLength = None 61 | 62 | # positions = { 63 | # 'stand': None, 64 | # # 'sit': None, 65 | # # 'neutral': None 66 | # } 67 | 68 | def __init__(self, params): 69 | """ 70 | Each leg has 4 servos/channels 71 | """ 72 | # # setup kinematics and servos 73 | # self.servos = [] 74 | # for ID, seg in enumerate(['coxa', 'femur', 'tibia', 'tarsus']): 75 | # self.servos.append(Servo(ID, params[seg][1], params[seg][2])) 76 | 77 | self.coxaLength = params['coxa'][0] 78 | self.femurLength = params['femur'][0] 79 | self.tibiaLength = params['tibia'][0] 80 | self.tarsusLength = params['tarsus'][0] 81 | 82 | # if 'stand' in params: 83 | # self.positions['neutral'] = self.forward(*params['stand']) 84 | # else: 85 | # raise Exception('Need to have "stand" angles in params file') 86 | 87 | def __del__(self): 88 | pass 89 | 90 | def forward(self, t1, t2, t3, t4, degrees=True): 91 | """ 92 | Forward kinematics of the leg, note, default angles are all degrees. 93 | The input angles are referenced to the DH frame arrangement. 94 | """ 95 | l1 = self.coxaLength 96 | l2 = self.femurLength 97 | l3 = self.tibiaLength 98 | l4 = self.tarsusLength 99 | 100 | if degrees: 101 | t1 = d2r(t1) 102 | t2 = d2r(t2) 103 | t3 = d2r(t3) 104 | t4 = d2r(t4) 105 | 106 | x = (l1 + l2*cos(t2) + l3*cos(t2 + t3) + l4*cos(t2 + t3 + t4))*cos(t1) 107 | y = (l1 + l2*cos(t2) + l3*cos(t2 + t3) + l4*cos(t2 + t3 + t4))*sin(t1) 108 | z = l2*sin(t2) + l3*sin(t2 + t3) + l4*sin(t2 + t3 + t4) 109 | 110 | return (x, y, z,) 111 | 112 | def inverse(self, x, y, z, o=90, degrees=True): 113 | """ 114 | Azimuth angle is between x and w and lies in the x-y plane 115 | 116 | ^ x 117 | w | 118 | \ | 119 | l1 \ | 120 | \ | 121 | \| 122 | <----------+ (z is out of the page - right hand rule) 123 | y 124 | 125 | Most of the robot arm move in the plane defined by w-z 126 | 127 | ^ z l3 128 | | o-----o 129 | | / \ l4 130 | | / l2 E 131 | | / 132 | +--o-------------> w 133 | l1 134 | 135 | l1: coxa 136 | l2: femur 137 | l3: tibia 138 | l4: tarsus 139 | 140 | All joint angles returned are in degrees: (t1, t2, t3, t4) 141 | """ 142 | def cosinelaw(a, b, c): 143 | # cosine law only used by this function 144 | # cos(g) = (a^2+b^2-c^2)/2ab 145 | try: 146 | ans = acos((a**2+b**2-c**2)/(2*a*b)) 147 | except ValueError: 148 | print("num: {}".format(a**2+b**2-c**2)) 149 | print("den: {}".format(2*a*b)) 150 | print("acos({})".format((a**2+b**2-c**2)/(2*a*b))) 151 | raise 152 | return ans 153 | 154 | l1 = self.coxaLength 155 | l2 = self.femurLength 156 | l3 = self.tibiaLength 157 | l4 = self.tarsusLength 158 | 159 | t1 = atan2(y, x) 160 | 161 | if degrees: 162 | o = o*pi/180 163 | 164 | try: 165 | w = sqrt(x**2 + y**2) - l1 166 | j4w = w + l4*cos(o) 167 | j4z = z + l4*sin(o) 168 | r = sqrt(j4w**2 + j4z**2) 169 | g1 = atan2(j4z, j4w) 170 | g2 = cosinelaw(l2, r, l3) 171 | t2 = g1+g2 172 | 173 | t3 = pi+cosinelaw(l2, l3, r) 174 | 175 | j2w = l2*cos(t2) 176 | j2z = l2*sin(t2) 177 | c = sqrt((w-j2w)**2 + (z-j2z)**2) 178 | t4 = pi+cosinelaw(l3, l4, c) 179 | except Exception as e: 180 | print('inverse({:.2f},{:.2f},{:.2f},{:.2f})'.format(x, y, z, o)) 181 | print('Error:', e) 182 | raise 183 | 184 | def check(t): 185 | if t > 150*pi/180: 186 | t -= 2*pi 187 | elif t < -150*pi/180: 188 | t += 2*pi 189 | return t 190 | 191 | # maxa = 150*pi/180 192 | # t1 = t1 if t1 <= maxa else t1-2*pi 193 | 194 | t1 = check(t1) # value?? check elsewhere? 195 | t2 = check(t2) 196 | t3 = check(t3) 197 | t4 = check(t4) 198 | 199 | if degrees: 200 | t1 *= 180/pi 201 | t2 *= 180/pi 202 | t3 *= 180/pi 203 | t4 *= 180/pi 204 | 205 | return (t1, t2, t3, t4) 206 | 207 | def generateDHAngles(self, footLoc, speed): 208 | """ 209 | This is a bulk process and takes all of the foot locations for an entire 210 | sequence of a gait cycle. It handles all legs at once. 211 | 212 | speed: this is the max movement speed 213 | 214 | footLoc: locations of feet from gait 215 | { step0 step1 ... 216 | 0: [(x,y,z), (x,y,z), ...] # leg0 217 | 2: [(x,y,z), (x,y,z), ...] # leg2 218 | ... 219 | } 220 | 221 | return 222 | { step 0 step 1 ... 223 | 0: [(t1,t2,t3,t4,speed), (t1,t2,t3,t4,speed), ...] # leg0 224 | 2: [(t1,t2,t3,t4,speed), (t1,t2,t3,t4,speed), ...] # leg2 225 | ... 226 | } where t=theta in DH space 227 | """ 228 | 229 | # get the keys and figure out some stuff 230 | keys = list(footLoc.keys()) 231 | angles = {} 232 | 233 | print("=[generateServoAngles2 speed servo[0-3]]===================") 234 | for legNum in keys: 235 | print("Leg[{}]-------------".format(legNum)) 236 | pos = footLoc[legNum] # grab foot positions for leg k 237 | angles[legNum] = [] 238 | 239 | # calculate the inverse DH angles 240 | for step, p in enumerate(pos): 241 | s = self.inverse(*p) # s0,s1,s2,s3 242 | scaled_speed = speed 243 | angles[legNum].append(s + (scaled_speed,)) 244 | print(" {:2}: {} {:7.2f} {:7.2f} {:7.2f} {:7.2f}".format(step, scaled_speed, *s)) 245 | 246 | return angles 247 | 248 | 249 | # def generateServoAngles(self, footLoc, speed): 250 | # """ 251 | # This is a bulk process and takes all of the foot locations for an entire 252 | # sequence of a gait cycle. It handles all legs at once. 253 | # 254 | # speed: this is the max movement speed 255 | # 256 | # footLoc: locations of feet from gait 257 | # { step0 step1 ... 258 | # 0: [(x,y,z), (x,y,z), ...] # leg0 259 | # 2: [(x,y,z), (x,y,z), ...] # leg2 260 | # ... 261 | # } 262 | # 263 | # return 264 | # { step 0 step 1 ... 265 | # 0: [[(t1,s1),(t2,s2),(t3,s3),(t4,s4)], [(t1,s1),(t2,s2),(t3,s3),(t4,s4)], ...] # leg0 266 | # 2: [[(t1,s1),(t2,s2),(t3,s3),(t4,s4)], [(t1,s1),(t2,s2),(t3,s3),(t4,s4)], ...] # leg2 267 | # ... 268 | # } where t=theta s=speed 269 | # """ 270 | # # FIXME: fix this to handle N legs, right now it only does 4 271 | # 272 | # # get the keys and figure out some stuff 273 | # keys = list(footLoc.keys()) 274 | # angles = {} 275 | # 276 | # for k in keys: 277 | # pos = footLoc[k] # grab foot positions for leg k 278 | # angles[k] = [] 279 | # 280 | # # calculate the inverse DH angles 281 | # numStep = len(pos) 282 | # for step, p in enumerate(pos): 283 | # s = self.inverse(*p) # s0,s1,s2,s3 284 | # tmp = self.DH2Servo(s) 285 | # # scaled_speed = int(speed*ramp(step, numStep)) 286 | # # if p[2] > -70: scaled_speed = speed 287 | # # else: scaled_speed = int(0.6*speed) 288 | # scaled_speed = speed 289 | # tmp2 = [(x, scaled_speed) for x in tmp] 290 | # angles[k].append(tmp2) 291 | # # print("speed", speed) 292 | # # print("tmp", tmp) 293 | # # exit(0) 294 | # 295 | # return angles 296 | 297 | 298 | # def generateServoAngles2(self, footLoc, speed): 299 | # """ 300 | # This is a bulk process and takes all of the foot locations for an entire 301 | # sequence of a gait cycle. It handles all legs at once. 302 | # 303 | # speed: this is the max movement speed 304 | # 305 | # footLoc: locations of feet from gait 306 | # { step0 step1 ... 307 | # 0: [(x,y,z), (x,y,z), ...] # leg0 308 | # 2: [(x,y,z), (x,y,z), ...] # leg2 309 | # ... 310 | # } 311 | # 312 | # return 313 | # { step 0 step 1 ... 314 | # 0: [(t1,t2,t3,t4,speed), (t1,t2,t3,t4,speed), ...] # leg0 315 | # 2: [(t1,t2,t3,t4,speed), (t1,t2,t3,t4,speed), ...] # leg2 316 | # ... 317 | # } where t=theta 318 | # """ 319 | # 320 | # # get the keys and figure out some stuff 321 | # keys = list(footLoc.keys()) 322 | # angles = {} 323 | # 324 | # print("=[generateServoAngles2 speed servo[0-3]]===================") 325 | # for legNum in keys: 326 | # print("Leg[{}]-------------".format(legNum)) 327 | # pos = footLoc[legNum] # grab foot positions for leg k 328 | # angles[legNum] = [] 329 | # # print('pos', pos) 330 | # 331 | # # calculate the inverse DH angles 332 | # # numStep = len(pos) 333 | # for step, p in enumerate(pos): 334 | # # print(' {:2}: {:7.2f} {:7.2f} {:7.2f}'.format(i, *pt)) 335 | # # print('step: {} p: {}'.format(step, p)) 336 | # s = self.inverse(*p) # s0,s1,s2,s3 337 | # tmp = self.DH2Servo(s) 338 | # # scaled_speed = int(speed*ramp(step, numStep)) 339 | # # if p[2] > -70: scaled_speed = speed 340 | # # else: scaled_speed = int(0.6*speed) 341 | # # tmp2 = [(x, scaled_speed) for x in tmp] 342 | # scaled_speed = speed 343 | # angles[legNum].append(tmp + (scaled_speed,)) 344 | # # print("speed", speed) 345 | # # print("tmp", tmp) 346 | # # # exit(0) 347 | # # print(" {:2}: {:7.2f} {:7.2f} {:7.2f} {:7.2f}".format(step, *tmp)) 348 | # print(" {:2}: {} {:7.2f} {:7.2f} {:7.2f} {:7.2f}".format(step, scaled_speed, *tmp)) 349 | # 350 | # return angles 351 | 352 | # def DH2Servo(self, angles): 353 | # tmp = [] 354 | # for s, a in list(zip(self.servos, angles)): 355 | # tmp.append(s.DH2Servo(a)) 356 | # return tuple(tmp) 357 | 358 | def pprint(self, step): 359 | print('*'*25) 360 | for leg in step: 361 | print(' DH: [{:.0f} {:.0f} {:.0f} {:.0f}]'.format(*leg)) 362 | 363 | # def getNeutralPos(self): 364 | # return self.positions['neutral'] 365 | 366 | # def generateServoAngles_DH(self, angles): 367 | # """ 368 | # This is a bulk process and takes all of the foot locations for an entire 369 | # sequence of a gait cycle. It handles all legs at once. 370 | # 371 | # footLoc: locations of feet from gait 372 | # { step0 step1 ... 373 | # 0: [[t1,t2,t3,t4], [t1,t2,t3,t4], ...] # leg0 DH space 374 | # 2: [[t1,t2,t3,t4], [t1,t2,t3,t4], ...] # leg2 DH space 375 | # ... 376 | # } 377 | # 378 | # return 379 | # { step 0 step 1 ... 380 | # 0: [[t1,t2,t3,t4], [t1,t2,t3,t4], ...] # leg0 servo space 381 | # 2: [[t1,t2,t3,t4], [t1,t2,t3,t4], ...] # leg2 servo space 382 | # ... 383 | # } 384 | # """ 385 | # # get the keys and figure out some stuff 386 | # keys = list(footLoc.keys()) 387 | # angles = {} 388 | # 389 | # for k in keys: 390 | # angles[k] = [] 391 | # # calculate the inverse DH angles 392 | # for s in angles: 393 | # # tmp = [0]*4 394 | # # tmp[0] = self.servos[0].DH2Servo(s[0]) 395 | # # tmp[1] = self.servos[1].DH2Servo(s[1]) 396 | # # tmp[2] = self.servos[2].DH2Servo(s[2]) 397 | # # tmp[3] = self.servos[3].DH2Servo(s[3]) 398 | # tmp = self.DH2Servo(s) 399 | # angles[k].append(tmp) 400 | # 401 | # return angles 402 | 403 | # def generateServoAngles(self, footLoc): 404 | # """ 405 | # This is a bulk process and takes all of the foot locations for an entire 406 | # sequence of a gait cycle. It handles all legs at once. 407 | # 408 | # footLoc: locations of feet from gait 409 | # { step0 step1 ... 410 | # 0: [(x,y,z), (x,y,z), ...] # leg0 411 | # 2: [(x,y,z), (x,y,z), ...] # leg2 412 | # ... 413 | # } 414 | # 415 | # return 416 | # { step 0 step 1 ... 417 | # 0: [[t1,t2,t3,t4], [t1,t2,t3,t4], ...] # leg0 418 | # 2: [[t1,t2,t3,t4], [t1,t2,t3,t4], ...] # leg2 419 | # ... 420 | # } 421 | # """ 422 | # # get the keys and figure out some stuff 423 | # keys = list(footLoc.keys()) 424 | # angles = {} 425 | # 426 | # for k in keys: 427 | # pos = footLoc[k] # grab foot positions for leg k 428 | # angles[k] = [] 429 | # 430 | # # calculate the inverse DH angles 431 | # for p in pos: 432 | # s = self.inverse(*p) # s0,s1,s2,s3 433 | # tmp = self.DH2Servo(s) 434 | # angles[k].append(tmp) 435 | # 436 | # return angles 437 | -------------------------------------------------------------------------------- /multiped/robot.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | ############################################## 4 | # The MIT License (MIT) 5 | # Copyright (c) 2016 Kevin Walchko 6 | # see LICENSE for full details 7 | ############################################## 8 | # Simple robot 9 | 10 | from __future__ import print_function 11 | from __future__ import division 12 | from multiped import Engine 13 | from multiped import DiscreteRippleGait 14 | # import time 15 | ########################## 16 | 17 | 18 | class Robot(object): 19 | """ 20 | This is a simple class that binds the others together. You could make it 21 | a base class for something fancier. 22 | """ 23 | def __init__(self, data, legType, servoType): 24 | """ 25 | Constructor. 26 | Engine - commands the leg servos to move 27 | gait - Ideally there are several types of gaits for different 28 | walking/running situations. If you don't give it an dict 29 | of gaits, then the default is just the standard 30 | DiscreteRippleGait. 31 | The gaits need to know: 32 | - how high to pick up a leg 33 | - what the neutral leg position is (where is the foot 34 | located when just standing?) 35 | """ 36 | self.engine = Engine(data, servoType) 37 | self.kinematics = legType(data) 38 | 39 | neutral = self.kinematics.getNeutralPos() 40 | 41 | if 'gaits' in data: 42 | self.gaits = data['gaits'] 43 | else: 44 | self.gaits = { 45 | 'crawl': DiscreteRippleGait(65.0, neutral) 46 | } 47 | -------------------------------------------------------------------------------- /multiped/servo.py: -------------------------------------------------------------------------------- 1 | ############################################## 2 | # The MIT License (MIT) 3 | # Copyright (c) 2016 Kevin Walchko 4 | # see LICENSE for full details 5 | ############################################## 6 | 7 | 8 | class Servo(object): 9 | """ 10 | Servo hold the parameters of a servo, it doesn't talk to real servos. This 11 | class does the conversion between DH angles to real servo angles 12 | """ 13 | 14 | def __init__(self, ID, offset=150): 15 | self.offset = offset 16 | # self.minAngle = minmax[0] # DH space 17 | # self.maxAngle = minmax[1] # DH space 18 | self.id = ID 19 | 20 | def DH2Servo(self, angle): 21 | """ 22 | Sets the servo angle and clamps it between [limitMinAngle, limitMaxAngle]. 23 | """ 24 | sangle = angle+self.offset 25 | if sangle > 300 or sangle < 0: 26 | raise Exception('{} angle out of range DH[-150,150]: {:.1f} Servo[0,300]: {:.1f} deg'.format(self.id, angle, sangle)) 27 | return sangle 28 | -------------------------------------------------------------------------------- /pics/ax-12a-spider.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MultipedRobotics/multiped/17c2023e577cb6593b3f5a5fcbd5a9a57a5da46a/pics/ax-12a-spider.jpg -------------------------------------------------------------------------------- /pics/quad-movement.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MultipedRobotics/multiped/17c2023e577cb6593b3f5a5fcbd5a9a57a5da46a/pics/quad-movement.png -------------------------------------------------------------------------------- /pics/rc-spider.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MultipedRobotics/multiped/17c2023e577cb6593b3f5a5fcbd5a9a57a5da46a/pics/rc-spider.jpg -------------------------------------------------------------------------------- /pics/xl-320-spider.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MultipedRobotics/multiped/17c2023e577cb6593b3f5a5fcbd5a9a57a5da46a/pics/xl-320-spider.jpg -------------------------------------------------------------------------------- /pyproject.toml: -------------------------------------------------------------------------------- 1 | [tool.poetry] 2 | name = "multiped" 3 | version = "0.5.2" 4 | description = "Yet another robot python driver" 5 | authors = ["walchko "] 6 | readme = "readme.md" 7 | license = "MIT" 8 | homepage = "https://pypi.org/project/multiped/" 9 | repository = 'http://github.com/MultipedRobotics/multiped' 10 | # documentation = "http://..." 11 | keywords = ["dynamixel", "servo", "smart", "multiped", "hexapod", "quadruped"] 12 | classifiers = [ 13 | 'Development Status :: 4 - Beta', 14 | 'Intended Audience :: Developers', 15 | 'License :: OSI Approved :: MIT License', 16 | 'Operating System :: OS Independent', 17 | 'Programming Language :: Python :: 3.6', 18 | 'Programming Language :: Python :: 3.7', 19 | 'Programming Language :: Python :: 3.8', 20 | 'Programming Language :: Python :: 3.9', 21 | 'Topic :: Software Development :: Libraries', 22 | 'Topic :: Software Development :: Libraries :: Python Modules', 23 | 'Topic :: Software Development :: Libraries :: Application Frameworks' 24 | ] 25 | 26 | [tool.poetry.dependencies] 27 | python = ">=3.6" 28 | # mdh = "*" 29 | colorama = "*" 30 | pyservos = "*" 31 | simplejson = "*" 32 | 33 | [tool.poetry.dev-dependencies] 34 | pytest = "*" 35 | 36 | [build-system] 37 | requires = ["poetry>=0.12"] 38 | build-backend = "poetry.masonry.api" 39 | -------------------------------------------------------------------------------- /readme.md: -------------------------------------------------------------------------------- 1 | # Multiped 2 | 3 | [![](https://img.shields.io/pypi/v/quadruped.svg)](https://github.com/MultipedRobotics/multiped) 4 | [![](https://img.shields.io/pypi/l/quadruped.svg)](https://github.com/MultipedRobotics/multiped) 5 | 6 | **Still Under Development** 7 | 8 | My walking robot software. 9 | 10 | Nature loves animals/insects with legs. Even birds have legs! Legs allow for easier 11 | navigation across rough terrain. The more legs you have, the more redundant and 12 | robust you are. If you are a centipede and you break a few legs, no big deal. 13 | However, if you are a human and break one leg, then walking (forget running) 14 | becomes extremely difficult. 15 | 16 | In robotics you see many different types of walking robots. Common ones are: 17 | 2 legs, four legs, and 6 legs. There are advantages and disadvantages for each 18 | of these types of robots. 19 | 20 | - 2 legs 21 | - balancing and shifting the body's center of mass is critical to the 22 | robot not falling over 23 | - the 2 legs are generally need to be more powerful to lift the body mass 24 | - 4 legs: 25 | - there is an inherent stability with this configuration. You only need 3 26 | legs (tripod) to keep the robot standing, leaving one leg to move freely 27 | as needed 28 | - each of the legs can be weaker than its 2 legged counter part 29 | - However, there is still a lot of motors, data lines, coordination complexity 30 | than if you only had 2 legs 31 | - 6 (or more) legs: 32 | - having more than 4 legs contains all of the same advantages/disadvantages 33 | of 4 legs 34 | - you also have more redundancy, essentially able to loose 2 legs and keep 35 | walking 36 | - there are also more types of gaits available to you, which is a fancy word 37 | for how you control your leg moves when walking, trotting, running, etc 38 | - You can even use some of your lets as arms to move objects around while still 39 | maintaining a stable tripod stance 40 | - However, more motors, more weight, more power consumption drives you to needing 41 | larger and more expensive batteries 42 | - at some point (maybe a centipede) more legs doesn't really add any advantage 43 | 44 | [Robot based on Robotis XL320 Servos](https://github.com/MultipedRobotics/quadruped-XL320): 45 | 46 | * [YouTube](https://www.youtube.com/watch?v=kH2hlxUfCNg) 47 | * [Vimeo](https://player.vimeo.com/video/194676675) 48 | 49 | [Robot based on Robotis AX12 Servos](https://github.com/MultipedRobotics/quadruped-AX12) 50 | 51 | * Not fully working yet 52 | 53 | ## Required Software 54 | 55 | This software requires [`pyservos`](https://github.com/MomsFriendlyRobotCompany/pyservos) 56 | to run. This software talks to both the XL-320 and AX-12A servos from Robotis. 57 | 58 | # Class Layout 59 | 60 | ![](pics/quad-movement.png) 61 | 62 | Here is *sort* of the layout of the code: 63 | 64 | ``` 65 | cmd 3d pts DH angles servo packet 66 | robot --> gait -----> legs --------> engine -----------> servos 67 | ``` 68 | 69 | - **Robot(data):** 70 | - Holds all of the other classes: Kinematics, Gait, Engine 71 | - Holds all sensors 72 | - Runs any logic/AI for the robot 73 | - **Gait:** 74 | - Gait calculates the foot positions for 1 cycle of a movement 75 | - All values are in (x,y,z) of units mm 76 | - command() - plans all feet through 1 gait cycle (say 15 steps) 77 | - **Engine:** 78 | - Engine takes the output from kinematics and translates it to servo space 79 | for each time stop and each leg in the cycle. It then sends the commands 80 | (in bulk) to move the servos each step 81 | - legs[4] 82 | - servos[3-4] 83 | - current angle 84 | - ID 85 | - angle offset (based on kinematics to align with DH reference frame) 86 | - **Kinematics:** 87 | Takes the 3d positions from gait and performs the inverse kinematics 88 | - leg parts: coxa, femur, tibia, *[tarsus]* 89 | - *forward()* - forward kinematics 90 | - *inverse()* - inverse kinematics 91 | 92 | The example quadruped (in the examples folder), takes a dictionary (or you can 93 | use a json file). Currently it takes: 94 | 95 | ```json 96 | data = { 97 | "numLegs": 4, 98 | "numServosPerLeg": 4, 99 | "coxa": [52, 150], 100 | "femur": [90, 123], 101 | "tibia": [89, 194], 102 | "tarsus": [90, 167], 103 | 104 | "sit": [80, 0, 1], 105 | "stand": [120, 0, -70], 106 | 107 | "servoType": 1, 108 | "bcm_pin": 11, 109 | "serialPort": "/dev/tty.usbserial-AL034G2K" 110 | } 111 | ``` 112 | 113 | If you don't pass it a serial port, then it falls back to a simulated serial 114 | port which does nothing but is useful for testing. 115 | 116 | ## Bulk Writing 117 | 118 | Normally, you would send an individual command to each servo and get a response 119 | back from it. That creates a lot of back an forth communications. Instead, 120 | this library uses a bulk write to send commands to all servos at once with no reply. 121 | This results in smoother motion and greatly reduced data transmission. 122 | 123 | ## Building and Documentation 124 | 125 | Details for how to build the robot and other information are in the `docs` folder 126 | in the [git repo](https://github.com/MultipedRobotics/multiped/tree/master/docs) 127 | 128 | # Tools 129 | 130 | This directory contains several tools for the robot: 131 | 132 | - get_leg_angles.py: prints out the joint angles for all 4 legs 133 | 134 | ```bash 135 | $ ./get_leg_angles.py /dev/tty.usbserial-AL034G2K 136 | Opened /dev/tty.usbserial-AL034G2K @ 1000000 137 | 138 | Servos: 1 - 12 139 | All angles are in degrees 140 | Leg 1 | Leg 2 | Leg 3 | Leg 4 | 141 | ID | Angle | ID | Angle | ID | Angle | ID | Angle | 142 | ----------------------------------------------------------------- 143 | 1 | 149.56 | 4 | 149.56 | 7 | 149.56 | 10 | 149.56 144 | 2 | 239.88 | 5 | 271.55 | 8 | 269.79 | 11 | 270.38 145 | 3 | 99.41 | 6 | 100.29 | 9 | 100.00 | 12 | 99.41 146 | ----------------------------------------------------------------- 147 | ``` 148 | 149 | - get_leg_info.py: prints out servo information for all 12 servos on the robot 150 | 151 | 152 | ```bash 153 | $ ./get_leg_info.py /dev/tty.usbserial-AL034G2K 154 | Opened /dev/tty.usbserial-AL034G2K @ 1000000 155 | 156 | Servos: 1 - 12 157 | -------------------------------------------------- 158 | Servo: 1 HW Error: 0 159 | Position [deg]: 149.6 Load: 0.0% CCW 160 | Voltage [V] 7.0 Temperature [F]: 80.6 161 | -------------------------------------------------- 162 | Servo: 2 HW Error: 0 163 | Position [deg]: 239.6 Load: 0.0% CCW 164 | Voltage [V] 7.2 Temperature [F]: 80.6 165 | -------------------------------------------------- 166 | Servo: 3 HW Error: 0 167 | Position [deg]: 99.4 Load: 0.0% CCW 168 | Voltage [V] 7.2 Temperature [F]: 82.4 169 | -------------------------------------------------- 170 | Servo: 4 HW Error: 0 171 | Position [deg]: 149.6 Load: 0.0% CCW 172 | Voltage [V] 7.3 Temperature [F]: 80.6 173 | -------------------------------------------------- 174 | Servo: 5 HW Error: 0 175 | Position [deg]: 271.6 Load: 0.0% CCW 176 | Voltage [V] 7.2 Temperature [F]: 80.6 177 | -------------------------------------------------- 178 | Servo: 6 HW Error: 0 179 | Position [deg]: 100.3 Load: 0.0% CCW 180 | Voltage [V] 7.4 Temperature [F]: 82.4 181 | -------------------------------------------------- 182 | Servo: 7 HW Error: 0 183 | Position [deg]: 149.6 Load: 0.0% CCW 184 | Voltage [V] 7.2 Temperature [F]: 80.6 185 | -------------------------------------------------- 186 | Servo: 8 HW Error: 0 187 | Position [deg]: 269.8 Load: 0.0% CCW 188 | Voltage [V] 7.1 Temperature [F]: 78.8 189 | -------------------------------------------------- 190 | Servo: 9 HW Error: 0 191 | Position [deg]: 99.4 Load: 0.8% CCW 192 | Voltage [V] 7.2 Temperature [F]: 82.4 193 | -------------------------------------------------- 194 | Servo: 10 HW Error: 0 195 | Position [deg]: 149.9 Load: 0.0% CCW 196 | Voltage [V] 7.1 Temperature [F]: 80.6 197 | -------------------------------------------------- 198 | Servo: 11 HW Error: 0 199 | Position [deg]: 270.1 Load: 0.0% CCW 200 | Voltage [V] 7.2 Temperature [F]: 80.6 201 | -------------------------------------------------- 202 | Servo: 12 HW Error: 0 203 | Position [deg]: 99.4 Load: 0.0% CCW 204 | Voltage [V] 7.1 Temperature [F]: 84.2 205 | -------------------------------------------------- 206 | ``` 207 | 208 | ## History 209 | 210 | ### RC Servos 211 | 212 | 213 | 214 | This was the original version, shown above around Aug 2016. It used toy RC 215 | servos (9 g's), they didn't work that great. I think I lost the code, because 216 | I can't find it. 217 | 218 | ### XL-320 Servos 219 | 220 | 221 | 222 | **work on this robot has stopped** This worked OK, see the movie above. 223 | The servos were not very strong and the lego like rivets were kind of 224 | a pain to use. The rivets also didn't hold things together very tight 225 | so there was a lot of wiggle. Overall, I prefer nuts/bolts. 226 | 227 | ### AX-12A Servos 228 | 229 | 230 | 231 | **this is the current robot I am focusing on** All of the current code 232 | is designed for this robot. I *sort of* maintain backwards compatability 233 | with the XL-320, but I don't guarentee it. 234 | 235 | 236 | # ToDo 237 | 238 | - change more return values to `tuples` instead of `lists` for performance 239 | - do CM shifting 240 | - make generic, so I can do 6 or 8 legs 241 | - flush out the config file better, I have hard coded too many things 242 | 243 | # Change Log 244 | 245 | | Date | Version | Notes | 246 | |---|---|---| 247 | | 2018-11-27 | 0.6.0 | Reorg'd, moved robots to own repo | 248 | | 2017-12-25 | 0.5.0 | Clean up and reorg, removed unnecessary libraries | 249 | | 2017-06-07 | 0.4.1 | broke out into package and published to PyPi | 250 | | 2016-08-10 | 0.0.1 | init | 251 | 252 | 253 | # MIT Software License 254 | 255 | **Copyright (c) 2016 Kevin J. Walchko** 256 | 257 | Permission is hereby granted, free of charge, to any person obtaining a copy of 258 | this software and associated documentation files (the "Software"), to deal in 259 | the Software without restriction, including without limitation the rights to 260 | use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies 261 | of the Software, and to permit persons to whom the Software is furnished to do 262 | so, subject to the following conditions: 263 | 264 | The above copyright notice and this permission notice shall be included in all 265 | copies or substantial portions of the Software. 266 | 267 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 268 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 269 | FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 270 | COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 271 | IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 272 | CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 273 | -------------------------------------------------------------------------------- /tests/test_all.py: -------------------------------------------------------------------------------- 1 | # from test_servo import * 2 | # from test_leg import * 3 | 4 | 5 | def test_dummy(): 6 | assert True 7 | -------------------------------------------------------------------------------- /tests/test_leg.py: -------------------------------------------------------------------------------- 1 | # #!/usr/bin/env python 2 | # 3 | # from __future__ import print_function 4 | # from __future__ import division 5 | # from pyxl320 import ServoSerial 6 | # import numpy as np 7 | # from math import sqrt 8 | # from quadruped.Leg import Leg 9 | # from quadruped.Servo import Servo 10 | # 11 | # 12 | # ser = ServoSerial('test_port', fake=True) 13 | # Servo.ser = ser 14 | # 15 | # def norm(a): 16 | # return sqrt(a[0]**2 + a[1]**2 + a[2]**2) 17 | # 18 | # def sub(a, b): 19 | # c = (a[0] - b[0], a[1] - b[1], a[2] - b[2]) 20 | # return norm(c) 21 | # 22 | # 23 | # def test_fk_ik(): 24 | # leg = Leg([0, 1, 2]) 25 | # 26 | # angles = [0, 45, -145] 27 | # 28 | # pts = leg.fk(*angles) 29 | # angles2 = leg.ik(*pts) 30 | # pts2 = leg.fk(*angles2) 31 | # # angles2 = [r2d(a), r2d(b), r2d(c)] 32 | # print('angles (orig):', angles, 'deg') 33 | # print('pts from fk(orig): {:.2f} {:.2f} {:.2f} mm'.format(*pts)) 34 | # print('angles2 from ik(pts): {:.2f} {:.2f} {:.2f} deg'.format(*angles2)) 35 | # print('pts2 from fk(angle2): {:.2f} {:.2f} {:.2f} mm'.format(*pts2)) 36 | # # print('diff:', np.linalg.norm(np.array(angles) - np.array(angles2))) 37 | # print('diff [mm]: {:.2f}'.format(norm(sub(pts, pts2)))) 38 | # # time.sleep(1) 39 | # assert (norm(sub(angles, angles2)) < 0.00001) 40 | # 41 | # 42 | # def printError(pts, pts2, angles, angles2): 43 | # print('****************************************************') 44 | # print('angles (orig):', angles) 45 | # print('angles2 from ik(pts): {:.2f} {:.2f} {:.2f}'.format(*angles2)) 46 | # print('pts from fk(orig): {:.2f} {:.2f} {:.2f}'.format(*pts)) 47 | # print('pts2 from fk(angle2): {:.2f} {:.2f} {:.2f}'.format(*pts2)) 48 | # # print('diff:', np.linalg.norm(np.array(angles) - np.array(angles2))) 49 | # print('diff [mm]: {:.2f}'.format(norm(sub(pts, pts2)))) 50 | # print('\nExiting\n') 51 | # print('****************************************************') 52 | # 53 | # 54 | # def test_full_fk_ik(): 55 | # channels = [0, 1, 2] 56 | # limits = [[-90, 90], [-90, 90], [-180, 0]] 57 | # leg = Leg(channels) 58 | # 59 | # for i in range(1, 3): 60 | # print('------------------------------------------------') 61 | # for a in range(limits[i][0], limits[i][1], 10): 62 | # angles = [0, 0, -10] 63 | # # if i == 2: a -= 90 64 | # angles[i] = a 65 | # pts = leg.fk(*angles) 66 | # if not pts.all(): 67 | # continue 68 | # angles2 = leg.ik(*pts) 69 | # pts2 = leg.fk(*angles2) 70 | # 71 | # angle_error = norm(sub(angles, angles2)) 72 | # pos_error = norm(sub(pts, pts2)) 73 | # # print(angle_error, pos_error) 74 | # 75 | # if angle_error > 0.0001: 76 | # print('Angle Error') 77 | # printError(pts, pts2, angles, angles2) 78 | # exit() 79 | # 80 | # elif pos_error > 0.0001: 81 | # print('Position Error') 82 | # printError(pts, pts2, angles, angles2) 83 | # exit() 84 | # 85 | # else: 86 | # print('Good: {} {} {} error(deg,mm): {:.4} {:.4}\n'.format(angles[0], angles[1], angles[2], angle_error, pos_error)) 87 | # leg.move(*pts) 88 | # # time.sleep(0.1) 89 | # # 90 | # # Servo.all_stop() 91 | # 92 | # 93 | # # def check_range(): 94 | # # length = { 95 | # # 'coxaLength': 26, 96 | # # 'femurLength': 42, 97 | # # 'tibiaLength': 63 98 | # # } 99 | # # 100 | # # channels = [0, 1, 2] 101 | # # # limits = [[-45, 45], [-45, 45], [-90, 0]] 102 | # # 103 | # # leg = Leg(length, channels) 104 | # # time.sleep(1) 105 | # # for servo in range(0, 3): 106 | # # leg.servos[0].angle = 45; time.sleep(0.01) 107 | # # leg.servos[1].angle = 0; time.sleep(0.01) 108 | # # leg.servos[2].angle = -90; time.sleep(0.01) 109 | # # for angle in range(-45, 45, 20): 110 | # # # if servo == 2: angle -= 90 111 | # # print('servo: {} angle: {}'.format(servo, angle)) 112 | # # leg.servos[servo].angle = angle 113 | # # time.sleep(1) 114 | # 115 | # # def test_DH(): 116 | # # coxa = 20 117 | # # femur = 50 118 | # # tibia = 100 119 | # # offset = 0 120 | # # t1 = 0 121 | # # t2 = 45 122 | # # t3 = -90-45 123 | # # # a, alpha, d, theta 124 | # # params = [ 125 | # # [ coxa, 90, 0, t1], 126 | # # [femur, 0, 0, t2], 127 | # # [tibia, -90, offset, t3] 128 | # # ] 129 | # # dh = DH() 130 | # # t = dh.fk(params) 131 | # # print(t) 132 | # # print('1 2 3: {:.2f} {:.2f} {:.2f}'.format(t[0,3],t[1,3],t[2,3])) 133 | # 134 | # def test_fk_ik2(): 135 | # channels = [0, 1, 2] 136 | # leg = Leg(channels) 137 | # 138 | # angles = [0, -90, 0] 139 | # 140 | # pts = leg.fk(*angles) 141 | # angles2 = leg.ik(*pts) 142 | # pts2 = leg.fk(*angles2) 143 | # print('angles (orig):', angles, 'deg') 144 | # print('pts from fk(orig): {:.2f} {:.2f} {:.2f} mm'.format(*pts)) 145 | # print('angles2 from ik(pts): {:.2f} {:.2f} {:.2f} deg'.format(*angles2)) 146 | # print('pts2 from fk(angle2): {:.2f} {:.2f} {:.2f} mm'.format(*pts2)) 147 | # # print('diff:', np.linalg.norm(np.array(angles) - np.array(angles2))) 148 | # print('diff [mm]: {:.2f}'.format(norm(sub(pts, pts2)))) 149 | # print('diff [deg]: {:.2f}'.format(norm(sub(angles, angles2)))) 150 | # # time.sleep(1) 151 | # # assert(np.linalg.norm(np.array(angles) - np.array(angles2)) < 0.00001) 152 | # 153 | # 154 | # def test_full_fk_ik2(): 155 | # channels = [0, 1, 2] 156 | # leg = Leg(channels) 157 | # 158 | # delta = 15 159 | # for a in range(-45, 45, delta): 160 | # for b in range(-45, 90, delta): 161 | # for g in range(-160, 0, delta): 162 | # angles = [a, b, g] 163 | # pts = leg.fk(*angles) 164 | # angles2 = leg.ik(*pts) 165 | # if angles2 is None: 166 | # print('Bad Pos: {:.1f} {:.1f} {:.1f}'.format(pts[0], pts[1], pts[2])) 167 | # continue 168 | # pts2 = leg.fk(*angles2) 169 | # 170 | # angle_error = norm(sub(angles, angles2)) 171 | # pos_error = np.linalg.norm(pts - pts2) 172 | # # print(angle_error, pos_error) 173 | # 174 | # if angle_error > 0.0001: 175 | # print('Angle Error') 176 | # printError(pts, pts2, angles, angles2) 177 | # exit() 178 | # 179 | # elif pos_error > 0.0001: 180 | # print('Position Error') 181 | # printError(pts, pts2, angles, angles2) 182 | # exit() 183 | # 184 | # # else: 185 | # # print('Angle: {:.1f} {:.1f} {:.1f}'.format(angles[0], angles[1], angles[2])) 186 | # # print('Pos: {:.1f} {:.1f} {:.1f}'.format(pts[0], pts[1], pts[2])) 187 | # # print('Error(deg,mm): {:.2f} {:.2f}'.format(angle_error, pos_error)) 188 | # # leg.move(*pts) 189 | # # time.sleep(0.1) 190 | -------------------------------------------------------------------------------- /tests/test_servo.py: -------------------------------------------------------------------------------- 1 | # #!/usr/bin/env python 2 | # ############################################## 3 | # # The MIT License (MIT) 4 | # # Copyright (c) 2016 Kevin Walchko 5 | # # see LICENSE for full details 6 | # ############################################## 7 | # # run this with "nosetests -v test_servo.py" 8 | # 9 | # from __future__ import print_function 10 | # from __future__ import division 11 | # from nose.tools import raises 12 | # from quadruped.Servo import Servo 13 | # from pyxl320 import ServoSerial 14 | # 15 | # 16 | # def test_limits(): 17 | # ser = ServoSerial('test_port', fake=True) 18 | # Servo.ser = ser 19 | # s = Servo(15) 20 | # s.setServoLimits(0, 0, 180) 21 | # s.angle = 0; assert (s.angle == 0) 22 | # s.angle = 45; assert (s.angle == 45) 23 | # s.angle = 90; assert (s.angle == 90) 24 | # s.angle = 180; assert (s.angle == 180) 25 | # s.angle = 270; assert (s.angle == 180), "angle is: {}".format(s.angle) 26 | # s.angle = -45; assert (s.angle == 0) 27 | # s.angle = -90; assert (s.angle == 0) 28 | # s.angle = -270; assert (s.angle == 0) 29 | # 30 | # s.setServoLimits(150, -90, 90) 31 | # s.angle = 0; assert (s.angle == 0) 32 | # s.angle = 45; assert (s.angle == 45) 33 | # s.angle = 90; assert (s.angle == 90) 34 | # s.angle = 180; assert (s.angle == 90) 35 | # s.angle = 270; assert (s.angle == 90) 36 | # s.angle = -45; assert (s.angle == -45) 37 | # s.angle = -90; assert (s.angle == -90) 38 | # s.angle = -270; assert (s.angle == -90) 39 | # 40 | # 41 | # @raises(Exception) 42 | # def test_fail2(): 43 | # ser = ServoSerial('test_port', fake=True) 44 | # Servo.ser = ser 45 | # s = Servo(15) 46 | # s.setServoLimits(0, 180, 0) # min > max ... error 47 | # 48 | # # def checks(): 49 | # # check = lambda x, a, b: max(min(b, x), a) 50 | # # print('0 in [0,180]:', check(0, 0, 180)) 51 | # # print('90 in [0,180]:', check(90, 0, 180)) 52 | # # print('180 in [0,180]:', check(180, 0, 180)) 53 | # # print('-90 in [0,180]:', check(-90, 0, 180)) 54 | # # print('-180 in [0,180]:', check(-180, 0, 180)) 55 | # # print('--------------------') 56 | # # print('these are wrong! Cannot reverse order') 57 | # # print('0 in [180,0]:', check(0, 180, 0)) 58 | # # print('45 in [180,0]:', check(45, 180, 0)) 59 | # # print('90 in [180,0]:', check(90, 180, 0)) 60 | # # print('180 in [180,0]:', check(180, 180, 0)) 61 | # # print('-90 in [180,0]:', check(-90, 180, 0)) 62 | # # print('-180 in [180,0]:', check(-180, 180, 0)) 63 | # # print('--------------------') 64 | # # print('0 in [-180,0]:', check(0, -180, 0)) 65 | # # print('90 in [-180,0]:', check(90, -180, 0)) 66 | # # print('180 in [-180,0]:', check(180, -180, 0)) 67 | # # print('-90 in [-180,0]:', check(-90, -180, 0)) 68 | # # print('-45 in [-180,0]:', check(-45, -180, 0)) 69 | # # print('-180 in [-180,0]:', check(-180, -180, 0)) 70 | --------------------------------------------------------------------------------