文件
Evo-RL/src/lerobot/robots/piper_follower/piper_follower.py
2026-03-07 02:21:09 +08:00

306 行
12 KiB
Python

#!/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 lerobot.cameras.utils import make_cameras_from_configs
from lerobot.motors import MotorCalibration
from lerobot.processor import RobotAction, RobotObservation
from lerobot.utils.decorators import check_if_already_connected, check_if_not_connected
from lerobot.utils.piper_sdk import (
PIPER_ACTION_KEYS,
PIPER_JOINT_ACTION_KEYS,
PIPER_JOINT_NAMES,
get_piper_sdk,
guard_piper_ctrl_mode_on_connect,
milli_to_unit,
parse_piper_log_level,
unit_to_milli,
wait_enable_piper,
)
from lerobot.utils.utils import enter_pressed, move_cursor_up
from ..robot import Robot
from .config_piper_follower import PiperFollowerConfig, PiperXFollowerConfig
logger = logging.getLogger(__name__)
PIPER_CALIB_KEYS = list(PIPER_ACTION_KEYS)
PIPER_CALIB_IDS = {key: idx for idx, key in enumerate(PIPER_CALIB_KEYS)}
class PiperFollower(Robot):
"""Piper follower arm controlled through the Piper SDK (CAN)."""
config_class = PiperFollowerConfig
name = "piper_follower"
def __init__(self, config: PiperFollowerConfig | PiperXFollowerConfig):
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 {**dict.fromkeys(PIPER_ACTION_KEYS, float), **self._cameras_ft}
@cached_property
def action_features(self) -> dict[str, type]:
return dict.fromkeys(PIPER_ACTION_KEYS, float)
@property
def is_connected(self) -> bool:
return self._is_connected and all(cam.is_connected for cam in self.cameras.values())
@check_if_already_connected
def connect(self, calibrate: bool = True) -> None:
self.arm.ConnectPort()
if self.config.startup_sleep_s > 0:
time.sleep(self.config.startup_sleep_s)
guard_piper_ctrl_mode_on_connect(arm=self.arm, interface_name=self.config.port)
self._is_connected = True
connected_cameras = []
try:
self.configure()
should_auto_calibrate = not self.is_calibrated and calibrate and self.config.require_calibration
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 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()
connected_cameras.append(cam)
except Exception:
self.arm.DisconnectPort()
for cam in connected_cameras:
cam.disconnect()
self._is_connected = False
raise
logger.info("%s connected.", self)
def _use_uncalibrated_passthrough(self) -> bool:
return not self.is_calibrated and not self.config.require_calibration
@property
def is_calibrated(self) -> bool:
if not all(key in self.calibration for key in PIPER_CALIB_KEYS):
return False
for key in PIPER_CALIB_KEYS:
cal = self.calibration[key]
if cal.range_max <= cal.range_min:
return False
return True
def calibrate(self) -> None:
if self.calibration and self.is_calibrated:
user_input = input(
f"Press ENTER to use existing calibration file for id '{self.id}', "
"or type 'c' and press ENTER to run a new calibration: "
)
if user_input.strip().lower() != "c":
return
logger.info("Running calibration for %s", self)
# Calibration for Piper follower should be sampled in pure backdrivable mode.
self.arm.DisableArm(7)
time.sleep(0.1)
input("Move piper-follower to your desired neutral/center pose, then press ENTER...")
neutral = self._read_raw_observation()
print("Move all piper-follower joints through full range. Press ENTER to stop recording...")
range_mins, range_maxes = self._record_ranges_of_motion()
self.calibration = {}
for key in PIPER_CALIB_KEYS:
min_deg = range_mins[key]
max_deg = range_maxes[key]
if max_deg <= min_deg:
raise ValueError(f"Invalid range for {key}: min={min_deg:.3f}, max={max_deg:.3f}")
neutral_deg = min(max_deg, max(min_deg, neutral[key]))
self.calibration[key] = MotorCalibration(
id=PIPER_CALIB_IDS[key],
drive_mode=0,
homing_offset=self._to_calibration_units(neutral_deg),
range_min=self._to_calibration_units(min_deg),
range_max=self._to_calibration_units(max_deg),
)
self._save_calibration()
print(f"Calibration saved to {self.calibration_fpath}")
def configure(self) -> None:
self._send_motion_mode()
def _send_motion_mode(self) -> None:
mit_mode = 0xAD if self.config.high_follow else 0x00
self.arm.MotionCtrl_2(0x01, 0x01, self.config.speed_ratio, mit_mode)
self._last_mode_refresh_t = time.monotonic()
def _refresh_motion_mode_if_needed(self) -> None:
interval_s = self.config.mode_refresh_interval_s
if interval_s <= 0:
return
now = time.monotonic()
if now - self._last_mode_refresh_t >= interval_s:
self._send_motion_mode()
def _wait_enable(self, timeout_s: float) -> bool:
return wait_enable_piper(self.arm, timeout_s)
def _read_raw_observation(self) -> RobotObservation:
joint_msg = self.arm.GetArmJointMsgs()
joint_state = getattr(joint_msg, "joint_state", None)
obs: RobotObservation = {}
for joint_name in PIPER_JOINT_NAMES:
raw_value = getattr(joint_state, joint_name, 0)
obs[f"{joint_name}.pos"] = milli_to_unit(raw_value)
gripper_msg = self.arm.GetArmGripperMsgs()
gripper_state = getattr(gripper_msg, "gripper_state", None)
obs["gripper.pos"] = abs(milli_to_unit(getattr(gripper_state, "grippers_angle", 0)))
return obs
def _to_calibration_units(self, angle_deg: float) -> int:
return int(round(angle_deg * self.config.calibration_scale))
def _from_calibration_units(self, value: int) -> float:
return float(value) / float(self.config.calibration_scale)
def _offset_to_target(self, key: str, offset_deg: float) -> float:
cal = self.calibration[key]
min_deg = self._from_calibration_units(cal.range_min)
max_deg = self._from_calibration_units(cal.range_max)
home_deg = self._from_calibration_units(cal.homing_offset)
centered = -offset_deg if cal.drive_mode else offset_deg
target = home_deg + centered
return min(max_deg, max(min_deg, target))
def _record_ranges_of_motion(self) -> tuple[dict[str, float], dict[str, float]]:
current = self._read_raw_observation()
mins = current.copy()
maxes = current.copy()
while True:
current = self._read_raw_observation()
mins = {key: min(mins[key], current[key]) for key in PIPER_CALIB_KEYS}
maxes = {key: max(maxes[key], current[key]) for key in PIPER_CALIB_KEYS}
print("\n-----------------------------")
print("JOINT | MIN | POS | MAX")
for key in PIPER_CALIB_KEYS:
print(f"{key:<11} | {mins[key]:>6.2f} | {current[key]:>6.2f} | {maxes[key]:>6.2f}")
if enter_pressed():
break
move_cursor_up(len(PIPER_CALIB_KEYS) + 3)
return mins, maxes
@check_if_not_connected
def get_observation(self) -> RobotObservation:
obs: dict[str, float] = self._read_raw_observation()
for cam_key, cam in self.cameras.items():
obs[cam_key] = cam.async_read()
return obs
@check_if_not_connected
def send_action(self, action: RobotAction) -> RobotAction:
if not self.is_calibrated and not self._use_uncalibrated_passthrough():
raise RuntimeError(
f"{self} is not calibrated. Run `lerobot-calibrate --robot.type={self.config.type} --robot.id={self.id}` first."
)
self._refresh_motion_mode_if_needed()
sent_action: dict[str, float] = {}
joint_keys = PIPER_JOINT_ACTION_KEYS
has_all_joints = all(key in action for key in joint_keys)
if has_all_joints:
if self._use_uncalibrated_passthrough():
joint_targets = [action[key] for key in joint_keys]
else:
joint_targets = [self._offset_to_target(key, action[key]) for key in joint_keys]
joint_commands = [unit_to_milli(value) for value in joint_targets]
self.arm.JointCtrl(*joint_commands)
sent_action.update(
{key: milli_to_unit(raw) for key, raw in zip(joint_keys, joint_commands, strict=True)}
)
elif any(key in action for key in joint_keys):
logger.debug("Ignoring partial Piper joint action. Need all six joint keys to send command.")
if self.config.sync_gripper and "gripper.pos" in action:
if self._use_uncalibrated_passthrough():
gripper_target = action["gripper.pos"]
else:
gripper_target = self._offset_to_target("gripper.pos", action["gripper.pos"])
gripper_pos_raw = unit_to_milli(gripper_target)
self.arm.GripperCtrl(
gripper_pos_raw,
self.config.gripper_effort_default,
self.config.gripper_status_code,
0x00,
)
sent_action["gripper.pos"] = milli_to_unit(gripper_pos_raw)
return sent_action
@check_if_not_connected
def disconnect(self) -> None:
try:
if self.config.disable_on_disconnect:
self.arm.DisableArm(7)
finally:
self.arm.DisconnectPort()
for cam in self.cameras.values():
cam.disconnect()
self._is_connected = False
logger.info("%s disconnected.", self)
class PiperXFollower(PiperFollower):
config_class = PiperXFollowerConfig
name = "piperx_follower"