镜像自地址
https://github.com/MINT-SJTU/Evo-RL.git
已同步 2026-11-20 18:17:06 +00:00
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
这个提交包含在:
@ -46,6 +46,10 @@ class PiperFollowerConfigBase:
|
|||||||
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
|
||||||
|
|
||||||
# Gripper forwarding behavior
|
# Gripper forwarding behavior
|
||||||
sync_gripper: bool = True
|
sync_gripper: bool = True
|
||||||
gripper_effort_default: int = 1000
|
gripper_effort_default: int = 1000
|
||||||
@ -69,6 +73,8 @@ class PiperFollowerConfig(RobotConfig, PiperFollowerConfigBase):
|
|||||||
raise ValueError("`mode_refresh_interval_s` must be >= 0.")
|
raise ValueError("`mode_refresh_interval_s` must be >= 0.")
|
||||||
if self.enable_timeout_s < 0:
|
if self.enable_timeout_s < 0:
|
||||||
raise ValueError("`enable_timeout_s` must be >= 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:
|
if self.startup_sleep_s < 0:
|
||||||
raise ValueError("`startup_sleep_s` must be >= 0.")
|
raise ValueError("`startup_sleep_s` must be >= 0.")
|
||||||
if not (0 <= self.gripper_effort_default <= 5000):
|
if not (0 <= self.gripper_effort_default <= 5000):
|
||||||
|
|||||||
@ -20,6 +20,7 @@ from functools import cached_property
|
|||||||
from typing import Any
|
from typing import Any
|
||||||
|
|
||||||
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 (
|
||||||
@ -30,11 +31,14 @@ from lerobot.utils.piper_sdk import (
|
|||||||
parse_piper_log_level,
|
parse_piper_log_level,
|
||||||
unit_to_milli,
|
unit_to_milli,
|
||||||
)
|
)
|
||||||
|
from lerobot.utils.utils import enter_pressed, move_cursor_up
|
||||||
|
|
||||||
from ..robot import Robot
|
from ..robot import Robot
|
||||||
from .config_piper_follower import PiperFollowerConfig
|
from .config_piper_follower import PiperFollowerConfig
|
||||||
|
|
||||||
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):
|
||||||
@ -78,7 +82,6 @@ class PiperFollower(Robot):
|
|||||||
|
|
||||||
@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)
|
||||||
@ -90,8 +93,14 @@ class PiperFollower(Robot):
|
|||||||
time.sleep(0.05)
|
time.sleep(0.05)
|
||||||
|
|
||||||
self.configure()
|
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.")
|
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():
|
for cam in self.cameras.values():
|
||||||
cam.connect()
|
cam.connect()
|
||||||
@ -104,10 +113,50 @@ class PiperFollower(Robot):
|
|||||||
|
|
||||||
@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:
|
||||||
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:
|
def configure(self) -> None:
|
||||||
self._send_motion_mode()
|
self._send_motion_mode()
|
||||||
@ -133,12 +182,11 @@ class PiperFollower(Robot):
|
|||||||
time.sleep(0.02)
|
time.sleep(0.02)
|
||||||
return False
|
return False
|
||||||
|
|
||||||
@check_if_not_connected
|
def _read_raw_observation(self) -> RobotObservation:
|
||||||
def get_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)
|
||||||
|
|
||||||
obs: dict[str, float] = {}
|
obs: RobotObservation = {}
|
||||||
for joint_name in PIPER_JOINT_NAMES:
|
for joint_name in PIPER_JOINT_NAMES:
|
||||||
raw_value = getattr(joint_state, joint_name, 0)
|
raw_value = getattr(joint_state, joint_name, 0)
|
||||||
obs[f"{joint_name}.pos"] = milli_to_unit(raw_value)
|
obs[f"{joint_name}.pos"] = milli_to_unit(raw_value)
|
||||||
@ -146,6 +194,47 @@ 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:
|
||||||
|
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()
|
||||||
@ -153,6 +242,11 @@ 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:
|
||||||
|
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()
|
self._refresh_motion_mode_if_needed()
|
||||||
|
|
||||||
sent_action: dict[str, float] = {}
|
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]
|
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)
|
has_all_joints = all(key in action for key in joint_keys)
|
||||||
if has_all_joints:
|
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)
|
self.arm.JointCtrl(*joint_commands)
|
||||||
sent_action.update({key: milli_to_unit(raw) for key, raw in zip(joint_keys, joint_commands, strict=True)})
|
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):
|
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.")
|
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:
|
||||||
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(
|
self.arm.GripperCtrl(
|
||||||
gripper_pos_raw,
|
gripper_pos_raw,
|
||||||
self.config.gripper_effort_default,
|
self.config.gripper_effort_default,
|
||||||
|
|||||||
@ -53,6 +53,10 @@ 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
|
||||||
|
|
||||||
|
# Calibration precision:
|
||||||
|
# homing_offset/range_min/range_max are stored as "degree * calibration_scale".
|
||||||
|
calibration_scale: int = 1000
|
||||||
|
|
||||||
# Safety behavior on disconnect
|
# Safety behavior on disconnect
|
||||||
disable_on_disconnect: bool = False
|
disable_on_disconnect: bool = False
|
||||||
|
|
||||||
@ -67,6 +71,8 @@ class PiperLeaderConfig(TeleoperatorConfig, PiperLeaderConfigBase):
|
|||||||
raise ValueError("`mode_refresh_interval_s` must be >= 0.")
|
raise ValueError("`mode_refresh_interval_s` must be >= 0.")
|
||||||
if self.enable_timeout_s < 0:
|
if self.enable_timeout_s < 0:
|
||||||
raise ValueError("`enable_timeout_s` must be >= 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:
|
if self.startup_sleep_s < 0:
|
||||||
raise ValueError("`startup_sleep_s` must be >= 0.")
|
raise ValueError("`startup_sleep_s` must be >= 0.")
|
||||||
if not (0 <= self.gripper_effort_default <= 5000):
|
if not (0 <= self.gripper_effort_default <= 5000):
|
||||||
|
|||||||
@ -19,6 +19,7 @@ import time
|
|||||||
from functools import cached_property
|
from functools import cached_property
|
||||||
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 (
|
||||||
@ -29,11 +30,14 @@ from lerobot.utils.piper_sdk import (
|
|||||||
parse_piper_log_level,
|
parse_piper_log_level,
|
||||||
unit_to_milli,
|
unit_to_milli,
|
||||||
)
|
)
|
||||||
|
from lerobot.utils.utils import enter_pressed, move_cursor_up
|
||||||
|
|
||||||
from ..teleoperator import Teleoperator
|
from ..teleoperator import Teleoperator
|
||||||
from .config_piper_leader import PiperLeaderConfig
|
from .config_piper_leader import PiperLeaderConfig
|
||||||
|
|
||||||
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 PiperLeader(Teleoperator):
|
class PiperLeader(Teleoperator):
|
||||||
@ -71,7 +75,6 @@ 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)
|
||||||
@ -87,6 +90,9 @@ class PiperLeader(Teleoperator):
|
|||||||
self.arm.MasterSlaveConfig(0xFC, 0x00, 0x00, 0x00)
|
self.arm.MasterSlaveConfig(0xFC, 0x00, 0x00, 0x00)
|
||||||
time.sleep(0.05)
|
time.sleep(0.05)
|
||||||
self.configure()
|
self.configure()
|
||||||
|
if not self.is_calibrated and calibrate:
|
||||||
|
logger.info("No piper-leader calibration file found for '%s'. Running lerobot-calibrate flow.", self.id)
|
||||||
|
self.calibrate()
|
||||||
except Exception:
|
except Exception:
|
||||||
self.arm.DisconnectPort()
|
self.arm.DisconnectPort()
|
||||||
self._is_connected = False
|
self._is_connected = False
|
||||||
@ -96,10 +102,47 @@ class PiperLeader(Teleoperator):
|
|||||||
|
|
||||||
@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:
|
||||||
logger.info("Piper leader 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)
|
||||||
|
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
|
||||||
@ -172,8 +215,7 @@ class PiperLeader(Teleoperator):
|
|||||||
return None
|
return None
|
||||||
return abs(milli_to_unit(getattr(gripper_state, "grippers_angle", 0)))
|
return abs(milli_to_unit(getattr(gripper_state, "grippers_angle", 0)))
|
||||||
|
|
||||||
@check_if_not_connected
|
def _read_raw_action(self) -> RobotAction:
|
||||||
def get_action(self) -> RobotAction:
|
|
||||||
action: dict[str, float] | None = None
|
action: dict[str, float] | None = None
|
||||||
if self.config.prefer_ctrl_messages:
|
if self.config.prefer_ctrl_messages:
|
||||||
action = self._read_joint_from_ctrl()
|
action = self._read_joint_from_ctrl()
|
||||||
@ -184,29 +226,92 @@ class PiperLeader(Teleoperator):
|
|||||||
if action is None:
|
if action is None:
|
||||||
action = {f"{joint_name}.pos": 0.0 for joint_name in PIPER_JOINT_NAMES}
|
action = {f"{joint_name}.pos": 0.0 for joint_name in PIPER_JOINT_NAMES}
|
||||||
|
|
||||||
if self.config.sync_gripper:
|
gripper_pos = self._read_gripper_from_ctrl() if self.config.prefer_ctrl_messages else None
|
||||||
gripper_pos = self._read_gripper_from_ctrl() if self.config.prefer_ctrl_messages else None
|
if gripper_pos is None and self.config.fallback_to_feedback:
|
||||||
if gripper_pos is None and self.config.fallback_to_feedback:
|
gripper_pos = self._read_gripper_from_feedback()
|
||||||
gripper_pos = self._read_gripper_from_feedback()
|
action["gripper.pos"] = 0.0 if gripper_pos is None else gripper_pos
|
||||||
action["gripper.pos"] = 0.0 if gripper_pos is None else gripper_pos
|
return action
|
||||||
else:
|
|
||||||
action["gripper.pos"] = 0.0
|
|
||||||
|
|
||||||
|
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:
|
||||||
|
raise RuntimeError(
|
||||||
|
f"{self} is not calibrated. Run `lerobot-calibrate --teleop.type=piper_leader --teleop.id={self.id}` first."
|
||||||
|
)
|
||||||
|
|
||||||
|
raw_action = self._read_raw_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
|
||||||
return action
|
return action
|
||||||
|
|
||||||
@check_if_not_connected
|
@check_if_not_connected
|
||||||
def send_feedback(self, feedback: dict[str, Any]) -> None:
|
def send_feedback(self, feedback: dict[str, Any]) -> None:
|
||||||
|
if not self.is_calibrated:
|
||||||
|
raise RuntimeError(
|
||||||
|
f"{self} is not calibrated. Run `lerobot-calibrate --teleop.type=piper_leader --teleop.id={self.id}` first."
|
||||||
|
)
|
||||||
|
|
||||||
self.set_manual_control(False)
|
self.set_manual_control(False)
|
||||||
self._refresh_command_mode_if_needed()
|
self._refresh_command_mode_if_needed()
|
||||||
|
|
||||||
joint_keys = [f"{joint_name}.pos" for joint_name in PIPER_JOINT_NAMES]
|
joint_keys = [f"{joint_name}.pos" for joint_name in PIPER_JOINT_NAMES]
|
||||||
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:
|
||||||
joint_commands = [unit_to_milli(feedback[key]) for key in joint_keys]
|
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]
|
||||||
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:
|
||||||
gripper_pos_raw = unit_to_milli(abs(feedback["gripper.pos"]))
|
gripper_target = self._offset_to_calibrated("gripper.pos", feedback["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,
|
||||||
|
|||||||
@ -3,6 +3,7 @@ from types import SimpleNamespace
|
|||||||
import lerobot.utils.piper_sdk as piper_sdk_utils
|
import lerobot.utils.piper_sdk as piper_sdk_utils
|
||||||
import lerobot.robots.piper_follower.piper_follower as piper_follower_module
|
import lerobot.robots.piper_follower.piper_follower as piper_follower_module
|
||||||
import lerobot.teleoperators.piper_leader.piper_leader as piper_leader_module
|
import lerobot.teleoperators.piper_leader.piper_leader as piper_leader_module
|
||||||
|
from lerobot.motors import MotorCalibration
|
||||||
from lerobot.robots.piper_follower import PiperFollower, PiperFollowerConfig
|
from lerobot.robots.piper_follower import PiperFollower, PiperFollowerConfig
|
||||||
from lerobot.robots.utils import make_robot_from_config
|
from lerobot.robots.utils import make_robot_from_config
|
||||||
from lerobot.teleoperators.piper_leader import PiperLeader, PiperLeaderConfig
|
from lerobot.teleoperators.piper_leader import PiperLeader, PiperLeaderConfig
|
||||||
@ -114,6 +115,19 @@ def patch_fake_sdk(monkeypatch):
|
|||||||
monkeypatch.setattr(piper_leader_module, "get_piper_sdk", fake_loader)
|
monkeypatch.setattr(piper_leader_module, "get_piper_sdk", fake_loader)
|
||||||
|
|
||||||
|
|
||||||
|
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([f"joint_{i}.pos" for i in range(1, 7)] + ["gripper.pos"])
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
def test_piper_leader_follower_teleop_roundtrip(monkeypatch):
|
def test_piper_leader_follower_teleop_roundtrip(monkeypatch):
|
||||||
patch_fake_sdk(monkeypatch)
|
patch_fake_sdk(monkeypatch)
|
||||||
|
|
||||||
@ -135,8 +149,11 @@ def test_piper_leader_follower_teleop_roundtrip(monkeypatch):
|
|||||||
assert isinstance(teleop, PiperLeader)
|
assert isinstance(teleop, PiperLeader)
|
||||||
assert isinstance(robot, PiperFollower)
|
assert isinstance(robot, PiperFollower)
|
||||||
|
|
||||||
teleop.connect()
|
teleop.calibration = make_identity_calibration()
|
||||||
robot.connect()
|
robot.calibration = make_identity_calibration()
|
||||||
|
|
||||||
|
teleop.connect(calibrate=False)
|
||||||
|
robot.connect(calibrate=False)
|
||||||
try:
|
try:
|
||||||
action = teleop.get_action()
|
action = teleop.get_action()
|
||||||
sent = robot.send_action(action)
|
sent = robot.send_action(action)
|
||||||
@ -167,14 +184,29 @@ def test_piper_leader_follower_teleop_roundtrip(monkeypatch):
|
|||||||
robot.disconnect()
|
robot.disconnect()
|
||||||
|
|
||||||
|
|
||||||
def test_piper_calibration_is_noop(monkeypatch):
|
def test_piper_requires_calibration(monkeypatch):
|
||||||
patch_fake_sdk(monkeypatch)
|
patch_fake_sdk(monkeypatch)
|
||||||
|
|
||||||
teleop = PiperLeader(PiperLeaderConfig(port="can1"))
|
teleop = PiperLeader(PiperLeaderConfig(port="can1"))
|
||||||
robot = PiperFollower(PiperFollowerConfig(port="can0"))
|
robot = PiperFollower(PiperFollowerConfig(port="can0"))
|
||||||
|
|
||||||
assert teleop.is_calibrated
|
assert not teleop.is_calibrated
|
||||||
assert robot.is_calibrated
|
assert not robot.is_calibrated
|
||||||
|
|
||||||
teleop.calibrate()
|
teleop.connect(calibrate=False)
|
||||||
robot.calibrate()
|
robot.connect(calibrate=False)
|
||||||
|
try:
|
||||||
|
try:
|
||||||
|
teleop.get_action()
|
||||||
|
assert False, "Expected teleop.get_action() to require calibration."
|
||||||
|
except RuntimeError:
|
||||||
|
pass
|
||||||
|
|
||||||
|
try:
|
||||||
|
robot.send_action({"joint_1.pos": 0.0})
|
||||||
|
assert False, "Expected robot.send_action() to require calibration."
|
||||||
|
except RuntimeError:
|
||||||
|
pass
|
||||||
|
finally:
|
||||||
|
teleop.disconnect()
|
||||||
|
robot.disconnect()
|
||||||
|
|||||||
在新工单中引用
屏蔽一个用户