镜像自地址
https://github.com/MINT-SJTU/Evo-RL.git
已同步 2026-11-20 18:17:06 +00:00
feat(piper): add dedicated piperx types and auto URDF selection
- add piperx_leader/piperx_follower config and runtime classes reusing piper logic - remove manual gravity_comp_urdf_path and select bundled URDF by teleop type - add bundled piper/piperx description assets tracked via git lfs - update README piper setup and refresh piper teleop tests
这个提交包含在:
2
.gitattributes
vendored
2
.gitattributes
vendored
@ -20,3 +20,5 @@
|
||||
tests/artifacts/cameras/*.png filter=lfs diff=lfs merge=lfs -text
|
||||
*.bag filter=lfs diff=lfs merge=lfs -text
|
||||
*.gif filter=lfs diff=lfs merge=lfs -text
|
||||
src/lerobot/assets/piper_description/** filter=lfs diff=lfs merge=lfs -text
|
||||
src/lerobot/assets/piper_x_description/** filter=lfs diff=lfs merge=lfs -text
|
||||
|
||||
@ -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 *
|
||||
|
||||
38
README.md
38
README.md
@ -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,26 @@ 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"
|
||||
|
||||
|
||||
class PiperLeader(Teleoperator):
|
||||
@ -46,13 +52,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 +87,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
|
||||
@ -171,26 +172,74 @@ 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:
|
||||
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."
|
||||
)
|
||||
urdf_path = str(default_urdf)
|
||||
if self._gravity_comp_loop is None:
|
||||
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 +346,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 +367,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 +389,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 +402,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,18 +168,25 @@ def make_identity_calibration():
|
||||
}
|
||||
|
||||
|
||||
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
|
||||
|
||||
|
||||
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,
|
||||
)
|
||||
|
||||
@ -193,6 +231,60 @@ def test_piper_leader_follower_teleop_roundtrip(monkeypatch):
|
||||
robot.disconnect()
|
||||
|
||||
|
||||
def test_piperx_leader_follower_teleop_roundtrip(monkeypatch):
|
||||
patch_fake_sdk(monkeypatch)
|
||||
|
||||
teleop_cfg = PiperXLeaderConfig(
|
||||
port="can1",
|
||||
manual_control=False,
|
||||
sync_gripper=True,
|
||||
)
|
||||
robot_cfg = PiperXFollowerConfig(
|
||||
port="can0",
|
||||
sync_gripper=True,
|
||||
)
|
||||
|
||||
teleop = make_teleoperator_from_config(teleop_cfg)
|
||||
robot = make_robot_from_config(robot_cfg)
|
||||
|
||||
assert isinstance(teleop, PiperXLeader)
|
||||
assert isinstance(robot, PiperXFollower)
|
||||
|
||||
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)
|
||||
|
||||
@ -269,11 +361,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 +387,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 +416,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 +428,77 @@ 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)
|
||||
|
||||
在新工单中引用
屏蔽一个用户