├── .github └── workflows │ └── test_and_distribute.yml ├── .gitignore ├── LICENSE.txt ├── MANIFEST.in ├── README.md ├── REST-APIs.md ├── changelog.md ├── doc ├── ErgoRobots.jpg ├── FAQ.rst ├── Makefile ├── README.md ├── _static │ └── my-css.css ├── _templates │ └── layout.html ├── about.rst ├── api.rst ├── banderole-pypot.jpg ├── conf.py ├── controller.rst ├── controlling_robot.rst ├── dynamixel.rst ├── extending.rst ├── index.rst ├── installation.rst ├── logging.rst ├── move.rst ├── poppy-creatures.jpg ├── primitive.rst ├── pypot-archi.jpg ├── pypot.dynamixel.rst ├── pypot.primitive.rst ├── pypot.robot.rst ├── pypot.sensor.rst ├── pypot.server.rst ├── pypot.tools.rst ├── pypot.utils.rst ├── pypot.vrep.rst ├── pypot_logo-144x144.png ├── pypot_logo-48x48.png ├── pypot_logo.png ├── quickstart.rst ├── remote_access.rst └── vrep.rst ├── pypot ├── __init__.py ├── _version.py ├── creatures │ ├── __init__.py │ ├── abstractcreature.py │ ├── configure_utility.py │ ├── ik.py │ └── services_launcher.py ├── dynamixel │ ├── __init__.py │ ├── controller.py │ ├── conversion.py │ ├── error.py │ ├── io │ │ ├── __init__.py │ │ ├── abstract_io.py │ │ ├── io.py │ │ └── io_320.py │ ├── motor.py │ ├── protocol │ │ ├── __init__.py │ │ ├── v1.py │ │ └── v2.py │ └── syncloop.py ├── kinematics.py ├── primitive │ ├── __init__.py │ ├── manager.py │ ├── move.py │ ├── primitive.py │ └── utils.py ├── robot │ ├── __init__.py │ ├── config.py │ ├── controller.py │ ├── io.py │ ├── motor.py │ ├── remote.py │ ├── robot.py │ └── sensor.py ├── sensor │ ├── __init__.py │ ├── arduino │ │ ├── __init__.py │ │ └── arduino_sensor.py │ ├── camera │ │ ├── __init__.py │ │ ├── abstractcam.py │ │ ├── dummy.py │ │ └── opencvcam.py │ ├── contact │ │ ├── __init__.py │ │ └── contact.py │ ├── depth │ │ ├── __init__.py │ │ └── sonar.py │ ├── imagefeature │ │ ├── __init__.py │ │ ├── blob.py │ │ ├── face.py │ │ └── marker.py │ ├── kinect │ │ ├── __init__.py │ │ └── sensor.py │ ├── optibridge.py │ └── optitrack.py ├── server │ ├── __init__.py │ ├── httpserver.py │ ├── rest.py │ ├── server.py │ ├── snap.py │ ├── snap_projects │ │ ├── blocs de base Poppy_FR.xml │ │ ├── legacy_projects │ │ │ ├── poppy-ergo-jr-lib.xml │ │ │ └── pypot-snap-record-orchestration-demo.xml │ │ ├── poppy-base_demo_FR.xml │ │ ├── poppy-demo-project.xml │ │ └── pypot-snap-blocks.xml │ ├── ws.py │ └── zmqserver.py ├── tools │ ├── __init__.py │ └── dxlconfig.py ├── utils │ ├── __init__.py │ ├── appdirs.py │ ├── flushed_print.py │ ├── i2c_controller.py │ ├── interpolation.py │ ├── pypot_time.py │ ├── stoppablethread.py │ └── trajectory.py ├── vpl │ ├── __init__.py │ ├── download.py │ ├── scratch.py │ └── snap.py └── vrep │ ├── __init__.py │ ├── child_script │ ├── inject_code.lua │ └── timer.lua │ ├── controller.py │ ├── io.py │ └── remoteApiBindings │ ├── __init__.py │ ├── lib │ ├── linux │ │ ├── 32Bit │ │ │ └── remoteApi.so │ │ └── 64Bit │ │ │ └── remoteApi.so │ ├── mac │ │ └── 64Bit │ │ │ └── remoteApi.dylib │ └── windows │ │ ├── 32Bit │ │ └── remoteApi.dll │ │ └── 64Bit │ │ └── remoteApi.dll │ ├── vrep.py │ └── vrepConst.py ├── samples ├── REST │ └── ruby │ │ ├── README.md │ │ ├── motor.rb │ │ ├── poppy-processing.rb │ │ └── poppy.rb ├── benchmarks │ ├── controller.png │ ├── controller_sync.ipynb │ ├── cpu_load.ipynb │ ├── cpu_usage.png │ ├── dxl-controller.py │ ├── dxl-single-motor.py │ ├── load_data.ipynb │ ├── packet.pdf │ ├── packet.png │ ├── robot.py │ ├── run-all.py │ ├── single_packet.ipynb │ └── vrep.py ├── notebooks │ ├── Accessing pypot REST API through HTTP requests.ipynb │ ├── Another language.ipynb │ ├── Benchmark your Poppy robot.ipynb │ ├── Controlling a Poppy Creature using SNAP.ipynb │ ├── QuickStart playing with a PoppyErgo.ipynb │ ├── Record, Save, and Play Moves on a Poppy Creature.ipynb │ ├── image │ │ ├── snap-basic-blocks.png │ │ ├── snap-find-pypot-blocks.png │ │ ├── snap-header.png │ │ ├── snap-hello-world.png │ │ ├── snap-import.png │ │ ├── snap-orchestration-demo.png │ │ ├── snap-ready.png │ │ ├── snap-right-click.png │ │ ├── snap-sinus.png │ │ ├── snap-slider-example.png │ │ ├── snap-slider.png │ │ └── snap.png │ ├── readme.md │ ├── robot-client.ipynb │ ├── robot-server.ipynb │ └── sum-of-sinus.ipynb ├── profiles │ ├── ergo-jr-rpi2-idle-light-syncloop.prof │ └── profiles.md ├── remoterobot-server.py └── test_connection.py ├── setup.cfg ├── setup.py └── tests ├── test_crashed_prim.py ├── test_dummy.py ├── test_ik.py ├── test_import.py ├── test_primitive.py ├── test_rest_api.py ├── test_snap.py ├── test_websocket.py └── utils.py /.github/workflows/test_and_distribute.yml: -------------------------------------------------------------------------------- 1 | name: pypot 2 | 3 | on: [push] 4 | 5 | jobs: 6 | build: 7 | 8 | runs-on: ubuntu-latest 9 | strategy: 10 | matrix: 11 | python-version: [3.6, 3.7, 3.8, 3.9] 12 | env: 13 | # The Python version from the matrix used to build and deploy to Pypi 14 | PYTHON_BUILD_VERSION: 3.8 15 | steps: 16 | - uses: actions/checkout@v2 17 | - name: Set up Python ${{ matrix.python-version }} 18 | uses: actions/setup-python@v2 19 | with: 20 | python-version: ${{ matrix.python-version }} 21 | - name: Install dependencies 22 | run: | 23 | python -m pip install --upgrade pip 24 | pip install flake8 build websocket ../pypot 25 | pip install poppy-ergo-jr 26 | - name: Lint with flake8 27 | run: | 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: Test package with unittest 33 | working-directory: tests 34 | run: | 35 | python -m unittest discover 36 | - name: Build package with Python ${{ env.PYTHON_BUILD_VERSION }} 37 | if: ${{ github.event_name == 'push' && startsWith(github.ref, 'refs/tags') && matrix.python-version == env.PYTHON_BUILD_VERSION }} 38 | run: python -m build 39 | - name: Publish package relying on pypa/gh-action-pypi-publish with Python ${{ env.PYTHON_BUILD_VERSION }} 40 | uses: pypa/gh-action-pypi-publish@27b31702a0e7fc50959f5ad993c78deac1bdfc29 41 | if: ${{ github.event_name == 'push' && startsWith(github.ref, 'refs/tags') && matrix.python-version == env.PYTHON_BUILD_VERSION }} 42 | with: 43 | user: __token__ 44 | password: ${{ secrets.PYPI_API_TOKEN }} 45 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | *$py.class 5 | 6 | # C extensions 7 | *.so 8 | 9 | # Distribution / packaging 10 | .Python 11 | env/ 12 | build/ 13 | develop-eggs/ 14 | dist/ 15 | downloads/ 16 | eggs/ 17 | .eggs/ 18 | lib/ 19 | lib64/ 20 | parts/ 21 | sdist/ 22 | var/ 23 | *.egg 24 | *.egg-info 25 | 26 | # Sphinx 27 | doc/_build 28 | 29 | # OS generated files 30 | .DS_Store 31 | .DS_Store? 32 | .Trashes 33 | ehthumbs.db 34 | Thumbs.db 35 | 36 | # Gedit 37 | *~ 38 | 39 | # Jupyter (ipython) 40 | .ipynb_checkpoints/ 41 | *-checkpoint.ipynb 42 | 43 | # Pypot record files 44 | *.record 45 | 46 | -------------------------------------------------------------------------------- /MANIFEST.in: -------------------------------------------------------------------------------- 1 | include pypot/vrep/remoteApiBindings/lib/*/*/remoteApi.* 2 | include pypot/server/snap_projects/* 3 | include *.md 4 | 5 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | [![PyPI](https://img.shields.io/pypi/v/pypot.svg)](https://pypi.python.org/pypi/pypot/) 2 | [![Build Status](https://github.com/poppy-project/pypot/actions/workflows/test_and_distribute.yml/badge.svg)](https://github.com/poppy-project/pypot/actions) 3 | [![DOI](https://zenodo.org/badge/DOI/10.5281/zenodo.591809.svg)](https://doi.org/10.5281/zenodo.591809) 4 | 5 | 6 | 7 | # Pypot ⚙️ A Python library for Dynamixel motor control 8 | 9 | Pypot is a cross-platform Python library making it easy and fast to control custom robots based on multiple models of Dynamixel motors. Use Pypot to: 10 | 11 | * control Robotis motors through USB2Dynamixel, USB2AX or [Pixl 4 Raspberry Pi](https://github.com/poppy-project/pixl) devices, 12 | * define kinematic chains of a custom robot and control it through high-level commands (Forward & Inverse Kinematics), 13 | * define primitives (motions applying to motor groups) and easily combine them to create custom complex behaviors (Robot dance, arm shaking, writing with a pen...). 14 | * define sensor access and processing (QRCode detection, force sensors, RGB-D, ...) 15 | 16 | Pypot is also compatible with the [CoppeliaSim simulator](http://www.coppeliarobotics.com) (formerly V-REP), embeds a [REST API](https://docs.poppy-project.org/en/programming/rest.html) for Web-based control, and supports visual programming via [Scratch](https://docs.poppy-project.org/en/getting-started/program-the-robot.html#using-scratch) and [Snap](https://docs.poppy-project.org/en/getting-started/program-the-robot.html#using-snap). 17 | 18 | ## 🔌 Compatible hardware 19 | 20 | **Compatible motors:** MX-106, MX-64, MX-28, MX-12, AX-12, AX-18, RX-24, RX-28, RX-64, XL-320, SR-RH4D, EX-106. Derivated versions are also supported (e.g. MX-28AT, MX-28R, MX-28T, ...). Both protocols v1 and v2 are supported but v2 is used only for XL-320. Use [Herborist](https://github.com/poppy-project/herborist#herborist) to help detect IDs and baudrates of motors. 21 | 22 | **Compatible sensors:** Kinect 1, QRCode from RGB camera, sonar, micro-switch from Raspberry Pi GPIO, digital or analog sensor connected to Arduino 23 | 24 | **Compatible interpreters:** Python 3.6, 3.7, 3.8, 3.9 25 | 26 | Other models of motors and sensors can be integrated with little effort and time. Other programming languages may be connected through the REST API. 27 | 28 | ## Read 📖 [Documentation](https://docs.poppy-project.org/en/software-libraries/pypot.html) and get ⁉️ [Assistance](https://forum.poppy-project.org/) 29 | 30 | ## Pypot is part of the opensource Poppy project 31 | 32 | Pypot is part of the [Poppy project](http://www.poppy-project.org) aiming at developing robotic creations that are easy to build, customize, deploy, and share. It promotes open-source by sharing 3D-printed hardware, software, and web tools. 33 | 34 | The Poppy creatures are: 35 | * **[Poppy Humanoid](https://www.poppy-project.org/en/robots/poppy-humanoid/)**: a kid-size humanoid robot designed for biped locomotion and physical human-robot interaction (25 DoF) for biped research and university workshops, 36 | * **[Poppy Torso](https://www.poppy-project.org/en/robots/poppy-torso/)**: just the torso of the humanoid robot, with a suction pad to stick it attach it firmly to a desk (13 DoF) for HRI research, university and high school workshops 37 | * **[Poppy Ergo Jr](https://www.poppy-project.org/en/robots/poppy-ergo-jr/)**: a low-cost robotic arm for primary to middle school (6 Dof) for primary or middle school workshops 38 | 39 | ![Poppy Humanoid](./doc/poppy-creatures.jpg) 40 | 41 | All those creatures are based on a combination of standard dynamixel actuators, 3D printed parts and open-source electronics such as Arduino boards. Both the hardware (3D models, electronics...) and software can be freely used, modified and duplicated. 42 | 43 | ## 💻 Installation 44 | 45 | If you are using a Poppy robot embedding a Raspberry Pi, Pypot is already shipped with it. For custom robots, just type ⌨️ `pip install pypot` in your system terminal! 46 | 47 | If you intend to modify or add features to Pypot, create a virtual environment and install it from sources instead: 48 | ```bash 49 | git clone https://github.com/poppy-project/pypot 50 | cd pypot/pypot 51 | pip install . 52 | ``` 53 | 54 | Additional drivers may be needed for USB2serial, depending of your OS. Check here: 55 | * [USB2AX](http://www.xevelabs.com/doku.php?id=product:usb2ax:quickstart) - this device is designed to manage TTL communication only 56 | * USB2Dynamixel - this device can manage both TTL and RS485 communication. 57 | * [Pixl board](https://github.com/poppy-project/pixl) for RaspberryPi 58 | 59 | ## 👨‍💻 Contributing 60 | 61 | If this is the first time you contribute to Pypot, it is a good idea to share your work on [the forum](https://forum.poppy-project.org/) first, we will be happy to give you a hand so that you can contribute to the opensource project. 62 | -------------------------------------------------------------------------------- /REST-APIs.md: -------------------------------------------------------------------------------- 1 | # Pypot REST APIs 2 | 3 | The pypot library provides a REST API which can be used to access the [Robot](http://poppy-project.github.io/pypot/pypot.robot.html) level and all its attached [motors](http://poppy-project.github.io/pypot/pypot.robot.html#module-pypot.robot.motor), [sensors](http://poppy-project.github.io/pypot/pypot.robot.html#module-pypot.robot.sensor), and [primitives](http://poppy-project.github.io/pypot/pypot.primitive.html). Through the REST API, you can: 4 | * **Motors** 5 | * get the motors list and get/set value from/to their registers 6 | * **Sensors**: 7 | * get the sensors list and get/set value from/to their registers 8 | * **Primitives**: 9 | * get the primitives list (running or not), start, stop, pause, and resume them. 10 | * you can also access their publicly available properties and methods. 11 | 12 | *Note that only the defined as **publicly available** registers or methods will be accessed through the REST API. Please refer to the [note for developers](#markdown-header-note-for-developers) below for details.* 13 | 14 | # REST APIs 15 | 16 | *Please note that all answers are always sent as json dictionary.* 17 | 18 | ## Robot 19 | 20 | ### Motor 21 | 22 | | | HTTP | JSON | Example of answer | 23 | |--------------------------------------------|:------------------------------------------------------------:|:---------------------------------------------------------------------------------------------------------------------------------------------------:|------------------------------------------------------------------------| 24 | | Get the motors list | GET /motor/list.json | {"robot": {"get_motors_list": {"alias": "motors"}}} | {'motors': ["l_elbow_y", "r_elbow_y", "r_knee_y", "head_y", "head_z"]} | 25 | | Get the motors alias list | GET /motor/alias/list.json | {"robot": {"get_motors_alias": {}}} | {'alias': ["r_leg", "torso", "l_leg_sagitall"]} | 26 | | Get the motors list of a specific alias | GET /motor/\/list.json | {"robot": {"get_motors_list": {"alias": ""}}} | {\: ["l_elbow_y", "r_elbow_y", "r_knee_y", "head_y", "head_z"]} | 27 | | Get the registers list of a specific motor | GET /motor/\/register/list.json | {"robot": {"get_registers_list": {"motor": ""}}} | {'registers': ["goal_speed", "compliant", "present_load", "id"]} | 28 | | Get the register value | GET /motor/\/register/\ | {"robot": {"get_register_value": {"motor": "", "register": ""}}} | {"present_position": 30} | 29 | | Set new value to a register | POST /motor/\/register/\/value.json | {"robot": {"set_register_value": {"motor": "", "register": "", "value": {"arg1": "val1", "arg2": "val2", "...": "..."}}} | {} | 30 | 31 | ### Sensor 32 | 33 | *Similar to the motor API. You just replace motor by sensor (for the moment there is no alias for sensors).* 34 | 35 | ## Primitive 36 | 37 | | | HTTP | JSON | Example of answer | 38 | |-----------------------------------|:-------------------------------------------------:|:--------------------------------------------------------------------------------------------------------------------------------------------:|:----------------------------------------------------------------------:| 39 | | Get the primitives list | GET /primitive/list.json | {"robot": {"get_primitives_list": ""}} | {'primitives': ["stand_up", "sit", "head_tracking"]} | 40 | | Get the running primitives list | GET /primitive/running/list.json | {"robot": {"get_running_primitives_list": ""}} | {'primitives': ["head_tracking"]} | 41 | | Start a primitive | GET /primitive/\/start.json | {"robot": {"start_primitive": {"primitive": ""}}} | {} | 42 | | Stop a primitive | GET /primitive/\/stop.json | {"robot": {"stop_primitive": {"primitive": ""}}} | {} | 43 | | Pause a primitive | GET /primitive/\/pause.json | {"robot": {"pause_primitive": {"primitive": ""}}} | {} | 44 | | Resume a primitive | GET /primitive/\/resume.json | {"robot": {"resume_primitive": {"primitive": ""}}} | {} | 45 | | Get the primitive properties list | GET /primitive/\/property/list.json | {"robot": {"get_primitive_properties_list": {"primitive": ""}}} | {"property": ["filter", "smooth"]} | 46 | | Get a primitive property value | GET /primitive/\/property/ | {"robot": {"get_primitive_property": {"primitive": "", "property": ""}}} | {"sin.amp": 30.0} | 47 | | Set a primitive property value | POST /primitive/\/property//value.json | {"robot": {"set_primitive_property": {"primitive": "", "property": "", "args": {"arg1": "val1", "arg2": "val2", "...": "..."}}}} | {} | 48 | | Get the primitive methods list | GET /primitive/\/method/list.json | {"robot": {"get_primitive_methods_list": {"primitive": ""}}} | {"methods": ["get_tracked_faces", "start", "stop", "pause", "resume"]} | 49 | | Call a method of a primitive | POST /primitive/\/method/\/args.json | {"robot": {"call_primitive_method": {"primitive": "", "method": "", "args": {"arg1": "val1", "arg2": "val2", "...": "..."}}}} | | 50 | 51 | 52 | ## Note for developers 53 | 54 | In order to **publicly** available through the REST API, the registers of the motors/sensors and the properties/methods of the primitives should be added to specific lists. 55 | 56 | More precisely, the [Motor](http://poppy-project.github.io/pypot/pypot.robot.html#pypot.robot.motor.Motor) class sets the [registers](http://poppy-project.github.io/pypot/pypot.dynamixel.html#pypot.dynamixel.motor.DxlMotor.registers) list (similarly for the [Sensor](http://poppy-project.github.io/pypot/pypot.sensor.html) class) and the [Primitives](http://poppy-project.github.io/pypot/pypot.primitive.html#pypot.primitive.primitive.Primitive) uses the [methods]() and [properties]() list. 57 | 58 | Those are class variables and can be extended when defining your own subclasses (see the [Sinus primitive](https://github.com/poppy-project/pypot/blob/REST-API-2.0/pypot/primitive/utils.py) as an example). 59 | -------------------------------------------------------------------------------- /changelog.md: -------------------------------------------------------------------------------- 1 | # Changelog 2 | 3 | ## V 4.1 4 | * Add support for EX motors 5 | 6 | ## V 4.0 7 | 8 | * Fix compatibility with Python 3.8 and drop compatibility with Python 2 9 | * Fix poppy-configure and dxl-config for recent Dynamixel firmwares with protocol v1 10 | * Replaced the Snap build from pypot by a Snap downloader for `poppy-services --snap` 11 | * Add Snap entry points for webcam access in the REST API 12 | * Jupyter notebooks update for py3 13 | * Updated ikpy to the latest version 14 | 15 | ## V 3.1 16 | 17 | * Add support for connecting multiple robots to a same VREP scene. 18 | * Fix a bug in the minjerk trajectory computation. 19 | 20 | ## V 2.11 21 | 22 | ## Features 23 | * Add dummy motors (mostly for unittest) 24 | * add native support for the pixl board 25 | * allow to disable sensor at loading (convenient for camera sensor) 26 | * add a dummy camera 27 | * Add support for RX-24 dynamixel motors 28 | * Add an event used to check if a "loopable" thread has been updated 29 | * Move can now be plotted using matplotlib 30 | 31 | ### Snap 32 | * Add blocs: “ping url ” and “set $robot host to ” which aim to fix DNS issues in some filtered networks. 33 | * update “set of motor(s) to value ” : speed register is now moving_speed instead of goal_speed. Able to use it through many motors at once now 34 | * fix “get of motor(s) ” 35 | * fix some default values of inputs variable for consistency 36 | * add entry for ik in SnapRemoteServer 37 | * check return-delay-time at startup to prevent timeouts with misconfigured motors 38 | 39 | ## Bugfix 40 | * many primitives threading issues 41 | * python >= 3.4 compatibility issues 42 | * setup unittest via dummy robot 43 | * fix the unclear exception "Cannot unpack *values" 44 | * fix cli tool `poppy-motor-reset` and rename it to `dxl-config` 45 | * Fix deprecation issue in get_control_table 46 | * Clear error when there is no "time script" in a v-rep scene 47 | * Fix a freeze when stopping a paused primitive 48 | * Fix offset/orientation issue in DummyController 49 | * Fix hostname resolution 50 | * Make initialization of synchronization loop more robust 51 | * Fix #155: Closing a DummyRobot raises an issue 52 | 53 | ## V 2.10 54 | ### Features 55 | * add support for led inside primitive (XL320) 56 | * remove RPICam and use v4l driver in opencv 57 | * support hampy marker in Snap! 58 | 59 | ### Fix 60 | * network issue for finding local ip when there is no interface 61 | * Python 3.5 compatibility 62 | 63 | 64 | 65 | ## V 2.9 66 | ### Image feature Sensors 67 | * face 68 | * blob 69 | * qrcode 70 | 71 | ### LED register for XL-320 motors 72 | ### various bug fixes 73 | 74 | ## V 2.8 75 | ### Sync Loop 76 | * possibility to define synchronous loop 77 | * define a "light" synchronization loop 78 | * can now choose sync. loop inside the config 79 | 80 | ### Better Sensor Integration 81 | * can specify sensor inside the config file 82 | 83 | ## V 2.1 84 | * now uses the poppy_creature package 85 | * add support for present_load/torque_limit/compliant in V-REP 86 | * fix a bug when using setup.py install 87 | * add minimum jerk of Steve N'Guyen 88 | * add safe compliance behavior 89 | * add camera sensor based on opencv 90 | 91 | ## V. 2.0 92 | 93 | ### Major changes 94 | * support for V-REP simulator 95 | * new controller implementation: [extending pypot](http://poppy-project.github.io/pypot/extending.html) 96 | 97 | ### Minor changes 98 | * Use of descriptors for motor registers 99 | * REST API / remote robot 100 | * Starts automatically the synchronization 101 | 102 | 103 | ## V. 1.7 104 | 105 | ### Minor changes 106 | * Autodetect robot 107 | -------------------------------------------------------------------------------- /doc/ErgoRobots.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/poppy-project/pypot/13263953a7970209efbcb476bd07b5049151f52e/doc/ErgoRobots.jpg -------------------------------------------------------------------------------- /doc/FAQ.rst: -------------------------------------------------------------------------------- 1 | FAQ 2 | *** 3 | 4 | Why is the default baud rate different for robotis and for pypot ? 5 | ------------------------------------------------------------------ 6 | 7 | Robotis motors are set up to work with a 57140 baud rate. Yet, in pypot we choose to use 1000000 baud rate as the default configuration. While everything would work with the robotis default baud rate, we choose to incitate people to modify this default configuration to allow for more performance. 8 | 9 | I got a DxlCommunicationError when scanning multiple motors on a bus 10 | -------------------------------------------------------------------- 11 | 12 | This exception is usually raised when two (or more) motors share the same id. This should never happened, all ids should be unique on a same bus. Otherwise, package will collide resulting in communication error. 13 | -------------------------------------------------------------------------------- /doc/Makefile: -------------------------------------------------------------------------------- 1 | # Makefile for Sphinx documentation 2 | # 3 | 4 | # You can set these variables from the command line. 5 | SPHINXOPTS = 6 | SPHINXBUILD = sphinx-build 7 | PAPER = 8 | BUILDDIR = _build 9 | 10 | # Internal variables. 11 | PAPEROPT_a4 = -D latex_paper_size=a4 12 | PAPEROPT_letter = -D latex_paper_size=letter 13 | ALLSPHINXOPTS = -d $(BUILDDIR)/doctrees $(PAPEROPT_$(PAPER)) $(SPHINXOPTS) . 14 | # the i18n builder cannot share the environment and doctrees with the others 15 | I18NSPHINXOPTS = $(PAPEROPT_$(PAPER)) $(SPHINXOPTS) . 16 | 17 | .PHONY: help clean html dirhtml singlehtml pickle json htmlhelp qthelp devhelp epub latex latexpdf text man changes linkcheck doctest gettext 18 | 19 | help: 20 | @echo "Please use \`make ' where is one of" 21 | @echo " html to make standalone HTML files" 22 | @echo " dirhtml to make HTML files named index.html in directories" 23 | @echo " singlehtml to make a single large HTML file" 24 | @echo " pickle to make pickle files" 25 | @echo " json to make JSON files" 26 | @echo " htmlhelp to make HTML files and a HTML help project" 27 | @echo " qthelp to make HTML files and a qthelp project" 28 | @echo " devhelp to make HTML files and a Devhelp project" 29 | @echo " epub to make an epub" 30 | @echo " latex to make LaTeX files, you can set PAPER=a4 or PAPER=letter" 31 | @echo " latexpdf to make LaTeX files and run them through pdflatex" 32 | @echo " text to make text files" 33 | @echo " man to make manual pages" 34 | @echo " texinfo to make Texinfo files" 35 | @echo " info to make Texinfo files and run them through makeinfo" 36 | @echo " gettext to make PO message catalogs" 37 | @echo " changes to make an overview of all changed/added/deprecated items" 38 | @echo " linkcheck to check all external links for integrity" 39 | @echo " doctest to run all doctests embedded in the documentation (if enabled)" 40 | 41 | clean: 42 | -rm -rf $(BUILDDIR)/* 43 | 44 | html: 45 | $(SPHINXBUILD) -b html $(ALLSPHINXOPTS) $(BUILDDIR)/html 46 | @echo 47 | @echo "Build finished. The HTML pages are in $(BUILDDIR)/html." 48 | 49 | dirhtml: 50 | $(SPHINXBUILD) -b dirhtml $(ALLSPHINXOPTS) $(BUILDDIR)/dirhtml 51 | @echo 52 | @echo "Build finished. The HTML pages are in $(BUILDDIR)/dirhtml." 53 | 54 | singlehtml: 55 | $(SPHINXBUILD) -b singlehtml $(ALLSPHINXOPTS) $(BUILDDIR)/singlehtml 56 | @echo 57 | @echo "Build finished. The HTML page is in $(BUILDDIR)/singlehtml." 58 | 59 | pickle: 60 | $(SPHINXBUILD) -b pickle $(ALLSPHINXOPTS) $(BUILDDIR)/pickle 61 | @echo 62 | @echo "Build finished; now you can process the pickle files." 63 | 64 | json: 65 | $(SPHINXBUILD) -b json $(ALLSPHINXOPTS) $(BUILDDIR)/json 66 | @echo 67 | @echo "Build finished; now you can process the JSON files." 68 | 69 | htmlhelp: 70 | $(SPHINXBUILD) -b htmlhelp $(ALLSPHINXOPTS) $(BUILDDIR)/htmlhelp 71 | @echo 72 | @echo "Build finished; now you can run HTML Help Workshop with the" \ 73 | ".hhp project file in $(BUILDDIR)/htmlhelp." 74 | 75 | qthelp: 76 | $(SPHINXBUILD) -b qthelp $(ALLSPHINXOPTS) $(BUILDDIR)/qthelp 77 | @echo 78 | @echo "Build finished; now you can run "qcollectiongenerator" with the" \ 79 | ".qhcp project file in $(BUILDDIR)/qthelp, like this:" 80 | @echo "# qcollectiongenerator $(BUILDDIR)/qthelp/PyPot.qhcp" 81 | @echo "To view the help file:" 82 | @echo "# assistant -collectionFile $(BUILDDIR)/qthelp/PyPot.qhc" 83 | 84 | devhelp: 85 | $(SPHINXBUILD) -b devhelp $(ALLSPHINXOPTS) $(BUILDDIR)/devhelp 86 | @echo 87 | @echo "Build finished." 88 | @echo "To view the help file:" 89 | @echo "# mkdir -p $$HOME/.local/share/devhelp/PyPot" 90 | @echo "# ln -s $(BUILDDIR)/devhelp $$HOME/.local/share/devhelp/PyPot" 91 | @echo "# devhelp" 92 | 93 | epub: 94 | $(SPHINXBUILD) -b epub $(ALLSPHINXOPTS) $(BUILDDIR)/epub 95 | @echo 96 | @echo "Build finished. The epub file is in $(BUILDDIR)/epub." 97 | 98 | latex: 99 | $(SPHINXBUILD) -b latex $(ALLSPHINXOPTS) $(BUILDDIR)/latex 100 | @echo 101 | @echo "Build finished; the LaTeX files are in $(BUILDDIR)/latex." 102 | @echo "Run \`make' in that directory to run these through (pdf)latex" \ 103 | "(use \`make latexpdf' here to do that automatically)." 104 | 105 | latexpdf: 106 | $(SPHINXBUILD) -b latex $(ALLSPHINXOPTS) $(BUILDDIR)/latex 107 | @echo "Running LaTeX files through pdflatex..." 108 | $(MAKE) -C $(BUILDDIR)/latex all-pdf 109 | @echo "pdflatex finished; the PDF files are in $(BUILDDIR)/latex." 110 | 111 | text: 112 | $(SPHINXBUILD) -b text $(ALLSPHINXOPTS) $(BUILDDIR)/text 113 | @echo 114 | @echo "Build finished. The text files are in $(BUILDDIR)/text." 115 | 116 | man: 117 | $(SPHINXBUILD) -b man $(ALLSPHINXOPTS) $(BUILDDIR)/man 118 | @echo 119 | @echo "Build finished. The manual pages are in $(BUILDDIR)/man." 120 | 121 | texinfo: 122 | $(SPHINXBUILD) -b texinfo $(ALLSPHINXOPTS) $(BUILDDIR)/texinfo 123 | @echo 124 | @echo "Build finished. The Texinfo files are in $(BUILDDIR)/texinfo." 125 | @echo "Run \`make' in that directory to run these through makeinfo" \ 126 | "(use \`make info' here to do that automatically)." 127 | 128 | info: 129 | $(SPHINXBUILD) -b texinfo $(ALLSPHINXOPTS) $(BUILDDIR)/texinfo 130 | @echo "Running Texinfo files through makeinfo..." 131 | make -C $(BUILDDIR)/texinfo info 132 | @echo "makeinfo finished; the Info files are in $(BUILDDIR)/texinfo." 133 | 134 | gettext: 135 | $(SPHINXBUILD) -b gettext $(I18NSPHINXOPTS) $(BUILDDIR)/locale 136 | @echo 137 | @echo "Build finished. The message catalogs are in $(BUILDDIR)/locale." 138 | 139 | changes: 140 | $(SPHINXBUILD) -b changes $(ALLSPHINXOPTS) $(BUILDDIR)/changes 141 | @echo 142 | @echo "The overview file is in $(BUILDDIR)/changes." 143 | 144 | linkcheck: 145 | $(SPHINXBUILD) -b linkcheck $(ALLSPHINXOPTS) $(BUILDDIR)/linkcheck 146 | @echo 147 | @echo "Link check complete; look for any errors in the above output " \ 148 | "or in $(BUILDDIR)/linkcheck/output.txt." 149 | 150 | doctest: 151 | $(SPHINXBUILD) -b doctest $(ALLSPHINXOPTS) $(BUILDDIR)/doctest 152 | @echo "Testing of doctests in the sources finished, look at the " \ 153 | "results in $(BUILDDIR)/doctest/output.txt." 154 | -------------------------------------------------------------------------------- /doc/README.md: -------------------------------------------------------------------------------- 1 | # Building docs # 2 | To build docs, run `make` in this directory. `make help` lists all targets. 3 | 4 | ## Requirements ## 5 | Sphinx and Latex is needed to build doc. 6 | 7 | **Spinx:** 8 | ```sh 9 | pip install sphinx sphinxjp.themes.basicstrap 10 | ``` 11 | 12 | **Latex Ubuntu:** 13 | ```sh 14 | sudo apt-get install -qq texlive texlive-latex-extra dvipng 15 | ``` 16 | 17 | **Latex Mac:** 18 | 19 | Install the full [MacTex](http://www.tug.org/mactex/) installation or install the smaller [BasicTex](http://www.tug.org/mactex/morepackages.html) and add *ucs* and *dvipng* packages: 20 | ```sh 21 | sudo tlmgr install ucs dvipng 22 | ``` 23 | -------------------------------------------------------------------------------- /doc/_static/my-css.css: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/poppy-project/pypot/13263953a7970209efbcb476bd07b5049151f52e/doc/_static/my-css.css -------------------------------------------------------------------------------- /doc/_templates/layout.html: -------------------------------------------------------------------------------- 1 | 2 | {# Import the theme's layout #} 3 | {% extends "!layout.html" %} 4 | 5 | {% set css_files = ['_static/my-css.css'] + css_files %} 6 | -------------------------------------------------------------------------------- /doc/about.rst: -------------------------------------------------------------------------------- 1 | What is pypot? 2 | ============== 3 | 4 | .. image:: banderole-pypot.jpg 5 | :width: 100% 6 | :align: center 7 | 8 | 9 | Pypot is a framework developed in the `Inria FLOWERS `_ team to make it easy and fast to control custom robots based on dynamixel motors. This framework provides different level of abstraction corresponding to different types of use. More precisely, you can use pypot to: 10 | 11 | * directly control robotis motors through a USB2serial device (both protocols v1 and v2 are supported: you can use it with AX, RX, MX and XL320 motors), 12 | * define the structure of your particular robot and control it through high-level commands. 13 | 14 | .. * define primitives and easily combine them to create complex behavior. 15 | 16 | Pypot has been entirely written in Python to allow for fast development, easy deployment and quick scripting by non-necessary expert developers. The serial communication is handled through the standard library and thus allows for rather high performance (10ms sensorimotor loop). It is crossed-platform and has been tested on Linux, Windows and Mac OS. It is distributed under the `GPL V3 open source license `_. 17 | 18 | Pypot is also compatible with the `V-REP simulator `_. This allows you to seamlessly switch from a real robot to its simulated equivalent without having to modify your code. 19 | 20 | The next sections describe how to :ref:`install ` pypot on your system and then the :ref:`first steps to control an Ergo-Robot `. If you decide to use pypot and want more details on what you can do with this framework, you can refer to the :ref:`tutorial `. 21 | 22 | .. note:: Pypot is part of the `poppy-project `_ and is mainly used to control Poppy Creatures. If you are not interested in the low-level communication with motors and sensors but rather on high-level behaviors of Poppy Robots, you should directly see the poppy-* libraries (e.g. `Poppy Humanoid `_ or `Poppy Ergo Jr `_). They are built on top of pypot and abstract most of its operating and already come with convenient method for creating and starting your robot. They are also a good starting point if you want to define your own Poppy Creatures. 23 | -------------------------------------------------------------------------------- /doc/api.rst: -------------------------------------------------------------------------------- 1 | Pypot's API 2 | *********** 3 | 4 | .. toctree:: 5 | pypot.dynamixel 6 | pypot.primitive 7 | pypot.robot 8 | pypot.sensor 9 | pypot.server 10 | pypot.vrep 11 | 12 | pypot.utils 13 | -------------------------------------------------------------------------------- /doc/banderole-pypot.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/poppy-project/pypot/13263953a7970209efbcb476bd07b5049151f52e/doc/banderole-pypot.jpg -------------------------------------------------------------------------------- /doc/controlling_robot.rst: -------------------------------------------------------------------------------- 1 | 2 | Controlling robot 3 | ***************** 4 | 5 | Pypot handles the communication with dynamixel motors from robotis. Using a USB communication device such as USB2DYNAMIXEL or USB2AX, you can open serial communication with robotis motors (MX, RX, AX and XL320) using communication protocols TTL or RS485. More specifically, it allows easy access (both reading and writing) to the different registers of any dynamixel motors. Those registers includes values such as position, speed or torque. The whole list of registers can directly be found on the `robotis website `_. 6 | 7 | You can access the register of the motors through two different ways: 8 | 9 | * **Low-level API:** In the first case, you can get or set a value to a motor by directly sending a request and waiting for the motor to answer. Here, you only use the low level API to communicate with the motor (refer to section :ref:`low_level` for more details). 10 | 11 | * **Controller API:** In the second case, you define requests which will automatically be sent at a predefined frequency. The values obtained from the requests are stored in a local copy that you can freely access at any time. However, you can only access the last synchronized value. This second method encapsulates the first approach to prevent you from writing repetitive request (refer to section :ref:`controller` for further details). 12 | 13 | While the second approach allows the writing of simpler code without detailed knowledge of how the communication with robotis motor works, the first approach may allow for more performance through fine tuning of the communication needed in particular applications. Examples of both approaches will be provided in the next sections. 14 | 15 | .. toctree:: 16 | dynamixel.rst 17 | controller.rst 18 | -------------------------------------------------------------------------------- /doc/index.rst: -------------------------------------------------------------------------------- 1 | .. Pypot documentation master file, created by 2 | sphinx-quickstart on Sat Sep 15 18:52:00 2012. 3 | You can adapt this file completely to your liking, but it should at least 4 | contain the root `toctree` directive. 5 | 6 | Pypot 7 | ***** 8 | 9 | Welcome to pypot's documentation! 10 | ================================= 11 | 12 | **Introduction** 13 | 14 | .. toctree:: 15 | :titlesonly: 16 | :maxdepth: 2 17 | 18 | about.rst 19 | installation.rst 20 | quickstart.rst 21 | 22 | .. _tutorial: 23 | 24 | **Tutorial** 25 | 26 | .. toctree:: 27 | :titlesonly: 28 | :maxdepth: 2 29 | 30 | controlling_robot.rst 31 | primitive.rst 32 | vrep.rst 33 | extending.rst 34 | remote_access.rst 35 | logging.rst 36 | 37 | 38 | **Tools** 39 | 40 | .. toctree:: 41 | :titlesonly: 42 | :maxdepth: 2 43 | 44 | move.rst 45 | 46 | 47 | **Misc** 48 | 49 | .. toctree:: 50 | :titlesonly: 51 | :maxdepth: 1 52 | 53 | api.rst 54 | FAQ.rst 55 | 56 | Indices and tables 57 | ================== 58 | 59 | * :ref:`genindex` 60 | * :ref:`modindex` 61 | * :ref:`search` 62 | -------------------------------------------------------------------------------- /doc/installation.rst: -------------------------------------------------------------------------------- 1 | .. _installation: 2 | 3 | Installation 4 | ============ 5 | Requirements 6 | ------------------- 7 | Pypot is written in `python `__ and need a python interpreter to be run. Moreover pypot has `scipy `_ and `numpy `_ for dependencies, as they are not fully written in python they need system side packages to be build, it easier to use pre-build binaries for your operating system. 8 | 9 | Windows 10 | ~~~~~~~~~~~~~~~~~~~ 11 | Install Python for Windows, then you can install pypot with `pip <#via-python-packages>`_ in the command prompt. 12 | 13 | GNU/Linux 14 | ~~~~~~~~~~~~~~~~~~~ 15 | Use the binaries provided by your default package manager. 16 | 17 | On Ubuntu & Debian:: 18 | 19 | sudo apt-get install python-pip python-numpy python-scipy python-matplotlib 20 | 21 | On Fedora:: 22 | 23 | sudo yum install python-pip numpy scipy python-matplotlib 24 | 25 | On Arch Linux:: 26 | 27 | sudo pacman -S python2-pip python2-scipy python2-numpy python2-matplotlib 28 | 29 | After that, you can install pypot with `pip <#via-python-packages>`_. 30 | 31 | Mac OSX 32 | ~~~~~~~~~~~~~~~~~~~ 33 | Mac OSX (unlike GNU/Linux distributions) don’t come with a package manager, but there are a couple of popular package managers you can install, like `Homebrew `_. 34 | 35 | The easier way is to install `Homebrew `_. You have to type these commands in a terminal:: 36 | 37 | ruby -e "$(curl -fsSL https://raw.githubusercontent.com/Homebrew/install/master/install)" 38 | 39 | An use Homebrew to install python:: 40 | 41 | brew install python 42 | 43 | After that, you can install pypot with `pip <#via-python-packages>`_. 44 | 45 | 46 | Via Python Packages 47 | ------------------- 48 | The pypot package is entirely written in Python. So, the install process should be rather straightforward. You can directly install it via easy_install or pip:: 49 | 50 | pip install pypot 51 | 52 | **or**:: 53 | 54 | easy_install pypot 55 | 56 | The up to date archive can also be directly downloaded `here `_. 57 | 58 | If you are on a GNU/Linux operating system, you will need to execute the above commands with **sudo**. 59 | 60 | From the source code 61 | -------------------- 62 | 63 | You can also install it from the source. You can clone/fork our repo directly on `github `_. 64 | 65 | Before you start building pypot, you need to make sure that the following packages are already installed on your computer: 66 | 67 | * `python `_ 68 | * `pyserial `_ 69 | * `numpy `_ 70 | * `scipy `_ 71 | * `enum34 `_ 72 | 73 | Other optional packages may be installed depending on your needs: 74 | 75 | * `sphinx `_ and `sphinx-bootstrap-theme `_ (to build the doc) 76 | * `PyQt4 `_ (for the graphical tools) 77 | * `bottle `_ and `tornado `_ for REST API support and http-server 78 | 79 | Once it is done, you can build and install pypot with the classical:: 80 | 81 | cd pypot 82 | sudo python setup.py install 83 | 84 | Testing your install 85 | -------------------- 86 | 87 | You can test if the installation went well with:: 88 | 89 | python -c "import pypot" 90 | 91 | You will also have to install the driver for the USB2serial port. There are two devices that have been tested with pypot that could be used: 92 | 93 | * USB2AX - this device is designed to manage TTL communication only 94 | * USB2Dynamixel - this device can manage both TTL and RS485 communication. 95 | 96 | On Windows and Mac, it will be necessary to download and install a FTDI (VCP) driver to run the USB2Dynamixel, you can find it `here `__. Linux distributions should already come with an appropriate driver. The USB2AX device should not require a driver installation under MAC or Linux, it should already exist. For Windows XP, it should automatically install the correct driver. 97 | 98 | .. note:: On the side of the USB2Dynamixel there is a switch. This is used to select the bus you wish to communicate on. This means that you cannot control two different bus protocols at the same time. 99 | 100 | On most Linux distributions you will not have the necessary permission to access the serial port. You can either run the command in sudo or better you can add yourself to the *dialout* or the *uucp* group (depending on your distribution):: 101 | 102 | sudo addgroup $USER dialout 103 | sudo addgroup $USER uucp 104 | 105 | At this point you should have a pypot ready to be used! In the extremely unlikely case where anything went wrong during the installation, please refer to the `issue tracker `_. 106 | -------------------------------------------------------------------------------- /doc/logging.rst: -------------------------------------------------------------------------------- 1 | Mastering the logging system 2 | ============================ 3 | 4 | Pypot used the Python's builtin `logging `_ module for logging. For details on how to use this module please refer to Python's own documentation or the one on `django webstite `_. Here, we will only describe what pypot is logging and how it is organised (see section :ref:`log_struct`). We will also present a few examples on how to use pypot logging and parse the information (see section :ref:`log_ex`). 5 | 6 | .. _log_struct: 7 | 8 | Logging structure 9 | ----------------- 10 | 11 | Pypot is logging information at all different levels: 12 | 13 | * low-level dynamixel IO 14 | * motor and robot abstraction 15 | * within each primitive 16 | * each request received by the server 17 | 18 | .. note:: As you probably do not want to log everything (pypot is sending a lot of messages!!!), you have to select in the logging structure what is relevant in your program and define it in your logging configuration. 19 | 20 | Pypot's logging naming convention is following pypot's architecture. Here is the detail of what pypot is logging with the associated logger's name: 21 | 22 | * The logger's name **pypot.dynamixel.io** is logging information related to opening/closing port (INFO) and each sent/received package (DEBUG). The communication and timeout error are also logged (WARNING). This logger always provides you the port name, the baudrate and timeout of your connection as extra information. 23 | 24 | * The logger **pypot.dynamixel.motor** is logging each time a register of a motor is set (DEBUG). The name of the register, the name of the motor and the set value are given in the message. 25 | 26 | * **pypot.robot.config** is logging information regarding the creation of a robot through a config dictionary. A message is sent for each motor, controller and alias added (INFO). A WARNING message is also sent when the angle limits of a motor are changed. We provide as extra the entire config dictionary. 27 | 28 | * The logger **pypot.robot.robot** is logging when the synchronization is started/stopped (INFO) and when a primitive is attached (INFO). 29 | 30 | * **pypot.primitive.primitive** logs a message when the primitive is started/stopped and paused/resumed (INFO). Eeach :meth:`~pypot.primitive.primitive.LoopPrimitive.update` of a LoopPrimitive is also logged (DEBUG). Each time a primitive sets a value to a register a message is also logged (DEBUG). 31 | 32 | * **pypot.primitive.manager** provides you information on how the values sent within primitives were combined (DEBUG). 33 | 34 | * **pypot.server** logs when the server is started (INFO) and each handled request (DEBUG). 35 | 36 | .. _log_ex: 37 | 38 | Using Pypot's logging 39 | --------------------- 40 | 41 | As an example of what you can do with the logging system, here is how you can check the "real" update frequency of a loop primitive. 42 | 43 | First, you have to define a logging config. As you can see, here we specify that we only want the log coming form 'pypot.primitive' and the message is formatted so we only keep the timestamp:: 44 | 45 | LOGGING = { 46 | 'version': 1, 47 | 'disable_existing_loggers': True, 48 | 'formatters': { 49 | 'time': { 50 | 'format': '%(asctime)s', 51 | }, 52 | }, 53 | 'handlers': { 54 | 'file': { 55 | 'level': 'DEBUG', 56 | 'class': 'logging.FileHandler', 57 | 'filename': 'pypot.log', 58 | 'formatter': 'time', 59 | }, 60 | }, 61 | 'loggers': { 62 | 'pypot.primitive': { 63 | 'handlers': ['file', ], 64 | 'level': 'DEBUG', 65 | }, 66 | }, 67 | } 68 | 69 | Then, we just have to write a simple program, where for instance we run our dummy primitive for ten seconds:: 70 | 71 | import pypot.robot 72 | [...] 73 | 74 | if __name__ == '__main__': 75 | logging.config.dictConfig(LOGGING) 76 | 77 | r = pypot.robot.from_config(ergo_config) 78 | 79 | class DummyPrimitive(LoopPrimitive): 80 | pass 81 | 82 | p = DummyPrimitive(r, 50) 83 | p.start() 84 | time.sleep(10) 85 | p.stop() 86 | 87 | The execution of the program above will create a file named 'pypot.log' where each line corresponds to the timestamp of each primitive update. This file can then be easily parsed:: 88 | 89 | t = [] 90 | 91 | with open('pypot.log') as f: 92 | for l in f.readlines(): 93 | d = datetime.datetime.strptime('%Y-%m-%d %H:%M:%S,%f\n') 94 | t.append(d) 95 | 96 | t = numpy.array(t) 97 | dt = map(lambda dt: dt.total_seconds(), numpy.diff(t)) 98 | dt = numpy.array(dt) * 1000 99 | 100 | print(numpy.mean(dt), numpy.std(dt)) 101 | 102 | plot(dt) 103 | show() 104 | -------------------------------------------------------------------------------- /doc/move.rst: -------------------------------------------------------------------------------- 1 | .. _move: 2 | 3 | Move recording and playing 4 | ========================== 5 | 6 | The :mod:`~pypot.primitive.move` module contains utility classes to help you record and play moves. Those :class:`~pypot.primitive.move.Move` is simply defined as a sequence of positions. 7 | 8 | .. note:: To keep the :mod:`~pypot.primitive.move` module as simple as possible, you can only define :class:`~pypot.primitive.move.Move` as a predefined frequency and you can not define keyframes whenever you want. This could be added if it seems like it would be useful. 9 | 10 | You can use the :mod:`~pypot.primitive.move` module to: 11 | 12 | * record moves, 13 | * play moves, 14 | * save/load them on the disk. 15 | 16 | The :class:`~pypot.primitive.move.MoveRecorder` and :class:`~pypot.primitive.move.MovePlayer` are defined as subclass of :class:`~pypot.primitive.primitive.LoopPrimitive` and can thus be used as such. For instance, if you want to record a 50Hz move on all the motor of an ergo-robot you can simply use the following code:: 17 | 18 | import time 19 | import pypot.robot 20 | 21 | from pypot.primitive.move import MoveRecorder, Move, MovePlayer 22 | 23 | ergo = pypot.robot.from_config(...) 24 | 25 | move_recorder = MoveRecorder(ergo, 50, ergo.motors) 26 | 27 | ergo.compliant = True 28 | 29 | move_recorder.start() 30 | time.sleep(5) 31 | move_recorder.stop() 32 | 33 | This move can then be saved on disk:: 34 | 35 | with open('my_nice_move.move', 'w') as f: 36 | move_recorder.move.save(f) 37 | 38 | And loaded and replayed:: 39 | 40 | with open('my_nice_move.move') as f: 41 | m = Move.load(f) 42 | 43 | ergo.compliant = False 44 | 45 | move_player = MovePlayer(ergo, m) 46 | move_player.start() 47 | 48 | .. warning:: It is important to note that you should be sure that you primitive actually runs at the same speed that the move has been recorded. If the player can not run as fast as the framerate of the recorded :class:`~pypot.primitive.move.Move`, it will be played slowly resulting in a slower version of your move. 49 | -------------------------------------------------------------------------------- /doc/poppy-creatures.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/poppy-project/pypot/13263953a7970209efbcb476bd07b5049151f52e/doc/poppy-creatures.jpg -------------------------------------------------------------------------------- /doc/pypot-archi.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/poppy-project/pypot/13263953a7970209efbcb476bd07b5049151f52e/doc/pypot-archi.jpg -------------------------------------------------------------------------------- /doc/pypot.dynamixel.rst: -------------------------------------------------------------------------------- 1 | :mod:`dynamixel` Package 2 | ======================== 3 | 4 | .. autofunction:: pypot.dynamixel.get_available_ports 5 | 6 | .. autofunction:: pypot.dynamixel.find_port 7 | 8 | .. autofunction:: pypot.dynamixel.autodetect_robot 9 | 10 | 11 | :mod:`~pypot.dynamixel.io` Package 12 | ---------------------------------- 13 | 14 | .. automodule:: pypot.dynamixel.io 15 | :members: 16 | :undoc-members: 17 | :show-inheritance: 18 | 19 | .. automodule:: pypot.dynamixel.io.io 20 | :members: 21 | :undoc-members: 22 | :show-inheritance: 23 | 24 | .. automodule:: pypot.dynamixel.io.io_320 25 | :members: 26 | :undoc-members: 27 | :show-inheritance: 28 | 29 | :mod:`motor` Module 30 | ------------------- 31 | 32 | .. automodule:: pypot.dynamixel.motor 33 | :members: 34 | :show-inheritance: 35 | 36 | .. autoattribute:: pypot.dynamixel.motor.DxlMotor.registers 37 | 38 | 39 | :mod:`controller` Module 40 | ------------------------ 41 | 42 | .. automodule:: pypot.dynamixel.controller 43 | :members: 44 | :undoc-members: 45 | :show-inheritance: 46 | 47 | 48 | :mod:`error` Module 49 | ------------------- 50 | 51 | .. automodule:: pypot.dynamixel.error 52 | :members: 53 | :undoc-members: 54 | :show-inheritance: 55 | 56 | :mod:`conversion` Module 57 | ------------------------ 58 | 59 | .. automodule:: pypot.dynamixel.conversion 60 | :members: 61 | :undoc-members: 62 | :show-inheritance: 63 | 64 | :mod:`protocol` Package 65 | ----------------------- 66 | 67 | :mod:`v1` Module 68 | ++++++++++++++++ 69 | 70 | .. automodule:: pypot.dynamixel.protocol.v1 71 | :members: 72 | :undoc-members: 73 | :show-inheritance: 74 | 75 | :mod:`v2` Module 76 | ++++++++++++++++ 77 | 78 | .. automodule:: pypot.dynamixel.protocol.v2 79 | :members: 80 | :undoc-members: 81 | :show-inheritance: 82 | -------------------------------------------------------------------------------- /doc/pypot.primitive.rst: -------------------------------------------------------------------------------- 1 | :mod:`primitive` Package 2 | ======================== 3 | 4 | 5 | :mod:`~pypot.primitive.primitive` Module 6 | ---------------------------------------- 7 | 8 | .. automodule:: pypot.primitive.primitive 9 | :members: 10 | :undoc-members: 11 | :show-inheritance: 12 | 13 | 14 | :mod:`~pypot.primitive.manager` Module 15 | -------------------------------------- 16 | 17 | .. automodule:: pypot.primitive.manager 18 | :members: 19 | :undoc-members: 20 | :show-inheritance: 21 | 22 | :mod:`~pypot.primitive.move` Module 23 | ------------------------------------ 24 | 25 | .. automodule:: pypot.primitive.move 26 | :members: 27 | :undoc-members: 28 | :show-inheritance: 29 | 30 | :mod:`~pypot.primitive.utils` Module 31 | ------------------------------------ 32 | 33 | .. automodule:: pypot.primitive.utils 34 | :members: 35 | :undoc-members: 36 | :show-inheritance: -------------------------------------------------------------------------------- /doc/pypot.robot.rst: -------------------------------------------------------------------------------- 1 | :mod:`robot` Package 2 | ==================== 3 | 4 | .. automodule:: pypot.robot 5 | :members: 6 | :undoc-members: 7 | :show-inheritance: 8 | 9 | 10 | :mod:`robot` Module 11 | ------------------- 12 | 13 | .. automodule:: pypot.robot.robot 14 | :members: 15 | :undoc-members: 16 | :show-inheritance: 17 | 18 | :mod:`motor` Module 19 | ------------------- 20 | 21 | .. automodule:: pypot.robot.motor 22 | :members: 23 | :undoc-members: 24 | :show-inheritance: 25 | 26 | :mod:`sensor` Module 27 | -------------------- 28 | 29 | .. automodule:: pypot.robot.sensor 30 | :members: 31 | :undoc-members: 32 | :show-inheritance: 33 | 34 | :mod:`controller` Module 35 | ------------------------ 36 | 37 | .. automodule:: pypot.robot.controller 38 | :members: 39 | :undoc-members: 40 | :show-inheritance: 41 | 42 | :mod:`io` Module 43 | ---------------- 44 | 45 | .. automodule:: pypot.robot.io 46 | :members: 47 | :undoc-members: 48 | :show-inheritance: 49 | 50 | :mod:`config` Module 51 | -------------------- 52 | 53 | .. automodule:: pypot.robot.config 54 | :members: 55 | :undoc-members: 56 | :show-inheritance: 57 | 58 | :mod:`remote` Module 59 | -------------------- 60 | 61 | .. automodule:: pypot.robot.remote 62 | :members: 63 | :undoc-members: 64 | :show-inheritance: 65 | -------------------------------------------------------------------------------- /doc/pypot.sensor.rst: -------------------------------------------------------------------------------- 1 | :mod:`sensor` Package 2 | ===================== 3 | 4 | .. automodule:: pypot.sensor 5 | :members: 6 | :undoc-members: 7 | :show-inheritance: 8 | 9 | 10 | :mod:`kinect` Module 11 | -------------------- 12 | 13 | .. automodule:: pypot.sensor.kinect.sensor 14 | :members: 15 | :undoc-members: 16 | :show-inheritance: 17 | 18 | :mod:`optitrack` Module 19 | ----------------------- 20 | 21 | .. automodule:: pypot.sensor.optitrack 22 | :members: 23 | :undoc-members: 24 | :show-inheritance: 25 | -------------------------------------------------------------------------------- /doc/pypot.server.rst: -------------------------------------------------------------------------------- 1 | :mod:`server` Package 2 | ===================== 3 | 4 | .. automodule:: pypot.server 5 | :members: 6 | :undoc-members: 7 | :show-inheritance: 8 | 9 | :mod:`rest` Module 10 | --------------------- 11 | 12 | .. automodule:: pypot.server.rest 13 | :members: 14 | :undoc-members: 15 | :show-inheritance: 16 | 17 | :mod:`httpserver` Module 18 | ------------------------ 19 | 20 | .. automodule:: pypot.server.httpserver 21 | :members: 22 | :undoc-members: 23 | :show-inheritance: 24 | 25 | 26 | :mod:`zmqserver` Module 27 | ----------------------- 28 | 29 | .. automodule:: pypot.server.zmqserver 30 | :members: 31 | :undoc-members: 32 | :show-inheritance: 33 | 34 | :mod:`snap` Module 35 | ----------------------- 36 | 37 | .. automodule:: pypot.server.snap 38 | :members: 39 | :undoc-members: 40 | :show-inheritance: 41 | -------------------------------------------------------------------------------- /doc/pypot.tools.rst: -------------------------------------------------------------------------------- 1 | pypot.tools package 2 | =================== 3 | 4 | Submodules 5 | ---------- 6 | 7 | .. toctree:: 8 | 9 | 10 | pypot.tools.dxl_reset module 11 | ---------------------------- 12 | 13 | .. automodule:: pypot.tools.dxl_reset 14 | :members: 15 | :undoc-members: 16 | :show-inheritance: 17 | 18 | -------------------------------------------------------------------------------- /doc/pypot.utils.rst: -------------------------------------------------------------------------------- 1 | :mod:`utils` Package 2 | ==================== 3 | 4 | .. automodule:: pypot.utils 5 | :members: 6 | :undoc-members: 7 | :show-inheritance: 8 | 9 | 10 | :mod:`stoppablethread` Module 11 | ----------------------------- 12 | 13 | .. automodule:: pypot.utils.stoppablethread 14 | :members: 15 | :undoc-members: 16 | :show-inheritance: 17 | 18 | :mod:`trajectory` Module 19 | ----------------------------- 20 | 21 | .. automodule:: pypot.utils.trajectory 22 | :members: 23 | :undoc-members: 24 | :show-inheritance: 25 | -------------------------------------------------------------------------------- /doc/pypot.vrep.rst: -------------------------------------------------------------------------------- 1 | :mod:`vrep` Package 2 | =================== 3 | 4 | .. automodule:: pypot.vrep 5 | :members: 6 | :undoc-members: 7 | :show-inheritance: 8 | 9 | 10 | :mod:`io` Module 11 | ---------------- 12 | 13 | .. automodule:: pypot.vrep.io 14 | :members: 15 | :undoc-members: 16 | :show-inheritance: 17 | 18 | 19 | :mod:`controller` Module 20 | ------------------------ 21 | 22 | .. automodule:: pypot.vrep.controller 23 | :members: 24 | :undoc-members: 25 | :show-inheritance: 26 | -------------------------------------------------------------------------------- /doc/pypot_logo-144x144.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/poppy-project/pypot/13263953a7970209efbcb476bd07b5049151f52e/doc/pypot_logo-144x144.png -------------------------------------------------------------------------------- /doc/pypot_logo-48x48.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/poppy-project/pypot/13263953a7970209efbcb476bd07b5049151f52e/doc/pypot_logo-48x48.png -------------------------------------------------------------------------------- /doc/pypot_logo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/poppy-project/pypot/13263953a7970209efbcb476bd07b5049151f52e/doc/pypot_logo.png -------------------------------------------------------------------------------- /doc/remote_access.rst: -------------------------------------------------------------------------------- 1 | REST API 2 | ======== 3 | 4 | We add the possibility to remotely access and control your robot through TCP network. This can be useful both to work with client/server architecture (e.g. to separate the low-level control running on an embedded computer and higher-level computation on a more powerful computer) and to allow you to plug your existing code written in another language to the pypot's API. 5 | 6 | We defined a protocol which permits the access of all the robot variables and method (including motors and primitives) via a JSON request. The protocol is entirely described in the section :ref:`remote_protocol` below. Two transport methods have been developed so far: 7 | 8 | * HTTP via GET and POST request (see the :class:`~pypot.server.httpserver.HTTPRobotServer`) 9 | * ZMQ socket (see the :class:`~pypot.server.zmqserver.ZMQRobotServer`) 10 | 11 | The :class:`~pypot.server.rest.RESTRobot` has been abstracted from the server, so you can easily add new transport methods if needed. 12 | 13 | As an example of what you can do, here is the code of getting the load of a motor and changing its position:: 14 | 15 | import zmq 16 | import threading 17 | 18 | robot = pypot.robot.from_config(...) 19 | 20 | server = pypot.server.ZMQServer(robot, host, port) 21 | # We launch the server inside a thread 22 | threading.Thread(target=lambda: server.run()).start() 23 | 24 | c = zmq.Context() 25 | s = c.socket(zmq.REQ) 26 | 27 | req = {"robot": {"get_register_value": {"motor": "m2", "register": "present_load"}}} 28 | s.send_json(req) 29 | answer = s.recv_json() 30 | print(answer) 31 | 32 | req = {"robot": {"set_register_value": {"motor": "m2", "register": "goal_position", "value": 20}}} 33 | s.send_json(req) 34 | answer = s.recv_json() 35 | print(answer) 36 | 37 | .. _remote_protocol: 38 | 39 | Protocol 40 | -------- 41 | 42 | The entire protocol is entirely described `here `_. 43 | -------------------------------------------------------------------------------- /doc/vrep.rst: -------------------------------------------------------------------------------- 1 | Using a simulated robot with V-REP 2 | ********************************** 3 | 4 | Connecting to V-REP 5 | ------------------- 6 | 7 | As it is often easier to work in simulation rather than with the real robot, pypot has been linked with the `V-REP simulator `_. It is described as the "Swiss army knife among robot simulators" and is a very powerful tool to quickly (re)create robotics setup. As presenting V-REP is way beyond the scope of this tutorial, we will here assume that you are already familiar with this tool. Otherwise, you should directly refer to `V-REP documentation `_. 8 | 9 | Details about how to connect pypot and V-REP can be found in `this post `_. 10 | 11 | The connection between pypot and V-REP was designed to let you seamlessly switch from your real robot to the simulated one. It is based on `V-REP's remote API `_. 12 | 13 | In order to connect to V-REP through pypot, you will only need to install the `V-REP `_ simulator. Pypot comes with a specific :class:`~pypot.vrep.io.VrepIO` designed to communicate with V-REP through its `remote API `_. 14 | 15 | This IO can be used to: 16 | 17 | * connect to the V-REP server : :class:`~pypot.vrep.io.VrepIO` 18 | * load a scene : :meth:`~pypot.vrep.io.VrepIO.load_scene` 19 | * start/stop/restart a simulation : :meth:`~pypot.vrep.io.VrepIO.start_simulation`, :meth:`~pypot.vrep.io.VrepIO.stop_simulation`, :meth:`~pypot.vrep.io.VrepIO.restart_simulation` 20 | * pause/resume the simulation : :meth:`~pypot.vrep.io.VrepIO.pause_simulation`, :meth:`~pypot.vrep.io.VrepIO.resume_simulation` 21 | * get/set a motor position : :meth:`~pypot.vrep.io.VrepIO.get_motor_position`, :meth:`~pypot.vrep.io.VrepIO.set_motor_position` 22 | * get an object position/orienation : :meth:`~pypot.vrep.io.VrepIO.get_object_position`, :meth:`~pypot.vrep.io.VrepIO.get_object_orientation` 23 | 24 | Switch between the simulation and the real robot in a single line of code 25 | ------------------------------------------------------------------------- 26 | 27 | As stated above, the bridge between V-REP and pypot has been designed to let you easily switch from the robot to the simulated version. In most case, you should only have to change the way you instantiate your robot:: 28 | 29 | # Working with the real robot 30 | import pypot.robot 31 | 32 | poppy = pypot.robot.from_config(config) 33 | 34 | poppy.walk.start() 35 | 36 | will become:: 37 | 38 | # Working with the simulated version 39 | import pypot.vrep 40 | 41 | poppy = pypot.vrep.from_vrep(config, vrep_host, vrep_port, vrep_scene) 42 | 43 | poppy.walk.start() 44 | 45 | In particular, the walking primitive should work exactly the same way in both cases without needing to change anything. 46 | 47 | .. note:: Not all dynamixel registers have their V-REP equivalent. For the moment, only the control of the position is used. More advanced features can be easily added thanks to the controller abstraction (see section :ref:`extending`). 48 | -------------------------------------------------------------------------------- /pypot/__init__.py: -------------------------------------------------------------------------------- 1 | import logging 2 | 3 | from ._version import __version__ 4 | 5 | logging.getLogger(__name__).addHandler(logging.NullHandler()) 6 | -------------------------------------------------------------------------------- /pypot/_version.py: -------------------------------------------------------------------------------- 1 | __version__ = '5.0.2' 2 | -------------------------------------------------------------------------------- /pypot/creatures/__init__.py: -------------------------------------------------------------------------------- 1 | import sys 2 | 3 | from .abstractcreature import AbstractPoppyCreature 4 | 5 | module = sys.modules[__name__] 6 | 7 | 8 | installed_poppy_creatures = {} 9 | # Feel free to make a pull request to add your own creature here 10 | existing_creatures = ['poppy-humanoid', 'poppy-torso', 'poppy-ergo-jr', 11 | 'poppy-ergo-starter', 'poppy-6dof-right-arm', 12 | 'poppy-dragster-mini', 'poppy-ergo', 'roboticia-quattro', 13 | 'roboticia-first', 'roboticia-uno', 'roboticia-drive', 14 | 'doggy'] 15 | 16 | for creature in existing_creatures: 17 | package = creature.replace('-', '_') 18 | cls_name = ''.join(x.capitalize() or '_' for x in package.split('_')) 19 | 20 | try: 21 | cls = getattr(__import__(package), cls_name) 22 | installed_poppy_creatures[creature] = cls 23 | setattr(module, cls_name, cls) 24 | 25 | except (ImportError, AttributeError): 26 | pass 27 | -------------------------------------------------------------------------------- /pypot/creatures/configure_utility.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | 4 | 5 | """ 6 | Poppy configuration tools 7 | 8 | Examples: 9 | * poppy-configure ergo-jr m2 10 | 11 | """ 12 | import sys 13 | 14 | from argparse import ArgumentParser, ArgumentDefaultsHelpFormatter 15 | from subprocess import call 16 | 17 | from pypot.creatures import installed_poppy_creatures 18 | from pypot.utils import flushed_print as print 19 | 20 | 21 | def find_port_for_motor(config, motor): 22 | def flatten_motorgroups(motors, groups): 23 | l = [] 24 | for m in motors: 25 | if m in groups: 26 | l += flatten_motorgroups(groups[m], groups) 27 | else: 28 | l.append(m) 29 | return l 30 | 31 | for bus in config['controllers']: 32 | motors_on_bus = config['controllers'][bus]["attached_motors"] 33 | motors = flatten_motorgroups(motors_on_bus, config['motorgroups']) 34 | if motor in motors: 35 | return config['controllers'][bus]["port"] 36 | 37 | raise ValueError('Something must be wrong in your configuration file. ' 38 | 'Could not find bus for motor {}'.format(motor)) 39 | 40 | 41 | def main(): 42 | robots = [c.replace('poppy-', '') for c in installed_poppy_creatures] 43 | 44 | parser = ArgumentParser(description='Configuration tool for Poppy robots ', 45 | formatter_class=ArgumentDefaultsHelpFormatter) 46 | 47 | parser.add_argument('robot', type=str, choices=robots, 48 | help='Robot used.') 49 | 50 | parser.add_argument('motor', type=str, 51 | help='Name of the motor to configure.') 52 | 53 | args = parser.parse_args() 54 | 55 | RobotCls = installed_poppy_creatures['poppy-{}'.format(args.robot)] 56 | c = RobotCls.default_config 57 | 58 | if args.motor not in c['motors']: 59 | print('"{}" is not a motor of "{}"! ' 60 | 'possibilities={}'.format(args.motor, args.robot, 61 | sorted(c['motors'].keys()))) 62 | print('Exiting now...') 63 | sys.exit(1) 64 | 65 | motor_config = c['motors'][args.motor] 66 | 67 | args = [ 68 | '--id', motor_config['id'], 69 | '--type', motor_config['type'], 70 | '--port', find_port_for_motor(c, args.motor), 71 | '--return-delay-time', 0 72 | ] 73 | 74 | if 'wheel_mode' in motor_config.keys(): 75 | args.extend(('--wheel-mode', motor_config['wheel_mode'])) 76 | else: 77 | args.extend(('--angle-limit',motor_config['angle_limit'][0],motor_config['angle_limit'][1], 78 | '--goto-zero')) 79 | 80 | call(['dxl-config'] + list(map(str, args))) 81 | 82 | 83 | if __name__ == '__main__': 84 | main() 85 | -------------------------------------------------------------------------------- /pypot/dynamixel/__init__.py: -------------------------------------------------------------------------------- 1 | import sys 2 | import platform 3 | import glob 4 | import logging 5 | 6 | import serial.tools.list_ports 7 | 8 | 9 | from .io import DxlIO, Dxl320IO 10 | from .error import BaseErrorHandler 11 | from .syncloop import BaseDxlController 12 | from .motor import DxlMXMotor, DxlAXRXMotor, DxlXL320Motor, DxlSRMotor 13 | from .io.abstract_io import DxlError 14 | 15 | from ..robot import Robot 16 | 17 | logger = logging.getLogger(__name__) 18 | 19 | 20 | def _get_available_ports(): 21 | """ Tries to find the available serial ports on your system. """ 22 | if platform.system() == 'Darwin': 23 | return glob.glob('/dev/tty.usb*') 24 | 25 | elif platform.system() == 'Linux': 26 | return glob.glob('/dev/ttyACM*') + glob.glob('/dev/ttyUSB*') + glob.glob('/dev/ttyAMA*') 27 | 28 | elif sys.platform.lower() == 'cygwin': 29 | return glob.glob('/dev/com*') 30 | 31 | elif platform.system() == 'Windows': 32 | import winreg 33 | import itertools 34 | 35 | ports = [] 36 | path = 'HARDWARE\\DEVICEMAP\\SERIALCOMM' 37 | key = winreg.OpenKey(winreg.HKEY_LOCAL_MACHINE, path) 38 | 39 | for i in itertools.count(): 40 | try: 41 | ports.append(str(winreg.EnumValue(key, i)[1])) 42 | except WindowsError: 43 | return ports 44 | else: 45 | raise EnvironmentError('{} is an unsupported platform, cannot find serial ports!'.format(platform.system())) 46 | return [] 47 | 48 | 49 | def get_available_ports(only_free=False): 50 | ports = _get_available_ports() 51 | 52 | if only_free: 53 | ports = [port for port in ports if port not in DxlIO.get_used_ports()] 54 | 55 | return ports 56 | 57 | 58 | def get_port_vendor_info(port=None): 59 | """ Return vendor informations of a usb2serial device. 60 | It may depends on the Operating System. 61 | :param string port: port of the usb2serial device 62 | 63 | :Example: 64 | 65 | Result with a USB2Dynamixel on Linux: 66 | In [1]: import pypot.dynamixel 67 | In [2]: pypot.dynamixel.get_port_vendor_info('/dev/ttyUSB0') 68 | Out[2]: 'USB VID:PID=0403:6001 SNR=A7005LKE' """ 69 | 70 | port_info_dict = dict((x[0], x[2]) for x in serial.tools.list_ports.comports()) 71 | return port_info_dict[port] if port is not None else port_info_dict 72 | 73 | 74 | def find_port(ids, strict=True): 75 | """ Find the port with the specified attached motor ids. 76 | 77 | :param list ids: list of motor ids to find 78 | :param bool strict: specify if all ids should be find (when set to False, only half motor must be found) 79 | 80 | .. warning:: If two (or more) ports are attached to the same list of motor ids the first match will be returned. 81 | 82 | """ 83 | ids_founds = [] 84 | for port in get_available_ports(): 85 | for DxlIOCls in (DxlIO, Dxl320IO): 86 | try: 87 | with DxlIOCls(port) as dxl: 88 | _ids_founds = dxl.scan(ids) 89 | ids_founds += _ids_founds 90 | 91 | if strict and sorted(_ids_founds) == sorted(ids): 92 | return port 93 | 94 | if not strict and len(_ids_founds) >= len(ids) / 2: 95 | logger.warning('Missing ids: {}'.format(ids, list(set(ids) - set(_ids_founds)))) 96 | return port 97 | 98 | if len(ids_founds) > 0: 99 | logger.warning('Port:{} ids found:{}'.format(port, _ids_founds)) 100 | 101 | except DxlError: 102 | logger.warning('DxlError on port {}'.format(port)) 103 | continue 104 | 105 | missing = list(set(ids) - set(ids_founds)) 106 | if len(missing) == 0: 107 | raise ValueError('All motors have been found but they are not connected as specified in the configuration file, please check connections') 108 | raise IndexError('No suitable port found for ids {}. These ids are missing {} !'.format(ids, missing)) 109 | 110 | 111 | def autodetect_robot(): 112 | """ Creates a :class:`~pypot.robot.robot.Robot` by detecting dynamixel motors on all available ports. """ 113 | motor_controllers = [] 114 | 115 | for port in get_available_ports(): 116 | for DxlIOCls in (DxlIO, Dxl320IO): 117 | dxl_io = DxlIOCls(port) 118 | ids = dxl_io.scan() 119 | 120 | if not ids: 121 | dxl_io.close() 122 | continue 123 | 124 | models = dxl_io.get_model(ids) 125 | 126 | motorcls = { 127 | 'MX': DxlMXMotor, 128 | 'RX': DxlAXRXMotor, 129 | 'AX': DxlAXRXMotor, 130 | 'XL': DxlXL320Motor, 131 | 'SR': DxlSRMotor, 132 | } 133 | 134 | motors = [motorcls[model[:2]](id, model=model) 135 | for id, model in zip(ids, models)] 136 | 137 | c = BaseDxlController(dxl_io, motors) 138 | motor_controllers.append(c) 139 | break 140 | 141 | return Robot(motor_controllers) 142 | -------------------------------------------------------------------------------- /pypot/dynamixel/error.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | 3 | import logging 4 | 5 | logger = logging.getLogger(__name__) 6 | 7 | 8 | class DxlErrorHandler(object): 9 | """ This class is used to represent all the error that you can/should handle. 10 | 11 | The errors can be of two types: 12 | 13 | * communication error (timeout, communication) 14 | * motor error (voltage, limit, overload...) 15 | 16 | This class was designed as an abstract class and so you should write your own handler by subclassing this class and defining the apropriate behavior for your program. 17 | 18 | .. warning:: The motor error should be overload carrefuly as they can indicate important mechanical issue. 19 | 20 | """ 21 | # MARK: - Communication errors 22 | 23 | def handle_timeout(self, timeout_error): 24 | raise NotImplementedError 25 | 26 | def handle_communication_error(self, communication_error): 27 | raise NotImplementedError 28 | 29 | # MARK: - Motor errors 30 | def handle_input_voltage_error(self, instruction_packet): 31 | raise NotImplementedError 32 | 33 | def handle_angle_limit_error(self, instruction_packet): 34 | raise NotImplementedError 35 | 36 | def handle_overheating_error(self, instruction_packet): 37 | raise NotImplementedError 38 | 39 | def handle_range_error(self, instruction_packet): 40 | raise NotImplementedError 41 | 42 | def handle_checksum_error(self, instruction_packet): 43 | raise NotImplementedError 44 | 45 | def handle_overload_error(self, instruction_packet): 46 | raise NotImplementedError 47 | 48 | def handle_instruction_error(self, instruction_packet): 49 | raise NotImplementedError 50 | 51 | def handle_none_error(self, instruction_packet): 52 | raise NotImplementedError 53 | 54 | 55 | class BaseErrorHandler(DxlErrorHandler): 56 | """ This class is a basic handler that just skip the communication errors. """ 57 | def handle_timeout(self, timeout_error): 58 | msg = 'Timeout after sending {} to motors {}'.format(timeout_error.instruction_packet, 59 | timeout_error.ids) 60 | logger.warning(msg, 61 | extra={'port': timeout_error.dxl_io.port, 62 | 'baudrate': timeout_error.dxl_io.baudrate, 63 | 'timeout': timeout_error.dxl_io.timeout}) 64 | 65 | def handle_communication_error(self, com_error): 66 | msg = 'Communication error after sending {}'.format(com_error.instruction_packet) 67 | 68 | logger.warning(msg, 69 | extra={'port': com_error.dxl_io.port, 70 | 'baudrate': com_error.dxl_io.baudrate, 71 | 'timeout': com_error.dxl_io.timeout}) 72 | 73 | def handle_none_error(self, instruction_packet): 74 | logger.info('None Error!') 75 | -------------------------------------------------------------------------------- /pypot/dynamixel/io/__init__.py: -------------------------------------------------------------------------------- 1 | from .io import DxlIO 2 | from .io_320 import Dxl320IO 3 | from .abstract_io import DxlError 4 | -------------------------------------------------------------------------------- /pypot/dynamixel/protocol/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/poppy-project/pypot/13263953a7970209efbcb476bd07b5049151f52e/pypot/dynamixel/protocol/__init__.py -------------------------------------------------------------------------------- /pypot/dynamixel/syncloop.py: -------------------------------------------------------------------------------- 1 | from ..robot.controller import MotorsController 2 | 3 | from .controller import (DxlController, 4 | PosSpeedLoadDxlController, 5 | AngleLimitRegisterController) 6 | 7 | 8 | class MetaDxlController(MotorsController): 9 | """ Synchronizes the reading/writing of :class:`~pypot.dynamixel.motor.DxlMotor` with the real motors. 10 | 11 | This class handles synchronization loops that automatically read/write values from the "software" :class:`~pypot.dynamixel.motor.DxlMotor` with their "hardware" equivalent. Those loops shared a same :class:`~pypot.dynamixel.io.DxlIO` connection to avoid collision in the bus. Each loop run within its own thread as its own frequency. 12 | 13 | .. warning:: As all the loop attached to a controller shared the same bus, you should make sure that they can run without slowing down the other ones. 14 | 15 | """ 16 | def __init__(self, io, motors, controllers): 17 | MotorsController.__init__(self, io, motors, 1.) 18 | self.controllers = controllers 19 | 20 | def setup(self): 21 | """ Starts all the synchronization loops. """ 22 | [c.start() for c in self.controllers] 23 | [c.wait_to_start() for c in self.controllers] 24 | 25 | def update(self): 26 | pass 27 | 28 | def teardown(self): 29 | """ Stops the synchronization loops. """ 30 | [c.stop() for c in self.controllers] 31 | 32 | 33 | class BaseDxlController(MetaDxlController): 34 | """ Implements a basic controller that synchronized the most frequently used values. 35 | 36 | More precisely, this controller: 37 | * reads the present position, speed, load at 50Hz 38 | * writes the goal position, moving speed and torque limit at 50Hz 39 | * writes the pid gains (or compliance margin and slope) at 10Hz 40 | * reads the present voltage and temperature at 1Hz 41 | 42 | """ 43 | def __init__(self, io, motors): 44 | controllers = [ 45 | PosSpeedLoadDxlController(io, motors, 50.), 46 | 47 | AngleLimitRegisterController(io, motors, 10., False), 48 | DxlController(io, motors, 1., False, 'get', 'present_voltage'), 49 | DxlController(io, motors, 1., False, 'get', 'present_temperature') 50 | ] 51 | 52 | pid_motors = [m for m in motors 53 | if (m.model.startswith('MX') or \ 54 | m.model.startswith('XL-320'))] 55 | if pid_motors: 56 | controllers.insert(0, DxlController(io, pid_motors, 10., False, 57 | 'set', 'pid_gain', 'pid')) 58 | 59 | force_control_motors = [m for m in motors if m.model.startswith('SR')] 60 | 61 | if force_control_motors: 62 | controllers.insert(0, DxlController(io, force_control_motors, 10., False, 63 | 'set', 'force_control_enable', 'force_control_enable')) 64 | controllers.insert(0, DxlController(io, force_control_motors, 10., False, 65 | 'set', 'goal_force', 'goal_force')) 66 | 67 | current_motors = [m for m in motors 68 | if (m.model.startswith('MX-64') or \ 69 | m.model.startswith('MX-106') or \ 70 | m.model.startswith('SR'))] 71 | 72 | if current_motors: 73 | controllers.insert(0, DxlController(io, current_motors, 10., False, 74 | 'get', 'present_current', 'present_current')) 75 | 76 | margin_slope_motors = [m for m in motors 77 | if (m.model.startswith('AX') or 78 | m.model.startswith('RX'))] 79 | if margin_slope_motors: 80 | controllers.append(DxlController(io, margin_slope_motors, 10, False, 81 | 'set', 'compliance_margin')) 82 | controllers.append(DxlController(io, margin_slope_motors, 10, False, 83 | 'set', 'compliance_slope')) 84 | 85 | MetaDxlController.__init__(self, io, motors, controllers) 86 | 87 | 88 | class LightDxlController(MetaDxlController): 89 | def __init__(self, io, motors): 90 | controllers = [ 91 | PosSpeedLoadDxlController(io, motors, 50.), 92 | 93 | AngleLimitRegisterController(io, motors, 10., True), 94 | DxlController(io, motors, 10., True, 'get', 'present_voltage'), 95 | DxlController(io, motors, 10., True, 'get', 'present_temperature') 96 | ] 97 | 98 | pid_motors = [m for m in motors 99 | if (m.model.startswith('MX') or 100 | m.model.startswith('XL-320'))] 101 | if pid_motors: 102 | controllers.insert(0, DxlController(io, pid_motors, 10., True, 103 | 'set', 'pid_gain', 'pid')) 104 | 105 | margin_slope_motors = [m for m in motors 106 | if (m.model.startswith('AX') or 107 | m.model.startswith('RX'))] 108 | if margin_slope_motors: 109 | controllers.append(DxlController(io, margin_slope_motors, 10., True, 110 | 'set', 'compliance_margin')) 111 | controllers.append(DxlController(io, margin_slope_motors, 10., True, 112 | 'set', 'compliance_slope')) 113 | 114 | led_motors = [m for m in motors if m.model.startswith('XL-320')] 115 | if led_motors: 116 | controllers.append(DxlController(io, led_motors, 5., False, 117 | 'set', 'LED_color', 'led')) 118 | 119 | MetaDxlController.__init__(self, io, motors, controllers) 120 | -------------------------------------------------------------------------------- /pypot/primitive/__init__.py: -------------------------------------------------------------------------------- 1 | from .primitive import Primitive, LoopPrimitive 2 | -------------------------------------------------------------------------------- /pypot/primitive/manager.py: -------------------------------------------------------------------------------- 1 | import logging 2 | import numpy 3 | 4 | from collections import defaultdict 5 | from functools import partial 6 | from threading import Lock 7 | 8 | from ..utils.stoppablethread import StoppableLoopThread 9 | 10 | 11 | logger = logging.getLogger(__name__) 12 | 13 | 14 | class PrimitiveManager(StoppableLoopThread): 15 | """ Combines all :class:`~pypot.primitive.primitive.Primitive` orders and affect them to the real motors. 16 | 17 | At a predefined frequency, the manager gathers all the orders sent by the primitive to the "fake" motors, combined them thanks to the filter function and affect them to the "real" motors. 18 | 19 | .. note:: The primitives are automatically added (resp. removed) to the manager when they are started (resp. stopped). 20 | 21 | """ 22 | def __init__(self, motors, freq=50, filter=partial(numpy.mean, axis=0)): 23 | """ 24 | :param motors: list of real motors used by the attached primitives 25 | :type motors: list of :class:`~pypot.dynamixel.motor.DxlMotor` 26 | :param int freq: update frequency 27 | :param func filter: function used to combine the different request (default mean) 28 | 29 | """ 30 | StoppableLoopThread.__init__(self, freq) 31 | 32 | self._prim = [] 33 | self._motors = motors 34 | self._filter = filter 35 | 36 | self.syncing = Lock() 37 | 38 | def add(self, p): 39 | """ Add a primitive to the manager. The primitive automatically attached itself when started. """ 40 | self._prim.append(p) 41 | 42 | def remove(self, p): 43 | """ Remove a primitive from the manager. The primitive automatically remove itself when stopped. """ 44 | self._prim.remove(p) 45 | 46 | @property 47 | def primitives(self): 48 | """ List of all attached :class:`~pypot.primitive.primitive.Primitive`. """ 49 | return self._prim 50 | 51 | def update(self): 52 | """ Combined at a predefined frequency the request orders and affect them to the real motors. """ 53 | with self.syncing: 54 | for m in self._motors: 55 | to_set = defaultdict(list) 56 | 57 | for p in self._prim: 58 | for key, val in getattr(p.robot, m.name)._to_set.items(): 59 | to_set[key].append(val) 60 | 61 | for key, val in to_set.items(): 62 | if key == 'led': 63 | colors = set(val) 64 | if len(colors) > 1: 65 | colors -= {'off'} 66 | filtred_val = colors.pop() 67 | else: 68 | filtred_val = self._filter(val) 69 | 70 | logger.debug('Combined %s.%s from %s to %s', 71 | m.name, key, val, filtred_val) 72 | setattr(m, key, filtred_val) 73 | 74 | [p._synced.set() for p in self._prim] 75 | 76 | def stop(self): 77 | """ Stop the primitive manager. """ 78 | for p in self.primitives[:]: 79 | p.stop() 80 | 81 | StoppableLoopThread.stop(self) 82 | -------------------------------------------------------------------------------- /pypot/primitive/utils.py: -------------------------------------------------------------------------------- 1 | import numpy 2 | 3 | from copy import deepcopy 4 | from collections import defaultdict 5 | 6 | from .primitive import Primitive, LoopPrimitive 7 | 8 | 9 | class Sinus(LoopPrimitive): 10 | """ Apply a sinus on the motor specified as argument. Parameters (amp, offset and phase) should be specified in degree. """ 11 | properties = LoopPrimitive.properties + ['frequency', 'amplitude', 'offset', 'phase'] 12 | 13 | def __init__(self, robot, refresh_freq, 14 | motor_list, 15 | amp=1, freq=0.5, offset=0, phase=0): 16 | 17 | LoopPrimitive.__init__(self, robot, refresh_freq) 18 | 19 | self._freq = freq 20 | self._amp = amp 21 | self._offset = offset 22 | self._phase = phase 23 | 24 | self.motor_list = [self.get_mockup_motor(m) for m in motor_list] 25 | 26 | def update(self): 27 | """ Compute the sin(t) where t is the elapsed time since the primitive has been started. """ 28 | pos = self._amp * numpy.sin(self._freq * 2.0 * numpy.pi * self.elapsed_time + 29 | self._phase * numpy.pi / 180.0) + self._offset 30 | 31 | for m in self.motor_list: 32 | m.goal_position = pos 33 | 34 | @property 35 | def frequency(self): 36 | return self._freq 37 | 38 | @frequency.setter 39 | def frequency(self, new_freq): 40 | self._freq = new_freq 41 | 42 | @property 43 | def amplitude(self): 44 | return self._amp 45 | 46 | @amplitude.setter 47 | def amplitude(self, new_amp): 48 | self._amp = new_amp 49 | 50 | @property 51 | def offset(self): 52 | return self._offset 53 | 54 | @offset.setter 55 | def offset(self, new_offset): 56 | self._offset = new_offset 57 | 58 | @property 59 | def phase(self): 60 | return self._phase 61 | 62 | @phase.setter 63 | def phase(self, new_phase): 64 | self._phase = new_phase 65 | 66 | 67 | class Cosinus(Sinus): 68 | """ Apply a cosinus on the motor specified as argument. Parameters (amp, offset and phase) should be specified in degree. """ 69 | 70 | def __init__(self, robot, refresh_freq, 71 | motor_list, 72 | amp=1, freq=0.5, offset=0, phase=0): 73 | 74 | Sinus.__init__(self, robot, refresh_freq, 75 | motor_list, 76 | amp, freq, offset, phase=(numpy.pi / 2 + phase)) 77 | 78 | 79 | class PositionWatcher(LoopPrimitive): 80 | def __init__(self, robot, refresh_freq, watched_motors): 81 | LoopPrimitive.__init__(self, robot, refresh_freq) 82 | 83 | self._pos = defaultdict(list) 84 | self.watched_motors = watched_motors 85 | self._duration = 0. 86 | 87 | @property 88 | def record_positions(self): 89 | return deepcopy(self._pos) 90 | 91 | def setup(self): 92 | self._pos.clear() 93 | 94 | def update(self): 95 | for m in self.watched_motors: 96 | self._pos[m.name].append(m.present_position) 97 | 98 | self._duration = self.elapsed_time 99 | 100 | def plot(self, ax): 101 | for m, pos in self._pos.items(): 102 | t = numpy.linspace(0, self._duration, len(pos)) 103 | ax.plot(t, pos) 104 | ax.set_ylabel('position (in deg)') 105 | ax.set_xlabel('time (in s)') 106 | ax.legend(list(self._pos.keys()), loc='best') 107 | 108 | 109 | class SimplePosture(Primitive): 110 | def __init__(self, robot, duration): 111 | Primitive.__init__(self, robot) 112 | 113 | self.duration = duration 114 | 115 | def setup(self): 116 | self._speeds = {m: m.moving_speed for m in self.robot.motors} 117 | 118 | if hasattr(self, 'leds'): 119 | for m, c in self.leds.items(): 120 | m.led = c 121 | 122 | def run(self): 123 | if not hasattr(self, 'target_position'): 124 | raise NotImplementedError('You have to define "target_position" first!') 125 | 126 | for m in self.robot.motors: 127 | m.compliant = False 128 | 129 | self.robot.goto_position(self.target_position, self.duration, wait=True) 130 | 131 | def teardown(self): 132 | for m, s in self._speeds.items(): 133 | m.moving_speed = s 134 | 135 | if hasattr(self, 'leds'): 136 | for m, c in self.leds.items(): 137 | m.led = 'off' 138 | -------------------------------------------------------------------------------- /pypot/robot/__init__.py: -------------------------------------------------------------------------------- 1 | from .robot import Robot 2 | from .config import from_config, from_json, use_dummy_robot 3 | 4 | try: 5 | from .remote import from_remote 6 | except ImportError: 7 | pass 8 | 9 | try: 10 | from ..vrep import from_vrep 11 | except (ImportError, OSError): 12 | pass 13 | -------------------------------------------------------------------------------- /pypot/robot/controller.py: -------------------------------------------------------------------------------- 1 | import time 2 | 3 | from math import copysign 4 | 5 | from ..utils.stoppablethread import StoppableLoopThread 6 | 7 | 8 | class AbstractController(StoppableLoopThread): 9 | """ Abstract class for motor/sensor controller. 10 | 11 | The controller role is to synchronize the reading/writing of a set of instances with their "hardware" equivalent through an :class:`~pypot.robot.io.AbstractIO` object. It is defined as a :class:`~pypot.utils.stoppablethread.StoppableLoopThread` where each loop update synchronizes values from the "software" objects with their "hardware" equivalent. 12 | 13 | To define your Controller, you need to define the :meth:`~pypot.utils.stoppablethread.StoppableLoopThread.update` method. This method will be called at the predefined frequency. An exemple of how to do it can be found in :class:`~pypot.dynamixel.controller.BaseDxlController`. 14 | 15 | """ 16 | def __init__(self, io, sync_freq): 17 | """ 18 | :param io: IO used to communicate with the hardware motors 19 | :type io: :class:`~pypot.robot.io.AbstractIO` 20 | :param float sync_freq: synchronization frequency 21 | 22 | """ 23 | StoppableLoopThread.__init__(self, sync_freq) 24 | 25 | self.io = io 26 | 27 | def start(self): 28 | StoppableLoopThread.start(self) 29 | self.wait_to_start() 30 | 31 | def close(self): 32 | """ Cleans and closes the controller. """ 33 | self.stop() 34 | 35 | if self.io is not None: 36 | self.io.close() 37 | 38 | 39 | class MotorsController(AbstractController): 40 | """ Abstract class for motors controller. 41 | 42 | The controller synchronizes the reading/writing of a set of motor instances with their "hardware". Each update loop synchronizes values from the "software" :class:`~pypot.dynamixel.motor.DxlMotor` with their "hardware" equivalent. 43 | 44 | """ 45 | def __init__(self, io, motors, sync_freq=50): 46 | """ 47 | :param io: IO used to communicate with the hardware motors 48 | :type io: :class:`~pypot.robot.io.AbstractIO` 49 | :param list motors: list of motors attached to the controller 50 | :param float sync_freq: synchronization frequency 51 | 52 | """ 53 | AbstractController.__init__(self, io, sync_freq) 54 | 55 | self.motors = motors 56 | 57 | 58 | class DummyController(MotorsController): 59 | def __init__(self, motors): 60 | MotorsController.__init__(self, None, motors) 61 | 62 | self.max_speed = 360.0 # degree per second. 63 | 64 | def setup(self): 65 | self.last_update = time.time() 66 | 67 | for m in self.motors: 68 | m.__dict__['moving_speed'] = 0.0 69 | m.__dict__['present_position'] = 0.0 70 | m.__dict__['goal_position'] = 0.0 71 | 72 | def update(self): 73 | delta_t = time.time() - self.last_update 74 | 75 | for m in self.motors: 76 | # acceleration infinite, present_speed always equal moving_speed 77 | delta_pos = m.__dict__['goal_position'] - m.__dict__['present_position'] # degree 78 | 79 | # degree par second, assumed absolute 80 | speed = (m.__dict__['moving_speed'] 81 | if m.__dict__['moving_speed'] != 0.0 else 82 | self.max_speed) 83 | 84 | delta_pos_effective = copysign(speed * delta_t, delta_pos) 85 | 86 | if abs(delta_pos_effective) >= abs(delta_pos): 87 | m.__dict__['present_position'] = m.__dict__['goal_position'] 88 | else: 89 | m.__dict__['present_position'] += delta_pos_effective 90 | 91 | self.last_update = time.time() 92 | 93 | 94 | class SensorsController(AbstractController): 95 | """ Abstract class for sensors controller. 96 | 97 | The controller frequently pulls new data from a "real" sensor and updates its corresponding software instance. 98 | 99 | """ 100 | def __init__(self, io, sensors, sync_freq=50.): 101 | """ 102 | :param io: IO used to communicate with the hardware motors 103 | :type io: :class:`~pypot.robot.io.AbstractIO` 104 | :param list sensors: list of sensors attached to the controller 105 | :param float sync_freq: synchronization frequency 106 | 107 | """ 108 | AbstractController.__init__(self, io, sync_freq) 109 | 110 | self.sensors = sensors 111 | -------------------------------------------------------------------------------- /pypot/robot/io.py: -------------------------------------------------------------------------------- 1 | from abc import ABCMeta, abstractmethod 2 | 3 | 4 | class AbstractIO(object, metaclass=ABCMeta): 5 | """ AbstractIO class which handles communication with "hardware" motors. """ 6 | 7 | @abstractmethod 8 | def close(self): 9 | """ Clean and close the IO connection. """ 10 | pass 11 | -------------------------------------------------------------------------------- /pypot/robot/motor.py: -------------------------------------------------------------------------------- 1 | class Motor(object): 2 | """ Purely abstract class representing any motor object. """ 3 | 4 | registers = [] 5 | 6 | def __init__(self, name): 7 | self._name = name 8 | 9 | @property 10 | def name(self): 11 | return self._name 12 | -------------------------------------------------------------------------------- /pypot/robot/remote.py: -------------------------------------------------------------------------------- 1 | import zerorpc 2 | 3 | 4 | class RemoteRobotClient(object): 5 | """ Remote Access to a Robot through the REST API. 6 | 7 | This RemoteRobot gives you access to motors and alias. 8 | For each motor you can read/write all of their registers. 9 | 10 | You also have access to primitives. 11 | More specifically you can start/stop them. 12 | 13 | """ 14 | def __init__(self, host, port): 15 | client = zerorpc.Client() 16 | client.connect('tcp://{}:{}'.format(host, port)) 17 | 18 | self.motors = [] 19 | 20 | for name in client.get_motors_list(): 21 | class Register(object): 22 | def __init__(self, motorname, regname): 23 | self.motorname = motorname 24 | self.regname = regname 25 | 26 | def __get__(self, instance, owner): 27 | return client.get_register_value(self.motorname, self.regname) 28 | 29 | def __set__(self, instance, value): 30 | client.set_register_value(self.motorname, self.regname, value) 31 | 32 | class Motor(object): 33 | def __repr__(self): 34 | return ('').format(self=self) 37 | 38 | for reg in client.get_registers_list(name): 39 | setattr(Motor, reg.decode(), Register(name, reg)) 40 | 41 | m = Motor() 42 | setattr(self, m.name, m) 43 | self.motors.append(m) 44 | 45 | for alias in client.get_motors_alias(): 46 | motors = [getattr(self, name) for name in client.get_motors_list(alias)] 47 | setattr(self, alias, motors) 48 | 49 | class Primitive(object): 50 | def __init__(self, name): 51 | self.name = name 52 | 53 | def start(self): 54 | client.start_primitive(self.name) 55 | 56 | def stop(self): 57 | client.stop_primitive(self.name) 58 | 59 | self.primitives = [] 60 | for p in client.get_primitives_list(): 61 | prim = Primitive(p) 62 | setattr(self, p.decode(), prim) 63 | self.primitives.append(prim) 64 | 65 | 66 | def from_remote(host, port): 67 | """ Remote access to a Robot through the REST API. """ 68 | return RemoteRobotClient(host, port) 69 | -------------------------------------------------------------------------------- /pypot/robot/sensor.py: -------------------------------------------------------------------------------- 1 | from numpy import array, zeros 2 | 3 | 4 | class Sensor(object): 5 | """ Purely abstract class representing any sensor object. """ 6 | registers = [] 7 | 8 | def __init__(self, name): 9 | self._name = name 10 | 11 | @property 12 | def name(self): 13 | return self._name 14 | 15 | 16 | class ObjectTracker(Sensor): 17 | registers = Sensor.registers + ['position', 'orientation'] 18 | 19 | def __init__(self, name): 20 | Sensor.__init__(self, name) 21 | 22 | self._pos = zeros(3) 23 | self._ori = zeros(3) 24 | 25 | @property 26 | def position(self): 27 | return self._pos 28 | 29 | @position.setter 30 | def position(self, new_pos): 31 | self._pos = array(new_pos) 32 | 33 | @property 34 | def orientation(self): 35 | return self._pos 36 | 37 | @orientation.setter 38 | def orientation(self, new_ori): 39 | self._ori = array(new_ori) 40 | -------------------------------------------------------------------------------- /pypot/sensor/__init__.py: -------------------------------------------------------------------------------- 1 | from .depth import * 2 | from .camera import * 3 | from .contact import * 4 | from .imagefeature import * 5 | from .arduino import * 6 | -------------------------------------------------------------------------------- /pypot/sensor/arduino/__init__.py: -------------------------------------------------------------------------------- 1 | try: 2 | from .arduino_sensor import ArduinoSensor 3 | 4 | except ImportError: 5 | pass 6 | -------------------------------------------------------------------------------- /pypot/sensor/arduino/arduino_sensor.py: -------------------------------------------------------------------------------- 1 | import serial 2 | import json 3 | 4 | from ...robot.sensor import Sensor 5 | from ...utils import StoppableLoopThread 6 | 7 | 8 | class ArduinoSensor(Sensor): 9 | """ Give access to arduino sensor. 10 | 11 | Here it is an example of the arduino code to retrieve the time: 12 | 13 | unsigned long time; 14 | void setup() { 15 | Serial.begin(1000000); 16 | } 17 | void loop() { 18 | // prints fixed data in json format 19 | Serial.print("{\"Day\":\"monday\","); 20 | Serial.print("\"Time\":"); 21 | time = millis(); 22 | // prints time since program started 23 | Serial.print(time); 24 | Serial.println("}"); 25 | // wait 20 ms to send the data at 50 Hz 26 | delay(0.02); 27 | } 28 | 29 | Be careful to not set the sync_freq of your controller 30 | slower than the data comes from your arduino (here 50 Hz). 31 | 32 | """ 33 | def __init__(self, name, port, baud, sync_freq=50.0): 34 | Sensor.__init__(self, name) 35 | self.port = port 36 | self.baud = baud 37 | self._controller = StoppableLoopThread(sync_freq, update=self.update) 38 | 39 | def start(self): 40 | self._ser = serial.Serial(self.port, self.baud) 41 | self._line = '' 42 | self._controller.start() 43 | 44 | def close(self): 45 | self._controller.stop() 46 | self._ser.close() 47 | 48 | def update(self): 49 | while self._ser.inWaiting() > 0: 50 | self._line = self._ser.readline().decode() 51 | try: 52 | self.sensor_dict = json.loads(self._line) 53 | except ValueError: 54 | pass 55 | -------------------------------------------------------------------------------- /pypot/sensor/camera/__init__.py: -------------------------------------------------------------------------------- 1 | from ...robot.controller import SensorsController 2 | 3 | from .dummy import DummyCamera 4 | 5 | 6 | try: 7 | from .opencvcam import OpenCVCamera 8 | except ImportError: 9 | pass 10 | -------------------------------------------------------------------------------- /pypot/sensor/camera/abstractcam.py: -------------------------------------------------------------------------------- 1 | import time 2 | 3 | from threading import Thread 4 | 5 | from ...robot.sensor import Sensor 6 | 7 | 8 | class AbstractCamera(Sensor): 9 | registers = Sensor.registers + ['frame', 'resolution', 'fps'] 10 | 11 | def __init__(self, name, resolution, fps): 12 | Sensor.__init__(self, name) 13 | 14 | self._res, self._fps = resolution, fps 15 | self._last_frame = self._grab_and_process() 16 | 17 | self.running = True 18 | self._processing = Thread(target=self._process_loop) 19 | self._processing.daemon = True 20 | self._processing.start() 21 | 22 | @property 23 | def frame(self): 24 | return self._last_frame 25 | 26 | def post_processing(self, image): 27 | return image 28 | 29 | def grab(self): 30 | raise NotImplementedError 31 | 32 | def _grab_and_process(self): 33 | return self.post_processing(self.grab()) 34 | 35 | def _process_loop(self): 36 | period = 1.0 / self.fps 37 | 38 | while self.running: 39 | self._last_frame = self._grab_and_process() 40 | time.sleep(period) 41 | 42 | @property 43 | def resolution(self): 44 | return list(reversed(self.frame.shape[:2])) 45 | 46 | @property 47 | def fps(self): 48 | return self._fps 49 | 50 | def close(self): 51 | self.running = False 52 | self._processing.join() 53 | -------------------------------------------------------------------------------- /pypot/sensor/camera/dummy.py: -------------------------------------------------------------------------------- 1 | import numpy 2 | 3 | from .abstractcam import AbstractCamera 4 | 5 | 6 | class DummyCamera(AbstractCamera): 7 | def __init__(self, name, resolution, fps, **extra): 8 | AbstractCamera.__init__(self, name, resolution, fps) 9 | 10 | def grab(self): 11 | if not hasattr(self, '_frame'): 12 | self._frame = numpy.zeros(list(self._res) + [3], dtype=numpy.uint8) 13 | 14 | return self._frame 15 | -------------------------------------------------------------------------------- /pypot/sensor/camera/opencvcam.py: -------------------------------------------------------------------------------- 1 | import cv2 2 | 3 | from .abstractcam import AbstractCamera 4 | 5 | 6 | class OpenCVCamera(AbstractCamera): 7 | registers = AbstractCamera.registers + ['index'] 8 | 9 | def __init__(self, name, index, fps, resolution=None): 10 | self._index = index 11 | self.capture = cv2.VideoCapture(self.index) 12 | if not self.capture.isOpened(): 13 | raise ValueError('Can not open camera device {}. You should start your robot with argument camera=\'dummy\'. E.g. p = PoppyErgoJr(camera=\'dummy\')'.format(index)) 14 | 15 | if resolution is not None: 16 | self.capture.set(cv2.CAP_PROP_FRAME_WIDTH, resolution[0]) 17 | self.capture.set(cv2.CAP_PROP_FRAME_HEIGHT, resolution[1]) 18 | 19 | AbstractCamera.__init__(self, name, resolution, fps) 20 | 21 | @property 22 | def index(self): 23 | return self._index 24 | 25 | def grab(self): 26 | rval, frame = self.capture.read() 27 | if not rval: 28 | raise EnvironmentError('Can not grab image from the camera!') 29 | 30 | return frame 31 | 32 | def close(self): 33 | AbstractCamera.close(self) 34 | self.capture.release() 35 | -------------------------------------------------------------------------------- /pypot/sensor/contact/__init__.py: -------------------------------------------------------------------------------- 1 | try: 2 | from .contact import ContactSensor 3 | except ImportError: 4 | pass 5 | -------------------------------------------------------------------------------- /pypot/sensor/contact/contact.py: -------------------------------------------------------------------------------- 1 | import RPi.GPIO as GPIO 2 | 3 | from ...robot.sensor import Sensor 4 | 5 | 6 | class ContactSensor(Sensor): 7 | """ Gives access to a micro switch sensor. """ 8 | registers = Sensor.registers + ['contact'] 9 | 10 | def __init__(self, name, gpio_data, gpio_vcc=None): 11 | Sensor.__init__(self, name) 12 | 13 | self._pin = gpio_data 14 | GPIO.setmode(GPIO.BCM) 15 | GPIO.setwarnings(False) 16 | GPIO.setup(self._pin, GPIO.IN) 17 | 18 | if gpio_vcc is not None: 19 | self._vcc = gpio_vcc 20 | GPIO.setup(self._vcc, GPIO.OUT) 21 | GPIO.output(self._vcc, GPIO.HIGH) 22 | 23 | @property 24 | def contact(self): 25 | return GPIO.input(self._pin) != 0 26 | -------------------------------------------------------------------------------- /pypot/sensor/depth/__init__.py: -------------------------------------------------------------------------------- 1 | try: 2 | from .sonar import SonarSensor 3 | 4 | except ImportError: 5 | pass 6 | -------------------------------------------------------------------------------- /pypot/sensor/depth/sonar.py: -------------------------------------------------------------------------------- 1 | 2 | 3 | import time 4 | import numpy 5 | 6 | from collections import deque 7 | 8 | from ...robot.sensor import Sensor 9 | from ...utils import StoppableLoopThread 10 | from ...utils.i2c_controller import I2cController 11 | 12 | 13 | class SonarSensor(Sensor): 14 | """ Give access to ultrasonic I2C modules SRF-02 in a *pypot way* 15 | 16 | It provides one register: distance (in meters). 17 | 18 | """ 19 | registers = Sensor.registers + ['distance'] 20 | 21 | def __init__(self, name, i2c_pin, address, sync_freq=50.0): 22 | Sensor.__init__(self, name) 23 | 24 | self._d = numpy.nan 25 | 26 | self._sonar = Sonar(i2c_pin, [address]) 27 | 28 | self._controller = StoppableLoopThread(sync_freq, update=self.update) 29 | self._controller.start() 30 | 31 | def close(self): 32 | self._controller.stop() 33 | 34 | def update(self): 35 | self._sonar.update() 36 | self.distance = self._sonar.data[0] 37 | 38 | @property 39 | def distance(self): 40 | return self._d 41 | 42 | @distance.setter 43 | def distance(self, d): 44 | self._d = d / 100 45 | 46 | 47 | class Sonar(object): 48 | """ Give access to ultrasonic I2C modules SRF-02 connected with I2C pin of your board. 49 | To get more information, go to http://www.robot-electronics.co.uk/htm/srf02techI2C.htm 50 | 51 | Example: 52 | 53 | > i2c = smbus.SMBus(1) 54 | > sonar = Sonar(i2c, addresses=[0x70, 0x71, 0x72]) 55 | > 56 | """ 57 | 58 | def __init__(self, pin_number, addresses=[0x70]): 59 | """ 0x70 is the default address for the SRF-02 I2C module. """ 60 | 61 | self.i2c = I2cController(pin_number) 62 | self.addresses = addresses 63 | 64 | self.data = None 65 | 66 | self._raw_data_queues = [deque([], 5) for _ in addresses] 67 | 68 | self.results_type = {'inches': 0x50, 69 | 'centimeters': 0x51, 70 | 'microseconds': 0x52} 71 | 72 | self.__errors = 0 73 | 74 | def update(self): 75 | self.ping() 76 | time.sleep(0.065) 77 | self.data = self._filter(self.read()) 78 | return self.data 79 | 80 | def ping(self): 81 | for addr in self.addresses: 82 | self._ping(addr) 83 | 84 | def read(self, reg=2): 85 | return [self._read(addr, reg) for addr in self.addresses] 86 | 87 | def _filter(self, data): 88 | """ Apply a filter to reduce noisy data. 89 | 90 | Return the median value of a heap of data. 91 | 92 | """ 93 | filtered_data = [] 94 | for queue, data in zip(self._raw_data_queues, data): 95 | queue.append(data) 96 | filtered_data.append(numpy.median(queue)) 97 | 98 | return filtered_data 99 | 100 | def _ping(self, address, data=None): 101 | d = data if data is not None else self.results_type['centimeters'] 102 | 103 | while True: 104 | try: 105 | self.i2c.write_byte_data(address, 0, d) 106 | break 107 | except IOError: 108 | time.sleep(0.005) 109 | self.__errors += 1 110 | 111 | def _read(self, address, reg=2): 112 | while True: 113 | try: 114 | return int(self.i2c.read_word_data(address, reg)) / 256 115 | except IOError: 116 | time.sleep(0.005) 117 | self.__errors += 1 118 | 119 | 120 | if __name__ == '__main__': 121 | import smbus 122 | 123 | from pylab import plot 124 | 125 | i2c = smbus.SMBus(1) 126 | sonar = Sonar(i2c) 127 | 128 | d = [] 129 | t = [time.time()] 130 | for _ in range(1000): 131 | sonar.update() 132 | d.append(sonar.data[0]) 133 | t.append(time.time() - t[0]) 134 | plot(t[1:], d) 135 | -------------------------------------------------------------------------------- /pypot/sensor/imagefeature/__init__.py: -------------------------------------------------------------------------------- 1 | try: 2 | from .marker import MarkerDetector 3 | except ImportError: 4 | pass 5 | 6 | try: 7 | from .blob import BlobDetector 8 | except ImportError: 9 | pass 10 | 11 | try: 12 | from .face import FaceDetector 13 | except ImportError: 14 | pass 15 | -------------------------------------------------------------------------------- /pypot/sensor/imagefeature/blob.py: -------------------------------------------------------------------------------- 1 | import cv2 2 | 3 | from numpy import ones, uint8, concatenate 4 | 5 | from ...robot.controller import SensorsController 6 | from ...robot.sensor import Sensor 7 | 8 | 9 | class Blob(Sensor): 10 | registers = Sensor.registers + ['center', 'radius'] 11 | 12 | def __init__(self, x, y, radius): 13 | self.center = x, y 14 | self.radius = radius 15 | 16 | def draw(self, img, color=(255, 0, 0), thickness=3): 17 | cv2.circle(img, self.center, self.radius, color, thickness) 18 | 19 | @property 20 | def json(self): 21 | return {"center": self.center, "radius": self.radius} 22 | 23 | 24 | class BlobDetector(SensorsController): 25 | channels = { 26 | 'R': 2, 'G': 1, 'B': 0, 27 | 'H': 0, 'S': 1, 'V': 2 28 | } 29 | 30 | def __init__(self, robot, name, cameras, freq, filters): 31 | SensorsController.__init__(self, None, [], freq) 32 | 33 | self.name = name 34 | 35 | self._robot = robot 36 | self._names = cameras 37 | self._blobs = [] 38 | self.filters = filters 39 | 40 | def detect_blob(self, img, filters): 41 | """ 42 | "filters" must be something similar to: 43 | filters = { 44 | 'R': (150, 255), # (min, max) 45 | 'S': (150, 255), 46 | } 47 | 48 | """ 49 | acc_mask = ones(img.shape[:2], dtype=uint8) * 255 50 | 51 | rgb = img.copy() 52 | hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV) 53 | 54 | for c, (min, max) in filters.items(): 55 | img = rgb if c in 'RGB' else hsv 56 | 57 | mask = img[:, :, self.channels[c]] 58 | mask[mask < min] = 0 59 | mask[mask > max] = 0 60 | 61 | acc_mask &= mask 62 | 63 | kernel = ones((5, 5), uint8) 64 | acc_mask = cv2.dilate(cv2.erode(acc_mask, kernel), kernel) 65 | 66 | circles = cv2.HoughCircles(acc_mask, cv2.HOUGH_GRADIENT, 3, img.shape[0] / 5.) 67 | return circles.reshape(-1, 3) if circles is not None else [] 68 | 69 | def update(self): 70 | if not hasattr(self, 'cameras'): 71 | self.cameras = [getattr(self._robot, c) for c in self._names] 72 | 73 | self._blobs = concatenate([self.detect_blob(c.frame, self.filters) 74 | for c in self.cameras]) 75 | 76 | @property 77 | def blobs(self): 78 | return [Blob(*b) for b in self._blobs] 79 | 80 | @property 81 | def registers(self): 82 | return ['blobs'] 83 | -------------------------------------------------------------------------------- /pypot/sensor/imagefeature/face.py: -------------------------------------------------------------------------------- 1 | import cv2 2 | 3 | from numpy import mean, concatenate 4 | 5 | from ...robot.controller import SensorsController 6 | from ...robot.sensor import Sensor 7 | 8 | 9 | class Face(Sensor): 10 | registers = Sensor.registers + ['center', 'rect'] 11 | 12 | def __init__(self, rect): 13 | x, y, w, h = rect 14 | self.center = mean([[x, x + w], [y, y + h]], axis=1) 15 | self.rect = rect 16 | 17 | def draw(self, img, color=(255, 0, 0), thickness=3): 18 | x, y, w, h = self.rect 19 | cv2.rectangle(img, (x, y), (x + w, y + h), color, thickness) 20 | 21 | @property 22 | def json(self): 23 | return {"center": self.center, "rect": self.rect} 24 | 25 | 26 | class FaceDetector(SensorsController): 27 | def __init__(self, robot, name, cameras, freq, 28 | cascade='/home/coyote/dev/opencv-3.0.0/data/haarcascades/haarcascade_frontalface_alt.xml'): 29 | SensorsController.__init__(self, None, [], freq) 30 | 31 | self.name = name 32 | 33 | self._robot = robot 34 | self._names = cameras 35 | self._faces = [] 36 | 37 | self.cascade = cv2.CascadeClassifier(cascade) 38 | 39 | def detect_face(self, img): 40 | rects = self.cascade.detectMultiScale(img, scaleFactor=1.3, 41 | minNeighbors=4, 42 | minSize=(20, 20)) 43 | return rects 44 | 45 | def update(self): 46 | if not hasattr(self, 'cameras'): 47 | self.cameras = [getattr(self._robot, c) for c in self._names] 48 | 49 | self._faces = concatenate([self.detect_face(c.frame) for c in self.cameras]) 50 | 51 | @property 52 | def faces(self): 53 | return [Face(f) for f in self._faces] 54 | 55 | @property 56 | def registers(self): 57 | return ['faces'] 58 | -------------------------------------------------------------------------------- /pypot/sensor/imagefeature/marker.py: -------------------------------------------------------------------------------- 1 | from multiprocessing import Process, Queue 2 | 3 | from hampy import detect_markers 4 | 5 | from ...robot.controller import SensorsController 6 | from ...robot.sensor import Sensor 7 | 8 | 9 | class Marker(Sensor): 10 | registers = Sensor.registers + ['position', 'id'] 11 | 12 | def __init__(self, marker): 13 | Sensor.__init__(self, 'marker_{}'.format(marker.id)) 14 | 15 | self.position = marker.normalized_center 16 | self.id = marker.id 17 | 18 | self._marker = marker 19 | 20 | def __getattr__(self, attr): 21 | return getattr(self._marker, attr) 22 | 23 | @property 24 | def json(self): 25 | return {"id": self.id, "position": self.position} 26 | 27 | 28 | class MarkerDetector(SensorsController): 29 | def __init__(self, robot, name, cameras, freq, multiprocess=True): 30 | SensorsController.__init__(self, None, [], freq) 31 | 32 | self.name = name 33 | 34 | self._robot = robot 35 | self._names = cameras 36 | 37 | self.detect = (lambda img: self._bg_detection(img) 38 | if multiprocess else list(detect_markers(img))) 39 | 40 | def update(self): 41 | if not hasattr(self, 'cameras'): 42 | self.cameras = [getattr(self._robot, c) for c in self._names] 43 | 44 | self._markers = sum([self.detect(c.frame) for c in self.cameras], []) 45 | self.sensors = [Marker(m) for m in self._markers] 46 | 47 | @property 48 | def markers(self): 49 | return self.sensors 50 | 51 | @property 52 | def registers(self): 53 | return ['markers'] 54 | 55 | def _detect(self, q, img): 56 | q.put(list(detect_markers(img))) 57 | 58 | def _bg_detection(self, img): 59 | if not hasattr(self, 'q'): 60 | self.q = Queue() 61 | 62 | p = Process(target=self._detect, args=(self.q, img)) 63 | p.start() 64 | return self.q.get() 65 | -------------------------------------------------------------------------------- /pypot/sensor/kinect/__init__.py: -------------------------------------------------------------------------------- 1 | try: 2 | from .sensor import KinectSensor 3 | except ImportError: 4 | pass 5 | -------------------------------------------------------------------------------- /pypot/sensor/kinect/sensor.py: -------------------------------------------------------------------------------- 1 | """ 2 | This code has been developed by Baptiste Busch: https://github.com/buschbapti 3 | 4 | This module allows you to retrieve Skeleton information from a Kinect device. 5 | It is only the client side of a zmq client/server application. 6 | 7 | The server part can be found at: https://bitbucket.org/buschbapti/kinectserver/src 8 | It used the Microsoft Kinect SDK and thus only work on Windows. 9 | 10 | Of course, the client side can be used on any platform. 11 | 12 | """ 13 | 14 | import zmq 15 | import numpy 16 | import threading 17 | 18 | from collections import namedtuple 19 | 20 | from ...utils import Point3D, Point2D, Quaternion 21 | 22 | torso_joints = ('hip_center', 'spine', 'shoulder_center', 'head') 23 | left_arm_joints = ('shoulder_left', 'elbow_left', 'wrist_left', 'hand_left') 24 | right_arm_joints = ('shoulder_right', 'elbow_right', 'wrist_right', 'hand_right') 25 | left_leg_joints = ('hip_left', 'knee_left', 'ankle_left', 'foot_left') 26 | right_leg_joints = ('hip_right', 'knee_right', 'ankle_right', 'foot_right') 27 | skeleton_joints = torso_joints + left_arm_joints + right_arm_joints + left_leg_joints + right_leg_joints 28 | 29 | 30 | class Skeleton(namedtuple('Skeleton', ('timestamp', 'user_id') + skeleton_joints)): 31 | joints = skeleton_joints 32 | 33 | 34 | Joint = namedtuple('Joint', ('position', 'orientation', 'pixel_coordinate')) 35 | 36 | 37 | class KinectSensor(object): 38 | def __init__(self, addr, port): 39 | self._lock = threading.Lock() 40 | self._skeleton = {} 41 | 42 | self.context = zmq.Context() 43 | self.sub_skel = self.context.socket(zmq.SUB) 44 | self.sub_skel.connect('tcp://{}:{}'.format(addr, port)) 45 | self.sub_skel.setsockopt(zmq.SUBSCRIBE, '') 46 | 47 | t = threading.Thread(target=self.get_skeleton) 48 | t.daemon = True 49 | t.start() 50 | 51 | def remove_user(self, user_index): 52 | with self._lock: 53 | del self._skeleton[user_index] 54 | 55 | def remove_all_users(self): 56 | with self._lock: 57 | self._skeleton = {} 58 | 59 | @property 60 | def tracked_skeleton(self): 61 | with self._lock: 62 | return self._skeleton 63 | 64 | @tracked_skeleton.setter 65 | def tracked_skeleton(self, skeleton): 66 | with self._lock: 67 | self._skeleton[skeleton.user_id] = skeleton 68 | 69 | def get_skeleton(self): 70 | while True: 71 | md = self.sub_skel.recv_json() 72 | msg = self.sub_skel.recv() 73 | skel_array = numpy.fromstring(msg, dtype=float, sep=",") 74 | skel_array = skel_array.reshape(md['shape']) 75 | 76 | nb_joints = md['shape'][0] 77 | joints = [] 78 | for i in range(nb_joints): 79 | x, y, z, w = skel_array[i][0:4] 80 | position = Point3D(x / w, y / w, z / w) 81 | pixel_coord = Point2D(*skel_array[i][4:6]) 82 | orientation = Quaternion(*skel_array[i][6:10]) 83 | joints.append(Joint(position, orientation, pixel_coord)) 84 | 85 | self.tracked_skeleton = Skeleton(md['timestamp'], md['user_index'], *joints) 86 | 87 | def run(self): 88 | cv2.startWindowThread() 89 | while True: 90 | img = numpy.zeros((480, 640, 3)) 91 | skeleton = kinect.tracked_skeleton 92 | if skeleton: 93 | for user, skel in skeleton.items(): 94 | for joint_name in skel.joints: 95 | x, y = getattr(skel, joint_name).pixel_coordinate 96 | pt = (int(x), int(y)) 97 | cv2.circle(img, pt, 5, (255, 255, 255), thickness=-1) 98 | kinect.remove_all_users() 99 | cv2.imshow('Skeleton', img) 100 | cv2.waitKey(50) 101 | 102 | self.sub_skel.close() 103 | self.context.term() 104 | 105 | 106 | if __name__ == '__main__': 107 | import cv2 108 | 109 | kinect = KinectSensor('193.50.110.177', 9999) 110 | kinect.run() 111 | -------------------------------------------------------------------------------- /pypot/sensor/optibridge.py: -------------------------------------------------------------------------------- 1 | import zmq 2 | import time 3 | import pickle 4 | import threading 5 | 6 | from . import optitrack 7 | 8 | 9 | class OptiBridgeServer(threading.Thread): 10 | def __init__(self, bridge_host, bridge_port, 11 | opti_addr, opti_port, obj_name): 12 | threading.Thread.__init__(self) 13 | self.daemon = True 14 | 15 | c = zmq.Context() 16 | self.s = c.socket(zmq.PUB) 17 | self.s.bind('tcp://{}:{}'.format(bridge_host, bridge_port)) 18 | 19 | self.optitrack = optitrack.OptiTrackClient(opti_addr, opti_port, obj_name) 20 | self.optitrack.start() 21 | 22 | self.obj_name = obj_name 23 | 24 | def run(self): 25 | while True: 26 | self.s.send(pickle.dumps(self.optitrack.recent_tracked_objects)) 27 | time.sleep(0.02) 28 | 29 | 30 | class OptiTrackClient(threading.Thread): 31 | def __init__(self, bridge_host, bridge_port, obj_name): 32 | threading.Thread.__init__(self) 33 | self.daemon = True 34 | 35 | c = zmq.Context() 36 | self.s = c.socket(zmq.SUB) 37 | self.s.connect('tcp://{}:{}'.format(bridge_host, bridge_port)) 38 | self.s.setsockopt(zmq.SUBSCRIBE, '') 39 | 40 | self.obj_name = obj_name 41 | self._tracked_obj = {} 42 | 43 | def run(self): 44 | while True: 45 | d = pickle.loads(self.s.recv()) 46 | self._tracked_obj = { 47 | k: d[k] 48 | for k in [k for k in list(d.keys()) if k in self.obj_name] 49 | } 50 | 51 | @property 52 | def tracked_objects(self): 53 | return self._tracked_obj 54 | 55 | @property 56 | def recent_tracked_objects(self): 57 | return self.tracked_objects 58 | -------------------------------------------------------------------------------- /pypot/sensor/optitrack.py: -------------------------------------------------------------------------------- 1 | import time 2 | import numpy 3 | import datetime 4 | import threading 5 | 6 | from collections import namedtuple 7 | 8 | 9 | TrackedObject = namedtuple('TrackedObject', ('position', 'quaternion', 'orientation', 'timestamp')) 10 | 11 | 12 | def quat2euler(q): 13 | qx, qy, qz, qw = q 14 | sqx, sqy, sqz, sqw = q ** 2 15 | invs = 1.0 / (sqx + sqy + sqz + sqw) 16 | 17 | yaw = numpy.arctan2(2.0 * (qx * qz + qy * qw) * invs, (sqx - sqy - sqz + sqw) * invs) 18 | pitch = -numpy.arcsin(2.0 * (qx * qy - qz * qw) * invs) 19 | roll = numpy.arctan2(2.0 * (qy * qz + qx * qw) * invs, (-sqx + sqy - sqz + sqw) * invs) 20 | 21 | return numpy.array((yaw, pitch, roll)) 22 | 23 | 24 | try: 25 | import vrpn 26 | 27 | class OptiTrackClient(threading.Thread): 28 | """ Retrieves position, orientation, and timestamp of each tracked object. 29 | 30 | The position is expressed in meters (X is left, Y is up, and Z is depth). 31 | The orientation is expressed in radians (yaw, pitch, roll). 32 | 33 | """ 34 | def __init__(self, addr, port, obj_names): 35 | threading.Thread.__init__(self) 36 | self.daemon = True 37 | 38 | self.trackers = [] 39 | for obj in obj_names: 40 | t = vrpn.receiver.Tracker('{}@{}:{}'.format(obj, addr, port)) 41 | t.register_change_handler(obj, self.handler, 'position') 42 | self.trackers.append(t) 43 | 44 | self._tracked_objects = {} 45 | 46 | @property 47 | def tracked_objects(self): 48 | return self._tracked_objects 49 | 50 | @property 51 | def recent_tracked_objects(self): 52 | """ Only returns the objects that have been tracked less than 20ms ago. """ 53 | dt = 0.02 54 | f = lambda name: (datetime.datetime.now() - self.tracked_objects[name].timestamp).total_seconds() 55 | return dict([(k, v) for k, v in self.tracked_objects.items() if f(k) < dt]) 56 | 57 | def handler(self, obj, data): 58 | self.tracked_objects[obj] = TrackedObject(numpy.array(*data['position']), 59 | numpy.array(data['quaternion']), 60 | quat2euler(numpy.array(data['quaternion'])), 61 | datetime.datetime.now()) 62 | 63 | def serve_forever(self): 64 | self.start() 65 | 66 | while True: 67 | try: 68 | self.join(timeout=1.0) 69 | except KeyboardInterrupt: 70 | break 71 | 72 | def run(self): 73 | while True: 74 | for t in self.trackers: 75 | t.mainloop() 76 | time.sleep(1.0 / 120) 77 | 78 | except ImportError: 79 | pass 80 | 81 | if __name__ == '__main__': 82 | c = OptiTrackClient('193.50.110.176', 3883, ('obj_1', )) 83 | c.serve_forever() 84 | -------------------------------------------------------------------------------- /pypot/server/__init__.py: -------------------------------------------------------------------------------- 1 | try: 2 | from .httpserver import HTTPRobotServer 3 | except ImportError: 4 | pass 5 | 6 | try: 7 | from .zmqserver import ZMQRobotServer 8 | except ImportError: 9 | pass 10 | 11 | try: 12 | from .server import RemoteRobotServer 13 | except ImportError: 14 | pass 15 | 16 | try: 17 | from .ws import WsRobotServer 18 | except ImportError: 19 | pass 20 | -------------------------------------------------------------------------------- /pypot/server/server.py: -------------------------------------------------------------------------------- 1 | from .rest import RESTRobot 2 | 3 | import logging 4 | 5 | logger = logging.getLogger(__name__) 6 | 7 | 8 | class AbstractServer(object): 9 | def __init__(self, robot, host, port): 10 | self.restful_robot = RESTRobot(robot) 11 | self.host, self.port = host, port 12 | 13 | def run(self): 14 | raise NotImplementedError 15 | 16 | 17 | class RemoteRobotServer(AbstractServer): 18 | def run(self): 19 | try: 20 | import zerorpc 21 | server = zerorpc.Server(self.restful_robot) 22 | server.bind('tcp://{}:{}'.format(self.host, self.port)) 23 | server.run() 24 | except ImportError: 25 | logger.warning(("Warning: The Python module 'zerorpc' is not installed. " 26 | "Therefore the feature RemoteRobotServer is disabled. " 27 | "On most systems this module can be installed with the command 'pip install zerorpc'.")) 28 | -------------------------------------------------------------------------------- /pypot/server/snap_projects/legacy_projects/poppy-ergo-jr-lib.xml: -------------------------------------------------------------------------------- 1 |
5
caribou 2 | tetris 3 | lapinFor internal use only
 -------------------------------------------------------------------------------- /pypot/server/ws.py: -------------------------------------------------------------------------------- 1 | 2 | 3 | import time 4 | import json 5 | 6 | from threading import Timer 7 | 8 | from tornado.ioloop import IOLoop 9 | from tornado.websocket import WebSocketHandler 10 | from tornado.web import Application 11 | 12 | from .server import AbstractServer 13 | 14 | 15 | class RepeatedTimer(object): 16 | def __init__(self, interval, function, *args, **kwargs): 17 | self._timer = None 18 | self.interval = interval 19 | self.function = function 20 | self.args = args 21 | self.kwargs = kwargs 22 | self.is_running = False 23 | self.start() 24 | 25 | def _run(self): 26 | self.is_running = False 27 | self.function(*self.args, **self.kwargs) 28 | self.start() 29 | 30 | def start(self): 31 | if not self.is_running: 32 | self._timer = Timer(self.interval, self._run) 33 | self._timer.start() 34 | self.is_running = True 35 | 36 | def stop(self): 37 | self._timer.cancel() 38 | self.is_running = False 39 | 40 | 41 | class WsSocketHandler(WebSocketHandler): 42 | time_step = 1 / 30 43 | 44 | def check_origin(self, origin): 45 | return True 46 | 47 | def open(self): 48 | if not self.quiet: 49 | print('WebSocket connection open.') 50 | 51 | self.rt = RepeatedTimer(self.time_step, self.publish_robot_state) 52 | 53 | def on_close(self): 54 | if not self.quiet: 55 | print('WebSocket connection closed: {0}'.format(self.close_reason)) 56 | self.rt.stop() 57 | 58 | def on_message(self, message): 59 | if not self.quiet: 60 | print('{}: Received {}'.format(time.time(), message)) 61 | 62 | self.handle_command(json.loads(message)) 63 | 64 | def publish_robot_state(self): 65 | state = { 66 | m.name: { 67 | 'present_position': m.present_position, 68 | 'present_speed': m.present_speed, 69 | 'present_load': m.present_load, 70 | 'led': m.led, 71 | 'present_temperature': m.present_temperature, 72 | } 73 | for m in self.robot.motors 74 | } 75 | self.write_message(json.dumps(state)) 76 | 77 | def handle_command(self, command): 78 | for motor, values in command.items(): 79 | m = getattr(self.robot, motor) 80 | 81 | for register, value in values.items(): 82 | setattr(m, register, value) 83 | 84 | 85 | class WsRobotServer(AbstractServer): 86 | def __init__(self, robot, host='0.0.0.0', port='9009', quiet=True): 87 | AbstractServer.__init__(self, robot, host, port) 88 | WsSocketHandler.robot = robot 89 | WsSocketHandler.quiet = quiet 90 | 91 | def run(self): 92 | loop = IOLoop() 93 | app = Application([ 94 | (r'/', WsSocketHandler) 95 | ]) 96 | app.listen(self.port) 97 | loop.start() 98 | -------------------------------------------------------------------------------- /pypot/server/zmqserver.py: -------------------------------------------------------------------------------- 1 | import zmq 2 | import json 3 | import logging 4 | 5 | from .server import AbstractServer 6 | 7 | 8 | logger = logging.getLogger(__name__) 9 | 10 | 11 | class ZMQRobotServer(AbstractServer): 12 | def __init__(self, robot, host, port): 13 | """ A ZMQServer allowing remote access of a robot instance. 14 | 15 | The server used the REQ/REP zmq pattern. You should always first send a request and then read the answer. 16 | 17 | """ 18 | AbstractServer.__init__(self, robot, host, port) 19 | 20 | c = zmq.Context() 21 | self.socket = c.socket(zmq.REP) 22 | self.socket.bind('tcp://{}:{}'.format(self.host, self.port)) 23 | 24 | logger.info('Starting ZMQServer on tcp://%s:%s', self.host, self.port) 25 | 26 | def run(self): 27 | """ Run an infinite REQ/REP loop. """ 28 | while True: 29 | req = self.socket.recv_json() 30 | 31 | try: 32 | answer = self.handle_request(req) 33 | self.socket.send(json.dumps(answer)) 34 | 35 | except (AttributeError, TypeError) as e: 36 | self.socket.send_json({'error': str(e)}) 37 | 38 | def handle_request(self, request): 39 | meth_name, kwargs = request['robot'].popitem() 40 | meth = getattr(self.restful_robot, meth_name) 41 | 42 | for key in ('value', 'args'): 43 | if key in kwargs: 44 | kwargs[key] = json.loads(kwargs[key]) 45 | 46 | ret = meth(**kwargs) 47 | ret = {} if ret is None else ret 48 | 49 | return ret 50 | -------------------------------------------------------------------------------- /pypot/tools/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/poppy-project/pypot/13263953a7970209efbcb476bd07b5049151f52e/pypot/tools/__init__.py -------------------------------------------------------------------------------- /pypot/tools/dxlconfig.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | 4 | 5 | """ 6 | 7 | Dynamixel Motor configuration. 8 | 9 | First, it runs a factory reset. And then apply the configuration given as parameter. 10 | 11 | *** WARNING: Make sure only motor is connected to the bus! Otherwise they will ALL be reset to their factory settings. *** 12 | 13 | Examples: 14 | dxlconfig --type=MX-28 --id=23 --angle-limit=(-100, 100) --zeroposition --port=/dev/ttyAMA0 15 | dxlconfig --help 16 | 17 | """ 18 | 19 | import sys 20 | import time 21 | 22 | from argparse import ArgumentParser, ArgumentDefaultsHelpFormatter 23 | 24 | from pypot.dynamixel.conversion import dynamixelModels 25 | from pypot.dynamixel import DxlIO, Dxl320IO, get_available_ports 26 | from pypot.utils import flushed_print as print 27 | 28 | 29 | def check(pred, msg): 30 | if not pred: 31 | print(msg) 32 | print('Exiting now...') 33 | sys.exit(1) 34 | 35 | 36 | def main(): 37 | available_ports = get_available_ports() 38 | default_port = available_ports[0] if available_ports else None 39 | parser = ArgumentParser(description='Configuration tool for dynamixel motors ' 40 | 'WARNING: ONLY ONE MOTOR SHOULD BE ' 41 | 'CONNECTED TO THE BUS WHEN CONFIGURING!', 42 | formatter_class=ArgumentDefaultsHelpFormatter) 43 | 44 | parser.add_argument('--id', type=int, required=True, 45 | help='Chosen motor id.') 46 | parser.add_argument('--type', type=str, required=True, 47 | choices=list(dynamixelModels.values()), 48 | help='Type of the motor to configure.') 49 | parser.add_argument('--port', type=str, 50 | choices=available_ports + ['auto'], default=default_port, 51 | help='Serial port connected to the motor.') 52 | parser.add_argument('--return-delay-time', type=int, 53 | help='Set new return delay time.') 54 | parser.add_argument('--wheel-mode', type=bool, default=False, 55 | help='Set wheel mode.') 56 | parser.add_argument('--angle-limit', type=float, nargs=2, 57 | help='Set new angle limit.') 58 | parser.add_argument('--goto-zero', action='store_true', 59 | help='Go to zero position after configuring the motor') 60 | parser.add_argument('--timeout', type=int, default=2, 61 | help='Timeout for the motor config (seconds)') 62 | args = parser.parse_args() 63 | 64 | check(1 <= args.id <= 253, 65 | 'Motor id must be in range [1:253]') 66 | 67 | check(available_ports, 68 | 'Could not find an available serial port!') 69 | 70 | protocol = 2 if args.type in 'XL-320' else 1 71 | serial_port = default_port if args.port == "auto" else args.port 72 | DxlIOPort = DxlIO if protocol == 1 else Dxl320IO 73 | 74 | # Factory Reset 75 | print('Factory reset, you must see the motor LED flickering 4 times') 76 | if protocol == 1: 77 | print("Using protocol 1...") 78 | for br in [57600, 1000000]: 79 | with DxlIO(serial_port, baudrate=br, timeout=0.01) as io: 80 | io.factory_reset() 81 | else: 82 | print("Using protocol 2...") 83 | with Dxl320IO(serial_port, baudrate=1000000, timeout=0.01) as io: 84 | io.factory_reset(ids=list(range(253))) 85 | print('Done!') 86 | 87 | factory_baudrate = 57600 if args.type.startswith('MX') else 1000000 88 | 89 | # Wait for the motor to "reboot..." 90 | for _ in range(10): 91 | with DxlIOPort(serial_port, baudrate=factory_baudrate, timeout=args.timeout) as io: 92 | if io.ping(1): 93 | break 94 | 95 | time.sleep(.5) 96 | else: 97 | print('Could not communicate with the motor...') 98 | print('Make sure one (and only one) is connected and try again') 99 | print('If the issue persists, use Dynamixel wizard to attempt a firmware recovery') 100 | sys.exit(1) 101 | 102 | # Switch to 1M bauds 103 | if args.type.startswith('MX') or args.type.startswith('SR'): 104 | print('Changing to 1M bauds...') 105 | with DxlIO(serial_port, baudrate=factory_baudrate, timeout=args.timeout) as io: 106 | io.change_baudrate({1: 1000000}) 107 | 108 | time.sleep(.5) 109 | print('Done!') 110 | 111 | # Change id 112 | print('Changing id to {}...'.format(args.id)) 113 | if args.id != 1: 114 | with DxlIOPort(serial_port, timeout=args.timeout) as io: 115 | io.change_id({1: args.id}) 116 | 117 | time.sleep(.5) 118 | check(io.ping(args.id), 119 | 'Could not change id to {}'.format(args.id)) 120 | print('Done!') 121 | 122 | # Set return delay time 123 | if args.return_delay_time is not None: 124 | print('Changing return delay time to {}...'.format(args.return_delay_time)) 125 | with DxlIOPort(serial_port, timeout=args.timeout) as io: 126 | io.set_return_delay_time({args.id: args.return_delay_time}) 127 | 128 | time.sleep(.5) 129 | check(io.get_return_delay_time([args.id])[0] == args.return_delay_time, 130 | 'Could not set return delay time to {}'.format(args.return_delay_time)) 131 | print('Done!') 132 | 133 | # Set wheel Mode 134 | if args.wheel_mode == True: 135 | print('Set wheel mode') 136 | with DxlIOPort(serial_port, timeout=args.timeout) as io: 137 | io.set_control_mode({args.id :'wheel'}) 138 | 139 | time.sleep(.5) 140 | check(io.get_control_mode([args.id])[0] == 'wheel', 141 | 'Could not set wheel Mode') 142 | print('Done!') 143 | 144 | 145 | # Set Angle Limit 146 | if args.angle_limit is not None: 147 | print('Changing angle limit to {}...'.format(args.angle_limit)) 148 | with DxlIOPort(serial_port, timeout=args.timeout) as io: 149 | io.set_angle_limit({args.id: args.angle_limit}) 150 | 151 | time.sleep(.5) 152 | check(all(map(lambda p1, p2: abs(p1 - p2) < 1., 153 | io.get_angle_limit([args.id])[0], 154 | args.angle_limit)), 155 | 'Could not change angle limit to {}'.format(args.angle_limit)) 156 | print('Done!') 157 | 158 | # GOTO ZERO 159 | if args.goto_zero: 160 | print('Going to position 0...') 161 | with DxlIOPort(serial_port, timeout=args.timeout) as io: 162 | io.set_moving_speed({args.id: 100.0}) 163 | io.set_goal_position({args.id: 0.0}) 164 | 165 | time.sleep(2.0) 166 | check(abs(io.get_present_position([args.id])[0]) < 5, 167 | 'Could not go to 0 position') 168 | 169 | print('Done!') 170 | 171 | 172 | if __name__ == '__main__': 173 | main() 174 | -------------------------------------------------------------------------------- /pypot/utils/__init__.py: -------------------------------------------------------------------------------- 1 | from collections import namedtuple 2 | from operator import attrgetter 3 | from threading import Event 4 | 5 | from .stoppablethread import StoppableThread, StoppableLoopThread 6 | from .pypot_time import time 7 | from .flushed_print import flushed_print 8 | 9 | Point2D = namedtuple('Point2D', ('x', 'y')) 10 | Point3D = namedtuple('Point3D', ('x', 'y', 'z')) 11 | Point = Point3D 12 | 13 | Vector3D = namedtuple('Vector3D', ('x', 'y', 'z')) 14 | Vector = Vector3D 15 | 16 | Quaternion = namedtuple('Quaternion', ('x', 'y', 'z', 'w')) 17 | 18 | 19 | def attrsetter(item): 20 | def resolve_attr(obj, attr): 21 | if not attr: 22 | return obj 23 | for name in attr.split('.'): 24 | obj = getattr(obj, name) 25 | return obj 26 | 27 | def g(obj, value): 28 | var_path, _, var_name = item.rpartition('.') 29 | setattr(resolve_attr(obj, var_path), var_name, value) 30 | 31 | return g 32 | 33 | 34 | class SyncEvent(object): 35 | def __init__(self, period=.1): 36 | self._event = Event() 37 | self._needed = False 38 | 39 | self._last_sync = 0. 40 | self.period = period 41 | 42 | def request(self): 43 | self._needed = True 44 | self._event.wait() 45 | self._event.clear() 46 | 47 | def done(self): 48 | self._event.set() 49 | self._last_sync = time() 50 | self._needed = False 51 | 52 | @property 53 | def is_recent(self): 54 | return (time() - self._last_sync) < self.period 55 | 56 | @property 57 | def needed(self): 58 | return self._needed and not self.is_recent 59 | -------------------------------------------------------------------------------- /pypot/utils/flushed_print.py: -------------------------------------------------------------------------------- 1 | 2 | import sys 3 | 4 | 5 | def flushed_print(*args, **kwargs): 6 | """ 7 | Use to replace print(*args, flush=True) that doesn't exist for python<3.3 8 | """ 9 | print(*args, **kwargs) 10 | file = kwargs.get('file', sys.stdout) 11 | file.flush() if file is not None else sys.stdout.flush() 12 | -------------------------------------------------------------------------------- /pypot/utils/i2c_controller.py: -------------------------------------------------------------------------------- 1 | import smbus 2 | import threading 3 | 4 | 5 | class I2cController(object): 6 | used_bus = {} 7 | 8 | def __init__(self, pin_number): 9 | if pin_number not in I2cController.used_bus: 10 | I2cController.used_bus[pin_number] = smbus.SMBus(pin_number) 11 | 12 | self.bus = I2cController.used_bus[pin_number] 13 | self.lock = threading.Lock() 14 | 15 | def read_byte_data(self, addr, cmd): 16 | with self.lock: 17 | return self.bus.read_byte_data(addr, cmd) 18 | 19 | def write_byte_data(self, addr, cmd, val): 20 | with self.lock: 21 | return self.bus.write_byte_data(addr, cmd, val) 22 | 23 | def read_word_data(self, addr, cmd): 24 | with self.lock: 25 | return self.bus.read_word_data(addr, cmd) 26 | 27 | def write_word_data(self, addr, cmd, val): 28 | with self.lock: 29 | return self.bus.write_word_data(addr, cmd, val) 30 | -------------------------------------------------------------------------------- /pypot/utils/interpolation.py: -------------------------------------------------------------------------------- 1 | from scipy.spatial import cKDTree 2 | from scipy.interpolate import interp1d 3 | import numpy as np 4 | 5 | 6 | class KDTreeDict(dict): 7 | 8 | def __init__(self, gen_tree_on_add=False, distance_upper_bound=0.2, k_neighbors=2): 9 | super(KDTreeDict, self).__init__() 10 | self.gen_tree_on_add = gen_tree_on_add 11 | self.distance_upper_bound = distance_upper_bound 12 | self.k_neighbors = k_neighbors 13 | self.__keys = [] 14 | self.__tree = None 15 | self.__stale = False 16 | 17 | def __setitem__(self, key, val): 18 | 19 | if not isinstance(key, tuple): 20 | _key = (key,) 21 | if _key not in self.__keys: 22 | self.__keys.append(_key) 23 | self.__stale = True 24 | if self.gen_tree_on_add: 25 | self.generate_tree() 26 | super(KDTreeDict, self).__setitem__(key, val) 27 | 28 | def __getitem__(self, key): 29 | # if not isinstance(key, tuple): 30 | # key = (key,) 31 | return super(KDTreeDict, self).__getitem__(key) 32 | 33 | 34 | def __len__(self): 35 | return len(self.__keys) 36 | 37 | def update(self, *args, **kwargs): 38 | if len(args) > 1: 39 | raise TypeError("update expected at most 1 arguments, got %d" % len(args)) 40 | other = dict(*args, **kwargs) 41 | for key in other: 42 | self[key] = other[key] 43 | 44 | def generate_tree(self): 45 | self.__tree = cKDTree(self.__keys) 46 | self.__stale = False 47 | 48 | def nearest_keys(self, key): 49 | """Find the nearest_keys (l2 distance) as strings thanks to a cKDTree query""" 50 | if not isinstance(key, tuple): 51 | _key = (key,) 52 | if self.__stale: 53 | self.generate_tree() 54 | d, idx = self.__tree.query( 55 | _key, self.k_neighbors, distance_upper_bound=self.distance_upper_bound) 56 | 57 | try: 58 | return [self.__keys[id][0] for id in idx if id < len(self.__keys)] 59 | except TypeError: 60 | # if k_neighbors = 1 query is not returnng arrays 61 | return self.__keys[idx] 62 | 63 | def interpolate_motor_positions(self, input_key, nearest_keys): 64 | """ 65 | Process linear interpolation to estimate current speed and position of motors 66 | Method specific to the :meth:~pypot.primitive.move.Move.position() structure 67 | it is a KDTreeDict[timestamp] = {dict[motor]=(position,speed)} 68 | 69 | :param input_key: string of a float as returned by cKDTree.query() 70 | :param nearest_keys: 2-tuples of strings of a float as returned by cKDTree.query() 71 | """ 72 | 73 | # WARNING: input_key and nearest_keys are string encoding floats as returned by cKDTree 74 | # TODO: to be rewrited with more style (map ?) 75 | 76 | if len(nearest_keys) == 1: 77 | return self[nearest_keys[0]] 78 | elif len(nearest_keys) == 0: 79 | raise KeyError('key {} exceed distance_upper_bound {}'.format( 80 | input_key, self.distance_upper_bound)) 81 | elif len(nearest_keys) != 2: 82 | raise NotImplementedError("interpolation works only for k_neighbors = 2") 83 | elif nearest_keys[0] == nearest_keys[1]: 84 | # Bug from nearest key ? 85 | return self[nearest_keys[0]] 86 | # Problem if ValueError: A value in x_new is above the interpolation range. 87 | else: 88 | float_nearest_keys = [float(k) for k in nearest_keys] 89 | float_input_key = float(input_key) 90 | if float_input_key < min(float_nearest_keys): 91 | return self[min(float_nearest_keys)] 92 | elif float_input_key > max(float_nearest_keys): 93 | return self[max(float_nearest_keys)] 94 | else: 95 | interpolated_positions = {} 96 | for (k, v), (k2, v2) in zip(self[nearest_keys[0]].items(), self[nearest_keys[1]].items()): 97 | if k == k2: 98 | x = np.array(float_nearest_keys) 99 | y_pos = np.array([v[0], v2[0]]) 100 | y_speed = np.array([v[1], v2[1]]) 101 | f_pos = interp1d(x, y_pos, bounds_error=False) 102 | f_speed = interp1d(x, y_speed, bounds_error=False) 103 | 104 | pos = f_pos(float_input_key) 105 | speed = f_speed(float_input_key) 106 | interpolated_positions[k] = (pos, speed) 107 | else: 108 | raise IndexError("key are not identics. Motor added during the record ?") 109 | return interpolated_positions 110 | 111 | def __missing__(self, key): 112 | if key is None: 113 | raise SyntaxError('invalid syntax, you must provide a key') 114 | return self.interpolate_motor_positions(key, self.nearest_keys(key)) 115 | -------------------------------------------------------------------------------- /pypot/utils/pypot_time.py: -------------------------------------------------------------------------------- 1 | import time as system_time 2 | 3 | 4 | def time(): 5 | return system_time.time() 6 | 7 | 8 | def sleep(t): 9 | if t > 10: 10 | print('WARNING: big sleep', t) 11 | t = 0.1 12 | system_time.sleep(t) 13 | -------------------------------------------------------------------------------- /pypot/utils/trajectory.py: -------------------------------------------------------------------------------- 1 | 2 | 3 | import numpy 4 | import collections.abc 5 | 6 | import pypot.utils.pypot_time as time 7 | from ..utils.stoppablethread import StoppableLoopThread 8 | 9 | 10 | class MinimumJerkTrajectory(object): 11 | def __init__(self, initial, final, duration, init_vel=0.0, init_acc=0.0, final_vel=0.0, final_acc=0.0): 12 | self.initial = initial 13 | self.final = final 14 | self.duration = duration 15 | self.init_vel = init_vel 16 | self.init_acc = init_acc 17 | self.final_vel = final_vel 18 | self.final_acc = final_acc 19 | 20 | self.durations = [0, duration] 21 | self.finals = [final] 22 | 23 | self.compute() 24 | 25 | def compute(self): # N'Guyen's magic 26 | a0 = self.initial 27 | a1 = self.init_vel 28 | a2 = self.init_acc / 2.0 29 | d = lambda x: self.duration ** x 30 | 31 | A = numpy.array([[d(3), d(4), d(5)], [3 * d(2), 4 * d(3), 5 * d(4)], [6 * d(1), 12 * d(2), 20 * d(3)]]) 32 | B = numpy.array([self.final - a0 - (a1 * d(1)) - (a2 * d(2)), self.final_vel - a1 - (2 * a2 * d(1)), self.final_acc - (2 * a2)]) 33 | X = numpy.linalg.solve(A, B) 34 | 35 | self.other_gen = None 36 | 37 | self._mylambda = lambda x: a0 + a1 * x + a2 * x ** 2 + X[0] * x ** 3 + X[1] * x ** 4 + X[2] * x ** 5 38 | 39 | self._generators = [self._mylambda] 40 | 41 | def get_value(self, t): 42 | return self._mygenerator[-1](t) 43 | 44 | def domain(self, x): 45 | if not isinstance(x, collections.abc.Iterable): 46 | x = numpy.array([x]) 47 | 48 | return numpy.array([ 49 | self.durations[0] <= xi < self.durations[1] 50 | for xi in x 51 | ]) 52 | 53 | def test_domain(self, x): 54 | return [((numpy.array(x) >= self.durations[i])) for i in range(len(self.durations) - 1)] 55 | 56 | def fix_input(self, x): 57 | return x if isinstance(x, collections.abc.Iterable) else numpy.array([0, x]) 58 | 59 | def get_generator(self): 60 | return lambda x: numpy.piecewise(x, self.domain(x), [self._generators[j] for j in range(len(self._generators))] + [self.finals[-1]]) 61 | 62 | 63 | class GotoMinJerk(StoppableLoopThread): 64 | def __init__(self, motor, position, duration, frequency=50): 65 | StoppableLoopThread.__init__(self, frequency) 66 | 67 | self.motor = motor 68 | self.goal = position # dict { 'motor1_name': x1, 'motor2_name': x2 } 69 | self.duration = duration # seconds 70 | 71 | def setup(self): 72 | if self.duration < numpy.finfo(float).eps: 73 | self.motor.goal_position = self.goal 74 | self.stop() 75 | else: 76 | self.trajs = MinimumJerkTrajectory(self.motor.present_position, self.goal, self.duration).get_generator() 77 | self.t0 = time.time() 78 | 79 | def update(self): 80 | if numpy.finfo(float).eps < self.duration > self.elapsed_time: 81 | self.motor.goal_position = self.trajs(self.elapsed_time) 82 | else: 83 | self.stop(wait=False) 84 | 85 | @property 86 | def elapsed_time(self): 87 | return time.time() - self.t0 88 | 89 | 90 | class GotoLinear(StoppableLoopThread): 91 | def __init__(self, motor, position, duration, frequency=50): 92 | StoppableLoopThread.__init__(self, frequency) 93 | 94 | self.motor = motor 95 | 96 | nb_step = round(duration * frequency) 97 | start = motor.goal_position 98 | end = position 99 | 100 | self.positions = numpy.linspace(start, end, nb_step + 1)[1:] 101 | self.positions = iter(self.positions) 102 | 103 | def update(self): 104 | try: 105 | p = next(self.positions) 106 | self.motor.goal_position = p 107 | 108 | except StopIteration: 109 | self.stop(wait=False) 110 | -------------------------------------------------------------------------------- /pypot/vpl/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/poppy-project/pypot/13263953a7970209efbcb476bd07b5049151f52e/pypot/vpl/__init__.py -------------------------------------------------------------------------------- /pypot/vpl/download.py: -------------------------------------------------------------------------------- 1 | import sys 2 | import tempfile 3 | from wget import download 4 | from urllib.error import URLError 5 | from zipfile import ZipFile 6 | from pathlib import Path 7 | 8 | def get_pypot_datadir(app_name="pypot"): 9 | """ 10 | Returns pypot's directory for peristent data. 11 | Attempt creation if create==True. 12 | 13 | # linux: ~/.local/share 14 | # macOS: ~/Library/Application Support 15 | # windows: C:/Users//AppData/Roaming 16 | """ 17 | home = Path.home() 18 | 19 | if sys.platform == "win32": 20 | data_dir = home / "AppData/Roaming" 21 | elif sys.platform == "linux": 22 | data_dir = home / ".local/share" 23 | elif sys.platform == "darwin": 24 | data_dir = home / "Library/Application Support" 25 | else: 26 | raise ValueError("Can't find the user data directory of your platform '{}'".format(sys.platform)) 27 | 28 | #app_name = app_name if version is None else app_name + "-" + str(version) 29 | pypot_dir = data_dir / app_name 30 | return pypot_dir 31 | 32 | 33 | def download_vpl_interactively(vpl_app_name, vpl_app_url, extract=False): 34 | """ 35 | Download the specified Visual Programming langage web app and returns its path. 36 | If it couldn't be downloaded, return None 37 | """ 38 | pypot_datadir = get_pypot_datadir() 39 | vpl_dir = pypot_datadir / vpl_app_name 40 | actual_vpl_dir = vpl_dir / vpl_app_name if extract else vpl_dir 41 | 42 | if vpl_dir.is_dir(): 43 | return actual_vpl_dir 44 | else: 45 | while True: 46 | response = input("This is the first time you are launching {}, it needs to be downloaded first. Proceed? [Y/n] ".format(vpl_app_name)) 47 | if response.lower() in ["y", ""]: 48 | try: 49 | vpl_dir.mkdir(parents=True) 50 | except FileExistsError: 51 | pass 52 | print("Downloading...") 53 | try: 54 | downloaded_app = download(vpl_app_url, tempfile.gettempdir()) 55 | except URLError as e: 56 | print("Cannot download the {} app from {}: {}".format(vpl_app_name, vpl_app_url, str(e)), file=sys.stderr) 57 | else: 58 | try: 59 | with ZipFile(downloaded_app, 'r') as archive: 60 | archive.extractall(vpl_dir) 61 | except FileNotFoundError: 62 | print("Couldn't extract {} from zipfile".format(vpl_app_name)) 63 | else: 64 | return actual_vpl_dir 65 | else: 66 | print("Download aborted by user", file=sys.stderr) 67 | return None 68 | -------------------------------------------------------------------------------- /pypot/vpl/scratch.py: -------------------------------------------------------------------------------- 1 | from .download import download_vpl_interactively 2 | 3 | # Scratch latest release download to local data directory in userspace 4 | VPL_APP_URL = "https://github.com/poppy-project/scratch-poppy/releases/latest/download/scratch-application.zip" 5 | VPL_APP_NAME = "scratch-application" 6 | VPL_EXTRACT_ZIP_ROOT = True 7 | 8 | def download_scratch_interactively(): 9 | """ 10 | Download the Scratch v1.0-beta Programming language web app and returns its path. 11 | If it couldn't be downloaded, return None 12 | """ 13 | return download_vpl_interactively(VPL_APP_NAME, VPL_APP_URL, VPL_EXTRACT_ZIP_ROOT) 14 | -------------------------------------------------------------------------------- /pypot/vpl/snap.py: -------------------------------------------------------------------------------- 1 | from .download import download_vpl_interactively 2 | 3 | # Snap 5.4.5 application download to local data directory in userspace 4 | VPL_APP_URL = "https://codeload.github.com/jmoenig/Snap/zip/v5.4.5" 5 | VPL_APP_NAME = "Snap-5.4.5" 6 | VPL_EXTRACT_ZIP_ROOT = True 7 | 8 | def download_snap_interactively(): 9 | """ 10 | Download the Snap 5.4.5 Programming langage web app and returns its path. 11 | If it couldn't be downloaded, return None 12 | """ 13 | return download_vpl_interactively(VPL_APP_NAME, VPL_APP_URL, VPL_EXTRACT_ZIP_ROOT) 14 | -------------------------------------------------------------------------------- /pypot/vrep/__init__.py: -------------------------------------------------------------------------------- 1 | import json 2 | import logging 3 | 4 | from functools import partial 5 | from collections import OrderedDict 6 | 7 | from .io import (VrepIO, close_all_connections, 8 | VrepIOError, VrepConnectionError) 9 | 10 | from .controller import VrepController, VrepObjectTracker 11 | from .controller import VrepCollisionTracker, VrepCollisionDetector 12 | 13 | from ..robot import Robot 14 | from ..robot.sensor import ObjectTracker 15 | from ..robot.config import motor_from_confignode, make_alias 16 | 17 | 18 | import pypot.utils.pypot_time as pypot_time 19 | import time as sys_time 20 | 21 | logger = logging.getLogger(__name__) 22 | 23 | 24 | class vrep_time(): 25 | def __init__(self, vrep_io): 26 | self.io = vrep_io 27 | 28 | def get_time(self, trial=0): 29 | t = self.io.get_simulation_current_time() 30 | 31 | if t == 0: 32 | sys_time.sleep(.5) 33 | return self.get_time(trial + 1) 34 | 35 | if trial > 10: 36 | raise EnvironmentError('Could not get current simulation time. Make sure the V-REP simulation is running. And that you have added the "time" child script to your scene.') 37 | 38 | return t 39 | 40 | def sleep(self, t): 41 | if t > 1000: # That's probably due to an error in get_time 42 | logger.warning('Big vrep sleep: {}'.format(t)) 43 | t = 1 44 | 45 | t0 = self.get_time() 46 | while (self.get_time() - t0) < t: 47 | if self.get_time() < t0: 48 | break 49 | sys_time.sleep(0.01) 50 | 51 | 52 | def from_vrep(config, vrep_host='127.0.0.1', vrep_port=19997, scene=None, 53 | tracked_objects=[], tracked_collisions=[], 54 | id=None, shared_vrep_io=None): 55 | """ Create a robot from a V-REP instance. 56 | 57 | :param config: robot configuration (either the path to the json or directly the dictionary) 58 | :type config: str or dict 59 | :param str vrep_host: host of the V-REP server 60 | :param int vrep_port: port of the V-REP server 61 | :param str scene: path to the V-REP scene to load and start 62 | :param list tracked_objects: list of V-REP dummy object to track 63 | :param list tracked_collisions: list of V-REP collision to track 64 | :param int id: robot id in simulator (useful when using a scene with multiple robots) 65 | :param vrep_io: use an already connected VrepIO (useful when using a scene with multiple robots) 66 | :type vrep_io: :class:`~pypot.vrep.io.VrepIO` 67 | 68 | This function tries to connect to a V-REP instance and expects to find motors with names corresponding as the ones found in the config. 69 | 70 | .. note:: The :class:`~pypot.robot.robot.Robot` returned will also provide a convenience reset_simulation method which resets the simulation and the robot position to its intial stance. 71 | 72 | .. note:: Using the same configuration, you should be able to switch from a real to a simulated robot just by switching from :func:`~pypot.robot.config.from_config` to :func:`~pypot.vrep.from_vrep`. 73 | For instance:: 74 | 75 | import json 76 | 77 | with open('my_config.json') as f: 78 | config = json.load(f) 79 | 80 | from pypot.robot import from_config 81 | from pypot.vrep import from_vrep 82 | 83 | real_robot = from_config(config) 84 | simulated_robot = from_vrep(config, '127.0.0.1', 19997, 'poppy.ttt') 85 | 86 | """ 87 | if shared_vrep_io is None: 88 | vrep_io = VrepIO(vrep_host, vrep_port) 89 | else: 90 | vrep_io = shared_vrep_io 91 | 92 | vreptime = vrep_time(vrep_io) 93 | pypot_time.time = vreptime.get_time 94 | pypot_time.sleep = vreptime.sleep 95 | 96 | if isinstance(config, str): 97 | with open(config) as f: 98 | config = json.load(f, object_pairs_hook=OrderedDict) 99 | 100 | motors = [motor_from_confignode(config, name) 101 | for name in config['motors'].keys()] 102 | 103 | vc = VrepController(vrep_io, scene, motors, id=id) 104 | vc._init_vrep_streaming() 105 | 106 | sensor_controllers = [] 107 | 108 | if tracked_objects: 109 | sensors = [ObjectTracker(name) for name in tracked_objects] 110 | vot = VrepObjectTracker(vrep_io, sensors) 111 | sensor_controllers.append(vot) 112 | 113 | if tracked_collisions: 114 | sensors = [VrepCollisionDetector(name) for name in tracked_collisions] 115 | vct = VrepCollisionTracker(vrep_io, sensors) 116 | sensor_controllers.append(vct) 117 | 118 | robot = Robot(motor_controllers=[vc], 119 | sensor_controllers=sensor_controllers) 120 | 121 | for m in robot.motors: 122 | m.goto_behavior = 'minjerk' 123 | 124 | init_pos = {m: m.goal_position for m in robot.motors} 125 | 126 | make_alias(config, robot) 127 | 128 | def start_simu(): 129 | vrep_io.start_simulation() 130 | 131 | for m, p in init_pos.items(): 132 | m.goal_position = p 133 | 134 | vc.start() 135 | 136 | if tracked_objects: 137 | vot.start() 138 | 139 | if tracked_collisions: 140 | vct.start() 141 | 142 | while vrep_io.get_simulation_current_time() < 1.: 143 | sys_time.sleep(0.1) 144 | 145 | def stop_simu(): 146 | if tracked_objects: 147 | vot.stop() 148 | 149 | if tracked_collisions: 150 | vct.stop() 151 | 152 | vc.stop() 153 | vrep_io.stop_simulation() 154 | 155 | def reset_simu(): 156 | stop_simu() 157 | sys_time.sleep(0.5) 158 | start_simu() 159 | 160 | robot.start_simulation = start_simu 161 | robot.stop_simulation = stop_simu 162 | robot.reset_simulation = reset_simu 163 | 164 | def current_simulation_time(robot): 165 | return robot._controllers[0].io.get_simulation_current_time() 166 | Robot.current_simulation_time = property(lambda robot: current_simulation_time(robot)) 167 | 168 | def get_object_position(robot, object, relative_to_object=None): 169 | return vrep_io.get_object_position(object, relative_to_object) 170 | Robot.get_object_position = partial(get_object_position, robot) 171 | 172 | def get_object_orientation(robot, object, relative_to_object=None): 173 | return vrep_io.get_object_orientation(object, relative_to_object) 174 | Robot.get_object_orientation = partial(get_object_orientation, robot) 175 | 176 | return robot 177 | -------------------------------------------------------------------------------- /pypot/vrep/child_script/inject_code.lua: -------------------------------------------------------------------------------- 1 | -- DO NOT WRITE CODE OUTSIDE OF THE if-then-end SECTIONS BELOW!! (unless the code is a function definition) 2 | 3 | if (sim_call_type==sim_childscriptcall_initialization) then 4 | 5 | -- Put some initialization code here 6 | 7 | -- Make sure you read the section on "Accessing general-type objects programmatically" 8 | -- For instance, if you wish to retrieve the handle of a scene object, use following instruction: 9 | -- 10 | -- handle=simGetObjectHandle('sceneObjectName') 11 | -- 12 | -- Above instruction retrieves the handle of 'sceneObjectName' if this script's name has no '#' in it 13 | -- 14 | -- If this script's name contains a '#' (e.g. 'someName#4'), then above instruction retrieves the handle of object 'sceneObjectName#4' 15 | -- This mechanism of handle retrieval is very convenient, since you don't need to adjust any code when a model is duplicated! 16 | -- So if the script's name (or rather the name of the object associated with this script) is: 17 | -- 18 | -- 'someName', then the handle of 'sceneObjectName' is retrieved 19 | -- 'someName#0', then the handle of 'sceneObjectName#0' is retrieved 20 | -- 'someName#1', then the handle of 'sceneObjectName#1' is retrieved 21 | -- ... 22 | -- 23 | -- If you always want to retrieve the same object's handle, no matter what, specify its full name, including a '#': 24 | -- 25 | -- handle=simGetObjectHandle('sceneObjectName#') always retrieves the handle of object 'sceneObjectName' 26 | -- handle=simGetObjectHandle('sceneObjectName#0') always retrieves the handle of object 'sceneObjectName#0' 27 | -- handle=simGetObjectHandle('sceneObjectName#1') always retrieves the handle of object 'sceneObjectName#1' 28 | -- ... 29 | -- 30 | -- Refer also to simGetCollisionhandle, simGetDistanceHandle, simGetIkGroupHandle, etc. 31 | -- 32 | -- Following 2 instructions might also be useful: simGetNameSuffix and simSetNameSuffix 33 | 34 | end 35 | 36 | 37 | if (sim_call_type==sim_childscriptcall_actuation) then 38 | 39 | -- Put your main ACTUATION code here 40 | 41 | -- For example: 42 | -- 43 | -- local position=simGetObjectPosition(handle,-1) 44 | -- position[1]=position[1]+0.001 45 | -- simSetObjectPosition(handle,-1,position) 46 | 47 | luacode = simGetStringSignal('my_lua_code') 48 | if (luacode) then 49 | f = loadstring(luacode) 50 | f() 51 | simClearStringSignal('my_lua_code') 52 | end 53 | end 54 | 55 | 56 | if (sim_call_type==sim_childscriptcall_sensing) then 57 | 58 | -- Put your main SENSING code here 59 | 60 | end 61 | 62 | 63 | if (sim_call_type==sim_childscriptcall_cleanup) then 64 | 65 | -- Put some restoration code here 66 | 67 | end 68 | -------------------------------------------------------------------------------- /pypot/vrep/child_script/timer.lua: -------------------------------------------------------------------------------- 1 | -- DO NOT WRITE CODE OUTSIDE OF THE if-then-end SECTIONS BELOW!! 2 | 3 | if (sim_call_type==sim_childscriptcall_initialization) then 4 | simSetScriptAttribute(sim_handle_self,sim_childscriptattribute_automaticcascadingcalls,false) 5 | end 6 | 7 | 8 | if (sim_call_type==sim_childscriptcall_actuation) then 9 | if not firstTimeHere93846738 then 10 | firstTimeHere93846738=0 11 | end 12 | simSetScriptAttribute(sim_handle_self,sim_scriptattribute_executioncount,firstTimeHere93846738) 13 | firstTimeHere93846738=firstTimeHere93846738+1 14 | 15 | ------------------------------------------------------------------------------ 16 | 17 | 18 | -- Check the end of the script for some explanations! 19 | if (simGetScriptExecutionCount()==0) then 20 | 21 | end 22 | 23 | simHandleChildScripts(sim_call_type) 24 | 25 | local currentTime=simGetSimulationTime() 26 | 27 | simSetFloatSignal('CurrentTime',currentTime) 28 | 29 | end 30 | 31 | 32 | if (sim_call_type==sim_childscriptcall_sensing) then 33 | simHandleChildScripts(sim_call_type) 34 | end 35 | 36 | 37 | if (sim_call_type==sim_childscriptcall_cleanup) then 38 | 39 | -- Put some restoration code here 40 | 41 | end 42 | -------------------------------------------------------------------------------- /pypot/vrep/remoteApiBindings/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/poppy-project/pypot/13263953a7970209efbcb476bd07b5049151f52e/pypot/vrep/remoteApiBindings/__init__.py -------------------------------------------------------------------------------- /pypot/vrep/remoteApiBindings/lib/linux/32Bit/remoteApi.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/poppy-project/pypot/13263953a7970209efbcb476bd07b5049151f52e/pypot/vrep/remoteApiBindings/lib/linux/32Bit/remoteApi.so -------------------------------------------------------------------------------- /pypot/vrep/remoteApiBindings/lib/linux/64Bit/remoteApi.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/poppy-project/pypot/13263953a7970209efbcb476bd07b5049151f52e/pypot/vrep/remoteApiBindings/lib/linux/64Bit/remoteApi.so -------------------------------------------------------------------------------- /pypot/vrep/remoteApiBindings/lib/mac/64Bit/remoteApi.dylib: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/poppy-project/pypot/13263953a7970209efbcb476bd07b5049151f52e/pypot/vrep/remoteApiBindings/lib/mac/64Bit/remoteApi.dylib -------------------------------------------------------------------------------- /pypot/vrep/remoteApiBindings/lib/windows/32Bit/remoteApi.dll: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/poppy-project/pypot/13263953a7970209efbcb476bd07b5049151f52e/pypot/vrep/remoteApiBindings/lib/windows/32Bit/remoteApi.dll -------------------------------------------------------------------------------- /pypot/vrep/remoteApiBindings/lib/windows/64Bit/remoteApi.dll: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/poppy-project/pypot/13263953a7970209efbcb476bd07b5049151f52e/pypot/vrep/remoteApiBindings/lib/windows/64Bit/remoteApi.dll -------------------------------------------------------------------------------- /samples/REST/ruby/README.md: -------------------------------------------------------------------------------- 1 | ## Poppy Ruby 2 | 3 | 4 | ### Example use. 5 | 6 | ```ruby 7 | 8 | $poppy = Poppy.new 9 | $poppy.set_default_motor_positions 10 | 11 | $poppy.set_compliant "true" 12 | $poppy.set_compliant "false" 13 | 14 | $motor = $poppy.motors["head_z"] 15 | 16 | #show the list of registers 17 | $motor.registers 18 | 19 | # You can access the registers directly 20 | puts $motor.compliant 21 | # false 22 | $motor.compliant = "true" 23 | 24 | ## goal_position and goal_speed registers can handle numeric values 25 | $motor.goal_position = 30 26 | $motor.goal_position = -30 27 | ``` 28 | 29 | -------------------------------------------------------------------------------- /samples/REST/ruby/motor.rb: -------------------------------------------------------------------------------- 1 | 2 | class Motor 3 | 4 | attr_reader :name, :registers 5 | 6 | def initialize (robot, motor_name) 7 | @name = motor_name 8 | @robot = robot 9 | load_registers 10 | end 11 | 12 | def set_name (name) 13 | @name = name 14 | end 15 | 16 | def load_registers 17 | 18 | @registers = Hash.new 19 | names = JSON.parse(@robot.motor_register_list self).values[0] 20 | 21 | names.each do |name| 22 | 23 | next if name == "registers" 24 | begin 25 | value = @robot.motor_register_get(self, name) 26 | value = JSON.parse(value) 27 | value = value[name] 28 | @registers[name] = value 29 | 30 | ## create a function to access this 31 | instance_variable_set("@#{name}", value) 32 | self.class.send(:attr_reader, name) 33 | 34 | if is_float_register(name) 35 | self.class.send(:define_method, name+"=") do |argument| 36 | puts "Sending value..." 37 | @robot.send_motor_register(self, name, argument.to_s) 38 | end 39 | else 40 | self.class.send(:define_method, name+"=") do |argument| 41 | puts "Sending ..." 42 | @robot.send_motor_register(self, name, argument) 43 | end 44 | 45 | end 46 | 47 | rescue 48 | # puts "Register not read " + name 49 | end 50 | end 51 | 52 | # update_registers 53 | end 54 | 55 | def is_float_register (name) 56 | if name == "goal_position" or name == "goal_speed" 57 | return true 58 | end 59 | false 60 | end 61 | 62 | def update_registers 63 | 64 | @registers.each do |name, value| 65 | begin 66 | value = @robot.motor_register_get(self, name) 67 | value = JSON.parse(value) 68 | value = value[name] 69 | @registers[name] = value 70 | 71 | instance_variable_set("@#{name}", value) 72 | rescue 73 | # puts "Register not read " + name 74 | end 75 | end 76 | end 77 | 78 | ## TODO: custom functions for key functions. 79 | ## not for the others. 80 | 81 | # def set_compliant (value) 82 | # compliant = value 83 | # end 84 | 85 | # def compliant= (value) 86 | # text = "true" if value == true 87 | # text = "false" if value == false 88 | # @robot.send_motor_register(self, "compliant", text) 89 | # end 90 | 91 | # def moving_speed= (value) 92 | # @robot.send_motor_register(self, "moving_speed", value) 93 | # end 94 | 95 | # def position= (value) 96 | # @robot.send_motor_register(self, "goal_position", value.to_s) 97 | # end 98 | 99 | # def position 100 | # present_position 101 | # end 102 | 103 | 104 | def to_s 105 | return "Motor: " + @name 106 | end 107 | 108 | # RestClient.get @url + '/motor/' + name + '/register/list.json' , {:accept => :json} 109 | 110 | 111 | 112 | end 113 | 114 | 115 | # class Register 116 | # attr_reader :name 117 | # end 118 | -------------------------------------------------------------------------------- /samples/REST/ruby/poppy-processing.rb: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | require 'ruby-processing' 3 | require './poppy' 4 | 5 | Processing::Runner 6 | Dir["#{Processing::RP_CONFIG['PROCESSING_ROOT']}/core/library/\*.jar"].each{ |jar| require jar } 7 | Processing::App::SKETCH_PATH = __FILE__ 8 | 9 | 10 | class Sketch < Processing::App 11 | 12 | attr_reader :poppy 13 | 14 | def setup 15 | size(800, 600, OPENGL) 16 | 17 | $poppy = Poppy.new "schtroumpf.local" 18 | @poppy = $poppy 19 | $motor = @poppy.motors["head_z"] 20 | 21 | 22 | 23 | end 24 | 25 | def draw 26 | background 200 27 | rect 0, 0, 100, 20 28 | 29 | if mouse_y < 20 and mouse_x < 100 30 | ellipse mouse_x, mouse_y, 10, 10 31 | end 32 | 33 | 34 | end 35 | 36 | def mouse_dragged 37 | if mouse_y < 20 38 | $motor.goal_position = mouse_x - 50 if mouse_x < 100 39 | end 40 | end 41 | 42 | end 43 | 44 | 45 | Sketch.new unless defined? $app 46 | -------------------------------------------------------------------------------- /samples/REST/ruby/poppy.rb: -------------------------------------------------------------------------------- 1 | require 'json' 2 | require 'rest-client' 3 | require 'parallel' 4 | 5 | require './motor' 6 | 7 | class Poppy 8 | 9 | attr_reader :ip, :port, :url, :motors 10 | 11 | def initialize ip 12 | @host = ip 13 | @port = "8080" 14 | @url = "http://" + @host + ":" + @port 15 | 16 | init_motors; 17 | end 18 | 19 | def init_motors 20 | @motors = Hash.new 21 | motor_list.each do |motor_name| 22 | @motors[motor_name] = Motor.new(self, motor_name) 23 | end 24 | end 25 | 26 | 27 | def motor_list_text 28 | RestClient.get @url + '/motor/list.json' 29 | end 30 | 31 | def motor_list 32 | JSON.parse(motor_list_text).values[0] 33 | end 34 | 35 | def to_s 36 | "Poppy :]" 37 | end 38 | 39 | def update_motor_registers 40 | each_motor do |motor| 41 | motor.update_registers 42 | end 43 | end 44 | 45 | def each_motor 46 | @motors.each_value do |motor| 47 | yield motor 48 | end 49 | end 50 | 51 | def set_compliant (is_compliant) 52 | each_motor do |motor| 53 | motor.compliant = is_compliant 54 | end 55 | end 56 | 57 | def set_default_motor_positions 58 | each_motor do |motor| 59 | motor.compliant = "false" 60 | motor.goal_speed = 0 61 | motor.goal_position = 0 62 | end 63 | 64 | end 65 | 66 | def set_default_motor_positions_parallel 67 | Parallel.each(@motors.values) do |motor| 68 | motor.compliant = "false" 69 | motor.goal_speed = 10 70 | motor.goal_position = 0 71 | end 72 | end 73 | 74 | ## Low level for motors ... 75 | 76 | def motor_values (name) 77 | RestClient.get @url + '/motor/' + name + '/register/list.json' , {:accept => :json} 78 | end 79 | 80 | def motor_register_get (motor, register) 81 | RestClient.get @url + '/motor/' + motor.name + '/register/' + register , {:accept => :json} 82 | end 83 | 84 | def motor_register_list (motor) 85 | RestClient.get @url + '/motor/' + motor.name + '/register/list.json' , {:accept => :json} 86 | end 87 | 88 | def send_motor_register (motor, register, value) 89 | RestClient.post @url + '/motor/' + motor.name + '/register/' + register + '/value.json', value, :content_type => :json 90 | end 91 | 92 | end 93 | -------------------------------------------------------------------------------- /samples/benchmarks/controller.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/poppy-project/pypot/13263953a7970209efbcb476bd07b5049151f52e/samples/benchmarks/controller.png -------------------------------------------------------------------------------- /samples/benchmarks/cpu_usage.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/poppy-project/pypot/13263953a7970209efbcb476bd07b5049151f52e/samples/benchmarks/cpu_usage.png -------------------------------------------------------------------------------- /samples/benchmarks/dxl-controller.py: -------------------------------------------------------------------------------- 1 | import os 2 | import time 3 | import argparse 4 | 5 | from numpy import argmax, mean 6 | 7 | from pypot.robot import from_json 8 | 9 | 10 | if __name__ == '__main__': 11 | parser = argparse.ArgumentParser() 12 | parser.add_argument('-l', '--log-file', 13 | type=str, required=True) 14 | parser.add_argument('-N', type=int, required=True) 15 | parser.add_argument('-c', '--config', 16 | type=str, required=True) 17 | args = parser.parse_args() 18 | 19 | bp = args.log_file 20 | if os.path.exists(bp): 21 | raise IOError('File already exists: {}'.format(bp)) 22 | 23 | robot = from_json(args.config) 24 | 25 | # We keep the controller with most motors connected 26 | motors = [len(c.motors) for c in robot._controllers] 27 | c = robot._controllers[argmax(motors)] 28 | 29 | print('Using controller with motors {}'.format([m.id for m in c.motors])) 30 | c = c.controllers[1] 31 | 32 | dt = [] 33 | for _ in range(args.N): 34 | start = time.time() 35 | c.get_present_position_speed_load(c.motors) 36 | end = time.time() 37 | dt.append(end - start) 38 | 39 | # We'll use raw file instead of numpy because of pypy 40 | with open(bp, 'w') as f: 41 | f.write(str(dt)) 42 | 43 | print('Done in {}ms'.format(mean(dt) * 1000)) 44 | -------------------------------------------------------------------------------- /samples/benchmarks/dxl-single-motor.py: -------------------------------------------------------------------------------- 1 | import os 2 | import time 3 | import numpy 4 | import argparse 5 | import itertools 6 | 7 | from pypot.dynamixel import DxlIO, get_available_ports 8 | from pypot.dynamixel.protocol.v1 import DxlReadDataPacket, DxlSyncWritePacket 9 | from pypot.dynamixel.conversion import dxl_code 10 | 11 | 12 | def timeit(rw_func, N, destfile): 13 | dt = [] 14 | for _ in range(N): 15 | start = time.time() 16 | rw_func() 17 | end = time.time() 18 | 19 | dt.append(end - start) 20 | 21 | # We'll use raw file instead of numpy because of pypy 22 | with open(destfile, 'w') as f: 23 | f.write(str(dt)) 24 | 25 | return numpy.mean(dt) 26 | 27 | if __name__ == '__main__': 28 | parser = argparse.ArgumentParser() 29 | parser.add_argument('-l', '--log-folder', 30 | type=str, required=True) 31 | parser.add_argument('-N', type=int, required=True) 32 | args = parser.parse_args() 33 | 34 | bp = args.log_folder 35 | if os.path.exists(bp): 36 | raise IOError('Folder already exists: {}'.format(bp)) 37 | os.mkdir(bp) 38 | 39 | available_ports = get_available_ports() 40 | 41 | port = available_ports[0] 42 | print('Connecting on port {}.'.format(port)) 43 | dxl = DxlIO(port) 44 | 45 | print('Scanning motors on the bus...') 46 | ids = dxl.scan() 47 | print('Found {}'.format(ids)) 48 | 49 | ids = ids[:1] 50 | print('Will use id: {}'.format(ids)) 51 | 52 | if dxl.get_return_delay_time(ids)[0] != 0: 53 | raise IOError('Make sure your motor has return delay time set to 0') 54 | 55 | print('Start testing !') 56 | 57 | print('Using "normal" pypot package...') 58 | def rw_pypot(): 59 | dxl.get_present_position(ids) 60 | dxl.set_goal_position(dict(zip(ids, itertools.repeat(0)))) 61 | 62 | dt = timeit(rw_pypot, args.N, os.path.join(bp, 'rw_pypot.list')) 63 | print('in {}ms'.format(dt * 1000)) 64 | 65 | print('Using pref-forged packet...') 66 | c_get = [c for c in DxlIO._AbstractDxlIO__controls 67 | if c.name == 'present position'][0] 68 | 69 | c_set = [c for c in DxlIO._AbstractDxlIO__controls 70 | if c.name == 'goal position'][0] 71 | 72 | pos = dxl_code(0, c_set.length) 73 | rp = DxlReadDataPacket(ids[0], c_get.address, c_get.length) 74 | sp = DxlSyncWritePacket(c_set.address, c_set.length, ids[:1] + list(pos)) 75 | 76 | def rw_forged_packet(): 77 | dxl._send_packet(rp) 78 | dxl._send_packet(sp, wait_for_status_packet=False) 79 | 80 | dt = timeit(rw_forged_packet, args.N, os.path.join(bp, 'rw_forged.list')) 81 | print('in {}ms'.format(dt * 1000)) 82 | 83 | print('Using raw serial communication...') 84 | s_read = rp.to_string() 85 | s_write = sp.to_string() 86 | 87 | def rw_serial(): 88 | dxl._serial.write(s_read) 89 | dxl._serial.read(8) 90 | dxl._serial.write(s_write) 91 | dt = timeit(rw_serial, args.N, os.path.join(bp, 'rw_serial.list')) 92 | print('in {}ms'.format(dt * 1000)) 93 | 94 | print('Done !') 95 | -------------------------------------------------------------------------------- /samples/benchmarks/load_data.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "metadata": { 3 | "name": "", 4 | "signature": "sha256:6886e71181444b36c7a33b59039603d34808e3af4a3cd70cd03670eb5d8d59b2" 5 | }, 6 | "nbformat": 3, 7 | "nbformat_minor": 0, 8 | "worksheets": [ 9 | { 10 | "cells": [ 11 | { 12 | "cell_type": "code", 13 | "collapsed": false, 14 | "input": [ 15 | "%pylab inline" 16 | ], 17 | "language": "python", 18 | "metadata": {}, 19 | "outputs": [ 20 | { 21 | "output_type": "stream", 22 | "stream": "stdout", 23 | "text": [ 24 | "Populating the interactive namespace from numpy and matplotlib\n" 25 | ] 26 | } 27 | ], 28 | "prompt_number": 1 29 | }, 30 | { 31 | "cell_type": "code", 32 | "collapsed": false, 33 | "input": [ 34 | "boards = ['odroid', 'pi', 'dell']\n", 35 | "pythons = ['3.4.1', '2.7.8', 'pypy-2.3.1']" 36 | ], 37 | "language": "python", 38 | "metadata": {}, 39 | "outputs": [], 40 | "prompt_number": 2 41 | }, 42 | { 43 | "cell_type": "code", 44 | "collapsed": false, 45 | "input": [ 46 | "import os\n", 47 | "\n", 48 | "from collections import defaultdict\n", 49 | "\n", 50 | "cpu_usage = defaultdict(dict)\n", 51 | "controller_rw_time = defaultdict(dict)\n", 52 | "\n", 53 | "packet = defaultdict(lambda: defaultdict(dict))\n", 54 | "\n", 55 | "for b in boards: \n", 56 | " for p in pythons:\n", 57 | " bp = os.path.join('res', b, p)\n", 58 | " \n", 59 | " l = eval(open(os.path.join(bp, 'robot.list')).read())\n", 60 | " cpu_usage[b][p] = array(l)\n", 61 | " \n", 62 | " l = eval(open(os.path.join(bp, 'dxl_controller.list')).read())\n", 63 | " controller_rw_time[b][p] = array(l)\n", 64 | " \n", 65 | " for s in ['serial', 'forged', 'pypot']:\n", 66 | " l = eval(open(os.path.join(bp, 'dxl_single', 'rw_{}.list'.format(s))).read())\n", 67 | " packet[s][b][p] = array(l)" 68 | ], 69 | "language": "python", 70 | "metadata": {}, 71 | "outputs": [], 72 | "prompt_number": 3 73 | }, 74 | { 75 | "cell_type": "code", 76 | "collapsed": false, 77 | "input": [ 78 | "data = {\n", 79 | " 'cpu_usage': dict(cpu_usage),\n", 80 | " 'controller_rw_time': dict(controller_rw_time),\n", 81 | " 'packet': dict(packet)\n", 82 | "}" 83 | ], 84 | "language": "python", 85 | "metadata": {}, 86 | "outputs": [] 87 | }, 88 | { 89 | "cell_type": "code", 90 | "collapsed": false, 91 | "input": [ 92 | "import pickle\n", 93 | "\n", 94 | "# get the file here: https://github.com/poppy-project/pypot/releases/download/2.4.0/data.pickle", 95 | "with open('data.pickle', 'wb') as f:\n", 96 | " pickle.dump(data, f)" 97 | ], 98 | "language": "python", 99 | "metadata": {}, 100 | "outputs": [] 101 | }, 102 | { 103 | "cell_type": "code", 104 | "collapsed": false, 105 | "input": [], 106 | "language": "python", 107 | "metadata": {}, 108 | "outputs": [] 109 | } 110 | ], 111 | "metadata": {} 112 | } 113 | ] 114 | } 115 | -------------------------------------------------------------------------------- /samples/benchmarks/packet.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/poppy-project/pypot/13263953a7970209efbcb476bd07b5049151f52e/samples/benchmarks/packet.pdf -------------------------------------------------------------------------------- /samples/benchmarks/packet.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/poppy-project/pypot/13263953a7970209efbcb476bd07b5049151f52e/samples/benchmarks/packet.png -------------------------------------------------------------------------------- /samples/benchmarks/robot.py: -------------------------------------------------------------------------------- 1 | import os 2 | import time 3 | import psutil 4 | import argparse 5 | 6 | from numpy import mean 7 | 8 | from pypot.robot import from_json 9 | 10 | 11 | if __name__ == '__main__': 12 | parser = argparse.ArgumentParser() 13 | parser.add_argument('-l', '--log-file', 14 | type=str, required=True) 15 | parser.add_argument('-t', '--running-time', 16 | type=int, required=True) 17 | parser.add_argument('-c', '--config', 18 | type=str, required=True) 19 | args = parser.parse_args() 20 | 21 | bp = args.log_file 22 | if os.path.exists(bp): 23 | raise IOError('File already exists: {}'.format(bp)) 24 | 25 | p = psutil.Process() 26 | p.cpu_percent() 27 | 28 | robot = from_json(args.config) 29 | robot.start_sync() 30 | time.sleep(60) 31 | 32 | loads = [] 33 | start = time.time() 34 | while time.time() - start < args.running_time: 35 | loads.append(p.cpu_percent()) 36 | time.sleep(1) 37 | load = mean(loads) 38 | print('After loading about {}%'.format(load)) 39 | 40 | # We'll use raw file instead of numpy because of pypy 41 | with open(bp, 'w') as f: 42 | f.write(str(loads)) 43 | -------------------------------------------------------------------------------- /samples/benchmarks/run-all.py: -------------------------------------------------------------------------------- 1 | import os 2 | import json 3 | import argparse 4 | 5 | from subprocess import call, check_output 6 | 7 | from pypot.robot import from_json 8 | 9 | N = 50000 10 | T = 15 * 60 11 | 12 | 13 | if __name__ == '__main__': 14 | parser = argparse.ArgumentParser() 15 | 16 | parser.add_argument('-l', '--log-folder', 17 | type=str, required=True) 18 | 19 | parser.add_argument('-c', '--config', 20 | type=str, required=True) 21 | 22 | args = parser.parse_args() 23 | 24 | bp = args.log_folder 25 | if os.path.exists(bp): 26 | raise IOError('Folder already exists: {}'.format(bp)) 27 | os.mkdir(bp) 28 | 29 | call(["python", "dxl-single-motor.py", 30 | "-l", os.path.join(bp, 'dxl_single'), 31 | "-N", str(N)]) 32 | 33 | call(["python", "dxl-controller.py", 34 | "-l", os.path.join(bp, 'dxl_controller.list'), 35 | "-N", str(N), 36 | "-c", args.config]) 37 | 38 | call(["python", "robot.py", 39 | "-l", os.path.join(bp, 'robot.list'), 40 | "-t", str(T), 41 | "-c", args.config]) 42 | 43 | print('Everything is done!!!') 44 | -------------------------------------------------------------------------------- /samples/benchmarks/vrep.py: -------------------------------------------------------------------------------- 1 | import os 2 | import json 3 | import time 4 | 5 | import pypot 6 | import poppytools 7 | 8 | from pypot.vrep import from_vrep 9 | from pypot.primitive import LoopPrimitive 10 | 11 | 12 | if __name__ == '__main__': 13 | DT = 30. 14 | 15 | class JustWaitingPrimitive(LoopPrimitive): 16 | def update(self): 17 | if self.elapsed_time > DT: 18 | self.stop(wait=False) 19 | 20 | configfile = os.path.join(os.path.dirname(poppytools.__file__), 21 | 'configuration', 'poppy_config.json') 22 | 23 | with open(configfile) as f: 24 | poppy_config = json.load(f) 25 | 26 | scene_path = os.path.join(os.path.dirname(pypot.__file__), 27 | '..', 'samples', 'notebooks', 'poppy-sitting.ttt') 28 | 29 | poppy = from_vrep(poppy_config, '127.0.0.1', 19997, scene_path) 30 | 31 | time.sleep(0.1) 32 | 33 | p = JustWaitingPrimitive(poppy, 50.) 34 | 35 | t0 = time.time() 36 | p.start() 37 | p.wait_to_stop() 38 | print('Running {}s of v-rep simulation took {}s'.format(DT, time.time() - t0)) 39 | -------------------------------------------------------------------------------- /samples/notebooks/image/snap-basic-blocks.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/poppy-project/pypot/13263953a7970209efbcb476bd07b5049151f52e/samples/notebooks/image/snap-basic-blocks.png -------------------------------------------------------------------------------- /samples/notebooks/image/snap-find-pypot-blocks.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/poppy-project/pypot/13263953a7970209efbcb476bd07b5049151f52e/samples/notebooks/image/snap-find-pypot-blocks.png -------------------------------------------------------------------------------- /samples/notebooks/image/snap-header.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/poppy-project/pypot/13263953a7970209efbcb476bd07b5049151f52e/samples/notebooks/image/snap-header.png -------------------------------------------------------------------------------- /samples/notebooks/image/snap-hello-world.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/poppy-project/pypot/13263953a7970209efbcb476bd07b5049151f52e/samples/notebooks/image/snap-hello-world.png -------------------------------------------------------------------------------- /samples/notebooks/image/snap-import.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/poppy-project/pypot/13263953a7970209efbcb476bd07b5049151f52e/samples/notebooks/image/snap-import.png -------------------------------------------------------------------------------- /samples/notebooks/image/snap-orchestration-demo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/poppy-project/pypot/13263953a7970209efbcb476bd07b5049151f52e/samples/notebooks/image/snap-orchestration-demo.png -------------------------------------------------------------------------------- /samples/notebooks/image/snap-ready.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/poppy-project/pypot/13263953a7970209efbcb476bd07b5049151f52e/samples/notebooks/image/snap-ready.png -------------------------------------------------------------------------------- /samples/notebooks/image/snap-right-click.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/poppy-project/pypot/13263953a7970209efbcb476bd07b5049151f52e/samples/notebooks/image/snap-right-click.png -------------------------------------------------------------------------------- /samples/notebooks/image/snap-sinus.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/poppy-project/pypot/13263953a7970209efbcb476bd07b5049151f52e/samples/notebooks/image/snap-sinus.png -------------------------------------------------------------------------------- /samples/notebooks/image/snap-slider-example.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/poppy-project/pypot/13263953a7970209efbcb476bd07b5049151f52e/samples/notebooks/image/snap-slider-example.png -------------------------------------------------------------------------------- /samples/notebooks/image/snap-slider.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/poppy-project/pypot/13263953a7970209efbcb476bd07b5049151f52e/samples/notebooks/image/snap-slider.png -------------------------------------------------------------------------------- /samples/notebooks/image/snap.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/poppy-project/pypot/13263953a7970209efbcb476bd07b5049151f52e/samples/notebooks/image/snap.png -------------------------------------------------------------------------------- /samples/notebooks/readme.md: -------------------------------------------------------------------------------- 1 | # Notebooks everywhere 2 | 3 | In the [Poppy Project](https://www.poppy-project.org), we are huge fans of [Jupyter Notebooks](http://jupyter.org) both for development and for documentation. Most of our tests, benchs, experiments, and even debugs are now done using notebooks. Thus, we have decided that pypot's documentation and more generally all the software documentation related to the [Poppy Project](https://www.poppy-project.org) will smoothly move towards notebooks. 4 | 5 | We strongly believe that they have an amazing potential: 6 | * for writing pedalogical contents - by **mixing code, explanation, results in an integrated form** 7 | * for easily sharing - they can be easily **review online** and then **run locally** 8 | * they can also be run remotely on the robot and thus **no installation at all is required on the client side, you just need a web browser** 9 | 10 | We already have written a few tutorial notebooks that you can browse here: https://poppy-project.gitbooks.io/poppy-docs/content/en/programming/notebooks.html 11 | 12 | Other notebooks should shorty extend this list! 13 | 14 | In the next sections, we decribe how those notebooks can be [viewed](https://github.com/poppy-project/pypot/blob/master/samples/notebooks/readme.md#browse-notebook-online), [installed](https://github.com/poppy-project/pypot/blob/master/samples/notebooks/readme.md#running-notebooks-locally), [run on your robot](https://github.com/poppy-project/pypot/blob/master/samples/notebooks/readme.md#connecting-to-a-remote-notebook) or [locally](https://github.com/poppy-project/pypot/blob/master/samples/notebooks/readme.md#running-notebooks-locally), shared and how you can [contribute](https://github.com/poppy-project/pypot/blob/master/samples/notebooks/readme.md#contribute) by writing your own notebooks! 15 | 16 | # Open Source 17 | All the resources content from the Poppy Project is open source. This naturally also extends to the notebooks. 18 | 19 | License | Notebooks | Library | 20 | | ----------- | :-------------: | :-------------: | 21 | | Name | [Creatives Commons BY-SA](http://creativecommons.org/licenses/by-sa/4.0/) |[GPL v3](http://www.gnu.org/licenses/gpl.html) | 22 | | Logo | [![Creative Commons BY-SA](https://i.creativecommons.org/l/by-sa/4.0/88x31.png) ](http://creativecommons.org/licenses/by-sa/4.0/) |[![GPL V3](https://www.gnu.org/graphics/gplv3-88x31.png)](http://www.gnu.org/licenses/gpl.html) | 23 | 24 | # Browse Notebook Online 25 | 26 | One of the great features of the Notebook is that they can be read directly online without needing to install anything on your laptop. The **only thing you need is a web browser!** 27 | 28 | Thanks to the [nbviewer](http://nbviewer.ipython.org) website, you can read any Notebook online. You can also directly view them on [Github](https://github.com). The Notebooks from the Poppy Project can be directly found [here](http://nbviewer.ipython.org/github/poppy-project/pypot/tree/master/samples/notebooks/). A list of the "best" Notebook is also maintained [here](https://poppy-project.gitbooks.io/poppy-docs/content/en/programming/notebooks.html). 29 | 30 | If you want to run notebooks yourself and play with the code you will have to use one of the other techniques described below. 31 | 32 | # Connecting to a Remote Notebook 33 | 34 | The Poppy Creature we developed are usually provided with an embedded board (e.g. a [raspberry pi](http://www.raspberrypi.org)) with all the software tools needed to run [pypot](https://github.com/poppy-project/pypot) already installed - this means a python interpreter, the pypot and poppy pacakges and [Jupyter Notebook](http://jupyter.org) server. Thus, you do not need to install anything specific on your personal computer to connect to a remote Notebook. 35 | 36 | When plugged, the embedded board of the Poppy Creature should automatically start a Jupyter Notebook server providing access to all the notebooks tutorials. 37 | 38 | Assuming that you creature is connected to the same network as your computer (see [here](https://github.com/pierre-rouanet/rasp-poppy) for details), they can be accessed just by connecting to an url such as: http://poppy.local. 39 | 40 | **It's important to note here that the notebook will actually run on the embedded board and not on your local machine!** You will thus not be able to easily access a file on your machine using this approach. Similarly, if you need to install an extra python package, you will have to install it directly on the board (e.g. using an [ssh connection](https://github.com/poppy-project/raspoppy)). 41 | 42 | # Running Notebooks Locally 43 | 44 | If you want to run Notebooks on your local machine, because you are working with the simulator for instance, you will have to start the jupyter notebook server yourself. 45 | 46 | To do this, you will need to find your way in the sometimes [confusing](http://captiongenerator.com/30052/Hitler-reacts-to-the-Python-ecosystem) [python packaging system](https://python-packaging-user-guide.readthedocs.org/en/latest/current.html). In details, you will need (this is not the only way to install all the tools but this is probably the most straightforward): 47 | * a [python](https://www.python.org) interpreter (we tested with *3.4* or *pypy-2.5*) 48 | * the [pip tool](https://pip.pypa.io) for installing Python packages - [this documentation describe how to install pip on your system](https://pip.pypa.io/en/latest/installing.html#install-pip). 49 | 50 | Now that you have a working Python environment, you can install the [Jupyter Notebook package](http://jupyter.readthedocs.org/en/latest/install.html) using pip. You only have to run the following line on a command line terminal: 51 | 52 | ```bash 53 | pip install jupyter 54 | ``` 55 | 56 | Linux users may have to run (depending on their python installation): 57 | ```bash 58 | sudo pip install jupyter 59 | ``` 60 | 61 | Then, you need to install pypot and the software for your creature. For instance, to use a PoppyErgoJr: 62 | ```bash 63 | pip install pypot poppy-ergo-jr 64 | ``` 65 | 66 | Finally, you can now run the notebook server from a terminal: 67 | ```bash 68 | jupyter notebook 69 | ``` 70 | You can also specify the folder where your notebooks are, for instance on my machine 71 | ```bash 72 | jupyter notebook ~/dev/pypot/samples/notebooks/ 73 | ``` 74 | 75 | # Support 76 | The [Poppy forum](forum.poppy-project.org) is the best place to ask for help! You can also use the [github issue tracker](https://github.com/poppy-project/pypot/labels/Notebooks) for specific notebooks issues. 77 | 78 | # Contribute 79 | First, please report any bug or issue to our [github issue tracker](https://github.com/poppy-project/pypot/labels/Notebooks). 80 | 81 | Second, you can discuss, suggest, or even better provide us with other Notebooks. We hope to gather as many and as diversified Notebooks as possible. They could be pedagogical content, experiments or just funny behaviours for robotic creatures. The easiest way to share them with us is trough pull requests. 82 | 83 | Finally, as we are not native English speaker [your](http://www.troll.me/images2/grammar-correction-guy/your-youre-welcome.jpg) encouraged to report any spell checking or any sentence which does not make much sense ^^. Translation to other language are also welcomed! 84 | -------------------------------------------------------------------------------- /samples/notebooks/robot-client.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "metadata": { 3 | "name": "", 4 | "signature": "sha256:9066a0365d9e68ffe50fe55b615e37151fcf20e4c270599513e57e5380ee885d" 5 | }, 6 | "nbformat": 3, 7 | "nbformat_minor": 0, 8 | "worksheets": [ 9 | { 10 | "cells": [ 11 | { 12 | "cell_type": "code", 13 | "collapsed": false, 14 | "input": [ 15 | "from pypot.robot import from_remote\n", 16 | "\n", 17 | "remote_robot = from_remote('127.0.0.1')" 18 | ], 19 | "language": "python", 20 | "metadata": {}, 21 | "outputs": [], 22 | "prompt_number": 1 23 | }, 24 | { 25 | "cell_type": "code", 26 | "collapsed": false, 27 | "input": [ 28 | "for m in remote_robot.arm:\n", 29 | " m.compliant = False\n", 30 | " m.moving_speed = 50\n", 31 | " m.goal_position = 0" 32 | ], 33 | "language": "python", 34 | "metadata": {}, 35 | "outputs": [ 36 | { 37 | "ename": "AttributeError", 38 | "evalue": "'RemoteRobot' object has no attribute 'arm'", 39 | "output_type": "pyerr", 40 | "traceback": [ 41 | "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m\n\u001b[0;31mAttributeError\u001b[0m Traceback (most recent call last)", 42 | "\u001b[0;32m\u001b[0m in \u001b[0;36m\u001b[0;34m()\u001b[0m\n\u001b[0;32m----> 1\u001b[0;31m \u001b[0;32mfor\u001b[0m \u001b[0mm\u001b[0m \u001b[0;32min\u001b[0m \u001b[0mremote_robot\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0marm\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0m\u001b[1;32m 2\u001b[0m \u001b[0mm\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mcompliant\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mFalse\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m 3\u001b[0m \u001b[0mm\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mmoving_speed\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0;36m50\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m 4\u001b[0m \u001b[0mm\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mgoal_position\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0;36m0\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n", 43 | "\u001b[0;31mAttributeError\u001b[0m: 'RemoteRobot' object has no attribute 'arm'" 44 | ] 45 | } 46 | ], 47 | "prompt_number": 2 48 | }, 49 | { 50 | "cell_type": "code", 51 | "collapsed": false, 52 | "input": [ 53 | "remote_robot.primitives" 54 | ], 55 | "language": "python", 56 | "metadata": {}, 57 | "outputs": [ 58 | { 59 | "metadata": {}, 60 | "output_type": "pyout", 61 | "prompt_number": 3, 62 | "text": [ 63 | "[]" 64 | ] 65 | } 66 | ], 67 | "prompt_number": 3 68 | }, 69 | { 70 | "cell_type": "code", 71 | "collapsed": false, 72 | "input": [ 73 | "for m in remote_robot.motors:\n", 74 | " m.compliant = False\n", 75 | " m.goal_position = 0" 76 | ], 77 | "language": "python", 78 | "metadata": {}, 79 | "outputs": [], 80 | "prompt_number": 4 81 | }, 82 | { 83 | "cell_type": "code", 84 | "collapsed": false, 85 | "input": [ 86 | "remote_robot.my_sinus.start()" 87 | ], 88 | "language": "python", 89 | "metadata": {}, 90 | "outputs": [], 91 | "prompt_number": 5 92 | }, 93 | { 94 | "cell_type": "code", 95 | "collapsed": false, 96 | "input": [ 97 | "remote_robot.my_sinus.stop()" 98 | ], 99 | "language": "python", 100 | "metadata": {}, 101 | "outputs": [], 102 | "prompt_number": 6 103 | }, 104 | { 105 | "cell_type": "code", 106 | "collapsed": false, 107 | "input": [], 108 | "language": "python", 109 | "metadata": {}, 110 | "outputs": [] 111 | } 112 | ], 113 | "metadata": {} 114 | } 115 | ] 116 | } -------------------------------------------------------------------------------- /samples/notebooks/robot-server.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "metadata": { 3 | "name": "", 4 | "signature": "sha256:25f974945afc9b1b27aaa5b6052b2450ccd9a439db8fbffa8906951404958c87" 5 | }, 6 | "nbformat": 3, 7 | "nbformat_minor": 0, 8 | "worksheets": [ 9 | { 10 | "cells": [ 11 | { 12 | "cell_type": "code", 13 | "collapsed": false, 14 | "input": [ 15 | "from pypot.dynamixel import autodetect_robot\n", 16 | "\n", 17 | "robot = autodetect_robot()\n", 18 | "robot.start_sync()" 19 | ], 20 | "language": "python", 21 | "metadata": {}, 22 | "outputs": [], 23 | "prompt_number": 1 24 | }, 25 | { 26 | "cell_type": "code", 27 | "collapsed": false, 28 | "input": [ 29 | "from pypot.primitive.utils import Sinus\n", 30 | "\n", 31 | "my_sinus = Sinus(robot, 50, robot.motors, amp=10)\n", 32 | "robot.attach_primitive(my_sinus, 'my_sinus')" 33 | ], 34 | "language": "python", 35 | "metadata": {}, 36 | "outputs": [], 37 | "prompt_number": 2 38 | }, 39 | { 40 | "cell_type": "code", 41 | "collapsed": false, 42 | "input": [ 43 | "from pypot.server.rest import RESTRobot\n", 44 | "\n", 45 | "robot_server = RESTRobot(robot)" 46 | ], 47 | "language": "python", 48 | "metadata": {}, 49 | "outputs": [], 50 | "prompt_number": 3 51 | }, 52 | { 53 | "cell_type": "code", 54 | "collapsed": false, 55 | "input": [ 56 | "import zerorpc\n", 57 | "\n", 58 | "s = zerorpc.Server(robot_server)\n", 59 | "s.bind(\"tcp://0.0.0.0:4242\")\n", 60 | "s.run()" 61 | ], 62 | "language": "python", 63 | "metadata": {}, 64 | "outputs": [ 65 | { 66 | "output_type": "stream", 67 | "stream": "stderr", 68 | "text": [ 69 | "KeyboardInterrupt\n" 70 | ] 71 | }, 72 | { 73 | "ename": "KeyboardInterrupt", 74 | "evalue": "", 75 | "output_type": "pyerr", 76 | "traceback": [ 77 | "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m\n\u001b[0;31mKeyboardInterrupt\u001b[0m Traceback (most recent call last)", 78 | "\u001b[0;32m\u001b[0m in \u001b[0;36m\u001b[0;34m()\u001b[0m\n\u001b[1;32m 3\u001b[0m \u001b[0ms\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mzerorpc\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mServer\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mrobot_server\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m 4\u001b[0m \u001b[0ms\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mbind\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;34m\"tcp://0.0.0.0:4242\"\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m----> 5\u001b[0;31m \u001b[0ms\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mrun\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0m", 79 | "\u001b[0;32m/usr/local/lib/python2.7/site-packages/zerorpc/core.pyc\u001b[0m in \u001b[0;36mrun\u001b[0;34m(self)\u001b[0m\n\u001b[1;32m 169\u001b[0m \u001b[0mself\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0m_acceptor_task\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mgevent\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mspawn\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mself\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0m_acceptor\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m 170\u001b[0m \u001b[0;32mtry\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m--> 171\u001b[0;31m \u001b[0mself\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0m_acceptor_task\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mget\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0m\u001b[1;32m 172\u001b[0m \u001b[0;32mfinally\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m 173\u001b[0m \u001b[0mself\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mstop\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n", 80 | "\u001b[0;32m/usr/local/lib/python2.7/site-packages/gevent/greenlet.pyc\u001b[0m in \u001b[0;36mget\u001b[0;34m(self, block, timeout)\u001b[0m\n\u001b[1;32m 256\u001b[0m \u001b[0mt\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mTimeout\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mstart_new\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mtimeout\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m 257\u001b[0m \u001b[0;32mtry\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m--> 258\u001b[0;31m \u001b[0mresult\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mself\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mparent\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mswitch\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0m\u001b[1;32m 259\u001b[0m \u001b[0;32massert\u001b[0m \u001b[0mresult\u001b[0m \u001b[0;32mis\u001b[0m \u001b[0mself\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0;34m'Invalid switch into Greenlet.get(): %r'\u001b[0m \u001b[0;34m%\u001b[0m \u001b[0;34m(\u001b[0m\u001b[0mresult\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m 260\u001b[0m \u001b[0;32mfinally\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n", 81 | "\u001b[0;32m/usr/local/lib/python2.7/site-packages/gevent/hub.pyc\u001b[0m in \u001b[0;36mswitch\u001b[0;34m(self)\u001b[0m\n\u001b[1;32m 329\u001b[0m \u001b[0;32mif\u001b[0m \u001b[0mswitch_out\u001b[0m \u001b[0;32mis\u001b[0m \u001b[0;32mnot\u001b[0m \u001b[0mNone\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m 330\u001b[0m \u001b[0mswitch_out\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m--> 331\u001b[0;31m \u001b[0;32mreturn\u001b[0m \u001b[0mgreenlet\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mswitch\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mself\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0m\u001b[1;32m 332\u001b[0m \u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m 333\u001b[0m \u001b[0;32mdef\u001b[0m \u001b[0mswitch_out\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mself\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n", 82 | "\u001b[0;31mKeyboardInterrupt\u001b[0m: " 83 | ] 84 | } 85 | ], 86 | "prompt_number": 4 87 | }, 88 | { 89 | "cell_type": "code", 90 | "collapsed": false, 91 | "input": [], 92 | "language": "python", 93 | "metadata": {}, 94 | "outputs": [] 95 | } 96 | ], 97 | "metadata": {} 98 | } 99 | ] 100 | } -------------------------------------------------------------------------------- /samples/profiles/ergo-jr-rpi2-idle-light-syncloop.prof: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/poppy-project/pypot/13263953a7970209efbcb476bd07b5049151f52e/samples/profiles/ergo-jr-rpi2-idle-light-syncloop.prof -------------------------------------------------------------------------------- /samples/profiles/profiles.md: -------------------------------------------------------------------------------- 1 | We generate the profile using [yappi](https://bitbucket.org/sumerc/yappi/) and export the stats using the pstat format. For instance, 2 | 3 | ```python 4 | 5 | import time 6 | import yappi 7 | 8 | from pypot.creatures import PoppyErgoJr 9 | jr = PoppyErgoJr() 10 | 11 | yappi.start();time.sleep(100); yappi.stop() 12 | yappi.get_func_stats().save('ergo-jr-rpi2-idle-light-syncloop.prof', type='pstat') 13 | 14 | ``` 15 | 16 | The stats can be visualized using [snakeviz](http://jiffyclub.github.io/snakeviz/?utm_content=buffer81a13&utm_medium=social&utm_source=twitter.com&utm_campaign=buffer). 17 | -------------------------------------------------------------------------------- /samples/remoterobot-server.py: -------------------------------------------------------------------------------- 1 | import argparse 2 | 3 | import zerorpc 4 | 5 | from pypot.robot import from_json 6 | from pypot.server import RESTRobot 7 | 8 | 9 | if __name__ == '__main__': 10 | parser = argparse.ArgumentParser() 11 | parser.add_argument('-c', '--config-file', type=str, required=True) 12 | parser.add_argument('--host', type=str, default='0.0.0.0') 13 | parser.add_argument('-p', '--port', type=int, default=4242) 14 | args = parser.parse_args() 15 | 16 | robot = from_json(args.config_file) 17 | robot.start_sync() 18 | 19 | rest_robot = RESTRobot(robot) 20 | 21 | server = zerorpc.Server(rest_robot) 22 | print(args.host, args.port) 23 | server.bind('tcp://{}:{}'.format(args.host, args.port)) 24 | server.run() 25 | -------------------------------------------------------------------------------- /samples/test_connection.py: -------------------------------------------------------------------------------- 1 | import argparse 2 | import numpy 3 | import time 4 | import sys 5 | 6 | import pypot.dynamixel 7 | 8 | N = 1000 9 | 10 | 11 | def read_register(dxl, register, ids): 12 | print('\tReading {} times the {}...'.format(N, register)) 13 | 14 | t = [] 15 | getter = getattr(dxl, 'get_{}'.format(register)) 16 | 17 | for _ in range(N): 18 | start = time.time() 19 | getter(ids) 20 | end = time.time() 21 | t.append(1000 * (end - start)) 22 | 23 | print('\tTook {}ms (STD={}) per read'.format(numpy.mean(t), numpy.std(t))) 24 | 25 | 26 | def full_test(dxl, ids): 27 | print('Testing the communication speed with motor{} {}'.format('s' if len(ids) else '', 28 | ids)) 29 | read_register(dxl, 'present_position', ids) 30 | read_register(dxl, 'control_table', ids) 31 | 32 | 33 | if __name__ == '__main__': 34 | available_ports = pypot.dynamixel.get_available_ports() 35 | 36 | parser = argparse.ArgumentParser(formatter_class=argparse.ArgumentDefaultsHelpFormatter, 37 | description='Test low-level communication with Dynamixel motors.') 38 | 39 | parser.add_argument('-p', '--ports', 40 | type=str, nargs='+', 41 | default=available_ports, 42 | help='Select which port(s) to test') 43 | 44 | parser.add_argument('-b', '--baudrate', 45 | type=int, choices=[57600, 1000000], default=1000000, 46 | help='Sets the baudrate') 47 | 48 | args = parser.parse_args() 49 | 50 | for port in args.ports: 51 | print('Now testing port {} with {}bds...'.format(port, args.baudrate)) 52 | 53 | print('Opening connection...') 54 | with pypot.dynamixel.DxlIO(port, baudrate=args.baudrate) as dxl: 55 | print('Ok!') 56 | print('Scanning the bus...',) 57 | sys.stdout.flush() 58 | ids = dxl.scan() 59 | print('Done!') 60 | print('Ids found: {}'.format(ids)) 61 | 62 | full_test(dxl, ids[:1]) 63 | full_test(dxl, ids) 64 | 65 | print('Closing port') 66 | -------------------------------------------------------------------------------- /setup.cfg: -------------------------------------------------------------------------------- 1 | [flake8] 2 | ignore = E501,E731,E741 3 | max-line-length = 160 4 | exclude = build,dist,*.egg-info,doc/*,tests/*,*remoteApiBindings/vrepConst.py,*remoteApiBindings/vrep.py,__init__.py,*samples/benchmarks/*,*utils/appdirs.py 5 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from io import open 4 | import re 5 | import os 6 | import sys 7 | 8 | from setuptools import setup, find_packages 9 | 10 | 11 | def version(): 12 | with open('pypot/_version.py') as f: 13 | return re.search(r"^__version__ = ['\"]([^'\"]*)['\"]", f.read()).group(1) 14 | 15 | install_requires = ['numpy', 16 | 'pyserial>2.6', 17 | 'tornado', 18 | 'scipy', 19 | 'ikpy==3.0.1', 20 | 'bottle', 21 | 'requests', 22 | 'opencv-contrib-python', 23 | 'wget', 24 | ] 25 | 26 | if sys.version_info < (3, 5): 27 | print("python version < 3.5 is not supported") 28 | sys.exit(1) 29 | 30 | def package_files(directory): 31 | paths = [] 32 | for (path, directories, filenames) in os.walk(directory): 33 | for filename in filenames: 34 | full_path = os.path.join(path, filename) 35 | paths.append((path, [full_path])) 36 | return paths 37 | 38 | 39 | setup(name='pypot', 40 | version=version(), 41 | packages=find_packages(), 42 | 43 | install_requires=install_requires, 44 | 45 | extras_require={ 46 | 'doc': ['sphinx', 'sphinxjp.themes.basicstrap', 'sphinx-bootstrap-theme'], 47 | 'zmq-server': ['zmq'], 48 | 'remote-robot': ['zerorpc'], 49 | 'camera': ['hampy', 'zmq'], # Extras require: opencv (not a PyPi packet) 50 | 'tests': ['requests', 'websocket-client', 'poppy-ergo-jr'], 51 | }, 52 | 53 | entry_points={ 54 | 'console_scripts': [ 55 | 'dxl-config = pypot.tools.dxlconfig:main', 56 | 'poppy-services=pypot.creatures.services_launcher:main', 57 | 'poppy-configure=pypot.creatures.configure_utility:main', 58 | ], 59 | }, 60 | 61 | include_package_data=True, 62 | exclude_package_data={'': ['.gitignore']}, 63 | 64 | zip_safe=False, 65 | 66 | author='See https://github.com/poppy-project/pypot/graphs/contributors', 67 | author_email='dev@poppy-station.org', 68 | description='Python 3 Library for Robot Control', 69 | long_description=open('README.md', encoding='utf-8').read(), 70 | url='https://github.com/poppy-project/pypot', 71 | license='GNU GENERAL PUBLIC LICENSE Version 3', 72 | 73 | classifiers=[ 74 | "Programming Language :: Python :: 3", 75 | "Topic :: Scientific/Engineering", ], 76 | ) 77 | -------------------------------------------------------------------------------- /tests/test_crashed_prim.py: -------------------------------------------------------------------------------- 1 | import unittest 2 | 3 | from pypot.robot.controller import MotorsController 4 | 5 | 6 | class ShittyController(MotorsController): 7 | def setup(self): 8 | raise Exception("Sorry I didn't do it on purpose...") 9 | 10 | 11 | class TestPrimLifeCycle(unittest.TestCase): 12 | def test_crashed_at_setup(self): 13 | sc = ShittyController(None, [], 50.) 14 | with self.assertRaises(RuntimeError): 15 | sc.start() 16 | 17 | 18 | if __name__ == '__main__': 19 | unittest.main() 20 | -------------------------------------------------------------------------------- /tests/test_dummy.py: -------------------------------------------------------------------------------- 1 | import unittest 2 | 3 | from pypot.creatures import PoppyErgoJr 4 | from pypot.primitive import LoopPrimitive 5 | 6 | 7 | class EmptyPrim(LoopPrimitive): 8 | def setup(self): 9 | pass 10 | 11 | def update(self): 12 | pass 13 | 14 | def teardown(self): 15 | pass 16 | 17 | 18 | class TestDummy(unittest.TestCase): 19 | def setUp(self): 20 | self.jr = PoppyErgoJr(simulator='dummy') 21 | 22 | def test_dummy_controller(self): 23 | for m in self.jr.motors: 24 | m.moving_speed = 10000 25 | m.goal_position = 25 26 | 27 | # Make sure it was synced 28 | self.jr._controllers[0]._updated.clear() 29 | self.jr._controllers[0]._updated.wait() 30 | 31 | for m in self.jr.motors: 32 | self.assertEqual(m.goal_position, m.present_position) 33 | 34 | def test_empty_primitive(self): 35 | p = EmptyPrim(self.jr, 50.0) 36 | p.start() 37 | p.stop() 38 | 39 | def tearDown(self): 40 | self.jr.close() 41 | 42 | 43 | if __name__ == '__main__': 44 | unittest.main() 45 | -------------------------------------------------------------------------------- /tests/test_ik.py: -------------------------------------------------------------------------------- 1 | import unittest 2 | 3 | from pypot.creatures import PoppyErgoJr 4 | 5 | 6 | class TestIK(unittest.TestCase): 7 | def test_lowerlimit_correctly_setup(self): 8 | self.jr = PoppyErgoJr(simulator='poppy-simu') 9 | self.jr.close() 10 | # TODO: We should also make a unit test with a real/vrep robot. 11 | 12 | 13 | if __name__ == '__main__': 14 | unittest.main() 15 | -------------------------------------------------------------------------------- /tests/test_import.py: -------------------------------------------------------------------------------- 1 | import os 2 | import random 3 | import unittest 4 | 5 | from setuptools import find_packages 6 | 7 | 8 | class TestImport(unittest.TestCase): 9 | def setUp(self): 10 | self.packages = find_packages('../') 11 | 12 | if os.uname()[-1].startswith('arm'): 13 | self.packages = [p for p in self.packages 14 | if not p.startswith('pypot.vrep')] 15 | 16 | random.shuffle(self.packages) 17 | 18 | def test_import(self): 19 | [__import__(package) for package in self.packages] 20 | 21 | 22 | if __name__ == '__main__': 23 | unittest.main() 24 | -------------------------------------------------------------------------------- /tests/test_primitive.py: -------------------------------------------------------------------------------- 1 | import unittest 2 | import random 3 | import time 4 | 5 | from threading import Event 6 | 7 | from pypot.creatures import PoppyErgoJr 8 | from pypot.primitive import LoopPrimitive 9 | 10 | 11 | class TestPrimLifeCycle(unittest.TestCase): 12 | def setUp(self): 13 | self.jr = PoppyErgoJr(simulator='dummy') 14 | 15 | def tearDown(self): 16 | self.jr.close() 17 | 18 | def test_teardown(self): 19 | self.jr.dance.start() 20 | time.sleep(random.random() * 5) 21 | self.jr.dance.stop() 22 | 23 | self.assertEqual({m.led for m in self.jr.motors}, {'off'}) 24 | self.assertEqual({m.led for m in self.jr.dance.robot.motors}, {'off'}) 25 | 26 | def test_set_once(self): 27 | class Switcher(LoopPrimitive): 28 | def setup(self): 29 | self.current_state = False 30 | self.old_state = self.current_state 31 | 32 | self.switched = Event() 33 | 34 | def update(self): 35 | if self.current_state != self.old_state: 36 | for m in self.robot.motors: 37 | self.affect_once(m, 'led', 38 | 'red' if self.current_state else 'off') 39 | 40 | self.old_state = self.current_state 41 | 42 | self.switched.set() 43 | 44 | p = Switcher(self.jr, 10) 45 | p.start() 46 | 47 | for m in self.jr.motors: 48 | m.led = 'off' 49 | 50 | self.jr.m3.led = 'pink' 51 | 52 | self.assertEqual([m.led for m in self.jr.motors], 53 | ['off', 'off', 'pink', 'off', 'off', 'off']) 54 | 55 | p.switched.clear() 56 | p.current_state = not p.current_state 57 | p.switched.wait() 58 | 59 | self.assertEqual([m.led for m in self.jr.motors], 60 | ['red', 'red', 'red', 'red', 'red', 'red']) 61 | 62 | self.jr.m3.led = 'blue' 63 | self.assertEqual([m.led for m in self.jr.motors], 64 | ['red', 'red', 'blue', 'red', 'red', 'red']) 65 | 66 | p.stop() 67 | 68 | def test_start_pause_stop(self): 69 | self.jr.dance.start() 70 | self.jr.dance.pause() 71 | self.jr.dance.stop() 72 | 73 | def test_start_stop_pause_resume_random_order(self): 74 | cmd = ['start', 'stop', 'pause', 'resume'] 75 | 76 | for _ in range(10): 77 | getattr(self.jr.dance, random.choice(cmd))() 78 | 79 | 80 | if __name__ == '__main__': 81 | unittest.main() 82 | -------------------------------------------------------------------------------- /tests/test_snap.py: -------------------------------------------------------------------------------- 1 | import unittest 2 | import requests 3 | import random 4 | import time 5 | 6 | from pypot.creatures import PoppyErgoJr 7 | from pypot.dynamixel.conversion import XL320LEDColors 8 | 9 | from utils import get_open_port 10 | 11 | 12 | class TestSnap(unittest.TestCase): 13 | def setUp(self): 14 | port = get_open_port() 15 | 16 | self.jr = PoppyErgoJr(simulator='poppy-simu', use_snap=True, snap_port=port) 17 | self.base_url = 'http://127.0.0.1:{}'.format(port) 18 | 19 | # Make sure the Snap API is running before actually testing. 20 | while True: 21 | try: 22 | self.get('/') 23 | break 24 | except requests.exceptions.ConnectionError: 25 | time.sleep(1) 26 | 27 | def get(self, url): 28 | url = '{}{}'.format(self.base_url, url) 29 | return requests.get(url) 30 | 31 | def test_led(self): 32 | c = random.choice(list(XL320LEDColors)) 33 | m = random.choice(self.jr.motors) 34 | 35 | r = self.get('/motor/{}/set/led/{}'.format(m.name, c.name)) 36 | self.assertEqual(r.status_code, 200) 37 | 38 | r = self.get('/motor/{}/get/led'.format(m.name)) 39 | self.assertEqual(r.status_code, 200) 40 | self.assertEqual(r.text, c.name) 41 | 42 | 43 | if __name__ == '__main__': 44 | unittest.main() 45 | -------------------------------------------------------------------------------- /tests/test_websocket.py: -------------------------------------------------------------------------------- 1 | import json 2 | import time 3 | import unittest 4 | import websocket 5 | 6 | from pypot.creatures import PoppyErgoJr 7 | from utils import get_open_port 8 | 9 | 10 | @unittest.skip("tornado.ioloop from jr.ws as well as js.http must be stopped by jr.close() before launching new jr instances") 11 | class TestWebsocketsCommunication(unittest.TestCase): 12 | """docstring for TestWebsocketsCommunication""" 13 | def setUp(self): 14 | port = get_open_port() 15 | self.jr = PoppyErgoJr(simulator='poppy-simu', use_ws=True, ws_port=port) 16 | 17 | self.ws_url = 'ws://127.0.0.1:{}'.format(port) 18 | 19 | while True: 20 | try: 21 | self.ws = websocket.WebSocket() 22 | self.ws.connect(self.ws_url) 23 | break 24 | except ConnectionError: 25 | time.sleep(1.0) 26 | 27 | def tearDown(self): 28 | self.ws.close() 29 | 30 | def test_connected(self): 31 | self.assertTrue(self.ws.connected) 32 | 33 | def test_recv_state(self): 34 | state = json.loads(self.ws.recv()) 35 | self.assertSetEqual(set(state.keys()), 36 | {m.name for m in self.jr.motors}) 37 | 38 | def test_led(self): 39 | obj = { 40 | 'm1': { 41 | 'led': 'red' 42 | } 43 | } 44 | self.ws.send(json.dumps(obj)) 45 | 46 | 47 | if __name__ == '__main__': 48 | unittest.main() 49 | -------------------------------------------------------------------------------- /tests/utils.py: -------------------------------------------------------------------------------- 1 | import socket 2 | 3 | from contextlib import closing 4 | 5 | 6 | def get_open_port(): 7 | with closing(socket.socket(socket.AF_INET, socket.SOCK_STREAM)) as s: 8 | s.bind(("", 0)) 9 | s.listen(1) 10 | port = s.getsockname()[1] 11 | return port 12 | --------------------------------------------------------------------------------