镜像自地址
https://github.com/MINT-SJTU/Evo-RL.git
已同步 2026-11-21 02:27:04 +00:00
feat(teleop): add piper leader/follower adapter for LeRobot
- add piper_follower robot and piper_leader teleoperator implementations based on Piper SDK CAN APIs - wire piper types into teleoperate/record/replay/calibrate command imports and device factories - add Piper SDK import helper with auto-discovery and unit conversion utilities - add piper teleop tests and camera-compatible follower observation support for record/hil flows
这个提交包含在:
@ -0,0 +1,191 @@
|
||||
#!/usr/bin/env python
|
||||
|
||||
# Copyright 2026 The HuggingFace Inc. team. All rights reserved.
|
||||
#
|
||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||
# you may not use this file except in compliance with the License.
|
||||
# You may obtain a copy of the License at
|
||||
#
|
||||
# http://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License.
|
||||
|
||||
import logging
|
||||
import time
|
||||
from functools import cached_property
|
||||
from typing import Any
|
||||
|
||||
from lerobot.cameras.utils import make_cameras_from_configs
|
||||
from lerobot.processor import RobotAction, RobotObservation
|
||||
from lerobot.utils.decorators import check_if_already_connected, check_if_not_connected
|
||||
from lerobot.utils.piper_sdk import (
|
||||
PIPER_ACTION_KEYS,
|
||||
PIPER_JOINT_NAMES,
|
||||
get_piper_sdk,
|
||||
milli_to_unit,
|
||||
parse_piper_log_level,
|
||||
unit_to_milli,
|
||||
)
|
||||
|
||||
from ..robot import Robot
|
||||
from .config_piper_follower import PiperFollowerConfig
|
||||
|
||||
logger = logging.getLogger(__name__)
|
||||
|
||||
|
||||
class PiperFollower(Robot):
|
||||
"""Piper follower arm controlled through the Piper SDK (CAN)."""
|
||||
|
||||
config_class = PiperFollowerConfig
|
||||
name = "piper_follower"
|
||||
|
||||
def __init__(self, config: PiperFollowerConfig):
|
||||
super().__init__(config)
|
||||
self.config = config
|
||||
self._is_connected = False
|
||||
self._last_mode_refresh_t = 0.0
|
||||
|
||||
interface_cls, _ = get_piper_sdk()
|
||||
self.arm = interface_cls(
|
||||
can_name=self.config.port,
|
||||
judge_flag=self.config.judge_flag,
|
||||
can_auto_init=self.config.can_auto_init,
|
||||
logger_level=parse_piper_log_level(self.config.log_level),
|
||||
)
|
||||
self.cameras = make_cameras_from_configs(config.cameras)
|
||||
|
||||
@property
|
||||
def _cameras_ft(self) -> dict[str, tuple]:
|
||||
return {
|
||||
cam: (self.config.cameras[cam].height, self.config.cameras[cam].width, 3) for cam in self.cameras
|
||||
}
|
||||
|
||||
@cached_property
|
||||
def observation_features(self) -> dict[str, type | tuple]:
|
||||
return {**{key: float for key in PIPER_ACTION_KEYS}, **self._cameras_ft}
|
||||
|
||||
@cached_property
|
||||
def action_features(self) -> dict[str, type]:
|
||||
return {key: float for key in PIPER_ACTION_KEYS}
|
||||
|
||||
@property
|
||||
def is_connected(self) -> bool:
|
||||
return self._is_connected and all(cam.is_connected for cam in self.cameras.values())
|
||||
|
||||
@check_if_already_connected
|
||||
def connect(self, calibrate: bool = True) -> None:
|
||||
del calibrate
|
||||
self.arm.ConnectPort()
|
||||
if self.config.startup_sleep_s > 0:
|
||||
time.sleep(self.config.startup_sleep_s)
|
||||
|
||||
self._is_connected = True
|
||||
try:
|
||||
if self.config.set_slave_mode_on_connect:
|
||||
self.arm.MasterSlaveConfig(0xFC, 0x00, 0x00, 0x00)
|
||||
time.sleep(0.05)
|
||||
|
||||
self.configure()
|
||||
if self.config.enable_on_connect and not self._wait_enable(self.config.enable_timeout_s):
|
||||
logger.warning("Piper follower did not report enabled state before timeout.")
|
||||
|
||||
for cam in self.cameras.values():
|
||||
cam.connect()
|
||||
except Exception:
|
||||
self.arm.DisconnectPort()
|
||||
self._is_connected = False
|
||||
raise
|
||||
|
||||
logger.info("%s connected.", self)
|
||||
|
||||
@property
|
||||
def is_calibrated(self) -> bool:
|
||||
return True
|
||||
|
||||
def calibrate(self) -> None:
|
||||
logger.info("Piper follower does not require lerobot joint-range calibration. Skipping.")
|
||||
|
||||
def configure(self) -> None:
|
||||
self._send_motion_mode()
|
||||
|
||||
def _send_motion_mode(self) -> None:
|
||||
mit_mode = 0xAD if self.config.high_follow else 0x00
|
||||
self.arm.MotionCtrl_2(0x01, 0x01, self.config.speed_ratio, mit_mode)
|
||||
self._last_mode_refresh_t = time.monotonic()
|
||||
|
||||
def _refresh_motion_mode_if_needed(self) -> None:
|
||||
interval_s = self.config.mode_refresh_interval_s
|
||||
if interval_s <= 0:
|
||||
return
|
||||
now = time.monotonic()
|
||||
if now - self._last_mode_refresh_t >= interval_s:
|
||||
self._send_motion_mode()
|
||||
|
||||
def _wait_enable(self, timeout_s: float) -> bool:
|
||||
deadline = time.monotonic() + timeout_s
|
||||
while time.monotonic() < deadline:
|
||||
if bool(self.arm.EnablePiper()):
|
||||
return True
|
||||
time.sleep(0.02)
|
||||
return False
|
||||
|
||||
@check_if_not_connected
|
||||
def get_observation(self) -> RobotObservation:
|
||||
joint_msg = self.arm.GetArmJointMsgs()
|
||||
joint_state = getattr(joint_msg, "joint_state", None)
|
||||
|
||||
obs: dict[str, float] = {}
|
||||
for joint_name in PIPER_JOINT_NAMES:
|
||||
raw_value = getattr(joint_state, joint_name, 0)
|
||||
obs[f"{joint_name}.pos"] = milli_to_unit(raw_value)
|
||||
|
||||
gripper_msg = self.arm.GetArmGripperMsgs()
|
||||
gripper_state = getattr(gripper_msg, "gripper_state", None)
|
||||
obs["gripper.pos"] = abs(milli_to_unit(getattr(gripper_state, "grippers_angle", 0)))
|
||||
|
||||
for cam_key, cam in self.cameras.items():
|
||||
obs[cam_key] = cam.async_read()
|
||||
return obs
|
||||
|
||||
@check_if_not_connected
|
||||
def send_action(self, action: RobotAction) -> RobotAction:
|
||||
self._refresh_motion_mode_if_needed()
|
||||
|
||||
sent_action: dict[str, float] = {}
|
||||
|
||||
joint_keys = [f"{joint_name}.pos" for joint_name in PIPER_JOINT_NAMES]
|
||||
has_all_joints = all(key in action for key in joint_keys)
|
||||
if has_all_joints:
|
||||
joint_commands = [unit_to_milli(action[key]) for key in joint_keys]
|
||||
self.arm.JointCtrl(*joint_commands)
|
||||
sent_action.update({key: milli_to_unit(raw) for key, raw in zip(joint_keys, joint_commands, strict=True)})
|
||||
elif any(key in action for key in joint_keys):
|
||||
logger.debug("Ignoring partial Piper joint action. Need all six joint keys to send command.")
|
||||
|
||||
if self.config.sync_gripper and "gripper.pos" in action:
|
||||
gripper_pos_raw = unit_to_milli(abs(action["gripper.pos"]))
|
||||
self.arm.GripperCtrl(
|
||||
gripper_pos_raw,
|
||||
self.config.gripper_effort_default,
|
||||
self.config.gripper_status_code,
|
||||
0x00,
|
||||
)
|
||||
sent_action["gripper.pos"] = milli_to_unit(gripper_pos_raw)
|
||||
|
||||
return sent_action
|
||||
|
||||
@check_if_not_connected
|
||||
def disconnect(self) -> None:
|
||||
try:
|
||||
if self.config.disable_on_disconnect:
|
||||
self.arm.DisableArm(7)
|
||||
finally:
|
||||
self.arm.DisconnectPort()
|
||||
for cam in self.cameras.values():
|
||||
cam.disconnect()
|
||||
self._is_connected = False
|
||||
logger.info("%s disconnected.", self)
|
||||
在新工单中引用
屏蔽一个用户