feat(piper): switch leader roles for teleop and policy control (#33)

* feat(piper): support read-only PiPER-X leader teleop

* trim PiPER-X read-only teleop scope

* refactor(piper): switch leader roles via manual control

* refactor(piper): remove calibration and gravity assets

* simplify(piper): use direct bimanual follower lifecycle

* simplify(piper): remove bimanual leader rollback logic

* refactor(piper): configure arm roles directly

* fix(piper): stabilize PiPER-X leader teleop startup

* fix(piper): configure leader gripper drag sensitivity

* fix(piper): forward bimanual gripper drag config

* revert(piper): remove recent startup workarounds

* fix(piper): make leader action reads non-blocking

* simplify(piper): remove follower send-only mode

* simplify(piper): remove follower mode refresh

* simplify(piper): use default leader connection

* simplify(piper): streamline leader role switching

* simplify(piper): streamline leader action reads

* simplify(piper): remove role switch delay

* docs(piper): remove role switching details

* feat(piper): address USB-CAN adapters by serial

---------

Co-authored-by: Elvin-yk <180030703+Elvin-yk@users.noreply.github.com>
这个提交包含在:
Travor King
2026-07-15 08:23:51 -05:00
提交者 GitHub
父节点 257d1326dc
当前提交 e439dd12d5
共有 33 个文件被更改,包括 396 次插入 和 1288 次删除

2
.gitattributes vendored
查看文件

@ -20,5 +20,3 @@
tests/artifacts/cameras/*.png filter=lfs diff=lfs merge=lfs -text tests/artifacts/cameras/*.png filter=lfs diff=lfs merge=lfs -text
*.bag filter=lfs diff=lfs merge=lfs -text *.bag filter=lfs diff=lfs merge=lfs -text
*.gif 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,4 +1,2 @@
include src/lerobot/templates/lerobot_modelcard_template.md include src/lerobot/templates/lerobot_modelcard_template.md
include src/lerobot/datasets/card_template.md include src/lerobot/datasets/card_template.md
recursive-include src/lerobot/assets/piper_x_description *
recursive-include src/lerobot/assets/piper_description *

查看文件

@ -206,20 +206,22 @@ If needed, you can also use temporary device paths (for example `/dev/ttyACM*` a
#### AgileX (PiPER/PiPER-X) #### AgileX (PiPER/PiPER-X)
PiPER arms in master/teaching mode cannot receive external control commands, so all arms must be configured to follower/motion-output mode (0xFC), and firmware must be version 1.8.5 or above. PiPER commands use the USB-CAN adapter's stable `ID_SERIAL_SHORT` instead of a temporary Linux
`canN` name. List the connected adapters with:
For PiPER-series robots, make sure Git LFS assets are pulled before running teleoperation:
```bash ```bash
git lfs pull --include="src/lerobot/assets/piper_description/**,src/lerobot/assets/piper_x_description/**" --exclude="*" for device in /sys/class/net/*; do
git lfs checkout src/lerobot/assets/piper_description src/lerobot/assets/piper_x_description [ "$(cat "$device/type" 2>/dev/null)" = "280" ] || continue
serial=$(udevadm info --query=property --path="$device" | sed -n 's/^ID_SERIAL_SHORT=//p')
echo "$(basename "$device"): $serial"
done
``` ```
For PiPER setup, PiPER uses CAN interfaces instead of serial ports. Then configure the adapters needed for the run:
So first run `lerobot-setup-can` to confirm CAN interfaces are available:
```bash ```bash
lerobot-setup-can --mode=setup --interfaces=<LEFT_FOLLOWER_CAN_PORT>,<LEFT_LEADER_CAN_PORT>,<RIGHT_FOLLOWER_CAN_PORT>,<RIGHT_LEADER_CAN_PORT> lerobot-setup-can --mode=setup \
--usb_can_serials=<USB_CAN_SERIAL_1>,<USB_CAN_SERIAL_2>,<USB_CAN_SERIAL_3>,<USB_CAN_SERIAL_4>
``` ```
For single-arm users, run the command below to verify the system is ready: For single-arm users, run the command below to verify the system is ready:
@ -227,13 +229,11 @@ For single-arm users, run the command below to verify the system is ready:
```bash ```bash
lerobot-teleoperate \ lerobot-teleoperate \
--robot.type=piperx_follower \ --robot.type=piperx_follower \
--robot.port=<FOLLOWER_CAN_PORT> \ --robot.port=<FOLLOWER_USB_CAN_SERIAL> \
--robot.id=my_piperx_follower \ --robot.id=my_piperx_follower \
--robot.require_calibration=false \
--teleop.type=piperx_leader \ --teleop.type=piperx_leader \
--teleop.port=<LEADER_CAN_PORT> \ --teleop.port=<LEADER_USB_CAN_SERIAL> \
--teleop.id=my_piperx_leader \ --teleop.id=my_piperx_leader
--teleop.require_calibration=false
``` ```
For bimanual users, run this command to verify dual-arm teleoperation: For bimanual users, run this command to verify dual-arm teleoperation:
@ -242,16 +242,12 @@ For bimanual users, run this command to verify dual-arm teleoperation:
lerobot-teleoperate \ lerobot-teleoperate \
--robot.type=bi_piperx_follower \ --robot.type=bi_piperx_follower \
--robot.id=my_bi_piperx_follower \ --robot.id=my_bi_piperx_follower \
--robot.left_arm_config.port=<LEFT_FOLLOWER_CAN_PORT> \ --robot.left_arm_config.port=<LEFT_FOLLOWER_USB_CAN_SERIAL> \
--robot.right_arm_config.port=<RIGHT_FOLLOWER_CAN_PORT> \ --robot.right_arm_config.port=<RIGHT_FOLLOWER_USB_CAN_SERIAL> \
--robot.left_arm_config.require_calibration=false \
--robot.right_arm_config.require_calibration=false \
--teleop.type=bi_piperx_leader \ --teleop.type=bi_piperx_leader \
--teleop.id=my_bi_piperx_leader \ --teleop.id=my_bi_piperx_leader \
--teleop.left_arm_config.port=<LEFT_LEADER_CAN_PORT> \ --teleop.left_arm_config.port=<LEFT_LEADER_USB_CAN_SERIAL> \
--teleop.right_arm_config.port=<RIGHT_LEADER_CAN_PORT> \ --teleop.right_arm_config.port=<RIGHT_LEADER_USB_CAN_SERIAL>
--teleop.left_arm_config.require_calibration=false \
--teleop.right_arm_config.require_calibration=false
``` ```
For PiPER (non-X), replace `bi_piperx_follower`/`bi_piperx_leader` with `bi_piper_follower`/`bi_piper_leader`. For PiPER (non-X), replace `bi_piperx_follower`/`bi_piperx_leader` with `bi_piper_follower`/`bi_piper_leader`.
@ -297,16 +293,12 @@ Bimanual template (left/right, PiPER-X example):
lerobot-human-inloop-record \ lerobot-human-inloop-record \
--robot.type=bi_piperx_follower \ --robot.type=bi_piperx_follower \
--robot.id=my_bi_piperx_follower \ --robot.id=my_bi_piperx_follower \
--robot.left_arm_config.port=<LEFT_FOLLOWER_CAN_PORT> \ --robot.left_arm_config.port=<LEFT_FOLLOWER_USB_CAN_SERIAL> \
--robot.right_arm_config.port=<RIGHT_FOLLOWER_CAN_PORT> \ --robot.right_arm_config.port=<RIGHT_FOLLOWER_USB_CAN_SERIAL> \
--robot.left_arm_config.require_calibration=false \
--robot.right_arm_config.require_calibration=false \
--teleop.type=bi_piperx_leader \ --teleop.type=bi_piperx_leader \
--teleop.id=my_bi_piperx_leader \ --teleop.id=my_bi_piperx_leader \
--teleop.left_arm_config.port=<LEFT_LEADER_CAN_PORT> \ --teleop.left_arm_config.port=<LEFT_LEADER_USB_CAN_SERIAL> \
--teleop.right_arm_config.port=<RIGHT_LEADER_CAN_PORT> \ --teleop.right_arm_config.port=<RIGHT_LEADER_USB_CAN_SERIAL> \
--teleop.left_arm_config.require_calibration=false \
--teleop.right_arm_config.require_calibration=false \
--dataset.repo_id=<HF_USERNAME_OR_ORG>/<DATASET_NAME> \ --dataset.repo_id=<HF_USERNAME_OR_ORG>/<DATASET_NAME> \
--dataset.single_task="<YOUR_TASK_DESCRIPTION>" \ --dataset.single_task="<YOUR_TASK_DESCRIPTION>" \
--dataset.num_episodes=<NUM_EPISODES> \ --dataset.num_episodes=<NUM_EPISODES> \

查看文件

@ -75,7 +75,6 @@ dependencies = [
"pynput>=1.7.7,<1.9.0", "pynput>=1.7.7,<1.9.0",
"pyserial>=3.5,<4.0", "pyserial>=3.5,<4.0",
"piper_sdk>=0.6.1,<0.7.0", "piper_sdk>=0.6.1,<0.7.0",
"pin>=3.0.0,<4.0.0",
"wandb>=0.24.0,<0.25.0", "wandb>=0.24.0,<0.25.0",
"torch>=2.2.1,<2.8.0", # TODO: Bumb dependency "torch>=2.2.1,<2.8.0", # TODO: Bumb dependency
@ -222,12 +221,6 @@ lerobot-export-boundary-frames="lerobot.scripts.lerobot_export_boundary_frames:m
[tool.setuptools.packages.find] [tool.setuptools.packages.find]
where = ["src"] where = ["src"]
[tool.setuptools.package-data]
lerobot = [
"assets/piper_description/**/*",
"assets/piper_x_description/**/*",
]
[tool.ruff] [tool.ruff]
target-version = "py310" target-version = "py310"
line-length = 110 line-length = 110

查看文件

@ -1,3 +0,0 @@
version https://git-lfs.github.com/spec/v1
oid sha256:fd4a3d6d0266205fe3f725f048645be66592c23564db22e08b0807d5569b2c64
size 607384

查看文件

@ -1,3 +0,0 @@
version https://git-lfs.github.com/spec/v1
oid sha256:9ee6e1d67c0241f42a0dbaf5c676d19230375ed078ee1943bb8c4c37ab303d14
size 447784

查看文件

@ -1,3 +0,0 @@
version https://git-lfs.github.com/spec/v1
oid sha256:0d37675b699b643f2e15348de99aed535fac10dc5f09a79b8e4b0acfaf4e0378
size 3658384

查看文件

@ -1,3 +0,0 @@
version https://git-lfs.github.com/spec/v1
oid sha256:e8ae51f17c0c7f6d67b753922e2cc269f5e8372a65734f859231639a64fe8fad
size 1845784

查看文件

@ -1,3 +0,0 @@
version https://git-lfs.github.com/spec/v1
oid sha256:efa33022976218f246f736a6133494b5d0e5486e8b50b52900c42f285ae85558
size 877984

查看文件

@ -1,3 +0,0 @@
version https://git-lfs.github.com/spec/v1
oid sha256:a1c69d1c7f6f8c60e2c82331c0f444725200ed4053d3e5ec23aa82cb5db5b29f
size 833684

查看文件

@ -1,3 +0,0 @@
version https://git-lfs.github.com/spec/v1
oid sha256:88374973bc3bd9e7389b5853cd6e555af9731c30b658b1ee6f85be34a126cb92
size 58884

查看文件

@ -1,3 +0,0 @@
version https://git-lfs.github.com/spec/v1
oid sha256:a93c1e3fc245d9d336a427fcc506cb1fc15615268053392b249816f90c011e50
size 8273

查看文件

@ -1,3 +0,0 @@
version https://git-lfs.github.com/spec/v1
oid sha256:0861e122e406f891a57a550551ad1ce0ef4ca5babf58f476cf8914395e144319
size 162084

查看文件

@ -1,3 +0,0 @@
version https://git-lfs.github.com/spec/v1
oid sha256:e78dee99bc84857c09183463f3fb3fdea292ea7570f1bfb0001a79674bb69d45
size 2323489

查看文件

@ -1,3 +0,0 @@
version https://git-lfs.github.com/spec/v1
oid sha256:97e9079580af6e423317c33b2a0f4fe0f6dbc67c3e1d9e6f4836c87c6c8f34a2
size 14480060

查看文件

@ -1,3 +0,0 @@
version https://git-lfs.github.com/spec/v1
oid sha256:db5b39eef745655a67e57870d8ee56f26a03651d2161635ca2ec3c58c9269907
size 5098355

查看文件

@ -1,3 +0,0 @@
version https://git-lfs.github.com/spec/v1
oid sha256:ccc08d3ef1cb8bdfb8ab5395ff65893d13386ad526d7bc954d468b7dc885d557
size 2037373

查看文件

@ -1,3 +0,0 @@
version https://git-lfs.github.com/spec/v1
oid sha256:78718679ffdf9d81130047c4267ed66a081315eff36f216833bc3781c6241417
size 2155609

查看文件

