feat(piper): add dedicated piperx types and auto URDF selection

- add dedicated piperx leader/follower types with shared piper logic
- auto-select bundled gravity-comp URDF by teleop type
- simplify piper teleop tests and tighten gravity-comp rollback/init safety
- bundle piper/piperx assets and refresh README/LFS guidance
这个提交包含在:
Ye Kai
2026-03-07 01:51:08 +08:00
提交者 GitHub
当前提交 7049938b87
共有 31 个文件被更改,包括 671 次插入 和 128 次删除

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

查看文件

@ -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 *

查看文件

@ -204,6 +204,13 @@ If needed, you can also use temporary device paths (for example `/dev/ttyACM*` a
#### AgileX PiPER
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:
@ -211,35 +218,18 @@ So first run `lerobot-setup-can` to confirm CAN interfaces are available:
lerobot-setup-can --mode=setup --interfaces=can0,can1
```
PiPER calibration is required by default.
If needed, you can disable it with
`--robot.require_calibration=false` and `--teleop.require_calibration=false`.
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.type=piperx_follower \
--robot.port=can0 \
--robot.id=my_piper_follower \
--teleop.type=piper_leader \
--robot.id=my_piperx_follower \
--robot.require_calibration=false \
--teleop.type=piperx_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
--teleop.id=my_piperx_leader \
--teleop.require_calibration=false
```
<a id="data-collection"></a>
@ -290,9 +280,11 @@ lerobot-human-inloop-record \
--robot.type=piper_follower \
--robot.port=can0 \
--robot.id=my_piper_follower \
--robot.require_calibration=false \
--teleop.type=piper_leader \
--teleop.port=can1 \
--teleop.id=my_piper_leader \
--teleop.require_calibration=false \
--dataset.repo_id=<HF_USERNAME_OR_ORG>/<DATASET_NAME> \
--dataset.single_task="<YOUR_TASK_DESCRIPTION>" \
--dataset.num_episodes=<NUM_EPISODES> \

查看文件

@ -75,6 +75,7 @@ dependencies = [
"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
@ -220,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

查看文件

@ -14,5 +14,5 @@
# 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
from .config_piper_follower import PiperFollowerConfig, PiperFollowerConfigBase, PiperXFollowerConfig
from .piper_follower import PiperFollower, PiperXFollower

查看文件

@ -35,9 +35,6 @@ class PiperFollowerConfigBase:
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
@ -65,24 +62,36 @@ class PiperFollowerConfigBase:
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__()
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 not isinstance(self.require_calibration, bool):
raise ValueError("require_calibration must be true or false.")
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.")
_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)

查看文件

@ -24,17 +24,19 @@ 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,
guard_piper_ctrl_mode_on_connect,
PIPER_JOINT_ACTION_KEYS,
PIPER_JOINT_NAMES,
get_piper_sdk,
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
from .config_piper_follower import PiperFollowerConfig, PiperXFollowerConfig
logger = logging.getLogger(__name__)
PIPER_CALIB_KEYS = list(PIPER_ACTION_KEYS)
@ -47,7 +49,7 @@ class PiperFollower(Robot):
config_class = PiperFollowerConfig
name = "piper_follower"
def __init__(self, config: PiperFollowerConfig):
def __init__(self, config: PiperFollowerConfig | PiperXFollowerConfig):
super().__init__(config)
self.config = config
self._is_connected = False
@ -85,14 +87,11 @@ class PiperFollower(Robot):
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:
if self.config.set_follower_mode_on_connect:
self.arm.MasterSlaveConfig(0xFC, 0x00, 0x00, 0x00)
time.sleep(0.05)
self.configure()
should_auto_calibrate = (
not self.is_calibrated
@ -190,12 +189,7 @@ class PiperFollower(Robot):
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
return wait_enable_piper(self.arm, timeout_s)
def _read_raw_observation(self) -> RobotObservation:
joint_msg = self.arm.GetArmJointMsgs()
@ -259,7 +253,7 @@ class PiperFollower(Robot):
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."
f"{self} is not calibrated. Run `lerobot-calibrate --robot.type={self.config.type} --robot.id={self.id}` first."
)
self._refresh_motion_mode_if_needed()
@ -308,3 +302,8 @@ class PiperFollower(Robot):
cam.disconnect()
self._is_connected = False
logger.info("%s disconnected.", self)
class PiperXFollower(PiperFollower):
config_class = PiperXFollowerConfig
name = "piperx_follower"

查看文件

@ -68,6 +68,10 @@ def make_robot_from_config(config: RobotConfig) -> Robot:
from .piper_follower import PiperFollower
return PiperFollower(config)
elif config.type == "piperx_follower":
from .piper_follower import PiperXFollower
return PiperXFollower(config)
elif config.type == "bi_openarm_follower":
from .bi_openarm_follower import BiOpenArmFollower

查看文件

@ -14,5 +14,5 @@
# 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
from .config_piper_leader import PiperLeaderConfig, PiperLeaderConfigBase, PiperXLeaderConfig
from .piper_leader import PiperLeader, PiperXLeader

查看文件

@ -16,8 +16,6 @@
from dataclasses import dataclass
from ..config import TeleoperatorConfig
@ -34,9 +32,6 @@ class PiperLeaderConfigBase:
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
@ -55,6 +50,14 @@ class PiperLeaderConfigBase:
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
@ -65,23 +68,42 @@ class PiperLeaderConfigBase:
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):
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 not isinstance(self.require_calibration, bool):
raise ValueError("require_calibration must be true or false.")
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.")
_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)

