reachy-mini 1.2.5rc1__py3-none-any.whl

reachy_mini/tools/reflash_motors.py

@@ -0,0 +1,116 @@
+"""Script to reflash Reachy Mini's motors firmware."""
+
+import argparse
+from importlib.resources import files
+from typing import Optional
+
+import questionary
+from rich.console import Console
+
+import reachy_mini
+from reachy_mini.daemon.utils import find_serial_port
+from reachy_mini.tools.setup_motor import (
+    check_configuration,
+    light_led_down,
+    light_led_up,
+    setup_motor,
+)
+from reachy_mini.utils.hardware_config.parser import parse_yaml_config
+
+BAUDRATE = 1000000
+
+
+def main() -> None:
+    """Entry point for the reflash_motors script."""
+    parser = argparse.ArgumentParser(
+        description="Reflash Reachy Mini motors' firmware.",
+    )
+    parser.add_argument(
+        "--serialport",
+        type=str,
+        required=False,
+        default=None,
+        help="Serial port of the Reachy Mini (e.g. /dev/ttyUSB0 or COM3). "
+        "If not specified, the script will try to automatically find it.",
+    )
+    args = parser.parse_args()
+    reflash_motors(args.serialport)
+
+
+def reflash_motors(
+    serialport: Optional[str] = None, dont_light_up: bool = False
+) -> None:
+    """Reflash Reachy Mini's motors."""
+    console = Console()
+
+    config_file_path = str(
+        files(reachy_mini).joinpath("assets/config/hardware_config.yaml")
+    )
+    config = parse_yaml_config(config_file_path)
+    motors = list(config.motors.keys())
+    if serialport is None:
+        console.print(
+            "Which version of Reachy Mini are you using?",
+        )
+        wireless_choice = questionary.select(
+            ">",
+            [
+                questionary.Choice("Lite", value=False),
+                questionary.Choice("Wireless", value=True),
+            ],
+        ).ask()
+        ports = find_serial_port(wireless_version=wireless_choice)
+
+        if len(ports) == 0:
+            raise RuntimeError(
+                "No Reachy Mini serial port found. "
+                "Check USB connection and permissions. "
+                "Or directly specify the serial port using --serialport."
+            )
+        elif len(ports) > 1:
+            raise RuntimeError(
+                f"Multiple Reachy Mini serial ports found {ports}."
+                "Please specify the serial port using --serialport."
+            )
+
+        serialport = ports[0]
+        console.print(f"Found Reachy Mini serial port: {serialport}", style="green")
+
+    for motor_name in motors:
+        motor_config = config.motors[motor_name]
+
+        from_id = motor_config.id
+
+        setup_motor(
+            motor_config,
+            serialport,
+            from_id=from_id,
+            from_baudrate=config.serial.baudrate,
+            target_baudrate=config.serial.baudrate,
+        )
+
+        try:
+            check_configuration(
+                motor_config,
+                serialport,
+                baudrate=config.serial.baudrate,
+            )
+        except RuntimeError as e:
+            console.print(
+                f"❌ Configuration check failed for motor '{motor_name}': {e}",
+                style="red",
+            )
+            return
+
+        light_led_up(
+            serialport,
+            motor_config.id,
+            baudrate=config.serial.baudrate,
+        )
+
+        if dont_light_up:
+            light_led_down(
+                serialport,
+                motor_config.id,
+                baudrate=config.serial.baudrate,
+            )

reachy_mini/tools/setup_motor.py