@ -1,3 +0,0 @@
version https://git-lfs.github.com/spec/v1
oid sha256:486dddcd07f072b2e3978087dc2f9b641a91722bccb1bcb3421c4c7ce64a39ed
size 1001764

查看文件

@ -1,3 +0,0 @@
version https://git-lfs.github.com/spec/v1
oid sha256:d6dc6d31b0fa56341b2323fee5e8a375c2500ae6fb02e31fb52e8e917b608439
size 8169

查看文件

@ -14,7 +14,6 @@
# See the License for the specific language governing permissions and # See the License for the specific language governing permissions and
# limitations under the License. # limitations under the License.
import logging
from functools import cached_property from functools import cached_property
from lerobot.processor import RobotAction, RobotObservation from lerobot.processor import RobotAction, RobotObservation
@ -29,8 +28,6 @@ from lerobot.utils.decorators import check_if_already_connected, check_if_not_co
from ..robot import Robot from ..robot import Robot
from .config_bi_piper_follower import BiPiperFollowerConfig, BiPiperXFollowerConfig from .config_bi_piper_follower import BiPiperFollowerConfig, BiPiperXFollowerConfig
logger = logging.getLogger(__name__)
class BiPiperFollower(Robot): class BiPiperFollower(Robot):
"""Bimanual PiPER/PiPER-X follower arms.""" """Bimanual PiPER/PiPER-X follower arms."""
@ -45,11 +42,8 @@ class BiPiperFollower(Robot):
"startup_sleep_s", "startup_sleep_s",
"speed_ratio", "speed_ratio",
"high_follow", "high_follow",
"mode_refresh_interval_s",
"enable_on_connect", "enable_on_connect",
"enable_timeout_s", "enable_timeout_s",
"calibration_scale",
"require_calibration",
"sync_gripper", "sync_gripper",
"gripper_effort_default", "gripper_effort_default",
"gripper_status_code", "gripper_status_code",
@ -60,11 +54,11 @@ class BiPiperFollower(Robot):
def _build_arm_config(self, arm_config_cls, side_cfg, side: str): def _build_arm_config(self, arm_config_cls, side_cfg, side: str):
kwargs = {name: getattr(side_cfg, name) for name in self._side_field_names} kwargs = {name: getattr(side_cfg, name) for name in self._side_field_names}
kwargs["id"] = f"{self.config.id}_{side}" if self.config.id else None kwargs["id"] = f"{self.config.id}_{side}" if self.config.id else None
kwargs["calibration_dir"] = self.config.calibration_dir
return arm_config_cls(**kwargs) return arm_config_cls(**kwargs)
def __init__(self, config: BiPiperFollowerConfig | BiPiperXFollowerConfig): def __init__(self, config: BiPiperFollowerConfig | BiPiperXFollowerConfig):
super().__init__(config) self.robot_type = self.name
self.id = config.id
self.config = config self.config = config
if config.type == "bi_piperx_follower": if config.type == "bi_piperx_follower":
@ -115,20 +109,16 @@ class BiPiperFollower(Robot):
@check_if_already_connected @check_if_already_connected
def connect(self, calibrate: bool = True) -> None: def connect(self, calibrate: bool = True) -> None:
self.left_arm.connect(calibrate) del calibrate
self.right_arm.connect(calibrate) self.left_arm.connect()
self.right_arm.connect()
def set_teleop_send_only_mode(self, enabled: bool) -> None:
self.left_arm.set_teleop_send_only_mode(enabled)
self.right_arm.set_teleop_send_only_mode(enabled)
@property @property
def is_calibrated(self) -> bool: def is_calibrated(self) -> bool:
return self.left_arm.is_calibrated and self.right_arm.is_calibrated return True
def calibrate(self) -> None: def calibrate(self) -> None:
self.left_arm.calibrate() pass
self.right_arm.calibrate()
def configure(self) -> None: def configure(self) -> None:
self.left_arm.configure() self.left_arm.configure()

查看文件

