镜像自地址
https://github.com/MINT-SJTU/Evo-RL.git
已同步 2026-11-21 02:27:04 +00:00
fix(piper): bool calibration flag and robust can setup/test fallback
这个提交包含在:
@ -213,7 +213,7 @@ lerobot-setup-can --mode=setup --interfaces=can0,can1
|
|||||||
|
|
||||||
PiPER calibration is required by default.
|
PiPER calibration is required by default.
|
||||||
If needed, you can disable it with
|
If needed, you can disable it with
|
||||||
`--robot.calibration_mode=off` and `--teleop.calibration_mode=off`.
|
`--robot.require_calibration=false` and `--teleop.require_calibration=false`.
|
||||||
|
|
||||||
If you switch master/follower roles with the flags below, you must power-cycle both arms before enabling.
|
If you switch master/follower roles with the flags below, you must power-cycle both arms before enabling.
|
||||||
|
|
||||||
|
|||||||
@ -17,7 +17,7 @@
|
|||||||
from dataclasses import dataclass, field
|
from dataclasses import dataclass, field
|
||||||
|
|
||||||
from lerobot.cameras import CameraConfig
|
from lerobot.cameras import CameraConfig
|
||||||
from lerobot.utils.piper_sdk import PIPER_CALIBRATION_MODES
|
|
||||||
|
|
||||||
from ..config import RobotConfig
|
from ..config import RobotConfig
|
||||||
|
|
||||||
@ -50,10 +50,8 @@ class PiperFollowerConfigBase:
|
|||||||
# Calibration precision:
|
# Calibration precision:
|
||||||
# homing_offset/range_min/range_max are stored as "degree * calibration_scale".
|
# homing_offset/range_min/range_max are stored as "degree * calibration_scale".
|
||||||
calibration_scale: int = 1000
|
calibration_scale: int = 1000
|
||||||
# Calibration policy:
|
# Whether calibration is required on connect when no calibration file exists
|
||||||
# - required: always require calibration
|
require_calibration: bool = True
|
||||||
# - off: never require calibration
|
|
||||||
calibration_mode: str = "required"
|
|
||||||
|
|
||||||
# Gripper forwarding behavior
|
# Gripper forwarding behavior
|
||||||
sync_gripper: bool = True
|
sync_gripper: bool = True
|
||||||
@ -80,8 +78,8 @@ class PiperFollowerConfig(RobotConfig, PiperFollowerConfigBase):
|
|||||||
raise ValueError("`enable_timeout_s` must be >= 0.")
|
raise ValueError("`enable_timeout_s` must be >= 0.")
|
||||||
if self.calibration_scale <= 0:
|
if self.calibration_scale <= 0:
|
||||||
raise ValueError("`calibration_scale` must be > 0.")
|
raise ValueError("`calibration_scale` must be > 0.")
|
||||||
if self.calibration_mode not in PIPER_CALIBRATION_MODES:
|
if not isinstance(self.require_calibration, bool):
|
||||||
raise ValueError("`calibration_mode` must be one of: required, off.")
|
raise ValueError("require_calibration must be true or false.")
|
||||||
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):
|
||||||
|
|||||||
@ -29,7 +29,6 @@ from lerobot.utils.piper_sdk import (
|
|||||||
get_piper_sdk,
|
get_piper_sdk,
|
||||||
milli_to_unit,
|
milli_to_unit,
|
||||||
parse_piper_log_level,
|
parse_piper_log_level,
|
||||||
should_require_piper_calibration,
|
|
||||||
unit_to_milli,
|
unit_to_milli,
|
||||||
)
|
)
|
||||||
from lerobot.utils.utils import enter_pressed, move_cursor_up
|
from lerobot.utils.utils import enter_pressed, move_cursor_up
|
||||||
@ -98,7 +97,7 @@ class PiperFollower(Robot):
|
|||||||
should_auto_calibrate = (
|
should_auto_calibrate = (
|
||||||
not self.is_calibrated
|
not self.is_calibrated
|
||||||
and calibrate
|
and calibrate
|
||||||
and should_require_piper_calibration(self.config.calibration_mode)
|
and self.config.require_calibration
|
||||||
)
|
)
|
||||||
if should_auto_calibrate:
|
if should_auto_calibrate:
|
||||||
logger.info(
|
logger.info(
|
||||||
@ -125,7 +124,7 @@ class PiperFollower(Robot):
|
|||||||
logger.info("%s connected.", self)
|
logger.info("%s connected.", self)
|
||||||
|
|
||||||
def _use_uncalibrated_passthrough(self) -> bool:
|
def _use_uncalibrated_passthrough(self) -> bool:
|
||||||
return not self.is_calibrated and not should_require_piper_calibration(self.config.calibration_mode)
|
return not self.is_calibrated and not self.config.require_calibration
|
||||||
|
|
||||||
@property
|
@property
|
||||||
def is_calibrated(self) -> bool:
|
def is_calibrated(self) -> bool:
|
||||||
|
|||||||
@ -93,8 +93,8 @@ def check_interface_status(interface: str) -> tuple[bool, str, bool]:
|
|||||||
return False, "ip command not found", False
|
return False, "ip command not found", False
|
||||||
|
|
||||||
|
|
||||||
def setup_interface(interface: str, bitrate: int, data_bitrate: int, use_fd: bool) -> bool:
|
def setup_interface(interface: str, bitrate: int, data_bitrate: int, use_fd: bool) -> tuple[bool, str]:
|
||||||
"""Configure a CAN interface."""
|
"""Configure a CAN interface and return (success, error_message)."""
|
||||||
try:
|
try:
|
||||||
subprocess.run(["sudo", "ip", "link", "set", interface, "down"], check=False, capture_output=True) # nosec B607
|
subprocess.run(["sudo", "ip", "link", "set", interface, "down"], check=False, capture_output=True) # nosec B607
|
||||||
|
|
||||||
@ -104,20 +104,42 @@ def setup_interface(interface: str, bitrate: int, data_bitrate: int, use_fd: boo
|
|||||||
|
|
||||||
result = subprocess.run(cmd, capture_output=True, text=True) # nosec B607
|
result = subprocess.run(cmd, capture_output=True, text=True) # nosec B607
|
||||||
if result.returncode != 0:
|
if result.returncode != 0:
|
||||||
print(f" ✗ Failed to configure: {result.stderr}")
|
err = (result.stderr or result.stdout or "unknown error").strip()
|
||||||
return False
|
return False, f"configure failed: {err}"
|
||||||
|
|
||||||
result = subprocess.run( # nosec B607
|
result = subprocess.run( # nosec B607
|
||||||
["sudo", "ip", "link", "set", interface, "up"], capture_output=True, text=True
|
["sudo", "ip", "link", "set", interface, "up"], capture_output=True, text=True
|
||||||
)
|
)
|
||||||
if result.returncode != 0:
|
if result.returncode != 0:
|
||||||
print(f" ✗ Failed to bring up: {result.stderr}")
|
err = (result.stderr or result.stdout or "unknown error").strip()
|
||||||
return False
|
return False, f"bring-up failed: {err}"
|
||||||
|
|
||||||
return True
|
return True, ""
|
||||||
except Exception as e:
|
except Exception as e:
|
||||||
print(f" ✗ Error: {e}")
|
return False, f"exception: {e}"
|
||||||
return False
|
|
||||||
|
|
||||||
|
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)."""
|
||||||
|
attempts = [cfg.use_fd, not cfg.use_fd]
|
||||||
|
labels = {True: "CAN FD", False: "CAN 2.0"}
|
||||||
|
attempt_msgs: list[str] = []
|
||||||
|
|
||||||
|
for idx, use_fd in enumerate(attempts, start=1):
|
||||||
|
print(f" Attempt {idx}/{len(attempts)}: {labels[use_fd]}")
|
||||||
|
ok, err = setup_interface(interface, cfg.bitrate, cfg.data_bitrate, use_fd)
|
||||||
|
if ok:
|
||||||
|
is_up, status, is_fd_iface = check_interface_status(interface)
|
||||||
|
if is_up:
|
||||||
|
print(f" ✓ {interface}: {status} (using {labels[use_fd]})")
|
||||||
|
return True, use_fd, labels[use_fd]
|
||||||
|
err = f"interface not up after setup ({status})"
|
||||||
|
|
||||||
|
print(f" ✗ {err}")
|
||||||
|
attempt_msgs.append(f"{labels[use_fd]} -> {err}")
|
||||||
|
|
||||||
|
return False, None, " | ".join(attempt_msgs)
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
def test_motor(bus, motor_id: int, timeout: float, use_fd: bool):
|
def test_motor(bus, motor_id: int, timeout: float, use_fd: bool):
|
||||||
@ -162,7 +184,7 @@ def test_interface(cfg: CANSetupConfig, interface: str):
|
|||||||
"""Test all motors on a CAN interface."""
|
"""Test all motors on a CAN interface."""
|
||||||
import can
|
import can
|
||||||
|
|
||||||
is_up, status, _ = check_interface_status(interface)
|
is_up, status, is_fd_iface = check_interface_status(interface)
|
||||||
print(f"\n{interface}: {status}")
|
print(f"\n{interface}: {status}")
|
||||||
|
|
||||||
if not is_up:
|
if not is_up:
|
||||||
@ -170,8 +192,13 @@ def test_interface(cfg: CANSetupConfig, interface: str):
|
|||||||
return {}
|
return {}
|
||||||
|
|
||||||
try:
|
try:
|
||||||
|
# Avoid EINVAL on non-FD interfaces.
|
||||||
|
effective_use_fd = cfg.use_fd and is_fd_iface
|
||||||
|
if cfg.use_fd and not is_fd_iface:
|
||||||
|
print(" ⚠ Interface is CAN 2.0; auto-fallback to CAN 2.0 test frames.")
|
||||||
|
|
||||||
kwargs = {"channel": interface, "interface": "socketcan", "bitrate": cfg.bitrate}
|
kwargs = {"channel": interface, "interface": "socketcan", "bitrate": cfg.bitrate}
|
||||||
if cfg.use_fd:
|
if effective_use_fd:
|
||||||
kwargs.update({"data_bitrate": cfg.data_bitrate, "fd": True})
|
kwargs.update({"data_bitrate": cfg.data_bitrate, "fd": True})
|
||||||
bus = can.interface.Bus(**kwargs)
|
bus = can.interface.Bus(**kwargs)
|
||||||
except Exception as e:
|
except Exception as e:
|
||||||
@ -180,12 +207,15 @@ def test_interface(cfg: CANSetupConfig, interface: str):
|
|||||||
|
|
||||||
results = {}
|
results = {}
|
||||||
try:
|
try:
|
||||||
while bus.recv(timeout=0.01):
|
# Drain stale frames briefly, but never block indefinitely on a busy bus.
|
||||||
pass
|
drain_deadline = time.time() + 0.2
|
||||||
|
while time.time() < drain_deadline:
|
||||||
|
if bus.recv(timeout=0.01) is None:
|
||||||
|
break
|
||||||
|
|
||||||
for motor_id in cfg.motor_ids:
|
for motor_id in cfg.motor_ids:
|
||||||
motor_name = MOTOR_NAMES.get(motor_id, f"motor_0x{motor_id:02X}")
|
motor_name = MOTOR_NAMES.get(motor_id, f"motor_0x{motor_id:02X}")
|
||||||
responses, error = test_motor(bus, motor_id, cfg.timeout, cfg.use_fd)
|
responses, error = test_motor(bus, motor_id, cfg.timeout, effective_use_fd)
|
||||||
|
|
||||||
if error:
|
if error:
|
||||||
print(f" Motor 0x{motor_id:02X} ({motor_name}): ✗ {error}")
|
print(f" Motor 0x{motor_id:02X} ({motor_name}): ✗ {error}")
|
||||||
@ -213,7 +243,7 @@ def speed_test(cfg: CANSetupConfig, interface: str):
|
|||||||
"""Test communication speed with motors."""
|
"""Test communication speed with motors."""
|
||||||
import can
|
import can
|
||||||
|
|
||||||
is_up, status, _ = check_interface_status(interface)
|
is_up, status, is_fd_iface = check_interface_status(interface)
|
||||||
if not is_up:
|
if not is_up:
|
||||||
print(f"{interface}: {status} - skipping")
|
print(f"{interface}: {status} - skipping")
|
||||||
return
|
return
|
||||||
@ -221,8 +251,13 @@ def speed_test(cfg: CANSetupConfig, interface: str):
|
|||||||
print(f"\n{interface}: Running speed test ({cfg.speed_iterations} iterations)...")
|
print(f"\n{interface}: Running speed test ({cfg.speed_iterations} iterations)...")
|
||||||
|
|
||||||
try:
|
try:
|
||||||
|
# Avoid EINVAL on non-FD interfaces.
|
||||||
|
effective_use_fd = cfg.use_fd and is_fd_iface
|
||||||
|
if cfg.use_fd and not is_fd_iface:
|
||||||
|
print(" ⚠ Interface is CAN 2.0; auto-fallback to CAN 2.0 test frames.")
|
||||||
|
|
||||||
kwargs = {"channel": interface, "interface": "socketcan", "bitrate": cfg.bitrate}
|
kwargs = {"channel": interface, "interface": "socketcan", "bitrate": cfg.bitrate}
|
||||||
if cfg.use_fd:
|
if effective_use_fd:
|
||||||
kwargs.update({"data_bitrate": cfg.data_bitrate, "fd": True})
|
kwargs.update({"data_bitrate": cfg.data_bitrate, "fd": True})
|
||||||
bus = can.interface.Bus(**kwargs)
|
bus = can.interface.Bus(**kwargs)
|
||||||
except Exception as e:
|
except Exception as e:
|
||||||
@ -269,29 +304,58 @@ def speed_test(cfg: CANSetupConfig, interface: str):
|
|||||||
print(" ✗ No successful responses")
|
print(" ✗ No successful responses")
|
||||||
|
|
||||||
|
|
||||||
def run_setup(cfg: CANSetupConfig):
|
def run_setup(cfg: CANSetupConfig) -> bool:
|
||||||
"""Setup CAN interfaces."""
|
"""Setup CAN interfaces."""
|
||||||
print("=" * 50)
|
print("=" * 50)
|
||||||
print("CAN Interface Setup")
|
print("CAN Interface Setup")
|
||||||
print("=" * 50)
|
print("=" * 50)
|
||||||
print(f"Mode: {'CAN FD' if cfg.use_fd else 'CAN 2.0'}")
|
print(f"Primary mode: {'CAN FD' if cfg.use_fd else 'CAN 2.0'}")
|
||||||
|
print(f"Fallback mode: {'CAN 2.0' if cfg.use_fd else 'CAN FD'}")
|
||||||
print(f"Bitrate: {cfg.bitrate / 1_000_000:.1f} Mbps")
|
print(f"Bitrate: {cfg.bitrate / 1_000_000:.1f} Mbps")
|
||||||
if cfg.use_fd:
|
if cfg.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()
|
||||||
|
|
||||||
interfaces = cfg.get_interfaces()
|
interfaces = cfg.get_interfaces()
|
||||||
|
success_count = 0
|
||||||
|
failed: dict[str, str] = {}
|
||||||
|
mode_map: dict[str, bool] = {}
|
||||||
|
|
||||||
for interface in interfaces:
|
for interface in interfaces:
|
||||||
print(f"Configuring {interface}...")
|
print(f"Configuring {interface}...")
|
||||||
if setup_interface(interface, cfg.bitrate, cfg.data_bitrate, cfg.use_fd):
|
ok, used_fd, summary = setup_interface_with_fallback(interface, cfg)
|
||||||
is_up, status, _ = check_interface_status(interface)
|
if ok:
|
||||||
print(f" ✓ {interface}: {status}")
|
success_count += 1
|
||||||
|
mode_map[interface] = bool(used_fd)
|
||||||
else:
|
else:
|
||||||
print(f" ✗ {interface}: Failed")
|
failed[interface] = summary
|
||||||
|
print(f" ✗ {interface}: setup failed ({summary})")
|
||||||
|
|
||||||
print("\nSetup complete!")
|
print()
|
||||||
print("\nNext: Test motors with:")
|
print("=" * 50)
|
||||||
|
print("Setup summary")
|
||||||
|
print("=" * 50)
|
||||||
|
print(f"Interfaces configured: {success_count}/{len(interfaces)}")
|
||||||
|
if mode_map:
|
||||||
|
print("Active mode:")
|
||||||
|
for interface in interfaces:
|
||||||
|
if interface in mode_map:
|
||||||
|
print(f" - {interface}: {'CAN FD' if mode_map[interface] else 'CAN 2.0'}")
|
||||||
|
if failed:
|
||||||
|
print("Failed interfaces:")
|
||||||
|
for interface in interfaces:
|
||||||
|
if interface in failed:
|
||||||
|
print(f" - {interface}: {failed[interface]}")
|
||||||
|
print()
|
||||||
|
print("✗ Setup failed")
|
||||||
|
return False
|
||||||
|
|
||||||
|
print()
|
||||||
|
print("✓ Setup succeeded")
|
||||||
|
print()
|
||||||
|
print("Next: Test motors with:")
|
||||||
print(f" lerobot-setup-can --mode=test --interfaces {','.join(interfaces)}")
|
print(f" lerobot-setup-can --mode=test --interfaces {','.join(interfaces)}")
|
||||||
|
return True
|
||||||
|
|
||||||
|
|
||||||
def run_test(cfg: CANSetupConfig):
|
def run_test(cfg: CANSetupConfig):
|
||||||
@ -341,7 +405,9 @@ def setup_can(cfg: CANSetupConfig):
|
|||||||
sys.exit(1)
|
sys.exit(1)
|
||||||
|
|
||||||
if cfg.mode == "setup":
|
if cfg.mode == "setup":
|
||||||
run_setup(cfg)
|
ok = run_setup(cfg)
|
||||||
|
if not ok:
|
||||||
|
sys.exit(2)
|
||||||
elif cfg.mode == "test":
|
elif cfg.mode == "test":
|
||||||
run_test(cfg)
|
run_test(cfg)
|
||||||
elif cfg.mode == "speed":
|
elif cfg.mode == "speed":
|
||||||
|
|||||||
@ -16,7 +16,7 @@
|
|||||||
|
|
||||||
from dataclasses import dataclass
|
from dataclasses import dataclass
|
||||||
|
|
||||||
from lerobot.utils.piper_sdk import PIPER_CALIBRATION_MODES
|
|
||||||
|
|
||||||
from ..config import TeleoperatorConfig
|
from ..config import TeleoperatorConfig
|
||||||
|
|
||||||
@ -58,10 +58,8 @@ class PiperLeaderConfigBase:
|
|||||||
# Calibration precision:
|
# Calibration precision:
|
||||||
# homing_offset/range_min/range_max are stored as "degree * calibration_scale".
|
# homing_offset/range_min/range_max are stored as "degree * calibration_scale".
|
||||||
calibration_scale: int = 1000
|
calibration_scale: int = 1000
|
||||||
# Calibration policy:
|
# Whether calibration is required on connect when no calibration file exists
|
||||||
# - required: always require calibration
|
require_calibration: bool = True
|
||||||
# - off: never require calibration
|
|
||||||
calibration_mode: str = "required"
|
|
||||||
|
|
||||||
# Safety behavior on disconnect
|
# Safety behavior on disconnect
|
||||||
disable_on_disconnect: bool = False
|
disable_on_disconnect: bool = False
|
||||||
@ -79,8 +77,8 @@ class PiperLeaderConfig(TeleoperatorConfig, PiperLeaderConfigBase):
|
|||||||
raise ValueError("`enable_timeout_s` must be >= 0.")
|
raise ValueError("`enable_timeout_s` must be >= 0.")
|
||||||
if self.calibration_scale <= 0:
|
if self.calibration_scale <= 0:
|
||||||
raise ValueError("`calibration_scale` must be > 0.")
|
raise ValueError("`calibration_scale` must be > 0.")
|
||||||
if self.calibration_mode not in PIPER_CALIBRATION_MODES:
|
if not isinstance(self.require_calibration, bool):
|
||||||
raise ValueError("`calibration_mode` must be one of: required, off.")
|
raise ValueError("require_calibration must be true or false.")
|
||||||
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):
|
||||||
|
|||||||
@ -29,7 +29,6 @@ from lerobot.utils.piper_sdk import (
|
|||||||
get_piper_sdk,
|
get_piper_sdk,
|
||||||
milli_to_unit,
|
milli_to_unit,
|
||||||
parse_piper_log_level,
|
parse_piper_log_level,
|
||||||
should_require_piper_calibration,
|
|
||||||
unit_to_milli,
|
unit_to_milli,
|
||||||
)
|
)
|
||||||
from lerobot.utils.utils import enter_pressed, move_cursor_up
|
from lerobot.utils.utils import enter_pressed, move_cursor_up
|
||||||
@ -97,7 +96,7 @@ class PiperLeader(Teleoperator):
|
|||||||
if (
|
if (
|
||||||
not self.is_calibrated
|
not self.is_calibrated
|
||||||
and calibrate
|
and calibrate
|
||||||
and should_require_piper_calibration(self.config.calibration_mode)
|
and self.config.require_calibration
|
||||||
):
|
):
|
||||||
logger.info(
|
logger.info(
|
||||||
"No piper-leader calibration file found for '%s'. Running lerobot-calibrate flow.",
|
"No piper-leader calibration file found for '%s'. Running lerobot-calibrate flow.",
|
||||||
@ -112,7 +111,7 @@ class PiperLeader(Teleoperator):
|
|||||||
logger.info("%s connected.", self)
|
logger.info("%s connected.", self)
|
||||||
|
|
||||||
def _use_uncalibrated_passthrough(self) -> bool:
|
def _use_uncalibrated_passthrough(self) -> bool:
|
||||||
return not self.is_calibrated and not should_require_piper_calibration(self.config.calibration_mode)
|
return not self.is_calibrated and not self.config.require_calibration
|
||||||
|
|
||||||
@property
|
@property
|
||||||
def is_calibrated(self) -> bool:
|
def is_calibrated(self) -> bool:
|
||||||
|
|||||||
@ -29,7 +29,7 @@ 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_CALIBRATION_MODES = {"required", "off"}
|
|
||||||
|
|
||||||
|
|
||||||
def milli_to_unit(value: float | int) -> float:
|
def milli_to_unit(value: float | int) -> float:
|
||||||
@ -64,9 +64,3 @@ def parse_piper_log_level(level_name: str) -> Any:
|
|||||||
) from exc
|
) from exc
|
||||||
|
|
||||||
|
|
||||||
def should_require_piper_calibration(calibration_mode: str) -> bool:
|
|
||||||
if calibration_mode == "required":
|
|
||||||
return True
|
|
||||||
if calibration_mode == "off":
|
|
||||||
return False
|
|
||||||
raise ValueError(f"Invalid Piper calibration mode '{calibration_mode}'. Expected one of: required, off.")
|
|
||||||
|
|||||||
在新工单中引用
屏蔽一个用户