查看文件

@ -17,6 +17,7 @@
import logging
import time
from functools import cached_property
from importlib import resources
from typing import Any
from lerobot.motors import MotorCalibration
@ -24,21 +25,41 @@ 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,
guard_piper_ctrl_mode_on_connect,
PIPER_JOINT_ACTION_KEYS,
PIPER_JOINT_NAMES,
get_piper_sdk,
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
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):
@ -46,13 +67,15 @@ class PiperLeader(Teleoperator):
config_class = PiperLeaderConfig
name = "piper_leader"
gravity_comp_urdf_relpath = DEFAULT_PIPER_GRAVITY_URDF
def __init__(self, config: PiperLeaderConfig):
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(
@ -79,19 +102,12 @@ class PiperLeader(Teleoperator):
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:
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
@ -104,6 +120,7 @@ class PiperLeader(Teleoperator):
)
self.calibrate()
except Exception:
self._stop_gravity_comp_loop_if_needed()
self.arm.DisconnectPort()
self._is_connected = False
raise
@ -171,26 +188,75 @@ class PiperLeader(Teleoperator):
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
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:
self.arm.DisableArm(7)
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)
@ -297,7 +363,7 @@ class PiperLeader(Teleoperator):
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."
f"{self} is not calibrated. Run `lerobot-calibrate --teleop.type={self.config.type} --teleop.id={self.id}` first."
)
raw_action = self._read_raw_action()
@ -318,7 +384,7 @@ class PiperLeader(Teleoperator):
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."
f"{self} is not calibrated. Run `lerobot-calibrate --teleop.type={self.config.type} --teleop.id={self.id}` first."
)
self.set_manual_control(False)
@ -340,16 +406,12 @@ class PiperLeader(Teleoperator):
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,
)
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:
@ -357,3 +419,9 @@ class PiperLeader(Teleoperator):
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

查看文件

@ -95,6 +95,10 @@ def make_teleoperator_from_config(config: TeleoperatorConfig) -> "Teleoperator":
from .piper_leader import PiperLeader
return PiperLeader(config)
elif config.type == "piperx_leader":
from .piper_leader import PiperXLeader
return PiperXLeader(config)
elif config.type == "bi_openarm_leader":
from .bi_openarm_leader import BiOpenArmLeader

查看文件

@ -16,6 +16,7 @@
from __future__ import annotations
import time
from functools import lru_cache
from typing import Any
@ -29,7 +30,8 @@ PIPER_JOINT_NAMES = (
)
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:
@ -64,3 +66,64 @@ def parse_piper_log_level(level_name: str) -> Any:
) 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 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."
)

查看文件

@ -1,4 +1,6 @@
# ruff: noqa: N802
import sys
import time
from types import SimpleNamespace
import pytest
@ -7,9 +9,9 @@ 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.piper_follower import PiperFollower, PiperFollowerConfig
from lerobot.robots.piper_follower import PiperFollower, PiperFollowerConfig, PiperXFollower, PiperXFollowerConfig
from lerobot.robots.utils import make_robot_from_config
from lerobot.teleoperators.piper_leader import PiperLeader, PiperLeaderConfig
from lerobot.teleoperators.piper_leader import PiperLeader, PiperLeaderConfig, PiperXLeader, PiperXLeaderConfig
from lerobot.teleoperators.utils import make_teleoperator_from_config
from lerobot.utils.piper_sdk import PIPER_ACTION_KEYS
@ -34,6 +36,8 @@ class FakePiperInterface:
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
@ -72,6 +76,21 @@ class FakePiperInterface:
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
@ -85,6 +104,8 @@ class FakePiperInterface:
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
@ -99,8 +120,12 @@ class FakePiperInterface:
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
@ -114,6 +139,12 @@ class FakePiperInterface:
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():
@ -137,26 +168,40 @@ def make_identity_calibration():
}
def test_piper_leader_follower_teleop_roundtrip(monkeypatch):
patch_fake_sdk(monkeypatch)
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
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,
)
@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, PiperLeader)
assert isinstance(robot, PiperFollower)
assert isinstance(teleop, teleop_cls)
assert isinstance(robot, robot_cls)
teleop.calibration = make_identity_calibration()
robot.calibration = make_identity_calibration()
@ -269,11 +314,11 @@ def test_piper_follower_connect_rolls_back_connected_cameras(monkeypatch):
assert not robot.is_connected
def test_piper_calibration_mode_off_allows_uncalibrated_control(monkeypatch):
def test_piper_require_calibration_false_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 = PiperLeader(PiperLeaderConfig(port="can1", require_calibration=False))
robot = PiperFollower(PiperFollowerConfig(port="can0", require_calibration=False))
teleop.connect(calibrate=False)
robot.connect(calibrate=False)
@ -295,7 +340,7 @@ def test_piper_follower_connect_calibrates_then_reenables(monkeypatch, tmp_path)
port="can0",
id="connect_reenable_after_calibration",
calibration_dir=tmp_path,
calibration_mode="required",
require_calibration=True,
enable_on_connect=True,
)
)
@ -324,7 +369,7 @@ def test_piper_follower_connect_without_calibration_still_enables(monkeypatch, t
port="can0",
id="connect_enable_without_calibration",
calibration_dir=tmp_path,
calibration_mode="required",
require_calibration=True,
enable_on_connect=True,
)
)
@ -336,3 +381,89 @@ def test_piper_follower_connect_without_calibration_still_enables(monkeypatch, t
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\n"
"oid sha256:deadbeef\n"
"size 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")