@ -25,7 +25,7 @@ from ..config import RobotConfig
class PiperFollowerConfigBase: class PiperFollowerConfigBase:
"""Configuration for a Piper follower arm controlled over CAN.""" """Configuration for a Piper follower arm controlled over CAN."""
# CAN interface name (e.g. "can0") # USB-CAN adapter ID_SERIAL_SHORT
port: str port: str
# Piper SDK connection options # Piper SDK connection options
@ -37,18 +37,11 @@ class PiperFollowerConfigBase:
# Motion mode for follower arm # Motion mode for follower arm
speed_ratio: int = 100 speed_ratio: int = 100
high_follow: bool = True high_follow: bool = True
mode_refresh_interval_s: float = 1.0
# Arm enable behavior # Arm enable behavior
enable_on_connect: bool = True enable_on_connect: bool = True
enable_timeout_s: float = 3.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
# Whether calibration is required on connect when no calibration file exists
require_calibration: bool = True
# Gripper forwarding behavior # Gripper forwarding behavior
sync_gripper: bool = True sync_gripper: bool = True
gripper_effort_default: int = 1000 gripper_effort_default: int = 1000
@ -64,14 +57,8 @@ class PiperFollowerConfigBase:
def _validate_piper_follower_config(config: PiperFollowerConfigBase) -> None: def _validate_piper_follower_config(config: PiperFollowerConfigBase) -> None:
if not (0 <= config.speed_ratio <= 100): if not (0 <= config.speed_ratio <= 100):
raise ValueError("`speed_ratio` must be between 0 and 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: if config.enable_timeout_s < 0:
raise ValueError("`enable_timeout_s` must be >= 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: if config.startup_sleep_s < 0:
raise ValueError("`startup_sleep_s` must be >= 0.") raise ValueError("`startup_sleep_s` must be >= 0.")
if not (0 <= config.gripper_effort_default <= 5000): if not (0 <= config.gripper_effort_default <= 5000):

查看文件

@ -19,28 +19,26 @@ import time
from functools import cached_property from functools import cached_property
from lerobot.cameras.utils import make_cameras_from_configs from lerobot.cameras.utils import make_cameras_from_configs
from lerobot.motors import MotorCalibration
from lerobot.processor import RobotAction, RobotObservation from lerobot.processor import RobotAction, RobotObservation
from lerobot.utils.decorators import check_if_already_connected, check_if_not_connected from lerobot.utils.decorators import check_if_already_connected, check_if_not_connected
from lerobot.utils.piper_sdk import ( from lerobot.utils.piper_sdk import (
PIPER_ACTION_KEYS, PIPER_ACTION_KEYS,
PIPER_JOINT_ACTION_KEYS, PIPER_JOINT_ACTION_KEYS,
PIPER_JOINT_NAMES, PIPER_JOINT_NAMES,
PIPER_ROLE_FOLLOWER,
get_piper_sdk, get_piper_sdk,
guard_piper_ctrl_mode_on_connect,
milli_to_unit, milli_to_unit,
parse_piper_log_level, parse_piper_log_level,
resolve_piper_can_interface,
set_piper_role,
unit_to_milli, unit_to_milli,
wait_enable_piper, wait_enable_piper,
) )
from lerobot.utils.utils import enter_pressed, move_cursor_up
from ..robot import Robot from ..robot import Robot
from .config_piper_follower import PiperFollowerConfig, PiperXFollowerConfig from .config_piper_follower import PiperFollowerConfig, PiperXFollowerConfig
logger = logging.getLogger(__name__) 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): class PiperFollower(Robot):
@ -50,15 +48,14 @@ class PiperFollower(Robot):
name = "piper_follower" name = "piper_follower"
def __init__(self, config: PiperFollowerConfig | PiperXFollowerConfig): def __init__(self, config: PiperFollowerConfig | PiperXFollowerConfig):
super().__init__(config) self.robot_type = self.name
self.id = config.id
self.config = config self.config = config
self._is_connected = False self._is_connected = False
self._last_mode_refresh_t = 0.0
self._teleop_send_only_mode = False
interface_cls, _ = get_piper_sdk() interface_cls, _ = get_piper_sdk()
self.arm = interface_cls( self.arm = interface_cls(
can_name=self.config.port, can_name=resolve_piper_can_interface(self.config.port),
judge_flag=self.config.judge_flag, judge_flag=self.config.judge_flag,
can_auto_init=self.config.can_auto_init, can_auto_init=self.config.can_auto_init,
logger_level=parse_piper_log_level(self.config.log_level), logger_level=parse_piper_log_level(self.config.log_level),
@ -85,47 +82,27 @@ class PiperFollower(Robot):
@property @property
def is_connected(self) -> bool: def is_connected(self) -> bool:
return self._is_connected and ( return self._is_connected and all(cam.is_connected for cam in self.cameras.values())
self._teleop_send_only_mode or all(cam.is_connected for cam in self.cameras.values())
)
def set_teleop_send_only_mode(self, enabled: bool) -> None:
if self._is_connected:
raise RuntimeError("teleop send-only mode must be configured before connecting the robot.")
self._teleop_send_only_mode = enabled
@check_if_already_connected @check_if_already_connected
def connect(self, calibrate: bool = True) -> None: def connect(self, calibrate: bool = True) -> None:
self.arm.ConnectPort(start_thread=not self._teleop_send_only_mode) del calibrate
if self.config.startup_sleep_s > 0: self.arm.ConnectPort()
time.sleep(self.config.startup_sleep_s)
if not self._teleop_send_only_mode:
guard_piper_ctrl_mode_on_connect(arm=self.arm, interface_name=self.config.port)
self._is_connected = True
connected_cameras = [] connected_cameras = []
try: try:
self.configure() if self.config.startup_sleep_s > 0:
should_auto_calibrate = not self.is_calibrated and calibrate and self.config.require_calibration time.sleep(self.config.startup_sleep_s)
if should_auto_calibrate:
logger.info(
"No piper-follower calibration file found for '%s'. Running lerobot-calibrate flow.",
self.id,
)
self.calibrate()
# Enable behavior should be controlled by enable_on_connect, independent from calibrate flag.
# This keeps connect(calibrate=False) commandable for callers that only want to skip interactive calibration.
should_enable = self.config.enable_on_connect
if should_enable:
if self._teleop_send_only_mode:
self.arm.EnablePiper()
elif not self._wait_enable(self.config.enable_timeout_s):
logger.warning("Piper follower did not report enabled state before timeout.")
if not self._teleop_send_only_mode: self._is_connected = True
for cam in self.cameras.values(): self.configure()
cam.connect() if self.config.enable_on_connect and not wait_enable_piper(
connected_cameras.append(cam) self.arm, self.config.enable_timeout_s
):
logger.warning("Piper follower did not report enabled state before timeout.")
for cam in self.cameras.values():
cam.connect()
connected_cameras.append(cam)
except Exception: except Exception:
self.arm.DisconnectPort() self.arm.DisconnectPort()
for cam in connected_cameras: for cam in connected_cameras:
@ -135,76 +112,20 @@ class PiperFollower(Robot):
logger.info("%s connected.", self) logger.info("%s connected.", self)
def _use_uncalibrated_passthrough(self) -> bool:
return not self.is_calibrated and not self.config.require_calibration
@property @property
def is_calibrated(self) -> bool: 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 return True
def calibrate(self) -> None: def calibrate(self) -> None:
if self.calibration and self.is_calibrated: pass
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: def configure(self) -> None:
self._send_motion_mode() set_piper_role(self.arm, PIPER_ROLE_FOLLOWER)
def _send_motion_mode(self) -> None:
mit_mode = 0xAD if self.config.high_follow else 0x00 mit_mode = 0xAD if self.config.high_follow else 0x00
self.arm.MotionCtrl_2(0x01, 0x01, self.config.speed_ratio, mit_mode) 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: @check_if_not_connected
interval_s = self.config.mode_refresh_interval_s def get_observation(self) -> RobotObservation:
if interval_s <= 0:
return
now = time.monotonic()
if now - self._last_mode_refresh_t >= interval_s:
self._send_motion_mode()
def _wait_enable(self, timeout_s: float) -> bool:
return wait_enable_piper(self.arm, timeout_s)
def _read_raw_observation(self) -> RobotObservation:
joint_msg = self.arm.GetArmJointMsgs() joint_msg = self.arm.GetArmJointMsgs()
joint_state = getattr(joint_msg, "joint_state", None) joint_state = getattr(joint_msg, "joint_state", None)
@ -216,51 +137,6 @@ class PiperFollower(Robot):
gripper_msg = self.arm.GetArmGripperMsgs() gripper_msg = self.arm.GetArmGripperMsgs()
gripper_state = getattr(gripper_msg, "gripper_state", None) gripper_state = getattr(gripper_msg, "gripper_state", None)
obs["gripper.pos"] = abs(milli_to_unit(getattr(gripper_state, "grippers_angle", 0))) 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:
if self._teleop_send_only_mode:
raise RuntimeError(
f"{self} was connected in teleop send-only mode, so follower observations are unavailable."
)
obs: dict[str, float] = self._read_raw_observation()
for cam_key, cam in self.cameras.items(): for cam_key, cam in self.cameras.items():
obs[cam_key] = cam.async_read() obs[cam_key] = cam.async_read()
@ -268,22 +144,12 @@ class PiperFollower(Robot):
@check_if_not_connected @check_if_not_connected
def send_action(self, action: RobotAction) -> RobotAction: def send_action(self, action: RobotAction) -> RobotAction:
if not self.is_calibrated and not self._use_uncalibrated_passthrough():
raise RuntimeError(
f"{self} is not calibrated. Run `lerobot-calibrate --robot.type={self.config.type} --robot.id={self.id}` first."
)
self._refresh_motion_mode_if_needed()
sent_action: dict[str, float] = {} sent_action: dict[str, float] = {}
joint_keys = PIPER_JOINT_ACTION_KEYS joint_keys = PIPER_JOINT_ACTION_KEYS
has_all_joints = all(key in action for key in joint_keys) has_all_joints = all(key in action for key in joint_keys)
if has_all_joints: if has_all_joints:
if self._use_uncalibrated_passthrough(): joint_targets = [action[key] for key in joint_keys]
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] joint_commands = [unit_to_milli(value) for value in joint_targets]
self.arm.JointCtrl(*joint_commands) self.arm.JointCtrl(*joint_commands)
sent_action.update( sent_action.update(
@ -293,11 +159,7 @@ class PiperFollower(Robot):
logger.debug("Ignoring partial Piper joint action. Need all six joint keys to send command.") 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.config.sync_gripper and "gripper.pos" in action:
if self._use_uncalibrated_passthrough(): gripper_pos_raw = unit_to_milli(action["gripper.pos"])
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( self.arm.GripperCtrl(
gripper_pos_raw, gripper_pos_raw,
self.config.gripper_effort_default, self.config.gripper_effort_default,
@ -316,8 +178,7 @@ class PiperFollower(Robot):
finally: finally:
self.arm.DisconnectPort() self.arm.DisconnectPort()
for cam in self.cameras.values(): for cam in self.cameras.values():
if cam.is_connected: cam.disconnect()
cam.disconnect()
self._is_connected = False self._is_connected = False
logger.info("%s disconnected.", self) logger.info("%s disconnected.", self)

查看文件

@ -13,7 +13,7 @@
# limitations under the License. # limitations under the License.
""" """
Setup and debug CAN interfaces for Damiao motors (e.g., OpenArms). Setup SocketCAN interfaces and debug Damiao motors (e.g., OpenArms).
Examples: Examples:
@ -22,6 +22,11 @@ Setup CAN interfaces with CAN FD:
lerobot-setup-can --mode=setup --interfaces=can0,can1,can2,can3 lerobot-setup-can --mode=setup --interfaces=can0,can1,can2,can3
``` ```
Setup PiPER interfaces by USB-CAN serial number:
```shell
lerobot-setup-can --mode=setup --usb_can_serials=<SERIAL_1>,<SERIAL_2>
```
Test motors on a single interface: Test motors on a single interface:
```shell ```shell
lerobot-setup-can --mode=test --interfaces=can0 lerobot-setup-can --mode=test --interfaces=can0
@ -46,6 +51,7 @@ from dataclasses import dataclass, field
import draccus import draccus
from lerobot.utils.import_utils import _can_available from lerobot.utils.import_utils import _can_available
from lerobot.utils.piper_sdk import resolve_piper_can_interface
MOTOR_NAMES = { MOTOR_NAMES = {
0x01: "joint_1", 0x01: "joint_1",
@ -63,6 +69,7 @@ MOTOR_NAMES = {
class CANSetupConfig: class CANSetupConfig:
mode: str = "test" mode: str = "test"
interfaces: str = "can0" # Comma-separated, e.g. "can0,can1,can2,can3" interfaces: str = "can0" # Comma-separated, e.g. "can0,can1,can2,can3"
usb_can_serials: str | None = None
bitrate: int = 1000000 bitrate: int = 1000000
data_bitrate: int = 5000000 data_bitrate: int = 5000000
use_fd: bool = True use_fd: bool = True
@ -71,6 +78,13 @@ class CANSetupConfig:
speed_iterations: int = 100 speed_iterations: int = 100
def get_interfaces(self) -> list[str]: def get_interfaces(self) -> list[str]:
if self.usb_can_serials is not None:
if self.mode != "setup":
raise ValueError("`usb_can_serials` is only supported with `mode=setup`.")
serials = [value.strip() for value in self.usb_can_serials.split(",") if value.strip()]
if not serials:
raise ValueError("`usb_can_serials` must contain at least one serial number.")
return [resolve_piper_can_interface(serial) for serial in serials]
return [i.strip() for i in self.interfaces.split(",") if i.strip()] return [i.strip() for i in self.interfaces.split(",") if i.strip()]
@ -121,13 +135,15 @@ def setup_interface(interface: str, bitrate: int, data_bitrate: int, use_fd: boo
def setup_interface_with_fallback(interface: str, cfg: CANSetupConfig) -> tuple[bool, bool | None, str]: def setup_interface_with_fallback(interface: str, cfg: CANSetupConfig) -> tuple[bool, bool | None, str]:
"""Try primary mode first, then fallback mode. Returns (success, used_fd, summary).""" """Try primary mode first, then fallback mode. Returns (success, used_fd, summary)."""
attempts = [cfg.use_fd, not cfg.use_fd] is_piper = cfg.usb_can_serials is not None
attempts = [False] if is_piper else [cfg.use_fd, not cfg.use_fd]
bitrate = 1000000 if is_piper else cfg.bitrate
labels = {True: "CAN FD", False: "CAN 2.0"} labels = {True: "CAN FD", False: "CAN 2.0"}
attempt_msgs: list[str] = [] attempt_msgs: list[str] = []
for idx, use_fd in enumerate(attempts, start=1): for idx, use_fd in enumerate(attempts, start=1):
print(f" Attempt {idx}/{len(attempts)}: {labels[use_fd]}") print(f" Attempt {idx}/{len(attempts)}: {labels[use_fd]}")
ok, err = setup_interface(interface, cfg.bitrate, cfg.data_bitrate, use_fd) ok, err = setup_interface(interface, bitrate, cfg.data_bitrate, use_fd)
if ok: if ok:
is_up, status, is_fd_iface = check_interface_status(interface) is_up, status, is_fd_iface = check_interface_status(interface)
if is_up: if is_up:
@ -305,13 +321,16 @@ def speed_test(cfg: CANSetupConfig, interface: str):
def run_setup(cfg: CANSetupConfig) -> bool: def run_setup(cfg: CANSetupConfig) -> bool:
"""Setup CAN interfaces.""" """Setup CAN interfaces."""
use_fd = False if cfg.usb_can_serials is not None else cfg.use_fd
bitrate = 1000000 if cfg.usb_can_serials is not None else cfg.bitrate
print("=" * 50) print("=" * 50)
print("CAN Interface Setup") print("CAN Interface Setup")
print("=" * 50) print("=" * 50)
print(f"Primary mode: {'CAN FD' if cfg.use_fd else 'CAN 2.0'}") print(f"Primary mode: {'CAN FD' if use_fd else 'CAN 2.0'}")
print(f"Fallback mode: {'CAN 2.0' if cfg.use_fd else 'CAN FD'}") if cfg.usb_can_serials is None:
print(f"Bitrate: {cfg.bitrate / 1_000_000:.1f} Mbps") print(f"Fallback mode: {'CAN 2.0' if use_fd else 'CAN FD'}")
if cfg.use_fd: print(f"Bitrate: {bitrate / 1_000_000:.1f} Mbps")
if use_fd:
print(f"Data bitrate: {cfg.data_bitrate / 1_000_000:.1f} Mbps") print(f"Data bitrate: {cfg.data_bitrate / 1_000_000:.1f} Mbps")
print() print()
@ -351,9 +370,10 @@ def run_setup(cfg: CANSetupConfig) -> bool:
print() print()
print("✓ Setup succeeded") print("✓ Setup succeeded")
print() if cfg.usb_can_serials is None:
print("Next: Test motors with:") print()
print(f" lerobot-setup-can --mode=test --interfaces {','.join(interfaces)}") print("Next: Test motors with:")
print(f" lerobot-setup-can --mode=test --interfaces {','.join(interfaces)}")
return True return True

查看文件

@ -173,10 +173,7 @@ def record_loop(
return return
if not hasattr(teleop_arm_for_mode_switch, "set_manual_control"): if not hasattr(teleop_arm_for_mode_switch, "set_manual_control"):
return return
try: teleop_arm_for_mode_switch.set_manual_control(enabled)
teleop_arm_for_mode_switch.set_manual_control(enabled)
except Exception:
logging.exception("Failed to switch teleop manual-control mode to %s", enabled)
if policy is None: if policy is None:
# During reset/teleop-only loops keep leader backdrivable for manual dragging. # During reset/teleop-only loops keep leader backdrivable for manual dragging.
@ -251,12 +248,12 @@ def record_loop(
events["toggle_intervention"] = False events["toggle_intervention"] = False
if intervention_enabled: if intervention_enabled:
if intervention_state == INTERVENTION_STATE_POLICY: if intervention_state == INTERVENTION_STATE_POLICY:
intervention_state = INTERVENTION_STATE_ACTIVE
set_teleop_manual_control(True) set_teleop_manual_control(True)
intervention_state = INTERVENTION_STATE_ACTIVE
logging.info("Intervention enabled (S1): teleop actions now override policy execution.") logging.info("Intervention enabled (S1): teleop actions now override policy execution.")
else: else:
intervention_state = INTERVENTION_STATE_RELEASE
set_teleop_manual_control(False) set_teleop_manual_control(False)
intervention_state = INTERVENTION_STATE_RELEASE
if policy is not None and preprocessor is not None and postprocessor is not None: if policy is not None and preprocessor is not None and postprocessor is not None:
policy.reset() policy.reset()
preprocessor.reset() preprocessor.reset()

查看文件

@ -14,9 +14,9 @@
# See the License for the specific language governing permissions and # See the License for the specific language governing permissions and
# limitations under the License. # limitations under the License.
import logging
import multiprocessing as mp import multiprocessing as mp
import traceback import traceback
from contextlib import suppress
from functools import cached_property from functools import cached_property
from typing import Any from typing import Any
@ -27,14 +27,12 @@ from lerobot.teleoperators.piper_leader import (
PiperXLeader, PiperXLeader,
PiperXLeaderConfig, PiperXLeaderConfig,
) )
from lerobot.utils.piper_sdk import PIPER_ACTION_KEYS
from lerobot.utils.decorators import check_if_already_connected, check_if_not_connected from lerobot.utils.decorators import check_if_already_connected, check_if_not_connected
from lerobot.utils.piper_sdk import PIPER_ACTION_KEYS
from ..teleoperator import Teleoperator from ..teleoperator import Teleoperator
from .config_bi_piper_leader import BiPiperLeaderConfig, BiPiperXLeaderConfig from .config_bi_piper_leader import BiPiperLeaderConfig, BiPiperXLeaderConfig
logger = logging.getLogger(__name__)
def _bi_piper_leader_worker(conn, arm_cls, arm_config) -> None: def _bi_piper_leader_worker(conn, arm_cls, arm_config) -> None:
arm = arm_cls(arm_config) arm = arm_cls(arm_config)
@ -46,12 +44,9 @@ def _bi_piper_leader_worker(conn, arm_cls, arm_config) -> None:
break break
try: try:
if command == "__get_is_calibrated__": args = request.get("args", ())
result = arm.is_calibrated kwargs = request.get("kwargs", {})
else: result = getattr(arm, command)(*args, **kwargs)
args = request.get("args", ())
kwargs = request.get("kwargs", {})
result = getattr(arm, command)(*args, **kwargs)
conn.send({"ok": True, "result": result}) conn.send({"ok": True, "result": result})
except Exception as exc: # noqa: BLE001 except Exception as exc: # noqa: BLE001
conn.send( conn.send(
@ -91,12 +86,6 @@ class _PiperLeaderProcessProxy:
def is_connected(self) -> bool: def is_connected(self) -> bool:
return self._is_connected return self._is_connected
@property
def is_calibrated(self) -> bool:
if self._process is None:
return False
return bool(self._call("__get_is_calibrated__"))
def _ensure_process(self) -> None: def _ensure_process(self) -> None:
if self._process is not None and self._process.is_alive(): if self._process is not None and self._process.is_alive():
return return
@ -123,17 +112,14 @@ class _PiperLeaderProcessProxy:
f"{response['traceback']}" f"{response['traceback']}"
) )
def connect(self, calibrate: bool = True) -> None: def connect(self) -> None:
try: try:
self._call("connect", calibrate) self._call("connect")
self._is_connected = True self._is_connected = True
except Exception: except Exception:
self.disconnect() self.disconnect()
raise raise
def calibrate(self) -> None:
self._call("calibrate")
def configure(self) -> None: def configure(self) -> None:
self._call("configure") self._call("configure")
@ -155,19 +141,13 @@ class _PiperLeaderProcessProxy:
return return
if self._parent_conn is not None: if self._parent_conn is not None:
try: with suppress(Exception):
if self._is_connected: if self._is_connected:
self._call("disconnect") self._call("disconnect")
except Exception: with suppress(Exception):
pass
try:
self._parent_conn.send({"command": "__close__"}) self._parent_conn.send({"command": "__close__"})
except Exception: with suppress(Exception):
pass
try:
self._parent_conn.close() self._parent_conn.close()
except Exception:
pass
if self._process.is_alive(): if self._process.is_alive():
self._process.join(timeout=2.0) self._process.join(timeout=2.0)
@ -192,8 +172,6 @@ class BiPiperLeader(Teleoperator):
"log_level", "log_level",
"startup_sleep_s", "startup_sleep_s",
"manual_control", "manual_control",
"prefer_ctrl_messages",
"fallback_to_feedback",
"sync_gripper", "sync_gripper",
"gripper_effort_default", "gripper_effort_default",
"gripper_status_code", "gripper_status_code",
@ -201,25 +179,16 @@ class BiPiperLeader(Teleoperator):
"command_high_follow", "command_high_follow",
"mode_refresh_interval_s", "mode_refresh_interval_s",
"enable_timeout_s", "enable_timeout_s",
"gravity_comp_control_hz",
"gravity_comp_tx_ratio",
"gravity_comp_torque_limit",
"gravity_comp_mit_kp",
"gravity_comp_mit_kd",
"gravity_comp_base_rpy_deg",
"calibration_scale",
"require_calibration",
"disable_on_disconnect", "disable_on_disconnect",
) )
def _build_arm_config(self, arm_config_cls, side_cfg, side: str): def _build_arm_config(self, arm_config_cls, side_cfg, side: str):
kwargs = {name: getattr(side_cfg, name) for name in self._side_field_names} kwargs = {name: getattr(side_cfg, name) for name in self._side_field_names}
kwargs["id"] = f"{self.config.id}_{side}" if self.config.id else None kwargs["id"] = f"{self.config.id}_{side}" if self.config.id else None
kwargs["calibration_dir"] = self.config.calibration_dir
return arm_config_cls(**kwargs) return arm_config_cls(**kwargs)
def __init__(self, config: BiPiperLeaderConfig | BiPiperXLeaderConfig): def __init__(self, config: BiPiperLeaderConfig | BiPiperXLeaderConfig):
super().__init__(config) self.id = config.id
self.config = config self.config = config
self._use_process_isolation = config.process_isolation self._use_process_isolation = config.process_isolation
@ -259,16 +228,16 @@ class BiPiperLeader(Teleoperator):
@check_if_already_connected @check_if_already_connected
def connect(self, calibrate: bool = True) -> None: def connect(self, calibrate: bool = True) -> None:
self.left_arm.connect(calibrate) del calibrate
self.right_arm.connect(calibrate) self.left_arm.connect()
self.right_arm.connect()
@property @property
def is_calibrated(self) -> bool: def is_calibrated(self) -> bool:
return self.left_arm.is_calibrated and self.right_arm.is_calibrated return True
def calibrate(self) -> None: def calibrate(self) -> None:
self.left_arm.calibrate() pass
self.right_arm.calibrate()
def configure(self) -> None: def configure(self) -> None:
self.left_arm.configure() self.left_arm.configure()

查看文件

@ -18,15 +18,12 @@ from dataclasses import dataclass
from ..config import TeleoperatorConfig from ..config import TeleoperatorConfig
DEFAULT_PIPER_GRAVITY_COMP_TX_RATIO = (0.2, 0.2, 0.2, 0.2, 0.2, 0.2)
DEFAULT_PIPERX_GRAVITY_COMP_TX_RATIO = (1.0, 1.0, 1.0, 1.0, 1.0, 1.0)
@dataclass @dataclass
class PiperLeaderConfigBase: class PiperLeaderConfigBase:
"""Configuration for a Piper leader arm used as teleoperator.""" """Configuration for a Piper leader arm used as teleoperator."""
# CAN interface name (e.g. "can1") # USB-CAN adapter ID_SERIAL_SHORT
port: str port: str
# Piper SDK connection options # Piper SDK connection options
@ -35,13 +32,9 @@ class PiperLeaderConfigBase:
log_level: str = "WARNING" log_level: str = "WARNING"
startup_sleep_s: float = 0.1 startup_sleep_s: float = 0.1
# Manual backdrivable mode for human teleop # Initial role: True uses hardware leader drag (0xFA); False accepts policy commands (0xFC).
manual_control: bool = True 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 # Gripper handling
sync_gripper: bool = True sync_gripper: bool = True
gripper_effort_default: int = 1000 gripper_effort_default: int = 1000
@ -53,20 +46,6 @@ class PiperLeaderConfigBase:
mode_refresh_interval_s: float = 1.0 mode_refresh_interval_s: float = 1.0
enable_timeout_s: float = 3.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] = DEFAULT_PIPER_GRAVITY_COMP_TX_RATIO
gravity_comp_torque_limit: float = 8.0
gravity_comp_mit_kp: float = 0.0
gravity_comp_mit_kd: float = 0.0
gravity_comp_base_rpy_deg: tuple[float, float, float] = (0.0, 0.0, 0.0)
# Calibration precision:
# homing_offset/range_min/range_max are stored as "degree * calibration_scale".
calibration_scale: int = 1000
# Whether calibration is required on connect when no calibration file exists
require_calibration: bool = True
# Safety behavior on disconnect # Safety behavior on disconnect
disable_on_disconnect: bool = False disable_on_disconnect: bool = False
@ -78,18 +57,6 @@ def _validate_piper_leader_config(config: PiperLeaderConfigBase) -> None:
raise ValueError("`mode_refresh_interval_s` must be >= 0.") raise ValueError("`mode_refresh_interval_s` must be >= 0.")
if config.enable_timeout_s < 0: if config.enable_timeout_s < 0:
raise ValueError("`enable_timeout_s` must be >= 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: if config.startup_sleep_s < 0:
raise ValueError("`startup_sleep_s` must be >= 0.") raise ValueError("`startup_sleep_s` must be >= 0.")
if not (0 <= config.gripper_effort_default <= 5000): if not (0 <= config.gripper_effort_default <= 5000):
@ -107,7 +74,7 @@ class PiperLeaderConfig(TeleoperatorConfig, PiperLeaderConfigBase):
@dataclass @dataclass
class PiperXLeaderConfigBase(PiperLeaderConfigBase): class PiperXLeaderConfigBase(PiperLeaderConfigBase):
gravity_comp_tx_ratio: tuple[float, float, float, float, float, float] = DEFAULT_PIPERX_GRAVITY_COMP_TX_RATIO pass
@TeleoperatorConfig.register_subclass("piperx_leader") @TeleoperatorConfig.register_subclass("piperx_leader")

查看文件

@ -1,192 +0,0 @@
#!/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,51 +17,29 @@
import logging import logging
import time import time
from functools import cached_property from functools import cached_property
from importlib import resources
from typing import Any from typing import Any
from lerobot.motors import MotorCalibration
from lerobot.processor import RobotAction from lerobot.processor import RobotAction
from lerobot.utils.decorators import check_if_already_connected, check_if_not_connected from lerobot.utils.decorators import check_if_already_connected, check_if_not_connected
from lerobot.utils.piper_sdk import ( from lerobot.utils.piper_sdk import (
PIPER_ACTION_KEYS, PIPER_ACTION_KEYS,
PIPER_JOINT_ACTION_KEYS, PIPER_JOINT_ACTION_KEYS,
PIPER_JOINT_NAMES, PIPER_JOINT_NAMES,
PIPER_ROLE_FOLLOWER,
PIPER_ROLE_LEADER,
get_piper_sdk, get_piper_sdk,
guard_piper_ctrl_mode_on_connect,
milli_to_unit, milli_to_unit,
parse_piper_log_level, parse_piper_log_level,
resolve_piper_can_interface,
set_piper_role,
unit_to_milli, unit_to_milli,
wait_enable_piper, wait_enable_piper,
) )
from lerobot.utils.utils import enter_pressed, move_cursor_up
from ..teleoperator import Teleoperator from ..teleoperator import Teleoperator
from .config_piper_leader import PiperLeaderConfig, PiperXLeaderConfig from .config_piper_leader import PiperLeaderConfig, PiperXLeaderConfig
from .gravity_compensation import PiperGravityCompensationLoop
logger = logging.getLogger(__name__) 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"
MANUAL_CONTROL_FRESH_FEEDBACK_TIMEOUT_S = 0.03
MANUAL_CONTROL_FRESH_FEEDBACK_POLL_S = 0.001
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): class PiperLeader(Teleoperator):
@ -69,21 +47,17 @@ class PiperLeader(Teleoperator):
config_class = PiperLeaderConfig config_class = PiperLeaderConfig
name = "piper_leader" name = "piper_leader"
gravity_comp_urdf_relpath = DEFAULT_PIPER_GRAVITY_URDF
def __init__(self, config: PiperLeaderConfig | PiperXLeaderConfig): def __init__(self, config: PiperLeaderConfig | PiperXLeaderConfig):
super().__init__(config) self.id = config.id
self.config = config self.config = config
self._is_connected = False self._is_connected = False
self._manual_control_enabled: bool | None = None self._manual_control_enabled: bool | None = None
self._last_mode_refresh_t = 0.0 self._last_mode_refresh_t = 0.0
self._gravity_comp_loop: PiperGravityCompensationLoop | None = None
self._last_feedback_joint_timestamp = 0.0
self._last_feedback_gripper_timestamp = 0.0
interface_cls, _ = get_piper_sdk() interface_cls, _ = get_piper_sdk()
self.arm = interface_cls( self.arm = interface_cls(
can_name=self.config.port, can_name=resolve_piper_can_interface(self.config.port),
judge_flag=self.config.judge_flag, judge_flag=self.config.judge_flag,
can_auto_init=self.config.can_auto_init, can_auto_init=self.config.can_auto_init,
logger_level=parse_piper_log_level(self.config.log_level), logger_level=parse_piper_log_level(self.config.log_level),
@ -103,78 +77,29 @@ class PiperLeader(Teleoperator):
@check_if_already_connected @check_if_already_connected
def connect(self, calibrate: bool = True) -> None: def connect(self, calibrate: bool = True) -> None:
del calibrate
self.arm.ConnectPort() self.arm.ConnectPort()
if self.config.startup_sleep_s > 0: if self.config.startup_sleep_s > 0:
time.sleep(self.config.startup_sleep_s) 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 self._is_connected = True
# Recompute control mode on every fresh connection.
self._manual_control_enabled = None self._manual_control_enabled = None
self._last_feedback_joint_timestamp = 0.0
self._last_feedback_gripper_timestamp = 0.0
try: try:
self.configure() self.configure()
if not self.is_calibrated and calibrate and self.config.require_calibration:
logger.info(
"No piper-leader calibration file found for '%s'. Running lerobot-calibrate flow.",
self.id,
)
self.calibrate()
except Exception: except Exception:
self._stop_gravity_comp_loop_if_needed()
self.arm.DisconnectPort() self.arm.DisconnectPort()
self._is_connected = False self._is_connected = False
self._manual_control_enabled = None
raise raise
logger.info("%s connected.", self) logger.info("%s connected.", self)
def _use_uncalibrated_passthrough(self) -> bool:
return not self.is_calibrated and not self.config.require_calibration
@property @property
def is_calibrated(self) -> bool: 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 return True
def calibrate(self) -> None: def calibrate(self) -> None:
if self.calibration and self.is_calibrated: pass
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: def _send_command_mode(self) -> None:
mit_mode = 0xAD if self.config.command_high_follow else 0x00 mit_mode = 0xAD if self.config.command_high_follow else 0x00
@ -189,9 +114,6 @@ class PiperLeader(Teleoperator):
if now - self._last_mode_refresh_t >= interval_s: if now - self._last_mode_refresh_t >= interval_s:
self._send_command_mode() self._send_command_mode()
def _wait_enable(self, timeout_s: float) -> bool:
return wait_enable_piper(self.arm, timeout_s)
def _send_gripper_ctrl(self, gripper_pos_raw: int, enabled: bool) -> None: def _send_gripper_ctrl(self, gripper_pos_raw: int, enabled: bool) -> None:
self.arm.GripperCtrl( self.arm.GripperCtrl(
gripper_pos_raw, gripper_pos_raw,
@ -213,249 +135,102 @@ class PiperLeader(Teleoperator):
def set_manual_control(self, enabled: bool) -> None: def set_manual_control(self, enabled: bool) -> None:
if not self._is_connected: if not self._is_connected:
raise RuntimeError(f"{self} is not connected.")
if enabled == self._manual_control_enabled:
return return
if enabled and self._manual_control_enabled is not True:
if not self._wait_enable(self.config.enable_timeout_s): self._manual_control_enabled = None
logger.warning( if enabled:
"Piper leader did not report enabled state before entering gravity compensation." set_piper_role(self.arm, PIPER_ROLE_LEADER)
) else:
if self.config.sync_gripper: set_piper_role(self.arm, PIPER_ROLE_FOLLOWER)
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() self._send_command_mode()
if not self._wait_enable(self.config.enable_timeout_s): if not wait_enable_piper(self.arm, self.config.enable_timeout_s):
logger.warning("Piper leader did not report enabled state before timeout.") raise RuntimeError(
f"[{self.config.port}] Piper leader did not enable after switching to follower role."
)
if self.config.sync_gripper: if self.config.sync_gripper:
self._set_gripper_enabled(True) self._set_gripper_enabled(True)
self._manual_control_enabled = False
def _ensure_gravity_comp_loop(self) -> PiperGravityCompensationLoop: self._manual_control_enabled = enabled
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: def configure(self) -> None:
self.set_manual_control(self.config.manual_control) self.set_manual_control(self.config.manual_control)
def _read_joint_from_ctrl(self) -> dict[str, float] | None: def _read_joint_from_ctrl(self) -> dict[str, float] | None:
joint_ctrl_msg = self.arm.GetArmJointCtrl() message = self.arm.GetArmJointCtrl()
if getattr(joint_ctrl_msg, "time_stamp", 0.0) <= 0: if message.time_stamp <= 0:
return None return None
joint_ctrl = getattr(joint_ctrl_msg, "joint_ctrl", None)
return { return {
f"{joint_name}.pos": milli_to_unit(getattr(joint_ctrl, joint_name, 0)) f"{joint_name}.pos": milli_to_unit(getattr(message.joint_ctrl, joint_name))
for joint_name in PIPER_JOINT_NAMES for joint_name in PIPER_JOINT_NAMES
} }
def _read_joint_from_feedback(self) -> dict[str, float] | None: def _read_joint_from_feedback(self) -> dict[str, float] | None:
joint_msg = self.arm.GetArmJointMsgs() message = self.arm.GetArmJointMsgs()
joint_state = getattr(joint_msg, "joint_state", None) if message.time_stamp <= 0:
if joint_state is None:
return None return None
return { return {
f"{joint_name}.pos": milli_to_unit(getattr(joint_state, joint_name, 0)) f"{joint_name}.pos": milli_to_unit(getattr(message.joint_state, joint_name))
for joint_name in PIPER_JOINT_NAMES for joint_name in PIPER_JOINT_NAMES
} }
def _read_gripper_from_ctrl(self) -> float | None: def _read_gripper_from_ctrl(self) -> float | None:
gripper_ctrl_msg = self.arm.GetArmGripperCtrl() message = self.arm.GetArmGripperCtrl()
if getattr(gripper_ctrl_msg, "time_stamp", 0.0) <= 0: if message.time_stamp <= 0:
return None return None
gripper_ctrl = getattr(gripper_ctrl_msg, "gripper_ctrl", None) return abs(milli_to_unit(message.gripper_ctrl.grippers_angle))
return abs(milli_to_unit(getattr(gripper_ctrl, "grippers_angle", 0)))
def _read_gripper_from_feedback(self) -> float | None: def _read_gripper_from_feedback(self) -> float | None:
gripper_msg = self.arm.GetArmGripperMsgs() message = self.arm.GetArmGripperMsgs()
gripper_state = getattr(gripper_msg, "gripper_state", None) if message.time_stamp <= 0:
if gripper_state is None:
return None return None
return abs(milli_to_unit(getattr(gripper_state, "grippers_angle", 0))) return abs(milli_to_unit(message.gripper_state.grippers_angle))
def _wait_for_fresh_feedback_if_needed(self) -> None:
if self._manual_control_enabled is not True:
return
needs_fresh_gripper = self.config.sync_gripper and self.config.fallback_to_feedback
deadline = time.monotonic() + MANUAL_CONTROL_FRESH_FEEDBACK_TIMEOUT_S
while time.monotonic() < deadline:
joint_ts = float(getattr(self.arm.GetArmJointMsgs(), "time_stamp", 0.0) or 0.0)
has_fresh_joint = joint_ts > self._last_feedback_joint_timestamp
if not has_fresh_joint:
time.sleep(MANUAL_CONTROL_FRESH_FEEDBACK_POLL_S)
continue
if not needs_fresh_gripper:
return
gripper_ts = float(getattr(self.arm.GetArmGripperMsgs(), "time_stamp", 0.0) or 0.0)
has_fresh_gripper = gripper_ts > self._last_feedback_gripper_timestamp
if has_fresh_gripper:
return
time.sleep(MANUAL_CONTROL_FRESH_FEEDBACK_POLL_S)
def _read_raw_action(self) -> RobotAction: def _read_raw_action(self) -> RobotAction:
used_feedback_for_joints = False if self._manual_control_enabled is True:
action: dict[str, float] | None = None
# In manual-control teleop, the operator physically backdrives the leader arm while a
# background gravity-compensation thread emits MIT commands. In that mode, `GetArmJointCtrl`
# reflects transmitted control targets rather than the operator's live pose, which can lag
# or jump under dual-arm load. Prefer direct joint feedback so teleop follows the real arm.
prefer_feedback = self._manual_control_enabled is True
if prefer_feedback:
self._wait_for_fresh_feedback_if_needed()
action = self._read_joint_from_feedback()
used_feedback_for_joints = action is not None
self._last_feedback_joint_timestamp = float(
getattr(self.arm.GetArmJointMsgs(), "time_stamp", 0.0) or 0.0
)
if action is None and self.config.prefer_ctrl_messages:
action = self._read_joint_from_ctrl() action = self._read_joint_from_ctrl()
gripper_pos = self._read_gripper_from_ctrl() if self.config.sync_gripper else None
if action is None and self.config.fallback_to_feedback and not used_feedback_for_joints: elif self._manual_control_enabled is False:
action = self._read_joint_from_feedback() action = self._read_joint_from_feedback()
used_feedback_for_joints = action is not None gripper_pos = self._read_gripper_from_feedback() if self.config.sync_gripper else None
else:
raise RuntimeError(f"[{self.config.port}] Piper leader control mode is unknown.")
if action is None: if action is None:
action = {f"{joint_name}.pos": 0.0 for joint_name in PIPER_JOINT_NAMES} return {}
if self.config.sync_gripper:
use_ctrl_for_gripper = ( if gripper_pos is None:
self.config.prefer_ctrl_messages and not prefer_feedback and not used_feedback_for_joints return {}
) action["gripper.pos"] = gripper_pos
gripper_pos = self._read_gripper_from_ctrl() if use_ctrl_for_gripper else None else:
if gripper_pos is None and self.config.fallback_to_feedback:
gripper_pos = self._read_gripper_from_feedback()
self._last_feedback_gripper_timestamp = float(
getattr(self.arm.GetArmGripperMsgs(), "time_stamp", 0.0) or 0.0
)
action["gripper.pos"] = 0.0 if gripper_pos is None else gripper_pos
return action
def _to_calibration_units(self, angle_deg: float) -> int:
return int(round(angle_deg * self.config.calibration_scale))
def _from_calibration_units(self, value: int) -> float:
return float(value) / float(self.config.calibration_scale)
def _calibrated_to_offset(self, key: str, raw_deg: float) -> float:
cal = self.calibration[key]
min_deg = self._from_calibration_units(cal.range_min)
max_deg = self._from_calibration_units(cal.range_max)
home_deg = self._from_calibration_units(cal.homing_offset)
bounded = min(max_deg, max(min_deg, raw_deg))
centered = bounded - home_deg
return -centered if cal.drive_mode else centered
def _offset_to_calibrated(self, key: str, offset_deg: float) -> float:
cal = self.calibration[key]
min_deg = self._from_calibration_units(cal.range_min)
max_deg = self._from_calibration_units(cal.range_max)
home_deg = self._from_calibration_units(cal.homing_offset)
centered = -offset_deg if cal.drive_mode else offset_deg
target = home_deg + centered
return min(max_deg, max(min_deg, target))
def _record_ranges_of_motion(self) -> tuple[dict[str, float], dict[str, float]]:
current = self._read_raw_action()
mins = current.copy()
maxes = current.copy()
while True:
current = self._read_raw_action()
mins = {key: min(mins[key], current[key]) for key in PIPER_CALIB_KEYS}
maxes = {key: max(maxes[key], current[key]) for key in PIPER_CALIB_KEYS}
print("\n-----------------------------")
print("JOINT | MIN | POS | MAX")
for key in PIPER_CALIB_KEYS:
print(f"{key:<11} | {mins[key]:>6.2f} | {current[key]:>6.2f} | {maxes[key]:>6.2f}")
if enter_pressed():
break
move_cursor_up(len(PIPER_CALIB_KEYS) + 3)
return mins, maxes
@check_if_not_connected
def get_action(self) -> RobotAction:
if not self.is_calibrated and not self._use_uncalibrated_passthrough():
raise RuntimeError(
f"{self} is not calibrated. Run `lerobot-calibrate --teleop.type={self.config.type} --teleop.id={self.id}` first."
)
raw_action = self._read_raw_action()
if self._use_uncalibrated_passthrough():
action: RobotAction = dict(raw_action)
if not self.config.sync_gripper:
action["gripper.pos"] = 0.0
return action
action: RobotAction = {
key: self._calibrated_to_offset(key, raw_action[key]) for key in PIPER_CALIB_KEYS
}
if not self.config.sync_gripper:
action["gripper.pos"] = 0.0 action["gripper.pos"] = 0.0
return action return action
@check_if_not_connected @check_if_not_connected
def send_feedback(self, feedback: dict[str, Any]) -> None: def get_action(self) -> RobotAction:
if not self.is_calibrated and not self._use_uncalibrated_passthrough(): return self._read_raw_action()
raise RuntimeError(
f"{self} is not calibrated. Run `lerobot-calibrate --teleop.type={self.config.type} --teleop.id={self.id}` first."
)
@check_if_not_connected
def send_feedback(self, feedback: dict[str, Any]) -> None:
self.set_manual_control(False) self.set_manual_control(False)
self._refresh_command_mode_if_needed() self._refresh_command_mode_if_needed()
joint_keys = PIPER_JOINT_ACTION_KEYS joint_keys = PIPER_JOINT_ACTION_KEYS
has_all_joints = all(key in feedback for key in joint_keys) has_all_joints = all(key in feedback for key in joint_keys)
if has_all_joints: if has_all_joints:
if self._use_uncalibrated_passthrough(): joint_targets = [feedback[key] for key in joint_keys]
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] joint_commands = [unit_to_milli(value) for value in joint_targets]
self.arm.JointCtrl(*joint_commands) self.arm.JointCtrl(*joint_commands)
if self.config.sync_gripper and "gripper.pos" in feedback: if self.config.sync_gripper and "gripper.pos" in feedback:
if self._use_uncalibrated_passthrough(): gripper_pos_raw = unit_to_milli(feedback["gripper.pos"])
gripper_target = feedback["gripper.pos"]
else:
gripper_target = self._offset_to_calibrated("gripper.pos", feedback["gripper.pos"])
gripper_pos_raw = unit_to_milli(gripper_target)
self._send_gripper_ctrl(gripper_pos_raw, enabled=True) self._send_gripper_ctrl(gripper_pos_raw, enabled=True)
@check_if_not_connected @check_if_not_connected
def disconnect(self) -> None: def disconnect(self) -> None:
try: try:
self._stop_gravity_comp_loop_if_needed() self.set_manual_control(True)
if self.config.disable_on_disconnect: if self.config.disable_on_disconnect:
self.arm.DisableArm(7) self.arm.DisableArm(7)
finally: finally:
@ -468,4 +243,3 @@ class PiperLeader(Teleoperator):
class PiperXLeader(PiperLeader): class PiperXLeader(PiperLeader):
config_class = PiperXLeaderConfig config_class = PiperXLeaderConfig
name = "piperx_leader" name = "piperx_leader"
gravity_comp_urdf_relpath = DEFAULT_PIPERX_GRAVITY_URDF

查看文件

@ -16,8 +16,10 @@
from __future__ import annotations from __future__ import annotations
import subprocess
import time import time
from functools import lru_cache from functools import lru_cache
from pathlib import Path
from typing import Any from typing import Any
PIPER_JOINT_NAMES = ( PIPER_JOINT_NAMES = (
@ -30,8 +32,33 @@ PIPER_JOINT_NAMES = (
) )
PIPER_JOINT_ACTION_KEYS = tuple(f"{joint}.pos" for joint in 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_ACTION_KEYS = PIPER_JOINT_ACTION_KEYS + ("gripper.pos",)
PIPER_CTRL_MODE_TEACH = 0x02 PIPER_ROLE_LEADER = 0xFA
PIPER_CTRL_MODE_LINKAGE_TEACH_INPUT = 0x06 PIPER_ROLE_FOLLOWER = 0xFC
_SYS_CLASS_NET = Path("/sys/class/net")
def resolve_piper_can_interface(serial_number: str) -> str:
"""Return the SocketCAN interface for a USB-CAN adapter serial number."""
for interface_path in _SYS_CLASS_NET.iterdir():
try:
if (interface_path / "type").read_text().strip() != "280":
continue
except OSError:
continue
try:
result = subprocess.run( # nosec B607
["udevadm", "info", "--query=property", f"--path={interface_path}"],
capture_output=True,
text=True,
)
except FileNotFoundError as exc:
raise RuntimeError("`udevadm` is required to find the PiPER USB-CAN adapter.") from exc
if f"ID_SERIAL_SHORT={serial_number}" in result.stdout.splitlines():
return interface_path.name
raise RuntimeError(f"No PiPER USB-CAN adapter found with serial '{serial_number}'.")
def milli_to_unit(value: float | int) -> float: def milli_to_unit(value: float | int) -> float:
@ -79,51 +106,5 @@ def wait_enable_piper(arm: Any, timeout_s: float, retry_interval_s: float = 0.2)
return False return False
def _piper_mode_to_int(value: Any) -> int | None: def set_piper_role(arm: Any, role: int) -> None:
if isinstance(value, bool): arm.MasterSlaveConfig(role, 0x00, 0x00, 0x00)
return int(value)
if isinstance(value, int):
return int(value)
if hasattr(value, "value") and isinstance(value.value, int):
return int(value.value)
if hasattr(value, "__int__"):
return int(value)
return None
def read_piper_ctrl_mode(arm: Any, timeout_s: float = 1.0, poll_s: float = 0.02) -> int | None:
deadline = time.monotonic() + max(0.0, timeout_s)
while time.monotonic() < deadline:
status_msg = arm.GetArmStatus()
if getattr(status_msg, "time_stamp", 0.0) > 0.0:
arm_status = getattr(status_msg, "arm_status", None)
if arm_status is not None:
mode = _piper_mode_to_int(getattr(arm_status, "ctrl_mode", None))
if mode is not None:
return mode
time.sleep(max(0.005, poll_s))
return None
def guard_piper_ctrl_mode_on_connect(
arm: Any,
*,
interface_name: str,
timeout_s: float = 0.5,
poll_s: float = 0.02,
settle_s: float = 0.05,
) -> None:
mode = read_piper_ctrl_mode(arm, timeout_s=timeout_s, poll_s=poll_s)
if mode is None:
raise RuntimeError(
f"[{interface_name}] could not read arm ctrl_mode within {timeout_s:.2f}s. "
"Check CAN wiring/power and rerun."
)
if mode in {PIPER_CTRL_MODE_TEACH, PIPER_CTRL_MODE_LINKAGE_TEACH_INPUT}:
arm.MasterSlaveConfig(0xFC, 0x00, 0x00, 0x00)
if settle_s > 0:
time.sleep(settle_s)
raise RuntimeError(
f"[{interface_name}] arm is in master/teaching role (ctrl_mode=0x{mode:02X}). "
"Follower role command (0xFC) has been sent. Power-cycle this arm, then retry."
)

查看文件

@ -12,14 +12,10 @@ def test_teleoperate_parses_bi_piperx_types():
"--robot.id=my_bi_piperx_follower", "--robot.id=my_bi_piperx_follower",
"--robot.left_arm_config.port=can0", "--robot.left_arm_config.port=can0",
"--robot.right_arm_config.port=can2", "--robot.right_arm_config.port=can2",
"--robot.left_arm_config.require_calibration=false",
"--robot.right_arm_config.require_calibration=false",
"--teleop.type=bi_piperx_leader", "--teleop.type=bi_piperx_leader",
"--teleop.id=my_bi_piperx_leader", "--teleop.id=my_bi_piperx_leader",
"--teleop.left_arm_config.port=can1", "--teleop.left_arm_config.port=can1",
"--teleop.right_arm_config.port=can3", "--teleop.right_arm_config.port=can3",
"--teleop.left_arm_config.require_calibration=false",
"--teleop.right_arm_config.require_calibration=false",
] ]
cfg = draccus.parse(config_class=TeleoperateConfig, config_path=None, args=args) cfg = draccus.parse(config_class=TeleoperateConfig, config_path=None, args=args)
assert cfg.robot.type == "bi_piperx_follower" assert cfg.robot.type == "bi_piperx_follower"
@ -32,14 +28,10 @@ def test_record_rejects_mixed_bi_piper_pair():
"--robot.id=my_bi_piper_follower", "--robot.id=my_bi_piper_follower",
"--robot.left_arm_config.port=can0", "--robot.left_arm_config.port=can0",
"--robot.right_arm_config.port=can2", "--robot.right_arm_config.port=can2",
"--robot.left_arm_config.require_calibration=false",
"--robot.right_arm_config.require_calibration=false",
"--teleop.type=bi_piperx_leader", "--teleop.type=bi_piperx_leader",
"--teleop.id=my_bi_piperx_leader", "--teleop.id=my_bi_piperx_leader",
"--teleop.left_arm_config.port=can1", "--teleop.left_arm_config.port=can1",
"--teleop.right_arm_config.port=can3", "--teleop.right_arm_config.port=can3",
"--teleop.left_arm_config.require_calibration=false",
"--teleop.right_arm_config.require_calibration=false",
"--dataset.repo_id=dummy/dummy", "--dataset.repo_id=dummy/dummy",
"--dataset.single_task=test", "--dataset.single_task=test",
"--dataset.num_episodes=1", "--dataset.num_episodes=1",
@ -62,8 +54,6 @@ def test_record_rejects_bimanual_piper_teleop_with_non_bimanual_robot():
"--teleop.id=my_bi_piperx_leader", "--teleop.id=my_bi_piperx_leader",
"--teleop.left_arm_config.port=can1", "--teleop.left_arm_config.port=can1",
"--teleop.right_arm_config.port=can3", "--teleop.right_arm_config.port=can3",
"--teleop.left_arm_config.require_calibration=false",
"--teleop.right_arm_config.require_calibration=false",
"--dataset.repo_id=dummy/dummy", "--dataset.repo_id=dummy/dummy",
"--dataset.single_task=test", "--dataset.single_task=test",
"--dataset.num_episodes=1", "--dataset.num_episodes=1",

查看文件

@ -1,7 +1,4 @@
# ruff: noqa: N802 # ruff: noqa: N802
import sys
import threading
import time
from types import SimpleNamespace from types import SimpleNamespace
import pytest import pytest
@ -10,7 +7,6 @@ import lerobot.robots.piper_follower.piper_follower as piper_follower_module
import lerobot.teleoperators.bi_piper_leader.bi_piper_leader as bi_piper_leader_module import lerobot.teleoperators.bi_piper_leader.bi_piper_leader as bi_piper_leader_module
import lerobot.teleoperators.piper_leader.piper_leader as piper_leader_module import lerobot.teleoperators.piper_leader.piper_leader as piper_leader_module
import lerobot.utils.piper_sdk as piper_sdk_utils import lerobot.utils.piper_sdk as piper_sdk_utils
from lerobot.motors import MotorCalibration
from lerobot.processor import make_default_processors from lerobot.processor import make_default_processors
from lerobot.robots.bi_piper_follower import ( from lerobot.robots.bi_piper_follower import (
BiPiperFollower, BiPiperFollower,
@ -26,6 +22,7 @@ from lerobot.robots.piper_follower import (
PiperXFollowerConfig, PiperXFollowerConfig,
) )
from lerobot.robots.utils import make_robot_from_config from lerobot.robots.utils import make_robot_from_config
from lerobot.scripts.lerobot_teleoperate import teleop_loop
from lerobot.teleoperators.bi_piper_leader import ( from lerobot.teleoperators.bi_piper_leader import (
BiPiperLeader, BiPiperLeader,
BiPiperLeaderConfig, BiPiperLeaderConfig,
@ -40,7 +37,6 @@ from lerobot.teleoperators.piper_leader import (
PiperXLeaderConfig, PiperXLeaderConfig,
PiperXLeaderConfigBase, PiperXLeaderConfigBase,
) )
from lerobot.scripts.lerobot_teleoperate import teleop_loop
from lerobot.teleoperators.utils import make_teleoperator_from_config from lerobot.teleoperators.utils import make_teleoperator_from_config
from lerobot.utils.piper_sdk import PIPER_ACTION_KEYS from lerobot.utils.piper_sdk import PIPER_ACTION_KEYS
@ -71,6 +67,7 @@ class FakePiperInterface:
self.disable_calls = 0 self.disable_calls = 0
self.is_enabled = False self.is_enabled = False
self.connect_calls = [] self.connect_calls = []
self.command_log = []
self._joint_ctrl = SimpleNamespace( self._joint_ctrl = SimpleNamespace(
time_stamp=1.0, time_stamp=1.0,
@ -134,14 +131,19 @@ class FakePiperInterface:
def MotionCtrl_2(self, *args): def MotionCtrl_2(self, *args):
self.mode_commands.append(args) self.mode_commands.append(args)
self.command_log.append(("motion_mode", args))
def MasterSlaveConfig(self, *args): def MasterSlaveConfig(self, *args):
self.role_commands.append(args) self.role_commands.append(args)
self.command_log.append(("role", args))
if args and args[0] == 0xFC: if args and args[0] == 0xFC:
self._arm_status.arm_status.ctrl_mode = 0x01 self._arm_status.arm_status.ctrl_mode = 0x01
elif args and args[0] == 0xFA:
self._arm_status.arm_status.ctrl_mode = 0x06
def EnablePiper(self): def EnablePiper(self):
self.enable_calls += 1 self.enable_calls += 1
self.command_log.append(("enable", ()))
self.is_enabled = True self.is_enabled = True
return True return True
@ -152,6 +154,7 @@ class FakePiperInterface:
def JointCtrl(self, *args): def JointCtrl(self, *args):
self.last_joint = args self.last_joint = args
self.command_log.append(("joint", args))
def JointMitCtrl(self, *args): def JointMitCtrl(self, *args):
self.joint_mit_calls.append(args) self.joint_mit_calls.append(args)
@ -159,6 +162,7 @@ class FakePiperInterface:
def GripperCtrl(self, *args): def GripperCtrl(self, *args):
self.last_gripper = args self.last_gripper = args
self.gripper_calls.append(args) self.gripper_calls.append(args)
self.command_log.append(("gripper", args))
def GetArmJointCtrl(self): def GetArmJointCtrl(self):
return self._joint_ctrl return self._joint_ctrl
@ -186,41 +190,21 @@ def patch_fake_sdk(monkeypatch):
monkeypatch.setattr(piper_sdk_utils, "get_piper_sdk", fake_loader) monkeypatch.setattr(piper_sdk_utils, "get_piper_sdk", fake_loader)
monkeypatch.setattr(piper_follower_module, "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) monkeypatch.setattr(piper_leader_module, "get_piper_sdk", fake_loader)
monkeypatch.setattr(piper_follower_module, "resolve_piper_can_interface", lambda serial: serial)
monkeypatch.setattr(piper_leader_module, "resolve_piper_can_interface", lambda serial: serial)
def make_identity_calibration():
return {
key: MotorCalibration(
id=idx,
drive_mode=0,
homing_offset=0,
range_min=-200000,
range_max=200000,
)
for idx, key in enumerate(PIPER_ACTION_KEYS)
}
def wait_until(predicate, timeout_s: float = 0.2, poll_s: float = 0.005) -> bool:
deadline = time.monotonic() + timeout_s
while time.monotonic() < deadline:
if predicate():
return True
time.sleep(poll_s)
return False
@pytest.mark.parametrize( @pytest.mark.parametrize(
("teleop_cfg", "robot_cfg", "teleop_cls", "robot_cls"), ("teleop_cfg", "robot_cfg", "teleop_cls", "robot_cls"),
[ [
( (
PiperLeaderConfig(port="can1", manual_control=False, sync_gripper=True), PiperLeaderConfig(port="can1", manual_control=True, sync_gripper=True),
PiperFollowerConfig(port="can0", sync_gripper=True), PiperFollowerConfig(port="can0", sync_gripper=True),
PiperLeader, PiperLeader,
PiperFollower, PiperFollower,
), ),
( (
PiperXLeaderConfig(port="can1", manual_control=False, sync_gripper=True), PiperXLeaderConfig(port="can1", manual_control=True, sync_gripper=True),
PiperXFollowerConfig(port="can0", sync_gripper=True), PiperXFollowerConfig(port="can0", sync_gripper=True),
PiperXLeader, PiperXLeader,
PiperXFollower, PiperXFollower,
@ -236,12 +220,11 @@ def test_piper_leader_follower_teleop_roundtrip(monkeypatch, teleop_cfg, robot_c
assert isinstance(teleop, teleop_cls) assert isinstance(teleop, teleop_cls)
assert isinstance(robot, robot_cls) assert isinstance(robot, robot_cls)
teleop.calibration = make_identity_calibration() teleop.connect()
robot.calibration = make_identity_calibration() robot.connect()
teleop.connect(calibrate=False)
robot.connect(calibrate=False)
try: try:
assert teleop.arm.role_commands[-1] == (0xFA, 0x00, 0x00, 0x00)
assert robot.arm.role_commands == [(0xFC, 0x00, 0x00, 0x00)]
action = teleop.get_action() action = teleop.get_action()
sent = robot.send_action(action) sent = robot.send_action(action)
obs = robot.get_observation() obs = robot.get_observation()
@ -259,6 +242,7 @@ def test_piper_leader_follower_teleop_roundtrip(monkeypatch, teleop_cfg, robot_c
assert obs["gripper.pos"] == 43.0 assert obs["gripper.pos"] == 43.0
teleop.send_feedback(action) teleop.send_feedback(action)
assert teleop.arm.role_commands[-1] == (0xFC, 0x00, 0x00, 0x00)
assert teleop.arm.last_joint == (10000, 20000, 30000, 40000, 50000, 60000) assert teleop.arm.last_joint == (10000, 20000, 30000, 40000, 50000, 60000)
assert teleop.arm.last_gripper == ( assert teleop.arm.last_gripper == (
42000, 42000,
@ -269,38 +253,49 @@ def test_piper_leader_follower_teleop_roundtrip(monkeypatch, teleop_cfg, robot_c
finally: finally:
teleop.disconnect() teleop.disconnect()
robot.disconnect() robot.disconnect()
assert teleop.arm.role_commands[-1] == (0xFA, 0x00, 0x00, 0x00)
def test_piper_requires_calibration(monkeypatch): def test_piper_does_not_initialize_calibration_state(monkeypatch):
patch_fake_sdk(monkeypatch) patch_fake_sdk(monkeypatch)
teleop = PiperLeader(PiperLeaderConfig(port="can1")) leader = PiperLeader(PiperLeaderConfig(port="can1"))
robot = PiperFollower(PiperFollowerConfig(port="can0")) follower = PiperFollower(PiperFollowerConfig(port="can0"))
bi_leader = BiPiperLeader(
BiPiperLeaderConfig(
left_arm_config=PiperLeaderConfigBase(port="can1"),
right_arm_config=PiperLeaderConfigBase(port="can3"),
process_isolation=False,
)
)
bi_follower = BiPiperFollower(
BiPiperFollowerConfig(
left_arm_config=PiperFollowerConfigBase(port="can0"),
right_arm_config=PiperFollowerConfigBase(port="can2"),
)
)
assert not teleop.is_calibrated devices = [
assert not robot.is_calibrated leader,
follower,
teleop.connect(calibrate=False) bi_leader,
robot.connect(calibrate=False) bi_leader.left_arm,
try: bi_leader.right_arm,
with pytest.raises(RuntimeError, match="is not calibrated"): bi_follower,
teleop.get_action() bi_follower.left_arm,
with pytest.raises(RuntimeError, match="is not calibrated"): bi_follower.right_arm,
robot.send_action({"joint_1.pos": 0.0}) ]
finally: assert all(not hasattr(device, "calibration") for device in devices)
teleop.disconnect() assert all(not hasattr(device, "calibration_fpath") for device in devices)
robot.disconnect()
def test_piper_leader_reconnect_reapplies_mode(monkeypatch): def test_piper_leader_reconnect_reapplies_mode(monkeypatch):
patch_fake_sdk(monkeypatch) patch_fake_sdk(monkeypatch)
teleop = PiperLeader(PiperLeaderConfig(port="can1", manual_control=False)) teleop = PiperLeader(PiperLeaderConfig(port="can1", manual_control=False))
teleop.calibration = make_identity_calibration() teleop.connect()
teleop.connect(calibrate=False)
teleop.disconnect() teleop.disconnect()
teleop.connect(calibrate=False) teleop.connect()
try: try:
assert teleop.arm.enable_calls == 2 assert teleop.arm.enable_calls == 2
finally: finally:
@ -337,51 +332,73 @@ def test_piper_follower_connect_rolls_back_connected_cameras(monkeypatch):
) )
robot = PiperFollower(PiperFollowerConfig(port="can0")) robot = PiperFollower(PiperFollowerConfig(port="can0"))
robot.calibration = make_identity_calibration()
with pytest.raises(RuntimeError, match="camera connect failed"): with pytest.raises(RuntimeError, match="camera connect failed"):
robot.connect(calibrate=False) robot.connect()
assert cam_ok.disconnect_calls == 1 assert cam_ok.disconnect_calls == 1
assert not cam_ok.is_connected assert not cam_ok.is_connected
assert not robot.is_connected assert not robot.is_connected
def test_piper_require_calibration_false_allows_uncalibrated_control(monkeypatch): def test_piper_leader_default_uses_hardware_leader_role(monkeypatch):
patch_fake_sdk(monkeypatch) patch_fake_sdk(monkeypatch)
teleop = PiperLeader(PiperLeaderConfig(port="can1"))
teleop = PiperLeader(PiperLeaderConfig(port="can1", require_calibration=False)) teleop.connect()
robot = PiperFollower(PiperFollowerConfig(port="can0", require_calibration=False))
teleop.connect(calibrate=False)
robot.connect(calibrate=False)
try: try:
assert teleop.arm.connect_calls[-1]["piper_init"] is False
assert teleop.arm.role_commands == [(0xFA, 0x00, 0x00, 0x00)]
assert teleop.arm.enable_calls == 0
assert teleop.arm.mode_commands == []
assert teleop.arm.gripper_calls == []
assert teleop.arm.joint_mit_calls == []
action = teleop.get_action() action = teleop.get_action()
sent = robot.send_action(action) assert action["joint_1.pos"] == 10.0
assert sent["joint_1.pos"] == 11.0 assert action["joint_2.pos"] == 20.0
assert sent["gripper.pos"] == 43.0 assert action["gripper.pos"] == 42.0
finally: finally:
teleop.disconnect() teleop.disconnect()
robot.disconnect()
def test_piper_gravity_comp_defaults_are_model_specific(): def test_piper_leader_switches_between_manual_and_policy_control(monkeypatch):
assert PiperLeaderConfig(port="can1").gravity_comp_tx_ratio == (0.2, 0.2, 0.2, 0.2, 0.2, 0.2)
assert PiperXLeaderConfig(port="can1").gravity_comp_tx_ratio == (1.0, 1.0, 1.0, 1.0, 1.0, 1.0)
def test_piper_leader_manual_control_prefers_feedback_state(monkeypatch):
patch_fake_sdk(monkeypatch) patch_fake_sdk(monkeypatch)
teleop = PiperLeader(PiperLeaderConfig(port="can1"))
teleop.connect()
try:
teleop.arm.command_log.clear()
teleop.set_manual_control(False)
assert teleop.arm.role_commands[-1] == (0xFC, 0x00, 0x00, 0x00)
assert teleop.arm.mode_commands[-1][:3] == (0x01, 0x01, teleop.config.command_speed_ratio)
assert teleop.arm.enable_calls == 1
assert [name for name, _ in teleop.arm.command_log[:3]] == ["role", "motion_mode", "enable"]
teleop = PiperLeader( role_count = len(teleop.arm.role_commands)
PiperLeaderConfig( action = {
port="can1", "joint_1.pos": 1.0,
manual_control=True, "joint_2.pos": 2.0,
require_calibration=False, "joint_3.pos": 3.0,
) "joint_4.pos": 4.0,
) "joint_5.pos": 5.0,
"joint_6.pos": 6.0,
"gripper.pos": 7.0,
}
teleop.send_feedback(action)
assert len(teleop.arm.role_commands) == role_count
assert teleop.arm.last_joint == (1000, 2000, 3000, 4000, 5000, 6000)
assert teleop.arm.last_gripper[0] == 7000
teleop.connect(calibrate=False) teleop.set_manual_control(True)
assert teleop.arm.role_commands[-1] == (0xFA, 0x00, 0x00, 0x00)
assert teleop.arm.joint_mit_calls == []
finally:
teleop.disconnect()
def test_piper_leader_policy_mode_reads_feedback(monkeypatch):
patch_fake_sdk(monkeypatch)
teleop = PiperLeader(PiperLeaderConfig(port="can1", manual_control=False))
teleop.connect()
try: try:
action = teleop.get_action() action = teleop.get_action()
assert action["joint_1.pos"] == 11.0 assert action["joint_1.pos"] == 11.0
@ -391,134 +408,51 @@ def test_piper_leader_manual_control_prefers_feedback_state(monkeypatch):
teleop.disconnect() teleop.disconnect()
def test_piper_leader_manual_control_waits_for_fresh_feedback(monkeypatch): def test_piper_leader_returns_empty_action_without_manual_frames(monkeypatch):
patch_fake_sdk(monkeypatch) patch_fake_sdk(monkeypatch)
teleop = PiperLeader(PiperLeaderConfig(port="can1"))
teleop = PiperLeader( teleop.connect()
PiperLeaderConfig(
port="can1",
manual_control=True,
require_calibration=False,
)
)
teleop.connect(calibrate=False)
try: try:
initial = teleop.get_action() teleop.arm._joint_ctrl.time_stamp = 0.0
assert initial["joint_1.pos"] == 11.0 assert teleop.get_action() == {}
def publish_fresh_feedback():
time.sleep(0.01)
teleop.arm._joint_state.time_stamp = 2.0
teleop.arm._joint_state.joint_state.joint_1 = 12000
teleop.arm._joint_state.joint_state.joint_2 = 22000
teleop.arm._joint_state.joint_state.joint_3 = 32000
teleop.arm._joint_state.joint_state.joint_4 = 42000
teleop.arm._joint_state.joint_state.joint_5 = 52000
teleop.arm._joint_state.joint_state.joint_6 = 62000
teleop.arm._gripper_state.time_stamp = 2.0
teleop.arm._gripper_state.gripper_state.grippers_angle = 44000
publisher = threading.Thread(target=publish_fresh_feedback, daemon=True)
publisher.start()
t0 = time.monotonic()
action = teleop.get_action()
publisher.join(timeout=0.1)
assert time.monotonic() - t0 >= 0.008
assert action["joint_1.pos"] == 12.0
assert action["joint_2.pos"] == 22.0
assert action["gripper.pos"] == 44.0
finally: finally:
teleop.disconnect() teleop.disconnect()
def test_piper_leader_manual_control_does_not_unblock_on_gripper_feedback_only(monkeypatch): def test_piper_leader_failed_policy_switch_restores_manual_role(monkeypatch):
patch_fake_sdk(monkeypatch) patch_fake_sdk(monkeypatch)
teleop = PiperLeader(PiperLeaderConfig(port="can1", enable_timeout_s=0.0))
teleop = PiperLeader( teleop.connect()
PiperLeaderConfig(
port="can1",
manual_control=True,
require_calibration=False,
)
)
teleop.connect(calibrate=False)
try: try:
initial = teleop.get_action() with pytest.raises(RuntimeError, match="did not enable"):
assert initial["joint_1.pos"] == 11.0 teleop.set_manual_control(False)
assert teleop._manual_control_enabled is True
def publish_feedback(): assert teleop.arm.role_commands[-1][0] == 0xFA
time.sleep(0.005)
teleop.arm._gripper_state.time_stamp = 2.0
teleop.arm._gripper_state.gripper_state.grippers_angle = 45000
time.sleep(0.01)
teleop.arm._joint_state.time_stamp = 2.0
teleop.arm._joint_state.joint_state.joint_1 = 13000
teleop.arm._joint_state.joint_state.joint_2 = 23000
teleop.arm._joint_state.joint_state.joint_3 = 33000
teleop.arm._joint_state.joint_state.joint_4 = 43000
teleop.arm._joint_state.joint_state.joint_5 = 53000
teleop.arm._joint_state.joint_state.joint_6 = 63000
publisher = threading.Thread(target=publish_feedback, daemon=True)
publisher.start()
t0 = time.monotonic()
action = teleop.get_action()
publisher.join(timeout=0.1)
assert time.monotonic() - t0 >= 0.013
assert action["joint_1.pos"] == 13.0
finally: finally:
teleop.disconnect() teleop.disconnect()
def test_piper_follower_connect_calibrates_then_reenables(monkeypatch, tmp_path): def test_piper_leader_disconnect_disables_if_role_restore_fails(monkeypatch):
patch_fake_sdk(monkeypatch) patch_fake_sdk(monkeypatch)
teleop = PiperLeader(PiperLeaderConfig(port="can1", manual_control=False))
teleop.connect()
robot = PiperFollower( def fail_role_restore(*args):
PiperFollowerConfig( raise RuntimeError("leader role restore failed")
port="can0",
id="connect_reenable_after_calibration",
calibration_dir=tmp_path,
require_calibration=True,
enable_on_connect=True,
)
)
def fake_calibrate(): monkeypatch.setattr(teleop.arm, "MasterSlaveConfig", fail_role_restore)
# Mirror real calibration's drag-mode behavior.
robot.arm.DisableArm(7)
robot.calibration = make_identity_calibration()
monkeypatch.setattr(robot, "calibrate", fake_calibrate) with pytest.raises(RuntimeError, match="leader role restore failed"):
teleop.disconnect()
robot.connect(calibrate=True) assert teleop.arm.disable_calls == 1
try: assert not teleop.arm.connected
assert robot.arm.disable_calls == 1
assert robot.arm.enable_calls == 1
assert robot.arm.is_enabled
finally:
robot.disconnect()
def test_piper_follower_connect_without_calibration_still_enables(monkeypatch, tmp_path): def test_piper_follower_connect_enables_arm(monkeypatch):
patch_fake_sdk(monkeypatch) patch_fake_sdk(monkeypatch)
robot = PiperFollower(PiperFollowerConfig(port="can0", enable_on_connect=True))
robot = PiperFollower( robot.connect()
PiperFollowerConfig(
port="can0",
id="connect_enable_without_calibration",
calibration_dir=tmp_path,
require_calibration=True,
enable_on_connect=True,
)
)
robot.connect(calibrate=False)
try: try:
assert robot.arm.enable_calls == 1 assert robot.arm.enable_calls == 1
assert robot.arm.is_enabled assert robot.arm.is_enabled
@ -527,90 +461,17 @@ def test_piper_follower_connect_without_calibration_still_enables(monkeypatch, t
robot.disconnect() robot.disconnect()
def test_piper_leader_gravity_comp_manual_control_uses_mit(monkeypatch): def test_piper_follower_connect_sets_follower_role_without_restart(monkeypatch):
patch_fake_sdk(monkeypatch) patch_fake_sdk(monkeypatch)
device = PiperFollower(PiperFollowerConfig(port="can0"))
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 device.arm._arm_status.arm_status.ctrl_mode = 0x06
with pytest.raises(RuntimeError, match="Follower role command .* sent.*Power-cycle"): device.connect()
device.connect(calibrate=False) try:
assert device.arm.role_commands == [(0xFC, 0x00, 0x00, 0x00)]
assert device.arm.role_commands[-1] == (0xFC, 0x00, 0x00, 0x00) assert [name for name, _ in device.arm.command_log[:3]] == ["role", "motion_mode", "enable"]
finally:
device.disconnect()
def test_piper_lfs_pointer_urdf_raises_actionable_error(tmp_path):
pointer_file = tmp_path / "lfs_pointer.urdf"
pointer_file.write_text("version https://git-lfs.github.com/spec/v1\noid sha256:deadbeef\nsize 123\n")
with pytest.raises(RuntimeError, match="Git LFS pointer files"):
piper_leader_module._ensure_not_lfs_pointer(
pointer_file, "assets/piper_description/urdf/pointer.urdf"
)
@pytest.mark.parametrize( @pytest.mark.parametrize(
@ -627,8 +488,8 @@ def test_piper_lfs_pointer_urdf_raises_actionable_error(tmp_path):
[ [
( (
BiPiperLeaderConfig( BiPiperLeaderConfig(
left_arm_config=PiperLeaderConfigBase(port="can1", manual_control=False, sync_gripper=True), left_arm_config=PiperLeaderConfigBase(port="can1", manual_control=True, sync_gripper=True),
right_arm_config=PiperLeaderConfigBase(port="can3", manual_control=False, sync_gripper=True), right_arm_config=PiperLeaderConfigBase(port="can3", manual_control=True, sync_gripper=True),
process_isolation=False, process_isolation=False,
), ),
BiPiperFollowerConfig( BiPiperFollowerConfig(
@ -644,8 +505,8 @@ def test_piper_lfs_pointer_urdf_raises_actionable_error(tmp_path):
), ),
( (
BiPiperXLeaderConfig( BiPiperXLeaderConfig(
left_arm_config=PiperXLeaderConfigBase(port="can1", manual_control=False, sync_gripper=True), left_arm_config=PiperXLeaderConfigBase(port="can1", manual_control=True, sync_gripper=True),
right_arm_config=PiperXLeaderConfigBase(port="can3", manual_control=False, sync_gripper=True), right_arm_config=PiperXLeaderConfigBase(port="can3", manual_control=True, sync_gripper=True),
process_isolation=False, process_isolation=False,
), ),
BiPiperXFollowerConfig( BiPiperXFollowerConfig(
@ -684,14 +545,11 @@ def test_bimanual_piper_leader_follower_roundtrip(
assert isinstance(robot.left_arm, left_robot_cls) assert isinstance(robot.left_arm, left_robot_cls)
assert isinstance(robot.right_arm, right_robot_cls) assert isinstance(robot.right_arm, right_robot_cls)
teleop.left_arm.calibration = make_identity_calibration() teleop.connect()
teleop.right_arm.calibration = make_identity_calibration() robot.connect()
robot.left_arm.calibration = make_identity_calibration()
robot.right_arm.calibration = make_identity_calibration()
teleop.connect(calibrate=False)
robot.connect(calibrate=False)
try: try:
assert teleop.left_arm.arm.role_commands[-1][0] == 0xFA
assert teleop.right_arm.arm.role_commands[-1][0] == 0xFA
action = teleop.get_action() action = teleop.get_action()
assert "left_joint_1.pos" in action assert "left_joint_1.pos" in action
assert "right_joint_1.pos" in action assert "right_joint_1.pos" in action
@ -714,6 +572,8 @@ def test_bimanual_piper_leader_follower_roundtrip(
assert obs["right_gripper.pos"] == 43.0 assert obs["right_gripper.pos"] == 43.0
teleop.send_feedback(action) teleop.send_feedback(action)
assert teleop.left_arm.arm.role_commands[-1][0] == 0xFC
assert teleop.right_arm.arm.role_commands[-1][0] == 0xFC
assert teleop.left_arm.arm.last_joint == (10000, 20000, 30000, 40000, 50000, 60000) assert teleop.left_arm.arm.last_joint == (10000, 20000, 30000, 40000, 50000, 60000)
assert teleop.right_arm.arm.last_joint == (10000, 20000, 30000, 40000, 50000, 60000) assert teleop.right_arm.arm.last_joint == (10000, 20000, 30000, 40000, 50000, 60000)
assert teleop.left_arm.arm.last_gripper[0] == 42000 assert teleop.left_arm.arm.last_gripper[0] == 42000
@ -721,6 +581,29 @@ def test_bimanual_piper_leader_follower_roundtrip(
finally: finally:
teleop.disconnect() teleop.disconnect()
robot.disconnect() robot.disconnect()
assert teleop.left_arm.arm.role_commands[-1][0] == 0xFA
assert teleop.right_arm.arm.role_commands[-1][0] == 0xFA
def test_bimanual_piper_get_action_returns_available_side(monkeypatch):
patch_fake_sdk(monkeypatch)
teleop = make_teleoperator_from_config(
BiPiperLeaderConfig(
left_arm_config=PiperLeaderConfigBase(port="can2"),
right_arm_config=PiperLeaderConfigBase(port="can3"),
process_isolation=False,
)
)
teleop.connect()
try:
teleop.right_arm.arm._joint_ctrl.time_stamp = 0.0
action = teleop.get_action()
assert "left_joint_1.pos" in action
assert "right_joint_1.pos" not in action
finally:
teleop.disconnect()
def test_bimanual_piper_follower_action_features_are_available_without_connect(monkeypatch): def test_bimanual_piper_follower_action_features_are_available_without_connect(monkeypatch):
@ -746,8 +629,8 @@ def test_bimanual_piper_teleop_loop_smoke(monkeypatch):
teleop = make_teleoperator_from_config( teleop = make_teleoperator_from_config(
BiPiperLeaderConfig( BiPiperLeaderConfig(
left_arm_config=PiperLeaderConfigBase(port="can0", manual_control=False, sync_gripper=True), left_arm_config=PiperLeaderConfigBase(port="can0", manual_control=True, sync_gripper=True),
right_arm_config=PiperLeaderConfigBase(port="can1", manual_control=False, sync_gripper=True), right_arm_config=PiperLeaderConfigBase(port="can1", manual_control=True, sync_gripper=True),
process_isolation=False, process_isolation=False,
) )
) )
@ -758,18 +641,13 @@ def test_bimanual_piper_teleop_loop_smoke(monkeypatch):
) )
) )
teleop.left_arm.calibration = make_identity_calibration()
teleop.right_arm.calibration = make_identity_calibration()
robot.left_arm.calibration = make_identity_calibration()
robot.right_arm.calibration = make_identity_calibration()
teleop_action_processor, robot_action_processor, robot_observation_processor = make_default_processors() teleop_action_processor, robot_action_processor, robot_observation_processor = make_default_processors()
robot.get_observation = lambda: (_ for _ in ()).throw( robot.get_observation = lambda: (_ for _ in ()).throw(
AssertionError("teleop_loop should not fetch observation for identity processors without display") AssertionError("teleop_loop should not fetch observation for identity processors without display")
) )
teleop.connect(calibrate=False) teleop.connect()
robot.connect(calibrate=False) robot.connect()
try: try:
teleop_loop( teleop_loop(
teleop=teleop, teleop=teleop,
@ -797,18 +675,13 @@ def test_bimanual_piper_leader_uses_process_proxy_by_default(monkeypatch):
self.arm_cls = arm_cls self.arm_cls = arm_cls
self.arm_config = arm_config self.arm_config = arm_config
self.is_connected = False self.is_connected = False
self.is_calibrated = True
action_features = dict.fromkeys(PIPER_ACTION_KEYS, float) action_features = dict.fromkeys(PIPER_ACTION_KEYS, float)
feedback_features = dict.fromkeys(PIPER_ACTION_KEYS, float) feedback_features = dict.fromkeys(PIPER_ACTION_KEYS, float)
def connect(self, calibrate=True): def connect(self):
del calibrate
self.is_connected = True self.is_connected = True
def calibrate(self):
pass
def configure(self): def configure(self):
pass pass
@ -838,79 +711,3 @@ def test_bimanual_piper_leader_uses_process_proxy_by_default(monkeypatch):
assert isinstance(teleop.left_arm, DummyProxy) assert isinstance(teleop.left_arm, DummyProxy)
assert isinstance(teleop.right_arm, DummyProxy) assert isinstance(teleop.right_arm, DummyProxy)
def test_piper_follower_send_only_mode_skips_sdk_reader_threads(monkeypatch):
patch_fake_sdk(monkeypatch)
robot = make_robot_from_config(
PiperFollowerConfig(port="can3", sync_gripper=True, require_calibration=False)
)
robot.calibration = make_identity_calibration()
robot.set_teleop_send_only_mode(True)
robot.connect(calibrate=False)
try:
assert robot.arm.connect_calls[-1]["start_thread"] is False
with pytest.raises(RuntimeError, match="send-only mode"):
robot.get_observation()
finally:
robot.disconnect()
def test_piper_follower_send_only_mode_skips_camera_connects_and_stays_connected(monkeypatch):
patch_fake_sdk(monkeypatch)
class FakeCamera:
def __init__(self):
self.is_connected = False
self.connect_calls = 0
self.disconnect_calls = 0
def connect(self):
self.connect_calls += 1
self.is_connected = True
def disconnect(self):
self.disconnect_calls += 1
self.is_connected = False
camera = FakeCamera()
monkeypatch.setattr(piper_follower_module, "make_cameras_from_configs", lambda _: {"front": camera})
robot = make_robot_from_config(
PiperFollowerConfig(port="can3", sync_gripper=True, require_calibration=False)
)
robot.calibration = make_identity_calibration()
robot.set_teleop_send_only_mode(True)
robot.connect(calibrate=False)
try:
assert robot.is_connected
assert camera.connect_calls == 0
assert not camera.is_connected
finally:
robot.disconnect()
assert camera.disconnect_calls == 0
def test_bimanual_piper_send_only_mode_propagates_to_both_followers(monkeypatch):
patch_fake_sdk(monkeypatch)
robot = make_robot_from_config(
BiPiperFollowerConfig(
left_arm_config=PiperFollowerConfigBase(port="can3", sync_gripper=True, require_calibration=False),
right_arm_config=PiperFollowerConfigBase(port="can2", sync_gripper=True, require_calibration=False),
)
)
robot.left_arm.calibration = make_identity_calibration()
robot.right_arm.calibration = make_identity_calibration()
robot.set_teleop_send_only_mode(True)
robot.connect(calibrate=False)
try:
assert robot.left_arm.arm.connect_calls[-1]["start_thread"] is False
assert robot.right_arm.arm.connect_calls[-1]["start_thread"] is False
finally:
robot.disconnect()

查看文件

@ -0,0 +1,34 @@
from types import SimpleNamespace
import lerobot.scripts.lerobot_setup_can as setup_can_module
import lerobot.utils.piper_sdk as piper_sdk_utils
def test_resolve_piper_can_interface(monkeypatch, tmp_path):
interface_path = tmp_path / "can2"
interface_path.mkdir()
(interface_path / "type").write_text("280\n")
monkeypatch.setattr(piper_sdk_utils, "_SYS_CLASS_NET", tmp_path)
monkeypatch.setattr(
piper_sdk_utils.subprocess,
"run",
lambda *args, **kwargs: SimpleNamespace(stdout="ID_SERIAL_SHORT=SERIAL_A\n"),
)
assert piper_sdk_utils.resolve_piper_can_interface("SERIAL_A") == "can2"
def test_setup_can_uses_classic_can_for_usb_serials(monkeypatch):
monkeypatch.setattr(setup_can_module, "resolve_piper_can_interface", lambda serial: "can2")
attempts = []
def fake_setup(interface, bitrate, data_bitrate, use_fd):
attempts.append((interface, bitrate, use_fd))
return False, "failed"
monkeypatch.setattr(setup_can_module, "setup_interface", fake_setup)
cfg = setup_can_module.CANSetupConfig(mode="setup", usb_can_serials="SERIAL_A")
setup_can_module.setup_interface_with_fallback(cfg.get_interfaces()[0], cfg)
assert attempts == [("can2", 1000000, False)]