镜像自地址
https://github.com/MINT-SJTU/Evo-RL.git
已同步 2026-11-20 18:17:06 +00:00
Merge branch 'kye/feature/piper_adapter' into develop
这个提交包含在:
126
README.md
126
README.md
@ -29,6 +29,15 @@
|
||||
|
||||
- **[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 +66,12 @@
|
||||
<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 +84,13 @@ 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 +197,55 @@ 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
|
||||
|
||||
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=can0,can1
|
||||
```
|
||||
|
||||
PiPER calibration is required by default.
|
||||
If needed, you can disable it with
|
||||
`--robot.calibration_mode=off` and `--teleop.calibration_mode=off`.
|
||||
|
||||
If you switch master/follower roles with the flags below, you must power-cycle both arms before enabling.
|
||||
|
||||
```bash
|
||||
--teleop.set_leader_mode_on_connect=true
|
||||
--robot.set_follower_mode_on_connect=true
|
||||
```
|
||||
|
||||
For single-arm users, run the command below to verify the system is ready:
|
||||
|
||||
```bash
|
||||
lerobot-teleoperate \
|
||||
--robot.type=piper_follower \
|
||||
--robot.port=can0 \
|
||||
--robot.id=my_piper_follower \
|
||||
--teleop.type=piper_leader \
|
||||
--teleop.port=can1 \
|
||||
--teleop.id=my_piper_leader
|
||||
```
|
||||
|
||||
Optional PiPER flags:
|
||||
|
||||
```bash
|
||||
--robot.sync_gripper=true|false
|
||||
--robot.gripper_effort_default=1000
|
||||
--teleop.sync_gripper=true|false
|
||||
```
|
||||
|
||||
<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 +270,32 @@ 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 mode note (policy-sync / human-in-the-loop):
|
||||
|
||||
- Leader/follower linkage is configured via `MasterSlaveConfig(linkage_config, feedback_offset, ctrl_offset, linkage_offset)`.
|
||||
- `linkage_config=0xFA`: teaching-input arm.
|
||||
- `linkage_config=0xFC`: motion-output arm.
|
||||
- `0xFA` is not suitable when the arm must accept external control commands. For policy execution + intervention switching, keep both teleop and robot arms in `0xFC`.
|
||||
- After switching role mode (`0xFA` <-> `0xFC`), perform a full power-cycle before validation.
|
||||
|
||||
```bash
|
||||
lerobot-human-inloop-record \
|
||||
--robot.type=piper_follower \
|
||||
--robot.port=can0 \
|
||||
--robot.id=my_piper_follower \
|
||||
--teleop.type=piper_leader \
|
||||
--teleop.port=can1 \
|
||||
--teleop.id=my_piper_leader \
|
||||
--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
|
||||
```
|
||||
|
||||
Hotkeys:
|
||||
|
||||
- `i`: toggle intervention mode (policy <-> teleop takeover)
|
||||
@ -230,6 +313,7 @@ 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 +352,7 @@ 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,32 +392,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
|
||||
|
||||
Train the policy with advantage-conditioned tags.
|
||||
@ -377,6 +452,7 @@ 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,7 @@ 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",
|
||||
"wandb>=0.24.0,<0.25.0",
|
||||
|
||||
"torch>=2.2.1,<2.8.0", # TODO: Bumb dependency
|
||||
|
||||
@ -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
|
||||
from .piper_follower import PiperFollower
|
||||
@ -0,0 +1,90 @@
|
||||
#!/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 lerobot.utils.piper_sdk import PIPER_CALIBRATION_MODES
|
||||
|
||||
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
|
||||
|
||||
# Optional role command on startup (0xFC = motion output/follower arm)
|
||||
set_follower_mode_on_connect: bool = False
|
||||
|
||||
# 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
|
||||
# Calibration policy:
|
||||
# - required: always require calibration
|
||||
# - off: never require calibration
|
||||
calibration_mode: str = "required"
|
||||
|
||||
# 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
|
||||
|
||||
|
||||
@RobotConfig.register_subclass("piper_follower")
|
||||
@dataclass
|
||||
class PiperFollowerConfig(RobotConfig, PiperFollowerConfigBase):
|
||||
def __post_init__(self):
|
||||
super().__post_init__()
|
||||
if not (0 <= self.speed_ratio <= 100):
|
||||
raise ValueError("`speed_ratio` must be between 0 and 100.")
|
||||
if self.mode_refresh_interval_s < 0:
|
||||
raise ValueError("`mode_refresh_interval_s` must be >= 0.")
|
||||
if self.enable_timeout_s < 0:
|
||||
raise ValueError("`enable_timeout_s` must be >= 0.")
|
||||
if self.calibration_scale <= 0:
|
||||
raise ValueError("`calibration_scale` must be > 0.")
|
||||
if self.calibration_mode not in PIPER_CALIBRATION_MODES:
|
||||
raise ValueError("`calibration_mode` must be one of: required, off.")
|
||||
if self.startup_sleep_s < 0:
|
||||
raise ValueError("`startup_sleep_s` must be >= 0.")
|
||||
if not (0 <= self.gripper_effort_default <= 5000):
|
||||
raise ValueError("`gripper_effort_default` must be between 0 and 5000.")
|
||||
if self.gripper_status_code not in {0x00, 0x01, 0x02, 0x03}:
|
||||
raise ValueError("`gripper_status_code` must be one of 0x00, 0x01, 0x02, 0x03.")
|
||||
@ -0,0 +1,302 @@
|
||||
#!/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 typing import Any
|
||||
|
||||
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,
|
||||
milli_to_unit,
|
||||
parse_piper_log_level,
|
||||
should_require_piper_calibration,
|
||||
unit_to_milli,
|
||||
)
|
||||
from lerobot.utils.utils import enter_pressed, move_cursor_up
|
||||
|
||||
from ..robot import Robot
|
||||
from .config_piper_follower import PiperFollowerConfig
|
||||
|
||||
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):
|
||||
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 {**{key: float for key in PIPER_ACTION_KEYS}, **self._cameras_ft}
|
||||
|
||||
@cached_property
|
||||
def action_features(self) -> dict[str, type]:
|
||||
return {key: float for key in PIPER_ACTION_KEYS}
|
||||
|
||||
@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)
|
||||
|
||||
self._is_connected = True
|
||||
connected_cameras = []
|
||||
try:
|
||||
if self.config.set_follower_mode_on_connect:
|
||||
self.arm.MasterSlaveConfig(0xFC, 0x00, 0x00, 0x00)
|
||||
time.sleep(0.05)
|
||||
|
||||
self.configure()
|
||||
# lerobot-calibrate calls connect(calibrate=False) before running calibrate().
|
||||
# Keep the arm in drag mode for calibration by skipping auto-enable in this path.
|
||||
should_enable = self.config.enable_on_connect and calibrate
|
||||
if should_enable and not self._wait_enable(self.config.enable_timeout_s):
|
||||
logger.warning("Piper follower did not report enabled state before timeout.")
|
||||
if not self.is_calibrated and calibrate and should_require_piper_calibration(self.config.calibration_mode):
|
||||
logger.info("No piper-follower calibration file found for '%s'. Running lerobot-calibrate flow.", self.id)
|
||||
self.calibrate()
|
||||
|
||||
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 should_require_piper_calibration(self.config.calibration_mode)
|
||||
|
||||
@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:
|
||||
deadline = time.monotonic() + timeout_s
|
||||
while time.monotonic() < deadline:
|
||||
if bool(self.arm.EnablePiper()):
|
||||
return True
|
||||
time.sleep(0.02)
|
||||
return False
|
||||
|
||||
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=piper_follower --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)
|
||||
@ -64,6 +64,10 @@ 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 == "bi_openarm_follower":
|
||||
from .bi_openarm_follower import BiOpenArmFollower
|
||||
|
||||
|
||||
@ -44,6 +44,7 @@ from lerobot.robots import ( # noqa: F401
|
||||
make_robot_from_config,
|
||||
omx_follower,
|
||||
openarm_follower,
|
||||
piper_follower,
|
||||
so_follower,
|
||||
)
|
||||
from lerobot.teleoperators import ( # noqa: F401
|
||||
@ -56,6 +57,7 @@ from lerobot.teleoperators import ( # noqa: F401
|
||||
make_teleoperator_from_config,
|
||||
omx_leader,
|
||||
openarm_leader,
|
||||
piper_leader,
|
||||
so_leader,
|
||||
unitree_g1,
|
||||
)
|
||||
|
||||
@ -93,6 +93,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,
|
||||
@ -113,6 +114,7 @@ from lerobot.teleoperators import ( # noqa: F401
|
||||
make_teleoperator_from_config,
|
||||
omx_leader,
|
||||
openarm_leader,
|
||||
piper_leader,
|
||||
reachy2_teleoperator,
|
||||
so_leader,
|
||||
unitree_g1,
|
||||
|
||||
@ -61,6 +61,7 @@ from lerobot.robots import ( # noqa: F401
|
||||
make_robot_from_config,
|
||||
omx_follower,
|
||||
openarm_follower,
|
||||
piper_follower,
|
||||
reachy2,
|
||||
so_follower,
|
||||
unitree_g1,
|
||||
|
||||
@ -78,6 +78,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,
|
||||
@ -94,6 +95,7 @@ from lerobot.teleoperators import ( # noqa: F401
|
||||
make_teleoperator_from_config,
|
||||
omx_leader,
|
||||
openarm_leader,
|
||||
piper_leader,
|
||||
reachy2_teleoperator,
|
||||
so_leader,
|
||||
unitree_g1,
|
||||
|
||||
@ -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
|
||||
from .piper_leader import PiperLeader
|
||||
@ -0,0 +1,89 @@
|
||||
#!/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.utils.piper_sdk import PIPER_CALIBRATION_MODES
|
||||
|
||||
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
|
||||
|
||||
# Optional role command on startup (0xFC = motion output arm)
|
||||
set_leader_mode_on_connect: bool = False
|
||||
|
||||
# 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
|
||||
|
||||
# Calibration precision:
|
||||
# homing_offset/range_min/range_max are stored as "degree * calibration_scale".
|
||||
calibration_scale: int = 1000
|
||||
# Calibration policy:
|
||||
# - required: always require calibration
|
||||
# - off: never require calibration
|
||||
calibration_mode: str = "required"
|
||||
|
||||
# Safety behavior on disconnect
|
||||
disable_on_disconnect: bool = False
|
||||
|
||||
|
||||
@TeleoperatorConfig.register_subclass("piper_leader")
|
||||
@dataclass
|
||||
class PiperLeaderConfig(TeleoperatorConfig, PiperLeaderConfigBase):
|
||||
def __post_init__(self):
|
||||
if not (0 <= self.command_speed_ratio <= 100):
|
||||
raise ValueError("`command_speed_ratio` must be between 0 and 100.")
|
||||
if self.mode_refresh_interval_s < 0:
|
||||
raise ValueError("`mode_refresh_interval_s` must be >= 0.")
|
||||
if self.enable_timeout_s < 0:
|
||||
raise ValueError("`enable_timeout_s` must be >= 0.")
|
||||
if self.calibration_scale <= 0:
|
||||
raise ValueError("`calibration_scale` must be > 0.")
|
||||
if self.calibration_mode not in PIPER_CALIBRATION_MODES:
|
||||
raise ValueError("`calibration_mode` must be one of: required, off.")
|
||||
if self.startup_sleep_s < 0:
|
||||
raise ValueError("`startup_sleep_s` must be >= 0.")
|
||||
if not (0 <= self.gripper_effort_default <= 5000):
|
||||
raise ValueError("`gripper_effort_default` must be between 0 and 5000.")
|
||||
if self.gripper_status_code not in {0x00, 0x01, 0x02, 0x03}:
|
||||
raise ValueError("`gripper_status_code` must be one of 0x00, 0x01, 0x02, 0x03.")
|
||||
@ -0,0 +1,353 @@
|
||||
#!/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 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,
|
||||
milli_to_unit,
|
||||
parse_piper_log_level,
|
||||
should_require_piper_calibration,
|
||||
unit_to_milli,
|
||||
)
|
||||
from lerobot.utils.utils import enter_pressed, move_cursor_up
|
||||
|
||||
from ..teleoperator import Teleoperator
|
||||
from .config_piper_leader import PiperLeaderConfig
|
||||
|
||||
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 PiperLeader(Teleoperator):
|
||||
"""Piper leader arm used as a teleoperator through Piper SDK CAN messages."""
|
||||
|
||||
config_class = PiperLeaderConfig
|
||||
name = "piper_leader"
|
||||
|
||||
def __init__(self, config: PiperLeaderConfig):
|
||||
super().__init__(config)
|
||||
self.config = config
|
||||
self._is_connected = False
|
||||
self._manual_control_enabled: bool | None = None
|
||||
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),
|
||||
)
|
||||
|
||||
@cached_property
|
||||
def action_features(self) -> dict[str, type]:
|
||||
return {key: float for key in PIPER_ACTION_KEYS}
|
||||
|
||||
@cached_property
|
||||
def feedback_features(self) -> dict[str, type]:
|
||||
return {key: float for key in PIPER_ACTION_KEYS}
|
||||
|
||||
@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)
|
||||
|
||||
self._is_connected = True
|
||||
# Recompute control mode on every fresh connection.
|
||||
self._manual_control_enabled = None
|
||||
try:
|
||||
if self.config.set_leader_mode_on_connect:
|
||||
# NOTE:
|
||||
# - Piper teaching-input mode (0xFA) does not accept external JointCtrl/GripperCtrl commands.
|
||||
# - For policy-sync and human-in-the-loop switching, the leader must remain commandable,
|
||||
# so we configure it as motion-output mode (0xFC), same as the follower side.
|
||||
# - Mode-role changes (e.g., 0xFA <-> 0xFC) may require a full power-cycle to take effect.
|
||||
self.arm.MasterSlaveConfig(0xFC, 0x00, 0x00, 0x00)
|
||||
time.sleep(0.05)
|
||||
self.configure()
|
||||
if not self.is_calibrated and calibrate and should_require_piper_calibration(self.config.calibration_mode):
|
||||
logger.info("No piper-leader calibration file found for '%s'. Running lerobot-calibrate flow.", self.id)
|
||||
self.calibrate()
|
||||
except Exception:
|
||||
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 should_require_piper_calibration(self.config.calibration_mode)
|
||||
|
||||
@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:
|
||||
deadline = time.monotonic() + timeout_s
|
||||
while time.monotonic() < deadline:
|
||||
if bool(self.arm.EnablePiper()):
|
||||
return True
|
||||
time.sleep(0.02)
|
||||
return False
|
||||
|
||||
def set_manual_control(self, enabled: bool) -> None:
|
||||
if not self._is_connected:
|
||||
return
|
||||
if enabled and self._manual_control_enabled is not True:
|
||||
self.arm.DisableArm(7)
|
||||
self._manual_control_enabled = True
|
||||
return
|
||||
if not enabled and self._manual_control_enabled is not False:
|
||||
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.")
|
||||
self._manual_control_enabled = False
|
||||
|
||||
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=piper_leader --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=piper_leader --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.arm.GripperCtrl(
|
||||
gripper_pos_raw,
|
||||
self.config.gripper_effort_default,
|
||||
self.config.gripper_status_code,
|
||||
0x00,
|
||||
)
|
||||
|
||||
@check_if_not_connected
|
||||
def disconnect(self) -> None:
|
||||
try:
|
||||
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)
|
||||
@ -91,6 +91,10 @@ 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 == "bi_openarm_leader":
|
||||
from .bi_openarm_leader import BiOpenArmLeader
|
||||
|
||||
|
||||
75
src/lerobot/utils/piper_sdk.py
普通文件
75
src/lerobot/utils/piper_sdk.py
普通文件
@ -0,0 +1,75 @@
|
||||
#!/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
|
||||
|
||||
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_CALIBRATION_MODES = {"required", "off"}
|
||||
|
||||
|
||||
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 should_require_piper_calibration(calibration_mode: str) -> bool:
|
||||
if calibration_mode == "required":
|
||||
return True
|
||||
if calibration_mode == "off":
|
||||
return False
|
||||
raise ValueError(
|
||||
f"Invalid Piper calibration mode '{calibration_mode}'. "
|
||||
"Expected one of: required, off."
|
||||
)
|
||||
288
tests/test_piper_teleop.py
普通文件
288
tests/test_piper_teleop.py
普通文件
@ -0,0 +1,288 @@
|
||||
from types import SimpleNamespace
|
||||
|
||||
import pytest
|
||||
|
||||
import lerobot.utils.piper_sdk as piper_sdk_utils
|
||||
import lerobot.robots.piper_follower.piper_follower as piper_follower_module
|
||||
import lerobot.teleoperators.piper_leader.piper_leader as piper_leader_module
|
||||
from lerobot.motors import MotorCalibration
|
||||
from lerobot.robots.piper_follower import PiperFollower, PiperFollowerConfig
|
||||
from lerobot.robots.utils import make_robot_from_config
|
||||
from lerobot.teleoperators.piper_leader import PiperLeader, PiperLeaderConfig
|
||||
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.enable_calls = 0
|
||||
self.disable_calls = 0
|
||||
|
||||
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),
|
||||
)
|
||||
|
||||
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)
|
||||
|
||||
def EnablePiper(self):
|
||||
self.enable_calls += 1
|
||||
return True
|
||||
|
||||
def DisableArm(self, motor_num):
|
||||
del motor_num
|
||||
self.disable_calls += 1
|
||||
|
||||
def JointCtrl(self, *args):
|
||||
self.last_joint = args
|
||||
|
||||
def GripperCtrl(self, *args):
|
||||
self.last_gripper = 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 patch_fake_sdk(monkeypatch):
|
||||
fake_loader = lambda: (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 test_piper_leader_follower_teleop_roundtrip(monkeypatch):
|
||||
patch_fake_sdk(monkeypatch)
|
||||
|
||||
teleop_cfg = PiperLeaderConfig(
|
||||
port="can1",
|
||||
set_leader_mode_on_connect=True,
|
||||
manual_control=False,
|
||||
sync_gripper=True,
|
||||
)
|
||||
robot_cfg = PiperFollowerConfig(
|
||||
port="can0",
|
||||
set_follower_mode_on_connect=True,
|
||||
sync_gripper=True,
|
||||
)
|
||||
|
||||
teleop = make_teleoperator_from_config(teleop_cfg)
|
||||
robot = make_robot_from_config(robot_cfg)
|
||||
|
||||
assert isinstance(teleop, PiperLeader)
|
||||
assert isinstance(robot, PiperFollower)
|
||||
|
||||
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:
|
||||
try:
|
||||
teleop.get_action()
|
||||
assert False, "Expected teleop.get_action() to require calibration."
|
||||
except RuntimeError:
|
||||
pass
|
||||
|
||||
try:
|
||||
robot.send_action({"joint_1.pos": 0.0})
|
||||
assert False, "Expected robot.send_action() to require calibration."
|
||||
except RuntimeError:
|
||||
pass
|
||||
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_calibration_mode_off_allows_uncalibrated_control(monkeypatch):
|
||||
patch_fake_sdk(monkeypatch)
|
||||
|
||||
teleop = PiperLeader(PiperLeaderConfig(port="can1", calibration_mode="off"))
|
||||
robot = PiperFollower(PiperFollowerConfig(port="can0", calibration_mode="off"))
|
||||
|
||||
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()
|
||||
在新工单中引用
屏蔽一个用户