镜像自地址
https://github.com/MINT-SJTU/Evo-RL.git
已同步 2026-11-20 18:17:06 +00:00
refactor(piper): tighten reconnect and rollback behavior
- reset leader manual-control cache on reconnect/disconnect to avoid stale mode state - rollback already-connected follower cameras when connect fails - reuse piper joint action key constant and add regression tests
这个提交包含在:
@ -25,6 +25,7 @@ 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,
|
||||
milli_to_unit,
|
||||
@ -87,6 +88,7 @@ class PiperFollower(Robot):
|
||||
time.sleep(self.config.startup_sleep_s)
|
||||
|
||||
self._is_connected = True
|
||||
connected_cameras = []
|
||||
try:
|
||||
if self.config.set_follower_mode_on_connect:
|
||||
self.arm.MasterSlaveConfig(0xFC, 0x00, 0x00, 0x00)
|
||||
@ -104,8 +106,11 @@ class PiperFollower(Robot):
|
||||
|
||||
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
|
||||
|
||||
@ -251,7 +256,7 @@ class PiperFollower(Robot):
|
||||
|
||||
sent_action: dict[str, float] = {}
|
||||
|
||||
joint_keys = [f"{joint_name}.pos" for joint_name in PIPER_JOINT_NAMES]
|
||||
joint_keys = PIPER_JOINT_ACTION_KEYS
|
||||
has_all_joints = all(key in action for key in joint_keys)
|
||||
if has_all_joints:
|
||||
joint_targets = [self._offset_to_target(key, action[key]) for key in joint_keys]
|
||||
|
||||
@ -24,6 +24,7 @@ from lerobot.processor import RobotAction
|
||||
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,
|
||||
milli_to_unit,
|
||||
@ -80,6 +81,8 @@ class PiperLeader(Teleoperator):
|
||||
time.sleep(self.config.startup_sleep_s)
|
||||
|
||||
self._is_connected = True
|
||||
# Recompute control mode on every fresh connection.
|
||||
self._manual_control_enabled = None
|
||||
try:
|
||||
if self.config.set_leader_mode_on_connect:
|
||||
# NOTE:
|
||||
@ -216,17 +219,20 @@ class PiperLeader(Teleoperator):
|
||||
return abs(milli_to_unit(getattr(gripper_state, "grippers_angle", 0)))
|
||||
|
||||
def _read_raw_action(self) -> RobotAction:
|
||||
used_feedback_for_joints = False
|
||||
action: dict[str, float] | None = None
|
||||
if self.config.prefer_ctrl_messages:
|
||||
action = self._read_joint_from_ctrl()
|
||||
|
||||
if action is None and self.config.fallback_to_feedback:
|
||||
action = self._read_joint_from_feedback()
|
||||
used_feedback_for_joints = action is not None
|
||||
|
||||
if action is None:
|
||||
action = {f"{joint_name}.pos": 0.0 for joint_name in PIPER_JOINT_NAMES}
|
||||
|
||||
gripper_pos = self._read_gripper_from_ctrl() if self.config.prefer_ctrl_messages else None
|
||||
use_ctrl_for_gripper = self.config.prefer_ctrl_messages and not used_feedback_for_joints
|
||||
gripper_pos = self._read_gripper_from_ctrl() if use_ctrl_for_gripper else None
|
||||
if gripper_pos is None and self.config.fallback_to_feedback:
|
||||
gripper_pos = self._read_gripper_from_feedback()
|
||||
action["gripper.pos"] = 0.0 if gripper_pos is None else gripper_pos
|
||||
@ -302,7 +308,7 @@ class PiperLeader(Teleoperator):
|
||||
self.set_manual_control(False)
|
||||
self._refresh_command_mode_if_needed()
|
||||
|
||||
joint_keys = [f"{joint_name}.pos" for joint_name in PIPER_JOINT_NAMES]
|
||||
joint_keys = PIPER_JOINT_ACTION_KEYS
|
||||
has_all_joints = all(key in feedback for key in joint_keys)
|
||||
if has_all_joints:
|
||||
joint_targets = [self._offset_to_calibrated(key, feedback[key]) for key in joint_keys]
|
||||
@ -327,4 +333,5 @@ class PiperLeader(Teleoperator):
|
||||
finally:
|
||||
self.arm.DisconnectPort()
|
||||
self._is_connected = False
|
||||
self._manual_control_enabled = None
|
||||
logger.info("%s disconnected.", self)
|
||||
|
||||
@ -27,7 +27,8 @@ PIPER_JOINT_NAMES = (
|
||||
"joint_5",
|
||||
"joint_6",
|
||||
)
|
||||
PIPER_ACTION_KEYS = tuple(f"{joint}.pos" for joint in PIPER_JOINT_NAMES) + ("gripper.pos",)
|
||||
PIPER_JOINT_ACTION_KEYS = tuple(f"{joint}.pos" for joint in PIPER_JOINT_NAMES)
|
||||
PIPER_ACTION_KEYS = PIPER_JOINT_ACTION_KEYS + ("gripper.pos",)
|
||||
|
||||
|
||||
def milli_to_unit(value: float | int) -> float:
|
||||
|
||||
@ -1,5 +1,7 @@
|
||||
from types import SimpleNamespace
|
||||
|
||||
import pytest
|
||||
|
||||
import lerobot.utils.piper_sdk as piper_sdk_utils
|
||||
import lerobot.robots.piper_follower.piper_follower as piper_follower_module
|
||||
import lerobot.teleoperators.piper_leader.piper_leader as piper_leader_module
|
||||
@ -8,6 +10,7 @@ from lerobot.robots.piper_follower import PiperFollower, PiperFollowerConfig
|
||||
from lerobot.robots.utils import make_robot_from_config
|
||||
from lerobot.teleoperators.piper_leader import PiperLeader, PiperLeaderConfig
|
||||
from lerobot.teleoperators.utils import make_teleoperator_from_config
|
||||
from lerobot.utils.piper_sdk import PIPER_ACTION_KEYS
|
||||
|
||||
|
||||
class FakeLogLevel:
|
||||
@ -124,7 +127,7 @@ def make_identity_calibration():
|
||||
range_min=-200000,
|
||||
range_max=200000,
|
||||
)
|
||||
for idx, key in enumerate([f"joint_{i}.pos" for i in range(1, 7)] + ["gripper.pos"])
|
||||
for idx, key in enumerate(PIPER_ACTION_KEYS)
|
||||
}
|
||||
|
||||
|
||||
@ -210,3 +213,58 @@ def test_piper_requires_calibration(monkeypatch):
|
||||
finally:
|
||||
teleop.disconnect()
|
||||
robot.disconnect()
|
||||
|
||||
|
||||
def test_piper_leader_reconnect_reapplies_mode(monkeypatch):
|
||||
patch_fake_sdk(monkeypatch)
|
||||
|
||||
teleop = PiperLeader(PiperLeaderConfig(port="can1", manual_control=False))
|
||||
teleop.calibration = make_identity_calibration()
|
||||
|
||||
teleop.connect(calibrate=False)
|
||||
teleop.disconnect()
|
||||
teleop.connect(calibrate=False)
|
||||
try:
|
||||
assert teleop.arm.enable_calls == 2
|
||||
finally:
|
||||
teleop.disconnect()
|
||||
|
||||
|
||||
def test_piper_follower_connect_rolls_back_connected_cameras(monkeypatch):
|
||||
patch_fake_sdk(monkeypatch)
|
||||
|
||||
class FakeCamera:
|
||||
def __init__(self, should_fail: bool):
|
||||
self.should_fail = should_fail
|
||||
self.is_connected = False
|
||||
self.disconnect_calls = 0
|
||||
|
||||
def connect(self):
|
||||
if self.should_fail:
|
||||
raise RuntimeError("camera connect failed")
|
||||
self.is_connected = True
|
||||
|
||||
def disconnect(self):
|
||||
self.disconnect_calls += 1
|
||||
self.is_connected = False
|
||||
|
||||
def async_read(self):
|
||||
return None
|
||||
|
||||
cam_ok = FakeCamera(should_fail=False)
|
||||
cam_fail = FakeCamera(should_fail=True)
|
||||
monkeypatch.setattr(
|
||||
piper_follower_module,
|
||||
"make_cameras_from_configs",
|
||||
lambda _: {"cam_ok": cam_ok, "cam_fail": cam_fail},
|
||||
)
|
||||
|
||||
robot = PiperFollower(PiperFollowerConfig(port="can0"))
|
||||
robot.calibration = make_identity_calibration()
|
||||
|
||||
with pytest.raises(RuntimeError, match="camera connect failed"):
|
||||
robot.connect(calibrate=False)
|
||||
|
||||
assert cam_ok.disconnect_calls == 1
|
||||
assert not cam_ok.is_connected
|
||||
assert not robot.is_connected
|
||||
|
||||
在新工单中引用
屏蔽一个用户