diff --git a/src/lerobot/robots/utils.py b/src/lerobot/robots/utils.py index 92da597f13c..a8b2532a67b 100644 --- a/src/lerobot/robots/utils.py +++ b/src/lerobot/robots/utils.py @@ -44,6 +44,10 @@ def make_robot_from_config(config: RobotConfig) -> Robot: from .lekiwi import LeKiwi return LeKiwi(config) + elif config.type == "lekiwi_base": + from .xlerobot.sub_robots.lekiwi_base import LeKiwiBase + + return LeKiwiBase(config) elif config.type == "hope_jr_hand": from .hope_jr import HopeJrHand @@ -72,6 +76,26 @@ def make_robot_from_config(config: RobotConfig) -> Robot: from tests.mocks.mock_robot import MockRobot return MockRobot(config) + elif config.type in ("biwheel_base", "biwheel_feetech"): + from .xlerobot.sub_robots.biwheel_base import BiwheelFeetech + + return BiwheelFeetech(config) + elif config.type == "biwheel_odrive": + from .xlerobot.sub_robots.biwheel_base import BiwheelODrive + + return BiwheelODrive(config) + elif config.type == "xlerobot_mount": + from .xlerobot.sub_robots.xlerobot_mount import XLeRobotMount + + return XLeRobotMount(config) + elif config.type == "panthera_arm": + from .xlerobot.sub_robots.panthera_arm import PantheraArm + + return PantheraArm(config) + elif config.type == "xlerobot": + from .xlerobot import XLeRobot + + return XLeRobot(config) else: try: return cast(Robot, make_device_from_device_class(config)) diff --git a/src/lerobot/robots/xlerobot/README.md b/src/lerobot/robots/xlerobot/README.md new file mode 100644 index 00000000000..017d17f998b --- /dev/null +++ b/src/lerobot/robots/xlerobot/README.md @@ -0,0 +1,471 @@ +# XLeRobot Modular Platform + +An example run with so101 arms: [video](https://youtu.be/OGI-Qtl3s6s) + +`xlerobot` is a configurable mobile manipulator platform composed from optional sub-robots: + +- **Arms** with support for dual SO-101/SO-100 follower arms or a single left/right `panthera_arm`. +- **Mobile base** abstraction with support for `lekiwi_base`, `biwheel_base`, `biwheel_feetech`, and `biwheel_odrive`. +- **Pan/Tilt camera mount** driven by Feetech servos. +- **Multi-camera rig** using the standard camera factory (robot-level and arm-attached cameras). +- **Shared bus or dedicated bus wiring** so Feetech components can share buses while ODrive/Panthera stay on dedicated connections. + +## Folder structure + +```text +src/lerobot/robots/xlerobot/ +|-- README.md +|-- __init__.py +|-- config_xlerobot.py +|-- xlerobot.py +|-- configs/ +| |-- base_only.json +| |-- xlerobot_biwheel_feetech.json +| `-- xlerobot_biwheel_odrive_panthera_left.json +|-- shared_bus_mode/ +| |-- component_assembly.py +| `-- shared_bus.py +`-- sub_robots/ + |-- __init__.py + |-- biwheel_base/ + | |-- config_biwheel_base.py + | |-- biwheel_base.py + | |-- biwheel_feetech.py + | `-- biwheel_odrive.py + |-- lekiwi_base/ + | |-- config.py + | `-- lekiwi_base.py + |-- panthera_arm/ + | |-- config.py + | `-- panthera_arm.py + `-- xlerobot_mount/ + |-- config.py + `-- xlerobot_mount.py +``` + +## The robot class + +`XLeRobot` orchestrates several sub-robots, each with its own configuration/handshake needs. The class: + +- Provides shared bus configs, injects IDs, and enforces that every component is routed through their declared shared bus (`shared_buses`). +- Bridges component observations and actions into a single namespace (`left_*`, `right_*`, `x.vel`, `mount_pan.pos`, …) for policies and scripts. +- Keeps the newest camera frame around in case a sensor read fails mid-run, which is crucial during mobile deployments. +- Provides safe connect/disconnect/calibration routines that cascade to all mounted components. +- Integrates with updated `lerobot-record`, `lerobot-replay`, and `lerobot-teleoperate` commands. No custom code required to capture trajectories or run inference. + +## Configuration example + +Use the config examples below directly with `lerobot-teleoperate`, or create an `XLeRobotConfig` instance with equivalent fields. + +Note, make sure on the shared buses, you have set the motor ID correctly. In subrobot's configs, the motor IDs index from 1. In the `shared_buses` field, the subrobot's IDs will be shifted by `motor_id_offset`. For example, the `pan_motor_id` for the `mount` will be 1 + 6 = 7. So, you would also need to set the FeeTech motor to be 7 using supported motor programming tool. This ensures the IDs do no collide with the other subrobots on the same bus. + +XLeRobot's default is to connect left arm and the mount on the same board, and right arm and the base on the same board. Refer to [doc](https://xlerobot.readthedocs.io/en/latest/hardware/getting_started/assemble.html) for more details. + +If you prefer to have each sub robot on a different board, configure each with a shared bus and you are good to go. + +```yaml +robot: + type: xlerobot + + left_arm: {id: xlerobot_arm_left} + right_arm: {id: xlerobot_arm_right} + base: + type: lekiwi_base + base_motor_ids: [2, 1, 3] + wheel_radius_m: 0.05 + base_radius_m: 0.125 + mount: + pan_motor_id: 1 + tilt_motor_id: 2 + motor_model: sts3215 + pan_key: "mount_pan.pos", + tilt_key: "mount_tilt.pos", + max_pan_speed_dps: 60.0, + max_tilt_speed_dps: 45.0, + pan_range: [-90.0, 90.0], + tilt_range: [-30.0, 60.0] + + cameras: + top: + type: opencv + index_or_path: 8 + width: 640 + height: 480 + fps: 30 + + shared_buses: + left_bus: + port: /dev/ttyACM2 + components: + - {component: left_arm} + - {component: mount, motor_id_offset: 6} + right_bus: + port: /dev/ttyACM3 + components: + - {component: right_arm} + - {component: base, motor_id_offset: 6} +``` + +With this config you can drive/record the platform via standard `lerobot-teleoperate`, `lerobot-record`, and `lerobot-replay`. + +Customize the base type (`lekiwi_base`, `biwheel_base`, `biwheel_feetech`, or `biwheel_odrive`), arm type (`so101_follower` or `panthera_arm`), mount gains, or camera set without editing Python. The config pipeline handles serialization, validation, and processor wiring for you. + +## Default configs + +Sample robot configuration files live in `src/lerobot/robots/xlerobot/configs/`. You can pass them directly to +`lerobot-teleoperate` via `--robot.config_file` and still override values on the CLI. The JSON files include a +`_comment` field that documents the intended CLI flag. + +```bash +lerobot-teleoperate \ + --robot.type=xlerobot \ + --robot.config_file=src/lerobot/robots/xlerobot/configs/xlerobot_biwheel_feetech.json \ + --teleop.type=xlerobot_default_composite \ + --teleop.config_file=src/lerobot/teleoperators/xlerobot_teleoperator/configs/xlerobot_default_composite_lekiwi.json \ + --teleop.base_type=biwheel_gamepad \ + --teleop.base='{"joystick_index": 0, "max_speed_mps": 0.8, "yaw_speed_deg": 45.0, "deadzone": 0.15}' \ + --robot.shared_buses.left_bus.port=/dev/ttyACM2 \ + --display_data=true +``` + +### Base-only (ODrive) + +If you only want the mobile base, use `base_only.json` as a reference. It disables arms/mount and configures +ODrive axis mapping and motor inversion (for example `invert_left_motor: true` and swapping `axis_left/right`) +so that `x.vel` drives forward instead of yawing. + +For both `biwheel_odrive` and `biwheel_feetech`, you can flip only forward/backward direction by setting +`base.reverse_front_direction: true` (or `--robot.base.reverse_front_direction=true` on CLI). +This keeps turning sign unchanged (`theta.vel` still uses the same left/right convention). + +```bash +lerobot-teleoperate \ + --robot.type=xlerobot \ + --robot.config_file=src/lerobot/robots/xlerobot/configs/base_only.json \ + --teleop.type=xlerobot_default_composite \ + --teleop.config_file=src/lerobot/teleoperators/xlerobot_teleoperator/configs/base_only.json \ + --display_data=true +``` + +### Base-only (Keyboard teleop) + +If you want keyboard control for the biwheel base, use the keyboard composite teleoperator: + +```bash +lerobot-teleoperate \ + --robot.type=xlerobot \ + --robot.config_file=src/lerobot/robots/xlerobot/configs/base_only.json \ + --teleop.type=xlerobot_keyboard_composite \ + --teleop.config_file=src/lerobot/teleoperators/xlerobot_teleoperator/configs/xlerobot_keyboard_composite_biwheel.json \ + --display_data=true +``` + +### Panthera arm + ODrive base (Keyboard teleop, Cartesian EE mapping) + +For a single left Panthera arm mounted on xlerobot: + +```bash +lerobot-teleoperate \ + --robot.type=xlerobot \ + --robot.config_file=src/lerobot/robots/xlerobot/configs/xlerobot_biwheel_odrive_panthera_left.json \ + --teleop.type=xlerobot_keyboard_composite \ + --teleop.config_file=src/lerobot/teleoperators/xlerobot_teleoperator/configs/xlerobot_keyboard_composite_panthera_left_ee.json \ + --display_data=true +``` +Panthera arms use a different motor/driver stack and must stay off `shared_buses` (`shared_bus: false`). +This example uses a single left Panthera arm. +This sample config enables Panthera joint impedance + gravity/friction compensation +(`left_arm.use_joint_impedance: true`). +It now also includes: + +- `left_arm.cameras.gripper_camera` (arm-attached camera) +- `cameras.base_camera` (robot-level/base camera) + +In observations these appear as `left_gripper_camera` and `base_camera`. +If your device indices differ, update `index_or_path` values in the JSON config before running. + +## Remote keyboard teleoperation with Rerun + +Use this when `lerobot-teleoperate` runs on Thor and Rerun runs on your local machine. + +### Linux local machine + +Start local viewer: + +```bash +rerun +``` + +Connect to Thor: + +```bash +ssh -Y thor@thor +``` + +Run teleop on Thor with your local viewer machine IP: + +```bash +lerobot-teleoperate \ + --robot.type=xlerobot \ + --robot.config_file=src/lerobot/robots/xlerobot/configs/xlerobot_biwheel_odrive_panthera_left.json \ + --teleop.type=xlerobot_keyboard_composite \ + --teleop.config_file=src/lerobot/teleoperators/xlerobot_teleoperator/configs/xlerobot_keyboard_composite_panthera_left_ee_with_base.json \ + --display_data=true \ + --fps=30 \ + --display_ip= \ + --display_port=9876 +``` + +### macOS local machine (XQuartz + RECORD + xterm keyboard capture) + +Install/start XQuartz: + +```bash +brew install --cask xquartz +open -a XQuartz +``` + +Enable test extensions and restart XQuartz: + +```bash +defaults write org.xquartz.X11 enable_test_extensions -boolean true +killall XQuartz +open -a XQuartz +``` + +Verify `RECORD` is available: + +```bash +/opt/X11/bin/xdpyinfo | grep -i RECORD +``` + +If `RECORD` is missing, keyboard teleop listeners using `pynput` may fail with +`AttributeError: record_create_context`. + +Open `xterm` from XQuartz and use it for keyboard teleop control. +Keyboard events for `pynput` are captured from the focused X11 window (`xterm`), not from regular macOS Terminal. + +Start Rerun on Mac and keep it running: + +```bash +rerun +``` + +In XQuartz `xterm`, connect to Thor: + +```bash +ssh -Y thor@thor +``` + +On Thor, verify X11 forwarding: + +```bash +echo "$DISPLAY" +``` + +`DISPLAY` must be non-empty before starting `lerobot-teleoperate`. + +Optional: tune Rerun micro-batching on Thor before starting teleop: + +```bash +export RERUN_FLUSH_TICK_SECS=0.03 +export RERUN_FLUSH_NUM_BYTES=65536 +export RERUN_FLUSH_NUM_ROWS=4096 +``` + +Then run teleop on Thor (direct to Mac IP): + +```bash +lerobot-teleoperate \ + --robot.type=xlerobot \ + --robot.config_file=src/lerobot/robots/xlerobot/configs/xlerobot_biwheel_odrive_panthera_left.json \ + --teleop.type=xlerobot_keyboard_composite \ + --teleop.config_file=src/lerobot/teleoperators/xlerobot_teleoperator/configs/xlerobot_keyboard_composite_panthera_left_ee_with_base.json \ + --display_data=true \ + --fps=30 \ + --display_ip= \ + --display_port=9876 \ + --display_compressed_images=True +``` + +Replace `` with your Mac LAN IP. + +## Dedicated buses (non-shared components) + +Some drivers (for example ODrive-based bases) cannot share a Feetech bus. In that case, omit the component from +`shared_buses` and set `shared_bus: false` in the component config. The component is then expected to manage its +own connection (for example via `port` or `odrive_serial`). + +```yaml +robot: + type: xlerobot + base: + type: biwheel_odrive + shared_bus: false + odrive_serial: "123456789ABC" + + shared_buses: + left_bus: + port: /dev/ttyACM2 + components: + - {component: left_arm} + - {component: mount, motor_id_offset: 6} +``` + +# XLeRobot integration based on + +- https://github.com/Astera-org/brainbot +- https://github.com/Vector-Wangel/XLeRobot +- https://github.com/bingogome/lerobot + +# Example Command Line Run + +```bash +lerobot-teleoperate \ + --robot.type=xlerobot \ + --robot.left_arm='{ + "id": "xlerobot_arm_left" + }' \ + --robot.right_arm='{ + "id": "xlerobot_arm_right" + }' \ + --robot.base='{ + "type": "lekiwi_base", + "wheel_radius_m": 0.05, + "base_radius_m": 0.125 + }' \ + --robot.mount='{ + "pan_motor_id": 1, + "tilt_motor_id": 2, + "motor_model": "sts3215", + "pan_key": "mount_pan.pos", + "tilt_key": "mount_tilt.pos", + "max_pan_speed_dps": 60.0, + "max_tilt_speed_dps": 45.0, + "pan_range": [-90.0, 90.0], + "tilt_range": [-30.0, 60.0] + }' \ + --robot.cameras='{ + "top": {"type": "opencv", "index_or_path": 8, "width": 640, "height": 480, "fps": 30} + }' \ + --robot.shared_buses='{ + "left_bus": { + "port": "/dev/ttyACM2", + "components": [ + {"component": "left_arm"}, + {"component": "mount", "motor_id_offset": 6} + ] + }, + "right_bus": { + "port": "/dev/ttyACM3", + "components": [ + {"component": "right_arm"}, + {"component": "base", "motor_id_offset": 6} + ] + } + }' \ + --teleop.type=xlerobot_default_composite \ + --teleop.base_type=lekiwi_base_gamepad \ + --teleop.arms='{ + "left_arm_config": { + "port": "/dev/ttyACM0" + }, + "right_arm_config": { + "port": "/dev/ttyACM1" + }, + "id": "leader" + }' \ + --teleop.base='{ + "joystick_index": 0, + "max_speed_mps": 0.8, + "deadzone": 0.15, + "yaw_speed_deg": 45 + }' \ + --teleop.mount='{ + "joystick_index": 0, + "deadzone": 0.15, + "polling_fps": 50, + "max_pan_speed_dps": 60.0, + "max_tilt_speed_dps": 45.0, + "pan_axis": 3, + "tilt_axis": 4, + "invert_pan": false, + "invert_tilt": true, + "pan_range": [-90.0, 90.0], + "tilt_range": [-30.0, 60.0] + }' \ + --display_data=true +``` + +Or, if you want to run without arms or mount, you can either use the base-only configs: + +```bash +lerobot-teleoperate \ + --robot.type=xlerobot \ + --robot.config_file=src/lerobot/robots/xlerobot/configs/base_only.json \ + --teleop.type=xlerobot_default_composite \ + --teleop.config_file=src/lerobot/teleoperators/xlerobot_teleoperator/configs/base_only.json \ + --display_data=true +``` + +…or stick to the original CLI-only configuration (no config files): + +```bash +lerobot-teleoperate \ + --robot.type=xlerobot \ + --robot.base='{ + "type": "lekiwi_base", + "wheel_radius_m": 0.05, + "base_radius_m": 0.125 + }' \ + --robot.mount='{ + "pan_motor_id": 1, + "tilt_motor_id": 2, + "motor_model": "sts3215", + "pan_key": "mount_pan.pos", + "tilt_key": "mount_tilt.pos", + "max_pan_speed_dps": 60.0, + "max_tilt_speed_dps": 45.0, + "pan_range": [-90.0, 90.0], + "tilt_range": [-30.0, 60.0] + }' \ + --robot.cameras='{}' \ + --robot.shared_buses='{ + "base_bus": { + "port": "/dev/ttyACM4", + "components": [ + {"component": "base"} + ] + }, + "mount_bus": { + "port": "/dev/ttyACM5", + "components": [ + {"component": "mount"} + ] + } + }' \ + --teleop.type=xlerobot_default_composite \ + --teleop.base_type=lekiwi_base_gamepad \ + --teleop.base='{ + "joystick_index": 0, + "max_speed_mps": 0.8, + "deadzone": 0.15, + "yaw_speed_deg": 45 + }' \ + --teleop.mount='{ + "joystick_index": 0, + "deadzone": 0.15, + "polling_fps": 50, + "max_pan_speed_dps": 60.0, + "max_tilt_speed_dps": 45.0, + "pan_axis": 3, + "tilt_axis": 4, + "invert_pan": false, + "invert_tilt": true, + "pan_range": [-90.0, 90.0], + "tilt_range": [-30.0, 60.0] + }' \ + --display_data=true +``` + +--- diff --git a/src/lerobot/robots/xlerobot/__init__.py b/src/lerobot/robots/xlerobot/__init__.py new file mode 100644 index 00000000000..33ba5ea6eb2 --- /dev/null +++ b/src/lerobot/robots/xlerobot/__init__.py @@ -0,0 +1,24 @@ +#!/usr/bin/env python + +# Copyright 2025 The HuggingFace Inc. team. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# XLeRobot integration based on +# +# https://github.com/Astera-org/brainbot +# https://github.com/Vector-Wangel/XLeRobot +# https://github.com/bingogome/lerobot + +from .config_xlerobot import XLeRobotConfig +from .xlerobot import XLeRobot diff --git a/src/lerobot/robots/xlerobot/config_xlerobot.py b/src/lerobot/robots/xlerobot/config_xlerobot.py new file mode 100644 index 00000000000..14279088b96 --- /dev/null +++ b/src/lerobot/robots/xlerobot/config_xlerobot.py @@ -0,0 +1,188 @@ +# Copyright 2025 The HuggingFace Inc. team. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# XLeRobot integration based on +# +# https://github.com/Astera-org/brainbot +# https://github.com/Vector-Wangel/XLeRobot +# https://github.com/bingogome/lerobot + +from dataclasses import asdict, dataclass, field, is_dataclass +from typing import Any + +import draccus + +from lerobot.configs import parser +from lerobot.cameras import CameraConfig + +from ..config import RobotConfig +from .shared_bus_mode.component_assembly import SharedBusConfig, SharedBusDeviceConfig +from .sub_robots.biwheel_base.config_biwheel_base import ( + BiwheelBaseConfig, + BiwheelFeetechConfig, + BiwheelODriveConfig, +) +from .sub_robots.lekiwi_base.config import LeKiwiBaseConfig + + +@RobotConfig.register_subclass("xlerobot") +@dataclass +class XLeRobotConfig(RobotConfig): + _comment: str | None = None + config_file: str | None = None + left_arm: dict[str, Any] = field(default_factory=dict) + right_arm: dict[str, Any] = field(default_factory=dict) + base: dict[str, Any] = field(default_factory=dict) + mount: dict[str, Any] = field(default_factory=dict) + shared_buses: dict[str, Any] = field(default_factory=dict) + cameras: dict[str, CameraConfig] = field(default_factory=dict) + + def __post_init__(self) -> None: + if self.config_file: + self._load_from_config_file(self.config_file) + super().__post_init__() + + self.left_arm = self._normalize_component_dict(self.left_arm) + self.right_arm = self._normalize_component_dict(self.right_arm) + self.base = self._normalize_base_dict(self.base) + self.mount = self._normalize_component_dict(self.mount) + + self.shared_buses_config, self.component_ports = self._coerce_shared_buses() + self._validate_component_ports() + + def _load_from_config_file(self, config_file: str) -> None: + cli_overrides = parser.get_cli_overrides("robot") or [] + cli_overrides = [arg for arg in cli_overrides if not arg.startswith("--config_file=")] + loaded = draccus.parse(config_class=RobotConfig, config_path=config_file, args=cli_overrides) + self.__dict__.update(loaded.__dict__) + self.config_file = config_file + + def _normalize_component_dict(self, cfg: dict[str, Any] | Any) -> dict[str, Any]: + if not cfg: + return {} + if isinstance(cfg, dict): + return dict(cfg) + if is_dataclass(cfg): + return asdict(cfg) + return dict(cfg) + + def _normalize_base_dict(self, cfg: dict[str, Any] | Any) -> dict[str, Any]: + if not cfg: + return {} + + if isinstance(cfg, dict): + data = dict(cfg) + elif isinstance(cfg, LeKiwiBaseConfig): + data = asdict(cfg) + data.setdefault("type", "lekiwi_base") + elif isinstance(cfg, (BiwheelFeetechConfig, BiwheelODriveConfig, BiwheelBaseConfig)): + data = asdict(cfg) + if isinstance(cfg, BiwheelODriveConfig): + data.setdefault("type", "biwheel_odrive") + elif isinstance(cfg, BiwheelFeetechConfig): + data.setdefault("type", "biwheel_feetech") + else: + data.setdefault("type", "biwheel_base") + elif is_dataclass(cfg): + data = asdict(cfg) + else: + data = dict(cfg) + + if "type" not in data: + raise ValueError("Base configuration must specify a 'type' field (e.g. 'lekiwi_base').") + return data + + def _coerce_shared_buses(self) -> tuple[dict[str, SharedBusConfig], dict[str, str]]: + if not self.shared_buses: + return {}, {} + + coerced: dict[str, SharedBusConfig] = {} + component_ports: dict[str, str] = {} + for name, value in self.shared_buses.items(): + if isinstance(value, SharedBusConfig): + bus_cfg = value + else: + port = value.get("port") + if not port: + raise ValueError(f"Shared bus '{name}' is missing required field 'port'.") + components_data = value.get("components", []) + components = [ + device if isinstance(device, SharedBusDeviceConfig) else SharedBusDeviceConfig(**device) + for device in components_data + ] + bus_cfg = SharedBusConfig( + port=port, + components=components, + handshake_on_connect=value.get("handshake_on_connect", True), + ) + + if not bus_cfg.components: + raise ValueError(f"Shared bus '{name}' must list at least one component.") + + for device in bus_cfg.components: + previous = component_ports.get(device.component) + if previous and previous != bus_cfg.port: + raise ValueError( + f"Component '{device.component}' is assigned to multiple shared buses " + f"({previous} vs {bus_cfg.port})." + ) + component_ports[device.component] = bus_cfg.port + + coerced[name] = bus_cfg + + return coerced, component_ports + + def _validate_component_ports(self) -> None: + for component_name, spec in ( + ("left_arm", self.left_arm), + ("right_arm", self.right_arm), + ("base", self.base), + ("mount", self.mount), + ): + if not spec: + continue + + if component_name in ("left_arm", "right_arm") and spec.get("type") == "panthera_arm": + if spec.get("shared_bus") is True: + raise ValueError( + f"Component '{component_name}' uses 'panthera_arm' and must not use shared buses. " + "Set `shared_bus: false` (or omit it) and remove this arm from `shared_buses`." + ) + + uses_shared_bus = self._component_uses_shared_bus(component_name, spec) + in_shared_bus = component_name in getattr(self, "component_ports", {}) + if uses_shared_bus and not in_shared_bus: + raise ValueError( + f"Component '{component_name}' is configured but missing from `shared_buses`. " + "Declare a shared bus entry that references it, or set `shared_bus: false` to opt out." + ) + if not uses_shared_bus and in_shared_bus: + raise ValueError( + f"Component '{component_name}' opts out of shared buses but is still listed in " + "`shared_buses`. Remove it from shared bus configuration." + ) + + def _component_uses_shared_bus(self, component_name: str, spec: dict[str, Any]) -> bool: + if not spec: + return False + if spec.get("shared_bus") is False: + return False + if component_name in ("left_arm", "right_arm") and spec.get("type") == "panthera_arm": + return False + # ODrive (or other external drivers) should opt out explicitly. + if component_name == "base" and ( + spec.get("driver") == "odrive" or spec.get("type") == "biwheel_odrive" + ): + return False + return True diff --git a/src/lerobot/robots/xlerobot/configs/base_only.json b/src/lerobot/robots/xlerobot/configs/base_only.json new file mode 100644 index 00000000000..83c484fdf34 --- /dev/null +++ b/src/lerobot/robots/xlerobot/configs/base_only.json @@ -0,0 +1,21 @@ +{ + "type": "xlerobot", + "left_arm": {}, + "right_arm": {}, + "mount": {}, + "cameras": {}, + "shared_buses": {}, + "base": { + "type": "biwheel_odrive", + "wheel_radius": 0.05, + "wheel_base": 0.25, + "reverse_front_direction": false, + "invert_left_motor": true, + "invert_right_motor": false, + "odrive_serial": null, + "axis_left": 1, + "axis_right": 0, + "disable_watchdog": true, + "request_closed_loop": true + } +} diff --git a/src/lerobot/robots/xlerobot/configs/xlerobot_biwheel_feetech.json b/src/lerobot/robots/xlerobot/configs/xlerobot_biwheel_feetech.json new file mode 100644 index 00000000000..7e62cda7847 --- /dev/null +++ b/src/lerobot/robots/xlerobot/configs/xlerobot_biwheel_feetech.json @@ -0,0 +1,55 @@ +{ + "_comment": "Use with --robot.config_file=src/lerobot/robots/xlerobot/configs/xlerobot_biwheel_feetech.json", + "type": "xlerobot", + "left_arm": { + "id": "xlerobot_arm_left" + }, + "right_arm": { + "id": "xlerobot_arm_right" + }, + "base": { + "type": "biwheel_feetech", + "wheel_radius": 0.05, + "wheel_base": 0.25, + "reverse_front_direction": false, + "base_motor_ids": [9, 10], + "invert_left_motor": true, + "invert_right_motor": false + }, + "mount": { + "pan_motor_id": 1, + "tilt_motor_id": 2, + "motor_model": "sts3215", + "pan_key": "mount_pan.pos", + "tilt_key": "mount_tilt.pos", + "max_pan_speed_dps": 60.0, + "max_tilt_speed_dps": 45.0, + "pan_range": [-90.0, 90.0], + "tilt_range": [-30.0, 60.0] + }, + "cameras": { + "top": { + "type": "opencv", + "index_or_path": 0, + "width": 640, + "height": 480, + "fps": 30 + } + }, + "shared_buses": { + "left_bus": { + "port": "/dev/ttyACM2", + "components": [ + {"component": "left_arm"}, + {"component": "mount", "motor_id_offset": 6} + ] + }, + "right_bus": { + "port": "/dev/ttyACM3", + "components": [ + {"component": "right_arm"}, + {"component": "base", "motor_id_offset": 6} + ] + } + } +} diff --git a/src/lerobot/robots/xlerobot/configs/xlerobot_biwheel_odrive_panthera_left.json b/src/lerobot/robots/xlerobot/configs/xlerobot_biwheel_odrive_panthera_left.json new file mode 100644 index 00000000000..3fd91df2822 --- /dev/null +++ b/src/lerobot/robots/xlerobot/configs/xlerobot_biwheel_odrive_panthera_left.json @@ -0,0 +1,53 @@ +{ + "_comment": "Use with --robot.config_file=src/lerobot/robots/xlerobot/configs/xlerobot_biwheel_odrive_panthera_left.json", + "type": "xlerobot", + "left_arm": { + "type": "panthera_arm", + "shared_bus": false, + "sdk_python_dir": "../xlerobot-HT_SDK/panthera_python/scripts", + "config_path": "../xlerobot-HT_SDK/panthera_python/robot_param/Follower.yaml", + "use_joint_impedance": true, + "run_startup_sequence": true, + "impedance_control_hz": 800.0, + "impedance_k_joint": [20.0, 25.0, 25.0, 20.0, 10.0, 5.0], + "impedance_b_joint": [1.0, 1.0, 1.0, 0.8, 0.4, 0.2], + "tau_limit": [10.0, 20.0, 20.0, 10.0, 5.0, 5.0], + "friction_fc": [0.05, 0.05, 0.05, 0.05, 0.0, 0.0], + "friction_fv": [0.03, 0.03, 0.03, 0.03, 0.0, 0.0], + "joint_velocity": [0.5, 0.5, 0.5, 0.5, 0.5, 0.5], + "cameras": { + "gripper_camera": { + "type": "opencv", + "index_or_path": 2, + "width": 640, + "height": 480, + "fps": 30 + } + } + }, + "right_arm": {}, + "mount": {}, + "cameras": { + "base_camera": { + "type": "opencv", + "index_or_path": 0, + "width": 640, + "height": 480, + "fps": 30 + } + }, + "shared_buses": {}, + "base": { + "type": "biwheel_odrive", + "wheel_radius": 0.05, + "wheel_base": 0.25, + "reverse_front_direction": false, + "invert_left_motor": true, + "invert_right_motor": false, + "odrive_serial": null, + "axis_left": 1, + "axis_right": 0, + "disable_watchdog": true, + "request_closed_loop": true + } +} diff --git a/src/lerobot/robots/xlerobot/shared_bus_mode/component_assembly.py b/src/lerobot/robots/xlerobot/shared_bus_mode/component_assembly.py new file mode 100644 index 00000000000..41d0bf239cf --- /dev/null +++ b/src/lerobot/robots/xlerobot/shared_bus_mode/component_assembly.py @@ -0,0 +1,36 @@ +#!/usr/bin/env python + +# Copyright 2025 The HuggingFace Inc. team. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from __future__ import annotations + +from dataclasses import dataclass, field + + +@dataclass +class SharedBusDeviceConfig: + """Describes how a logical component attaches to a shared bus.""" + + component: str + motor_id_offset: int = 0 + + +@dataclass +class SharedBusConfig: + """Configuration for a shared Feetech motor bus.""" + + port: str + components: list[SharedBusDeviceConfig] = field(default_factory=list) + handshake_on_connect: bool = True diff --git a/src/lerobot/robots/xlerobot/shared_bus_mode/shared_bus.py b/src/lerobot/robots/xlerobot/shared_bus_mode/shared_bus.py new file mode 100644 index 00000000000..057b4a125dd --- /dev/null +++ b/src/lerobot/robots/xlerobot/shared_bus_mode/shared_bus.py @@ -0,0 +1,284 @@ +#!/usr/bin/env python + +# Copyright 2025 The HuggingFace Inc. team. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from __future__ import annotations + +import contextlib +from collections.abc import Iterable +from copy import deepcopy +from dataclasses import dataclass + +from lerobot.motors import Motor, MotorCalibration +from lerobot.motors.feetech import FeetechMotorsBus + + +@dataclass +class SharedComponentAttachment: + """Represents a component that should attach to a shared bus.""" + + name: str + component: object + motor_names: tuple[str, ...] + motor_id_offset: int = 0 + + @property + def calibration(self) -> dict[str, MotorCalibration]: + return getattr(self.component, "calibration", {}) or {} + + @property + def bus(self) -> FeetechMotorsBus: + return self.component.bus + + +class SharedFeetechBusGroup: + """Owns a real Feetech bus and provides reference-counted access to it.""" + + def __init__( + self, + name: str, + port: str, + motors: dict[str, Motor], + calibration: dict[str, MotorCalibration], + *, + handshake_on_connect: bool = True, + ): + self.name = name + self.bus = FeetechMotorsBus(port=port, motors=motors, calibration=calibration) + self.handshake_on_connect = handshake_on_connect + self._active_clients = 0 + + @property + def is_connected(self) -> bool: + return self.bus.is_connected + + def acquire(self, *, handshake: bool | None = None) -> None: + should_connect = not self.bus.is_connected + if should_connect: + use_handshake = self.handshake_on_connect if handshake is None else handshake + self.bus.connect(handshake=use_handshake) + self._active_clients += 1 + + def release(self, *, disable_torque: bool = True) -> None: + self._active_clients = max(0, self._active_clients - 1) + if self._active_clients == 0 and self.bus.is_connected: + self.bus.disconnect(disable_torque=disable_torque) + + +class SharedFeetechBusView: + """Component-scoped view onto a shared Feetech bus.""" + + def __init__(self, group: SharedFeetechBusGroup, motor_names: Iterable[str]): + self._group = group + self._motor_names = tuple(motor_names) + self._local_connections = 0 + self.port = group.bus.port + self.motors = {name: deepcopy(group.bus.motors[name]) for name in self._motor_names} + + # ------------------------------------------------------------------ + # Helpers + # ------------------------------------------------------------------ + def _ensure_subset(self, motors: str | list[str] | None) -> list[str]: + if motors is None: + return list(self._motor_names) + if isinstance(motors, str): + motors = [motors] + invalid = [m for m in motors if m not in self._motor_names] + if invalid: + raise KeyError(f"Motors {invalid} are not part of this shared-bus view.") + return motors + + # ------------------------------------------------------------------ + # Connection lifecycle + # ------------------------------------------------------------------ + @property + def is_connected(self) -> bool: + return self._local_connections > 0 + + @property + def is_calibrated(self) -> bool: + if not self._group.bus.calibration: + return False + current = self._group.bus.read_calibration() + bus_cal = self._group.bus.calibration + for motor in self._motor_names: + if motor not in current or motor not in bus_cal: + return False + cal_expected = bus_cal[motor] + cal_current = current[motor] + if ( + cal_expected.homing_offset != cal_current.homing_offset + or cal_expected.range_min != cal_current.range_min + or cal_expected.range_max != cal_current.range_max + ): + return False + return True + + def connect(self, handshake: bool | None = None) -> None: + self._group.acquire(handshake=handshake) + self._local_connections += 1 + + def disconnect(self, disable_torque: bool = True) -> None: + if self._local_connections == 0: + return + if disable_torque and self._group.bus.is_connected and self._motor_names: + self._group.bus.disable_torque(list(self._motor_names)) + self._local_connections = max(0, self._local_connections - 1) + self._group.release(disable_torque=disable_torque) + + # ------------------------------------------------------------------ + # Bus operations (subset-aware) + # ------------------------------------------------------------------ + def disable_torque(self, motors: str | list[str] | None = None, num_retry: int = 0) -> None: + self._group.bus.disable_torque(self._ensure_subset(motors), num_retry=num_retry) + + def enable_torque(self, motors: str | list[str] | None = None, num_retry: int = 0) -> None: + self._group.bus.enable_torque(self._ensure_subset(motors), num_retry=num_retry) + + @contextlib.contextmanager + def torque_disabled(self, motors: str | list[str] | None = None): + names = self._ensure_subset(motors) + with self._group.bus.torque_disabled(names): + yield + + def configure_motors(self, *args, **kwargs) -> None: + # Configuring the full bus is acceptable; settings are idempotent. + self._group.bus.configure_motors(*args, **kwargs) + + def write_calibration(self, calibration_dict: dict[str, MotorCalibration], cache: bool = True) -> None: + subset = {name: calibration_dict[name] for name in self._motor_names if name in calibration_dict} + if not subset: + return + self._group.bus.write_calibration(subset, cache=cache) + + def write( + self, + data_name: str, + motor: str, + value, + *, + normalize: bool = True, + num_retry: int = 0, + ) -> None: + if motor not in self._motor_names: + raise KeyError(f"Motor '{motor}' is not owned by this shared-bus view.") + self._group.bus.write( + data_name, + motor, + value, + normalize=normalize, + num_retry=num_retry, + ) + + def sync_read( + self, + data_name: str, + motors: str | list[str] | None = None, + *, + normalize: bool = True, + num_retry: int = 0, + ) -> dict[str, float]: + names = self._ensure_subset(motors) + return self._group.bus.sync_read( + data_name, + names, + normalize=normalize, + num_retry=num_retry, + ) + + def sync_write( + self, + data_name: str, + values, + *, + normalize: bool = True, + num_retry: int = 0, + ) -> None: + if not isinstance(values, dict): + values = dict.fromkeys(self._motor_names, values) + else: + missing = [name for name in values if name not in self._motor_names] + if missing: + raise KeyError(f"Motors {missing} are not part of this shared-bus view.") + self._group.bus.sync_write( + data_name, + values, + normalize=normalize, + num_retry=num_retry, + ) + + def set_half_turn_homings(self, motors: str | list[str] | None = None): + names = self._ensure_subset(motors) + return self._group.bus.set_half_turn_homings(names) + + def record_ranges_of_motion(self, motors: str | list[str] | None = None, display_values: bool = True): + names = self._ensure_subset(motors) + return self._group.bus.record_ranges_of_motion(names, display_values=display_values) + + def setup_motor(self, motor: str, initial_baudrate: int | None = None, initial_id: int | None = None): + if motor not in self._motor_names: + raise KeyError(f"Motor '{motor}' is not part of this shared-bus view.") + return self._group.bus.setup_motor(motor, initial_baudrate=initial_baudrate, initial_id=initial_id) + + +def build_shared_bus_group( + name: str, + port: str, + attachments: list[SharedComponentAttachment], + *, + handshake_on_connect: bool = True, +) -> tuple[SharedFeetechBusGroup, dict[str, SharedFeetechBusView]]: + """Create a shared bus group and corresponding component views.""" + + if not attachments: + raise ValueError(f"Shared bus '{name}' has no attached components.") + + motors: dict[str, Motor] = {} + calibration: dict[str, MotorCalibration] = {} + motor_lookup: dict[str, list[str]] = {} + + for attachment in attachments: + for motor_name in attachment.motor_names: + motor = attachment.bus.motors[motor_name] + if motor_name in motors: + raise ValueError( + f"Motor name '{motor_name}' is duplicated across components on shared bus '{name}'." + ) + shifted_motor = deepcopy(motor) + shifted_motor.id += attachment.motor_id_offset + motors[motor_name] = shifted_motor + motor_lookup.setdefault(attachment.name, []).append(motor_name) + + for cal_name, cal in attachment.calibration.items(): + shifted_cal = deepcopy(cal) + shifted_cal.id += attachment.motor_id_offset + calibration[cal_name] = shifted_cal + + group = SharedFeetechBusGroup( + name=name, + port=port, + motors=motors, + calibration=calibration, + handshake_on_connect=handshake_on_connect, + ) + + views: dict[str, SharedFeetechBusView] = {} + for attachment in attachments: + motor_names = motor_lookup.get(attachment.name, []) + view = SharedFeetechBusView(group, motor_names) + attachment.component.bus = view + views[attachment.name] = view + + return group, views diff --git a/src/lerobot/robots/xlerobot/sub_robots/__init__.py b/src/lerobot/robots/xlerobot/sub_robots/__init__.py new file mode 100644 index 00000000000..dcca505bbe9 --- /dev/null +++ b/src/lerobot/robots/xlerobot/sub_robots/__init__.py @@ -0,0 +1,28 @@ +"""Reusable building blocks for the XLeRobot platform.""" + +from .biwheel_base import ( # noqa: F401 + BiwheelBase, + BiwheelFeetech, + BiwheelODrive, + BiwheelBaseConfig, + BiwheelFeetechConfig, + BiwheelODriveConfig, +) +from .lekiwi_base import LeKiwiBase, LeKiwiBaseConfig # noqa: F401 +from .panthera_arm import PantheraArm, PantheraArmConfig # noqa: F401 +from .xlerobot_mount import XLeRobotMount, XLeRobotMountConfig # noqa: F401 + +__all__ = [ + "BiwheelBase", + "BiwheelFeetech", + "BiwheelODrive", + "BiwheelBaseConfig", + "BiwheelFeetechConfig", + "BiwheelODriveConfig", + "LeKiwiBase", + "LeKiwiBaseConfig", + "PantheraArm", + "PantheraArmConfig", + "XLeRobotMount", + "XLeRobotMountConfig", +] diff --git a/src/lerobot/robots/xlerobot/sub_robots/biwheel_base/__init__.py b/src/lerobot/robots/xlerobot/sub_robots/biwheel_base/__init__.py new file mode 100644 index 00000000000..5627711ca9a --- /dev/null +++ b/src/lerobot/robots/xlerobot/sub_robots/biwheel_base/__init__.py @@ -0,0 +1,29 @@ +#!/usr/bin/env python + +# Copyright 2025 The HuggingFace Inc. team. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from .biwheel_base import BiwheelBase +from .biwheel_feetech import BiwheelFeetech +from .biwheel_odrive import BiwheelODrive +from .config_biwheel_base import BiwheelBaseConfig, BiwheelFeetechConfig, BiwheelODriveConfig + +__all__ = [ + "BiwheelBase", + "BiwheelFeetech", + "BiwheelODrive", + "BiwheelBaseConfig", + "BiwheelFeetechConfig", + "BiwheelODriveConfig", +] diff --git a/src/lerobot/robots/xlerobot/sub_robots/biwheel_base/biwheel_base.py b/src/lerobot/robots/xlerobot/sub_robots/biwheel_base/biwheel_base.py new file mode 100644 index 00000000000..dea9d728fbb --- /dev/null +++ b/src/lerobot/robots/xlerobot/sub_robots/biwheel_base/biwheel_base.py @@ -0,0 +1,85 @@ +#!/usr/bin/env python + +# Copyright 2025 The HuggingFace Inc. team. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import abc +from functools import cached_property + +import numpy as np + +from ....robot import Robot +from .config_biwheel_base import BiwheelBaseConfig + + +class BiwheelBase(Robot, abc.ABC): + """Base class for biwheel robots with shared kinematics.""" + + config_class = BiwheelBaseConfig + name = "biwheel_base" + + @property + def _state_ft(self) -> dict[str, type]: + return { + "x.vel": float, + "theta.vel": float, + } + + @cached_property + def observation_features(self) -> dict[str, type]: + return self._state_ft + + @cached_property + def action_features(self) -> dict[str, type]: + return self._state_ft + + def _apply_front_direction(self, x: float, theta: float) -> tuple[float, float]: + """Apply optional front-direction flip to linear motion only.""" + if getattr(self.config, "reverse_front_direction", False): + return -x, theta + return x, theta + + def _body_to_wheel_linear(self, x: float, theta: float) -> tuple[float, float]: + """Convert body-frame velocities into wheel linear velocities (m/s).""" + x, theta = self._apply_front_direction(x, theta) + theta_rad = np.deg2rad(theta) + half_wheelbase = self.config.wheel_base / 2.0 + left_linear = x - theta_rad * half_wheelbase + right_linear = x + theta_rad * half_wheelbase + return left_linear, right_linear + + def _wheel_linear_to_body(self, left_linear: float, right_linear: float) -> dict[str, float]: + """Convert wheel linear velocities (m/s) into body-frame velocities.""" + x_vel = (left_linear + right_linear) / 2.0 + theta_rad = (right_linear - left_linear) / self.config.wheel_base + theta_vel = np.rad2deg(theta_rad) + x_vel, theta_vel = self._apply_front_direction(x_vel, theta_vel) + return { + "x.vel": x_vel, + "theta.vel": theta_vel, + } + + def _apply_inversion(self, left: float, right: float) -> tuple[float, float]: + if self.config.invert_left_motor: + left = -left + if self.config.invert_right_motor: + right = -right + return left, right + + def _remove_inversion(self, left: float, right: float) -> tuple[float, float]: + if self.config.invert_left_motor: + left = -left + if self.config.invert_right_motor: + right = -right + return left, right diff --git a/src/lerobot/robots/xlerobot/sub_robots/biwheel_base/biwheel_feetech.py b/src/lerobot/robots/xlerobot/sub_robots/biwheel_base/biwheel_feetech.py new file mode 100644 index 00000000000..f104145302f --- /dev/null +++ b/src/lerobot/robots/xlerobot/sub_robots/biwheel_base/biwheel_feetech.py @@ -0,0 +1,286 @@ +#!/usr/bin/env python + +# Copyright 2025 The HuggingFace Inc. team. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import logging +from typing import Any + +import numpy as np + +from lerobot.motors import Motor, MotorCalibration, MotorNormMode +from lerobot.motors.feetech import FeetechMotorsBus, OperatingMode +from lerobot.utils.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError + +from .biwheel_base import BiwheelBase +from .config_biwheel_base import BiwheelFeetechConfig + +logger = logging.getLogger(__name__) + + +class BiwheelFeetech(BiwheelBase): + """Biwheel robot class driven by Feetech servos.""" + + config_class = BiwheelFeetechConfig + name = "biwheel_feetech" + supports_shared_bus = True + + def __init__(self, config: BiwheelFeetechConfig): + super().__init__(config) + self.config = config + + left_id, right_id = self.config.base_motor_ids + self.bus = FeetechMotorsBus( + port=self.config.port, + motors={ + "base_left_wheel": Motor(left_id, "sts3215", MotorNormMode.RANGE_M100_100), + "base_right_wheel": Motor(right_id, "sts3215", MotorNormMode.RANGE_M100_100), + }, + calibration=self.calibration, + ) + self.base_motors = list(self.bus.motors.keys()) + + @property + def is_connected(self) -> bool: + return self.bus.is_connected + + def connect(self, calibrate: bool = True, handshake: bool | None = None) -> None: + if self.is_connected: + raise DeviceAlreadyConnectedError(f"{self} already connected") + + handshake_flag = self.config.handshake_on_connect if handshake is None else handshake + self.bus.connect(handshake=handshake_flag) + + if calibrate and not self.is_calibrated: + self.calibrate() + + self.configure() + logger.info("%s connected.", self) + + @property + def is_calibrated(self) -> bool: + return bool(self.calibration) and self.bus.is_calibrated + + def calibrate(self) -> None: + logger.info("Calibrating %s base motors", self) + + homing_offsets = dict.fromkeys(self.base_motors, 0) + range_mins = dict.fromkeys(self.base_motors, 0) + range_maxes = dict.fromkeys(self.base_motors, 4095) + + self.calibration = {} + for name, motor in self.bus.motors.items(): + self.calibration[name] = MotorCalibration( + id=motor.id, + drive_mode=0, + homing_offset=homing_offsets[name], + range_min=range_mins[name], + range_max=range_maxes[name], + ) + + self.bus.write_calibration(self.calibration) + self._save_calibration() + logger.info("Calibration saved at %s", self.calibration_fpath) + + def configure(self) -> None: + self.bus.disable_torque() + self.bus.configure_motors() + for name in self.base_motors: + self.bus.write("Operating_Mode", name, OperatingMode.VELOCITY.value) + self.bus.enable_torque() + + def setup_motors(self) -> None: + for motor in self.base_motors: + input(f"Connect the controller board to the '{motor}' motor only and press ENTER.") + self.bus.setup_motor(motor) + print(f"'{motor}' motor id set to {self.bus.motors[motor].id}") + + @staticmethod + def _raw_to_degps(raw_speed: int) -> float: + steps_per_deg = 4096.0 / 360.0 + return raw_speed / steps_per_deg + + @staticmethod + def _degps_to_raw(degps: float) -> int: + steps_per_deg = 4096.0 / 360.0 + speed_int = int(round(degps * steps_per_deg)) + return max(min(speed_int, 0x7FFF), -0x8000) + + def _body_to_wheel_raw( + self, + x: float, + theta: float, + wheel_radius: float = 0.05, + wheel_base: float = 0.25, + max_raw: int = 3000, + ) -> dict[str, int]: + """ + Convert desired body-frame velocities into wheel raw commands for differential drive. + + Parameters: + x_cmd : Linear velocity in x (m/s). + theta_cmd : Rotational velocity (deg/s). + wheel_radius: Radius of each wheel (meters). + wheel_base : Distance between left and right wheels (meters). + max_raw : Maximum allowed raw command (ticks) per wheel. + + Returns: + A dictionary with wheel raw commands: + {"base_left_wheel": value, "base_right_wheel": value}. + + Notes: + - Differential drive kinematics: only x and theta are controllable + - y velocity is ignored (differential drive cannot move sideways) + """ + wheel_radius = wheel_radius or self.config.wheel_radius + wheel_base = wheel_base or self.config.wheel_base + max_raw = max_raw or self.config.max_wheel_raw + x, theta = self._apply_front_direction(x, theta) + + theta_rad = np.deg2rad(theta) + + half_wheelbase = wheel_base / 2 + left_wheel_speed = (x - theta_rad * half_wheelbase) / wheel_radius + right_wheel_speed = (x + theta_rad * half_wheelbase) / wheel_radius + + # Convert to deg/s + wheel_speeds_degps = np.rad2deg(np.array([left_wheel_speed, right_wheel_speed])) + # Apply scaling + steps_per_deg = 4096.0 / 360.0 + raw_floats = np.abs(wheel_speeds_degps) * steps_per_deg + max_raw_computed = np.max(raw_floats) + + if max_raw_computed > max_raw: + wheel_speeds_degps *= max_raw / max_raw_computed + + # Convert to raw integers + left_wheel_raw = self._degps_to_raw(wheel_speeds_degps[0]) + right_wheel_raw = self._degps_to_raw(wheel_speeds_degps[1]) + + # Apply motor direction inversion if configured + if self.config.invert_left_motor: + left_wheel_raw = -left_wheel_raw + if self.config.invert_right_motor: + right_wheel_raw = -right_wheel_raw + + return { + "base_left_wheel": left_wheel_raw, + "base_right_wheel": right_wheel_raw, + } + + def _wheel_raw_to_body( + self, + left_wheel_speed: int, + right_wheel_speed: int, + wheel_radius: float = 0.05, + wheel_base: float = 0.25, + ) -> dict[str, float]: + """ + Convert wheel raw command feedback back into body-frame velocities for differential drive. + + Parameters: + left_wheel_speed : Raw command for left wheel. + right_wheel_speed : Raw command for right wheel. + wheel_radius : Radius of each wheel (meters). Defaults to self.config.wheel_radius. + wheel_base : Distance between left and right wheels (meters). Defaults to self.config.wheelbase. + + Returns: + Dictionary containing: + - "x.vel": Linear velocity in m/s + - "theta.vel": Angular velocity in deg/s + """ + # Use default values from config if not provided + wheel_radius = wheel_radius or self.config.wheel_radius + wheel_base = wheel_base or self.config.wheel_base + + # Convert raw commands to angular speeds (deg/s) + left_degps = self._raw_to_degps(left_wheel_speed) + right_degps = self._raw_to_degps(right_wheel_speed) + + # Convert angular speeds from deg/s to rad/s + left_radps = np.deg2rad(left_degps) + right_radps = np.deg2rad(right_degps) + + # Calculate linear speed (m/s) for each wheel + left_linear_speed = left_radps * wheel_radius + right_linear_speed = right_radps * wheel_radius + + # Apply differential drive inverse kinematics + # Linear velocity: v = (v_left + v_right) / 2 + x_vel = (left_linear_speed + right_linear_speed) / 2.0 + + # Angular velocity: ω = (v_right - v_left) / L + theta_rad = (right_linear_speed - left_linear_speed) / wheel_base + theta_vel = np.rad2deg(theta_rad) + x_vel, theta_vel = self._apply_front_direction(x_vel, theta_vel) + + return { + "x.vel": x_vel, # Linear velocity (m/s) + "theta.vel": theta_vel, # Angular velocity (deg/s) + } + + def get_observation(self) -> dict[str, Any]: + if not self.is_connected: + raise DeviceNotConnectedError(f"{self} is not connected.") + + try: + base_wheel_vel = self.bus.sync_read("Present_Velocity", self.base_motors) + base_vel = self._wheel_raw_to_body( + base_wheel_vel["base_left_wheel"], + base_wheel_vel["base_right_wheel"], + ) + except ConnectionError as e: + logger.warning(f"Failed to read observation: {e}.") + raise + + return base_vel + + def send_action(self, action: dict[str, Any]) -> dict[str, Any]: + if not self.is_connected: + raise DeviceNotConnectedError(f"{self} is not connected.") + + base_goal_vel = { + "x.vel": float(action.get("x.vel", 0.0)), + "theta.vel": float(action.get("theta.vel", 0.0)), + } + + wheel_goal_vel = self._body_to_wheel_raw( + base_goal_vel["x.vel"], + base_goal_vel["theta.vel"], + ) + + # Debug logging + logger.debug( + f"Action: x.vel={base_goal_vel['x.vel']:.3f} m/s, theta.vel={base_goal_vel['theta.vel']:.1f} deg/s " + f"-> Wheels: L={wheel_goal_vel['base_left_wheel']}, R={wheel_goal_vel['base_right_wheel']}" + ) + + self.bus.sync_write("Goal_Velocity", wheel_goal_vel) + return base_goal_vel + + def stop_base(self) -> None: + try: + self.bus.sync_write("Goal_Velocity", dict.fromkeys(self.base_motors, 0), num_retry=5) + logger.info("Base motors stopped") + except ConnectionError as e: + logger.warning(f"Failed to stop base motors: {e}") + + def disconnect(self) -> None: + if not self.is_connected: + raise DeviceNotConnectedError(f"{self} is not connected.") + + self.stop_base() + self.bus.disconnect(self.config.disable_torque_on_disconnect) + + logger.info("%s disconnected.", self) diff --git a/src/lerobot/robots/xlerobot/sub_robots/biwheel_base/biwheel_odrive.py b/src/lerobot/robots/xlerobot/sub_robots/biwheel_base/biwheel_odrive.py new file mode 100644 index 00000000000..9eb6340a94f --- /dev/null +++ b/src/lerobot/robots/xlerobot/sub_robots/biwheel_base/biwheel_odrive.py @@ -0,0 +1,163 @@ +#!/usr/bin/env python + +# Copyright 2025 The HuggingFace Inc. team. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import logging +import time +from typing import Any + +import numpy as np + +from lerobot.utils.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError + +from .biwheel_base import BiwheelBase +from .config_biwheel_base import BiwheelODriveConfig + +logger = logging.getLogger(__name__) + + +class BiwheelODrive(BiwheelBase): + """Biwheel robot class driven by ODrive controllers.""" + + config_class = BiwheelODriveConfig + name = "biwheel_odrive" + supports_shared_bus = False + + def __init__(self, config: BiwheelODriveConfig): + super().__init__(config) + self.config = config + self._odrv = None + self._left = None + self._right = None + self._wheel_circumference = 2.0 * np.pi * self.config.wheel_radius + + @property + def is_connected(self) -> bool: + return self._odrv is not None and self._left is not None and self._right is not None + + def connect(self, calibrate: bool = True, handshake: bool | None = None) -> None: + if self.is_connected: + raise DeviceAlreadyConnectedError(f"{self} already connected") + _ = calibrate + _ = handshake + + odrive, enums = self._load_odrive() + serial = self.config.odrive_serial + timeout = self.config.odrive_timeout_s + self._odrv = ( + odrive.find_any(serial_number=serial, timeout=timeout) + if serial + else odrive.find_any(timeout=timeout) + ) + + self._left = getattr(self._odrv, f"axis{self.config.axis_left}") + self._right = getattr(self._odrv, f"axis{self.config.axis_right}") + + self._left.clear_errors() + self._right.clear_errors() + time.sleep(0.1) + + self._left.controller.config.control_mode = enums.CONTROL_MODE_VELOCITY_CONTROL + self._right.controller.config.control_mode = enums.CONTROL_MODE_VELOCITY_CONTROL + self._left.controller.config.input_mode = enums.INPUT_MODE_PASSTHROUGH + self._right.controller.config.input_mode = enums.INPUT_MODE_PASSTHROUGH + + if self.config.disable_watchdog: + self._left.config.enable_watchdog = False + self._right.config.enable_watchdog = False + + if self.config.request_closed_loop: + self._left.requested_state = enums.AXIS_STATE_CLOSED_LOOP_CONTROL + self._right.requested_state = enums.AXIS_STATE_CLOSED_LOOP_CONTROL + time.sleep(0.5) + + if self._left.error or self._right.error: + logger.warning( + "ODrive reported errors after connect (left=%s, right=%s).", + hex(self._left.error), + hex(self._right.error), + ) + + logger.info("%s connected.", self) + + @property + def is_calibrated(self) -> bool: + return True + + def calibrate(self) -> None: + pass + + def configure(self) -> None: + pass + + def get_observation(self) -> dict[str, Any]: + if not self.is_connected: + raise DeviceNotConnectedError(f"{self} is not connected.") + + left_rps = self._left.encoder.vel_estimate + right_rps = self._right.encoder.vel_estimate + left_linear = left_rps * self._wheel_circumference + right_linear = right_rps * self._wheel_circumference + left_linear, right_linear = self._remove_inversion(left_linear, right_linear) + return self._wheel_linear_to_body(left_linear, right_linear) + + def send_action(self, action: dict[str, Any]) -> dict[str, Any]: + if not self.is_connected: + raise DeviceNotConnectedError(f"{self} is not connected.") + + base_goal_vel = { + "x.vel": float(action.get("x.vel", 0.0)), + "theta.vel": float(action.get("theta.vel", 0.0)), + } + + left_linear, right_linear = self._body_to_wheel_linear( + base_goal_vel["x.vel"], + base_goal_vel["theta.vel"], + ) + left_linear, right_linear = self._apply_inversion(left_linear, right_linear) + + left_rps = left_linear / self._wheel_circumference + right_rps = right_linear / self._wheel_circumference + + self._left.controller.input_vel = left_rps + self._right.controller.input_vel = right_rps + + return base_goal_vel + + def stop_base(self) -> None: + if not self.is_connected: + return + self._left.controller.input_vel = 0.0 + self._right.controller.input_vel = 0.0 + + def disconnect(self) -> None: + if not self.is_connected: + raise DeviceNotConnectedError(f"{self} is not connected.") + + _, enums = self._load_odrive() + self.stop_base() + self._left.requested_state = enums.AXIS_STATE_IDLE + self._right.requested_state = enums.AXIS_STATE_IDLE + self._odrv = None + self._left = None + self._right = None + logger.info("%s disconnected.", self) + + @staticmethod + def _load_odrive(): + import odrive + import odrive.enums as enums + + return odrive, enums diff --git a/src/lerobot/robots/xlerobot/sub_robots/biwheel_base/config_biwheel_base.py b/src/lerobot/robots/xlerobot/sub_robots/biwheel_base/config_biwheel_base.py new file mode 100644 index 00000000000..9597e4f65af --- /dev/null +++ b/src/lerobot/robots/xlerobot/sub_robots/biwheel_base/config_biwheel_base.py @@ -0,0 +1,57 @@ +#!/usr/bin/env python + +# Copyright 2025 The HuggingFace Inc. team. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +from dataclasses import dataclass + +from ....config import RobotConfig + + +@dataclass +class BiwheelBaseConfig(RobotConfig): + # Differential drive parameters + wheel_radius: float = 0.05 + wheel_base: float = 0.25 + + # Motor direction inversion flags (True to invert, False to keep original) + invert_left_motor: bool = True + invert_right_motor: bool = False + + # If True, treat the opposite chassis side as "front" by inverting only x direction. + # Turning sign (theta) is intentionally kept unchanged. + reverse_front_direction: bool = False + + +@RobotConfig.register_subclass("biwheel_base") +@RobotConfig.register_subclass("biwheel_feetech") +@dataclass +class BiwheelFeetechConfig(BiwheelBaseConfig): + port: str = "/dev/ttyACM0" + disable_torque_on_disconnect: bool = True + + max_wheel_raw: int = 2000 + base_motor_ids: tuple[int, int] = (9, 10) + + handshake_on_connect: bool = True + + +@RobotConfig.register_subclass("biwheel_odrive") +@dataclass +class BiwheelODriveConfig(BiwheelBaseConfig): + odrive_serial: str | None = None + axis_left: int = 0 + axis_right: int = 1 + odrive_timeout_s: float = 30.0 + disable_watchdog: bool = True + request_closed_loop: bool = True diff --git a/src/lerobot/robots/xlerobot/sub_robots/lekiwi_base/__init__.py b/src/lerobot/robots/xlerobot/sub_robots/lekiwi_base/__init__.py new file mode 100644 index 00000000000..193a6699ac0 --- /dev/null +++ b/src/lerobot/robots/xlerobot/sub_robots/lekiwi_base/__init__.py @@ -0,0 +1,18 @@ +#!/usr/bin/env python + +# Copyright 2025 The HuggingFace Inc. team. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from .config import LeKiwiBaseConfig +from .lekiwi_base import LeKiwiBase diff --git a/src/lerobot/robots/xlerobot/sub_robots/lekiwi_base/config.py b/src/lerobot/robots/xlerobot/sub_robots/lekiwi_base/config.py new file mode 100644 index 00000000000..b3b36a2b2e8 --- /dev/null +++ b/src/lerobot/robots/xlerobot/sub_robots/lekiwi_base/config.py @@ -0,0 +1,36 @@ +# Copyright 2025 The HuggingFace Inc. team. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from dataclasses import dataclass + +from ....config import RobotConfig + + +@RobotConfig.register_subclass("lekiwi_base") +@dataclass +class LeKiwiBaseConfig(RobotConfig): + port: str = "/dev/ttyACM0" + + disable_torque_on_disconnect: bool = True + + # Geometry and kinematics parameters of the omni-wheel base + wheel_radius_m: float = 0.05 + base_radius_m: float = 0.125 + wheel_axis_angles_deg: tuple[float, float, float] = (240.0, 0.0, 120.0) + # Motor IDs in order: (left, back, right). + base_motor_ids: tuple[int, int, int] = (2, 1, 3) + max_wheel_raw: int = 3000 + + # Whether to perform the UART handshake when connecting to the bus. + handshake_on_connect: bool = True diff --git a/src/lerobot/robots/xlerobot/sub_robots/lekiwi_base/lekiwi_base.py b/src/lerobot/robots/xlerobot/sub_robots/lekiwi_base/lekiwi_base.py new file mode 100644 index 00000000000..868fd6a7624 --- /dev/null +++ b/src/lerobot/robots/xlerobot/sub_robots/lekiwi_base/lekiwi_base.py @@ -0,0 +1,241 @@ +# Copyright 2025 The HuggingFace Inc. team. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import logging +from functools import cached_property +from typing import Any + +import numpy as np + +from lerobot.motors import Motor, MotorCalibration, MotorNormMode +from lerobot.motors.feetech import FeetechMotorsBus, OperatingMode +from lerobot.utils.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError + +from ....robot import Robot +from .config import LeKiwiBaseConfig + +logger = logging.getLogger(__name__) + + +class LeKiwiBase(Robot): + """Three-wheel LeKiwi mobile base driven by Feetech servos.""" + + config_class = LeKiwiBaseConfig + name = "lekiwi_base" + + def __init__(self, config: LeKiwiBaseConfig): + super().__init__(config) + self.config = config + + left_id, back_id, right_id = self.config.base_motor_ids + self.bus = FeetechMotorsBus( + port=self.config.port, + motors={ + "base_left_wheel": Motor(left_id, "sts3215", MotorNormMode.RANGE_M100_100), + "base_back_wheel": Motor(back_id, "sts3215", MotorNormMode.RANGE_M100_100), + "base_right_wheel": Motor(right_id, "sts3215", MotorNormMode.RANGE_M100_100), + }, + calibration=self.calibration, + ) + self.base_motors = list(self.bus.motors.keys()) + + @property + def _state_ft(self) -> dict[str, type]: + return dict.fromkeys(("x.vel", "y.vel", "theta.vel"), float) + + @cached_property + def observation_features(self) -> dict[str, type]: + return self._state_ft + + @cached_property + def action_features(self) -> dict[str, type]: + return self._state_ft + + @property + def is_connected(self) -> bool: + return self.bus.is_connected + + def connect(self, calibrate: bool = True, handshake: bool | None = None) -> None: + if self.is_connected: + raise DeviceAlreadyConnectedError(f"{self} already connected") + + handshake_flag = self.config.handshake_on_connect if handshake is None else handshake + self.bus.connect(handshake=handshake_flag) + + if calibrate and not self.is_calibrated: + self.calibrate() + + self.configure() + logger.info("%s connected.", self) + + @property + def is_calibrated(self) -> bool: + return bool(self.calibration) and self.bus.is_calibrated + + def calibrate(self) -> None: + logger.info("Calibrating %s base motors", self) + + homing_offsets = dict.fromkeys(self.base_motors, 0) + range_mins = dict.fromkeys(self.base_motors, 0) + range_maxes = dict.fromkeys(self.base_motors, 4095) + + self.calibration = {} + for name, motor in self.bus.motors.items(): + self.calibration[name] = MotorCalibration( + id=motor.id, + drive_mode=0, + homing_offset=homing_offsets[name], + range_min=range_mins[name], + range_max=range_maxes[name], + ) + + self.bus.write_calibration(self.calibration) + self._save_calibration() + logger.info("Calibration saved at %s", self.calibration_fpath) + + def configure(self) -> None: + self.bus.disable_torque() + self.bus.configure_motors() + for name in self.base_motors: + self.bus.write("Operating_Mode", name, OperatingMode.VELOCITY.value) + self.bus.enable_torque() + + def setup_motors(self) -> None: + for motor in self.base_motors: + input(f"Connect the controller board to the '{motor}' motor only and press ENTER.") + self.bus.setup_motor(motor) + print(f"'{motor}' motor id set to {self.bus.motors[motor].id}") + + @staticmethod + def _degps_to_raw(degps: float) -> int: + steps_per_deg = 4096.0 / 360.0 + speed_int = int(round(degps * steps_per_deg)) + return max(min(speed_int, 0x7FFF), -0x8000) + + @staticmethod + def _raw_to_degps(raw_speed: int) -> float: + steps_per_deg = 4096.0 / 360.0 + return raw_speed / steps_per_deg + + def _body_to_wheel_raw( + self, + x: float, + y: float, + theta: float, + wheel_radius: float | None = None, + base_radius: float | None = None, + max_raw: int | None = None, + ) -> dict[str, int]: + wheel_radius = wheel_radius or self.config.wheel_radius_m + base_radius = base_radius or self.config.base_radius_m + max_raw = max_raw or self.config.max_wheel_raw + + theta_rad = theta * (np.pi / 180.0) + velocity_vector = np.array([x, y, theta_rad]) + + angles = np.radians(np.asarray(self.config.wheel_axis_angles_deg, dtype=float) - 90.0) + m = np.array([[np.cos(a), np.sin(a), base_radius] for a in angles]) + + wheel_linear_speeds = m.dot(velocity_vector) + wheel_angular_speeds = wheel_linear_speeds / wheel_radius + wheel_degps = wheel_angular_speeds * (180.0 / np.pi) + + steps_per_deg = 4096.0 / 360.0 + max_raw_computed = max(abs(degps) * steps_per_deg for degps in wheel_degps) if wheel_degps.size else 0 + if max_raw_computed > max_raw and max_raw_computed > 0: + scale = max_raw / max_raw_computed + wheel_degps = wheel_degps * scale + + wheel_raw = [self._degps_to_raw(deg) for deg in wheel_degps] + return { + "base_left_wheel": wheel_raw[0], + "base_back_wheel": wheel_raw[1], + "base_right_wheel": wheel_raw[2], + } + + def _wheel_raw_to_body( + self, + left_wheel_speed: int, + back_wheel_speed: int, + right_wheel_speed: int, + wheel_radius: float | None = None, + base_radius: float | None = None, + ) -> dict[str, Any]: + wheel_radius = wheel_radius or self.config.wheel_radius_m + base_radius = base_radius or self.config.base_radius_m + + wheel_degps = np.array( + [ + self._raw_to_degps(left_wheel_speed), + self._raw_to_degps(back_wheel_speed), + self._raw_to_degps(right_wheel_speed), + ], + dtype=float, + ) + + wheel_radps = wheel_degps * (np.pi / 180.0) + wheel_linear_speeds = wheel_radps * wheel_radius + + angles = np.radians(np.asarray(self.config.wheel_axis_angles_deg, dtype=float) - 90.0) + m = np.array([[np.cos(a), np.sin(a), base_radius] for a in angles]) + m_inv = np.linalg.inv(m) + x, y, theta_rad = m_inv.dot(wheel_linear_speeds) + theta = theta_rad * (180.0 / np.pi) + + return {"x.vel": x, "y.vel": y, "theta.vel": theta} + + def get_observation(self) -> dict[str, Any]: + if not self.is_connected: + raise DeviceNotConnectedError(f"{self} is not connected.") + + base_wheel_vel = self.bus.sync_read("Present_Velocity", self.base_motors) + base_vel = self._wheel_raw_to_body( + base_wheel_vel["base_left_wheel"], + base_wheel_vel["base_back_wheel"], + base_wheel_vel["base_right_wheel"], + ) + + return base_vel + + def send_action(self, action: dict[str, Any]) -> dict[str, Any]: + if not self.is_connected: + raise DeviceNotConnectedError(f"{self} is not connected.") + + base_goal_vel = { + "x.vel": float(action.get("x.vel", 0.0)), + "y.vel": float(action.get("y.vel", 0.0)), + "theta.vel": float(action.get("theta.vel", 0.0)), + } + + wheel_goal_vel = self._body_to_wheel_raw( + base_goal_vel["x.vel"], + base_goal_vel["y.vel"], + base_goal_vel["theta.vel"], + ) + + self.bus.sync_write("Goal_Velocity", wheel_goal_vel) + return base_goal_vel + + def stop_base(self) -> None: + self.bus.sync_write("Goal_Velocity", dict.fromkeys(self.base_motors, 0), num_retry=5) + logger.info("Base motors stopped") + + def disconnect(self) -> None: + if not self.is_connected: + raise DeviceNotConnectedError(f"{self} is not connected.") + + self.stop_base() + self.bus.disconnect(self.config.disable_torque_on_disconnect) + + logger.info("%s disconnected.", self) diff --git a/src/lerobot/robots/xlerobot/sub_robots/panthera_arm/__init__.py b/src/lerobot/robots/xlerobot/sub_robots/panthera_arm/__init__.py new file mode 100644 index 00000000000..19fff4baab0 --- /dev/null +++ b/src/lerobot/robots/xlerobot/sub_robots/panthera_arm/__init__.py @@ -0,0 +1,20 @@ +#!/usr/bin/env python + +# Copyright 2026 The HuggingFace Inc. team. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from .config import PantheraArmConfig +from .panthera_arm import PantheraArm + +__all__ = ["PantheraArm", "PantheraArmConfig"] diff --git a/src/lerobot/robots/xlerobot/sub_robots/panthera_arm/config.py b/src/lerobot/robots/xlerobot/sub_robots/panthera_arm/config.py new file mode 100644 index 00000000000..9c8eef56d25 --- /dev/null +++ b/src/lerobot/robots/xlerobot/sub_robots/panthera_arm/config.py @@ -0,0 +1,93 @@ +#!/usr/bin/env python + +# Copyright 2026 The HuggingFace Inc. team. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from dataclasses import dataclass, field + +from lerobot.cameras import CameraConfig + +from ....config import RobotConfig + + +@RobotConfig.register_subclass("panthera_arm") +@dataclass +class PantheraArmConfig(RobotConfig): + """ + Configuration for Panthera arm robot wrapper. + + This wrapper expects the Panthera SDK python scripts directory to contain + `Panthera_lib` (from xlerobot-HT_SDK). + """ + + sdk_python_dir: str = "../xlerobot-HT_SDK/panthera_python/scripts" + config_path: str | None = None + cameras: dict[str, CameraConfig] = field(default_factory=dict) + + # Cartesian delta action scaling + xy_step_m: float = 0.001 + vertical_step_m: float = 0.001 + rotation_step_deg: float = 0.2 + + # IK options + ik_max_iter: int = 1000 + ik_eps: float = 1e-3 + ik_damping: float = 1e-2 + ik_adaptive_damping: bool = True + ik_multi_init: bool = False + + # Joint command options + joint_velocity: list[float] = field(default_factory=lambda: [0.5] * 6) + max_torque: list[float] | None = None + + # High-rate joint-impedance mode. + use_joint_impedance: bool = False + impedance_control_hz: float = 800.0 + impedance_k_joint: list[float] = field(default_factory=lambda: [20.0, 25.0, 25.0, 20.0, 10.0, 5.0]) + impedance_b_joint: list[float] = field(default_factory=lambda: [1.0, 1.0, 1.0, 0.8, 0.4, 0.2]) + tau_limit: list[float] = field(default_factory=lambda: [10.0, 20.0, 20.0, 10.0, 5.0, 5.0]) + enable_coriolis_comp: bool = False + friction_fc: list[float] = field(default_factory=lambda: [0.05, 0.05, 0.05, 0.05, 0.0, 0.0]) + friction_fv: list[float] = field(default_factory=lambda: [0.03, 0.03, 0.03, 0.03, 0.0, 0.0]) + friction_vel_threshold: float = 0.02 + dq_lpf_cutoff_hz: float = 40.0 + enforce_joint_limit_margin: bool = True + joint_limit_margin_rad: float = 0.1 + impedance_fail_safe_stop: bool = True + impedance_max_consecutive_errors: int = 5 + impedance_error_log_interval_s: float = 1.0 + + # Optional startup sequence inspired by the manufacturer script. + # Disabled by default to avoid unexpected autonomous moves on connect. + run_startup_sequence: bool = False + startup_home_pos_m: list[float] = field(default_factory=lambda: [0.24, 0.0, 0.15]) + startup_home_euler_rad: list[float] = field(default_factory=lambda: [0.0, 1.5707963267948966, 0.0]) + startup_lift_m: float = 0.1 + startup_movej_duration_s: float = 3.0 + startup_lift_duration_s: float = 2.0 + + # Gripper control options + gripper_step: float = 0.02 + gripper_velocity: float = 0.5 + gripper_max_torque: float = 0.5 + gripper_kp: float = 8.0 + gripper_kd: float = 0.5 + + # Optional workspace clamping + min_radius_m: float = 0.05 + max_radius_m: float = 0.8 + min_z_m: float = -0.1 + max_z_m: float = 0.8 + + stop_on_disconnect: bool = True diff --git a/src/lerobot/robots/xlerobot/sub_robots/panthera_arm/panthera_arm.py b/src/lerobot/robots/xlerobot/sub_robots/panthera_arm/panthera_arm.py new file mode 100644 index 00000000000..296d01020c3 --- /dev/null +++ b/src/lerobot/robots/xlerobot/sub_robots/panthera_arm/panthera_arm.py @@ -0,0 +1,757 @@ +#!/usr/bin/env python + +# Copyright 2026 The HuggingFace Inc. team. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from __future__ import annotations + +import logging +import sys +import threading +import time +from functools import cached_property +from pathlib import Path +from typing import Any + +import numpy as np +from scipy.spatial.transform import Rotation as Rot + +from lerobot.cameras.utils import make_cameras_from_configs +from lerobot.processor import RobotAction, RobotObservation +from lerobot.utils.decorators import check_if_already_connected, check_if_not_connected + +from ....robot import Robot +from .config import PantheraArmConfig + +logger = logging.getLogger(__name__) + + +class PantheraArm(Robot): + """Panthera arm wrapper with Cartesian EE command support. https://github.com/HighTorque-Robotics""" + + config_class = PantheraArmConfig + name = "panthera_arm" + supports_shared_bus = False + + def __init__(self, config: PantheraArmConfig): + super().__init__(config) + self.config = config + self.camera_configs = dict(config.cameras) + self.cameras = make_cameras_from_configs(self.camera_configs) + self._last_camera_obs: dict[str, np.ndarray] = { + name: self._make_blank_camera_obs(name) for name in self.cameras + } + self._robot = None + self._target_pos: np.ndarray | None = None + self._target_rot: np.ndarray | None = None + self._joint_target_q: np.ndarray | None = None + self._gripper_target: float = 0.0 + self._target_lock = threading.Lock() + self._impedance_thread: threading.Thread | None = None + self._impedance_stop_event = threading.Event() + self._impedance_enabled_event = threading.Event() + self._last_impedance_error_log_s: float = 0.0 + self._last_limit_warn_log_s: float = 0.0 + + @cached_property + def observation_features(self) -> dict[str, type | tuple[int, int, int]]: + features: dict[str, type | tuple[int, int, int]] = { + "joint1.pos": float, + "joint2.pos": float, + "joint3.pos": float, + "joint4.pos": float, + "joint5.pos": float, + "joint6.pos": float, + "joint1.vel": float, + "joint2.vel": float, + "joint3.vel": float, + "joint4.vel": float, + "joint5.vel": float, + "joint6.vel": float, + "joint1.torque": float, + "joint2.torque": float, + "joint3.torque": float, + "joint4.torque": float, + "joint5.torque": float, + "joint6.torque": float, + "gripper.pos": float, + "gripper.torque": float, + "ee.x": float, + "ee.y": float, + "ee.z": float, + } + features.update(self._cameras_ft) + return features + + @cached_property + def action_features(self) -> dict[str, type]: + return { + "delta_x": float, + "delta_y": float, + "delta_z": float, + "delta_roll": float, + "delta_pitch": float, + "delta_yaw": float, + "gripper": float, + } + + @property + def is_connected(self) -> bool: + return self._robot is not None and all(cam.is_connected for cam in self.cameras.values()) + + def _make_blank_camera_obs(self, cam_key: str) -> np.ndarray: + cam_config = self.camera_configs.get(cam_key) + height = getattr(cam_config, "height", None) or 1 + width = getattr(cam_config, "width", None) or 1 + return np.zeros((height, width, 3), dtype=np.uint8) + + @property + def _cameras_ft(self) -> dict[str, tuple[int, int, int]]: + camera_features: dict[str, tuple[int, int, int]] = {} + for cam_key, cam_config in self.camera_configs.items(): + height = getattr(cam_config, "height", None) or 1 + width = getattr(cam_config, "width", None) or 1 + camera_features[cam_key] = (height, width, 3) + return camera_features + + @property + def is_calibrated(self) -> bool: + return True + + def _impedance_enabled(self) -> bool: + return bool(self.config.use_joint_impedance) + + @check_if_already_connected + def connect(self, calibrate: bool = True) -> None: + del calibrate + sdk_python_dir = self._resolve_sdk_python_dir() + panthera_cls = self._import_panthera_class(sdk_python_dir) + config_path = self._resolve_config_path(sdk_python_dir) + self._robot = panthera_cls(config_path) if config_path else panthera_cls() + + if self._impedance_enabled() and self.config.run_startup_sequence: + self._run_impedance_startup_sequence() + + fk = self._robot.forward_kinematics() + if fk is None: + raise RuntimeError("Panthera forward_kinematics() returned None during connect.") + + with self._target_lock: + self._target_pos = np.array(fk["position"], dtype=float) + self._target_rot = np.array(fk["rotation"], dtype=float) + self._joint_target_q = np.asarray(self._robot.get_current_pos(), dtype=float) + self._gripper_target = float(self._robot.get_current_pos_gripper()) + + for cam in self.cameras.values(): + cam.connect() + + if self._impedance_enabled(): + self._start_impedance_loop() + + logger.info("%s connected.", self) + + def calibrate(self) -> None: + return None + + def configure(self) -> None: + return None + + @check_if_not_connected + def get_observation(self) -> RobotObservation: + assert self._robot is not None + q = np.asarray(self._robot.get_current_pos(), dtype=float) + dq = np.asarray(self._robot.get_current_vel(), dtype=float) + tau = np.asarray(self._robot.get_current_torque(), dtype=float) + fk = self._robot.forward_kinematics(q) + pos = np.asarray(fk["position"], dtype=float) if fk is not None else np.zeros(3, dtype=float) + gripper_torque = self._get_gripper_torque(self._robot) + + obs = { + "joint1.pos": float(q[0]), + "joint2.pos": float(q[1]), + "joint3.pos": float(q[2]), + "joint4.pos": float(q[3]), + "joint5.pos": float(q[4]), + "joint6.pos": float(q[5]), + "joint1.vel": float(dq[0]), + "joint2.vel": float(dq[1]), + "joint3.vel": float(dq[2]), + "joint4.vel": float(dq[3]), + "joint5.vel": float(dq[4]), + "joint6.vel": float(dq[5]), + "joint1.torque": float(tau[0]), + "joint2.torque": float(tau[1]), + "joint3.torque": float(tau[2]), + "joint4.torque": float(tau[3]), + "joint5.torque": float(tau[4]), + "joint6.torque": float(tau[5]), + "gripper.pos": float(self._robot.get_current_pos_gripper()), + "gripper.torque": gripper_torque, + "ee.x": float(pos[0]), + "ee.y": float(pos[1]), + "ee.z": float(pos[2]), + } + for name, cam in self.cameras.items(): + try: + frame = cam.async_read() + except Exception as exc: + logger.warning("Failed to read Panthera camera %s (%s); using cached frame", name, exc) + frame = self._last_camera_obs.get(name) + if frame is None: + frame = self._make_blank_camera_obs(name) + self._last_camera_obs[name] = frame + else: + self._last_camera_obs[name] = frame + obs[name] = frame + return obs + + @check_if_not_connected + def send_action(self, action: RobotAction) -> RobotAction: + assert self._robot is not None + assert self._target_pos is not None and self._target_rot is not None + + delta_x = float(action.get("delta_x", 0.0)) + delta_y = float(action.get("delta_y", 0.0)) + delta_z = float(action.get("delta_z", 0.0)) + delta_roll = float(action.get("delta_roll", 0.0)) + delta_pitch = float(action.get("delta_pitch", 0.0)) + delta_yaw = float(action.get("delta_yaw", 0.0)) + gripper = float(action.get("gripper", 1.0)) + + with self._target_lock: + self._apply_cartesian_delta(delta_x=delta_x, delta_y=delta_y, delta_z=delta_z) + self._apply_rotation_delta( + delta_roll=delta_roll, + delta_pitch=delta_pitch, + delta_yaw=delta_yaw, + ) + self._apply_gripper_delta(gripper, command_immediately=not self._impedance_enabled()) + target_pos = np.array(self._target_pos, dtype=float) + target_rot = np.array(self._target_rot, dtype=float) + q_seed = ( + np.array(self._joint_target_q, dtype=float) + if self._joint_target_q is not None + else np.asarray(self._robot.get_current_pos(), dtype=float) + ) + + if self._impedance_enabled(): + q_goal = self._robot.inverse_kinematics( + target_position=target_pos, + target_rotation=target_rot, + init_q=q_seed, + max_iter=self.config.ik_max_iter, + eps=self.config.ik_eps, + damping=self.config.ik_damping, + adaptive_damping=self.config.ik_adaptive_damping, + multi_init=self.config.ik_multi_init, + ) + if q_goal is not None: + with self._target_lock: + self._joint_target_q = np.asarray(q_goal, dtype=float) + else: + logger.debug("Panthera IK failed for impedance target %s", target_pos.tolist()) + return { + "delta_x": delta_x, + "delta_y": delta_y, + "delta_z": delta_z, + "delta_roll": delta_roll, + "delta_pitch": delta_pitch, + "delta_yaw": delta_yaw, + "gripper": gripper, + } + + q_cur = np.asarray(self._robot.get_current_pos(), dtype=float) + q_goal = self._robot.inverse_kinematics( + target_position=target_pos, + target_rotation=target_rot, + init_q=q_cur, + max_iter=self.config.ik_max_iter, + eps=self.config.ik_eps, + damping=self.config.ik_damping, + adaptive_damping=self.config.ik_adaptive_damping, + multi_init=self.config.ik_multi_init, + ) + + if q_goal is None: + logger.debug("Panthera IK failed for target %s", target_pos.tolist()) + return { + "delta_x": delta_x, + "delta_y": delta_y, + "delta_z": delta_z, + "delta_roll": delta_roll, + "delta_pitch": delta_pitch, + "delta_yaw": delta_yaw, + "gripper": gripper, + } + + vel = self.config.joint_velocity + if len(vel) != 6: + raise ValueError("panthera_arm.joint_velocity must contain exactly 6 elements.") + + self._robot.Joint_Pos_Vel( + pos=np.asarray(q_goal, dtype=float), + vel=np.asarray(vel, dtype=float), + max_tqu=np.asarray(self.config.max_torque, dtype=float) if self.config.max_torque else None, + iswait=False, + ) + + return { + "delta_x": delta_x, + "delta_y": delta_y, + "delta_z": delta_z, + "delta_roll": delta_roll, + "delta_pitch": delta_pitch, + "delta_yaw": delta_yaw, + "gripper": gripper, + } + + @check_if_not_connected + def disconnect(self) -> None: + assert self._robot is not None + + if self._impedance_thread is not None: + self._impedance_enabled_event.clear() + self._impedance_stop_event.set() + self._impedance_thread.join(timeout=1.5) + self._impedance_thread = None + self._impedance_stop_event.clear() + + if self.config.stop_on_disconnect and hasattr(self._robot, "set_stop"): + try: + self._robot.set_stop() + except Exception: + logger.debug("Panthera set_stop failed during disconnect", exc_info=True) + for cam in self.cameras.values(): + try: + cam.disconnect() + except Exception: + logger.warning("Failed to disconnect Panthera camera", exc_info=True) + self._robot = None + with self._target_lock: + self._target_pos = None + self._target_rot = None + self._joint_target_q = None + logger.info("%s disconnected.", self) + + def _apply_cartesian_delta(self, delta_x: float, delta_y: float, delta_z: float) -> None: + assert self._target_pos is not None + if delta_x == 0.0 and delta_y == 0.0 and delta_z == 0.0: + return + + x = float(self._target_pos[0] + delta_x * self.config.xy_step_m) + y = float(self._target_pos[1] + delta_y * self.config.xy_step_m) + z = float(self._target_pos[2] + delta_z * self.config.vertical_step_m) + + z = float(np.clip(z, self.config.min_z_m, self.config.max_z_m)) + radius = float(np.hypot(x, y)) + clamped_radius = float(np.clip(radius, self.config.min_radius_m, self.config.max_radius_m)) + if radius > 1e-9 and clamped_radius != radius: + scale = clamped_radius / radius + x *= scale + y *= scale + + self._target_pos = np.array([x, y, z], dtype=float) + + def _apply_rotation_delta(self, delta_roll: float, delta_pitch: float, delta_yaw: float) -> None: + assert self._target_rot is not None + if delta_roll == 0.0 and delta_pitch == 0.0 and delta_yaw == 0.0: + return + d_rot = Rot.from_euler( + "xyz", + [ + delta_roll * self.config.rotation_step_deg, + delta_pitch * self.config.rotation_step_deg, + delta_yaw * self.config.rotation_step_deg, + ], + degrees=True, + ).as_matrix() + self._target_rot = self._target_rot @ d_rot + + def _apply_gripper_delta(self, gripper_action: float, command_immediately: bool = True) -> None: + assert self._robot is not None + if gripper_action > 1.5: + self._gripper_target += self.config.gripper_step + elif gripper_action < 0.5: + self._gripper_target -= self.config.gripper_step + else: + return + + lower = -0.1 + upper = 2.0 + if hasattr(self._robot, "gripper_limits") and self._robot.gripper_limits: + lower = float(self._robot.gripper_limits.get("lower", lower)) + upper = float(self._robot.gripper_limits.get("upper", upper)) + self._gripper_target = float(np.clip(self._gripper_target, lower, upper)) + if not command_immediately: + return + self._robot.gripper_control( + pos=self._gripper_target, + vel=self.config.gripper_velocity, + max_tqu=self.config.gripper_max_torque, + ) + + def _start_impedance_loop(self) -> None: + assert self._robot is not None + required_methods = ("get_Gravity", "get_friction_compensation", "pos_vel_tqe_kp_kd", "inverse_kinematics") + missing_methods = [name for name in required_methods if not hasattr(self._robot, name)] + if missing_methods: + raise RuntimeError( + "Panthera SDK instance is missing required methods for impedance mode: " + f"{missing_methods}" + ) + self._validate_impedance_config() + self._impedance_enabled_event.set() + self._impedance_stop_event.clear() + self._impedance_thread = threading.Thread( + target=self._impedance_loop, + name="panthera_impedance_loop", + daemon=True, + ) + self._impedance_thread.start() + logger.info( + "Panthera joint impedance loop started at %.1f Hz.", + self.config.impedance_control_hz, + ) + + def _run_impedance_startup_sequence(self) -> None: + assert self._robot is not None + robot = self._robot + + required_methods = ("Joint_Pos_Vel", "inverse_kinematics", "moveJ", "moveL", "rotation_matrix_from_euler") + missing = [name for name in required_methods if not hasattr(robot, name)] + if missing: + logger.warning( + "Skipping Panthera startup sequence because SDK methods are missing: %s", + missing, + ) + return + + try: + motor_count = int(getattr(robot, "motor_count", 6)) + if motor_count <= 0: + return + + zero_pos = [0.0] * motor_count + zero_vel = list(self.config.joint_velocity[:motor_count]) + if len(zero_vel) < motor_count: + zero_vel.extend([0.5] * (motor_count - len(zero_vel))) + + max_torque = None + if self.config.max_torque: + max_torque = list(self.config.max_torque[:motor_count]) + if len(max_torque) < motor_count: + max_torque.extend([10.0] * (motor_count - len(max_torque))) + + robot.Joint_Pos_Vel(zero_pos, zero_vel, max_torque, iswait=True) + + if len(self.config.startup_home_pos_m) != 3 or len(self.config.startup_home_euler_rad) != 3: + raise ValueError( + "panthera_arm.startup_home_pos_m and startup_home_euler_rad must contain exactly 3 elements." + ) + + home_rot = robot.rotation_matrix_from_euler(*self.config.startup_home_euler_rad) + q_init = robot.inverse_kinematics( + target_position=self.config.startup_home_pos_m, + target_rotation=home_rot, + init_q=np.asarray(robot.get_current_pos(), dtype=float), + ) + if q_init is not None: + robot.moveJ( + q_init, + duration=self.config.startup_movej_duration_s, + max_tqu=max_torque, + iswait=True, + ) + + fk = robot.forward_kinematics() + if fk is not None: + p_lift = np.asarray(fk["position"], dtype=float) + p_lift[2] += float(self.config.startup_lift_m) + robot.moveL( + target_position=p_lift, + target_rotation=np.asarray(fk["rotation"], dtype=float), + duration=self.config.startup_lift_duration_s, + use_spline=True, + ) + except Exception: + logger.warning("Panthera startup sequence failed; continuing without startup pose sequence.", exc_info=True) + + def _validate_impedance_config(self) -> None: + if self.config.impedance_control_hz <= 0: + raise ValueError("panthera_arm.impedance_control_hz must be > 0.") + if self.config.joint_limit_margin_rad < 0: + raise ValueError("panthera_arm.joint_limit_margin_rad must be >= 0.") + if self.config.impedance_max_consecutive_errors <= 0: + raise ValueError("panthera_arm.impedance_max_consecutive_errors must be > 0.") + if self.config.impedance_error_log_interval_s <= 0: + raise ValueError("panthera_arm.impedance_error_log_interval_s must be > 0.") + + joint_vectors = ( + ("impedance_k_joint", self.config.impedance_k_joint), + ("impedance_b_joint", self.config.impedance_b_joint), + ("tau_limit", self.config.tau_limit), + ("friction_fc", self.config.friction_fc), + ("friction_fv", self.config.friction_fv), + ) + for name, values in joint_vectors: + if len(values) != 6: + raise ValueError(f"panthera_arm.{name} must contain exactly 6 elements.") + if len(self.config.startup_home_pos_m) != 3: + raise ValueError("panthera_arm.startup_home_pos_m must contain exactly 3 elements.") + if len(self.config.startup_home_euler_rad) != 3: + raise ValueError("panthera_arm.startup_home_euler_rad must contain exactly 3 elements.") + + def _impedance_loop(self) -> None: + assert self._robot is not None + robot = self._robot + dt = 1.0 / self.config.impedance_control_hz + consecutive_errors = 0 + alpha_dq = 1.0 + if self.config.dq_lpf_cutoff_hz > 0: + alpha_dq = (2.0 * np.pi * self.config.dq_lpf_cutoff_hz * dt) / ( + 1.0 + 2.0 * np.pi * self.config.dq_lpf_cutoff_hz * dt + ) + + k_joint = np.asarray(self.config.impedance_k_joint, dtype=float) + b_joint = np.asarray(self.config.impedance_b_joint, dtype=float) + tau_limit = np.asarray(self.config.tau_limit, dtype=float) + friction_fc = np.asarray(self.config.friction_fc, dtype=float) + friction_fv = np.asarray(self.config.friction_fv, dtype=float) + + dq_filtered = np.zeros(6, dtype=float) + dq_initialized = False + with self._target_lock: + if self._joint_target_q is None: + self._joint_target_q = np.asarray(robot.get_current_pos(), dtype=float) + if self._target_pos is None or self._target_rot is None: + fk = robot.forward_kinematics() + if fk is not None: + self._target_pos = np.array(fk["position"], dtype=float) + self._target_rot = np.array(fk["rotation"], dtype=float) + if self._target_pos is None: + self._target_pos = np.zeros(3, dtype=float) + if self._target_rot is None: + self._target_rot = np.eye(3, dtype=float) + last_feasible_pos = np.array(self._target_pos, dtype=float) + last_feasible_rot = np.array(self._target_rot, dtype=float) + last_feasible_q = np.array(self._joint_target_q, dtype=float) + + joint_lower: np.ndarray | None = None + joint_upper: np.ndarray | None = None + if ( + self.config.enforce_joint_limit_margin + and hasattr(robot, "joint_limits") + and getattr(robot, "joint_limits", None) + ): + lower = np.asarray(robot.joint_limits.get("lower"), dtype=float) + upper = np.asarray(robot.joint_limits.get("upper"), dtype=float) + if lower.shape[0] >= 6 and upper.shape[0] >= 6: + margin = float(self.config.joint_limit_margin_rad) + joint_lower = lower[:6] + margin + joint_upper = upper[:6] - margin + else: + logger.warning("Panthera joint_limits has unexpected shape; skipping joint limit margin checks.") + + while not self._impedance_stop_event.is_set(): + t0 = time.perf_counter() + if not self._impedance_enabled_event.is_set(): + time.sleep(min(dt, 0.01)) + continue + try: + q = np.asarray(robot.get_current_pos(), dtype=float) + dq_raw = np.asarray(robot.get_current_vel(), dtype=float) + if q.shape[0] != 6 or dq_raw.shape[0] != 6: + raise RuntimeError( + "Panthera impedance mode expects 6 arm joints. " + f"Got q={q.shape}, dq={dq_raw.shape}" + ) + + if not dq_initialized: + dq_filtered[:] = dq_raw + dq_initialized = True + else: + dq_filtered[:] = alpha_dq * dq_raw + (1.0 - alpha_dq) * dq_filtered + dq = dq_filtered + + if joint_lower is not None and joint_upper is not None: + violated = (q < joint_lower) | (q > joint_upper) + if np.any(violated): + with self._target_lock: + self._target_pos = np.array(last_feasible_pos, dtype=float) + self._target_rot = np.array(last_feasible_rot, dtype=float) + self._joint_target_q = np.array(last_feasible_q, dtype=float) + + violated_parts = [] + for i in range(6): + if violated[i]: + side = "lower" if q[i] < joint_lower[i] else "upper" + violated_parts.append(f"j{i+1}:{side}:{q[i]:+.3f}") + + logger.error( + "Panthera joint limit margin violation detected (%s). Triggering immediate hardware stop.", + ", ".join(violated_parts), + ) + try: + if hasattr(robot, "set_stop"): + robot.set_stop() + else: + logger.error( + "Panthera SDK does not expose set_stop(); cannot issue immediate hardware stop." + ) + except Exception: + logger.warning("Panthera set_stop failed during joint-limit emergency stop.", exc_info=True) + self._impedance_enabled_event.clear() + self._impedance_stop_event.set() + break + + with self._target_lock: + target_q = ( + np.array(self._joint_target_q, dtype=float) + if self._joint_target_q is not None + else np.array(q, dtype=float) + ) + target_pos = np.array(self._target_pos, dtype=float) if self._target_pos is not None else None + target_rot = np.array(self._target_rot, dtype=float) if self._target_rot is not None else None + gripper_target = float(self._gripper_target) + if target_q.shape[0] != 6: + raise RuntimeError(f"Panthera impedance target must contain 6 joints, got {target_q.shape}") + if target_pos is not None and target_rot is not None: + last_feasible_pos = target_pos.copy() + last_feasible_rot = target_rot.copy() + last_feasible_q = target_q.copy() + + tau_imp = k_joint * (target_q - q) + b_joint * (-dq) + tau_gravity = self._get_gravity_vector(robot, q) + tau_coriolis = np.zeros(6, dtype=float) + if self.config.enable_coriolis_comp and hasattr(robot, "get_Coriolis_vector"): + tau_coriolis = np.asarray(robot.get_Coriolis_vector(q, dq), dtype=float) + tau_friction = np.asarray( + robot.get_friction_compensation( + dq, + friction_fc, + friction_fv, + self.config.friction_vel_threshold, + ), + dtype=float, + ) + + tau_cmd = np.clip(tau_imp + tau_gravity + tau_coriolis + tau_friction, -tau_limit, tau_limit) + zero = np.zeros(6, dtype=float) + + # Match the manufacturer script: set gripper motor target before arm torque command. + if hasattr(robot, "Motors") and hasattr(robot, "gripper_id"): + robot.Motors[robot.gripper_id - 1].pos_vel_tqe_kp_kd( + gripper_target, + 0.0, + 0.0, + self.config.gripper_kp, + self.config.gripper_kd, + ) + else: + robot.gripper_control( + pos=gripper_target, + vel=self.config.gripper_velocity, + max_tqu=self.config.gripper_max_torque, + ) + + robot.pos_vel_tqe_kp_kd( + zero.tolist(), + zero.tolist(), + tau_cmd.tolist(), + zero.tolist(), + zero.tolist(), + ) + consecutive_errors = 0 + except Exception as exc: + consecutive_errors += 1 + now_s = time.perf_counter() + if now_s - self._last_impedance_error_log_s >= self.config.impedance_error_log_interval_s: + logger.warning("Panthera impedance loop iteration failed: %s", exc, exc_info=True) + self._last_impedance_error_log_s = now_s + if self.config.impedance_fail_safe_stop and consecutive_errors >= self.config.impedance_max_consecutive_errors: + logger.error( + "Panthera impedance loop entering fail-safe stop after %d consecutive errors.", + consecutive_errors, + ) + try: + if hasattr(robot, "set_stop"): + robot.set_stop() + except Exception: + logger.warning("Panthera fail-safe stop command failed.", exc_info=True) + self._impedance_enabled_event.clear() + self._impedance_stop_event.set() + break + + sleep_s = dt - (time.perf_counter() - t0) + if sleep_s > 0: + time.sleep(sleep_s) + + @staticmethod + def _get_gravity_vector(robot: Any, q: np.ndarray) -> np.ndarray: + return np.asarray(robot.get_Gravity(q), dtype=float) + + @staticmethod + def _get_gripper_torque(robot: Any) -> float: + if hasattr(robot, "get_current_torque_gripper"): + return float(robot.get_current_torque_gripper()) + state = robot.get_current_state_gripper() + return float(state.torque) + + def _resolve_sdk_python_dir(self) -> Path: + sdk_python_dir = Path(self.config.sdk_python_dir).expanduser() + if not sdk_python_dir.is_absolute(): + sdk_python_dir = (Path.cwd() / sdk_python_dir).resolve() + if not sdk_python_dir.exists(): + raise FileNotFoundError( + "Panthera SDK python directory not found: " + f"{sdk_python_dir}. Set `sdk_python_dir` in panthera_arm config." + ) + return sdk_python_dir + + def _resolve_config_path(self, sdk_python_dir: Path) -> str | None: + if not self.config.config_path: + return None + + path_in_cfg = Path(self.config.config_path).expanduser() + candidates: list[Path] = [] + if path_in_cfg.is_absolute(): + candidates.append(path_in_cfg.resolve()) + else: + candidates.append((Path.cwd() / path_in_cfg).resolve()) + # Manufacturer reference keeps config files under panthera_python/robot_param. + candidates.append((sdk_python_dir.parent / path_in_cfg).resolve()) + + for candidate in candidates: + if candidate.exists(): + return str(candidate) + + raise FileNotFoundError( + "Panthera config file not found. Checked: " + + ", ".join(str(path) for path in candidates) + + ". Update `config_path` in panthera_arm config." + ) + + def _import_panthera_class(self, sdk_python_dir: Path): + sdk_path = str(sdk_python_dir) + if sdk_path not in sys.path: + sys.path.insert(0, sdk_path) + + try: + from Panthera_lib import Panthera + except Exception as exc: + raise ImportError( + "Failed to import Panthera SDK (`from Panthera_lib import Panthera`). " + f"Verify sdk_python_dir and dependencies. sdk_python_dir={sdk_python_dir}" + ) from exc + return Panthera diff --git a/src/lerobot/robots/xlerobot/sub_robots/xlerobot_mount/__init__.py b/src/lerobot/robots/xlerobot/sub_robots/xlerobot_mount/__init__.py new file mode 100644 index 00000000000..8961309d7a5 --- /dev/null +++ b/src/lerobot/robots/xlerobot/sub_robots/xlerobot_mount/__init__.py @@ -0,0 +1,18 @@ +#!/usr/bin/env python + +# Copyright 2025 The HuggingFace Inc. team. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from .config import XLeRobotMountConfig +from .xlerobot_mount import XLeRobotMount diff --git a/src/lerobot/robots/xlerobot/sub_robots/xlerobot_mount/config.py b/src/lerobot/robots/xlerobot/sub_robots/xlerobot_mount/config.py new file mode 100644 index 00000000000..716249e3395 --- /dev/null +++ b/src/lerobot/robots/xlerobot/sub_robots/xlerobot_mount/config.py @@ -0,0 +1,41 @@ +# Copyright 2025 The HuggingFace Inc. team. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from dataclasses import dataclass + +from ....config import RobotConfig + + +@RobotConfig.register_subclass("xlerobot_mount") +@dataclass +class XLeRobotMountConfig(RobotConfig): + # Serial communication + port: str = "/dev/ttyACM5" # USB-to-serial port (adjust for your system) + + # Motor IDs + pan_motor_id: int = 1 + tilt_motor_id: int = 2 + motor_model: str = "sts3215" # Feetech STS3215 servo + + # Feature keys for observation/action dictionaries + pan_key: str = "mount_pan.pos" + tilt_key: str = "mount_tilt.pos" + + # Operational parameters + max_pan_speed_dps: float = 60.0 # Maximum pan speed in degrees per second + max_tilt_speed_dps: float = 45.0 # Maximum tilt speed in degrees per second + + # Safety limits (degrees) + pan_range: tuple[float, float] = (-90.0, 90.0) # Pan angle limits + tilt_range: tuple[float, float] = (-30.0, 60.0) # Tilt angle limits diff --git a/src/lerobot/robots/xlerobot/sub_robots/xlerobot_mount/xlerobot_mount.py b/src/lerobot/robots/xlerobot/sub_robots/xlerobot_mount/xlerobot_mount.py new file mode 100644 index 00000000000..3000d7109b1 --- /dev/null +++ b/src/lerobot/robots/xlerobot/sub_robots/xlerobot_mount/xlerobot_mount.py @@ -0,0 +1,185 @@ +# Copyright 2025 The HuggingFace Inc. team. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""XLeRobot Mount - Two-motor pan/tilt neck control for camera positioning.""" + +import logging +from functools import cached_property +from typing import Any + +from lerobot.motors import Motor, MotorCalibration, MotorNormMode +from lerobot.motors.feetech import FeetechMotorsBus, OperatingMode +from lerobot.utils.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError + +from ....robot import Robot +from .config import XLeRobotMountConfig + +logger = logging.getLogger(__name__) + + +class XLeRobotMount(Robot): + config_class = XLeRobotMountConfig + name = "xlerobot_mount" + + def __init__(self, config: XLeRobotMountConfig): + super().__init__(config) + self.config = config + + # Initialize motor bus with pan and tilt servos + self.bus = FeetechMotorsBus( + port=self.config.port, + motors={ + "mount_pan": Motor( + id=self.config.pan_motor_id, + model=self.config.motor_model, + norm_mode=MotorNormMode.DEGREES, + ), + "mount_tilt": Motor( + id=self.config.tilt_motor_id, + model=self.config.motor_model, + norm_mode=MotorNormMode.DEGREES, + ), + }, + calibration=self.calibration, + ) + + @cached_property + def observation_features(self) -> dict[str, type]: + return { + self.config.pan_key: float, + self.config.tilt_key: float, + } + + @cached_property + def action_features(self) -> dict[str, type]: + return { + self.config.pan_key: float, + self.config.tilt_key: float, + } + + @property + def is_connected(self) -> bool: + return self.bus.is_connected + + def connect(self, calibrate: bool = True, handshake: bool | None = None) -> None: + if self.is_connected: + raise DeviceAlreadyConnectedError(f"{self} already connected") + + self.bus.connect() + + if calibrate and not self.is_calibrated: + self.calibrate() + + self.configure() + logger.info("%s connected.", self) + + @property + def is_calibrated(self) -> bool: + return self.bus.is_calibrated + + def calibrate(self) -> None: + logger.info("Calibrating %s", self) + + if self.calibration: + user_input = input( + f"Press ENTER to use existing calibration (ID: {self.id}), " + f"or type 'c' and press ENTER to run new calibration: " + ) + if user_input.strip().lower() != "c": + logger.info("Using existing calibration file") + self.bus.write_calibration(self.calibration) + return + + # Run new calibration + logger.info("Setting up new calibration for mount motors") + self.bus.disable_torque() + + # Set motors to position mode for calibration + for motor in self.bus.motors: + self.bus.write("Operating_Mode", motor, OperatingMode.POSITION.value) + + input("Move the mount to center position (pan=0°, tilt=0°) and press ENTER...") + homing_offsets = self.bus.set_half_turn_homings() + + print( + "Move both pan and tilt joints through their full range of motion.\n" + "Recording positions. Press ENTER when done..." + ) + range_mins, range_maxes = self.bus.record_ranges_of_motion() + + # Create calibration for each motor + self.calibration = {} + for motor, m in self.bus.motors.items(): + self.calibration[motor] = MotorCalibration( + id=m.id, + drive_mode=0, + homing_offset=homing_offsets[motor], + range_min=range_mins[motor], + range_max=range_maxes[motor], + ) + + self.bus.write_calibration(self.calibration) + self._save_calibration() + logger.info("Calibration saved to %s", self.calibration_fpath) + + def configure(self) -> None: + with self.bus.torque_disabled(): + self.bus.configure_motors() + for motor in self.bus.motors: + # Set position control mode + self.bus.write("Operating_Mode", motor, OperatingMode.POSITION.value) + # Adjust PID coefficients for smooth motion (lower P to reduce shakiness) + self.bus.write("P_Coefficient", motor, 16) + self.bus.write("I_Coefficient", motor, 0) + self.bus.write("D_Coefficient", motor, 32) + + def disconnect(self) -> None: + if not self.is_connected: + raise DeviceNotConnectedError(f"{self} not connected") + + self.bus.disable_torque() + self.bus.disconnect() + logger.info("%s disconnected.", self) + + def get_observation(self) -> dict[str, Any]: + if not self.is_connected: + raise DeviceNotConnectedError(f"{self} not connected") + + positions = self.bus.sync_read("Present_Position", ["mount_pan", "mount_tilt"]) + return { + self.config.pan_key: positions["mount_pan"], + self.config.tilt_key: positions["mount_tilt"], + } + + def send_action(self, action: dict[str, Any]) -> dict[str, Any]: + if not self.is_connected: + raise DeviceNotConnectedError(f"{self} not connected") + + # Enforce safety limits + pan_cmd = max(self.config.pan_range[0], min(self.config.pan_range[1], action[self.config.pan_key])) + tilt_cmd = max( + self.config.tilt_range[0], min(self.config.tilt_range[1], action[self.config.tilt_key]) + ) + + # Send commands to motors + goal_positions = { + "mount_pan": pan_cmd, + "mount_tilt": tilt_cmd, + } + self.bus.sync_write("Goal_Position", goal_positions) + + return { + self.config.pan_key: pan_cmd, + self.config.tilt_key: tilt_cmd, + } diff --git a/src/lerobot/robots/xlerobot/xlerobot.py b/src/lerobot/robots/xlerobot/xlerobot.py new file mode 100644 index 00000000000..5953b6a7a45 --- /dev/null +++ b/src/lerobot/robots/xlerobot/xlerobot.py @@ -0,0 +1,471 @@ +#!/usr/bin/env python + +# Copyright 2025 The HuggingFace Inc. team. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# XLeRobot integration based on +# +# https://github.com/Astera-org/brainbot +# https://github.com/Vector-Wangel/XLeRobot +# https://github.com/bingogome/lerobot + +import logging +from functools import cached_property +from typing import Any + +import numpy as np + +from lerobot.cameras import CameraConfig +from lerobot.cameras.utils import make_cameras_from_configs +from lerobot.utils.errors import DeviceNotConnectedError + +from ..robot import Robot +from ..so_follower.config_so_follower import SO101FollowerConfig +from ..so_follower.so_follower import SO101Follower +from .config_xlerobot import XLeRobotConfig +from .shared_bus_mode.shared_bus import SharedComponentAttachment, build_shared_bus_group +from .sub_robots.biwheel_base.biwheel_feetech import BiwheelFeetech +from .sub_robots.biwheel_base.biwheel_odrive import BiwheelODrive +from .sub_robots.biwheel_base.config_biwheel_base import ( + BiwheelFeetechConfig, + BiwheelODriveConfig, +) +from .sub_robots.lekiwi_base.config import LeKiwiBaseConfig +from .sub_robots.lekiwi_base.lekiwi_base import LeKiwiBase +from .sub_robots.panthera_arm.config import PantheraArmConfig +from .sub_robots.panthera_arm.panthera_arm import PantheraArm +from .sub_robots.xlerobot_mount.config import XLeRobotMountConfig +from .sub_robots.xlerobot_mount.xlerobot_mount import XLeRobotMount + +logger = logging.getLogger(__name__) + + +class XLeRobot(Robot): + """Combined platform: bimanual SO-101 follower arms mounted on a configurable mobile base.""" + + config_class = XLeRobotConfig + name = "xlerobot" + + def __init__(self, config: XLeRobotConfig): + super().__init__(config) + self.config = config + self._component_ports = dict(config.component_ports) + + self.left_arm = self._build_arm("left_arm") + self.right_arm = self._build_arm("right_arm") + self.base = self._build_base_robot() + self.mount = self._build_mount_robot() + self.camera_configs = dict(config.cameras) + self.cameras = make_cameras_from_configs(self.camera_configs) + self._last_camera_obs: dict[str, np.ndarray] = { + name: self._make_blank_camera_obs(name) for name in self.cameras + } + self._shared_buses: dict[str, Any] = {} + self._setup_shared_buses() + + def _build_arm(self, component_name: str) -> Robot | None: + spec = getattr(self.config, component_name, {}) or {} + if not spec: + return None + arm_type = spec.get("type", "so101_follower") + + spec_copy = dict(spec) + spec_copy.pop("type", None) + + if arm_type in ("so101_follower", "so100_follower", "so_follower"): + cfg_dict = self._prepare_component_spec(component_name, spec_copy) + arm_config = SO101FollowerConfig(**cfg_dict) + return SO101Follower(arm_config) + if arm_type == "panthera_arm": + if spec_copy.get("shared_bus") is True: + raise ValueError( + f"Component '{component_name}' uses 'panthera_arm' and must not use shared buses " + "(different motor/driver stack). Set `shared_bus: false` and remove it from `shared_buses`." + ) + spec_copy["shared_bus"] = False + cfg_dict = self._prepare_component_spec(component_name, spec_copy) + arm_config = PantheraArmConfig(**cfg_dict) + return PantheraArm(arm_config) + + raise ValueError( + f"Unsupported arm type '{arm_type}' for {component_name}. " + "Supported: so101_follower, so100_follower, panthera_arm." + ) + + def _build_base_robot(self) -> Robot | None: + spec = getattr(self.config, "base", {}) or {} + if not spec: + return None + base_type = spec.get("type") + if base_type == "lekiwi_base": + cfg_cls = LeKiwiBaseConfig + robot_cls = LeKiwiBase + elif base_type in ("biwheel_base", "biwheel_feetech", "biwheel_odrive"): + driver = spec.get("driver") + use_odrive = base_type == "biwheel_odrive" or driver == "odrive" + if use_odrive: + cfg_cls = BiwheelODriveConfig + robot_cls = BiwheelODrive + else: + cfg_cls = BiwheelFeetechConfig + robot_cls = BiwheelFeetech + else: + raise ValueError( + "Base configuration must include a supported 'type' " + "(lekiwi_base, biwheel_base, biwheel_feetech, or biwheel_odrive)." + ) + + spec_copy = dict(spec) + spec_copy.pop("type", None) + spec_copy.pop("driver", None) + if use_odrive: + spec_copy.setdefault("shared_bus", False) + cfg_dict = self._prepare_component_spec("base", spec_copy) + base_config = cfg_cls(**cfg_dict) + return robot_cls(base_config) + + def _build_mount_robot(self) -> XLeRobotMount | None: + spec = getattr(self.config, "mount", {}) or {} + if not spec: + return None + cfg_dict = self._prepare_component_spec("mount", spec) + mount_config = XLeRobotMountConfig(**cfg_dict) + return XLeRobotMount(mount_config) + + def _make_blank_camera_obs(self, cam_key: str) -> np.ndarray: + cam_config = self.camera_configs.get(cam_key) + height = getattr(cam_config, "height", None) or 1 + width = getattr(cam_config, "width", None) or 1 + return np.zeros((height, width, 3), dtype=np.uint8) + + @property + def _cameras_ft(self) -> dict[str, tuple[int, int, int]]: + camera_features: dict[str, tuple[int, int, int]] = {} + for cam_key, cam_config in self.camera_configs.items(): + height = getattr(cam_config, "height", None) or 1 + width = getattr(cam_config, "width", None) or 1 + camera_features[cam_key] = (height, width, 3) + return camera_features + + @cached_property + def observation_features(self) -> dict[str, Any]: + features: dict[str, Any] = {} + if self.left_arm: + features.update(self._prefixed_features(self.left_arm.observation_features, "left_")) + if self.right_arm: + features.update(self._prefixed_features(self.right_arm.observation_features, "right_")) + if self.base: + features.update(self.base.observation_features) + if self.mount: + features.update(self.mount.observation_features) + features.update(self._cameras_ft) + return features + + @cached_property + def action_features(self) -> dict[str, Any]: + features: dict[str, Any] = {} + if self.left_arm: + features.update(self._prefixed_features(self.left_arm.action_features, "left_")) + if self.right_arm: + features.update(self._prefixed_features(self.right_arm.action_features, "right_")) + if self.base: + features.update(self.base.action_features) + if self.mount: + features.update(self.mount.action_features) + return features + + @property + def is_connected(self) -> bool: + components_connected = all( + comp.is_connected + for comp in (self.left_arm, self.right_arm, self.base, self.mount) + if comp is not None + ) + cameras_connected = all(cam.is_connected for cam in self.cameras.values()) + return components_connected and cameras_connected + + def connect(self, calibrate: bool = True) -> None: + if self.left_arm: + self.left_arm.connect(calibrate=calibrate) + if self.right_arm: + self.right_arm.connect(calibrate=calibrate) + if self.base: + handshake = getattr(self.base.config, "handshake_on_connect", True) + self.base.connect(calibrate=calibrate, handshake=handshake) + if self.mount: + self.mount.connect(calibrate=calibrate) + for cam in self.cameras.values(): + cam.connect() + + @property + def is_calibrated(self) -> bool: + return all( + comp.is_calibrated + for comp in (self.left_arm, self.right_arm, self.base, self.mount) + if comp is not None + ) + + def calibrate(self) -> None: + logger.info("Calibrating XLeRobot components") + if self.left_arm: + self.left_arm.calibrate() + if self.right_arm: + self.right_arm.calibrate() + if self.base: + self.base.calibrate() + if self.mount: + self.mount.calibrate() + + def configure(self) -> None: + if self.left_arm: + self.left_arm.configure() + if self.right_arm: + self.right_arm.configure() + if self.base: + self.base.configure() + if self.mount: + self.mount.configure() + + def setup_motors(self) -> None: + if self.left_arm and hasattr(self.left_arm, "setup_motors"): + self.left_arm.setup_motors() + if self.right_arm and hasattr(self.right_arm, "setup_motors"): + self.right_arm.setup_motors() + if self.base and hasattr(self.base, "setup_motors"): + self.base.setup_motors() + if self.mount and hasattr(self.mount, "setup_motors"): + self.mount.setup_motors() + + def get_observation(self) -> dict[str, Any]: + if not self.is_connected: + raise DeviceNotConnectedError("XLeRobot is not connected.") + + obs = {} + if self.left_arm: + obs.update(self._prefixed_obs(self.left_arm.get_observation(), "left_")) + if self.right_arm: + obs.update(self._prefixed_obs(self.right_arm.get_observation(), "right_")) + if self.base: + obs.update(self.base.get_observation()) + if self.mount: + obs.update(self.mount.get_observation()) + for name, cam in self.cameras.items(): + try: + frame = cam.async_read() + except Exception as exc: + logger.warning("Failed to read camera %s (%s); using cached frame", name, exc) + frame = self._last_camera_obs.get(name) + if frame is None: + frame = self._make_blank_camera_obs(name) + self._last_camera_obs[name] = frame + else: + self._last_camera_obs[name] = frame + obs[name] = frame + return obs + + def send_action(self, action: dict[str, Any]) -> dict[str, Any]: + if not self.is_connected: + raise DeviceNotConnectedError("XLeRobot is not connected.") + + sent: dict[str, Any] = {} + + if self.left_arm: + left_action = self._extract_prefixed_action(action, "left_") + sent.update(self._prefixed_obs(self.left_arm.send_action(left_action), "left_")) + + if self.right_arm: + right_action = self._extract_prefixed_action(action, "right_") + sent.update(self._prefixed_obs(self.right_arm.send_action(right_action), "right_")) + + if self.base: + base_action = {} + for key in ("x.vel", "y.vel", "theta.vel"): + if key in action: + base_action[key] = action[key] + elif f"base.{key}" in action: + base_action[key] = action[f"base.{key}"] + sent.update(self.base.send_action(base_action)) + + if self.mount: + mount_keys = set(self.mount.action_features.keys()) + mount_action = {key: value for key, value in action.items() if key in mount_keys} + if mount_action: + sent.update(self.mount.send_action(mount_action)) + + return sent + + def disconnect(self) -> None: + for comp in (self.base, self.mount, self.left_arm, self.right_arm): + if comp and comp.is_connected: + self._safe_disconnect(comp) + for cam in self.cameras.values(): + try: + cam.disconnect() + except DeviceNotConnectedError: + logger.debug("Camera %s not connected during disconnect", cam, exc_info=False) + except Exception: + logger.warning("Failed to disconnect camera", exc_info=True) + + def _prefixed_features(self, features: dict[str, Any], prefix: str) -> dict[str, Any]: + return {f"{prefix}{key}": value for key, value in features.items()} + + def _prefixed_obs(self, obs: dict[str, Any], prefix: str) -> dict[str, Any]: + return {f"{prefix}{key}": value for key, value in obs.items()} + + def _extract_prefixed_action(self, action: dict[str, Any], prefix: str) -> dict[str, Any]: + return {key.removeprefix(prefix): value for key, value in action.items() if key.startswith(prefix)} + + def _component_port(self, component_name: str) -> str: + port = self._component_ports.get(component_name) + if not port: + raise ValueError( + f"No shared bus provides a port for component '{component_name}'. " + "Declare it in `shared_buses`." + ) + return port + + def _prepare_component_spec(self, component_name: str, spec: dict[str, Any]) -> dict[str, Any]: + cfg = dict(spec) + if "cameras" in cfg and cfg["cameras"]: + cfg["cameras"] = self._coerce_camera_configs(cfg["cameras"], component_name) + shared_bus = cfg.pop("shared_bus", True) + if not shared_bus and component_name in self._component_ports: + raise ValueError( + f"Component '{component_name}' opts out of shared bus (`shared_bus: false`) but is still listed " + "in `shared_buses`." + ) + if component_name in self._component_ports: + port = self._component_port(component_name) + existing_port = cfg.get("port") + if existing_port and existing_port != port: + raise ValueError( + f"Component '{component_name}' specifies port '{existing_port}' but shared bus assigns '{port}'." + ) + cfg["port"] = port + elif shared_bus: + raise ValueError( + f"No shared bus provides a port for component '{component_name}'. " + "Declare it in `shared_buses`, or set `shared_bus: false` to opt out." + ) + if self.config.id and "id" not in cfg: + cfg["id"] = f"{self.config.id}_{component_name}" + if self.config.calibration_dir and cfg.get("calibration_dir") is None: + cfg["calibration_dir"] = self.config.calibration_dir + return cfg + + def _coerce_camera_configs( + self, camera_specs: dict[str, Any], component_name: str + ) -> dict[str, CameraConfig]: + coerced: dict[str, CameraConfig] = {} + for camera_name, camera_cfg in camera_specs.items(): + if isinstance(camera_cfg, CameraConfig): + coerced[camera_name] = camera_cfg + continue + if not isinstance(camera_cfg, dict): + raise TypeError( + f"Component '{component_name}' camera '{camera_name}' must be a dict or CameraConfig, " + f"got {type(camera_cfg)}." + ) + camera_type = camera_cfg.get("type") + if not camera_type: + raise ValueError( + f"Component '{component_name}' camera '{camera_name}' is missing required field 'type'." + ) + camera_cls = CameraConfig.get_choice_class(camera_type) + camera_kwargs = dict(camera_cfg) + camera_kwargs.pop("type", None) + coerced[camera_name] = camera_cls(**camera_kwargs) + return coerced + + def _setup_shared_buses(self) -> None: + if not getattr(self.config, "shared_buses_config", None): + return + + component_map = { + "left_arm": self.left_arm, + "right_arm": self.right_arm, + "base": self.base, + "mount": self.mount, + } + + for name, bus_cfg in self.config.shared_buses_config.items(): + attachments: list[SharedComponentAttachment] = [] + for device_cfg in bus_cfg.components: + component = component_map.get(device_cfg.component) + if component is None: + continue + if getattr(component, "supports_shared_bus", True) is False: + raise ValueError( + f"Component '{device_cfg.component}' does not support shared buses but is " + "listed in `shared_buses`." + ) + if not hasattr(component, "bus"): + continue + motor_names = tuple(component.bus.motors.keys()) + attachments.append( + SharedComponentAttachment( + name=device_cfg.component, + component=component, + motor_names=motor_names, + motor_id_offset=device_cfg.motor_id_offset, + ) + ) + if attachments: + group, _ = build_shared_bus_group( + name, + port=bus_cfg.port, + attachments=attachments, + handshake_on_connect=bus_cfg.handshake_on_connect, + ) + self._shared_buses[name] = group + + def _safe_disconnect(self, component: Robot) -> None: + try: + if hasattr(component, "stop_base"): + component.stop_base() + component.disconnect() + except DeviceNotConnectedError: + logger.debug("%s already disconnected", component, exc_info=False) + except Exception as exc: + detail = self._describe_component(component) + exc_type = type(exc).__name__ + logger.warning( + "Failed to disconnect %s (%s: %s). Details: %s", + component, + exc_type, + exc, + detail, + exc_info=True, + ) + print(f"[XLeRobot] Failed to disconnect {component}: {exc_type}: {exc} | {detail}") + + def _describe_component(self, component: Robot) -> str: + parts: list[str] = [f"class={component.__class__.__name__}"] + config = getattr(component, "config", None) + if config: + comp_id = getattr(config, "id", None) + if comp_id: + parts.append(f"id={comp_id}") + port = getattr(config, "port", None) + if port: + parts.append(f"port={port}") + bus = getattr(component, "bus", None) + if bus: + bus_port = getattr(bus, "port", None) + if bus_port: + parts.append(f"bus_port={bus_port}") + motors = getattr(bus, "motors", None) + if motors: + parts.append(f"motors={list(motors.keys())}") + return ", ".join(parts) diff --git a/src/lerobot/scripts/lerobot_record.py b/src/lerobot/scripts/lerobot_record.py index dc682fe6f7f..80a185c7d33 100644 --- a/src/lerobot/scripts/lerobot_record.py +++ b/src/lerobot/scripts/lerobot_record.py @@ -114,6 +114,13 @@ reachy2, so_follower, unitree_g1 as unitree_g1_robot, + xlerobot, +) +from lerobot.robots.xlerobot.sub_robots import ( # noqa: F401 + biwheel_base, + lekiwi_base, + panthera_arm, + xlerobot_mount, ) from lerobot.teleoperators import ( # noqa: F401 Teleoperator, @@ -129,6 +136,7 @@ reachy2_teleoperator, so_leader, unitree_g1, + xlerobot_teleoperator, ) from lerobot.teleoperators.keyboard.teleop_keyboard import KeyboardTeleop from lerobot.utils.constants import ACTION, OBS_STR diff --git a/src/lerobot/scripts/lerobot_replay.py b/src/lerobot/scripts/lerobot_replay.py index 7c0b5b96b45..3caa524bf3f 100644 --- a/src/lerobot/scripts/lerobot_replay.py +++ b/src/lerobot/scripts/lerobot_replay.py @@ -64,6 +64,13 @@ reachy2, so_follower, unitree_g1, + xlerobot, +) +from lerobot.robots.xlerobot.sub_robots import ( # noqa: F401 + biwheel_base, + lekiwi_base, + panthera_arm, + xlerobot_mount, ) from lerobot.utils.constants import ACTION from lerobot.utils.import_utils import register_third_party_plugins diff --git a/src/lerobot/scripts/lerobot_teleoperate.py b/src/lerobot/scripts/lerobot_teleoperate.py index f050d572a8d..f75e3364643 100644 --- a/src/lerobot/scripts/lerobot_teleoperate.py +++ b/src/lerobot/scripts/lerobot_teleoperate.py @@ -82,6 +82,13 @@ reachy2, so_follower, unitree_g1 as unitree_g1_robot, + xlerobot, +) +from lerobot.robots.xlerobot.sub_robots import ( # noqa: F401 + biwheel_base, + lekiwi_base, + panthera_arm, + xlerobot_mount, ) from lerobot.teleoperators import ( # noqa: F401 Teleoperator, @@ -99,6 +106,7 @@ reachy2_teleoperator, so_leader, unitree_g1, + xlerobot_teleoperator, ) from lerobot.utils.import_utils import register_third_party_plugins from lerobot.utils.robot_utils import precise_sleep diff --git a/src/lerobot/teleoperators/utils.py b/src/lerobot/teleoperators/utils.py index db685f39634..91537e1bc12 100644 --- a/src/lerobot/teleoperators/utils.py +++ b/src/lerobot/teleoperators/utils.py @@ -99,6 +99,32 @@ def make_teleoperator_from_config(config: TeleoperatorConfig) -> "Teleoperator": from .openarm_mini import OpenArmMini return OpenArmMini(config) + elif config.type == "lekiwi_base_gamepad": + from .xlerobot_teleoperator.sub_teleoperators.lekiwi_base_gamepad.teleop_lekiwi_base_gamepad import ( + LeKiwiBaseTeleop, + ) + + return LeKiwiBaseTeleop(config) + elif config.type == "biwheel_gamepad": + from .xlerobot_teleoperator.sub_teleoperators.biwheel_gamepad import BiwheelGamepadTeleop + + return BiwheelGamepadTeleop(config) + elif config.type == "xlerobot_mount_gamepad": + from .xlerobot_teleoperator.sub_teleoperators.xlerobot_mount_gamepad import XLeRobotMountGamepadTeleop + + return XLeRobotMountGamepadTeleop(config) + elif config.type == "panthera_keyboard_ee": + from .xlerobot_teleoperator.sub_teleoperators.panthera_keyboard_ee import PantheraKeyboardEETeleop + + return PantheraKeyboardEETeleop(config) + elif config.type == "xlerobot_default_composite": + from .xlerobot_teleoperator.default_composite import XLeRobotDefaultComposite + + return XLeRobotDefaultComposite(config) + elif config.type == "xlerobot_keyboard_composite": + from .xlerobot_teleoperator.keyboard_composite import XLeRobotKeyboardComposite + + return XLeRobotKeyboardComposite(config) else: try: return cast("Teleoperator", make_device_from_device_class(config)) diff --git a/src/lerobot/teleoperators/xlerobot_teleoperator/README.md b/src/lerobot/teleoperators/xlerobot_teleoperator/README.md new file mode 100644 index 00000000000..87144bb6ee7 --- /dev/null +++ b/src/lerobot/teleoperators/xlerobot_teleoperator/README.md @@ -0,0 +1,231 @@ +# XLeRobot Teleoperators + +An example run with so101 arms: [video](https://youtu.be/OGI-Qtl3s6s) + +This package introduces the teleoperation stack for the modular `xlerobot` platform. It includes two composite teleoperators (`xlerobot_default_composite` and `xlerobot_keyboard_composite`) plus reusable sub-teleoperators for LeKiwi/BiWheel base control, mount pan/tilt, bimanual SO-101 leader arms, and single-arm Panthera keyboard control (cartesian). + +## Folder structure + +```text +src/lerobot/teleoperators/xlerobot_teleoperator/ +|-- README.md +|-- __init__.py +|-- configs/ +| |-- base_only.json +| |-- xlerobot_default_composite_lekiwi.json +| |-- xlerobot_keyboard_composite_biwheel.json +| |-- xlerobot_keyboard_composite_panthera_left_ee.json +| `-- xlerobot_keyboard_composite_panthera_left_ee_with_base.json +|-- default_composite/ +| |-- __init__.py +| |-- config.py +| `-- teleop.py +|-- keyboard_composite/ +| |-- __init__.py +| |-- config.py +| `-- teleop.py +`-- sub_teleoperators/ + |-- __init__.py + |-- biwheel_gamepad/ + | |-- config_biwheel_gamepad.py + | `-- teleop_biwheel_gamepad.py + |-- biwheel_keyboard/ + | |-- configuration_biwheel_keyboard.py + | `-- teleop_biwheel_keyboard.py + |-- lekiwi_base_gamepad/ + | |-- config_lekiwi_base_gamepad.py + | `-- teleop_lekiwi_base_gamepad.py + |-- panthera_keyboard_ee/ + | |-- config_panthera_keyboard_ee.py + | `-- teleop_panthera_keyboard_ee.py + `-- xlerobot_mount_gamepad/ + |-- config.py + `-- teleop.py +``` + +## Default configs + +Sample teleoperator configuration files live in `src/lerobot/teleoperators/xlerobot_teleoperator/configs/`. +You can use them with `lerobot-teleoperate` via `--teleop.config_file` and override individual fields on the CLI. +The JSON files include a `_comment` field that documents the intended CLI flag. + +```bash +lerobot-teleoperate \ + --robot.type=xlerobot \ + --robot.config_file=src/lerobot/robots/xlerobot/configs/xlerobot_biwheel_feetech.json \ + --teleop.type=xlerobot_default_composite \ + --teleop.config_file=src/lerobot/teleoperators/xlerobot_teleoperator/configs/xlerobot_default_composite_lekiwi.json \ + --teleop.base.max_speed_mps=0.6 +``` + +Keyboard base control with the built-in LeRobot keyboard teleop: + +```bash +lerobot-teleoperate \ + --robot.type=xlerobot \ + --robot.config_file=src/lerobot/robots/xlerobot/configs/base_only.json \ + --teleop.type=xlerobot_keyboard_composite \ + --teleop.config_file=src/lerobot/teleoperators/xlerobot_teleoperator/configs/xlerobot_keyboard_composite_biwheel.json +``` + +Panthera arm keyboard teleop (Cartesian EE mapping) with xlerobot: + +```bash +lerobot-teleoperate \ + --robot.type=xlerobot \ + --robot.config_file=src/lerobot/robots/xlerobot/configs/xlerobot_biwheel_odrive_panthera_left.json \ + --teleop.type=xlerobot_keyboard_composite \ + --teleop.config_file=src/lerobot/teleoperators/xlerobot_teleoperator/configs/xlerobot_keyboard_composite_panthera_left_ee.json +``` +The provided robot config enables Panthera joint impedance + gravity/friction compensation. + +For Cartesian arm + keyboard base together, use: + +```bash +lerobot-teleoperate \ + --robot.type=xlerobot \ + --robot.config_file=src/lerobot/robots/xlerobot/configs/xlerobot_biwheel_odrive_panthera_left.json \ + --teleop.type=xlerobot_keyboard_composite \ + --teleop.config_file=src/lerobot/teleoperators/xlerobot_teleoperator/configs/xlerobot_keyboard_composite_panthera_left_ee_with_base.json +``` + +The original CLI-only setup still works too (no config files needed): + +```bash +lerobot-teleoperate \ + --robot.type=xlerobot \ + --teleop.type=xlerobot_default_composite \ + --teleop.base_type=lekiwi_base_gamepad \ + --teleop.base='{"joystick_index": 0, "max_speed_mps": 0.6, "deadzone": 0.15, "yaw_speed_deg": 45}' +``` + +## Remote keyboard teleoperation with Rerun + +Use this when `lerobot-teleoperate` runs on Thor and you want to view Rerun on your local machine. + +### Linux local machine + +Start local viewer: + +```bash +rerun +``` + +Connect to Thor: + +```bash +ssh -Y thor@thor +``` + +Then run teleop on Thor with your local viewer machine IP: + +```bash +lerobot-teleoperate \ + --robot.type=xlerobot \ + --robot.config_file=src/lerobot/robots/xlerobot/configs/xlerobot_biwheel_odrive_panthera_left.json \ + --teleop.type=xlerobot_keyboard_composite \ + --teleop.config_file=src/lerobot/teleoperators/xlerobot_teleoperator/configs/xlerobot_keyboard_composite_panthera_left_ee_with_base.json \ + --display_data=true \ + --fps=30 \ + --display_ip= \ + --display_port=9876 +``` + +### macOS local machine (XQuartz + RECORD + xterm keyboard capture) + +Install and start XQuartz: + +```bash +brew install --cask xquartz +open -a XQuartz +``` + +Enable test extensions (includes `RECORD`), then restart XQuartz: + +```bash +defaults write org.xquartz.X11 enable_test_extensions -boolean true +killall XQuartz +open -a XQuartz +``` + +Verify `RECORD` exists: + +```bash +/opt/X11/bin/xdpyinfo | grep -i RECORD +``` + +If this prints nothing, `pynput` keyboard listeners can fail with +`AttributeError: record_create_context`. + +Open `xterm` from XQuartz and use it for keyboard teleop control. +Keyboard events for `pynput` are captured from the focused X11 window (`xterm`), not from regular macOS Terminal. + +Start Rerun on Mac and keep it running: + +```bash +rerun +``` + +Connect to Thor: + +```bash +ssh -Y thor@thor +``` + +On Thor, confirm X11 forwarding is active: + +```bash +echo "$DISPLAY" +``` + +It should be non-empty (for example `localhost:10.0`). + +Optional: tune Rerun micro-batching on Thor before starting teleop: + +```bash +export RERUN_FLUSH_TICK_SECS=0.03 +export RERUN_FLUSH_NUM_BYTES=65536 +export RERUN_FLUSH_NUM_ROWS=4096 +``` + +Then run teleop on Thor (direct to Mac IP): + +```bash +lerobot-teleoperate \ + --robot.type=xlerobot \ + --robot.config_file=src/lerobot/robots/xlerobot/configs/xlerobot_biwheel_odrive_panthera_left.json \ + --teleop.type=xlerobot_keyboard_composite \ + --teleop.config_file=src/lerobot/teleoperators/xlerobot_teleoperator/configs/xlerobot_keyboard_composite_panthera_left_ee_with_base.json \ + --display_data=true \ + --fps=30 \ + --display_ip= \ + --display_port=9876 \ + --display_compressed_images=True +``` + +Replace `` with your Mac LAN IP. + +*Note: After the robot starts, use XQuartz `xterm` for keyboard teleoperation. Keystrokes entered in the macOS Terminal app are not captured.* + +## Default composite (`xlerobot_default_composite`) + +Defined in `default_composite/teleop.py`, this teleoperator merges: + +- A `BiSOLeader` instance (`arms_config`) for dual-arm control. +- A base gamepad teleop (`base_config`) that can target either the LeKiwi or BiWheel drive via Xbox gamepad axes. +- A mount gamepad teleop (`mount_config`) for pan/tilt camera motion. + +Each sub-teleoperator contributes its action/feedback schema, and the composite exposes a single dictionary that policies, recorders, and replayers can consume. Script updates (`lerobot-record`, `lerobot-replay`, `lerobot-teleoperate`) already know about this new teleop type. + +## Keyboard composite (`xlerobot_keyboard_composite`) + +Defined in `keyboard_composite/teleop.py`, this variant keeps the same composite pattern while swapping the base controller: + +- An optional Panthera arm keyboard teleop (`arm_config`) with type `panthera_keyboard_ee`. +- `arm_side` routes arm actions to either `left_` or `right_` action namespace. +- A `KeyboardRoverTeleop` instance (`base_config`) from `lerobot.teleoperators.keyboard`. +- An optional `XLeRobotMountGamepadTeleop` instance (`mount_config`) for pan/tilt. + +The keyboard rover output (`linear.vel`, `angular.vel`) is mapped to XLeRobot base keys (`x.vel`, `theta.vel`) so it works with `biwheel_base`/`biwheel_odrive` action interfaces. Panthera arm keyboard output is prefixed (`left_...` or `right_...`) for xlerobot arm routing. + +--- diff --git a/src/lerobot/teleoperators/xlerobot_teleoperator/__init__.py b/src/lerobot/teleoperators/xlerobot_teleoperator/__init__.py new file mode 100644 index 00000000000..d69f8951afe --- /dev/null +++ b/src/lerobot/teleoperators/xlerobot_teleoperator/__init__.py @@ -0,0 +1,51 @@ +# Copyright 2025 The HuggingFace Inc. team. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# XLeRobot integration based on +# +# https://github.com/Astera-org/brainbot +# https://github.com/Vector-Wangel/XLeRobot +# https://github.com/bingogome/lerobot + +from .default_composite import XLeRobotDefaultComposite, XLeRobotDefaultCompositeConfig +from .keyboard_composite import XLeRobotKeyboardComposite, XLeRobotKeyboardCompositeConfig +from .sub_teleoperators import ( + BiwheelGamepadTeleop, + BiwheelGamepadTeleopConfig, + BiwheelKeyboardTeleop, + BiwheelKeyboardTeleopConfig, + LeKiwiBaseTeleop, + LeKiwiBaseTeleopConfig, + PantheraKeyboardEETeleop, + PantheraKeyboardEETeleopConfig, + XLeRobotMountGamepadTeleop, + XLeRobotMountGamepadTeleopConfig, +) + +__all__ = [ + "XLeRobotDefaultComposite", + "XLeRobotDefaultCompositeConfig", + "XLeRobotKeyboardComposite", + "XLeRobotKeyboardCompositeConfig", + "BiwheelGamepadTeleop", + "BiwheelGamepadTeleopConfig", + "BiwheelKeyboardTeleop", + "BiwheelKeyboardTeleopConfig", + "LeKiwiBaseTeleop", + "LeKiwiBaseTeleopConfig", + "PantheraKeyboardEETeleop", + "PantheraKeyboardEETeleopConfig", + "XLeRobotMountGamepadTeleop", + "XLeRobotMountGamepadTeleopConfig", +] diff --git a/src/lerobot/teleoperators/xlerobot_teleoperator/configs/base_only.json b/src/lerobot/teleoperators/xlerobot_teleoperator/configs/base_only.json new file mode 100644 index 00000000000..1879154e8ef --- /dev/null +++ b/src/lerobot/teleoperators/xlerobot_teleoperator/configs/base_only.json @@ -0,0 +1,12 @@ +{ + "type": "xlerobot_default_composite", + "arms": null, + "base_type": "biwheel_gamepad", + "base": { + "joystick_index": 0, + "max_speed_mps": 0.8, + "yaw_speed_deg": 45.0, + "deadzone": 0.15 + }, + "mount": {} +} diff --git a/src/lerobot/teleoperators/xlerobot_teleoperator/configs/xlerobot_default_composite_lekiwi.json b/src/lerobot/teleoperators/xlerobot_teleoperator/configs/xlerobot_default_composite_lekiwi.json new file mode 100644 index 00000000000..8df4fe99a88 --- /dev/null +++ b/src/lerobot/teleoperators/xlerobot_teleoperator/configs/xlerobot_default_composite_lekiwi.json @@ -0,0 +1,32 @@ +{ + "_comment": "Use with --teleop.config_file=src/lerobot/teleoperators/xlerobot_teleoperator/configs/xlerobot_default_composite_lekiwi.json", + "type": "xlerobot_default_composite", + "arms": { + "left_arm_config": { + "port": "/dev/ttyACM0" + }, + "right_arm_config": { + "port": "/dev/ttyACM1" + } + }, + "base_type": "lekiwi_base_gamepad", + "base": { + "joystick_index": 0, + "max_speed_mps": 1.0, + "yaw_speed_deg": 30.0, + "deadzone": 0.15 + }, + "mount": { + "joystick_index": 0, + "deadzone": 0.15, + "polling_fps": 50, + "max_pan_speed_dps": 60.0, + "max_tilt_speed_dps": 45.0, + "pan_axis": 3, + "tilt_axis": 4, + "invert_pan": false, + "invert_tilt": true, + "pan_range": [-90.0, 90.0], + "tilt_range": [-30.0, 60.0] + } +} diff --git a/src/lerobot/teleoperators/xlerobot_teleoperator/configs/xlerobot_keyboard_composite_biwheel.json b/src/lerobot/teleoperators/xlerobot_teleoperator/configs/xlerobot_keyboard_composite_biwheel.json new file mode 100644 index 00000000000..792ba17c4dd --- /dev/null +++ b/src/lerobot/teleoperators/xlerobot_teleoperator/configs/xlerobot_keyboard_composite_biwheel.json @@ -0,0 +1,16 @@ +{ + "_comment": "Use with --teleop.config_file=src/lerobot/teleoperators/xlerobot_teleoperator/configs/xlerobot_keyboard_composite_biwheel.json", + "type": "xlerobot_keyboard_composite", + "arm_side": "left", + "arm": {}, + "base": { + "linear_speed": 0.4, + "angular_speed": 80.0, + "speed_increment": 0.05, + "turn_assist_ratio": 0.0, + "angular_speed_ratio": 200.0, + "min_linear_speed": 0.05, + "min_angular_speed": 10.0 + }, + "mount": {} +} diff --git a/src/lerobot/teleoperators/xlerobot_teleoperator/configs/xlerobot_keyboard_composite_panthera_left_ee.json b/src/lerobot/teleoperators/xlerobot_teleoperator/configs/xlerobot_keyboard_composite_panthera_left_ee.json new file mode 100644 index 00000000000..849c7c8a0dd --- /dev/null +++ b/src/lerobot/teleoperators/xlerobot_teleoperator/configs/xlerobot_keyboard_composite_panthera_left_ee.json @@ -0,0 +1,24 @@ +{ + "_comment": "Use with --teleop.config_file=src/lerobot/teleoperators/xlerobot_teleoperator/configs/xlerobot_keyboard_composite_panthera_left_ee.json", + "type": "xlerobot_keyboard_composite", + "arm_side": "left", + "arm": { + "type": "panthera_keyboard_ee", + "key_x_pos": "t", + "key_x_neg": "g", + "key_y_pos": "f", + "key_y_neg": "h", + "key_z_pos": "r", + "key_z_neg": "v", + "key_pitch_pos": "i", + "key_pitch_neg": "k", + "key_yaw_pos": "j", + "key_yaw_neg": "l", + "key_roll_pos": "u", + "key_roll_neg": "o", + "key_gripper_close": "z", + "key_gripper_open": "c" + }, + "base": null, + "mount": {} +} diff --git a/src/lerobot/teleoperators/xlerobot_teleoperator/configs/xlerobot_keyboard_composite_panthera_left_ee_with_base.json b/src/lerobot/teleoperators/xlerobot_teleoperator/configs/xlerobot_keyboard_composite_panthera_left_ee_with_base.json new file mode 100644 index 00000000000..1b1bbff314d --- /dev/null +++ b/src/lerobot/teleoperators/xlerobot_teleoperator/configs/xlerobot_keyboard_composite_panthera_left_ee_with_base.json @@ -0,0 +1,32 @@ +{ + "_comment": "Use with --teleop.config_file=src/lerobot/teleoperators/xlerobot_teleoperator/configs/xlerobot_keyboard_composite_panthera_left_ee_with_base.json", + "type": "xlerobot_keyboard_composite", + "arm_side": "left", + "arm": { + "type": "panthera_keyboard_ee", + "key_x_pos": "t", + "key_x_neg": "g", + "key_y_pos": "f", + "key_y_neg": "h", + "key_z_pos": "r", + "key_z_neg": "v", + "key_pitch_pos": "i", + "key_pitch_neg": "k", + "key_yaw_pos": "j", + "key_yaw_neg": "l", + "key_roll_pos": "u", + "key_roll_neg": "o", + "key_gripper_close": "z", + "key_gripper_open": "c" + }, + "base": { + "linear_speed": 0.4, + "angular_speed": 80.0, + "speed_increment": 0.05, + "turn_assist_ratio": 0.0, + "angular_speed_ratio": 200.0, + "min_linear_speed": 0.05, + "min_angular_speed": 10.0 + }, + "mount": {} +} diff --git a/src/lerobot/teleoperators/xlerobot_teleoperator/default_composite/__init__.py b/src/lerobot/teleoperators/xlerobot_teleoperator/default_composite/__init__.py new file mode 100644 index 00000000000..90b16e511c6 --- /dev/null +++ b/src/lerobot/teleoperators/xlerobot_teleoperator/default_composite/__init__.py @@ -0,0 +1,27 @@ +# Copyright 2025 The HuggingFace Inc. team. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# XLeRobot integration based on +# +# https://github.com/Astera-org/brainbot +# https://github.com/Vector-Wangel/XLeRobot +# https://github.com/bingogome/lerobot + +from .config import XLeRobotDefaultCompositeConfig +from .teleop import XLeRobotDefaultComposite + +__all__ = [ + "XLeRobotDefaultComposite", + "XLeRobotDefaultCompositeConfig", +] diff --git a/src/lerobot/teleoperators/xlerobot_teleoperator/default_composite/config.py b/src/lerobot/teleoperators/xlerobot_teleoperator/default_composite/config.py new file mode 100644 index 00000000000..97264beae0c --- /dev/null +++ b/src/lerobot/teleoperators/xlerobot_teleoperator/default_composite/config.py @@ -0,0 +1,135 @@ +# Copyright 2025 The HuggingFace Inc. team. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# XLeRobot integration based on +# +# https://github.com/Astera-org/brainbot +# https://github.com/Vector-Wangel/XLeRobot +# https://github.com/bingogome/lerobot + +from dataclasses import dataclass, field +from typing import Any + +import draccus + +from lerobot.configs import parser + +from ...bi_so_leader import BiSOLeaderConfig +from ...config import TeleoperatorConfig +from ...so_leader import SOLeaderConfig +from ..sub_teleoperators.biwheel_gamepad.config_biwheel_gamepad import BiwheelGamepadTeleopConfig +from ..sub_teleoperators.lekiwi_base_gamepad.config_lekiwi_base_gamepad import LeKiwiBaseTeleopConfig +from ..sub_teleoperators.xlerobot_mount_gamepad.config import XLeRobotMountGamepadTeleopConfig + + +@TeleoperatorConfig.register_subclass("xlerobot_default_composite") +@dataclass +class XLeRobotDefaultCompositeConfig(TeleoperatorConfig): + """Configuration for composite XLeRobot teleoperation with leader arms and gamepad. + + This composite teleoperator combines: + - BiSOLeader: Leader arms for controlling follower arms + - LeKiwiBaseTeleop: Xbox gamepad left stick for base control + - XLeRobotMountGamepadTeleop: Xbox gamepad right stick for mount control + """ + + BASE_TYPE_LEKIWI = "lekiwi_base_gamepad" + BASE_TYPE_BIWHEEL = "biwheel_gamepad" + + _comment: str | None = None + config_file: str | None = None + arms: dict[str, Any] = field(default_factory=dict) + base: dict[str, Any] = field(default_factory=dict) + mount: dict[str, Any] = field(default_factory=dict) + base_type: str | None = BASE_TYPE_LEKIWI + + def __post_init__(self) -> None: + if self.config_file: + self._load_from_config_file(self.config_file) + + arms_cfg = self._parse_arms_config(self.arms) + + base_cfg: LeKiwiBaseTeleopConfig | BiwheelGamepadTeleopConfig | None = None + base_type = self.base_type or self.BASE_TYPE_LEKIWI + if self.base: + if base_type == self.BASE_TYPE_LEKIWI: + base_cfg = ( + self.base + if isinstance(self.base, LeKiwiBaseTeleopConfig) + else LeKiwiBaseTeleopConfig(**self.base) + ) + elif base_type == self.BASE_TYPE_BIWHEEL: + base_cfg = ( + self.base + if isinstance(self.base, BiwheelGamepadTeleopConfig) + else BiwheelGamepadTeleopConfig(**self.base) + ) + else: + raise ValueError(f"Unsupported XLeRobot base type: {base_type}") + else: + base_type = None + + mount_cfg: XLeRobotMountGamepadTeleopConfig | None = None + if isinstance(self.mount, XLeRobotMountGamepadTeleopConfig): + mount_cfg = self.mount + elif self.mount: + mount_cfg = XLeRobotMountGamepadTeleopConfig(**self.mount) + + self.arms_config = arms_cfg + self.base_config = base_cfg + self.mount_config = mount_cfg + self.base_type = base_type + + def _parse_so_leader_config(self, value: SOLeaderConfig | dict[str, Any], field_name: str) -> SOLeaderConfig: + if isinstance(value, SOLeaderConfig): + return value + if isinstance(value, dict): + data = dict(value) + data.pop("type", None) + return SOLeaderConfig(**data) + raise TypeError( + f"xlerobot_default_composite.arms.{field_name} must be a dict or SOLeaderConfig; " + f"got {type(value).__name__}." + ) + + def _parse_arms_config(self, value: object) -> BiSOLeaderConfig | None: + if value is None or value == {}: + return None + if isinstance(value, BiSOLeaderConfig): + return value + if not isinstance(value, dict): + raise TypeError( + "xlerobot_default_composite.arms must be a dict with " + "`left_arm_config` and `right_arm_config`." + ) + + left_arm = value.get("left_arm_config") + right_arm = value.get("right_arm_config") + if left_arm is None or right_arm is None: + raise ValueError( + "xlerobot_default_composite.arms must define both " + "`left_arm_config` and `right_arm_config`." + ) + + return BiSOLeaderConfig( + left_arm_config=self._parse_so_leader_config(left_arm, "left_arm_config"), + right_arm_config=self._parse_so_leader_config(right_arm, "right_arm_config"), + ) + + def _load_from_config_file(self, config_file: str) -> None: + cli_overrides = parser.get_cli_overrides("teleop") or [] + cli_overrides = [arg for arg in cli_overrides if not arg.startswith("--config_file=")] + loaded = draccus.parse(config_class=TeleoperatorConfig, config_path=config_file, args=cli_overrides) + self.__dict__.update(loaded.__dict__) + self.config_file = config_file diff --git a/src/lerobot/teleoperators/xlerobot_teleoperator/default_composite/teleop.py b/src/lerobot/teleoperators/xlerobot_teleoperator/default_composite/teleop.py new file mode 100644 index 00000000000..d8db2c61ab0 --- /dev/null +++ b/src/lerobot/teleoperators/xlerobot_teleoperator/default_composite/teleop.py @@ -0,0 +1,128 @@ +# Copyright 2025 The HuggingFace Inc. team. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# XLeRobot integration based on +# +# https://github.com/Astera-org/brainbot +# https://github.com/Vector-Wangel/XLeRobot +# https://github.com/bingogome/lerobot + +from __future__ import annotations + +from contextlib import suppress +from functools import cached_property +from typing import Any + +from ...bi_so_leader.bi_so_leader import BiSOLeader +from ...teleoperator import Teleoperator +from ..sub_teleoperators.biwheel_gamepad.teleop_biwheel_gamepad import BiwheelGamepadTeleop +from ..sub_teleoperators.lekiwi_base_gamepad.teleop_lekiwi_base_gamepad import LeKiwiBaseTeleop +from ..sub_teleoperators.xlerobot_mount_gamepad.teleop import XLeRobotMountGamepadTeleop +from .config import XLeRobotDefaultCompositeConfig + + +class XLeRobotDefaultComposite(Teleoperator): + """Composite teleoperator for XLeRobot combining leader arms with gamepad inputs. + + This teleoperator combines three input methods: + - BiSOLeader: Leader arms for controlling follower arms + - LeKiwiBaseTeleop: Xbox gamepad left stick for base movement + - XLeRobotMountGamepadTeleop: Xbox gamepad right stick for mount pan/tilt + + All three inputs are merged into a single action dictionary that controls + the complete XLeRobot system. + """ + + config_class = XLeRobotDefaultCompositeConfig + name = "xlerobot_default_composite" + + def __init__(self, config: XLeRobotDefaultCompositeConfig): + self.config = config + super().__init__(config) + self.arm_teleop = BiSOLeader(config.arms_config) if config.arms_config else None + self.base_teleop = self._build_base_teleop() + self.mount_teleop = self._build_mount_teleop() + + def _build_base_teleop(self) -> Teleoperator | None: + base_config = getattr(self.config, "base_config", None) + if base_config is None: + return None + base_type = getattr(self.config, "base_type", None) or XLeRobotDefaultCompositeConfig.BASE_TYPE_LEKIWI + if base_type == XLeRobotDefaultCompositeConfig.BASE_TYPE_LEKIWI: + return LeKiwiBaseTeleop(base_config) + if base_type == XLeRobotDefaultCompositeConfig.BASE_TYPE_BIWHEEL: + return BiwheelGamepadTeleop(base_config) + raise ValueError(f"Unsupported base teleoperator type: {base_type}") + + def _build_mount_teleop(self) -> Teleoperator | None: + mount_config = getattr(self.config, "mount_config", None) + if mount_config is None: + return None + return XLeRobotMountGamepadTeleop(mount_config) + + def _iter_active_teleops(self) -> tuple[Teleoperator, ...]: + return tuple(tp for tp in (self.arm_teleop, self.base_teleop, self.mount_teleop) if tp is not None) + + @cached_property + def action_features(self) -> dict[str, type]: + features: dict[str, type] = {} + for teleop in self._iter_active_teleops(): + features.update(teleop.action_features) + return features + + @cached_property + def feedback_features(self) -> dict[str, type]: + features: dict[str, type] = {} + for teleop in self._iter_active_teleops(): + features.update(teleop.feedback_features) + return features + + @property + def is_connected(self) -> bool: + return all(teleop.is_connected for teleop in self._iter_active_teleops()) + + def connect(self, calibrate: bool = True) -> None: + for teleop in self._iter_active_teleops(): + teleop.connect(calibrate=calibrate) + + def disconnect(self) -> None: + for teleop in self._iter_active_teleops(): + teleop.disconnect() + + def calibrate(self) -> None: + for teleop in self._iter_active_teleops(): + teleop.calibrate() + + def configure(self) -> None: + for teleop in self._iter_active_teleops(): + teleop.configure() + + def on_observation(self, robot_obs: dict[str, Any]) -> None: + if self.mount_teleop and hasattr(self.mount_teleop, "on_observation"): + with suppress(Exception): + self.mount_teleop.on_observation(robot_obs) + + def get_action(self) -> dict[str, float]: + action: dict[str, float] = {} + for teleop in self._iter_active_teleops(): + action.update(teleop.get_action()) + return action + + def send_feedback(self, feedback: dict[str, float]) -> None: + for teleop in self._iter_active_teleops(): + teleop.send_feedback(feedback) + + @property + def is_calibrated(self) -> bool: + return all(getattr(teleop, "is_calibrated", True) for teleop in self._iter_active_teleops()) diff --git a/src/lerobot/teleoperators/xlerobot_teleoperator/keyboard_composite/__init__.py b/src/lerobot/teleoperators/xlerobot_teleoperator/keyboard_composite/__init__.py new file mode 100644 index 00000000000..b309395fb93 --- /dev/null +++ b/src/lerobot/teleoperators/xlerobot_teleoperator/keyboard_composite/__init__.py @@ -0,0 +1,27 @@ +# Copyright 2025 The HuggingFace Inc. team. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# XLeRobot integration based on +# +# https://github.com/Astera-org/brainbot +# https://github.com/Vector-Wangel/XLeRobot +# https://github.com/bingogome/lerobot + +from .config import XLeRobotKeyboardCompositeConfig +from .teleop import XLeRobotKeyboardComposite + +__all__ = [ + "XLeRobotKeyboardComposite", + "XLeRobotKeyboardCompositeConfig", +] diff --git a/src/lerobot/teleoperators/xlerobot_teleoperator/keyboard_composite/config.py b/src/lerobot/teleoperators/xlerobot_teleoperator/keyboard_composite/config.py new file mode 100644 index 00000000000..1d8cc2b7fd7 --- /dev/null +++ b/src/lerobot/teleoperators/xlerobot_teleoperator/keyboard_composite/config.py @@ -0,0 +1,114 @@ +# Copyright 2025 The HuggingFace Inc. team. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# XLeRobot integration based on +# +# https://github.com/Astera-org/brainbot +# https://github.com/Vector-Wangel/XLeRobot +# https://github.com/bingogome/lerobot + +from dataclasses import dataclass, field +from typing import Any + +import draccus + +from lerobot.configs import parser + +from ...config import TeleoperatorConfig +from ...keyboard.configuration_keyboard import KeyboardRoverTeleopConfig +from ..sub_teleoperators.panthera_keyboard_ee import PantheraKeyboardEETeleopConfig +from ..sub_teleoperators.xlerobot_mount_gamepad.config import XLeRobotMountGamepadTeleopConfig + +DEFAULT_BASE_KEYBOARD_CONFIG = { + "linear_speed": 0.2, + "angular_speed": 40.0, + "speed_increment": 0.05, + "turn_assist_ratio": 0.0, + "angular_speed_ratio": 200.0, + "min_linear_speed": 0.05, + "min_angular_speed": 10.0, +} + +PantheraArmKeyboardTeleopConfig = PantheraKeyboardEETeleopConfig + + +@TeleoperatorConfig.register_subclass("xlerobot_keyboard_composite") +@dataclass +class XLeRobotKeyboardCompositeConfig(TeleoperatorConfig): + """Config for XLeRobot teleop with keyboard-controlled base and optional Panthera arm.""" + + _comment: str | None = None + config_file: str | None = None + arm: dict[str, Any] = field(default_factory=dict) + arm_side: str = "left" + base: dict[str, Any] = field(default_factory=dict) + mount: dict[str, Any] = field(default_factory=dict) + + def __post_init__(self) -> None: + if self.config_file: + self._load_from_config_file(self.config_file) + + if self.arm_side not in ("left", "right"): + raise ValueError("xlerobot_keyboard_composite.arm_side must be either 'left' or 'right'.") + + self.arm_config = self._coerce_arm_config(self.arm) + self.base_config = self._coerce_base_config(self.base) + self.mount_config = self._coerce_mount_config(self.mount) + + def _load_from_config_file(self, config_file: str) -> None: + cli_overrides = parser.get_cli_overrides("teleop") or [] + cli_overrides = [arg for arg in cli_overrides if not arg.startswith("--config_file=")] + loaded = draccus.parse(config_class=TeleoperatorConfig, config_path=config_file, args=cli_overrides) + self.__dict__.update(loaded.__dict__) + self.config_file = config_file + + def _coerce_arm_config( + self, value: PantheraArmKeyboardTeleopConfig | dict[str, Any] | None + ) -> PantheraArmKeyboardTeleopConfig | None: + if isinstance(value, PantheraKeyboardEETeleopConfig): + return value + if not value: + return None + + data = dict(value) + arm_type = str(data.pop("type", "panthera_keyboard_ee")) + if arm_type == "panthera_keyboard_ee": + return PantheraKeyboardEETeleopConfig(**data) + raise ValueError( + "xlerobot_keyboard_composite.arm.type must be 'panthera_keyboard_ee'." + ) + + def _coerce_base_config( + self, value: KeyboardRoverTeleopConfig | dict[str, Any] | None + ) -> KeyboardRoverTeleopConfig | None: + if isinstance(value, KeyboardRoverTeleopConfig): + return value + if value is None: + return None + + data = dict(DEFAULT_BASE_KEYBOARD_CONFIG) + data.update(dict(value)) + data.pop("type", None) + return KeyboardRoverTeleopConfig(**data) + + def _coerce_mount_config( + self, value: XLeRobotMountGamepadTeleopConfig | dict[str, Any] | None + ) -> XLeRobotMountGamepadTeleopConfig | None: + if isinstance(value, XLeRobotMountGamepadTeleopConfig): + return value + if not value: + return None + data = dict(value) + data.pop("type", None) + return XLeRobotMountGamepadTeleopConfig(**data) diff --git a/src/lerobot/teleoperators/xlerobot_teleoperator/keyboard_composite/teleop.py b/src/lerobot/teleoperators/xlerobot_teleoperator/keyboard_composite/teleop.py new file mode 100644 index 00000000000..6639d191630 --- /dev/null +++ b/src/lerobot/teleoperators/xlerobot_teleoperator/keyboard_composite/teleop.py @@ -0,0 +1,160 @@ +# Copyright 2025 The HuggingFace Inc. team. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# XLeRobot integration based on +# +# https://github.com/Astera-org/brainbot +# https://github.com/Vector-Wangel/XLeRobot +# https://github.com/bingogome/lerobot + +from __future__ import annotations + +from contextlib import suppress +from functools import cached_property +from inspect import signature +from typing import Any + +from ...keyboard import KeyboardRoverTeleop +from ...teleoperator import Teleoperator +from ..sub_teleoperators.panthera_keyboard_ee import PantheraKeyboardEETeleop, PantheraKeyboardEETeleopConfig +from ..sub_teleoperators.xlerobot_mount_gamepad.teleop import XLeRobotMountGamepadTeleop +from .config import PantheraArmKeyboardTeleopConfig, XLeRobotKeyboardCompositeConfig + + +class XLeRobotKeyboardComposite(Teleoperator): + """Composite teleoperator for XLeRobot with keyboard base control and optional mount gamepad.""" + + config_class = XLeRobotKeyboardCompositeConfig + name = "xlerobot_keyboard_composite" + + def __init__(self, config: XLeRobotKeyboardCompositeConfig): + self.config = config + super().__init__(config) + self.arm_teleop = self._make_arm_teleop(config.arm_config) + self.base_teleop = KeyboardRoverTeleop(config.base_config) if config.base_config else None + self.mount_teleop = XLeRobotMountGamepadTeleop(config.mount_config) if config.mount_config else None + + @staticmethod + def _make_arm_teleop(config: PantheraArmKeyboardTeleopConfig | None) -> Teleoperator | None: + if config is None: + return None + if isinstance(config, PantheraKeyboardEETeleopConfig): + return PantheraKeyboardEETeleop(config) + raise TypeError(f"Unsupported Panthera arm teleop config type: {type(config).__name__}") + + def _iter_active_teleops(self) -> tuple[Teleoperator, ...]: + return tuple(tp for tp in (self.arm_teleop, self.base_teleop, self.mount_teleop) if tp is not None) + + @cached_property + def action_features(self) -> dict[str, type]: + features: dict[str, type] = {} + for teleop in self._iter_active_teleops(): + if teleop is self.arm_teleop: + prefix = self._arm_prefix() + features.update({f"{prefix}{key}": value for key, value in teleop.action_features.items()}) + elif teleop is self.base_teleop: + features.update({"x.vel": float, "theta.vel": float}) + else: + features.update(teleop.action_features) + return features + + @cached_property + def feedback_features(self) -> dict[str, type]: + features: dict[str, type] = {} + for teleop in self._iter_active_teleops(): + features.update(teleop.feedback_features) + return features + + @property + def is_connected(self) -> bool: + return all(teleop.is_connected for teleop in self._iter_active_teleops()) + + def connect(self, calibrate: bool = True) -> None: + for teleop in self._iter_active_teleops(): + self._connect_teleop(teleop, calibrate=calibrate) + for teleop in self._iter_active_teleops(): + if not teleop.is_connected: + raise RuntimeError( + f"{type(teleop).__name__} is unavailable: no active input listener. " + "On Linux, this usually means `pynput` could not access a DISPLAY. " + "When using SSH, run with X11 forwarding (e.g. `ssh -Y`) or use a non-keyboard teleop." + ) + + @staticmethod + def _connect_teleop(teleop: Teleoperator, calibrate: bool) -> None: + try: + if "calibrate" in signature(teleop.connect).parameters: + teleop.connect(calibrate=calibrate) + return + except (TypeError, ValueError): + pass + teleop.connect() + + def disconnect(self) -> None: + for teleop in self._iter_active_teleops(): + if teleop.is_connected: + teleop.disconnect() + + def calibrate(self) -> None: + for teleop in self._iter_active_teleops(): + teleop.calibrate() + + def configure(self) -> None: + for teleop in self._iter_active_teleops(): + teleop.configure() + + def on_observation(self, robot_obs: dict[str, Any]) -> None: + if self.mount_teleop and hasattr(self.mount_teleop, "on_observation"): + with suppress(Exception): + self.mount_teleop.on_observation(robot_obs) + + def get_action(self) -> dict[str, float]: + action: dict[str, float] = {} + for teleop in self._iter_active_teleops(): + if not teleop.is_connected: + raise RuntimeError( + f"{type(teleop).__name__} is not connected. " + "Ensure a DISPLAY is available for pynput (local desktop or SSH X11 forwarding)." + ) + teleop_action = teleop.get_action() + if teleop is self.arm_teleop: + action.update(self._map_arm_action(teleop_action)) + elif teleop is self.base_teleop: + action.update(self._map_base_action(teleop_action)) + else: + action.update(teleop_action) + return action + + def _map_arm_action(self, action: dict[str, Any]) -> dict[str, float]: + prefix = self._arm_prefix() + return {f"{prefix}{key}": float(value) for key, value in action.items()} + + def _arm_prefix(self) -> str: + if self.config.arm_side not in ("left", "right"): + raise ValueError(f"Unsupported arm_side '{self.config.arm_side}'. Use 'left' or 'right'.") + return f"{self.config.arm_side}_" + + @staticmethod + def _map_base_action(action: dict[str, Any]) -> dict[str, float]: + linear = action.get("x.vel", action.get("linear.vel", 0.0)) + angular = action.get("theta.vel", action.get("angular.vel", 0.0)) + return {"x.vel": float(linear), "theta.vel": float(angular)} + + def send_feedback(self, feedback: dict[str, float]) -> None: + for teleop in self._iter_active_teleops(): + teleop.send_feedback(feedback) + + @property + def is_calibrated(self) -> bool: + return all(getattr(teleop, "is_calibrated", True) for teleop in self._iter_active_teleops()) diff --git a/src/lerobot/teleoperators/xlerobot_teleoperator/sub_teleoperators/__init__.py b/src/lerobot/teleoperators/xlerobot_teleoperator/sub_teleoperators/__init__.py new file mode 100644 index 00000000000..49f8cf859cd --- /dev/null +++ b/src/lerobot/teleoperators/xlerobot_teleoperator/sub_teleoperators/__init__.py @@ -0,0 +1,20 @@ +"""Reusable teleoperation blocks that compose the XLeRobot teleoperator.""" + +from .biwheel_gamepad import BiwheelGamepadTeleop, BiwheelGamepadTeleopConfig +from .biwheel_keyboard import BiwheelKeyboardTeleop, BiwheelKeyboardTeleopConfig +from .lekiwi_base_gamepad import LeKiwiBaseTeleop, LeKiwiBaseTeleopConfig +from .panthera_keyboard_ee import PantheraKeyboardEETeleop, PantheraKeyboardEETeleopConfig +from .xlerobot_mount_gamepad import XLeRobotMountGamepadTeleop, XLeRobotMountGamepadTeleopConfig + +__all__ = [ + "BiwheelGamepadTeleop", + "BiwheelGamepadTeleopConfig", + "BiwheelKeyboardTeleop", + "BiwheelKeyboardTeleopConfig", + "LeKiwiBaseTeleop", + "LeKiwiBaseTeleopConfig", + "PantheraKeyboardEETeleop", + "PantheraKeyboardEETeleopConfig", + "XLeRobotMountGamepadTeleop", + "XLeRobotMountGamepadTeleopConfig", +] diff --git a/src/lerobot/teleoperators/xlerobot_teleoperator/sub_teleoperators/biwheel_gamepad/__init__.py b/src/lerobot/teleoperators/xlerobot_teleoperator/sub_teleoperators/biwheel_gamepad/__init__.py new file mode 100644 index 00000000000..66a0ed158ce --- /dev/null +++ b/src/lerobot/teleoperators/xlerobot_teleoperator/sub_teleoperators/biwheel_gamepad/__init__.py @@ -0,0 +1,18 @@ +#!/usr/bin/env python + +# Copyright 2025 The HuggingFace Inc. team. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from .config_biwheel_gamepad import BiwheelGamepadTeleopConfig +from .teleop_biwheel_gamepad import BiwheelGamepadTeleop diff --git a/src/lerobot/teleoperators/xlerobot_teleoperator/sub_teleoperators/biwheel_gamepad/config_biwheel_gamepad.py b/src/lerobot/teleoperators/xlerobot_teleoperator/sub_teleoperators/biwheel_gamepad/config_biwheel_gamepad.py new file mode 100644 index 00000000000..e158cb89bbe --- /dev/null +++ b/src/lerobot/teleoperators/xlerobot_teleoperator/sub_teleoperators/biwheel_gamepad/config_biwheel_gamepad.py @@ -0,0 +1,35 @@ +#!/usr/bin/env python + +# Copyright 2025 The HuggingFace Inc. team. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from dataclasses import dataclass + +from ....config import TeleoperatorConfig + + +@TeleoperatorConfig.register_subclass("biwheel_gamepad") +@dataclass +class BiwheelGamepadTeleopConfig(TeleoperatorConfig): + """Configuration for the differential (biwheel) base gamepad teleoperator.""" + + joystick_index: int = 0 + max_speed_mps: float = 0.2 + yaw_speed_deg: float = 40.0 + deadzone: float = 0.15 + polling_fps: int = 60 + axis_forward: int = 1 + axis_yaw: int = 0 + invert_forward: bool = True + invert_yaw: bool = False diff --git a/src/lerobot/teleoperators/xlerobot_teleoperator/sub_teleoperators/biwheel_gamepad/teleop_biwheel_gamepad.py b/src/lerobot/teleoperators/xlerobot_teleoperator/sub_teleoperators/biwheel_gamepad/teleop_biwheel_gamepad.py new file mode 100644 index 00000000000..53e555fb4e4 --- /dev/null +++ b/src/lerobot/teleoperators/xlerobot_teleoperator/sub_teleoperators/biwheel_gamepad/teleop_biwheel_gamepad.py @@ -0,0 +1,280 @@ +#!/usr/bin/env python + +# Example teleoperation command for the biwheel base using this gamepad teleop: +# +# lerobot-teleoperate \ +# --robot.type=biwheel \ +# --robot.port=/dev/ttyACM4 \ +# --robot.id=biwheel_base \ +# --teleop.type=biwheel_gamepad \ +# --teleop.joystick_index=0 \ +# --teleop.id=gamepad \ +# --display_data=true + +# Copyright 2025 The HuggingFace Inc. team. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from __future__ import annotations + +import time +from contextlib import suppress +from dataclasses import dataclass +from pathlib import Path +from typing import Any + +import draccus + +from lerobot.utils.errors import DeviceNotConnectedError + +from ....teleoperator import Teleoperator +from .config_biwheel_gamepad import BiwheelGamepadTeleopConfig + + +@dataclass +class GamepadCalibration: + axis_forward: int + axis_yaw: int + invert_forward: bool + invert_yaw: bool + + +class BiwheelGamepadTeleop(Teleoperator): + """Teleoperator that maps an Xbox-style gamepad to differential base commands.""" + + config_class = BiwheelGamepadTeleopConfig + name = "biwheel_gamepad" + + def __init__(self, config: BiwheelGamepadTeleopConfig): + # _load_calibration which in turn calls _apply_calibration_to_config that needs self.config + self.config = config + super().__init__(config) + self._pygame = None + self._joystick = None + self._clock = None + self._last_action = {"x.vel": 0.0, "theta.vel": 0.0} + calibration = self.calibration if isinstance(self.calibration, GamepadCalibration) else None + self.calibration: GamepadCalibration | None = calibration + self._calibrated = calibration is not None + if calibration is not None: + self._apply_calibration_to_config() + + @property + def action_features(self) -> dict[str, type]: + return {"x.vel": float, "theta.vel": float} + + @property + def feedback_features(self) -> dict[str, type]: + return {} + + def connect(self, calibrate: bool = True) -> None: + try: + import pygame + except ImportError as exc: + raise RuntimeError( + "pygame is required for BiwheelGamepadTeleop but is not installed. " + "Install it with `pip install pygame`." + ) from exc + + pygame.init() + pygame.joystick.init() + + if pygame.joystick.get_count() <= self.config.joystick_index: + raise RuntimeError("No joystick detected at the configured index.") + + self._pygame = pygame + self._joystick = pygame.joystick.Joystick(self.config.joystick_index) + self._joystick.init() + self._clock = pygame.time.Clock() + self._last_action = {"x.vel": 0.0, "theta.vel": 0.0} + + if calibrate and not self.is_calibrated: + self.calibrate() + elif self.calibration is not None: + self._apply_calibration_to_config() + self._calibrated = True + + @property + def is_connected(self) -> bool: + return self._joystick is not None + + def calibrate(self) -> None: + if not self.is_connected or self._pygame is None: + raise DeviceNotConnectedError("Biwheel gamepad teleoperator is not connected.") + + axis_threshold = 0.4 + release_threshold = 0.2 + poll_interval = 0.05 + + if self.calibration is not None: + user_input = input( + "Press ENTER to use existing calibration, or type 'c' then ENTER to recalibrate: " + ) + if user_input.strip().lower() != "c": + print("Using saved calibration.") + self._apply_calibration_to_config() + self._calibrated = True + return + + print("Calibration started. Follow the prompts.") + forward_axis, invert_forward = self._detect_axis( + prompt="Push the stick forward (away from you) to identify the forward axis.", + exclude_axes=set(), + axis_threshold=axis_threshold, + release_threshold=release_threshold, + poll_interval=poll_interval, + ) + yaw_axis, invert_yaw = self._detect_axis( + prompt="Push the stick to the right to identify the yaw axis.", + exclude_axes={forward_axis}, + axis_threshold=axis_threshold, + release_threshold=release_threshold, + poll_interval=poll_interval, + ) + + self.config.axis_forward = forward_axis + self.config.axis_yaw = yaw_axis + self.config.invert_forward = invert_forward + self.config.invert_yaw = invert_yaw + + self.calibration = GamepadCalibration( + axis_forward=forward_axis, + axis_yaw=yaw_axis, + invert_forward=invert_forward, + invert_yaw=invert_yaw, + ) + self._save_calibration() + print(f"Calibration saved to {self.calibration_fpath}") + self._calibrated = True + + @property + def is_calibrated(self) -> bool: + return self._calibrated + + def configure(self) -> None: + return None + + def get_action(self) -> dict[str, Any]: + if not self.is_connected or self._pygame is None: + raise DeviceNotConnectedError("Biwheel gamepad teleoperator is not connected.") + + pygame = self._pygame + joy_removed = getattr(pygame, "JOYDEVICEREMOVED", None) + for event in pygame.event.get(): + if joy_removed is not None and event.type == joy_removed: + self.disconnect() + raise DeviceNotConnectedError("Gamepad disconnected.") + + axis_forward = self._joystick.get_axis(self.config.axis_forward) + axis_yaw = self._joystick.get_axis(self.config.axis_yaw) + + if self.config.invert_forward: + axis_forward = -axis_forward + if self.config.invert_yaw: + axis_yaw = -axis_yaw + + forward_cmd = self._apply_deadzone(axis_forward) * self.config.max_speed_mps + yaw_cmd = self._apply_deadzone(axis_yaw) * self.config.yaw_speed_deg + + action = {"x.vel": forward_cmd, "theta.vel": yaw_cmd} + self._last_action = action + + if self._clock and self.config.polling_fps > 0: + self._clock.tick(self.config.polling_fps) + + return action + + def send_feedback(self, feedback: dict[str, Any]) -> None: + del feedback + + def disconnect(self) -> None: + if self._joystick is not None: + if hasattr(self._joystick, "quit"): + with suppress(Exception): + self._joystick.quit() + self._joystick = None + + if self._pygame is not None: + with suppress(Exception): + if hasattr(self._pygame, "joystick"): + self._pygame.joystick.quit() + self._pygame = None + + self._clock = None + + def _apply_deadzone(self, value: float) -> float: + if abs(value) < self.config.deadzone: + return 0.0 + return max(-1.0, min(1.0, value)) + + def _detect_axis( + self, + prompt: str, + exclude_axes: set[int], + axis_threshold: float, + release_threshold: float, + poll_interval: float, + ) -> tuple[int, bool]: + assert self._pygame is not None and self._joystick is not None + pygame = self._pygame + joystick = self._joystick + + print(prompt) + pygame.event.pump() + baseline = [joystick.get_axis(i) for i in range(joystick.get_numaxes())] + + while True: + pygame.event.pump() + num_axes = joystick.get_numaxes() + best_axis = None + best_delta = 0.0 + for idx in range(num_axes): + if idx in exclude_axes: + continue + delta = joystick.get_axis(idx) - baseline[idx] + if abs(delta) > abs(best_delta): + best_delta = delta + best_axis = idx + if best_axis is not None and abs(best_delta) >= axis_threshold: + invert = best_delta < 0 + direction = "negative" if invert else "positive" + print(f"Detected axis {best_axis} ({direction})") + print("Release the stick to continue.") + while abs(joystick.get_axis(best_axis) - baseline[best_axis]) > release_threshold: + pygame.event.pump() + time.sleep(poll_interval) + return best_axis, invert + time.sleep(poll_interval) + + def _apply_calibration_to_config(self) -> None: + if self.calibration is None: + return + self.config.axis_forward = self.calibration.axis_forward + self.config.axis_yaw = self.calibration.axis_yaw + self.config.invert_forward = self.calibration.invert_forward + self.config.invert_yaw = self.calibration.invert_yaw + + def _load_calibration(self, fpath: Path | None = None) -> None: + fpath = self.calibration_fpath if fpath is None else fpath + with open(fpath) as f, draccus.config_type("json"): + calibration = draccus.load(GamepadCalibration, f) + self.calibration = calibration + self._apply_calibration_to_config() + self._calibrated = True + + def _save_calibration(self, fpath: Path | None = None) -> None: + if self.calibration is None: + return + fpath = self.calibration_fpath if fpath is None else fpath + with open(fpath, "w") as f, draccus.config_type("json"): + draccus.dump(self.calibration, f, indent=4) diff --git a/src/lerobot/teleoperators/xlerobot_teleoperator/sub_teleoperators/biwheel_keyboard/__init__.py b/src/lerobot/teleoperators/xlerobot_teleoperator/sub_teleoperators/biwheel_keyboard/__init__.py new file mode 100644 index 00000000000..f81c41c48b7 --- /dev/null +++ b/src/lerobot/teleoperators/xlerobot_teleoperator/sub_teleoperators/biwheel_keyboard/__init__.py @@ -0,0 +1,23 @@ +#!/usr/bin/env python + +# Copyright 2025 The HuggingFace Inc. team. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from .configuration_biwheel_keyboard import BiwheelKeyboardTeleopConfig +from .teleop_biwheel_keyboard import BiwheelKeyboardTeleop + +__all__ = [ + "BiwheelKeyboardTeleopConfig", + "BiwheelKeyboardTeleop", +] diff --git a/src/lerobot/teleoperators/xlerobot_teleoperator/sub_teleoperators/biwheel_keyboard/configuration_biwheel_keyboard.py b/src/lerobot/teleoperators/xlerobot_teleoperator/sub_teleoperators/biwheel_keyboard/configuration_biwheel_keyboard.py new file mode 100644 index 00000000000..4712493b6b0 --- /dev/null +++ b/src/lerobot/teleoperators/xlerobot_teleoperator/sub_teleoperators/biwheel_keyboard/configuration_biwheel_keyboard.py @@ -0,0 +1,59 @@ +#!/usr/bin/env python + +# Copyright 2025 The HuggingFace Inc. team. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from dataclasses import dataclass, field + +from ....config import TeleoperatorConfig + + +@TeleoperatorConfig.register_subclass("biwheel_keyboard") +@dataclass +class BiwheelKeyboardTeleopConfig(TeleoperatorConfig): + """ + Configuration for bidirectional wheel (differential drive) keyboard teleop. + + This configuration supports smooth acceleration/deceleration control for + differential drive mobile bases with configurable speed parameters. + """ + + # Smooth control parameters + acceleration_rate: float = 3.0 # acceleration slope (speed/second) - reduced from 10.0 + deceleration_rate: float = 5.0 # deceleration slope (speed/second) - reduced from 10.0 + max_speed_multiplier: float = 3.0 # maximum speed multiplier - reduced from 6.0 + min_velocity_threshold: float = 0.01 # minimum velocity to send during deceleration + + # Key mappings for base control + key_forward: str = "w" + key_backward: str = "s" + key_rotate_left: str = "a" + key_rotate_right: str = "d" + key_speed_up: str = "=" + key_speed_down: str = "-" + key_quit: str = "q" + + # Speed level settings (list of dicts with 'linear' and 'angular' keys) + speed_levels: list = field( + default_factory=lambda: [ + {"linear": 0.04, "angular": 20}, # Level 1: Slow (half of max) + {"linear": 0.08, "angular": 40}, # Level 2: Fast (motor limit) + ] + ) + + # Initial speed level index (0-based) + initial_speed_index: int = 0 # Default to very slow speed (Level 1) + + # Enable debug output + debug: bool = True diff --git a/src/lerobot/teleoperators/xlerobot_teleoperator/sub_teleoperators/biwheel_keyboard/teleop_biwheel_keyboard.py b/src/lerobot/teleoperators/xlerobot_teleoperator/sub_teleoperators/biwheel_keyboard/teleop_biwheel_keyboard.py new file mode 100644 index 00000000000..718921e7c7b --- /dev/null +++ b/src/lerobot/teleoperators/xlerobot_teleoperator/sub_teleoperators/biwheel_keyboard/teleop_biwheel_keyboard.py @@ -0,0 +1,403 @@ +#!/usr/bin/env python + +# Copyright 2025 The HuggingFace Inc. team. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import logging +import os +import sys +import time +from queue import Queue +from typing import Any + +from lerobot.utils.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError + +from ....teleoperator import Teleoperator +from ....utils import TeleopEvents +from .configuration_biwheel_keyboard import BiwheelKeyboardTeleopConfig + +PYNPUT_AVAILABLE = True +try: + if ("DISPLAY" not in os.environ) and ("linux" in sys.platform): + logging.info("No DISPLAY set. Skipping pynput import.") + raise ImportError("pynput blocked intentionally due to no display.") + + from pynput import keyboard +except ImportError: + keyboard = None + PYNPUT_AVAILABLE = False +except Exception as e: + keyboard = None + PYNPUT_AVAILABLE = False + logging.info(f"Could not import pynput: {e}") + + +class SmoothBaseController: + """ + Simplified smooth base controller with acceleration/deceleration. + + This controller implements linear acceleration when keys are pressed and + linear deceleration when keys are released, providing smooth motion control + for differential drive mobile bases. + """ + + def __init__(self, config: BiwheelKeyboardTeleopConfig): + self.config = config + self.current_speed = 0.0 + self.last_time = time.time() + self.last_direction = {"x.vel": 0.0, "theta.vel": 0.0} + self.is_moving = False + self.speed_index = config.initial_speed_index + + # Validate speed_index + if not (0 <= self.speed_index < len(config.speed_levels)): + logging.warning( + f"Initial speed index {self.speed_index} out of range. " + f"Setting to {len(config.speed_levels) // 2}" + ) + self.speed_index = len(config.speed_levels) // 2 + + def change_speed_level(self, delta: int) -> None: + """Change the speed level by delta.""" + old_index = self.speed_index + self.speed_index = max(0, min(len(self.config.speed_levels) - 1, self.speed_index + delta)) + + if old_index != self.speed_index and self.config.debug: + level = self.config.speed_levels[self.speed_index] + logging.info( + f"[BASE] Speed level changed: {old_index + 1} -> {self.speed_index + 1} " + f"(Linear: {level['linear']:.2f}m/s, Angular: {level['angular']:.0f}°/s)" + ) + + def update(self, pressed_keys: set) -> dict[str, float]: + """ + Update smooth control and return base action. + + Args: + pressed_keys: Set of currently pressed key characters + + Returns: + Dictionary with 'x.vel' (linear velocity) and 'theta.vel' (angular velocity) + """ + current_time = time.time() + dt = current_time - self.last_time + self.last_time = current_time + + # Check if any base movement keys are pressed + base_keys = [ + self.config.key_forward, + self.config.key_backward, + self.config.key_rotate_left, + self.config.key_rotate_right, + ] + any_key_pressed = any(key in pressed_keys for key in base_keys) + + # Handle speed level changes + if self.config.key_speed_up in pressed_keys: + self.change_speed_level(1) + if self.config.key_speed_down in pressed_keys: + self.change_speed_level(-1) + + # Calculate base action + base_action = {"x.vel": 0.0, "theta.vel": 0.0} + + if any_key_pressed: + # Keys pressed - calculate direction and accelerate + if not self.is_moving: + self.is_moving = True + if self.config.debug: + logging.info("[BASE] Starting acceleration") + + # Get current speed level settings + speed_setting = self.config.speed_levels[self.speed_index] + linear_speed = speed_setting["linear"] + angular_speed = speed_setting["angular"] + + # Calculate direction based on pressed keys + if self.config.key_forward in pressed_keys: + base_action["x.vel"] += linear_speed + if self.config.key_backward in pressed_keys: + base_action["x.vel"] -= linear_speed + if self.config.key_rotate_left in pressed_keys: + base_action["theta.vel"] += angular_speed + if self.config.key_rotate_right in pressed_keys: + base_action["theta.vel"] -= angular_speed + + # Store current direction for deceleration + self.last_direction = base_action.copy() + + # Accelerate + self.current_speed += self.config.acceleration_rate * dt + self.current_speed = min(self.current_speed, self.config.max_speed_multiplier) + + else: + # No keys pressed - decelerate + if self.is_moving: + self.is_moving = False + if self.config.debug: + logging.info("[BASE] Starting deceleration") + + # Use last direction for deceleration + if self.current_speed > 0.01 and self.last_direction: + base_action = self.last_direction.copy() + + # Decelerate + self.current_speed -= self.config.deceleration_rate * dt + self.current_speed = max(self.current_speed, 0.0) + + # Apply speed multiplier + if base_action: + for key in base_action: + if "vel" in key: + original_value = base_action[key] + base_action[key] *= self.current_speed + + # Ensure minimum velocity during deceleration + if ( + self.current_speed > 0.01 + and abs(base_action[key]) < self.config.min_velocity_threshold + ): + base_action[key] = ( + self.config.min_velocity_threshold + if original_value > 0 + else -self.config.min_velocity_threshold + ) + + # Debug output + if self.config.debug: + if any_key_pressed: + logging.debug(f"[BASE] ACCEL: Speed={self.current_speed:.2f}, Action={base_action}") + elif self.current_speed > 0.01: + logging.debug(f"[BASE] DECEL: Speed={self.current_speed:.2f}, Action={base_action}") + elif self.is_moving: + logging.debug(f"[BASE] STOPPED: Speed={self.current_speed:.2f}") + + return base_action + + +class BiwheelKeyboardTeleop(Teleoperator): + """ + Teleop class to use keyboard inputs for bidirectional wheel (differential drive) control. + + This teleoperator provides smooth acceleration/deceleration control for mobile bases + with differential drive kinematics. It supports configurable key mappings, multiple + speed levels, and smooth transitions between motion states. + """ + + config_class = BiwheelKeyboardTeleopConfig + name = "biwheel_keyboard" + + def __init__(self, config: BiwheelKeyboardTeleopConfig): + self.config = config + super().__init__(config) + + self.event_queue = Queue() + self.current_pressed = {} + self.listener = None + self.logs = {} + + # Initialize smooth controller + self.smooth_controller = SmoothBaseController(config) + + # Queue for misc keys (for teleop events) + self.misc_keys_queue = Queue() + + @property + def action_features(self) -> dict: + """Define the action space for differential drive control.""" + return { + "dtype": "float32", + "shape": (2,), + "names": {"x.vel": 0, "theta.vel": 1}, + } + + @property + def feedback_features(self) -> dict: + return {} + + @property + def is_connected(self) -> bool: + return PYNPUT_AVAILABLE and isinstance(self.listener, keyboard.Listener) and self.listener.is_alive() + + @property + def is_calibrated(self) -> bool: + return True + + def connect(self) -> None: + if self.is_connected: + raise DeviceAlreadyConnectedError( + "Biwheel Keyboard is already connected. Do not run `connect()` twice." + ) + + if PYNPUT_AVAILABLE: + logging.info("pynput is available - enabling local keyboard listener for biwheel control.") + self.listener = keyboard.Listener( + on_press=self._on_press, + on_release=self._on_release, + ) + self.listener.start() + self._print_keymap() + else: + logging.warning("pynput not available - keyboard teleop will not work.") + self.listener = None + + def calibrate(self) -> None: + pass + + def _on_press(self, key): + if hasattr(key, "char"): + self.event_queue.put((key.char, True)) + + def _on_release(self, key): + if hasattr(key, "char"): + self.event_queue.put((key.char, False)) + if key == keyboard.Key.esc: + logging.info("ESC pressed, disconnecting.") + self.disconnect() + + def _drain_pressed_keys(self): + while not self.event_queue.empty(): + key_char, is_pressed = self.event_queue.get_nowait() + self.current_pressed[key_char] = is_pressed + + def configure(self): + pass + + def get_action(self) -> dict[str, Any]: + before_read_t = time.perf_counter() + + if not self.is_connected: + raise DeviceNotConnectedError( + "BiwheelKeyboardTeleop is not connected. You need to run `connect()` before `get_action()`." + ) + + # Update current pressed keys + self._drain_pressed_keys() + + # Get set of currently pressed keys + pressed_keys = {key for key, val in self.current_pressed.items() if val} + + # Check for misc keys and add to queue + control_keys = { + self.config.key_forward, + self.config.key_backward, + self.config.key_rotate_left, + self.config.key_rotate_right, + self.config.key_speed_up, + self.config.key_speed_down, + self.config.key_quit, + } + + for key in pressed_keys: + if key not in control_keys: + self.misc_keys_queue.put(key) + + # Get smooth base action with acceleration/deceleration + action = self.smooth_controller.update(pressed_keys) + + self.logs["read_pos_dt_s"] = time.perf_counter() - before_read_t + + return action + + def send_feedback(self, feedback: dict[str, Any]) -> None: + pass + + def disconnect(self) -> None: + if not self.is_connected: + raise DeviceNotConnectedError( + "BiwheelKeyboardTeleop is not connected. You need to run `connect()` before `disconnect()`." + ) + if self.listener is not None: + self.listener.stop() + + def get_teleop_events(self) -> dict[str, Any]: + """ + Get extra control events from the keyboard such as intervention status, + episode termination, success indicators, etc. + + Keyboard mappings (beyond movement keys): + - Any movement keys pressed = intervention active + - 's' key = success (terminate episode successfully) + - 'r' key = rerecord episode (terminate and rerecord) + - 'q' key or config.key_quit = quit episode + + Returns: + Dictionary containing: + - is_intervention: bool - Whether human is currently intervening + - terminate_episode: bool - Whether to terminate the current episode + - success: bool - Whether the episode was successful + - rerecord_episode: bool - Whether to rerecord the episode + """ + if not self.is_connected: + return { + TeleopEvents.IS_INTERVENTION: False, + TeleopEvents.TERMINATE_EPISODE: False, + TeleopEvents.SUCCESS: False, + TeleopEvents.RERECORD_EPISODE: False, + } + + # Check if any movement keys are currently pressed (indicates intervention) + movement_keys = [ + self.config.key_forward, + self.config.key_backward, + self.config.key_rotate_left, + self.config.key_rotate_right, + ] + is_intervention = any(self.current_pressed.get(key, False) for key in movement_keys) + + # Check for episode control commands from misc_keys_queue + terminate_episode = False + success = False + rerecord_episode = False + + # Process any pending misc keys + while not self.misc_keys_queue.empty(): + key = self.misc_keys_queue.get_nowait() + if key == "s": + success = True + terminate_episode = True + elif key == "r": + terminate_episode = True + rerecord_episode = True + elif key == self.config.key_quit: + terminate_episode = True + success = False + + return { + TeleopEvents.IS_INTERVENTION: is_intervention, + TeleopEvents.TERMINATE_EPISODE: terminate_episode, + TeleopEvents.SUCCESS: success, + TeleopEvents.RERECORD_EPISODE: rerecord_episode, + } + + def _print_keymap(self): + """Print the keymap information to console.""" + + print("\n📱 Base Control (Differential Drive):") + print(f" {self.config.key_forward}: Forward") + print(f" {self.config.key_backward}: Backward") + print(f" {self.config.key_rotate_left}: Rotate Left") + print(f" {self.config.key_rotate_right}: Rotate Right") + print(f" {self.config.key_speed_up}: Speed Up") + print(f" {self.config.key_speed_down}: Speed Down") + print(f" {self.config.key_quit}: Quit") + + print("\n Speed Configuration:") + print(f" Current Level: {self.smooth_controller.speed_index + 1}/{len(self.config.speed_levels)}") + print(" Speed Levels:") + for i, level in enumerate(self.config.speed_levels): + marker = "→" if i == self.smooth_controller.speed_index else " " + print( + f" {marker} Level {i + 1}: Linear {level['linear']:.2f}m/s, Angular {level['angular']:.0f}°/s" + ) diff --git a/src/lerobot/teleoperators/xlerobot_teleoperator/sub_teleoperators/lekiwi_base_gamepad/__init__.py b/src/lerobot/teleoperators/xlerobot_teleoperator/sub_teleoperators/lekiwi_base_gamepad/__init__.py new file mode 100644 index 00000000000..accedca8612 --- /dev/null +++ b/src/lerobot/teleoperators/xlerobot_teleoperator/sub_teleoperators/lekiwi_base_gamepad/__init__.py @@ -0,0 +1,18 @@ +#!/usr/bin/env python + +# Copyright 2025 The HuggingFace Inc. team. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from .config_lekiwi_base_gamepad import LeKiwiBaseTeleopConfig +from .teleop_lekiwi_base_gamepad import LeKiwiBaseTeleop diff --git a/src/lerobot/teleoperators/xlerobot_teleoperator/sub_teleoperators/lekiwi_base_gamepad/config_lekiwi_base_gamepad.py b/src/lerobot/teleoperators/xlerobot_teleoperator/sub_teleoperators/lekiwi_base_gamepad/config_lekiwi_base_gamepad.py new file mode 100644 index 00000000000..49c06e33cb9 --- /dev/null +++ b/src/lerobot/teleoperators/xlerobot_teleoperator/sub_teleoperators/lekiwi_base_gamepad/config_lekiwi_base_gamepad.py @@ -0,0 +1,37 @@ +# Copyright 2025 The HuggingFace Inc. team. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from dataclasses import dataclass + +from ....config import TeleoperatorConfig + + +@TeleoperatorConfig.register_subclass("lekiwi_base_gamepad") +@dataclass +class LeKiwiBaseTeleopConfig(TeleoperatorConfig): + """Configuration for the LeKiwi base gamepad teleoperator.""" + + joystick_index: int = 0 + max_speed_mps: float = 1.0 + deadzone: float = 0.15 + yaw_speed_deg: float = 30.0 + polling_fps: int = 60 + normalize_diagonal: bool = True + axis_x: int = 0 + axis_y: int = 1 + invert_x: bool = False + invert_y: bool = True + hat_index: int = 0 + dpad_left_button: int = 14 + dpad_right_button: int = 13 diff --git a/src/lerobot/teleoperators/xlerobot_teleoperator/sub_teleoperators/lekiwi_base_gamepad/teleop_lekiwi_base_gamepad.py b/src/lerobot/teleoperators/xlerobot_teleoperator/sub_teleoperators/lekiwi_base_gamepad/teleop_lekiwi_base_gamepad.py new file mode 100644 index 00000000000..e12b4438d5a --- /dev/null +++ b/src/lerobot/teleoperators/xlerobot_teleoperator/sub_teleoperators/lekiwi_base_gamepad/teleop_lekiwi_base_gamepad.py @@ -0,0 +1,356 @@ +#!/usr/bin/env python + +# Example teleoperation command for the LeKiwi mobile base using this gamepad teleop: +# +# lerobot-teleoperate \ +# --robot.type=lekiwi_base \ +# --robot.port=/dev/ttyACM2 \ +# --robot.id=lekiwi_base \ +# --teleop.type=lekiwi_base_gamepad \ +# --teleop.joystick_index=0 \ +# --teleop.id=gamepad \ +# --display_data=true + +# Copyright 2025 The HuggingFace Inc. team. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import math +import time +from contextlib import suppress +from dataclasses import dataclass +from pathlib import Path +from typing import Any + +import draccus + +from lerobot.utils.errors import DeviceNotConnectedError + +from ....teleoperator import Teleoperator +from .config_lekiwi_base_gamepad import LeKiwiBaseTeleopConfig + + +@dataclass +class GamepadCalibration: + axis_x: int + axis_y: int + invert_x: bool + invert_y: bool + hat_index: int + dpad_left_button: int + dpad_right_button: int + + +class LeKiwiBaseTeleop(Teleoperator): + """Teleoperator that maps an Xbox-style gamepad to LeKiwi base velocity commands.""" + + config_class = LeKiwiBaseTeleopConfig + name = "lekiwi_base_gamepad" + + def __init__(self, config: LeKiwiBaseTeleopConfig): + self.config = config + super().__init__(config) + self._pygame = None + self._joystick = None + self._clock = None + self._last_action = {"x.vel": 0.0, "y.vel": 0.0, "theta.vel": 0.0} + calibration = self.calibration if isinstance(self.calibration, GamepadCalibration) else None + self.calibration: GamepadCalibration | None = calibration + self._calibrated = calibration is not None + if calibration is not None: + self._apply_calibration_to_config() + + @property + def action_features(self) -> dict[str, type]: + return {"x.vel": float, "y.vel": float, "theta.vel": float} + + @property + def feedback_features(self) -> dict: + return {} + + def connect(self, calibrate: bool = True) -> None: + try: + import pygame + except ImportError as exc: + raise RuntimeError( + "pygame is required for LeKiwiBaseTeleop but is not installed. " + "Install it with `pip install pygame`." + ) from exc + + pygame.init() + pygame.joystick.init() + + if pygame.joystick.get_count() <= self.config.joystick_index: + raise RuntimeError("No joystick detected at the configured index.") + + self._pygame = pygame + self._joystick = pygame.joystick.Joystick(self.config.joystick_index) + self._joystick.init() + self._clock = pygame.time.Clock() + self._last_action = {"x.vel": 0.0, "y.vel": 0.0, "theta.vel": 0.0} + + if calibrate and not self.is_calibrated: + self.calibrate() + + @property + def is_connected(self) -> bool: + return self._joystick is not None + + def calibrate(self) -> None: + if not self.is_connected or self._pygame is None: + raise DeviceNotConnectedError("LeKiwi base teleoperator is not connected.") + + pygame = self._pygame + joystick = self._joystick + + axis_threshold = 0.4 + release_threshold = 0.2 + poll_interval = 0.05 + + if self.calibration is not None: + user_input = input( + "Press ENTER to use existing calibration file associated with this teleoperator, " + "or type 'c' and press ENTER to run calibration: " + ) + if user_input.strip().lower() != "c": + print("Using saved calibration.") + self._apply_calibration_to_config() + self._calibrated = True + return + + print("\nStarting gamepad calibration.") + print("Follow the prompts. Move/press the indicated control and hold until it is detected.") + + def read_axes() -> list[float]: + pygame.event.pump() + return [joystick.get_axis(i) for i in range(joystick.get_numaxes())] + + def wait_for_axis(prompt: str) -> tuple[int, float]: + print(f"- {prompt}") + baseline = read_axes() + while True: + time.sleep(poll_interval) + current = read_axes() + if not current: + continue + diffs = [current[i] - baseline[i] for i in range(len(current))] + axis_index = max(range(len(diffs)), key=lambda idx: abs(diffs[idx])) + change = diffs[axis_index] + if abs(change) >= axis_threshold: + detected_value = current[axis_index] + print(f" detected axis {axis_index} with value {detected_value:+.2f}") + while True: + time.sleep(poll_interval) + pygame.event.pump() + value = joystick.get_axis(axis_index) + if abs(value - baseline[axis_index]) <= release_threshold: + break + return axis_index, detected_value + + def read_hats() -> list[tuple[int, int]]: + pygame.event.pump() + return [joystick.get_hat(i) for i in range(joystick.get_numhats())] + + def read_buttons() -> list[int]: + pygame.event.pump() + return [joystick.get_button(i) for i in range(joystick.get_numbuttons())] + + def wait_for_dpad(prompt: str, expected_x: int) -> tuple[str, int]: + print(f"- {prompt}") + baseline_buttons = read_buttons() + while True: + time.sleep(poll_interval) + hats = read_hats() + for idx, value in enumerate(hats): + if value[0] == expected_x: + print(f" detected hat {idx}") + while True: + time.sleep(poll_interval) + pygame.event.pump() + if joystick.get_hat(idx)[0] == 0: + break + return "hat", idx + buttons = read_buttons() + for idx, pressed in enumerate(buttons): + if pressed and (idx >= len(baseline_buttons) or not baseline_buttons[idx]): + print(f" detected button {idx}") + while joystick.get_button(idx): + time.sleep(poll_interval) + pygame.event.pump() + return "button", idx + + axis_y, axis_y_value = wait_for_axis("Move the left stick UP and hold it") + axis_x, axis_x_value = wait_for_axis("Move the left stick RIGHT and hold it") + dpad_right_source = wait_for_dpad("Press the D-pad RIGHT", expected_x=1) + dpad_left_source = wait_for_dpad("Press the D-pad LEFT", expected_x=-1) + + self.config.axis_y = axis_y + self.config.invert_y = axis_y_value < 0 + self.config.axis_x = axis_x + self.config.invert_x = axis_x_value < 0 + + use_hat = False + hat_index = None + if dpad_right_source[0] == "hat": + use_hat = True + hat_index = dpad_right_source[1] + if dpad_left_source[0] == "hat": + use_hat = True + hat_index = dpad_left_source[1] + + if use_hat and hat_index is not None: + self.config.hat_index = hat_index + else: + self.config.hat_index = -1 + + if dpad_right_source[0] == "button": + self.config.dpad_right_button = dpad_right_source[1] + if dpad_left_source[0] == "button": + self.config.dpad_left_button = dpad_left_source[1] + + print("\nCalibration results:") + print(f" axis_y -> axis {self.config.axis_y} | invert_y={self.config.invert_y}") + print(f" axis_x -> axis {self.config.axis_x} | invert_x={self.config.invert_x}") + if self.config.hat_index >= 0: + print(f" using hat {self.config.hat_index} for yaw commands") + if self.config.hat_index < 0 or dpad_right_source[0] == "button" or dpad_left_source[0] == "button": + print( + " button mapping: " + f"right={self.config.dpad_right_button}, left={self.config.dpad_left_button}" + ) + + self.calibration = GamepadCalibration( + axis_x=self.config.axis_x, + axis_y=self.config.axis_y, + invert_x=self.config.invert_x, + invert_y=self.config.invert_y, + hat_index=self.config.hat_index, + dpad_left_button=self.config.dpad_left_button, + dpad_right_button=self.config.dpad_right_button, + ) + self._save_calibration() + print(f"Calibration saved to {self.calibration_fpath}") + self._calibrated = True + + @property + def is_calibrated(self) -> bool: + return self._calibrated + + def configure(self) -> None: + return None + + def get_action(self) -> dict[str, Any]: + if not self.is_connected or self._pygame is None: + raise DeviceNotConnectedError("LeKiwi base teleoperator is not connected.") + + pygame = self._pygame + joy_removed = getattr(pygame, "JOYDEVICEREMOVED", None) + for event in pygame.event.get(): + if joy_removed is not None and event.type == joy_removed: + self.disconnect() + raise DeviceNotConnectedError("Gamepad disconnected.") + + axis_x = self._joystick.get_axis(self.config.axis_x) + axis_y = self._joystick.get_axis(self.config.axis_y) + if self.config.invert_x: + axis_x = -axis_x + if self.config.invert_y: + axis_y = -axis_y + + norm_x = self._apply_deadzone(axis_x) + norm_y = self._apply_deadzone(axis_y) + + if self.config.normalize_diagonal: + magnitude = math.hypot(norm_x, norm_y) + if magnitude > 1.0: + scale = 1.0 / magnitude + norm_x *= scale + norm_y *= scale + + x_vel, y_vel = self._vector_to_velocities(norm_x, norm_y) + theta_vel = self._read_yaw_velocity() + + action = {"x.vel": x_vel, "y.vel": y_vel, "theta.vel": theta_vel} + self._last_action = action + + if self._clock and self.config.polling_fps > 0: + self._clock.tick(self.config.polling_fps) + + return action + + def send_feedback(self, feedback: dict[str, Any]) -> None: + del feedback + + def disconnect(self) -> None: + if self._joystick is not None: + # Not all pygame versions expose quit() on Joystick, guard accordingly. + if hasattr(self._joystick, "quit"): + with suppress(Exception): + self._joystick.quit() + self._joystick = None + if self._pygame is not None: + self._pygame.joystick.quit() + self._pygame = None + self._clock = None + + def _apply_deadzone(self, value: float) -> float: + if abs(value) < self.config.deadzone: + return 0.0 + return max(-1.0, min(1.0, value)) + + def _vector_to_velocities(self, x_axis: float, y_axis: float) -> tuple[float, float]: + x_vel = y_axis * self.config.max_speed_mps + y_vel = x_axis * self.config.max_speed_mps + return -x_vel, y_vel + + def _read_yaw_velocity(self) -> float: + if not self.is_connected: + return 0.0 + + joystick = self._joystick + if self.config.hat_index >= 0 and joystick.get_numhats() > self.config.hat_index: + hat = joystick.get_hat(self.config.hat_index) + hx = hat[0] + else: + left = joystick.get_button(self.config.dpad_left_button) + right = joystick.get_button(self.config.dpad_right_button) + hx = -1 if left else 0 + hx += 1 if right else 0 + + return -self.config.yaw_speed_deg * hx + + def _apply_calibration_to_config(self) -> None: + if self.calibration is None: + return + self.config.axis_x = self.calibration.axis_x + self.config.axis_y = self.calibration.axis_y + self.config.invert_x = self.calibration.invert_x + self.config.invert_y = self.calibration.invert_y + self.config.hat_index = self.calibration.hat_index + self.config.dpad_left_button = self.calibration.dpad_left_button + self.config.dpad_right_button = self.calibration.dpad_right_button + + def _load_calibration(self, fpath: Path | None = None) -> None: + fpath = self.calibration_fpath if fpath is None else fpath + with open(fpath) as f, draccus.config_type("json"): + calibration = draccus.load(GamepadCalibration, f) + self.calibration = calibration + self._apply_calibration_to_config() + self._calibrated = True + + def _save_calibration(self, fpath: Path | None = None) -> None: + if self.calibration is None: + return + fpath = self.calibration_fpath if fpath is None else fpath + with open(fpath, "w") as f, draccus.config_type("json"): + draccus.dump(self.calibration, f, indent=4) diff --git a/src/lerobot/teleoperators/xlerobot_teleoperator/sub_teleoperators/panthera_keyboard_ee/__init__.py b/src/lerobot/teleoperators/xlerobot_teleoperator/sub_teleoperators/panthera_keyboard_ee/__init__.py new file mode 100644 index 00000000000..66c0318f1bf --- /dev/null +++ b/src/lerobot/teleoperators/xlerobot_teleoperator/sub_teleoperators/panthera_keyboard_ee/__init__.py @@ -0,0 +1,23 @@ +#!/usr/bin/env python + +# Copyright 2026 The HuggingFace Inc. team. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from .config_panthera_keyboard_ee import PantheraKeyboardEETeleopConfig +from .teleop_panthera_keyboard_ee import PantheraKeyboardEETeleop + +__all__ = [ + "PantheraKeyboardEETeleop", + "PantheraKeyboardEETeleopConfig", +] diff --git a/src/lerobot/teleoperators/xlerobot_teleoperator/sub_teleoperators/panthera_keyboard_ee/config_panthera_keyboard_ee.py b/src/lerobot/teleoperators/xlerobot_teleoperator/sub_teleoperators/panthera_keyboard_ee/config_panthera_keyboard_ee.py new file mode 100644 index 00000000000..85421e6de92 --- /dev/null +++ b/src/lerobot/teleoperators/xlerobot_teleoperator/sub_teleoperators/panthera_keyboard_ee/config_panthera_keyboard_ee.py @@ -0,0 +1,49 @@ +#!/usr/bin/env python + +# Copyright 2026 The HuggingFace Inc. team. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from dataclasses import dataclass + +from ....config import TeleoperatorConfig + + +@TeleoperatorConfig.register_subclass("panthera_keyboard_ee") +@dataclass +class PantheraKeyboardEETeleopConfig(TeleoperatorConfig): + """Keyboard teleop config for Panthera EE with Cartesian translation mapping. + + Defaults are chosen to avoid collisions with KeyboardRoverTeleop + (W/A/S/D/Q/E/X for base driving): + - T/G -> delta_x +/- + - F/H -> delta_y +/- + - R/V -> delta_z +/- + """ + + key_x_pos: str = "t" + key_x_neg: str = "g" + key_y_pos: str = "f" + key_y_neg: str = "h" + key_z_pos: str = "r" + key_z_neg: str = "v" + + key_pitch_pos: str = "i" + key_pitch_neg: str = "k" + key_yaw_pos: str = "j" + key_yaw_neg: str = "l" + key_roll_pos: str = "u" + key_roll_neg: str = "o" + + key_gripper_close: str = "z" + key_gripper_open: str = "c" diff --git a/src/lerobot/teleoperators/xlerobot_teleoperator/sub_teleoperators/panthera_keyboard_ee/teleop_panthera_keyboard_ee.py b/src/lerobot/teleoperators/xlerobot_teleoperator/sub_teleoperators/panthera_keyboard_ee/teleop_panthera_keyboard_ee.py new file mode 100644 index 00000000000..bea475d1046 --- /dev/null +++ b/src/lerobot/teleoperators/xlerobot_teleoperator/sub_teleoperators/panthera_keyboard_ee/teleop_panthera_keyboard_ee.py @@ -0,0 +1,150 @@ +#!/usr/bin/env python + +# Copyright 2026 The HuggingFace Inc. team. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import logging +import os +import sys +from queue import Queue +from typing import Any + +from lerobot.processor import RobotAction +from lerobot.utils.decorators import check_if_not_connected + +from ....teleoperator import Teleoperator +from .config_panthera_keyboard_ee import PantheraKeyboardEETeleopConfig + +logger = logging.getLogger(__name__) + +PYNPUT_AVAILABLE = True +try: + if ("DISPLAY" not in os.environ) and ("linux" in sys.platform): + logger.info("No DISPLAY set. Skipping pynput import.") + raise ImportError("pynput blocked intentionally due to no display.") + + from pynput import keyboard +except ImportError: + keyboard = None + PYNPUT_AVAILABLE = False +except Exception as e: + keyboard = None + PYNPUT_AVAILABLE = False + logger.info("Could not import pynput: %s", e) + + +class PantheraKeyboardEETeleop(Teleoperator): + """Keyboard teleop for Panthera end-effector Cartesian + orientation control.""" + + config_class = PantheraKeyboardEETeleopConfig + name = "panthera_keyboard_ee" + + def __init__(self, config: PantheraKeyboardEETeleopConfig): + super().__init__(config) + self.config = config + self.listener = None + self.event_queue: Queue[tuple[str, bool]] = Queue() + self.current_pressed: dict[str, bool] = {} + + @property + def action_features(self) -> dict[str, type]: + return { + "delta_x": float, + "delta_y": float, + "delta_z": float, + "delta_roll": float, + "delta_pitch": float, + "delta_yaw": float, + "gripper": float, + } + + @property + def feedback_features(self) -> dict[str, type]: + return {} + + @property + def is_connected(self) -> bool: + return PYNPUT_AVAILABLE and isinstance(self.listener, keyboard.Listener) and self.listener.is_alive() + + @property + def is_calibrated(self) -> bool: + return True + + def connect(self, calibrate: bool = True) -> None: + del calibrate + if not PYNPUT_AVAILABLE: + logger.warning("pynput not available - Panthera keyboard EE teleop disabled.") + self.listener = None + return + + self.listener = keyboard.Listener(on_press=self._on_press, on_release=self._on_release) + self.listener.start() + logger.info("Panthera keyboard EE teleop connected.") + + def calibrate(self) -> None: + return None + + def configure(self) -> None: + return None + + def _on_press(self, key: Any) -> None: + if hasattr(key, "char") and key.char is not None: + self.event_queue.put((key.char.lower(), True)) + + def _on_release(self, key: Any) -> None: + if hasattr(key, "char") and key.char is not None: + self.event_queue.put((key.char.lower(), False)) + if keyboard is not None and key == keyboard.Key.esc: + self.disconnect() + + def _drain_pressed_keys(self) -> None: + while not self.event_queue.empty(): + key_char, is_pressed = self.event_queue.get_nowait() + self.current_pressed[key_char] = is_pressed + + @check_if_not_connected + def get_action(self) -> RobotAction: + self._drain_pressed_keys() + active = {key for key, pressed in self.current_pressed.items() if pressed} + + delta_x = float((self.config.key_x_pos in active) - (self.config.key_x_neg in active)) + delta_y = float((self.config.key_y_pos in active) - (self.config.key_y_neg in active)) + delta_z = float((self.config.key_z_pos in active) - (self.config.key_z_neg in active)) + delta_roll = float((self.config.key_roll_pos in active) - (self.config.key_roll_neg in active)) + delta_pitch = float((self.config.key_pitch_pos in active) - (self.config.key_pitch_neg in active)) + delta_yaw = float((self.config.key_yaw_pos in active) - (self.config.key_yaw_neg in active)) + + gripper = 1.0 + if self.config.key_gripper_open in active: + gripper = 2.0 + elif self.config.key_gripper_close in active: + gripper = 0.0 + + return { + "delta_x": delta_x, + "delta_y": delta_y, + "delta_z": delta_z, + "delta_roll": delta_roll, + "delta_pitch": delta_pitch, + "delta_yaw": delta_yaw, + "gripper": gripper, + } + + def send_feedback(self, feedback: dict[str, Any]) -> None: + del feedback + + def disconnect(self) -> None: + if self.listener is not None: + self.listener.stop() + self.listener = None diff --git a/src/lerobot/teleoperators/xlerobot_teleoperator/sub_teleoperators/xlerobot_mount_gamepad/__init__.py b/src/lerobot/teleoperators/xlerobot_teleoperator/sub_teleoperators/xlerobot_mount_gamepad/__init__.py new file mode 100644 index 00000000000..bf212e42736 --- /dev/null +++ b/src/lerobot/teleoperators/xlerobot_teleoperator/sub_teleoperators/xlerobot_mount_gamepad/__init__.py @@ -0,0 +1,18 @@ +# Copyright 2025 The HuggingFace Inc. team. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Xbox gamepad teleoperation for XLeRobot mount (pan/tilt control).""" + +from .config import XLeRobotMountGamepadTeleopConfig +from .teleop import XLeRobotMountGamepadTeleop diff --git a/src/lerobot/teleoperators/xlerobot_teleoperator/sub_teleoperators/xlerobot_mount_gamepad/config.py b/src/lerobot/teleoperators/xlerobot_teleoperator/sub_teleoperators/xlerobot_mount_gamepad/config.py new file mode 100644 index 00000000000..1541c2dc708 --- /dev/null +++ b/src/lerobot/teleoperators/xlerobot_teleoperator/sub_teleoperators/xlerobot_mount_gamepad/config.py @@ -0,0 +1,37 @@ +# Copyright 2025 The HuggingFace Inc. team. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + + +from dataclasses import dataclass + +from ....config import TeleoperatorConfig + + +@TeleoperatorConfig.register_subclass("xlerobot_mount_gamepad") +@dataclass +class XLeRobotMountGamepadTeleopConfig(TeleoperatorConfig): + joystick_index: int = 0 # Controller index (0 for first controller) + deadzone: float = 0.15 # Deadzone threshold (0.0-1.0) + polling_fps: int = 50 # Control loop frequency in Hz + + max_pan_speed_dps: float = 60.0 # Maximum pan angular velocity + max_tilt_speed_dps: float = 45.0 # Maximum tilt angular velocity + pan_axis: int = 3 # Right stick horizontal + tilt_axis: int = 4 # Right stick vertical + + invert_pan: bool = False # Invert pan direction if needed + invert_tilt: bool = False # Invert tilt direction if needed + + pan_range: tuple[float, float] = (-90.0, 90.0) + tilt_range: tuple[float, float] = (-30.0, 60.0) diff --git a/src/lerobot/teleoperators/xlerobot_teleoperator/sub_teleoperators/xlerobot_mount_gamepad/teleop.py b/src/lerobot/teleoperators/xlerobot_teleoperator/sub_teleoperators/xlerobot_mount_gamepad/teleop.py new file mode 100644 index 00000000000..e6eea9ebc36 --- /dev/null +++ b/src/lerobot/teleoperators/xlerobot_teleoperator/sub_teleoperators/xlerobot_mount_gamepad/teleop.py @@ -0,0 +1,347 @@ +#!/usr/bin/env python + +# Example teleoperation command for XLeRobot mount with this gamepad teleop: +# +# lerobot-teleoperate \ +# --robot.type=xlerobot_mount \ +# --robot.port=/dev/ttyACM5 \ +# --robot.id=mount \ +# --teleop.type=xlerobot_mount_gamepad \ +# --teleop.joystick_index=0 \ +# --teleop.id=gamepad \ +# --display_data=true + +# Copyright 2025 The HuggingFace Inc. team. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import time +from contextlib import suppress +from dataclasses import dataclass +from pathlib import Path +from typing import Any + +import draccus +import numpy as np + +from lerobot.utils.errors import DeviceNotConnectedError + +from ....teleoperator import Teleoperator +from .config import XLeRobotMountGamepadTeleopConfig + + +@dataclass +class MountGamepadCalibration: + pan_axis: int + tilt_axis: int + invert_pan: bool + invert_tilt: bool + + +class XLeRobotMountGamepadTeleop(Teleoperator): + config_class = XLeRobotMountGamepadTeleopConfig + name = "xlerobot_mount_gamepad" + + def __init__(self, config: XLeRobotMountGamepadTeleopConfig): + self.config = config + super().__init__(config) + self._pygame = None + self._joystick = None + self._clock = None + + # Track current position for incremental control (initialized lazily from observation) + self._current_pan: float | None = None + self._current_tilt: float | None = None + self._pan_bias = 0.0 + self._tilt_bias = 0.0 + calibration = self.calibration if isinstance(self.calibration, MountGamepadCalibration) else None + self.calibration: MountGamepadCalibration | None = calibration + self._calibrated = calibration is not None + if calibration is not None: + self._apply_calibration_to_config() + + @property + def action_features(self) -> dict[str, type]: + return { + "mount_pan.pos": float, + "mount_tilt.pos": float, + } + + @property + def feedback_features(self) -> dict: + return {} + + def connect(self, calibrate: bool = True) -> None: + try: + import pygame + except ImportError as exc: + raise RuntimeError( + "pygame is required for XLeRobotMountGamepadTeleop but is not installed. " + "Install it with `pip install pygame`." + ) from exc + + pygame.init() + pygame.joystick.init() + + if pygame.joystick.get_count() <= self.config.joystick_index: + raise RuntimeError( + f"No joystick detected at index {self.config.joystick_index}. " + f"Found {pygame.joystick.get_count()} joystick(s)." + ) + + self._pygame = pygame + self._joystick = pygame.joystick.Joystick(self.config.joystick_index) + self._joystick.init() + self._clock = pygame.time.Clock() + + # Reset current readings; they will be synced from observations if available + self._current_pan = None + self._current_tilt = None + self._pan_bias = 0.0 + self._tilt_bias = 0.0 + + if calibrate and not self.is_calibrated: + self.calibrate() + elif self.calibration is not None: + self._apply_calibration_to_config() + self._calibrated = True + + self._sync_biases() + + @property + def is_connected(self) -> bool: + return self._joystick is not None + + def calibrate(self) -> None: + if not self.is_connected or self._pygame is None: + raise DeviceNotConnectedError("Mount gamepad teleoperator is not connected.") + + pygame = self._pygame + joystick = self._joystick + + axis_threshold = 0.4 + release_threshold = 0.2 + poll_interval = 0.05 + + if self.calibration is not None: + user_input = input( + "Press ENTER to use existing calibration file associated with this teleoperator, " + "or type 'c' and press ENTER to run calibration: " + ) + if user_input.strip().lower() != "c": + print("Using saved calibration.") + self._apply_calibration_to_config() + self._calibrated = True + self._sync_biases() + return + + print("\nStarting gamepad calibration for XLeRobot mount.") + print("Follow the prompts. Move the indicated control and hold until it is detected.") + + def read_axes() -> list[float]: + pygame.event.pump() + return [joystick.get_axis(i) for i in range(joystick.get_numaxes())] + + def wait_for_axis(prompt: str) -> tuple[int, float]: + print(f"- {prompt}") + baseline = read_axes() + while True: + time.sleep(poll_interval) + current = read_axes() + if not current: + continue + diffs = [current[i] - baseline[i] for i in range(len(current))] + axis_index = max(range(len(diffs)), key=lambda idx: abs(diffs[idx])) + change = diffs[axis_index] + if abs(change) >= axis_threshold: + detected_value = current[axis_index] + print(f" detected axis {axis_index} with value {detected_value:+.2f}") + while True: + time.sleep(poll_interval) + pygame.event.pump() + value = joystick.get_axis(axis_index) + if abs(value - baseline[axis_index]) <= release_threshold: + break + return axis_index, detected_value + + pan_axis, pan_value = wait_for_axis("Move the right stick to the RIGHT and hold it") + + while True: + tilt_axis, tilt_value = wait_for_axis("Move the right stick UP and hold it") + if tilt_axis == pan_axis: + print(" detected the same axis as pan; release the stick and try moving straight up.") + continue + break + + self.config.pan_axis = pan_axis + self.config.invert_pan = pan_value < 0 + self.config.tilt_axis = tilt_axis + self.config.invert_tilt = tilt_value < 0 + + print("\nCalibration results:") + print(f" pan_axis -> axis {self.config.pan_axis} | invert_pan={self.config.invert_pan}") + print(f" tilt_axis -> axis {self.config.tilt_axis} | invert_tilt={self.config.invert_tilt}") + + self.calibration = MountGamepadCalibration( + pan_axis=self.config.pan_axis, + tilt_axis=self.config.tilt_axis, + invert_pan=self.config.invert_pan, + invert_tilt=self.config.invert_tilt, + ) + self._save_calibration() + print(f"Calibration saved to {self.calibration_fpath}") + + self._calibrated = True + self._sync_biases() + + @property + def is_calibrated(self) -> bool: + return self._calibrated + + def configure(self) -> None: + return None + + def on_observation(self, robot_obs: dict[str, Any]) -> None: + if not isinstance(robot_obs, dict): + return + + pan_val = robot_obs.get("mount_pan.pos") + tilt_val = robot_obs.get("mount_tilt.pos") + + if isinstance(pan_val, (int, float)): + self._current_pan = float(pan_val) + + if isinstance(tilt_val, (int, float)): + self._current_tilt = float(tilt_val) + + def get_action(self) -> dict[str, Any]: + if not self.is_connected or self._pygame is None: + raise DeviceNotConnectedError("Mount gamepad teleoperator is not connected.") + + pygame = self._pygame + + # Handle gamepad disconnect events + joy_removed = getattr(pygame, "JOYDEVICEREMOVED", None) + for event in pygame.event.get(): + if joy_removed is not None and event.type == joy_removed: + self.disconnect() + raise DeviceNotConnectedError("Gamepad disconnected.") + + # Read right stick axes + raw_pan = self._joystick.get_axis(self.config.pan_axis) + raw_tilt = self._joystick.get_axis(self.config.tilt_axis) + + # Compensate for resting offsets (e.g., triggers default to -1) + pan_input = raw_pan - self._pan_bias + tilt_input = raw_tilt - self._tilt_bias + + # Apply axis inversions + if self.config.invert_pan: + pan_input = -pan_input + if self.config.invert_tilt: + tilt_input = -tilt_input + + # Apply deadzone + pan_input = self._apply_deadzone(pan_input) + tilt_input = self._apply_deadzone(tilt_input) + + # Convert to velocity and integrate (incremental control) + if self._current_pan is None: + self._current_pan = 0.0 + if self._current_tilt is None: + self._current_tilt = 0.0 + + dt = 1.0 / self.config.polling_fps + self._current_pan += pan_input * self.config.max_pan_speed_dps * dt + self._current_tilt += tilt_input * self.config.max_tilt_speed_dps * dt + + # Clamp to safe ranges + self._current_pan = np.clip(self._current_pan, self.config.pan_range[0], self.config.pan_range[1]) + self._current_tilt = np.clip(self._current_tilt, self.config.tilt_range[0], self.config.tilt_range[1]) + + action = { + "mount_pan.pos": self._current_pan, + "mount_tilt.pos": self._current_tilt, + } + + # Rate limit control loop + if self._clock and self.config.polling_fps > 0: + self._clock.tick(self.config.polling_fps) + + return action + + def send_feedback(self, feedback: dict[str, Any]) -> None: + del feedback + + def disconnect(self) -> None: + if self._joystick is not None: + # Not all pygame versions expose quit() on Joystick, guard accordingly + if hasattr(self._joystick, "quit"): + with suppress(Exception): + self._joystick.quit() + self._joystick = None + + if self._pygame is not None: + with suppress(Exception): + if hasattr(self._pygame, "joystick"): + self._pygame.joystick.quit() + with suppress(Exception): + if hasattr(self._pygame, "quit"): + self._pygame.quit() + self._pygame = None + + self._clock = None + + def _apply_deadzone(self, value: float) -> float: + if abs(value) < self.config.deadzone: + return 0.0 + return max(-1.0, min(1.0, value)) + + def _sync_biases(self) -> None: + if not self.is_connected or self._joystick is None: + self._pan_bias = 0.0 + self._tilt_bias = 0.0 + return + + num_axes = self._joystick.get_numaxes() + if self.config.pan_axis >= num_axes or self.config.tilt_axis >= num_axes: + raise RuntimeError( + "Calibrated axis index exceeds available joystick axes. " + "Re-run calibration for the gamepad teleoperator." + ) + + self._pan_bias = self._joystick.get_axis(self.config.pan_axis) + self._tilt_bias = self._joystick.get_axis(self.config.tilt_axis) + + def _apply_calibration_to_config(self) -> None: + if self.calibration is None: + return + self.config.pan_axis = self.calibration.pan_axis + self.config.tilt_axis = self.calibration.tilt_axis + self.config.invert_pan = self.calibration.invert_pan + self.config.invert_tilt = self.calibration.invert_tilt + + def _load_calibration(self, fpath: Path | None = None) -> None: + fpath = self.calibration_fpath if fpath is None else fpath + with open(fpath) as f, draccus.config_type("json"): + calibration = draccus.load(MountGamepadCalibration, f) + self.calibration = calibration + self._apply_calibration_to_config() + self._calibrated = True + + def _save_calibration(self, fpath: Path | None = None) -> None: + if self.calibration is None: + return + fpath = self.calibration_fpath if fpath is None else fpath + with open(fpath, "w") as f, draccus.config_type("json"): + draccus.dump(self.calibration, f, indent=4)