feat(piper): add calibration-driven leader/follower mapping

- add calibration_scale config and validation for piper leader/follower

- implement interactive calibration capture and calibrated offset conversion

- require calibration before action/feedback paths and update teleop tests
这个提交包含在:
Elvin
2026-03-05 21:09:46 +08:00
父节点 df08db9005
当前提交 422f5c50e7
共有 5 个文件被更改,包括 273 次插入 和 28 次删除

查看文件

@ -46,6 +46,10 @@ class PiperFollowerConfigBase:
enable_on_connect: bool = True
enable_timeout_s: float = 3.0
# Calibration precision:
# homing_offset/range_min/range_max are stored as "degree * calibration_scale".
calibration_scale: int = 1000
# Gripper forwarding behavior
sync_gripper: bool = True
gripper_effort_default: int = 1000
@ -69,6 +73,8 @@ class PiperFollowerConfig(RobotConfig, PiperFollowerConfigBase):
raise ValueError("`mode_refresh_interval_s` must be >= 0.")
if self.enable_timeout_s < 0:
raise ValueError("`enable_timeout_s` must be >= 0.")
if self.calibration_scale <= 0:
raise ValueError("`calibration_scale` must be > 0.")
if self.startup_sleep_s < 0:
raise ValueError("`startup_sleep_s` must be >= 0.")
if not (0 <= self.gripper_effort_default <= 5000):

查看文件

@ -20,6 +20,7 @@ from functools import cached_property
from typing import Any
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 (
@ -30,11 +31,14 @@ from lerobot.utils.piper_sdk import (
parse_piper_log_level,
unit_to_milli,
)
from lerobot.utils.utils import enter_pressed, move_cursor_up
from ..robot import Robot
from .config_piper_follower import PiperFollowerConfig
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):
@ -78,7 +82,6 @@ class PiperFollower(Robot):
@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)
@ -90,8 +93,14 @@ class PiperFollower(Robot):
time.sleep(0.05)
self.configure()
if self.config.enable_on_connect and not self._wait_enable(self.config.enable_timeout_s):
# lerobot-calibrate calls connect(calibrate=False) before running calibrate().
# Keep the arm in drag mode for calibration by skipping auto-enable in this path.
should_enable = self.config.enable_on_connect and calibrate
if should_enable and not self._wait_enable(self.config.enable_timeout_s):
logger.warning("Piper follower did not report enabled state before timeout.")
if not self.is_calibrated and calibrate:
logger.info("No piper-follower calibration file found for '%s'. Running lerobot-calibrate flow.", self.id)
self.calibrate()
for cam in self.cameras.values():
cam.connect()
@ -104,10 +113,50 @@ class PiperFollower(Robot):
@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:
logger.info("Piper follower does not require lerobot joint-range calibration. Skipping.")
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()
@ -133,12 +182,11 @@ class PiperFollower(Robot):
time.sleep(0.02)
return False
@check_if_not_connected
def get_observation(self) -> RobotObservation:
def _read_raw_observation(self) -> RobotObservation:
joint_msg = self.arm.GetArmJointMsgs()
joint_state = getattr(joint_msg, "joint_state", None)
obs: dict[str, float] = {}
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)
@ -146,6 +194,47 @@ class PiperFollower(Robot):
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()
@ -153,6 +242,11 @@ class PiperFollower(Robot):
@check_if_not_connected
def send_action(self, action: RobotAction) -> RobotAction:
if not self.is_calibrated:
raise RuntimeError(
f"{self} is not calibrated. Run `lerobot-calibrate --robot.type=piper_follower --robot.id={self.id}` first."
)
self._refresh_motion_mode_if_needed()
sent_action: dict[str, float] = {}
@ -160,14 +254,16 @@ class PiperFollower(Robot):
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]
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:
gripper_pos_raw = unit_to_milli(abs(action["gripper.pos"]))
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,