@@ -0,0 +1,422 @@
+"""Motor setup script for the Reachy Mini robot.
+
+This script allows to configure the motors of the Reachy Mini robot by setting their ID, baudrate, offset, angle limits, return delay time, and removing the input voltage error.
+
+The motor needs to be configured one by one, so you will need to connect only one motor at a time to the serial port. You can specify which motor to configure by passing its name as an argument.
+
+If not specified, it assumes the motor is in the factory settings (ID 1 and baudrate 57600). If it's not the case, you will need to use a tool like Dynamixel Wizard to first reset it or manually specify the ID and baudrate.
+
+Please note that all values given in the configuration file are in the motor's raw units.
+"""
+
+import argparse
+import time
+from pathlib import Path
+
+from rustypot import Xl330PyController
+
+from reachy_mini.utils.hardware_config.parser import MotorConfig, parse_yaml_config
+
+FACTORY_DEFAULT_ID = 1
+FACTORY_DEFAULT_BAUDRATE = 57600
+SERIAL_TIMEOUT = 0.01  # seconds
+MOTOR_SETUP_DELAY = 0.1  # seconds
+
+XL_BAUDRATE_CONV_TABLE = {
+    9600: 0,
+    57600: 1,
+    115200: 2,
+    1000000: 3,
+    2000000: 4,
+    3000000: 5,
+    4000000: 6,
+}
+
+
+def setup_motor(
+    motor_config: MotorConfig,
+    serial_port: str,
+    from_baudrate: int,
+    target_baudrate: int,
+    from_id: int,
+) -> None:
+    """Set up the motor with the given configuration."""
+    if not lookup_for_motor(
+        serial_port,
+        from_id,
+        from_baudrate,
+    ):
+        raise RuntimeError(
+            f"No motor found on port {serial_port}. "
+            f"Make sure the motor is in factory settings (ID {from_id} and baudrate {from_baudrate}) and connected to the specified port."
+        )
+
+    # Make sure the torque is disabled to be able to write EEPROM
+    disable_torque(serial_port, from_id, from_baudrate)
+    try:
+        if from_baudrate != target_baudrate:
+            change_baudrate(
+                serial_port,
+                id=from_id,
+                base_baudrate=from_baudrate,
+                target_baudrate=target_baudrate,
+            )
+            time.sleep(MOTOR_SETUP_DELAY)
+
+        if from_id != motor_config.id:
+            change_id(
+                serial_port,
+                current_id=from_id,
+                new_id=motor_config.id,
+                baudrate=target_baudrate,
+            )
+            time.sleep(MOTOR_SETUP_DELAY)
+
+        change_offset(
+            serial_port,
+            id=motor_config.id,
+            offset=motor_config.offset,
+            baudrate=target_baudrate,
+        )
+
+        time.sleep(MOTOR_SETUP_DELAY)
+
+        change_angle_limits(
+            serial_port,
+            id=motor_config.id,
+            angle_limit_min=motor_config.angle_limit_min,
+            angle_limit_max=motor_config.angle_limit_max,
+            baudrate=target_baudrate,
+        )
+
+        time.sleep(MOTOR_SETUP_DELAY)
+
+        change_shutdown_error(
+            serial_port,
+            id=motor_config.id,
+            baudrate=target_baudrate,
+            shutdown_error=motor_config.shutdown_error,
+        )
+
+        time.sleep(MOTOR_SETUP_DELAY)
+
+        change_return_delay_time(
+            serial_port,
+            id=motor_config.id,
+            return_delay_time=motor_config.return_delay_time,
+            baudrate=target_baudrate,
+        )
+
+        time.sleep(MOTOR_SETUP_DELAY)
+
+        change_operating_mode(
+            serial_port,
+            id=motor_config.id,
+            operating_mode=motor_config.operating_mode,
+            baudrate=target_baudrate,
+        )
+
+        time.sleep(MOTOR_SETUP_DELAY)
+    except Exception as e:
+        print(f"Error while setting up motor ID {from_id}: {e}")
+        raise e
+
+
+def lookup_for_motor(
+    serial_port: str, id: int, baudrate: int, silent: bool = False
+) -> bool:
+    """Check if a motor with the given ID is reachable on the specified serial port."""
+    if not silent:
+        print(
+            f"Looking for motor with ID {id} on port {serial_port}...",
+            end="",
+            flush=True,
+        )
+    c = Xl330PyController(serial_port, baudrate=baudrate, timeout=SERIAL_TIMEOUT)
+    ret = c.ping(id)
+    if not silent:
+        print(f"{'✅' if ret else '❌'}")
+    return ret
+
+
+def disable_torque(serial_port: str, id: int, baudrate: int) -> None:
+    """Disable the torque of the motor with the given ID on the specified serial port."""
+    print(f"Disabling torque for motor with ID {id}...", end="", flush=True)
+    c = Xl330PyController(serial_port, baudrate=baudrate, timeout=SERIAL_TIMEOUT)
+    c.write_torque_enable(id, False)
+    print("✅")
+
+
+def change_baudrate(
+    serial_port: str, id: int, base_baudrate: int, target_baudrate: int
+) -> None:
+    """Change the baudrate of the motor with the given ID on the specified serial port."""
+    print(f"Changing baudrate to {target_baudrate}...", end="", flush=True)
+    c = Xl330PyController(serial_port, baudrate=base_baudrate, timeout=SERIAL_TIMEOUT)
+    c.write_baud_rate(id, XL_BAUDRATE_CONV_TABLE[target_baudrate])
+    print("✅")
+
+
+def change_id(serial_port: str, current_id: int, new_id: int, baudrate: int) -> None:
+    """Change the ID of the motor with the given current ID on the specified serial port."""
+    print(f"Changing ID from {current_id} to {new_id}...", end="", flush=True)
+    c = Xl330PyController(serial_port, baudrate=baudrate, timeout=SERIAL_TIMEOUT)
+    c.write_id(current_id, new_id)
+    print("✅")
+
+
+def change_offset(serial_port: str, id: int, offset: int, baudrate: int) -> None:
+    """Change the offset of the motor with the given ID on the specified serial port."""
+    print(f"Changing offset for motor with ID {id} to {offset}...", end="", flush=True)
+    c = Xl330PyController(serial_port, baudrate=baudrate, timeout=SERIAL_TIMEOUT)
+    c.write_homing_offset(id, offset)
+    print("✅")
+
+
+def change_operating_mode(
+    serial_port: str, id: int, operating_mode: int, baudrate: int
+) -> None:
+    """Change the operating mode of the motor with the given ID on the specified serial port."""
+    print(
+        f"Changing operating mode for motor with ID {id} to {operating_mode}...",
+        end="",
+        flush=True,
+    )
+    c = Xl330PyController(serial_port, baudrate=baudrate, timeout=SERIAL_TIMEOUT)
+    c.write_operating_mode(id, operating_mode)
+    print("✅")
+
+
+def change_angle_limits(
+    serial_port: str,
+    id: int,
+    angle_limit_min: int,
+    angle_limit_max: int,
+    baudrate: int,
+) -> None:
+    """Change the angle limits of the motor with the given ID on the specified serial port."""
+    print(
+        f"Changing angle limits for motor with ID {id} to [{angle_limit_min}, {angle_limit_max}]...",
+        end="",
+        flush=True,
+    )
+    c = Xl330PyController(serial_port, baudrate=baudrate, timeout=SERIAL_TIMEOUT)
+    c.write_raw_min_position_limit(id, angle_limit_min)
+    c.write_raw_max_position_limit(id, angle_limit_max)
+    print("✅")
+
+
+def change_shutdown_error(
+    serial_port: str, id: int, baudrate: int, shutdown_error: int
+) -> None:
+    """Change the shutdown error of the motor with the given ID on the specified serial port."""
+    print(
+        f"Changing shutdown error for motor with ID {id} to {shutdown_error}...",
+        end="",
+        flush=True,
+    )
+    c = Xl330PyController(serial_port, baudrate=baudrate, timeout=SERIAL_TIMEOUT)
+    c.write_shutdown(id, shutdown_error)
+    print("✅")
+
+
+def change_return_delay_time(
+    serial_port: str, id: int, return_delay_time: int, baudrate: int
+) -> None:
+    """Change the return delay time of the motor with the given ID on the specified serial port."""
+    print(
+        f"Changing return delay time for motor with ID {id} to {return_delay_time}...",
+        end="",
+        flush=True,
+    )
+    c = Xl330PyController(serial_port, baudrate=baudrate, timeout=SERIAL_TIMEOUT)
+    c.write_return_delay_time(id, return_delay_time)
+    print("✅")
+
+
+def light_led_up(serial_port: str, id: int, baudrate: int) -> None:
+    """Light the LED of the motor with the given ID on the specified serial port."""
+    c = Xl330PyController(serial_port, baudrate=baudrate, timeout=SERIAL_TIMEOUT)
+
+    trials = 0
+
+    while trials < 3:
+        try:
+            c.write_led(id, 1)
+            break
+        except RuntimeError as e:
+            print(f"Error while turning on LED for motor ID {id}: {e}")
+        trials += 1
+
+
+def light_led_down(serial_port: str, id: int, baudrate: int) -> None:
+    """Light the LED of the motor with the given ID on the specified serial port."""
+    c = Xl330PyController(serial_port, baudrate=baudrate, timeout=SERIAL_TIMEOUT)
+    trials = 0
+
+    while trials < 3:
+        try:
+            c.write_led(id, 0)
+            break
+        except RuntimeError as e:
+            print(f"Error while turning off LED for motor ID {id}: {e}")
+        trials += 1
+
+
+def check_configuration(
+    motor_config: MotorConfig, serial_port: str, baudrate: int
+) -> None:
+    """Check the configuration of the motor with the given ID on the specified serial port."""
+    c = Xl330PyController(serial_port, baudrate=baudrate, timeout=SERIAL_TIMEOUT)
+
+    print("Checking configuration...")
+
+    # Check if there is a motor with the desired ID
+    if not c.ping(motor_config.id):
+        raise RuntimeError(f"No motor with ID {motor_config.id} found, cannot proceed")
+    print(f"Found motor with ID {motor_config.id} ✅.")
+
+    # Read return delay time
+    return_delay = c.read_return_delay_time(motor_config.id)[0]
+    if return_delay != motor_config.return_delay_time:
+        raise RuntimeError(
+            f"Return delay time is {return_delay}, expected {motor_config.return_delay_time}"
+        )
+    print(f"Return delay time is correct: {return_delay} ✅.")
+
+    # Read operating mode
+    operating_mode = c.read_operating_mode(motor_config.id)[0]
+    if operating_mode != motor_config.operating_mode:
+        raise RuntimeError(
+            f"Operating mode is {operating_mode}, expected {motor_config.operating_mode}"
+        )
+    print(f"Operating mode is correct: {operating_mode} ✅.")
+
+    # Read angle limits
+    angle_limit_min = c.read_raw_min_position_limit(motor_config.id)[0]
+    angle_limit_max = c.read_raw_max_position_limit(motor_config.id)[0]
+    if angle_limit_min != motor_config.angle_limit_min:
+        raise RuntimeError(
+            f"Angle limit min is {angle_limit_min}, expected {motor_config.angle_limit_min}"
+        )
+    if angle_limit_max != motor_config.angle_limit_max:
+        raise RuntimeError(
+            f"Angle limit max is {angle_limit_max}, expected {motor_config.angle_limit_max}"
+        )
+    print(
+        f"Angle limits are correct: [{motor_config.angle_limit_min}, {motor_config.angle_limit_max}] ✅."
+    )
+
+    # Read homing offset
+    offset = c.read_homing_offset(motor_config.id)[0]
+    if offset != motor_config.offset:
+        raise RuntimeError(f"Homing offset is {offset}, expected {motor_config.offset}")
+    print(f"Homing offset is correct: {offset} ✅.")
+
+    # Read shutdown
+    shutdown = c.read_shutdown(motor_config.id)[0]
+    if shutdown != motor_config.shutdown_error:
+        raise RuntimeError(
+            f"Shutdown is {shutdown}, expected {motor_config.shutdown_error}"
+        )
+    print(f"Shutdown error is correct: {shutdown} ✅.")
+
+    print("Configuration is correct ✅!")
+
+
+def run(args: argparse.Namespace) -> None:
+    """Entry point for the Reachy Mini motor configuration tool."""
+    config = parse_yaml_config(args.config_file)
+
+    if args.motor_name == "all":
+        motors = list(config.motors.keys())
+    else:
+        motors = [args.motor_name]
+
+    for motor_name in motors:
+        motor_config = config.motors[motor_name]
+
+        if args.update_config:
+            args.from_id = motor_config.id
+            args.from_baudrate = config.serial.baudrate
+
+        if not args.check_only:
+            setup_motor(
+                motor_config,
+                args.serialport,
+                from_id=args.from_id,
+                from_baudrate=args.from_baudrate,
+                target_baudrate=config.serial.baudrate,
+            )
+
+        try:
+            check_configuration(
+                motor_config,
+                args.serialport,
+                baudrate=config.serial.baudrate,
+            )
+        except RuntimeError as e:
+            print(f"❌ Configuration check failed for motor '{motor_name}': {e}")
+            return
+
+        light_led_up(
+            args.serialport,
+            motor_config.id,
+            baudrate=config.serial.baudrate,
+        )
+
+
+if __name__ == "__main__":
+    """Entry point for the Reachy Mini motor configuration tool."""
+    parser = argparse.ArgumentParser(description="Motor Configuration tool")
+    parser.add_argument(
+        "config_file",
+        type=Path,
+        help="Path to the hardware configuration file (default: hardware_config.yaml).",
+    )
+    parser.add_argument(
+        "motor_name",
+        type=str,
+        help="Name of the motor to configure.",
+        choices=[
+            "body_rotation",
+            "stewart_1",
+            "stewart_2",
+            "stewart_3",
+            "stewart_4",
+            "stewart_5",
+            "stewart_6",
+            "right_antenna",
+            "left_antenna",
+            "all",
+        ],
+    )
+    parser.add_argument(
+        "serialport",
+        type=str,
+        help="Serial port for communication with the motor.",
+    )
+    parser.add_argument(
+        "--check-only",
+        action="store_true",
+        help="Only check the configuration without applying changes.",
+    )
+    parser.add_argument(
+        "--from-id",
+        type=int,
+        default=FACTORY_DEFAULT_ID,
+        help=f"Current ID of the motor (default: {FACTORY_DEFAULT_ID}).",
+    )
+    parser.add_argument(
+        "--from-baudrate",
+        type=int,
+        default=FACTORY_DEFAULT_BAUDRATE,
+        help=f"Current baudrate of the motor (default: {FACTORY_DEFAULT_BAUDRATE}).",
+    )
+    parser.add_argument(
+        "--update-config",
+        action="store_true",
+        help="Update a specific motor (assumes it already has the correct id and baudrate).",
+    )
+    args = parser.parse_args()
+    run(args)

