镜像自地址
https://github.com/MINT-SJTU/Evo-RL.git
已同步 2026-11-20 18:17:06 +00:00
Merge develop into main (Piper integration + CI/test updates)
Merge develop into main (Piper integration + CI/test updates)
这个提交包含在:
2
.gitattributes
vendored
2
.gitattributes
vendored
@ -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
|
||||
|
||||
2
.github/workflows/deploy-website-pages.yml
vendored
2
.github/workflows/deploy-website-pages.yml
vendored
@ -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
|
||||
|
||||
2
.github/workflows/fast_tests.yml
vendored
2
.github/workflows/fast_tests.yml
vendored
@ -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:
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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 *
|
||||
|
||||
149
README.md
149
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) |
|
||||
|
||||
<p align="center"><strong>Value Visual Results</strong></p>
|
||||
|
||||
<p align="center"><small><strong>Success Case</strong></small></p>
|
||||
@ -57,10 +67,14 @@
|
||||
<img alt="Human-in-the-Loop Result 2" src="./website/assets/gifs/hitl_2.gif" width="48%"/>
|
||||
</p>
|
||||
|
||||
<a id="quick-start"></a>
|
||||
|
||||
## ⚡ 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.
|
||||
|
||||
<a id="installation"></a>
|
||||
|
||||
### 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).
|
||||
|
||||
<a id="hardware-setup"></a>
|
||||
|
||||
### 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.
|
||||
|
||||
<a id="agilex-piper-setup"></a>
|
||||
|
||||
#### 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=<LEFT_FOLLOWER_CAN_PORT>,<LEFT_LEADER_CAN_PORT>,<RIGHT_FOLLOWER_CAN_PORT>,<RIGHT_LEADER_CAN_PORT>
|
||||
```
|
||||
|
||||
For single-arm users, run the command below to verify the system is ready:
|
||||
|
||||
```bash
|
||||
lerobot-teleoperate \
|
||||
--robot.type=piperx_follower \
|
||||
--robot.port=<FOLLOWER_CAN_PORT> \
|
||||
--robot.id=my_piperx_follower \
|
||||
--robot.require_calibration=false \
|
||||
--teleop.type=piperx_leader \
|
||||
--teleop.port=<LEADER_CAN_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=<LEFT_FOLLOWER_CAN_PORT> \
|
||||
--robot.right_arm_config.port=<RIGHT_FOLLOWER_CAN_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=<LEFT_LEADER_CAN_PORT> \
|
||||
--teleop.right_arm_config.port=<RIGHT_LEADER_CAN_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`.
|
||||
|
||||
<a id="data-collection"></a>
|
||||
|
||||
### 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=<LEFT_FOLLOWER_CAN_PORT> \
|
||||
--robot.right_arm_config.port=<RIGHT_FOLLOWER_CAN_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=<LEFT_LEADER_CAN_PORT> \
|
||||
--teleop.right_arm_config.port=<RIGHT_LEADER_CAN_PORT> \
|
||||
--teleop.left_arm_config.require_calibration=false \
|
||||
--teleop.right_arm_config.require_calibration=false \
|
||||
--dataset.repo_id=<HF_USERNAME_OR_ORG>/<DATASET_NAME> \
|
||||
--dataset.single_task="<YOUR_TASK_DESCRIPTION>" \
|
||||
--dataset.num_episodes=<NUM_EPISODES> \
|
||||
--dataset.episode_time_s=<EPISODE_SECONDS> \
|
||||
--dataset.reset_time_s=<RESET_SECONDS> \
|
||||
--dataset.push_to_hub=true \
|
||||
--display_data=true
|
||||
```
|
||||
|
||||
Hotkeys:
|
||||
|
||||
- `i`: toggle intervention mode (policy <-> teleop takeover)
|
||||
@ -230,6 +332,8 @@ lerobot-dataset-report --dataset <HF_USERNAME_OR_ORG>/<DATASET_NAME>
|
||||
|
||||
This prints: dataset meta, totals, episode-length stats/histogram, success/intervention metrics, task list, and full feature schema.
|
||||
|
||||
<a id="value-function-training"></a>
|
||||
|
||||
### 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/<your_value>/processor_<your_value>.py` with `make_<your_value>_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`.
|
||||
|
||||
<a id="value-inference"></a>
|
||||
|
||||
### 5) Value Inference
|
||||
|
||||
Infer value signals and write value/advantage/indicator back to the dataset:
|
||||
@ -307,31 +413,22 @@ CUDA_VISIBLE_DEVICES=<GPU_ID_LIST> 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_<TAG>`
|
||||
- `complementary_info.advantage_<TAG>`
|
||||
- `complementary_info.acp_indicator_<TAG>`
|
||||
```bash
|
||||
complementary_info.value_<TAG>
|
||||
complementary_info.advantage_<TAG>
|
||||
complementary_info.acp_indicator_<TAG>
|
||||
```
|
||||
|
||||
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/<RUN_NAME>/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=<CAMERA_KEY>` (default picks `front` if available)
|
||||
- `--viz.overwrite=true|false`
|
||||
- `--viz.vcodec=<CODEC>` and `--viz.frame_storage_mode=memory|disk`
|
||||
|
||||
Value overlay examples (SO101, smooth=11):
|
||||
|
||||
- GIF assets are stored under `website/assets/gifs/`.
|
||||
<a id="policy-training"></a>
|
||||
|
||||
### 6) Policy Training
|
||||
|
||||
@ -377,6 +474,8 @@ CUDA_VISIBLE_DEVICES=<GPU_ID_LIST> accelerate launch \
|
||||
<POLICY_TRAIN_ARGS>
|
||||
```
|
||||
|
||||
<a id="closed-loop-rollout-and-next-round"></a>
|
||||
|
||||
### 7) Closed-loop Rollout and Next Round
|
||||
|
||||
Deploy the trained policy in human-in-loop mode and collect the next dataset round:
|
||||
|
||||
@ -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
|
||||
|
||||
@ -0,0 +1,3 @@
|
||||
version https://git-lfs.github.com/spec/v1
|
||||
oid sha256:fd4a3d6d0266205fe3f725f048645be66592c23564db22e08b0807d5569b2c64
|
||||
size 607384
|
||||
@ -0,0 +1,3 @@
|
||||
version https://git-lfs.github.com/spec/v1
|
||||
oid sha256:9ee6e1d67c0241f42a0dbaf5c676d19230375ed078ee1943bb8c4c37ab303d14
|
||||
size 447784
|
||||
@ -0,0 +1,3 @@
|
||||
version https://git-lfs.github.com/spec/v1
|
||||
oid sha256:0d37675b699b643f2e15348de99aed535fac10dc5f09a79b8e4b0acfaf4e0378
|
||||
size 3658384
|
||||
@ -0,0 +1,3 @@
|
||||
version https://git-lfs.github.com/spec/v1
|
||||
oid sha256:e8ae51f17c0c7f6d67b753922e2cc269f5e8372a65734f859231639a64fe8fad
|
||||
size 1845784
|
||||
@ -0,0 +1,3 @@
|
||||
version https://git-lfs.github.com/spec/v1
|
||||
oid sha256:efa33022976218f246f736a6133494b5d0e5486e8b50b52900c42f285ae85558
|
||||
size 877984
|
||||
@ -0,0 +1,3 @@
|
||||
version https://git-lfs.github.com/spec/v1
|
||||
oid sha256:a1c69d1c7f6f8c60e2c82331c0f444725200ed4053d3e5ec23aa82cb5db5b29f
|
||||
size 833684
|
||||
@ -0,0 +1,3 @@
|
||||
version https://git-lfs.github.com/spec/v1
|
||||
oid sha256:88374973bc3bd9e7389b5853cd6e555af9731c30b658b1ee6f85be34a126cb92
|
||||
size 58884
|
||||
@ -0,0 +1,3 @@
|
||||
version https://git-lfs.github.com/spec/v1
|
||||
oid sha256:a93c1e3fc245d9d336a427fcc506cb1fc15615268053392b249816f90c011e50
|
||||
size 8273
|
||||
@ -0,0 +1,3 @@
|
||||
version https://git-lfs.github.com/spec/v1
|
||||
oid sha256:0861e122e406f891a57a550551ad1ce0ef4ca5babf58f476cf8914395e144319
|
||||
size 162084
|
||||
@ -0,0 +1,3 @@
|
||||
version https://git-lfs.github.com/spec/v1
|
||||
oid sha256:e78dee99bc84857c09183463f3fb3fdea292ea7570f1bfb0001a79674bb69d45
|
||||
size 2323489
|
||||
@ -0,0 +1,3 @@
|
||||
version https://git-lfs.github.com/spec/v1
|
||||
oid sha256:97e9079580af6e423317c33b2a0f4fe0f6dbc67c3e1d9e6f4836c87c6c8f34a2
|
||||
size 14480060
|
||||
@ -0,0 +1,3 @@
|
||||
version https://git-lfs.github.com/spec/v1
|
||||
oid sha256:db5b39eef745655a67e57870d8ee56f26a03651d2161635ca2ec3c58c9269907
|
||||
size 5098355
|
||||
@ -0,0 +1,3 @@
|
||||
version https://git-lfs.github.com/spec/v1
|
||||
oid sha256:ccc08d3ef1cb8bdfb8ab5395ff65893d13386ad526d7bc954d468b7dc885d557
|
||||
size 2037373
|
||||
@ -0,0 +1,3 @@
|
||||
version https://git-lfs.github.com/spec/v1
|
||||
oid sha256:78718679ffdf9d81130047c4267ed66a081315eff36f216833bc3781c6241417
|
||||
size 2155609
|
||||
@ -0,0 +1,3 @@
|
||||
version https://git-lfs.github.com/spec/v1
|
||||
oid sha256:486dddcd07f072b2e3978087dc2f9b641a91722bccb1bcb3421c4c7ce64a39ed
|
||||
size 1001764
|
||||
@ -0,0 +1,3 @@
|
||||
version https://git-lfs.github.com/spec/v1
|
||||
oid sha256:d6dc6d31b0fa56341b2323fee5e8a375c2500ae6fb02e31fb52e8e917b608439
|
||||
size 8169
|
||||
@ -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",
|
||||
]
|
||||
|
||||
@ -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 (
|
||||
|
||||
@ -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
|
||||
@ -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"
|
||||
@ -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
|
||||
@ -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
|
||||
@ -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)
|
||||
@ -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"
|
||||
@ -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
|
||||
|
||||
|
||||
@ -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,
|
||||
)
|
||||
|
||||
@ -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.")
|
||||
|
||||
|
||||
@ -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,
|
||||
|
||||
@ -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":
|
||||
|
||||
@ -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",
|
||||
]
|
||||
|
||||
|
||||
@ -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)
|
||||
|
||||
@ -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
|
||||
@ -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"
|
||||
@ -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
|
||||
@ -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
|
||||
@ -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)
|
||||
@ -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)
|
||||
@ -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
|
||||
@ -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
|
||||
|
||||
|
||||
@ -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}'."
|
||||
)
|
||||
|
||||
129
src/lerobot/utils/piper_sdk.py
普通文件
129
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."
|
||||
)
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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__)
|
||||
601
tests/test_piper_teleop.py
普通文件
601
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()
|
||||
@ -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),
|
||||
)
|
||||
@ -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)
|
||||
|
||||
|
||||
在新工单中引用
屏蔽一个用户