Rustypot: a Rust package to communicate with Dynamixel/Feetech motors

Getting started

Rustypot is a communication library for Dynamixel/Feetech motors. It is notably used in the Reachy project. More types of servo can be added in the future.

Feature Overview

• Relies on serialport for serial communication
• Support for dynamixel protocol v1 and v2 (can also use both on the same bus)
• Support for sync read and sync write operations, plus fast sync read (protocol v2, instruction 0x8A) to read a whole bus in a single status packet
• Easy support for new type of motors (register definition through macros). Currently support for dynamixel XL320, XL330, XL430, XM430, MX*, AX*, Orbita 2D & 3D.
• Pure Rust plus python bindings (using pyo3).

To add new servo, please refer to the Servo documentation.

Wizard

If you want to quickly setup/check motors configuration, you can directly use a terminal UI built on top of rustypot: https://github.com/pollen-robotics/rustypot_wizard

APIs

It exposes three APIs, from the wire up:

• DynamixelProtocolHandler: low-level API. It handles the serial communication and the Dynamixel protocol parsing. It can be used for fine-grained control of the shared bus with other communication.
• Controller: one per servo model, with a typed accessor for each register. Simpler and cleaner API but it takes full ownership of the io (it can still be shared if wrapped with a mutex for instance).
• Bus and ServoDefinition: a higher-level, family-agnostic API, designed mostly for the LeRobot library. Motors of several models and of both protocols on one port, registers by name, the facts about each servo, finding and setting up motors, and the torque of a whole bus. Some of its choices are LeRobot conventions rather than vendor rules; the controllers stay the per-servo API underneath.

See the examples below for usage.

Examples

use rustypot::{DynamixelProtocolHandler, servo::dynamixel::mx};
use std::time::Duration;

fn main() {
    let mut serial_port = serialport::new("/dev/ttyACM0", 1_000_000)
        .timeout(Duration::from_millis(10))
        .open()
        .expect("Failed to open port");

    let dph = DynamixelProtocolHandler::v1();

    loop {
        let pos =
            mx::read_present_position(&dph, serial_port.as_mut(), 11).expect("Communication error");
        println!("Motor 11 present position: {:?}", pos);
    }
}

use rustypot::servo::feetech::sts3215::STS3215Controller;
use std::time::Duration;

fn main() {
    let serial_port = serialport::new("/dev/ttyUSB0", 1_000_000)
        .timeout(Duration::from_millis(1000))
        .open()
        .unwrap();

    let mut c = STS3215Controller::new()
            .with_protocol_v1()
            .with_serial_port(serial_port);

    let pos = c.sync_read_present_position(&vec![1, 2]).unwrap();
    println!("Motors present position: {:?}", pos);

    c.sync_write_goal_position(&vec![1, 2], &vec![1000, 2000]).unwrap();
}

Tools

Simple bus scanning tool:

cargo run --bin=scan -- --serialport=/dev/ttyUSB0 --baudrate=1000000 --protocol=v1

Documentation

See https://docs.rs/rustypot for more information on APIs and examples.

See python/README.md for information on how to use the python bindings.

Python bindings