reachy_mini/tools/setup_motor_rpi.py

@@ -0,0 +1,108 @@
+"""You need to `pip install gpiozero lgpio`."""
+
+import argparse
+import os
+import time
+from typing import List
+
+import numpy as np
+from gpiozero import DigitalOutputDevice
+from setup_motor import (
+    FACTORY_DEFAULT_BAUDRATE,
+    FACTORY_DEFAULT_ID,
+    light_led_down,
+    lookup_for_motor,
+    parse_yaml_config,
+    run,
+)
+
+assets_root_path = "../src/reachy_mini/assets/"
+
+
+UART_PORT = "/dev/ttyAMA3"
+CONFIG_FILE_PATH = os.path.join(assets_root_path, "config", "hardware_config.yaml")
+
+ID_TO_CHANNEL = {
+    10: 0,
+    11: 1,
+    12: 2,
+    13: 3,
+    14: 4,
+    15: 5,
+    16: 6,
+    17: 7,
+    18: 8,
+}
+CHANNEL_TO_ID = {v: k for k, v in ID_TO_CHANNEL.items()}
+
+S0 = DigitalOutputDevice(25)
+S1 = DigitalOutputDevice(8)
+S2 = DigitalOutputDevice(7)
+S3 = DigitalOutputDevice(1)
+
+
+def get_channel_binary(channel: int) -> List[int]:
+    """Convert channel number (0-8) to 4-bit binary representation."""
+    assert channel in np.arange(9), "Channel must be between 0 and 8"
+    bits = [int(b) for b in f"{channel:04b}"]  # 4-bit binary
+    return bits[::-1]  # flip the order
+
+
+def select_channel(channel: int) -> None:
+    """Select a channel on the multiplexer."""
+    bits = get_channel_binary(channel)
+    S0.on() if bits[0] else S0.off()
+    S1.on() if bits[1] else S1.off()
+    S2.on() if bits[2] else S2.off()
+    S3.on() if bits[3] else S3.off()
+
+
+def main() -> None:
+    """Scan all channels of the multiplexer to find motors in factory default state, and set them up one by one."""
+    config = parse_yaml_config(CONFIG_FILE_PATH)
+    motor_name_to_id = {m: config.motors[m].id for m in config.motors}
+    id_to_motor_name = {v: k for k, v in motor_name_to_id.items()}
+
+    print("Starting motor setup...")
+    current_channel = 0
+    while True:
+        current_channel = (current_channel + 1) % 9
+        select_channel(current_channel)
+        target_id = CHANNEL_TO_ID[current_channel]
+        target_name = id_to_motor_name[target_id]
+        if lookup_for_motor(
+            UART_PORT, FACTORY_DEFAULT_ID, FACTORY_DEFAULT_BAUDRATE, silent=True
+        ):
+            print(f"Found motor on channel {current_channel}!")
+            args = argparse.Namespace(
+                config_file=CONFIG_FILE_PATH,
+                motor_name=target_name,
+                serialport=UART_PORT,
+                check_only=False,
+                from_id=FACTORY_DEFAULT_ID,
+                from_baudrate=FACTORY_DEFAULT_BAUDRATE,
+                update_config=False,
+            )
+            run(args)
+
+        elif lookup_for_motor(UART_PORT, current_channel + 10, 1000000, silent=True):
+            print(f"Motor on channel {current_channel} already set up.")
+            # light_led_up(UART_PORT, current_channel+10, 1000000)
+            light_led_down(UART_PORT, current_channel + 10, 1000000)
+            args = argparse.Namespace(
+                config_file=CONFIG_FILE_PATH,
+                motor_name=target_name,
+                serialport=UART_PORT,
+                check_only=False,
+                from_id=current_channel + 10,
+                from_baudrate=1000000,
+                update_config=False,
+            )
+            run(args)
+            time.sleep(2)
+
+        time.sleep(0.01)
+
+
+if __name__ == "__main__":
+    main()

