diff --git a/.gitattributes b/.gitattributes index 302ff0a..dc964c8 100644 --- a/.gitattributes +++ b/.gitattributes @@ -20,3 +20,5 @@ tests/artifacts/cameras/*.png filter=lfs diff=lfs merge=lfs -text *.bag filter=lfs diff=lfs merge=lfs -text *.gif filter=lfs diff=lfs merge=lfs -text +src/lerobot/assets/piper_description/** filter=lfs diff=lfs merge=lfs -text +src/lerobot/assets/piper_x_description/** filter=lfs diff=lfs merge=lfs -text diff --git a/.github/workflows/deploy-website-pages.yml b/.github/workflows/deploy-website-pages.yml index fcc5e79..07922c4 100644 --- a/.github/workflows/deploy-website-pages.yml +++ b/.github/workflows/deploy-website-pages.yml @@ -15,7 +15,7 @@ concurrency: jobs: deploy: # Temporarily disabled to keep the website non-public. - if: ${{ false }} + if: false runs-on: ubuntu-latest environment: name: github-pages diff --git a/.github/workflows/fast_tests.yml b/.github/workflows/fast_tests.yml index 10ec911..e6eb9f5 100644 --- a/.github/workflows/fast_tests.yml +++ b/.github/workflows/fast_tests.yml @@ -61,6 +61,8 @@ jobs: MUJOCO_GL: egl HF_HOME: /mnt/cache/.cache/huggingface HF_LEROBOT_HOME: /mnt/cache/.cache/huggingface/lerobot + # Keep dataset/model downloads on the regular Hub path in CI to avoid hf_xet flakiness. + HF_HUB_DISABLE_XET: "1" steps: - uses: actions/checkout@v6 with: diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index bfa3340..642c50d 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -36,7 +36,9 @@ repos: - id: check-yaml - id: check-toml - id: end-of-file-fixer + exclude: ^src/lerobot/assets/(piper_description|piper_x_description)/ - id: trailing-whitespace + exclude: ^src/lerobot/assets/(piper_description|piper_x_description)/ - repo: https://github.com/astral-sh/ruff-pre-commit rev: v0.14.1 diff --git a/MANIFEST.in b/MANIFEST.in index c1fb2ea..7e40031 100644 --- a/MANIFEST.in +++ b/MANIFEST.in @@ -1,2 +1,4 @@ include src/lerobot/templates/lerobot_modelcard_template.md include src/lerobot/datasets/card_template.md +recursive-include src/lerobot/assets/piper_x_description * +recursive-include src/lerobot/assets/piper_description * diff --git a/README.md b/README.md index c0a4cae..8e6fb8d 100644 --- a/README.md +++ b/README.md @@ -21,14 +21,24 @@ ## 🎯 Evo-RL Focus -- **Open real-world RL on two platforms**: we build and release full real-world RL pipelines on SO101 and AgileX PiPER. +- **Open real-world RL on two platforms**: we build and release full real-world RL pipelines on SO101 and AgileX (PiPER/PiPER-X). - **Open code, models, and datasets for reproducibility**: we continuously release runnable offline RL assets so more people can reproduce results and apply them to real-world tasks. - **Open algorithm and community co-evolution**: we reproduce existing real-world RL methods, propose new methods, and keep publishing data/benchmarks to grow a collaborative open-source community. ## 🚀 News +- **[2026-03-07]** Added AgileX (PiPER/PiPER-X) support for real-world RL. - **[2026-02-26]** First SO101 real-world RL baseline and reproducible CLI workflow are released. +## 🧭 Table of Contents + +| Getting Started | Training Pipeline | Project Info | +| -------------------------------------- | ---------------------------------------------------------------------------- | ------------------------------------------- | +| [⚡ Quick Start](#quick-start) | [4) Value Function Training](#value-function-training) | [Model & Dataset](#model--dataset) | +| [1) Installation](#installation) | [5) Value Inference](#value-inference) | [Community Channels](#community-channels) | +| [2) Hardware Setup](#hardware-setup) | [6) Policy Training](#policy-training) | [Affiliations](#affiliations) | +| [3) Data Collection](#data-collection) | [7) Closed-loop Rollout and Next Round](#closed-loop-rollout-and-next-round) | [Citation](#citation) / [License](#license) | +

Value Visual Results

Success Case

@@ -57,10 +67,14 @@ Human-in-the-Loop Result 2

+ + ## ⚡ Quick Start **LeRobot-aligned foundation:** we use LeRobot as the base of this codebase because its inference and data-collection logic are highly aligned with real-world RL workflows. + + ### 1) Installation ```bash @@ -73,13 +87,14 @@ pip install -e . For setup details and platform-specific dependencies, follow the official [LeRobot configuration guide](https://huggingface.co/docs/lerobot/installation). + + ### 2) Hardware Setup -#### SO101 +#### SO Series (SO100/SO101) -Due to hardware limitations, SO100 is **not supported** for real-world reinforcement learning. - -For SO101 setup, please follow the [official SO101 tutorial](https://wiki.seeedstudio.com/cn/lerobot_so100m/) in detail and complete all installation and configuration steps there before continuing. +For SO-series setup, please follow the [official tutorial](https://wiki.seeedstudio.com/cn/lerobot_so100m/) in detail and complete all installation and configuration steps there before continuing. +The examples below use **SO101** as the reference configuration. #### Device path recommendation @@ -186,9 +201,69 @@ For dual-arm camera mapping, it is fine to attach `front` under either the left- If needed, you can also use temporary device paths (for example `/dev/ttyACM*` and `/dev/video*`) during initial debugging. + + +#### AgileX (PiPER/PiPER-X) + +PiPER arms in master/teaching mode cannot receive external control commands, so all arms must be configured to follower/motion-output mode (0xFC), and firmware must be version 1.8.5 or above. + +For PiPER-series robots, make sure Git LFS assets are pulled before running teleoperation: + +```bash +git lfs pull --include="src/lerobot/assets/piper_description/**,src/lerobot/assets/piper_x_description/**" --exclude="*" +git lfs checkout src/lerobot/assets/piper_description src/lerobot/assets/piper_x_description +``` + +For PiPER setup, PiPER uses CAN interfaces instead of serial ports. +So first run `lerobot-setup-can` to confirm CAN interfaces are available: + +```bash +lerobot-setup-can --mode=setup --interfaces=,,, +``` + +For single-arm users, run the command below to verify the system is ready: + +```bash +lerobot-teleoperate \ + --robot.type=piperx_follower \ + --robot.port= \ + --robot.id=my_piperx_follower \ + --robot.require_calibration=false \ + --teleop.type=piperx_leader \ + --teleop.port= \ + --teleop.id=my_piperx_leader \ + --teleop.require_calibration=false +``` + +For bimanual users, run this command to verify dual-arm teleoperation: + +```bash +lerobot-teleoperate \ + --robot.type=bi_piperx_follower \ + --robot.id=my_bi_piperx_follower \ + --robot.left_arm_config.port= \ + --robot.right_arm_config.port= \ + --robot.left_arm_config.require_calibration=false \ + --robot.right_arm_config.require_calibration=false \ + --teleop.type=bi_piperx_leader \ + --teleop.id=my_bi_piperx_leader \ + --teleop.left_arm_config.port= \ + --teleop.right_arm_config.port= \ + --teleop.left_arm_config.require_calibration=false \ + --teleop.right_arm_config.require_calibration=false +``` + +For PiPER (non-X), replace `bi_piperx_follower`/`bi_piperx_leader` with `bi_piper_follower`/`bi_piper_leader`. + + + ### 3) Data Collection -Collect rollout data with `lerobot-human-inloop-record`. Bimanual template: +Collect rollout data with `lerobot-human-inloop-record`. + +#### SO Series (SO100/SO101) + +Bimanual template: ```bash lerobot-human-inloop-record \ @@ -213,6 +288,33 @@ lerobot-human-inloop-record \ Recommendation: use **`fourcc: "MJPG"`** for OpenCV and **`warmup_s`** for RealSense. In this example `front` uses RealSense, but you can switch it to OpenCV with the same structure. +#### AgileX (PiPER/PiPER-X) + +Bimanual template (left/right, PiPER-X example): + +```bash +lerobot-human-inloop-record \ + --robot.type=bi_piperx_follower \ + --robot.id=my_bi_piperx_follower \ + --robot.left_arm_config.port= \ + --robot.right_arm_config.port= \ + --robot.left_arm_config.require_calibration=false \ + --robot.right_arm_config.require_calibration=false \ + --teleop.type=bi_piperx_leader \ + --teleop.id=my_bi_piperx_leader \ + --teleop.left_arm_config.port= \ + --teleop.right_arm_config.port= \ + --teleop.left_arm_config.require_calibration=false \ + --teleop.right_arm_config.require_calibration=false \ + --dataset.repo_id=/ \ + --dataset.single_task="" \ + --dataset.num_episodes= \ + --dataset.episode_time_s= \ + --dataset.reset_time_s= \ + --dataset.push_to_hub=true \ + --display_data=true +``` + Hotkeys: - `i`: toggle intervention mode (policy <-> teleop takeover) @@ -230,6 +332,8 @@ lerobot-dataset-report --dataset / This prints: dataset meta, totals, episode-length stats/histogram, success/intervention metrics, task list, and full feature schema. + + ### 4) Value Function Training Train the value function on the current dataset. Current default: [Pi\*0.6](https://www.pi.website/blog/pistar06) (`--value.type=pistar06`). @@ -268,6 +372,8 @@ To plug in a different value function, minimal path in this repo: - Add `src/lerobot/values//processor_.py` with `make__pre_post_processors(...)`. - Remove/replace the current `pistar06`-only type checks in `src/lerobot/configs/value_train.py` and `src/lerobot/scripts/lerobot_value_infer.py`. + + ### 5) Value Inference Infer value signals and write value/advantage/indicator back to the dataset: @@ -307,31 +413,22 @@ CUDA_VISIBLE_DEVICES= accelerate launch \ Parameter notes: -- `--acp.n_step`: n-step advantage horizon. -- `--acp.positive_ratio`: positive label ratio after advantage binarization (e.g., `0.3` = top 30% per task). +```bash +--acp.n_step: n-step advantage horizon. +--acp.positive_ratio: positive label ratio after advantage binarization (e.g., 0.3 = top 30% per task). +``` Expected new columns: -- `complementary_info.value_` -- `complementary_info.advantage_` -- `complementary_info.acp_indicator_` +```bash +complementary_info.value_ +complementary_info.advantage_ +complementary_info.acp_indicator_ +``` These columns are written back to the original dataset specified by `--dataset.repo_id`. -Visualization (optional): - -- Enable with `--viz.enable=true`. -- Output videos are saved to `outputs/value_infer//value/viz/`. -- Each video overlays a value curve and per-frame text (`advantage`, `acp_indicator`) on top of camera frames. -- You can control scope with: - - `--viz.episodes=all` or ranges like `0-9,12` - - `--viz.video_key=` (default picks `front` if available) - - `--viz.overwrite=true|false` - - `--viz.vcodec=` and `--viz.frame_storage_mode=memory|disk` - -Value overlay examples (SO101, smooth=11): - -- GIF assets are stored under `website/assets/gifs/`. + ### 6) Policy Training @@ -377,6 +474,8 @@ CUDA_VISIBLE_DEVICES= accelerate launch \ ``` + + ### 7) Closed-loop Rollout and Next Round Deploy the trained policy in human-in-loop mode and collect the next dataset round: diff --git a/pyproject.toml b/pyproject.toml index fbb3296..ed11012 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -74,6 +74,8 @@ dependencies = [ "packaging>=24.2,<26.0", "pynput>=1.7.7,<1.9.0", "pyserial>=3.5,<4.0", + "piper_sdk>=0.6.1,<0.7.0", + "pin>=3.0.0,<4.0.0", "wandb>=0.24.0,<0.25.0", "torch>=2.2.1,<2.8.0", # TODO: Bumb dependency @@ -219,6 +221,12 @@ lerobot-dataset-report="lerobot.scripts.lerobot_dataset_report:main" [tool.setuptools.packages.find] where = ["src"] +[tool.setuptools.package-data] +lerobot = [ + "assets/piper_description/**/*", + "assets/piper_x_description/**/*", +] + [tool.ruff] target-version = "py310" line-length = 110 diff --git a/src/lerobot/assets/piper_description/meshes/base_link.STL b/src/lerobot/assets/piper_description/meshes/base_link.STL new file mode 100644 index 0000000..e724054 --- /dev/null +++ b/src/lerobot/assets/piper_description/meshes/base_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:fd4a3d6d0266205fe3f725f048645be66592c23564db22e08b0807d5569b2c64 +size 607384 diff --git a/src/lerobot/assets/piper_description/meshes/link1.STL b/src/lerobot/assets/piper_description/meshes/link1.STL new file mode 100644 index 0000000..310339a --- /dev/null +++ b/src/lerobot/assets/piper_description/meshes/link1.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:9ee6e1d67c0241f42a0dbaf5c676d19230375ed078ee1943bb8c4c37ab303d14 +size 447784 diff --git a/src/lerobot/assets/piper_description/meshes/link2.STL b/src/lerobot/assets/piper_description/meshes/link2.STL new file mode 100644 index 0000000..cc15d93 --- /dev/null +++ b/src/lerobot/assets/piper_description/meshes/link2.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:0d37675b699b643f2e15348de99aed535fac10dc5f09a79b8e4b0acfaf4e0378 +size 3658384 diff --git a/src/lerobot/assets/piper_description/meshes/link3.STL b/src/lerobot/assets/piper_description/meshes/link3.STL new file mode 100644 index 0000000..68b71f1 --- /dev/null +++ b/src/lerobot/assets/piper_description/meshes/link3.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:e8ae51f17c0c7f6d67b753922e2cc269f5e8372a65734f859231639a64fe8fad +size 1845784 diff --git a/src/lerobot/assets/piper_description/meshes/link4.STL b/src/lerobot/assets/piper_description/meshes/link4.STL new file mode 100644 index 0000000..51b3f77 --- /dev/null +++ b/src/lerobot/assets/piper_description/meshes/link4.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:efa33022976218f246f736a6133494b5d0e5486e8b50b52900c42f285ae85558 +size 877984 diff --git a/src/lerobot/assets/piper_description/meshes/link5.STL b/src/lerobot/assets/piper_description/meshes/link5.STL new file mode 100644 index 0000000..81a4bf9 --- /dev/null +++ b/src/lerobot/assets/piper_description/meshes/link5.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:a1c69d1c7f6f8c60e2c82331c0f444725200ed4053d3e5ec23aa82cb5db5b29f +size 833684 diff --git a/src/lerobot/assets/piper_description/meshes/link6.STL b/src/lerobot/assets/piper_description/meshes/link6.STL new file mode 100644 index 0000000..2550b39 --- /dev/null +++ b/src/lerobot/assets/piper_description/meshes/link6.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:88374973bc3bd9e7389b5853cd6e555af9731c30b658b1ee6f85be34a126cb92 +size 58884 diff --git a/src/lerobot/assets/piper_description/urdf/piper_no_gripper_description.urdf b/src/lerobot/assets/piper_description/urdf/piper_no_gripper_description.urdf new file mode 100644 index 0000000..99cbf30 --- /dev/null +++ b/src/lerobot/assets/piper_description/urdf/piper_no_gripper_description.urdf @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:a93c1e3fc245d9d336a427fcc506cb1fc15615268053392b249816f90c011e50 +size 8273 diff --git a/src/lerobot/assets/piper_x_description/meshes/Link6.STL b/src/lerobot/assets/piper_x_description/meshes/Link6.STL new file mode 100644 index 0000000..7d5dccd --- /dev/null +++ b/src/lerobot/assets/piper_x_description/meshes/Link6.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:0861e122e406f891a57a550551ad1ce0ef4ca5babf58f476cf8914395e144319 +size 162084 diff --git a/src/lerobot/assets/piper_x_description/meshes/dae/J1.dae b/src/lerobot/assets/piper_x_description/meshes/dae/J1.dae new file mode 100644 index 0000000..2834be8 --- /dev/null +++ b/src/lerobot/assets/piper_x_description/meshes/dae/J1.dae @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:e78dee99bc84857c09183463f3fb3fdea292ea7570f1bfb0001a79674bb69d45 +size 2323489 diff --git a/src/lerobot/assets/piper_x_description/meshes/dae/J2.dae b/src/lerobot/assets/piper_x_description/meshes/dae/J2.dae new file mode 100644 index 0000000..dd9a994 --- /dev/null +++ b/src/lerobot/assets/piper_x_description/meshes/dae/J2.dae @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:97e9079580af6e423317c33b2a0f4fe0f6dbc67c3e1d9e6f4836c87c6c8f34a2 +size 14480060 diff --git a/src/lerobot/assets/piper_x_description/meshes/dae/J3.dae b/src/lerobot/assets/piper_x_description/meshes/dae/J3.dae new file mode 100644 index 0000000..79de32f --- /dev/null +++ b/src/lerobot/assets/piper_x_description/meshes/dae/J3.dae @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:db5b39eef745655a67e57870d8ee56f26a03651d2161635ca2ec3c58c9269907 +size 5098355 diff --git a/src/lerobot/assets/piper_x_description/meshes/dae/J4.dae b/src/lerobot/assets/piper_x_description/meshes/dae/J4.dae new file mode 100644 index 0000000..364f5df --- /dev/null +++ b/src/lerobot/assets/piper_x_description/meshes/dae/J4.dae @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:ccc08d3ef1cb8bdfb8ab5395ff65893d13386ad526d7bc954d468b7dc885d557 +size 2037373 diff --git a/src/lerobot/assets/piper_x_description/meshes/dae/J5.dae b/src/lerobot/assets/piper_x_description/meshes/dae/J5.dae new file mode 100644 index 0000000..f648b03 --- /dev/null +++ b/src/lerobot/assets/piper_x_description/meshes/dae/J5.dae @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:78718679ffdf9d81130047c4267ed66a081315eff36f216833bc3781c6241417 +size 2155609 diff --git a/src/lerobot/assets/piper_x_description/meshes/dae/base_link.dae b/src/lerobot/assets/piper_x_description/meshes/dae/base_link.dae new file mode 100644 index 0000000..97f74b6 --- /dev/null +++ b/src/lerobot/assets/piper_x_description/meshes/dae/base_link.dae @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:486dddcd07f072b2e3978087dc2f9b641a91722bccb1bcb3421c4c7ce64a39ed +size 1001764 diff --git a/src/lerobot/assets/piper_x_description/urdf/piper_x_description_no_gripper.urdf b/src/lerobot/assets/piper_x_description/urdf/piper_x_description_no_gripper.urdf new file mode 100644 index 0000000..9885a63 --- /dev/null +++ b/src/lerobot/assets/piper_x_description/urdf/piper_x_description_no_gripper.urdf @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:d6dc6d31b0fa56341b2323fee5e8a375c2500ae6fb02e31fb52e8e917b608439 +size 8169 diff --git a/src/lerobot/async_inference/constants.py b/src/lerobot/async_inference/constants.py index 56910e6..7659c3c 100644 --- a/src/lerobot/async_inference/constants.py +++ b/src/lerobot/async_inference/constants.py @@ -26,4 +26,13 @@ DEFAULT_OBS_QUEUE_TIMEOUT = 2 SUPPORTED_POLICIES = ["act", "smolvla", "diffusion", "tdmpc", "vqbet", "pi0", "pi05", "groot"] # TODO: Add all other robots -SUPPORTED_ROBOTS = ["so100_follower", "so101_follower", "bi_so_follower", "omx_follower"] +SUPPORTED_ROBOTS = [ + "so100_follower", + "so101_follower", + "bi_so_follower", + "piper_follower", + "piperx_follower", + "bi_piper_follower", + "bi_piperx_follower", + "omx_follower", +] diff --git a/src/lerobot/async_inference/robot_client.py b/src/lerobot/async_inference/robot_client.py index e4d2165..b5d068d 100644 --- a/src/lerobot/async_inference/robot_client.py +++ b/src/lerobot/async_inference/robot_client.py @@ -52,10 +52,12 @@ from lerobot.cameras.realsense.configuration_realsense import RealSenseCameraCon from lerobot.robots import ( # noqa: F401 Robot, RobotConfig, + bi_piper_follower, bi_so_follower, koch_follower, make_robot_from_config, omx_follower, + piper_follower, so_follower, ) from lerobot.transport import ( diff --git a/src/lerobot/robots/bi_piper_follower/__init__.py b/src/lerobot/robots/bi_piper_follower/__init__.py new file mode 100644 index 0000000..5fd09c4 --- /dev/null +++ b/src/lerobot/robots/bi_piper_follower/__init__.py @@ -0,0 +1,18 @@ +#!/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 .bi_piper_follower import BiPiperFollower, BiPiperXFollower +from .config_bi_piper_follower import BiPiperFollowerConfig, BiPiperXFollowerConfig diff --git a/src/lerobot/robots/bi_piper_follower/bi_piper_follower.py b/src/lerobot/robots/bi_piper_follower/bi_piper_follower.py new file mode 100644 index 0000000..7031275 --- /dev/null +++ b/src/lerobot/robots/bi_piper_follower/bi_piper_follower.py @@ -0,0 +1,171 @@ +#!/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 +from functools import cached_property + +from lerobot.processor import RobotAction, RobotObservation +from lerobot.robots.piper_follower import ( + PiperFollower, + PiperFollowerConfig, + PiperXFollower, + PiperXFollowerConfig, +) +from lerobot.utils.decorators import check_if_already_connected, check_if_not_connected + +from ..robot import Robot +from .config_bi_piper_follower import BiPiperFollowerConfig, BiPiperXFollowerConfig + +logger = logging.getLogger(__name__) + + +class BiPiperFollower(Robot): + """Bimanual PiPER/PiPER-X follower arms.""" + + config_class = BiPiperFollowerConfig + name = "bi_piper_follower" + _side_field_names = ( + "port", + "judge_flag", + "can_auto_init", + "log_level", + "startup_sleep_s", + "speed_ratio", + "high_follow", + "mode_refresh_interval_s", + "enable_on_connect", + "enable_timeout_s", + "calibration_scale", + "require_calibration", + "sync_gripper", + "gripper_effort_default", + "gripper_status_code", + "cameras", + "disable_on_disconnect", + ) + + def _build_arm_config(self, arm_config_cls, side_cfg, side: str): + kwargs = {name: getattr(side_cfg, name) for name in self._side_field_names} + kwargs["id"] = f"{self.config.id}_{side}" if self.config.id else None + kwargs["calibration_dir"] = self.config.calibration_dir + return arm_config_cls(**kwargs) + + def __init__(self, config: BiPiperFollowerConfig | BiPiperXFollowerConfig): + super().__init__(config) + self.config = config + + if config.type == "bi_piperx_follower": + arm_config_cls = PiperXFollowerConfig + arm_cls = PiperXFollower + else: + arm_config_cls = PiperFollowerConfig + arm_cls = PiperFollower + + left_arm_config = self._build_arm_config(arm_config_cls, config.left_arm_config, "left") + right_arm_config = self._build_arm_config(arm_config_cls, config.right_arm_config, "right") + + self.left_arm = arm_cls(left_arm_config) + self.right_arm = arm_cls(right_arm_config) + + # Only for compatibility with other parts of the codebase that expect `robot.cameras`. + self.cameras = {**self.left_arm.cameras, **self.right_arm.cameras} + + @property + def _motors_ft(self) -> dict[str, type]: + left_arm_motors_ft = self.left_arm._motors_ft + right_arm_motors_ft = self.right_arm._motors_ft + return { + **{f"left_{k}": v for k, v in left_arm_motors_ft.items()}, + **{f"right_{k}": v for k, v in right_arm_motors_ft.items()}, + } + + @property + def _cameras_ft(self) -> dict[str, tuple]: + left_arm_cameras_ft = self.left_arm._cameras_ft + right_arm_cameras_ft = self.right_arm._cameras_ft + return { + **{f"left_{k}": v for k, v in left_arm_cameras_ft.items()}, + **{f"right_{k}": v for k, v in right_arm_cameras_ft.items()}, + } + + @cached_property + def observation_features(self) -> dict[str, type | tuple]: + return {**self._motors_ft, **self._cameras_ft} + + @cached_property + def action_features(self) -> dict[str, type]: + return self._motors_ft + + @property + def is_connected(self) -> bool: + return self.left_arm.is_connected and self.right_arm.is_connected + + @check_if_already_connected + def connect(self, calibrate: bool = True) -> None: + self.left_arm.connect(calibrate) + self.right_arm.connect(calibrate) + + @property + def is_calibrated(self) -> bool: + return self.left_arm.is_calibrated and self.right_arm.is_calibrated + + def calibrate(self) -> None: + self.left_arm.calibrate() + self.right_arm.calibrate() + + def configure(self) -> None: + self.left_arm.configure() + self.right_arm.configure() + + def setup_motors(self) -> None: + self.left_arm.setup_motors() + self.right_arm.setup_motors() + + @check_if_not_connected + def get_observation(self) -> RobotObservation: + obs_dict: RobotObservation = {} + left_obs = self.left_arm.get_observation() + obs_dict.update({f"left_{key}": value for key, value in left_obs.items()}) + right_obs = self.right_arm.get_observation() + obs_dict.update({f"right_{key}": value for key, value in right_obs.items()}) + return obs_dict + + @check_if_not_connected + def send_action(self, action: RobotAction) -> RobotAction: + left_action: RobotAction = {} + right_action: RobotAction = {} + for key, value in action.items(): + if key.startswith("left_"): + left_action[key.removeprefix("left_")] = value + elif key.startswith("right_"): + right_action[key.removeprefix("right_")] = value + + sent_action_left = self.left_arm.send_action(left_action) + sent_action_right = self.right_arm.send_action(right_action) + + prefixed_sent_action_left = {f"left_{key}": value for key, value in sent_action_left.items()} + prefixed_sent_action_right = {f"right_{key}": value for key, value in sent_action_right.items()} + return {**prefixed_sent_action_left, **prefixed_sent_action_right} + + @check_if_not_connected + def disconnect(self): + self.left_arm.disconnect() + self.right_arm.disconnect() + + +class BiPiperXFollower(BiPiperFollower): + config_class = BiPiperXFollowerConfig + name = "bi_piperx_follower" diff --git a/src/lerobot/robots/bi_piper_follower/config_bi_piper_follower.py b/src/lerobot/robots/bi_piper_follower/config_bi_piper_follower.py new file mode 100644 index 0000000..63f6feb --- /dev/null +++ b/src/lerobot/robots/bi_piper_follower/config_bi_piper_follower.py @@ -0,0 +1,39 @@ +#!/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 lerobot.robots.piper_follower import PiperFollowerConfigBase + +from ..config import RobotConfig + + +@RobotConfig.register_subclass("bi_piper_follower") +@dataclass +class BiPiperFollowerConfig(RobotConfig): + """Configuration class for bimanual PiPER follower robots.""" + + left_arm_config: PiperFollowerConfigBase + right_arm_config: PiperFollowerConfigBase + + +@RobotConfig.register_subclass("bi_piperx_follower") +@dataclass +class BiPiperXFollowerConfig(RobotConfig): + """Configuration class for bimanual PiPER-X follower robots.""" + + left_arm_config: PiperFollowerConfigBase + right_arm_config: PiperFollowerConfigBase diff --git a/src/lerobot/robots/piper_follower/__init__.py b/src/lerobot/robots/piper_follower/__init__.py new file mode 100644 index 0000000..a3c5a01 --- /dev/null +++ b/src/lerobot/robots/piper_follower/__init__.py @@ -0,0 +1,18 @@ +#!/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_piper_follower import PiperFollowerConfig, PiperFollowerConfigBase, PiperXFollowerConfig +from .piper_follower import PiperFollower, PiperXFollower diff --git a/src/lerobot/robots/piper_follower/config_piper_follower.py b/src/lerobot/robots/piper_follower/config_piper_follower.py new file mode 100644 index 0000000..7f97432 --- /dev/null +++ b/src/lerobot/robots/piper_follower/config_piper_follower.py @@ -0,0 +1,96 @@ +#!/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 + + +@dataclass +class PiperFollowerConfigBase: + """Configuration for a Piper follower arm controlled over CAN.""" + + # CAN interface name (e.g. "can0") + port: str + + # Piper SDK connection options + judge_flag: bool = False + can_auto_init: bool = True + log_level: str = "WARNING" + startup_sleep_s: float = 0.1 + + # Motion mode for follower arm + speed_ratio: int = 100 + high_follow: bool = True + mode_refresh_interval_s: float = 1.0 + + # Arm enable behavior + enable_on_connect: bool = True + enable_timeout_s: float = 3.0 + + # Calibration precision: + # homing_offset/range_min/range_max are stored as "degree * calibration_scale". + calibration_scale: int = 1000 + # Whether calibration is required on connect when no calibration file exists + require_calibration: bool = True + + # Gripper forwarding behavior + sync_gripper: bool = True + gripper_effort_default: int = 1000 + gripper_status_code: int = 0x01 + + # Optional cameras + cameras: dict[str, CameraConfig] = field(default_factory=dict) + + # Safety behavior on disconnect + disable_on_disconnect: bool = False + + +def _validate_piper_follower_config(config: PiperFollowerConfigBase) -> None: + if not (0 <= config.speed_ratio <= 100): + raise ValueError("`speed_ratio` must be between 0 and 100.") + if config.mode_refresh_interval_s < 0: + raise ValueError("`mode_refresh_interval_s` must be >= 0.") + if config.enable_timeout_s < 0: + raise ValueError("`enable_timeout_s` must be >= 0.") + if config.calibration_scale <= 0: + raise ValueError("`calibration_scale` must be > 0.") + if not isinstance(config.require_calibration, bool): + raise ValueError("require_calibration must be true or false.") + if config.startup_sleep_s < 0: + raise ValueError("`startup_sleep_s` must be >= 0.") + if not (0 <= config.gripper_effort_default <= 5000): + raise ValueError("`gripper_effort_default` must be between 0 and 5000.") + if config.gripper_status_code not in {0x00, 0x01, 0x02, 0x03}: + raise ValueError("`gripper_status_code` must be one of 0x00, 0x01, 0x02, 0x03.") + + +@RobotConfig.register_subclass("piper_follower") +@dataclass +class PiperFollowerConfig(RobotConfig, PiperFollowerConfigBase): + def __post_init__(self): + super().__post_init__() + _validate_piper_follower_config(self) + + +@RobotConfig.register_subclass("piperx_follower") +@dataclass +class PiperXFollowerConfig(RobotConfig, PiperFollowerConfigBase): + def __post_init__(self): + super().__post_init__() + _validate_piper_follower_config(self) diff --git a/src/lerobot/robots/piper_follower/piper_follower.py b/src/lerobot/robots/piper_follower/piper_follower.py new file mode 100644 index 0000000..35874bd --- /dev/null +++ b/src/lerobot/robots/piper_follower/piper_follower.py @@ -0,0 +1,305 @@ +#!/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 time +from functools import cached_property + +from lerobot.cameras.utils import make_cameras_from_configs +from lerobot.motors import MotorCalibration +from lerobot.processor import RobotAction, RobotObservation +from lerobot.utils.decorators import check_if_already_connected, check_if_not_connected +from lerobot.utils.piper_sdk import ( + PIPER_ACTION_KEYS, + PIPER_JOINT_ACTION_KEYS, + PIPER_JOINT_NAMES, + get_piper_sdk, + guard_piper_ctrl_mode_on_connect, + milli_to_unit, + parse_piper_log_level, + unit_to_milli, + wait_enable_piper, +) +from lerobot.utils.utils import enter_pressed, move_cursor_up + +from ..robot import Robot +from .config_piper_follower import PiperFollowerConfig, PiperXFollowerConfig + +logger = logging.getLogger(__name__) +PIPER_CALIB_KEYS = list(PIPER_ACTION_KEYS) +PIPER_CALIB_IDS = {key: idx for idx, key in enumerate(PIPER_CALIB_KEYS)} + + +class PiperFollower(Robot): + """Piper follower arm controlled through the Piper SDK (CAN).""" + + config_class = PiperFollowerConfig + name = "piper_follower" + + def __init__(self, config: PiperFollowerConfig | PiperXFollowerConfig): + super().__init__(config) + self.config = config + self._is_connected = False + self._last_mode_refresh_t = 0.0 + + interface_cls, _ = get_piper_sdk() + self.arm = interface_cls( + can_name=self.config.port, + judge_flag=self.config.judge_flag, + can_auto_init=self.config.can_auto_init, + logger_level=parse_piper_log_level(self.config.log_level), + ) + self.cameras = make_cameras_from_configs(config.cameras) + + @property + def _cameras_ft(self) -> dict[str, tuple]: + return { + cam: (self.config.cameras[cam].height, self.config.cameras[cam].width, 3) for cam in self.cameras + } + + @cached_property + def observation_features(self) -> dict[str, type | tuple]: + return {**dict.fromkeys(PIPER_ACTION_KEYS, float), **self._cameras_ft} + + @cached_property + def action_features(self) -> dict[str, type]: + return dict.fromkeys(PIPER_ACTION_KEYS, float) + + @property + def is_connected(self) -> bool: + return self._is_connected and all(cam.is_connected for cam in self.cameras.values()) + + @check_if_already_connected + def connect(self, calibrate: bool = True) -> None: + self.arm.ConnectPort() + if self.config.startup_sleep_s > 0: + time.sleep(self.config.startup_sleep_s) + guard_piper_ctrl_mode_on_connect(arm=self.arm, interface_name=self.config.port) + + self._is_connected = True + connected_cameras = [] + try: + self.configure() + should_auto_calibrate = not self.is_calibrated and calibrate and self.config.require_calibration + if should_auto_calibrate: + logger.info( + "No piper-follower calibration file found for '%s'. Running lerobot-calibrate flow.", + self.id, + ) + self.calibrate() + # Enable behavior should be controlled by enable_on_connect, independent from calibrate flag. + # This keeps connect(calibrate=False) commandable for callers that only want to skip interactive calibration. + should_enable = self.config.enable_on_connect + if should_enable and not self._wait_enable(self.config.enable_timeout_s): + logger.warning("Piper follower did not report enabled state before timeout.") + + for cam in self.cameras.values(): + cam.connect() + connected_cameras.append(cam) + except Exception: + self.arm.DisconnectPort() + for cam in connected_cameras: + cam.disconnect() + self._is_connected = False + raise + + logger.info("%s connected.", self) + + def _use_uncalibrated_passthrough(self) -> bool: + return not self.is_calibrated and not self.config.require_calibration + + @property + def is_calibrated(self) -> bool: + if not all(key in self.calibration for key in PIPER_CALIB_KEYS): + return False + for key in PIPER_CALIB_KEYS: + cal = self.calibration[key] + if cal.range_max <= cal.range_min: + return False + return True + + def calibrate(self) -> None: + if self.calibration and self.is_calibrated: + user_input = input( + f"Press ENTER to use existing calibration file for id '{self.id}', " + "or type 'c' and press ENTER to run a new calibration: " + ) + if user_input.strip().lower() != "c": + return + + logger.info("Running calibration for %s", self) + # Calibration for Piper follower should be sampled in pure backdrivable mode. + self.arm.DisableArm(7) + time.sleep(0.1) + input("Move piper-follower to your desired neutral/center pose, then press ENTER...") + neutral = self._read_raw_observation() + print("Move all piper-follower joints through full range. Press ENTER to stop recording...") + range_mins, range_maxes = self._record_ranges_of_motion() + + self.calibration = {} + for key in PIPER_CALIB_KEYS: + min_deg = range_mins[key] + max_deg = range_maxes[key] + if max_deg <= min_deg: + raise ValueError(f"Invalid range for {key}: min={min_deg:.3f}, max={max_deg:.3f}") + + neutral_deg = min(max_deg, max(min_deg, neutral[key])) + self.calibration[key] = MotorCalibration( + id=PIPER_CALIB_IDS[key], + drive_mode=0, + homing_offset=self._to_calibration_units(neutral_deg), + range_min=self._to_calibration_units(min_deg), + range_max=self._to_calibration_units(max_deg), + ) + + self._save_calibration() + print(f"Calibration saved to {self.calibration_fpath}") + + def configure(self) -> None: + self._send_motion_mode() + + def _send_motion_mode(self) -> None: + mit_mode = 0xAD if self.config.high_follow else 0x00 + self.arm.MotionCtrl_2(0x01, 0x01, self.config.speed_ratio, mit_mode) + self._last_mode_refresh_t = time.monotonic() + + def _refresh_motion_mode_if_needed(self) -> None: + interval_s = self.config.mode_refresh_interval_s + if interval_s <= 0: + return + now = time.monotonic() + if now - self._last_mode_refresh_t >= interval_s: + self._send_motion_mode() + + def _wait_enable(self, timeout_s: float) -> bool: + return wait_enable_piper(self.arm, timeout_s) + + def _read_raw_observation(self) -> RobotObservation: + joint_msg = self.arm.GetArmJointMsgs() + joint_state = getattr(joint_msg, "joint_state", None) + + obs: RobotObservation = {} + for joint_name in PIPER_JOINT_NAMES: + raw_value = getattr(joint_state, joint_name, 0) + obs[f"{joint_name}.pos"] = milli_to_unit(raw_value) + + gripper_msg = self.arm.GetArmGripperMsgs() + gripper_state = getattr(gripper_msg, "gripper_state", None) + obs["gripper.pos"] = abs(milli_to_unit(getattr(gripper_state, "grippers_angle", 0))) + return obs + + def _to_calibration_units(self, angle_deg: float) -> int: + return int(round(angle_deg * self.config.calibration_scale)) + + def _from_calibration_units(self, value: int) -> float: + return float(value) / float(self.config.calibration_scale) + + def _offset_to_target(self, key: str, offset_deg: float) -> float: + cal = self.calibration[key] + min_deg = self._from_calibration_units(cal.range_min) + max_deg = self._from_calibration_units(cal.range_max) + home_deg = self._from_calibration_units(cal.homing_offset) + centered = -offset_deg if cal.drive_mode else offset_deg + target = home_deg + centered + return min(max_deg, max(min_deg, target)) + + def _record_ranges_of_motion(self) -> tuple[dict[str, float], dict[str, float]]: + current = self._read_raw_observation() + mins = current.copy() + maxes = current.copy() + + while True: + current = self._read_raw_observation() + mins = {key: min(mins[key], current[key]) for key in PIPER_CALIB_KEYS} + maxes = {key: max(maxes[key], current[key]) for key in PIPER_CALIB_KEYS} + + print("\n-----------------------------") + print("JOINT | MIN | POS | MAX") + for key in PIPER_CALIB_KEYS: + print(f"{key:<11} | {mins[key]:>6.2f} | {current[key]:>6.2f} | {maxes[key]:>6.2f}") + + if enter_pressed(): + break + move_cursor_up(len(PIPER_CALIB_KEYS) + 3) + + return mins, maxes + + @check_if_not_connected + def get_observation(self) -> RobotObservation: + obs: dict[str, float] = self._read_raw_observation() + + for cam_key, cam in self.cameras.items(): + obs[cam_key] = cam.async_read() + return obs + + @check_if_not_connected + def send_action(self, action: RobotAction) -> RobotAction: + if not self.is_calibrated and not self._use_uncalibrated_passthrough(): + raise RuntimeError( + f"{self} is not calibrated. Run `lerobot-calibrate --robot.type={self.config.type} --robot.id={self.id}` first." + ) + + self._refresh_motion_mode_if_needed() + + sent_action: dict[str, float] = {} + + joint_keys = PIPER_JOINT_ACTION_KEYS + has_all_joints = all(key in action for key in joint_keys) + if has_all_joints: + if self._use_uncalibrated_passthrough(): + joint_targets = [action[key] for key in joint_keys] + else: + joint_targets = [self._offset_to_target(key, action[key]) for key in joint_keys] + joint_commands = [unit_to_milli(value) for value in joint_targets] + self.arm.JointCtrl(*joint_commands) + sent_action.update( + {key: milli_to_unit(raw) for key, raw in zip(joint_keys, joint_commands, strict=True)} + ) + elif any(key in action for key in joint_keys): + logger.debug("Ignoring partial Piper joint action. Need all six joint keys to send command.") + + if self.config.sync_gripper and "gripper.pos" in action: + if self._use_uncalibrated_passthrough(): + gripper_target = action["gripper.pos"] + else: + gripper_target = self._offset_to_target("gripper.pos", action["gripper.pos"]) + gripper_pos_raw = unit_to_milli(gripper_target) + self.arm.GripperCtrl( + gripper_pos_raw, + self.config.gripper_effort_default, + self.config.gripper_status_code, + 0x00, + ) + sent_action["gripper.pos"] = milli_to_unit(gripper_pos_raw) + + return sent_action + + @check_if_not_connected + def disconnect(self) -> None: + try: + if self.config.disable_on_disconnect: + self.arm.DisableArm(7) + finally: + self.arm.DisconnectPort() + for cam in self.cameras.values(): + cam.disconnect() + self._is_connected = False + logger.info("%s disconnected.", self) + + +class PiperXFollower(PiperFollower): + config_class = PiperXFollowerConfig + name = "piperx_follower" diff --git a/src/lerobot/robots/utils.py b/src/lerobot/robots/utils.py index 92da597..c71cd71 100644 --- a/src/lerobot/robots/utils.py +++ b/src/lerobot/robots/utils.py @@ -64,6 +64,18 @@ def make_robot_from_config(config: RobotConfig) -> Robot: from .openarm_follower import OpenArmFollower return OpenArmFollower(config) + elif config.type == "piper_follower": + from .piper_follower import PiperFollower + + return PiperFollower(config) + elif config.type == "piperx_follower": + from .piper_follower import PiperXFollower + + return PiperXFollower(config) + elif config.type in {"bi_piper_follower", "bi_piperx_follower"}: + from .bi_piper_follower import BiPiperFollower, BiPiperXFollower + + return BiPiperXFollower(config) if config.type == "bi_piperx_follower" else BiPiperFollower(config) elif config.type == "bi_openarm_follower": from .bi_openarm_follower import BiOpenArmFollower diff --git a/src/lerobot/scripts/lerobot_calibrate.py b/src/lerobot/scripts/lerobot_calibrate.py index 1b30021..cd5aba3 100644 --- a/src/lerobot/scripts/lerobot_calibrate.py +++ b/src/lerobot/scripts/lerobot_calibrate.py @@ -37,6 +37,7 @@ from lerobot.robots import ( # noqa: F401 Robot, RobotConfig, bi_openarm_follower, + bi_piper_follower, bi_so_follower, hope_jr, koch_follower, @@ -44,18 +45,21 @@ from lerobot.robots import ( # noqa: F401 make_robot_from_config, omx_follower, openarm_follower, + piper_follower, so_follower, ) from lerobot.teleoperators import ( # noqa: F401 Teleoperator, TeleoperatorConfig, bi_openarm_leader, + bi_piper_leader, bi_so_leader, homunculus, koch_leader, make_teleoperator_from_config, omx_leader, openarm_leader, + piper_leader, so_leader, unitree_g1, ) diff --git a/src/lerobot/scripts/lerobot_record.py b/src/lerobot/scripts/lerobot_record.py index be9a55d..53940c1 100644 --- a/src/lerobot/scripts/lerobot_record.py +++ b/src/lerobot/scripts/lerobot_record.py @@ -86,6 +86,7 @@ from lerobot.processor.rename_processor import rename_stats from lerobot.robots import ( # noqa: F401 RobotConfig, bi_openarm_follower, + bi_piper_follower, bi_so_follower, earthrover_mini_plus, hope_jr, @@ -93,6 +94,7 @@ from lerobot.robots import ( # noqa: F401 make_robot_from_config, omx_follower, openarm_follower, + piper_follower, reachy2, so_follower, unitree_g1 as unitree_g1_robot, @@ -107,12 +109,14 @@ from lerobot.scripts.recording_loop import record_loop from lerobot.teleoperators import ( # noqa: F401 TeleoperatorConfig, bi_openarm_leader, + bi_piper_leader, bi_so_leader, homunculus, koch_leader, make_teleoperator_from_config, omx_leader, openarm_leader, + piper_leader, reachy2_teleoperator, so_leader, unitree_g1, @@ -121,6 +125,7 @@ from lerobot.utils.constants import ACTION from lerobot.utils.control_utils import ( init_keyboard_listener, is_headless, + sanity_check_bimanual_piper_pair, sanity_check_dataset_name, sanity_check_dataset_robot_compatibility, ) @@ -247,6 +252,7 @@ class RecordConfig: if self.teleop is None and self.policy is None: raise ValueError("Choose a policy, a teleoperator or both to control the robot") + sanity_check_bimanual_piper_pair(self.robot, self.teleop) if not self.intervention_toggle_key or len(self.intervention_toggle_key) != 1: raise ValueError("`intervention_toggle_key` must be a single character.") diff --git a/src/lerobot/scripts/lerobot_replay.py b/src/lerobot/scripts/lerobot_replay.py index c9a559d..eacc7c2 100644 --- a/src/lerobot/scripts/lerobot_replay.py +++ b/src/lerobot/scripts/lerobot_replay.py @@ -54,6 +54,7 @@ from lerobot.robots import ( # noqa: F401 Robot, RobotConfig, bi_openarm_follower, + bi_piper_follower, bi_so_follower, earthrover_mini_plus, hope_jr, @@ -61,6 +62,7 @@ from lerobot.robots import ( # noqa: F401 make_robot_from_config, omx_follower, openarm_follower, + piper_follower, reachy2, so_follower, unitree_g1, diff --git a/src/lerobot/scripts/lerobot_setup_can.py b/src/lerobot/scripts/lerobot_setup_can.py index a31727e..9637d35 100644 --- a/src/lerobot/scripts/lerobot_setup_can.py +++ b/src/lerobot/scripts/lerobot_setup_can.py @@ -93,8 +93,8 @@ def check_interface_status(interface: str) -> tuple[bool, str, bool]: return False, "ip command not found", False -def setup_interface(interface: str, bitrate: int, data_bitrate: int, use_fd: bool) -> bool: - """Configure a CAN interface.""" +def setup_interface(interface: str, bitrate: int, data_bitrate: int, use_fd: bool) -> tuple[bool, str]: + """Configure a CAN interface and return (success, error_message).""" try: subprocess.run(["sudo", "ip", "link", "set", interface, "down"], check=False, capture_output=True) # nosec B607 @@ -104,20 +104,41 @@ def setup_interface(interface: str, bitrate: int, data_bitrate: int, use_fd: boo result = subprocess.run(cmd, capture_output=True, text=True) # nosec B607 if result.returncode != 0: - print(f" ✗ Failed to configure: {result.stderr}") - return False + err = (result.stderr or result.stdout or "unknown error").strip() + return False, f"configure failed: {err}" result = subprocess.run( # nosec B607 ["sudo", "ip", "link", "set", interface, "up"], capture_output=True, text=True ) if result.returncode != 0: - print(f" ✗ Failed to bring up: {result.stderr}") - return False + err = (result.stderr or result.stdout or "unknown error").strip() + return False, f"bring-up failed: {err}" - return True + return True, "" except Exception as e: - print(f" ✗ Error: {e}") - return False + return False, f"exception: {e}" + + +def setup_interface_with_fallback(interface: str, cfg: CANSetupConfig) -> tuple[bool, bool | None, str]: + """Try primary mode first, then fallback mode. Returns (success, used_fd, summary).""" + attempts = [cfg.use_fd, not cfg.use_fd] + labels = {True: "CAN FD", False: "CAN 2.0"} + attempt_msgs: list[str] = [] + + for idx, use_fd in enumerate(attempts, start=1): + print(f" Attempt {idx}/{len(attempts)}: {labels[use_fd]}") + ok, err = setup_interface(interface, cfg.bitrate, cfg.data_bitrate, use_fd) + if ok: + is_up, status, is_fd_iface = check_interface_status(interface) + if is_up: + print(f" ✓ {interface}: {status} (using {labels[use_fd]})") + return True, use_fd, labels[use_fd] + err = f"interface not up after setup ({status})" + + print(f" ✗ {err}") + attempt_msgs.append(f"{labels[use_fd]} -> {err}") + + return False, None, " | ".join(attempt_msgs) def test_motor(bus, motor_id: int, timeout: float, use_fd: bool): @@ -162,7 +183,7 @@ def test_interface(cfg: CANSetupConfig, interface: str): """Test all motors on a CAN interface.""" import can - is_up, status, _ = check_interface_status(interface) + is_up, status, is_fd_iface = check_interface_status(interface) print(f"\n{interface}: {status}") if not is_up: @@ -170,8 +191,13 @@ def test_interface(cfg: CANSetupConfig, interface: str): return {} try: + # Avoid EINVAL on non-FD interfaces. + effective_use_fd = cfg.use_fd and is_fd_iface + if cfg.use_fd and not is_fd_iface: + print(" ⚠ Interface is CAN 2.0; auto-fallback to CAN 2.0 test frames.") + kwargs = {"channel": interface, "interface": "socketcan", "bitrate": cfg.bitrate} - if cfg.use_fd: + if effective_use_fd: kwargs.update({"data_bitrate": cfg.data_bitrate, "fd": True}) bus = can.interface.Bus(**kwargs) except Exception as e: @@ -180,12 +206,15 @@ def test_interface(cfg: CANSetupConfig, interface: str): results = {} try: - while bus.recv(timeout=0.01): - pass + # Drain stale frames briefly, but never block indefinitely on a busy bus. + drain_deadline = time.time() + 0.2 + while time.time() < drain_deadline: + if bus.recv(timeout=0.01) is None: + break for motor_id in cfg.motor_ids: motor_name = MOTOR_NAMES.get(motor_id, f"motor_0x{motor_id:02X}") - responses, error = test_motor(bus, motor_id, cfg.timeout, cfg.use_fd) + responses, error = test_motor(bus, motor_id, cfg.timeout, effective_use_fd) if error: print(f" Motor 0x{motor_id:02X} ({motor_name}): ✗ {error}") @@ -213,7 +242,7 @@ def speed_test(cfg: CANSetupConfig, interface: str): """Test communication speed with motors.""" import can - is_up, status, _ = check_interface_status(interface) + is_up, status, is_fd_iface = check_interface_status(interface) if not is_up: print(f"{interface}: {status} - skipping") return @@ -221,8 +250,13 @@ def speed_test(cfg: CANSetupConfig, interface: str): print(f"\n{interface}: Running speed test ({cfg.speed_iterations} iterations)...") try: + # Avoid EINVAL on non-FD interfaces. + effective_use_fd = cfg.use_fd and is_fd_iface + if cfg.use_fd and not is_fd_iface: + print(" ⚠ Interface is CAN 2.0; auto-fallback to CAN 2.0 test frames.") + kwargs = {"channel": interface, "interface": "socketcan", "bitrate": cfg.bitrate} - if cfg.use_fd: + if effective_use_fd: kwargs.update({"data_bitrate": cfg.data_bitrate, "fd": True}) bus = can.interface.Bus(**kwargs) except Exception as e: @@ -231,7 +265,7 @@ def speed_test(cfg: CANSetupConfig, interface: str): responding_motor = None for motor_id in cfg.motor_ids: - responses, _ = test_motor(bus, motor_id, 0.5, cfg.use_fd) + responses, _ = test_motor(bus, motor_id, 0.5, effective_use_fd) if responses: responding_motor = motor_id break @@ -250,7 +284,7 @@ def speed_test(cfg: CANSetupConfig, interface: str): arbitration_id=responding_motor, data=[0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFC], is_extended_id=False, - is_fd=cfg.use_fd, + is_fd=effective_use_fd, ) bus.send(msg) resp = bus.recv(timeout=0.1) @@ -269,29 +303,58 @@ def speed_test(cfg: CANSetupConfig, interface: str): print(" ✗ No successful responses") -def run_setup(cfg: CANSetupConfig): +def run_setup(cfg: CANSetupConfig) -> bool: """Setup CAN interfaces.""" print("=" * 50) print("CAN Interface Setup") print("=" * 50) - print(f"Mode: {'CAN FD' if cfg.use_fd else 'CAN 2.0'}") + print(f"Primary mode: {'CAN FD' if cfg.use_fd else 'CAN 2.0'}") + print(f"Fallback mode: {'CAN 2.0' if cfg.use_fd else 'CAN FD'}") print(f"Bitrate: {cfg.bitrate / 1_000_000:.1f} Mbps") if cfg.use_fd: print(f"Data bitrate: {cfg.data_bitrate / 1_000_000:.1f} Mbps") print() interfaces = cfg.get_interfaces() + success_count = 0 + failed: dict[str, str] = {} + mode_map: dict[str, bool] = {} + for interface in interfaces: print(f"Configuring {interface}...") - if setup_interface(interface, cfg.bitrate, cfg.data_bitrate, cfg.use_fd): - is_up, status, _ = check_interface_status(interface) - print(f" ✓ {interface}: {status}") + ok, used_fd, summary = setup_interface_with_fallback(interface, cfg) + if ok: + success_count += 1 + mode_map[interface] = bool(used_fd) else: - print(f" ✗ {interface}: Failed") + failed[interface] = summary + print(f" ✗ {interface}: setup failed ({summary})") - print("\nSetup complete!") - print("\nNext: Test motors with:") + print() + print("=" * 50) + print("Setup summary") + print("=" * 50) + print(f"Interfaces configured: {success_count}/{len(interfaces)}") + if mode_map: + print("Active mode:") + for interface in interfaces: + if interface in mode_map: + print(f" - {interface}: {'CAN FD' if mode_map[interface] else 'CAN 2.0'}") + if failed: + print("Failed interfaces:") + for interface in interfaces: + if interface in failed: + print(f" - {interface}: {failed[interface]}") + print() + print("✗ Setup failed") + return False + + print() + print("✓ Setup succeeded") + print() + print("Next: Test motors with:") print(f" lerobot-setup-can --mode=test --interfaces {','.join(interfaces)}") + return True def run_test(cfg: CANSetupConfig): @@ -341,7 +404,9 @@ def setup_can(cfg: CANSetupConfig): sys.exit(1) if cfg.mode == "setup": - run_setup(cfg) + ok = run_setup(cfg) + if not ok: + sys.exit(2) elif cfg.mode == "test": run_test(cfg) elif cfg.mode == "speed": diff --git a/src/lerobot/scripts/lerobot_setup_motors.py b/src/lerobot/scripts/lerobot_setup_motors.py index 01af95b..3d675c6 100644 --- a/src/lerobot/scripts/lerobot_setup_motors.py +++ b/src/lerobot/scripts/lerobot_setup_motors.py @@ -30,6 +30,7 @@ import draccus from lerobot.robots import ( # noqa: F401 RobotConfig, + bi_piper_follower, bi_so_follower, koch_follower, lekiwi, @@ -39,6 +40,7 @@ from lerobot.robots import ( # noqa: F401 ) from lerobot.teleoperators import ( # noqa: F401 TeleoperatorConfig, + bi_piper_leader, bi_so_leader, koch_leader, make_teleoperator_from_config, @@ -55,6 +57,10 @@ COMPATIBLE_DEVICES = [ "so100_leader", "so101_follower", "so101_leader", + "bi_piper_follower", + "bi_piperx_follower", + "bi_piper_leader", + "bi_piperx_leader", "lekiwi", ] diff --git a/src/lerobot/scripts/lerobot_teleoperate.py b/src/lerobot/scripts/lerobot_teleoperate.py index b6aa4a7..96ff492 100644 --- a/src/lerobot/scripts/lerobot_teleoperate.py +++ b/src/lerobot/scripts/lerobot_teleoperate.py @@ -71,6 +71,7 @@ from lerobot.robots import ( # noqa: F401 Robot, RobotConfig, bi_openarm_follower, + bi_piper_follower, bi_so_follower, earthrover_mini_plus, hope_jr, @@ -78,6 +79,7 @@ from lerobot.robots import ( # noqa: F401 make_robot_from_config, omx_follower, openarm_follower, + piper_follower, reachy2, so_follower, unitree_g1 as unitree_g1_robot, @@ -86,6 +88,7 @@ from lerobot.teleoperators import ( # noqa: F401 Teleoperator, TeleoperatorConfig, bi_openarm_leader, + bi_piper_leader, bi_so_leader, gamepad, homunculus, @@ -94,10 +97,12 @@ from lerobot.teleoperators import ( # noqa: F401 make_teleoperator_from_config, omx_leader, openarm_leader, + piper_leader, reachy2_teleoperator, so_leader, unitree_g1, ) +from lerobot.utils.control_utils import sanity_check_bimanual_piper_pair from lerobot.utils.import_utils import register_third_party_plugins from lerobot.utils.robot_utils import precise_sleep from lerobot.utils.utils import init_logging, move_cursor_up @@ -204,6 +209,7 @@ def teleop_loop( @parser.wrap() def teleoperate(cfg: TeleoperateConfig): init_logging() + sanity_check_bimanual_piper_pair(cfg.robot, cfg.teleop) logging.info(pformat(asdict(cfg))) if cfg.display_data: init_rerun(session_name="teleoperation", ip=cfg.display_ip, port=cfg.display_port) diff --git a/src/lerobot/teleoperators/bi_piper_leader/__init__.py b/src/lerobot/teleoperators/bi_piper_leader/__init__.py new file mode 100644 index 0000000..90633c8 --- /dev/null +++ b/src/lerobot/teleoperators/bi_piper_leader/__init__.py @@ -0,0 +1,18 @@ +#!/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 .bi_piper_leader import BiPiperLeader, BiPiperXLeader +from .config_bi_piper_leader import BiPiperLeaderConfig, BiPiperXLeaderConfig diff --git a/src/lerobot/teleoperators/bi_piper_leader/bi_piper_leader.py b/src/lerobot/teleoperators/bi_piper_leader/bi_piper_leader.py new file mode 100644 index 0000000..b6c5ae5 --- /dev/null +++ b/src/lerobot/teleoperators/bi_piper_leader/bi_piper_leader.py @@ -0,0 +1,163 @@ +#!/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 +from functools import cached_property +from typing import Any + +from lerobot.processor import RobotAction +from lerobot.teleoperators.piper_leader import ( + PiperLeader, + PiperLeaderConfig, + PiperXLeader, + PiperXLeaderConfig, +) +from lerobot.utils.decorators import check_if_already_connected, check_if_not_connected + +from ..teleoperator import Teleoperator +from .config_bi_piper_leader import BiPiperLeaderConfig, BiPiperXLeaderConfig + +logger = logging.getLogger(__name__) + + +class BiPiperLeader(Teleoperator): + """Bimanual PiPER/PiPER-X leader arms.""" + + config_class = BiPiperLeaderConfig + name = "bi_piper_leader" + _side_field_names = ( + "port", + "judge_flag", + "can_auto_init", + "log_level", + "startup_sleep_s", + "manual_control", + "prefer_ctrl_messages", + "fallback_to_feedback", + "sync_gripper", + "gripper_effort_default", + "gripper_status_code", + "command_speed_ratio", + "command_high_follow", + "mode_refresh_interval_s", + "enable_timeout_s", + "gravity_comp_control_hz", + "gravity_comp_tx_ratio", + "gravity_comp_torque_limit", + "gravity_comp_mit_kp", + "gravity_comp_mit_kd", + "gravity_comp_base_rpy_deg", + "calibration_scale", + "require_calibration", + "disable_on_disconnect", + ) + + def _build_arm_config(self, arm_config_cls, side_cfg, side: str): + kwargs = {name: getattr(side_cfg, name) for name in self._side_field_names} + kwargs["id"] = f"{self.config.id}_{side}" if self.config.id else None + kwargs["calibration_dir"] = self.config.calibration_dir + return arm_config_cls(**kwargs) + + def __init__(self, config: BiPiperLeaderConfig | BiPiperXLeaderConfig): + super().__init__(config) + self.config = config + + if config.type == "bi_piperx_leader": + arm_config_cls = PiperXLeaderConfig + arm_cls = PiperXLeader + else: + arm_config_cls = PiperLeaderConfig + arm_cls = PiperLeader + + left_arm_config = self._build_arm_config(arm_config_cls, config.left_arm_config, "left") + right_arm_config = self._build_arm_config(arm_config_cls, config.right_arm_config, "right") + + self.left_arm = arm_cls(left_arm_config) + self.right_arm = arm_cls(right_arm_config) + + @cached_property + def action_features(self) -> dict[str, type]: + left_arm_features = self.left_arm.action_features + right_arm_features = self.right_arm.action_features + return { + **{f"left_{k}": v for k, v in left_arm_features.items()}, + **{f"right_{k}": v for k, v in right_arm_features.items()}, + } + + @cached_property + def feedback_features(self) -> dict[str, type]: + return {} + + @property + def is_connected(self) -> bool: + return self.left_arm.is_connected and self.right_arm.is_connected + + @check_if_already_connected + def connect(self, calibrate: bool = True) -> None: + self.left_arm.connect(calibrate) + self.right_arm.connect(calibrate) + + @property + def is_calibrated(self) -> bool: + return self.left_arm.is_calibrated and self.right_arm.is_calibrated + + def calibrate(self) -> None: + self.left_arm.calibrate() + self.right_arm.calibrate() + + def configure(self) -> None: + self.left_arm.configure() + self.right_arm.configure() + + def setup_motors(self) -> None: + self.left_arm.setup_motors() + self.right_arm.setup_motors() + + @check_if_not_connected + def set_manual_control(self, enabled: bool) -> None: + self.left_arm.set_manual_control(enabled) + self.right_arm.set_manual_control(enabled) + + @check_if_not_connected + def get_action(self) -> RobotAction: + action_dict: RobotAction = {} + left_action = self.left_arm.get_action() + action_dict.update({f"left_{key}": value for key, value in left_action.items()}) + right_action = self.right_arm.get_action() + action_dict.update({f"right_{key}": value for key, value in right_action.items()}) + return action_dict + + @check_if_not_connected + def send_feedback(self, feedback: dict[str, Any]) -> None: + left_feedback: dict[str, Any] = {} + right_feedback: dict[str, Any] = {} + for key, value in feedback.items(): + if key.startswith("left_"): + left_feedback[key.removeprefix("left_")] = value + elif key.startswith("right_"): + right_feedback[key.removeprefix("right_")] = value + self.left_arm.send_feedback(left_feedback) + self.right_arm.send_feedback(right_feedback) + + @check_if_not_connected + def disconnect(self) -> None: + self.left_arm.disconnect() + self.right_arm.disconnect() + + +class BiPiperXLeader(BiPiperLeader): + config_class = BiPiperXLeaderConfig + name = "bi_piperx_leader" diff --git a/src/lerobot/teleoperators/bi_piper_leader/config_bi_piper_leader.py b/src/lerobot/teleoperators/bi_piper_leader/config_bi_piper_leader.py new file mode 100644 index 0000000..51e3601 --- /dev/null +++ b/src/lerobot/teleoperators/bi_piper_leader/config_bi_piper_leader.py @@ -0,0 +1,39 @@ +#!/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 lerobot.teleoperators.piper_leader import PiperLeaderConfigBase + +from ..config import TeleoperatorConfig + + +@TeleoperatorConfig.register_subclass("bi_piper_leader") +@dataclass +class BiPiperLeaderConfig(TeleoperatorConfig): + """Configuration class for bimanual PiPER leader teleoperators.""" + + left_arm_config: PiperLeaderConfigBase + right_arm_config: PiperLeaderConfigBase + + +@TeleoperatorConfig.register_subclass("bi_piperx_leader") +@dataclass +class BiPiperXLeaderConfig(TeleoperatorConfig): + """Configuration class for bimanual PiPER-X leader teleoperators.""" + + left_arm_config: PiperLeaderConfigBase + right_arm_config: PiperLeaderConfigBase diff --git a/src/lerobot/teleoperators/piper_leader/__init__.py b/src/lerobot/teleoperators/piper_leader/__init__.py new file mode 100644 index 0000000..d063ff6 --- /dev/null +++ b/src/lerobot/teleoperators/piper_leader/__init__.py @@ -0,0 +1,18 @@ +#!/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_piper_leader import PiperLeaderConfig, PiperLeaderConfigBase, PiperXLeaderConfig +from .piper_leader import PiperLeader, PiperXLeader diff --git a/src/lerobot/teleoperators/piper_leader/config_piper_leader.py b/src/lerobot/teleoperators/piper_leader/config_piper_leader.py new file mode 100644 index 0000000..7376850 --- /dev/null +++ b/src/lerobot/teleoperators/piper_leader/config_piper_leader.py @@ -0,0 +1,109 @@ +#!/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 + + +@dataclass +class PiperLeaderConfigBase: + """Configuration for a Piper leader arm used as teleoperator.""" + + # CAN interface name (e.g. "can1") + port: str + + # Piper SDK connection options + judge_flag: bool = False + can_auto_init: bool = True + log_level: str = "WARNING" + startup_sleep_s: float = 0.1 + + # Manual backdrivable mode for human teleop + manual_control: bool = True + + # Read control messages from leader first, fallback to feedback state if missing + prefer_ctrl_messages: bool = True + fallback_to_feedback: bool = True + + # Gripper handling + sync_gripper: bool = True + gripper_effort_default: int = 1000 + gripper_status_code: int = 0x01 + + # Command mode for send_feedback + command_speed_ratio: int = 100 + command_high_follow: bool = True + mode_refresh_interval_s: float = 1.0 + enable_timeout_s: float = 3.0 + + # Gravity compensation settings (used when manual_control=true) + gravity_comp_control_hz: float = 200.0 + gravity_comp_tx_ratio: tuple[float, float, float, float, float, float] = (1.0, 1.0, 1.0, 1.0, 1.0, 1.0) + gravity_comp_torque_limit: float = 8.0 + gravity_comp_mit_kp: float = 0.0 + gravity_comp_mit_kd: float = 0.0 + gravity_comp_base_rpy_deg: tuple[float, float, float] = (0.0, 0.0, 0.0) + + # Calibration precision: + # homing_offset/range_min/range_max are stored as "degree * calibration_scale". + calibration_scale: int = 1000 + # Whether calibration is required on connect when no calibration file exists + require_calibration: bool = True + + # Safety behavior on disconnect + disable_on_disconnect: bool = False + + +def _validate_piper_leader_config(config: PiperLeaderConfigBase) -> None: + if not (0 <= config.command_speed_ratio <= 100): + raise ValueError("`command_speed_ratio` must be between 0 and 100.") + if config.mode_refresh_interval_s < 0: + raise ValueError("`mode_refresh_interval_s` must be >= 0.") + if config.enable_timeout_s < 0: + raise ValueError("`enable_timeout_s` must be >= 0.") + if config.gravity_comp_control_hz <= 0: + raise ValueError("`gravity_comp_control_hz` must be > 0.") + if len(config.gravity_comp_tx_ratio) != 6: + raise ValueError("`gravity_comp_tx_ratio` must contain exactly 6 values.") + if config.gravity_comp_torque_limit <= 0: + raise ValueError("`gravity_comp_torque_limit` must be > 0.") + if len(config.gravity_comp_base_rpy_deg) != 3: + raise ValueError("`gravity_comp_base_rpy_deg` must contain exactly 3 values.") + if config.calibration_scale <= 0: + raise ValueError("`calibration_scale` must be > 0.") + if not isinstance(config.require_calibration, bool): + raise ValueError("require_calibration must be true or false.") + if config.startup_sleep_s < 0: + raise ValueError("`startup_sleep_s` must be >= 0.") + if not (0 <= config.gripper_effort_default <= 5000): + raise ValueError("`gripper_effort_default` must be between 0 and 5000.") + if config.gripper_status_code not in {0x00, 0x01, 0x02, 0x03}: + raise ValueError("`gripper_status_code` must be one of 0x00, 0x01, 0x02, 0x03.") + + +@TeleoperatorConfig.register_subclass("piper_leader") +@dataclass +class PiperLeaderConfig(TeleoperatorConfig, PiperLeaderConfigBase): + def __post_init__(self): + _validate_piper_leader_config(self) + + +@TeleoperatorConfig.register_subclass("piperx_leader") +@dataclass +class PiperXLeaderConfig(TeleoperatorConfig, PiperLeaderConfigBase): + def __post_init__(self): + _validate_piper_leader_config(self) diff --git a/src/lerobot/teleoperators/piper_leader/gravity_compensation.py b/src/lerobot/teleoperators/piper_leader/gravity_compensation.py new file mode 100644 index 0000000..fcdaf1e --- /dev/null +++ b/src/lerobot/teleoperators/piper_leader/gravity_compensation.py @@ -0,0 +1,192 @@ +#!/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 math +import threading +import time +from pathlib import Path +from typing import Any + +import numpy as np + +logger = logging.getLogger(__name__) + + +def _rpy_deg_to_rot(roll_deg: float, pitch_deg: float, yaw_deg: float) -> np.ndarray: + roll = math.radians(roll_deg) + pitch = math.radians(pitch_deg) + yaw = math.radians(yaw_deg) + + cx, sx = math.cos(roll), math.sin(roll) + cy, sy = math.cos(pitch), math.sin(pitch) + cz, sz = math.cos(yaw), math.sin(yaw) + + rot_x = np.array([[1.0, 0.0, 0.0], [0.0, cx, -sx], [0.0, sx, cx]], dtype=np.float64) + rot_y = np.array([[cy, 0.0, sy], [0.0, 1.0, 0.0], [-sy, 0.0, cy]], dtype=np.float64) + rot_z = np.array([[cz, -sz, 0.0], [sz, cz, 0.0], [0.0, 0.0, 1.0]], dtype=np.float64) + return rot_z @ rot_y @ rot_x + + +class PiperGravityCompensationLoop: + """Background MIT torque loop for gravity compensation on Piper leader arm.""" + + def __init__( + self, + *, + arm: Any, + urdf_path: str, + control_hz: float, + tx_ratio: tuple[float, float, float, float, float, float], + torque_limit: float, + mit_kp: float, + mit_kd: float, + base_rpy_deg: tuple[float, float, float], + mode_refresh_interval_s: float, + move_speed_ratio: int, + ) -> None: + import pinocchio as pin + + self._pin = pin + self._arm = arm + self._control_hz = control_hz + self._tx_ratio = np.asarray(tx_ratio, dtype=np.float64) + self._torque_limit = float(torque_limit) + self._mit_kp = float(mit_kp) + self._mit_kd = float(mit_kd) + self._mode_refresh_interval_s = max(0.0, float(mode_refresh_interval_s)) + self._move_speed_ratio = int(move_speed_ratio) + + urdf = Path(urdf_path).expanduser().resolve() + if not urdf.is_file(): + raise FileNotFoundError(f"gravity compensation URDF does not exist: {urdf}") + + package_dirs = [str(urdf.parent.parent)] + self._robot = pin.RobotWrapper.BuildFromURDF(str(urdf), package_dirs) + self._robot.data = self._robot.model.createData() + self._nq = 6 + + rot_world_base = _rpy_deg_to_rot(*base_rpy_deg) + gravity_world = np.array([0.0, 0.0, -9.81], dtype=np.float64) + gravity_base = rot_world_base.T @ gravity_world + self._robot.model.gravity.linear = gravity_base + + self._thread: threading.Thread | None = None + self._stop_event = threading.Event() + self._last_mode_refresh_t = 0.0 + + def start(self) -> None: + if self._thread is not None and self._thread.is_alive(): + return + self._stop_event.clear() + self._thread = threading.Thread( + target=self._run, + name="piper-gravity-comp-loop", + daemon=True, + ) + self._thread.start() + + def stop(self) -> None: + self._stop_event.set() + if self._thread is None: + return + self._thread.join(timeout=1.0) + if self._thread.is_alive(): + logger.warning("Piper gravity compensation thread did not stop within timeout.") + self._thread = None + + def _send_mit_mode(self) -> None: + # Use MOVE M + MIT to ensure torque pass-through mode is active. + self._arm.MotionCtrl_2(0x01, 0x04, self._move_speed_ratio, 0xAD) + self._last_mode_refresh_t = time.monotonic() + + def _refresh_mit_mode_if_needed(self) -> None: + if self._mode_refresh_interval_s <= 0.0: + return + now = time.monotonic() + if now - self._last_mode_refresh_t >= self._mode_refresh_interval_s: + self._send_mit_mode() + + def _read_q_v(self) -> tuple[np.ndarray, np.ndarray]: + joint_msg = self._arm.GetArmJointMsgs() + joint_state = joint_msg.joint_state + q_deg = np.array( + [ + float(joint_state.joint_1) * 1e-3, + float(joint_state.joint_2) * 1e-3, + float(joint_state.joint_3) * 1e-3, + float(joint_state.joint_4) * 1e-3, + float(joint_state.joint_5) * 1e-3, + float(joint_state.joint_6) * 1e-3, + ], + dtype=np.float64, + ) + q_rad = np.deg2rad(q_deg) + + hs = self._arm.GetArmHighSpdInfoMsgs() + v_rad = np.array( + [ + float(hs.motor_1.motor_speed) * 1e-3, + float(hs.motor_2.motor_speed) * 1e-3, + float(hs.motor_3.motor_speed) * 1e-3, + float(hs.motor_4.motor_speed) * 1e-3, + float(hs.motor_5.motor_speed) * 1e-3, + float(hs.motor_6.motor_speed) * 1e-3, + ], + dtype=np.float64, + ) + return q_rad, v_rad + + def _compute_gravity_torque(self, q_rad: np.ndarray, v_rad: np.ndarray) -> np.ndarray: + q_full = np.zeros(self._robot.model.nq, dtype=np.float64) + v_full = np.zeros(self._robot.model.nv, dtype=np.float64) + q_full[: self._nq] = q_rad + v_full[: self._nq] = v_rad + tau_full = self._pin.rnea( + self._robot.model, + self._robot.data, + q_full, + v_full, + np.zeros(self._robot.model.nv, dtype=np.float64), + ) + return np.asarray(tau_full[: self._nq], dtype=np.float64) + + def _run(self) -> None: + self._send_mit_mode() + dt = 1.0 / self._control_hz + while not self._stop_event.is_set(): + start_t = time.perf_counter() + self._refresh_mit_mode_if_needed() + + q_rad, v_rad = self._read_q_v() + tau = self._compute_gravity_torque(q_rad, v_rad) + tau = np.clip(self._tx_ratio * tau, -self._torque_limit, self._torque_limit) + + for idx in range(6): + self._arm.JointMitCtrl( + idx + 1, + 0.0, + 0.0, + self._mit_kp, + self._mit_kd, + float(tau[idx]), + ) + + remain = dt - (time.perf_counter() - start_t) + if remain > 0: + time.sleep(remain) diff --git a/src/lerobot/teleoperators/piper_leader/piper_leader.py b/src/lerobot/teleoperators/piper_leader/piper_leader.py new file mode 100644 index 0000000..ffca58b --- /dev/null +++ b/src/lerobot/teleoperators/piper_leader/piper_leader.py @@ -0,0 +1,425 @@ +#!/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 time +from functools import cached_property +from importlib import resources +from typing import Any + +from lerobot.motors import MotorCalibration +from lerobot.processor import RobotAction +from lerobot.utils.decorators import check_if_already_connected, check_if_not_connected +from lerobot.utils.piper_sdk import ( + PIPER_ACTION_KEYS, + PIPER_JOINT_ACTION_KEYS, + PIPER_JOINT_NAMES, + get_piper_sdk, + guard_piper_ctrl_mode_on_connect, + milli_to_unit, + parse_piper_log_level, + unit_to_milli, + wait_enable_piper, +) +from lerobot.utils.utils import enter_pressed, move_cursor_up + +from ..teleoperator import Teleoperator +from .config_piper_leader import PiperLeaderConfig, PiperXLeaderConfig +from .gravity_compensation import PiperGravityCompensationLoop + +logger = logging.getLogger(__name__) +PIPER_CALIB_KEYS = list(PIPER_ACTION_KEYS) +PIPER_CALIB_IDS = {key: idx for idx, key in enumerate(PIPER_CALIB_KEYS)} +DEFAULT_PIPER_GRAVITY_URDF = "assets/piper_description/urdf/piper_no_gripper_description.urdf" +DEFAULT_PIPERX_GRAVITY_URDF = "assets/piper_x_description/urdf/piper_x_description_no_gripper.urdf" +GIT_LFS_POINTER_PREFIX = b"version https://git-lfs.github.com/spec/v1" + + +def _ensure_not_lfs_pointer(path_obj: Any, relpath: str) -> None: + with path_obj.open("rb") as f: + head = f.read(128) + if head.startswith(GIT_LFS_POINTER_PREFIX): + raise RuntimeError( + "Bundled PiPER assets are still Git LFS pointer files. " + "Please run `git lfs pull --include=" + '"src/lerobot/assets/piper_description/**,src/lerobot/assets/piper_x_description/**" ' + '--exclude="*"` and `git lfs checkout src/lerobot/assets/piper_description ' + "src/lerobot/assets/piper_x_description`, then retry. " + f"(file: {relpath})" + ) + + +class PiperLeader(Teleoperator): + """Piper leader arm used as a teleoperator through Piper SDK CAN messages.""" + + config_class = PiperLeaderConfig + name = "piper_leader" + gravity_comp_urdf_relpath = DEFAULT_PIPER_GRAVITY_URDF + + def __init__(self, config: PiperLeaderConfig | PiperXLeaderConfig): + super().__init__(config) + self.config = config + self._is_connected = False + self._manual_control_enabled: bool | None = None + self._last_mode_refresh_t = 0.0 + self._gravity_comp_loop: PiperGravityCompensationLoop | None = None + + interface_cls, _ = get_piper_sdk() + self.arm = interface_cls( + can_name=self.config.port, + judge_flag=self.config.judge_flag, + can_auto_init=self.config.can_auto_init, + logger_level=parse_piper_log_level(self.config.log_level), + ) + + @cached_property + def action_features(self) -> dict[str, type]: + return dict.fromkeys(PIPER_ACTION_KEYS, float) + + @cached_property + def feedback_features(self) -> dict[str, type]: + return dict.fromkeys(PIPER_ACTION_KEYS, float) + + @property + def is_connected(self) -> bool: + return self._is_connected + + @check_if_already_connected + def connect(self, calibrate: bool = True) -> None: + self.arm.ConnectPort() + if self.config.startup_sleep_s > 0: + time.sleep(self.config.startup_sleep_s) + guard_piper_ctrl_mode_on_connect(arm=self.arm, interface_name=self.config.port) + + self._is_connected = True + # Recompute control mode on every fresh connection. + self._manual_control_enabled = None + try: + self.configure() + if not self.is_calibrated and calibrate and self.config.require_calibration: + logger.info( + "No piper-leader calibration file found for '%s'. Running lerobot-calibrate flow.", + self.id, + ) + self.calibrate() + except Exception: + self._stop_gravity_comp_loop_if_needed() + self.arm.DisconnectPort() + self._is_connected = False + raise + + logger.info("%s connected.", self) + + def _use_uncalibrated_passthrough(self) -> bool: + return not self.is_calibrated and not self.config.require_calibration + + @property + def is_calibrated(self) -> bool: + if not all(key in self.calibration for key in PIPER_CALIB_KEYS): + return False + for key in PIPER_CALIB_KEYS: + cal = self.calibration[key] + if cal.range_max <= cal.range_min: + return False + return True + + def calibrate(self) -> None: + if self.calibration and self.is_calibrated: + user_input = input( + f"Press ENTER to use existing calibration file for id '{self.id}', " + "or type 'c' and press ENTER to run a new calibration: " + ) + if user_input.strip().lower() != "c": + return + + logger.info("Running calibration for %s", self) + input("Move piper-leader to your desired neutral/center pose, then press ENTER...") + neutral = self._read_raw_action() + print("Move all piper-leader joints through full range. Press ENTER to stop recording...") + range_mins, range_maxes = self._record_ranges_of_motion() + + self.calibration = {} + for key in PIPER_CALIB_KEYS: + min_deg = range_mins[key] + max_deg = range_maxes[key] + if max_deg <= min_deg: + raise ValueError(f"Invalid range for {key}: min={min_deg:.3f}, max={max_deg:.3f}") + + neutral_deg = min(max_deg, max(min_deg, neutral[key])) + self.calibration[key] = MotorCalibration( + id=PIPER_CALIB_IDS[key], + drive_mode=0, + homing_offset=self._to_calibration_units(neutral_deg), + range_min=self._to_calibration_units(min_deg), + range_max=self._to_calibration_units(max_deg), + ) + + self._save_calibration() + print(f"Calibration saved to {self.calibration_fpath}") + + def _send_command_mode(self) -> None: + mit_mode = 0xAD if self.config.command_high_follow else 0x00 + self.arm.MotionCtrl_2(0x01, 0x01, self.config.command_speed_ratio, mit_mode) + self._last_mode_refresh_t = time.monotonic() + + def _refresh_command_mode_if_needed(self) -> None: + interval_s = self.config.mode_refresh_interval_s + if interval_s <= 0: + return + now = time.monotonic() + if now - self._last_mode_refresh_t >= interval_s: + self._send_command_mode() + + def _wait_enable(self, timeout_s: float) -> bool: + return wait_enable_piper(self.arm, timeout_s) + + def _send_gripper_ctrl(self, gripper_pos_raw: int, enabled: bool) -> None: + self.arm.GripperCtrl( + gripper_pos_raw, + self.config.gripper_effort_default if enabled else 0, + self.config.gripper_status_code if enabled else 0x00, + 0x00, + ) + + def _set_gripper_enabled(self, enabled: bool) -> None: + gripper_pos_raw = 0 + try: + gripper_msg = self.arm.GetArmGripperMsgs() + gripper_state = getattr(gripper_msg, "gripper_state", None) + if gripper_state is not None: + gripper_pos_raw = abs(int(getattr(gripper_state, "grippers_angle", 0))) + except Exception: + logger.debug("Could not read current gripper angle before setting enable=%s.", enabled) + self._send_gripper_ctrl(gripper_pos_raw, enabled) + + def set_manual_control(self, enabled: bool) -> None: + if not self._is_connected: + return + if enabled and self._manual_control_enabled is not True: + if not self._wait_enable(self.config.enable_timeout_s): + logger.warning( + "Piper leader did not report enabled state before entering gravity compensation." + ) + if self.config.sync_gripper: + self._set_gripper_enabled(False) + self._ensure_gravity_comp_loop().start() + self._manual_control_enabled = True + return + if not enabled and self._manual_control_enabled is not False: + self._stop_gravity_comp_loop_if_needed() + self._send_command_mode() + if not self._wait_enable(self.config.enable_timeout_s): + logger.warning("Piper leader did not report enabled state before timeout.") + if self.config.sync_gripper: + self._set_gripper_enabled(True) + self._manual_control_enabled = False + + def _ensure_gravity_comp_loop(self) -> PiperGravityCompensationLoop: + if self._gravity_comp_loop is None: + default_urdf = resources.files("lerobot").joinpath(self.gravity_comp_urdf_relpath) + if not default_urdf.is_file(): + raise FileNotFoundError( + "Bundled gravity compensation URDF is missing: " + f"{self.gravity_comp_urdf_relpath}. Reinstall the package." + ) + _ensure_not_lfs_pointer(default_urdf, self.gravity_comp_urdf_relpath) + urdf_path = str(default_urdf) + self._gravity_comp_loop = PiperGravityCompensationLoop( + arm=self.arm, + urdf_path=urdf_path, + control_hz=self.config.gravity_comp_control_hz, + tx_ratio=self.config.gravity_comp_tx_ratio, + torque_limit=self.config.gravity_comp_torque_limit, + mit_kp=self.config.gravity_comp_mit_kp, + mit_kd=self.config.gravity_comp_mit_kd, + base_rpy_deg=self.config.gravity_comp_base_rpy_deg, + mode_refresh_interval_s=self.config.mode_refresh_interval_s, + move_speed_ratio=self.config.command_speed_ratio, + ) + return self._gravity_comp_loop + + def _stop_gravity_comp_loop_if_needed(self) -> None: + if self._gravity_comp_loop is not None: + self._gravity_comp_loop.stop() + + def configure(self) -> None: + self.set_manual_control(self.config.manual_control) + + def _read_joint_from_ctrl(self) -> dict[str, float] | None: + joint_ctrl_msg = self.arm.GetArmJointCtrl() + if getattr(joint_ctrl_msg, "time_stamp", 0.0) <= 0: + return None + joint_ctrl = getattr(joint_ctrl_msg, "joint_ctrl", None) + return { + f"{joint_name}.pos": milli_to_unit(getattr(joint_ctrl, joint_name, 0)) + for joint_name in PIPER_JOINT_NAMES + } + + def _read_joint_from_feedback(self) -> dict[str, float] | None: + joint_msg = self.arm.GetArmJointMsgs() + joint_state = getattr(joint_msg, "joint_state", None) + if joint_state is None: + return None + return { + f"{joint_name}.pos": milli_to_unit(getattr(joint_state, joint_name, 0)) + for joint_name in PIPER_JOINT_NAMES + } + + def _read_gripper_from_ctrl(self) -> float | None: + gripper_ctrl_msg = self.arm.GetArmGripperCtrl() + if getattr(gripper_ctrl_msg, "time_stamp", 0.0) <= 0: + return None + gripper_ctrl = getattr(gripper_ctrl_msg, "gripper_ctrl", None) + return abs(milli_to_unit(getattr(gripper_ctrl, "grippers_angle", 0))) + + def _read_gripper_from_feedback(self) -> float | None: + gripper_msg = self.arm.GetArmGripperMsgs() + gripper_state = getattr(gripper_msg, "gripper_state", None) + if gripper_state is None: + return None + return abs(milli_to_unit(getattr(gripper_state, "grippers_angle", 0))) + + def _read_raw_action(self) -> RobotAction: + used_feedback_for_joints = False + action: dict[str, float] | None = None + if self.config.prefer_ctrl_messages: + action = self._read_joint_from_ctrl() + + if action is None and self.config.fallback_to_feedback: + action = self._read_joint_from_feedback() + used_feedback_for_joints = action is not None + + if action is None: + action = {f"{joint_name}.pos": 0.0 for joint_name in PIPER_JOINT_NAMES} + + use_ctrl_for_gripper = self.config.prefer_ctrl_messages and not used_feedback_for_joints + gripper_pos = self._read_gripper_from_ctrl() if use_ctrl_for_gripper else None + if gripper_pos is None and self.config.fallback_to_feedback: + gripper_pos = self._read_gripper_from_feedback() + action["gripper.pos"] = 0.0 if gripper_pos is None else gripper_pos + return action + + def _to_calibration_units(self, angle_deg: float) -> int: + return int(round(angle_deg * self.config.calibration_scale)) + + def _from_calibration_units(self, value: int) -> float: + return float(value) / float(self.config.calibration_scale) + + def _calibrated_to_offset(self, key: str, raw_deg: float) -> float: + cal = self.calibration[key] + min_deg = self._from_calibration_units(cal.range_min) + max_deg = self._from_calibration_units(cal.range_max) + home_deg = self._from_calibration_units(cal.homing_offset) + bounded = min(max_deg, max(min_deg, raw_deg)) + centered = bounded - home_deg + return -centered if cal.drive_mode else centered + + def _offset_to_calibrated(self, key: str, offset_deg: float) -> float: + cal = self.calibration[key] + min_deg = self._from_calibration_units(cal.range_min) + max_deg = self._from_calibration_units(cal.range_max) + home_deg = self._from_calibration_units(cal.homing_offset) + centered = -offset_deg if cal.drive_mode else offset_deg + target = home_deg + centered + return min(max_deg, max(min_deg, target)) + + def _record_ranges_of_motion(self) -> tuple[dict[str, float], dict[str, float]]: + current = self._read_raw_action() + mins = current.copy() + maxes = current.copy() + + while True: + current = self._read_raw_action() + mins = {key: min(mins[key], current[key]) for key in PIPER_CALIB_KEYS} + maxes = {key: max(maxes[key], current[key]) for key in PIPER_CALIB_KEYS} + + print("\n-----------------------------") + print("JOINT | MIN | POS | MAX") + for key in PIPER_CALIB_KEYS: + print(f"{key:<11} | {mins[key]:>6.2f} | {current[key]:>6.2f} | {maxes[key]:>6.2f}") + + if enter_pressed(): + break + move_cursor_up(len(PIPER_CALIB_KEYS) + 3) + + return mins, maxes + + @check_if_not_connected + def get_action(self) -> RobotAction: + if not self.is_calibrated and not self._use_uncalibrated_passthrough(): + raise RuntimeError( + f"{self} is not calibrated. Run `lerobot-calibrate --teleop.type={self.config.type} --teleop.id={self.id}` first." + ) + + raw_action = self._read_raw_action() + if self._use_uncalibrated_passthrough(): + action: RobotAction = dict(raw_action) + if not self.config.sync_gripper: + action["gripper.pos"] = 0.0 + return action + + action: RobotAction = { + key: self._calibrated_to_offset(key, raw_action[key]) for key in PIPER_CALIB_KEYS + } + if not self.config.sync_gripper: + action["gripper.pos"] = 0.0 + return action + + @check_if_not_connected + def send_feedback(self, feedback: dict[str, Any]) -> None: + if not self.is_calibrated and not self._use_uncalibrated_passthrough(): + raise RuntimeError( + f"{self} is not calibrated. Run `lerobot-calibrate --teleop.type={self.config.type} --teleop.id={self.id}` first." + ) + + self.set_manual_control(False) + self._refresh_command_mode_if_needed() + + joint_keys = PIPER_JOINT_ACTION_KEYS + has_all_joints = all(key in feedback for key in joint_keys) + if has_all_joints: + if self._use_uncalibrated_passthrough(): + joint_targets = [feedback[key] for key in joint_keys] + else: + joint_targets = [self._offset_to_calibrated(key, feedback[key]) for key in joint_keys] + joint_commands = [unit_to_milli(value) for value in joint_targets] + self.arm.JointCtrl(*joint_commands) + + if self.config.sync_gripper and "gripper.pos" in feedback: + if self._use_uncalibrated_passthrough(): + gripper_target = feedback["gripper.pos"] + else: + gripper_target = self._offset_to_calibrated("gripper.pos", feedback["gripper.pos"]) + gripper_pos_raw = unit_to_milli(gripper_target) + self._send_gripper_ctrl(gripper_pos_raw, enabled=True) + + @check_if_not_connected + def disconnect(self) -> None: + try: + self._stop_gravity_comp_loop_if_needed() + if self.config.disable_on_disconnect: + self.arm.DisableArm(7) + finally: + self.arm.DisconnectPort() + self._is_connected = False + self._manual_control_enabled = None + logger.info("%s disconnected.", self) + + +class PiperXLeader(PiperLeader): + config_class = PiperXLeaderConfig + name = "piperx_leader" + gravity_comp_urdf_relpath = DEFAULT_PIPERX_GRAVITY_URDF diff --git a/src/lerobot/teleoperators/utils.py b/src/lerobot/teleoperators/utils.py index 16454d5..2f47181 100644 --- a/src/lerobot/teleoperators/utils.py +++ b/src/lerobot/teleoperators/utils.py @@ -91,6 +91,18 @@ def make_teleoperator_from_config(config: TeleoperatorConfig) -> "Teleoperator": from .openarm_leader import OpenArmLeader return OpenArmLeader(config) + elif config.type == "piper_leader": + from .piper_leader import PiperLeader + + return PiperLeader(config) + elif config.type == "piperx_leader": + from .piper_leader import PiperXLeader + + return PiperXLeader(config) + elif config.type in {"bi_piper_leader", "bi_piperx_leader"}: + from .bi_piper_leader import BiPiperLeader, BiPiperXLeader + + return BiPiperXLeader(config) if config.type == "bi_piperx_leader" else BiPiperLeader(config) elif config.type == "bi_openarm_leader": from .bi_openarm_leader import BiOpenArmLeader diff --git a/src/lerobot/utils/control_utils.py b/src/lerobot/utils/control_utils.py index c2be198..8bb7517 100644 --- a/src/lerobot/utils/control_utils.py +++ b/src/lerobot/utils/control_utils.py @@ -262,3 +262,28 @@ def sanity_check_dataset_robot_compatibility( raise ValueError( "Dataset metadata compatibility check failed with mismatches:\n" + "\n".join(mismatches) ) + + +def sanity_check_bimanual_piper_pair(robot_cfg, teleop_cfg) -> None: + """Ensure bimanual PiPER configs are not mixed between PiPER and PiPER-X variants.""" + if teleop_cfg is None: + return + + robot_type = getattr(robot_cfg, "type", None) + teleop_type = getattr(teleop_cfg, "type", None) + expected_teleop_by_robot = { + "bi_piper_follower": "bi_piper_leader", + "bi_piperx_follower": "bi_piperx_leader", + } + expected_robot_by_teleop = {teleop: robot for robot, teleop in expected_teleop_by_robot.items()} + + if robot_type in expected_teleop_by_robot and teleop_type != expected_teleop_by_robot[robot_type]: + expected = expected_teleop_by_robot[robot_type] + raise ValueError( + f"In bimanual PiPER mode, '{robot_type}' must be paired with '{expected}', got '{teleop_type}'." + ) + if teleop_type in expected_robot_by_teleop and robot_type != expected_robot_by_teleop[teleop_type]: + expected = expected_robot_by_teleop[teleop_type] + raise ValueError( + f"In bimanual PiPER mode, '{teleop_type}' must be paired with '{expected}', got '{robot_type}'." + ) diff --git a/src/lerobot/utils/piper_sdk.py b/src/lerobot/utils/piper_sdk.py new file mode 100644 index 0000000..be1634d --- /dev/null +++ b/src/lerobot/utils/piper_sdk.py @@ -0,0 +1,129 @@ +#!/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 time +from functools import lru_cache +from typing import Any + +PIPER_JOINT_NAMES = ( + "joint_1", + "joint_2", + "joint_3", + "joint_4", + "joint_5", + "joint_6", +) +PIPER_JOINT_ACTION_KEYS = tuple(f"{joint}.pos" for joint in PIPER_JOINT_NAMES) +PIPER_ACTION_KEYS = PIPER_JOINT_ACTION_KEYS + ("gripper.pos",) +PIPER_CTRL_MODE_TEACH = 0x02 +PIPER_CTRL_MODE_LINKAGE_TEACH_INPUT = 0x06 + + +def milli_to_unit(value: float | int) -> float: + return float(value) * 1e-3 + + +def unit_to_milli(value: float | int) -> int: + return int(round(float(value) * 1e3)) + + +@lru_cache(maxsize=1) +def get_piper_sdk() -> tuple[type[Any], Any]: + try: + from piper_sdk import C_PiperInterface_V2, LogLevel + + return C_PiperInterface_V2, LogLevel + except ModuleNotFoundError as exc: + raise ModuleNotFoundError( + "Could not import `piper_sdk`. Install Evo-RL dependencies first (for example: `pip install -e .`)." + ) from exc + + +def parse_piper_log_level(level_name: str) -> Any: + _, log_level_enum = get_piper_sdk() + normalized = level_name.upper() + try: + return getattr(log_level_enum, normalized) + except AttributeError as exc: + raise ValueError( + f"Invalid Piper log level '{level_name}'. " + "Expected one of: DEBUG, INFO, WARNING, ERROR, CRITICAL, SILENT." + ) from exc + + +def wait_enable_piper(arm: Any, timeout_s: float, retry_interval_s: float = 0.2) -> bool: + deadline = time.monotonic() + max(0.0, timeout_s) + interval_s = max(0.01, retry_interval_s) + while time.monotonic() < deadline: + if bool(arm.EnablePiper()): + return True + remaining_s = deadline - time.monotonic() + if remaining_s <= 0: + break + time.sleep(min(interval_s, remaining_s)) + return False + + +def _piper_mode_to_int(value: Any) -> int | None: + if isinstance(value, bool): + return int(value) + if isinstance(value, int): + return int(value) + if hasattr(value, "value") and isinstance(value.value, int): + return int(value.value) + if hasattr(value, "__int__"): + return int(value) + return None + + +def read_piper_ctrl_mode(arm: Any, timeout_s: float = 1.0, poll_s: float = 0.02) -> int | None: + deadline = time.monotonic() + max(0.0, timeout_s) + while time.monotonic() < deadline: + status_msg = arm.GetArmStatus() + if getattr(status_msg, "time_stamp", 0.0) > 0.0: + arm_status = getattr(status_msg, "arm_status", None) + if arm_status is not None: + mode = _piper_mode_to_int(getattr(arm_status, "ctrl_mode", None)) + if mode is not None: + return mode + time.sleep(max(0.005, poll_s)) + return None + + +def guard_piper_ctrl_mode_on_connect( + arm: Any, + *, + interface_name: str, + timeout_s: float = 0.5, + poll_s: float = 0.02, + settle_s: float = 0.05, +) -> None: + mode = read_piper_ctrl_mode(arm, timeout_s=timeout_s, poll_s=poll_s) + if mode is None: + raise RuntimeError( + f"[{interface_name}] could not read arm ctrl_mode within {timeout_s:.2f}s. " + "Check CAN wiring/power and rerun." + ) + if mode in {PIPER_CTRL_MODE_TEACH, PIPER_CTRL_MODE_LINKAGE_TEACH_INPUT}: + arm.MasterSlaveConfig(0xFC, 0x00, 0x00, 0x00) + if settle_s > 0: + time.sleep(settle_s) + raise RuntimeError( + f"[{interface_name}] arm is in master/teaching role (ctrl_mode=0x{mode:02X}). " + "Follower role command (0xFC) has been sent. Power-cycle this arm, then retry." + ) diff --git a/tests/policies/pi0_fast/test_pi0_fast_original_vs_lerobot.py b/tests/policies/pi0_fast/test_pi0_fast_original_vs_lerobot.py index 9ebc4ba..c485e2b 100644 --- a/tests/policies/pi0_fast/test_pi0_fast_original_vs_lerobot.py +++ b/tests/policies/pi0_fast/test_pi0_fast_original_vs_lerobot.py @@ -29,8 +29,10 @@ import torch pytest.importorskip("transformers") pytest.importorskip("scipy") pytestmark = pytest.mark.skipif( - os.environ.get("CI") == "true" or os.environ.get("GITHUB_ACTIONS") == "true", - reason="This test requires accepting the model license", + os.environ.get("CI") == "true" + or os.environ.get("GITHUB_ACTIONS") == "true" + or not (os.environ.get("HF_TOKEN") or os.environ.get("HUGGING_FACE_HUB_TOKEN")), + reason="This test requires accepted model license and authenticated HF access token", ) from lerobot.policies.pi0_fast.configuration_pi0_fast import PI0FastConfig diff --git a/tests/policies/pi0_pi05/test_pi0_rtc.py b/tests/policies/pi0_pi05/test_pi0_rtc.py index 68e94dd..9f2ee16 100644 --- a/tests/policies/pi0_pi05/test_pi0_rtc.py +++ b/tests/policies/pi0_pi05/test_pi0_rtc.py @@ -23,8 +23,10 @@ import torch # Skip this entire module in CI pytestmark = pytest.mark.skipif( - os.environ.get("CI") == "true" or os.environ.get("GITHUB_ACTIONS") == "true", - reason="This test requires local OpenPI installation and is not meant for CI", + os.environ.get("CI") == "true" + or os.environ.get("GITHUB_ACTIONS") == "true" + or not (os.environ.get("HF_TOKEN") or os.environ.get("HUGGING_FACE_HUB_TOKEN")), + reason="This test requires local OpenPI setup and authenticated HF access token", ) from lerobot.configs.types import FeatureType, PolicyFeature, RTCAttentionSchedule # noqa: E402 diff --git a/tests/scripts/test_bi_piper_config_parsing.py b/tests/scripts/test_bi_piper_config_parsing.py new file mode 100644 index 0000000..ff04269 --- /dev/null +++ b/tests/scripts/test_bi_piper_config_parsing.py @@ -0,0 +1,78 @@ +import draccus +import pytest +from draccus.utils import ParsingError + +from lerobot.scripts.lerobot_record import RecordConfig +from lerobot.scripts.lerobot_teleoperate import TeleoperateConfig + + +def test_teleoperate_parses_bi_piperx_types(): + args = [ + "--robot.type=bi_piperx_follower", + "--robot.id=my_bi_piperx_follower", + "--robot.left_arm_config.port=can0", + "--robot.right_arm_config.port=can2", + "--robot.left_arm_config.require_calibration=false", + "--robot.right_arm_config.require_calibration=false", + "--teleop.type=bi_piperx_leader", + "--teleop.id=my_bi_piperx_leader", + "--teleop.left_arm_config.port=can1", + "--teleop.right_arm_config.port=can3", + "--teleop.left_arm_config.require_calibration=false", + "--teleop.right_arm_config.require_calibration=false", + ] + cfg = draccus.parse(config_class=TeleoperateConfig, config_path=None, args=args) + assert cfg.robot.type == "bi_piperx_follower" + assert cfg.teleop.type == "bi_piperx_leader" + + +def test_record_rejects_mixed_bi_piper_pair(): + args = [ + "--robot.type=bi_piper_follower", + "--robot.id=my_bi_piper_follower", + "--robot.left_arm_config.port=can0", + "--robot.right_arm_config.port=can2", + "--robot.left_arm_config.require_calibration=false", + "--robot.right_arm_config.require_calibration=false", + "--teleop.type=bi_piperx_leader", + "--teleop.id=my_bi_piperx_leader", + "--teleop.left_arm_config.port=can1", + "--teleop.right_arm_config.port=can3", + "--teleop.left_arm_config.require_calibration=false", + "--teleop.right_arm_config.require_calibration=false", + "--dataset.repo_id=dummy/dummy", + "--dataset.single_task=test", + "--dataset.num_episodes=1", + "--dataset.episode_time_s=1", + "--dataset.reset_time_s=1", + "--dataset.push_to_hub=false", + ] + with pytest.raises(ParsingError) as exc_info: + draccus.parse(config_class=RecordConfig, config_path=None, args=args) + assert "Couldn't instantiate class RecordConfig" in str(exc_info.value) + assert exc_info.value.__cause__ is not None + assert "must be paired" in str(exc_info.value.__cause__) + + +def test_record_rejects_bimanual_piper_teleop_with_non_bimanual_robot(): + args = [ + "--robot.type=so101_follower", + "--robot.port=/dev/mock", + "--teleop.type=bi_piperx_leader", + "--teleop.id=my_bi_piperx_leader", + "--teleop.left_arm_config.port=can1", + "--teleop.right_arm_config.port=can3", + "--teleop.left_arm_config.require_calibration=false", + "--teleop.right_arm_config.require_calibration=false", + "--dataset.repo_id=dummy/dummy", + "--dataset.single_task=test", + "--dataset.num_episodes=1", + "--dataset.episode_time_s=1", + "--dataset.reset_time_s=1", + "--dataset.push_to_hub=false", + ] + with pytest.raises(ParsingError) as exc_info: + draccus.parse(config_class=RecordConfig, config_path=None, args=args) + assert "Couldn't instantiate class RecordConfig" in str(exc_info.value) + assert exc_info.value.__cause__ is not None + assert "must be paired" in str(exc_info.value.__cause__) diff --git a/tests/test_piper_teleop.py b/tests/test_piper_teleop.py new file mode 100644 index 0000000..effb41c --- /dev/null +++ b/tests/test_piper_teleop.py @@ -0,0 +1,601 @@ +# ruff: noqa: N802 +import sys +import time +from types import SimpleNamespace + +import pytest + +import lerobot.robots.piper_follower.piper_follower as piper_follower_module +import lerobot.teleoperators.piper_leader.piper_leader as piper_leader_module +import lerobot.utils.piper_sdk as piper_sdk_utils +from lerobot.motors import MotorCalibration +from lerobot.robots.bi_piper_follower import ( + BiPiperFollower, + BiPiperFollowerConfig, + BiPiperXFollower, + BiPiperXFollowerConfig, +) +from lerobot.robots.piper_follower import ( + PiperFollower, + PiperFollowerConfig, + PiperFollowerConfigBase, + PiperXFollower, + PiperXFollowerConfig, +) +from lerobot.robots.utils import make_robot_from_config +from lerobot.teleoperators.bi_piper_leader import ( + BiPiperLeader, + BiPiperLeaderConfig, + BiPiperXLeader, + BiPiperXLeaderConfig, +) +from lerobot.teleoperators.piper_leader import ( + PiperLeader, + PiperLeaderConfig, + PiperLeaderConfigBase, + PiperXLeader, + PiperXLeaderConfig, +) +from lerobot.teleoperators.utils import make_teleoperator_from_config +from lerobot.utils.piper_sdk import PIPER_ACTION_KEYS + + +class FakeLogLevel: + DEBUG = "DEBUG" + INFO = "INFO" + WARNING = "WARNING" + ERROR = "ERROR" + CRITICAL = "CRITICAL" + SILENT = "SILENT" + + +class FakePiperInterface: + def __init__(self, can_name, judge_flag=False, can_auto_init=True, logger_level=None): + self.can_name = can_name + self.judge_flag = judge_flag + self.can_auto_init = can_auto_init + self.logger_level = logger_level + self.connected = False + self.mode_commands = [] + self.role_commands = [] + self.last_joint = None + self.last_gripper = None + self.gripper_calls = [] + self.joint_mit_calls = [] + self.enable_calls = 0 + self.disable_calls = 0 + self.is_enabled = False + + self._joint_ctrl = SimpleNamespace( + time_stamp=1.0, + Hz=120.0, + joint_ctrl=SimpleNamespace( + joint_1=10000, + joint_2=20000, + joint_3=30000, + joint_4=40000, + joint_5=50000, + joint_6=60000, + ), + ) + self._joint_state = SimpleNamespace( + time_stamp=1.0, + Hz=120.0, + joint_state=SimpleNamespace( + joint_1=11000, + joint_2=21000, + joint_3=31000, + joint_4=41000, + joint_5=51000, + joint_6=61000, + ), + ) + self._gripper_ctrl = SimpleNamespace( + time_stamp=1.0, + Hz=120.0, + gripper_ctrl=SimpleNamespace(grippers_angle=42000, grippers_effort=1500, status_code=0x01), + ) + self._gripper_state = SimpleNamespace( + time_stamp=1.0, + Hz=120.0, + gripper_state=SimpleNamespace(grippers_angle=43000, grippers_effort=1400, status_code=0x01), + ) + self._high_spd = SimpleNamespace( + time_stamp=1.0, + Hz=120.0, + motor_1=SimpleNamespace(motor_speed=0), + motor_2=SimpleNamespace(motor_speed=0), + motor_3=SimpleNamespace(motor_speed=0), + motor_4=SimpleNamespace(motor_speed=0), + motor_5=SimpleNamespace(motor_speed=0), + motor_6=SimpleNamespace(motor_speed=0), + ) + self._arm_status = SimpleNamespace( + time_stamp=1.0, + Hz=120.0, + arm_status=SimpleNamespace(ctrl_mode=0x01), + ) + + def ConnectPort(self): + self.connected = True + + def DisconnectPort(self, thread_timeout=0.1): + del thread_timeout + self.connected = False + + def MotionCtrl_2(self, *args): + self.mode_commands.append(args) + + def MasterSlaveConfig(self, *args): + self.role_commands.append(args) + if args and args[0] == 0xFC: + self._arm_status.arm_status.ctrl_mode = 0x01 + + def EnablePiper(self): + self.enable_calls += 1 + self.is_enabled = True + return True + + def DisableArm(self, motor_num): + del motor_num + self.disable_calls += 1 + self.is_enabled = False + + def JointCtrl(self, *args): + self.last_joint = args + + def JointMitCtrl(self, *args): + self.joint_mit_calls.append(args) + + def GripperCtrl(self, *args): + self.last_gripper = args + self.gripper_calls.append(args) + + def GetArmJointCtrl(self): + return self._joint_ctrl + + def GetArmJointMsgs(self): + return self._joint_state + + def GetArmGripperCtrl(self): + return self._gripper_ctrl + + def GetArmGripperMsgs(self): + return self._gripper_state + + def GetArmHighSpdInfoMsgs(self): + return self._high_spd + + def GetArmStatus(self): + return self._arm_status + + +def patch_fake_sdk(monkeypatch): + def fake_loader(): + return (FakePiperInterface, FakeLogLevel) + + monkeypatch.setattr(piper_sdk_utils, "get_piper_sdk", fake_loader) + monkeypatch.setattr(piper_follower_module, "get_piper_sdk", fake_loader) + monkeypatch.setattr(piper_leader_module, "get_piper_sdk", fake_loader) + + +def make_identity_calibration(): + return { + key: MotorCalibration( + id=idx, + drive_mode=0, + homing_offset=0, + range_min=-200000, + range_max=200000, + ) + for idx, key in enumerate(PIPER_ACTION_KEYS) + } + + +def wait_until(predicate, timeout_s: float = 0.2, poll_s: float = 0.005) -> bool: + deadline = time.monotonic() + timeout_s + while time.monotonic() < deadline: + if predicate(): + return True + time.sleep(poll_s) + return False + + +@pytest.mark.parametrize( + ("teleop_cfg", "robot_cfg", "teleop_cls", "robot_cls"), + [ + ( + PiperLeaderConfig(port="can1", manual_control=False, sync_gripper=True), + PiperFollowerConfig(port="can0", sync_gripper=True), + PiperLeader, + PiperFollower, + ), + ( + PiperXLeaderConfig(port="can1", manual_control=False, sync_gripper=True), + PiperXFollowerConfig(port="can0", sync_gripper=True), + PiperXLeader, + PiperXFollower, + ), + ], +) +def test_piper_leader_follower_teleop_roundtrip(monkeypatch, teleop_cfg, robot_cfg, teleop_cls, robot_cls): + patch_fake_sdk(monkeypatch) + + teleop = make_teleoperator_from_config(teleop_cfg) + robot = make_robot_from_config(robot_cfg) + + assert isinstance(teleop, teleop_cls) + assert isinstance(robot, robot_cls) + + teleop.calibration = make_identity_calibration() + robot.calibration = make_identity_calibration() + + teleop.connect(calibrate=False) + robot.connect(calibrate=False) + try: + action = teleop.get_action() + sent = robot.send_action(action) + obs = robot.get_observation() + + assert robot.arm.last_joint == (10000, 20000, 30000, 40000, 50000, 60000) + assert robot.arm.last_gripper == ( + 42000, + robot_cfg.gripper_effort_default, + robot_cfg.gripper_status_code, + 0x00, + ) + assert sent["joint_1.pos"] == 10.0 + assert sent["gripper.pos"] == 42.0 + assert obs["joint_1.pos"] == 11.0 + assert obs["gripper.pos"] == 43.0 + + teleop.send_feedback(action) + assert teleop.arm.last_joint == (10000, 20000, 30000, 40000, 50000, 60000) + assert teleop.arm.last_gripper == ( + 42000, + teleop_cfg.gripper_effort_default, + teleop_cfg.gripper_status_code, + 0x00, + ) + finally: + teleop.disconnect() + robot.disconnect() + + +def test_piper_requires_calibration(monkeypatch): + patch_fake_sdk(monkeypatch) + + teleop = PiperLeader(PiperLeaderConfig(port="can1")) + robot = PiperFollower(PiperFollowerConfig(port="can0")) + + assert not teleop.is_calibrated + assert not robot.is_calibrated + + teleop.connect(calibrate=False) + robot.connect(calibrate=False) + try: + with pytest.raises(RuntimeError, match="is not calibrated"): + teleop.get_action() + with pytest.raises(RuntimeError, match="is not calibrated"): + robot.send_action({"joint_1.pos": 0.0}) + finally: + teleop.disconnect() + robot.disconnect() + + +def test_piper_leader_reconnect_reapplies_mode(monkeypatch): + patch_fake_sdk(monkeypatch) + + teleop = PiperLeader(PiperLeaderConfig(port="can1", manual_control=False)) + teleop.calibration = make_identity_calibration() + + teleop.connect(calibrate=False) + teleop.disconnect() + teleop.connect(calibrate=False) + try: + assert teleop.arm.enable_calls == 2 + finally: + teleop.disconnect() + + +def test_piper_follower_connect_rolls_back_connected_cameras(monkeypatch): + patch_fake_sdk(monkeypatch) + + class FakeCamera: + def __init__(self, should_fail: bool): + self.should_fail = should_fail + self.is_connected = False + self.disconnect_calls = 0 + + def connect(self): + if self.should_fail: + raise RuntimeError("camera connect failed") + self.is_connected = True + + def disconnect(self): + self.disconnect_calls += 1 + self.is_connected = False + + def async_read(self): + return None + + cam_ok = FakeCamera(should_fail=False) + cam_fail = FakeCamera(should_fail=True) + monkeypatch.setattr( + piper_follower_module, + "make_cameras_from_configs", + lambda _: {"cam_ok": cam_ok, "cam_fail": cam_fail}, + ) + + robot = PiperFollower(PiperFollowerConfig(port="can0")) + robot.calibration = make_identity_calibration() + + with pytest.raises(RuntimeError, match="camera connect failed"): + robot.connect(calibrate=False) + + assert cam_ok.disconnect_calls == 1 + assert not cam_ok.is_connected + assert not robot.is_connected + + +def test_piper_require_calibration_false_allows_uncalibrated_control(monkeypatch): + patch_fake_sdk(monkeypatch) + + teleop = PiperLeader(PiperLeaderConfig(port="can1", require_calibration=False)) + robot = PiperFollower(PiperFollowerConfig(port="can0", require_calibration=False)) + + teleop.connect(calibrate=False) + robot.connect(calibrate=False) + try: + action = teleop.get_action() + sent = robot.send_action(action) + assert sent["joint_1.pos"] == 10.0 + assert sent["gripper.pos"] == 42.0 + finally: + teleop.disconnect() + robot.disconnect() + + +def test_piper_follower_connect_calibrates_then_reenables(monkeypatch, tmp_path): + patch_fake_sdk(monkeypatch) + + robot = PiperFollower( + PiperFollowerConfig( + port="can0", + id="connect_reenable_after_calibration", + calibration_dir=tmp_path, + require_calibration=True, + enable_on_connect=True, + ) + ) + + def fake_calibrate(): + # Mirror real calibration's drag-mode behavior. + robot.arm.DisableArm(7) + robot.calibration = make_identity_calibration() + + monkeypatch.setattr(robot, "calibrate", fake_calibrate) + + robot.connect(calibrate=True) + try: + assert robot.arm.disable_calls == 1 + assert robot.arm.enable_calls == 1 + assert robot.arm.is_enabled + finally: + robot.disconnect() + + +def test_piper_follower_connect_without_calibration_still_enables(monkeypatch, tmp_path): + patch_fake_sdk(monkeypatch) + + robot = PiperFollower( + PiperFollowerConfig( + port="can0", + id="connect_enable_without_calibration", + calibration_dir=tmp_path, + require_calibration=True, + enable_on_connect=True, + ) + ) + + robot.connect(calibrate=False) + try: + assert robot.arm.enable_calls == 1 + assert robot.arm.is_enabled + assert robot.arm.disable_calls == 0 + finally: + robot.disconnect() + + +def test_piper_leader_gravity_comp_manual_control_uses_mit(monkeypatch): + patch_fake_sdk(monkeypatch) + + class FakeModel: + def __init__(self): + self.nq = 6 + self.nv = 6 + self.gravity = SimpleNamespace(linear=None) + + def createData(self): + return object() + + class FakeRobotWrapper: + @staticmethod + def BuildFromURDF(urdf_path, package_dirs): + del urdf_path, package_dirs + return SimpleNamespace(model=FakeModel(), data=object()) + + class FakePinocchio: + RobotWrapper = FakeRobotWrapper + + @staticmethod + def rnea(model, data, q, v, a): + del model, data, q, v, a + return [0.1] * 6 + + monkeypatch.setitem(sys.modules, "pinocchio", FakePinocchio) + + teleop = PiperLeader( + PiperLeaderConfig( + port="can1", + manual_control=True, + gravity_comp_control_hz=200.0, + mode_refresh_interval_s=0.01, + ) + ) + teleop.calibration = make_identity_calibration() + + teleop.connect(calibrate=False) + try: + assert wait_until(lambda: len(teleop.arm.joint_mit_calls) > 0) + assert teleop.arm.disable_calls == 0 + assert len(teleop.arm.joint_mit_calls) > 0 + assert len(teleop.arm.gripper_calls) > 0 + assert teleop.arm.gripper_calls[0][2] == 0x00 + + teleop.set_manual_control(False) + calls_after_stop = len(teleop.arm.joint_mit_calls) + time.sleep(0.03) + assert len(teleop.arm.joint_mit_calls) == calls_after_stop + assert teleop.arm.gripper_calls[-1][2] == 0x01 + finally: + teleop.disconnect() + + +@pytest.mark.parametrize( + ("device_kind", "port", "robot_id"), + [("leader", "can1", None), ("follower", "can0", "piper_role_guard")], +) +def test_piper_connect_fails_and_writes_follower_role_when_in_teach_mode( + monkeypatch, device_kind, port, robot_id +): + patch_fake_sdk(monkeypatch) + + if device_kind == "leader": + device = PiperLeader(PiperLeaderConfig(port=port)) + else: + device = PiperFollower(PiperFollowerConfig(port=port, id=robot_id)) + device.arm._arm_status.arm_status.ctrl_mode = 0x06 + + with pytest.raises(RuntimeError, match="Follower role command .* sent.*Power-cycle"): + device.connect(calibrate=False) + + assert device.arm.role_commands[-1] == (0xFC, 0x00, 0x00, 0x00) + + +def test_piper_lfs_pointer_urdf_raises_actionable_error(tmp_path): + pointer_file = tmp_path / "lfs_pointer.urdf" + pointer_file.write_text("version https://git-lfs.github.com/spec/v1\noid sha256:deadbeef\nsize 123\n") + + with pytest.raises(RuntimeError, match="Git LFS pointer files"): + piper_leader_module._ensure_not_lfs_pointer( + pointer_file, "assets/piper_description/urdf/pointer.urdf" + ) + + +@pytest.mark.parametrize( + ( + "teleop_cfg", + "robot_cfg", + "bi_teleop_cls", + "bi_robot_cls", + "left_teleop_cls", + "right_teleop_cls", + "left_robot_cls", + "right_robot_cls", + ), + [ + ( + BiPiperLeaderConfig( + left_arm_config=PiperLeaderConfigBase(port="can1", manual_control=False, sync_gripper=True), + right_arm_config=PiperLeaderConfigBase(port="can3", manual_control=False, sync_gripper=True), + ), + BiPiperFollowerConfig( + left_arm_config=PiperFollowerConfigBase(port="can0", sync_gripper=True), + right_arm_config=PiperFollowerConfigBase(port="can2", sync_gripper=True), + ), + BiPiperLeader, + BiPiperFollower, + PiperLeader, + PiperLeader, + PiperFollower, + PiperFollower, + ), + ( + BiPiperXLeaderConfig( + left_arm_config=PiperLeaderConfigBase(port="can1", manual_control=False, sync_gripper=True), + right_arm_config=PiperLeaderConfigBase(port="can3", manual_control=False, sync_gripper=True), + ), + BiPiperXFollowerConfig( + left_arm_config=PiperFollowerConfigBase(port="can0", sync_gripper=True), + right_arm_config=PiperFollowerConfigBase(port="can2", sync_gripper=True), + ), + BiPiperXLeader, + BiPiperXFollower, + PiperXLeader, + PiperXLeader, + PiperXFollower, + PiperXFollower, + ), + ], +) +def test_bimanual_piper_leader_follower_roundtrip( + monkeypatch, + teleop_cfg, + robot_cfg, + bi_teleop_cls, + bi_robot_cls, + left_teleop_cls, + right_teleop_cls, + left_robot_cls, + right_robot_cls, +): + patch_fake_sdk(monkeypatch) + + teleop = make_teleoperator_from_config(teleop_cfg) + robot = make_robot_from_config(robot_cfg) + + assert isinstance(teleop, bi_teleop_cls) + assert isinstance(robot, bi_robot_cls) + assert isinstance(teleop.left_arm, left_teleop_cls) + assert isinstance(teleop.right_arm, right_teleop_cls) + assert isinstance(robot.left_arm, left_robot_cls) + assert isinstance(robot.right_arm, right_robot_cls) + + teleop.left_arm.calibration = make_identity_calibration() + teleop.right_arm.calibration = make_identity_calibration() + robot.left_arm.calibration = make_identity_calibration() + robot.right_arm.calibration = make_identity_calibration() + + teleop.connect(calibrate=False) + robot.connect(calibrate=False) + try: + action = teleop.get_action() + assert "left_joint_1.pos" in action + assert "right_joint_1.pos" in action + + sent = robot.send_action(action) + obs = robot.get_observation() + + assert robot.left_arm.arm.last_joint == (10000, 20000, 30000, 40000, 50000, 60000) + assert robot.right_arm.arm.last_joint == (10000, 20000, 30000, 40000, 50000, 60000) + assert robot.left_arm.arm.last_gripper[0] == 42000 + assert robot.right_arm.arm.last_gripper[0] == 42000 + + assert sent["left_joint_1.pos"] == 10.0 + assert sent["right_joint_1.pos"] == 10.0 + assert sent["left_gripper.pos"] == 42.0 + assert sent["right_gripper.pos"] == 42.0 + assert obs["left_joint_1.pos"] == 11.0 + assert obs["right_joint_1.pos"] == 11.0 + assert obs["left_gripper.pos"] == 43.0 + assert obs["right_gripper.pos"] == 43.0 + + teleop.send_feedback(action) + assert teleop.left_arm.arm.last_joint == (10000, 20000, 30000, 40000, 50000, 60000) + assert teleop.right_arm.arm.last_joint == (10000, 20000, 30000, 40000, 50000, 60000) + assert teleop.left_arm.arm.last_gripper[0] == 42000 + assert teleop.right_arm.arm.last_gripper[0] == 42000 + finally: + teleop.disconnect() + robot.disconnect() diff --git a/tests/utils/test_control_utils.py b/tests/utils/test_control_utils.py new file mode 100644 index 0000000..bbf3b14 --- /dev/null +++ b/tests/utils/test_control_utils.py @@ -0,0 +1,41 @@ +from types import SimpleNamespace + +import pytest + +from lerobot.utils.control_utils import sanity_check_bimanual_piper_pair + + +@pytest.mark.parametrize( + ("robot_type", "teleop_type"), + [ + ("bi_piper_follower", "bi_piper_leader"), + ("bi_piperx_follower", "bi_piperx_leader"), + ("so101_follower", "so101_leader"), + ], +) +def test_sanity_check_bimanual_piper_pair_accepts_valid_pairs(robot_type, teleop_type): + sanity_check_bimanual_piper_pair( + SimpleNamespace(type=robot_type), + SimpleNamespace(type=teleop_type), + ) + + +def test_sanity_check_bimanual_piper_pair_accepts_missing_teleop(): + sanity_check_bimanual_piper_pair(SimpleNamespace(type="bi_piperx_follower"), None) + + +@pytest.mark.parametrize( + ("robot_type", "teleop_type"), + [ + ("bi_piper_follower", "bi_piperx_leader"), + ("bi_piperx_follower", "bi_piper_leader"), + ("so101_follower", "bi_piperx_leader"), + ("so101_follower", "bi_piper_leader"), + ], +) +def test_sanity_check_bimanual_piper_pair_rejects_mixed_pairs(robot_type, teleop_type): + with pytest.raises(ValueError, match="must be paired"): + sanity_check_bimanual_piper_pair( + SimpleNamespace(type=robot_type), + SimpleNamespace(type=teleop_type), + ) diff --git a/tests/value/test_lerobot_value_infer_viz.py b/tests/value/test_lerobot_value_infer_viz.py index 6bf3bdc..bb4bb46 100644 --- a/tests/value/test_lerobot_value_infer_viz.py +++ b/tests/value/test_lerobot_value_infer_viz.py @@ -96,6 +96,7 @@ def test_export_overlay_videos_uses_episode_index_for_subset_alignment(monkeypat video_backend, frame_storage_mode, temp_dir_root, + smooth_window=1, ): assert isinstance(src, Path) assert isinstance(dst, Path) @@ -108,17 +109,19 @@ def test_export_overlay_videos_uses_episode_index_for_subset_alignment(monkeypat assert isinstance(ep_values, np.ndarray) assert isinstance(ep_advantages, np.ndarray) assert isinstance(ep_indicators, np.ndarray) + assert smooth_window == 1 return dst monkeypatch.setattr(value_infer_viz, "_export_single_episode", _fake_export_single_episode) - written = value_infer._export_overlay_videos( + written = value_infer_viz._export_overlay_videos( dataset=dataset, value_field="complementary_info.value", advantage_field="complementary_info.advantage", indicator_field="complementary_info.acp_indicator", viz_episodes="all", video_key=None, + video_keys=None, output_dir=tmp_path, overwrite=True, vcodec="libsvtav1", @@ -146,22 +149,25 @@ def test_export_overlay_videos_passes_disk_storage_mode(monkeypatch, tmp_path: P video_backend, frame_storage_mode, temp_dir_root, + smooth_window=1, ): assert isinstance(episode_timestamps_s, np.ndarray) assert tolerance_s > 0 assert frame_storage_mode == "disk" assert temp_dir_root == tmp_path + assert smooth_window == 1 return dst monkeypatch.setattr(value_infer_viz, "_export_single_episode", _fake_export_single_episode) - written = value_infer._export_overlay_videos( + written = value_infer_viz._export_overlay_videos( dataset=dataset, value_field="complementary_info.value", advantage_field="complementary_info.advantage", indicator_field="complementary_info.acp_indicator", viz_episodes="all", video_key=None, + video_keys=None, output_dir=tmp_path, overwrite=True, vcodec="h264", @@ -172,7 +178,7 @@ def test_export_overlay_videos_passes_disk_storage_mode(monkeypatch, tmp_path: P def test_get_video_encode_options_for_h264_nvenc(): - options, pix_fmt = value_infer._get_video_encode_options("h264_nvenc") + options, pix_fmt = value_infer_viz._get_video_encode_options("h264_nvenc") assert pix_fmt == "yuv420p" assert options["preset"] == "p4" assert options["rc"] == "vbr" @@ -183,7 +189,7 @@ def test_get_video_encode_options_for_h264_nvenc(): def test_get_episode_value_bounds_returns_episode_min_max(): - y_min, y_max = value_infer._get_episode_value_bounds(np.array([-0.7, -0.3, -0.5], dtype=np.float32)) + y_min, y_max = value_infer_viz._get_episode_value_bounds(np.array([-0.7, -0.3, -0.5], dtype=np.float32)) assert np.isclose(y_min, -0.7) assert np.isclose(y_max, -0.3)