The python bindings are generated using pyo3. They are available on pypi(https://pypi.org/project/rustypot/). You can install them using pip.

pip install rustypot

To build them locally, you can use maturin.

First, generate the type annotations for the python bindings, by running:

cargo run --release --bin stub_gen --features python

Then, you can build the python bindings using maturin. You can either build the wheel files to distribute them, or install them directly in your local python environment.

To build the wheel files, you can run:

maturin build --release --features python --features pyo3/extension-module

or, if you want to install them in your local python environment:

maturin develop --release --features python --features pyo3/extension-module

See maturin official documentation for more information on how to use it.

Using the Python bindings

The Python bindings exposes the same API as the Controller API in the rust crate.

You first need to create a Controller object. For instance, to communicate with a serial port to Feetech STS3215 motors, you can do the following:

from rustypot import Sts3215PyController

c = Sts3215PyController(serial_port='/dev/ttyUSB0', baudrate=1_000_000, timeout=0.1)

Both port settings can be changed later without closing it, with c.set_baudrate(500_000) and c.set_timeout(0.01) (seconds).

To find out what is on the bus, c.scan() returns {id: model number} for every id that answers among those the protocol allows (c.scan(ids) for a few), under a read timeout sized to the baud rate so that the absent ones go by quickly.

Then, you can directly read/write any register of the motor. For instance, to read the present position of the motor with id 1, you can do:

pos = c.read_present_position(1)
print(pos)

You can also write to the motors. For instance, to set the goal position of the motors with id 1 to 90° you can do:

import numpy as np
c.write_goal_position(1, np.deg2rad(90.0))

Then, you can also sync_read any registers on multiple motors in a single operations. For instance, to read the present position of the motors with id 1 and 2, you can do:

pos = c.sync_read_present_position([1, 2])
print(pos)

Same with sync_write. For instance, to set the goal position of the motors with id 1 and 2 to 0.0 and 90° respectively, you can do:

import numpy as np
c.sync_write_goal_position([1, 2], [0.0, np.deg2rad(90.0)])

Register tables

Each controller class carries its servo's control table, reachable without a serial port. registers() lists every register in declaration order, and register(name) looks one up by the name its accessors use (present_position for read_present_position). Paired with the raw-address methods, this lets code pick registers at runtime instead of hardcoding addresses:

from rustypot import RegisterAccess, Sts3215PyController

for r in Sts3215PyController.registers():
    print(r.name, r.addr, r.size, r.access)

pos = Sts3215PyController.register("present_position")
assert pos.access == RegisterAccess.Read
raw = c.read_raw_data(1, pos.addr, pos.size)

register() returns None for a name the servo does not define.

For an integer, the name is enough: read_register, write_register, sync_read_register and sync_write_register (and read_register_with_error, write_register_with_error, sync_read_register_with_error, which also hand back the status error byte) take the register's name and apply its byte order and sign encoding, so a Feetech homing offset comes back as -709 rather than as the bytes of 0x0AC5:

offset = c.read_register(1, "homing_offset")
c.write_register(1, "goal_position", -100)
positions = c.sync_read_register([1, 2], "present_position")
c.sync_write_register([1, 2], "goal_position", [0, 2047])

A value that does not fit the register, or a name the servo does not define, raises before anything reaches the bus.

A raw read hands back bytes, and what they mean is on the class too: word_order() gives the byte order ("little" or "big"), each register's encoding says whether the value is "unsigned", "twos_complement" or "sign_magnitude" (with sign_bit for the last), resolution() is the number of steps per turn, baudrates() maps a baud rate to the value of the baud rate register, supports_sync_read() tells whether the firmware answers Sync Read, and models() lists the model numbers the definition covers:

info = Sts3215PyController
info.models()                      # {'SM8512BL': 11272, 'STS3036': 521, 'STS3215': 777, 'STS3250': 2825}
info.resolution(), info.word_order()  # (4096, 'little')
info.register("present_position").sign_bit  # 15

Mixed buses, and the LeRobot-oriented API

Bus and ServoDefinition are the higher-level, family-agnostic API (see APIs), designed mostly for the LeRobot library; the controllers above stay the per-servo API.

A controller speaks one servo definition. When a port carries motors of several definitions, or of both protocols (Reachy Mini has STS3215 motors on protocol v1 and XL330 motors on v2 on one port), open a Bus with the definition of each motor:

from rustypot import Bus, Sts3215PyController, Xl330PyController

bus = Bus("/dev/ttyUSB0", 1_000_000, 0.1, {
    11: Sts3215PyController.definition(),
    1: Xl330PyController.definition(),
    2: Xl330PyController.definition(),
})
positions = bus.sync_read_register([11, 1, 2], "present_position", retries=1)
bus.sync_write_register([1, 2], "goal_position", [0, 2047])
found = bus.scan(Xl330PyController.definition())  # the v2 motors, as {id: model number}

Each motor is read and written through its own definition and protocol. A sync read or write sends one instruction per group of motors sharing a protocol and the register's address and size, and hands the values back in the order asked: here, one Sync Read for motor 11 and one for motors 1 and 2. A group with a motor that has no Sync Read is read one id at a time.

Beyond register access, they carry what a library driving several motor families needs:

• find_model("xl330-m288") returns the model number and the definition, whose getters give the servo's facts (resolution, baudrates, factory_baudrate, homing_offset_sign, operating_modes, ...);
• bus.broadcast_scan(definition) finds the motors on the port with one broadcast ping, and bus.change_id / bus.change_baudrate set a new motor up;
• bus.set_torque(ids, enabled) turns the torque of every motor on or off, and reports the ones that failed;
• with Bus(...) as bus: releases the port on the way out.

Some of these follow LeRobot's conventions rather than a vendor's: set_torque closes the lock of a Feetech servo when the torque goes on and opens it when it goes off.

In Rust the same access is on each definition, taking the protocol handler and the port like the typed module functions: sts3215::DEFINITION.read_register(&dph_v1, port, 11, "present_position"). servo::definition::sync_read_register and sync_write_register reach motors of several definitions sharing a protocol and a layout in one instruction, and bus::Bus groups them for a whole bus.

Threading and the GIL

Every binding releases the GIL for the duration of the serial transaction, so other Python threads -- a camera loop, a policy -- keep running while the bus is busy.

How much they actually get back depends on sys.setswitchinterval, which is 5 ms by default. That is the delay a thread waits before it can take the GIL from another, and on a bus transaction of a millisecond or two it dominates. Lowering it is worth testing in any threaded control loop:

import sys
sys.setswitchinterval(0.001)  # default is 0.005

On one six-motor chain at 1 Mbaud, with a second Python thread competing, this moved the bus from 126 Hz to 340 Hz without taking GIL time away from the other thread. The right value is workload-specific, so measure it rather than copying this one.

Contributing

If you want to contribute to Rustypot, please fork the repository and create a pull request. We welcome any contributions, including bug fixes, new features, and documentation improvements. We especially appreciate support for new servos. If you want to add support for a new servo, please follow the instructions in the Servo documentation.

License

This library is licensed under the Apache License 2.0.

Support

Rustypot is developed and maintained by Pollen-Robotics. They developed open-source hardware and tools for robotics. Visit https://pollen-robotics.com to learn more or join the Discord community if you have any questions or want to share your projects.