reachy_mini/utils/__init__.py

@@ -0,0 +1,46 @@
+"""Utility functions for Reachy Mini.
+
+These functions provide various utilities such as creating head poses, performing minimum jerk interpolation,
+checking if the Reachy Mini daemon is running, and performing linear pose interpolation.
+
+"""
+
+import numpy as np
+import numpy.typing as npt
+from scipy.spatial.transform import Rotation as R
+
+
+def create_head_pose(
+    x: float = 0,
+    y: float = 0,
+    z: float = 0,
+    roll: float = 0,
+    pitch: float = 0,
+    yaw: float = 0,
+    mm: bool = False,
+    degrees: bool = True,
+) -> npt.NDArray[np.float64]:
+    """Create a homogeneous transformation matrix representing a pose in 6D space (position and orientation).
+
+    Args:
+        x (float): X coordinate of the position.
+        y (float): Y coordinate of the position.
+        z (float): Z coordinate of the position.
+        roll (float): Roll angle
+        pitch (float): Pitch angle
+        yaw (float): Yaw angle
+        mm (bool): If True, convert position from millimeters to meters.
+        degrees (bool): If True, interpret roll, pitch, and yaw as degrees; otherwise as radians.
+
+    Returns:
+        np.ndarray: A 4x4 homogeneous transformation matrix representing the pose.
+
+    """
+    pose = np.eye(4)
+    rot = R.from_euler("xyz", [roll, pitch, yaw], degrees=degrees).as_matrix()
+    pose[:3, :3] = rot
+    pose[:, 3] = [x, y, z, 1]
+    if mm:
+        pose[:3, 3] /= 1000
+
+    return pose

reachy_mini/utils/constants.py

@@ -0,0 +1,9 @@
+"""Utility constants for the reachy_mini package."""
+
+from importlib.resources import files
+
+import reachy_mini
+
+URDF_ROOT_PATH: str = str(files(reachy_mini).joinpath("descriptions/reachy_mini/urdf"))
+ASSETS_ROOT_PATH: str = str(files(reachy_mini).joinpath("assets/"))
+MODELS_ROOT_PATH: str = str(files(reachy_mini).joinpath("assets/models"))

reachy_mini/utils/hardware_config/__init__.py

@@ -0,0 +1 @@
+"""Hardware configuration utilities."""