feat(piper): switch leader roles for teleop and policy control (#33)

* feat(piper): support read-only PiPER-X leader teleop

* trim PiPER-X read-only teleop scope

* refactor(piper): switch leader roles via manual control

* refactor(piper): remove calibration and gravity assets

* simplify(piper): use direct bimanual follower lifecycle

* simplify(piper): remove bimanual leader rollback logic

* refactor(piper): configure arm roles directly

* fix(piper): stabilize PiPER-X leader teleop startup

* fix(piper): configure leader gripper drag sensitivity

* fix(piper): forward bimanual gripper drag config

* revert(piper): remove recent startup workarounds

* fix(piper): make leader action reads non-blocking

* simplify(piper): remove follower send-only mode

* simplify(piper): remove follower mode refresh

* simplify(piper): use default leader connection

* simplify(piper): streamline leader role switching

* simplify(piper): streamline leader action reads

* simplify(piper): remove role switch delay

* docs(piper): remove role switching details

* feat(piper): address USB-CAN adapters by serial

---------

Co-authored-by: Elvin-yk <180030703+Elvin-yk@users.noreply.github.com>
这个提交包含在:
Travor King
2026-07-15 08:23:51 -05:00
提交者 GitHub
父节点 257d1326dc
当前提交 e439dd12d5
共有 33 个文件被更改,包括 396 次插入 和 1288 次删除

查看文件

@ -12,14 +12,10 @@ def test_teleoperate_parses_bi_piperx_types():
"--robot.id=my_bi_piperx_follower",
"--robot.left_arm_config.port=can0",
"--robot.right_arm_config.port=can2",
"--robot.left_arm_config.require_calibration=false",
"--robot.right_arm_config.require_calibration=false",
"--teleop.type=bi_piperx_leader",
"--teleop.id=my_bi_piperx_leader",
"--teleop.left_arm_config.port=can1",
"--teleop.right_arm_config.port=can3",
"--teleop.left_arm_config.require_calibration=false",
"--teleop.right_arm_config.require_calibration=false",
]
cfg = draccus.parse(config_class=TeleoperateConfig, config_path=None, args=args)
assert cfg.robot.type == "bi_piperx_follower"
@ -32,14 +28,10 @@ def test_record_rejects_mixed_bi_piper_pair():
"--robot.id=my_bi_piper_follower",
"--robot.left_arm_config.port=can0",
"--robot.right_arm_config.port=can2",
"--robot.left_arm_config.require_calibration=false",
"--robot.right_arm_config.require_calibration=false",
"--teleop.type=bi_piperx_leader",
"--teleop.id=my_bi_piperx_leader",
"--teleop.left_arm_config.port=can1",
"--teleop.right_arm_config.port=can3",
"--teleop.left_arm_config.require_calibration=false",
"--teleop.right_arm_config.require_calibration=false",
"--dataset.repo_id=dummy/dummy",
"--dataset.single_task=test",
"--dataset.num_episodes=1",
@ -62,8 +54,6 @@ def test_record_rejects_bimanual_piper_teleop_with_non_bimanual_robot():
"--teleop.id=my_bi_piperx_leader",
"--teleop.left_arm_config.port=can1",
"--teleop.right_arm_config.port=can3",
"--teleop.left_arm_config.require_calibration=false",
"--teleop.right_arm_config.require_calibration=false",
"--dataset.repo_id=dummy/dummy",
"--dataset.single_task=test",
"--dataset.num_episodes=1",

查看文件

@ -1,7 +1,4 @@
# ruff: noqa: N802
import sys
import threading
import time
from types import SimpleNamespace
import pytest
@ -10,7 +7,6 @@ import lerobot.robots.piper_follower.piper_follower as piper_follower_module
import lerobot.teleoperators.bi_piper_leader.bi_piper_leader as bi_piper_leader_module
import lerobot.teleoperators.piper_leader.piper_leader as piper_leader_module
import lerobot.utils.piper_sdk as piper_sdk_utils
from lerobot.motors import MotorCalibration
from lerobot.processor import make_default_processors
from lerobot.robots.bi_piper_follower import (
BiPiperFollower,
@ -26,6 +22,7 @@ from lerobot.robots.piper_follower import (
PiperXFollowerConfig,
)
from lerobot.robots.utils import make_robot_from_config
from lerobot.scripts.lerobot_teleoperate import teleop_loop
from lerobot.teleoperators.bi_piper_leader import (
BiPiperLeader,
BiPiperLeaderConfig,
@ -40,7 +37,6 @@ from lerobot.teleoperators.piper_leader import (
PiperXLeaderConfig,
PiperXLeaderConfigBase,
)
from lerobot.scripts.lerobot_teleoperate import teleop_loop
from lerobot.teleoperators.utils import make_teleoperator_from_config
from lerobot.utils.piper_sdk import PIPER_ACTION_KEYS
@ -71,6 +67,7 @@ class FakePiperInterface:
self.disable_calls = 0
self.is_enabled = False
self.connect_calls = []
self.command_log = []
self._joint_ctrl = SimpleNamespace(
time_stamp=1.0,
@ -134,14 +131,19 @@ class FakePiperInterface:
def MotionCtrl_2(self, *args):
self.mode_commands.append(args)
self.command_log.append(("motion_mode", args))
def MasterSlaveConfig(self, *args):
self.role_commands.append(args)
self.command_log.append(("role", args))
if args and args[0] == 0xFC:
self._arm_status.arm_status.ctrl_mode = 0x01
elif args and args[0] == 0xFA:
self._arm_status.arm_status.ctrl_mode = 0x06
def EnablePiper(self):
self.enable_calls += 1
self.command_log.append(("enable", ()))
self.is_enabled = True
return True
@ -152,6 +154,7 @@ class FakePiperInterface:
def JointCtrl(self, *args):
self.last_joint = args
self.command_log.append(("joint", args))
def JointMitCtrl(self, *args):
self.joint_mit_calls.append(args)
@ -159,6 +162,7 @@ class FakePiperInterface:
def GripperCtrl(self, *args):
self.last_gripper = args
self.gripper_calls.append(args)
self.command_log.append(("gripper", args))
def GetArmJointCtrl(self):
return self._joint_ctrl
@ -186,41 +190,21 @@ def patch_fake_sdk(monkeypatch):
monkeypatch.setattr(piper_sdk_utils, "get_piper_sdk", fake_loader)
monkeypatch.setattr(piper_follower_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(PIPER_ACTION_KEYS)
}
def wait_until(predicate, timeout_s: float = 0.2, poll_s: float = 0.005) -> bool:
deadline = time.monotonic() + timeout_s
while time.monotonic() < deadline:
if predicate():
return True
time.sleep(poll_s)
return False
monkeypatch.setattr(piper_follower_module, "resolve_piper_can_interface", lambda serial: serial)
monkeypatch.setattr(piper_leader_module, "resolve_piper_can_interface", lambda serial: serial)
@pytest.mark.parametrize(
("teleop_cfg", "robot_cfg", "teleop_cls", "robot_cls"),
[
(
PiperLeaderConfig(port="can1", manual_control=False, sync_gripper=True),
PiperLeaderConfig(port="can1", manual_control=True, sync_gripper=True),
PiperFollowerConfig(port="can0", sync_gripper=True),
PiperLeader,
PiperFollower,
),
(
PiperXLeaderConfig(port="can1", manual_control=False, sync_gripper=True),
PiperXLeaderConfig(port="can1", manual_control=True, sync_gripper=True),
PiperXFollowerConfig(port="can0", sync_gripper=True),
PiperXLeader,
PiperXFollower,
@ -236,12 +220,11 @@ def test_piper_leader_follower_teleop_roundtrip(monkeypatch, teleop_cfg, robot_c
assert isinstance(teleop, teleop_cls)
assert isinstance(robot, robot_cls)
teleop.calibration = make_identity_calibration()
robot.calibration = make_identity_calibration()
teleop.connect(calibrate=False)
robot.connect(calibrate=False)
teleop.connect()
robot.connect()
try:
assert teleop.arm.role_commands[-1] == (0xFA, 0x00, 0x00, 0x00)
assert robot.arm.role_commands == [(0xFC, 0x00, 0x00, 0x00)]
action = teleop.get_action()
sent = robot.send_action(action)
obs = robot.get_observation()
@ -259,6 +242,7 @@ def test_piper_leader_follower_teleop_roundtrip(monkeypatch, teleop_cfg, robot_c
assert obs["gripper.pos"] == 43.0
teleop.send_feedback(action)
assert teleop.arm.role_commands[-1] == (0xFC, 0x00, 0x00, 0x00)
assert teleop.arm.last_joint == (10000, 20000, 30000, 40000, 50000, 60000)
assert teleop.arm.last_gripper == (
42000,
@ -269,38 +253,49 @@ def test_piper_leader_follower_teleop_roundtrip(monkeypatch, teleop_cfg, robot_c
finally:
teleop.disconnect()
robot.disconnect()
assert teleop.arm.role_commands[-1] == (0xFA, 0x00, 0x00, 0x00)
def test_piper_requires_calibration(monkeypatch):
def test_piper_does_not_initialize_calibration_state(monkeypatch):
patch_fake_sdk(monkeypatch)
teleop = PiperLeader(PiperLeaderConfig(port="can1"))
robot = PiperFollower(PiperFollowerConfig(port="can0"))
leader = PiperLeader(PiperLeaderConfig(port="can1"))
follower = PiperFollower(PiperFollowerConfig(port="can0"))
bi_leader = BiPiperLeader(
BiPiperLeaderConfig(
left_arm_config=PiperLeaderConfigBase(port="can1"),
right_arm_config=PiperLeaderConfigBase(port="can3"),
process_isolation=False,
)
)
bi_follower = BiPiperFollower(
BiPiperFollowerConfig(
left_arm_config=PiperFollowerConfigBase(port="can0"),
right_arm_config=PiperFollowerConfigBase(port="can2"),
)
)
assert not teleop.is_calibrated
assert not robot.is_calibrated
teleop.connect(calibrate=False)
robot.connect(calibrate=False)
try:
with pytest.raises(RuntimeError, match="is not calibrated"):
teleop.get_action()
with pytest.raises(RuntimeError, match="is not calibrated"):
robot.send_action({"joint_1.pos": 0.0})
finally:
teleop.disconnect()
robot.disconnect()
devices = [
leader,
follower,
bi_leader,
bi_leader.left_arm,
bi_leader.right_arm,
bi_follower,
bi_follower.left_arm,
bi_follower.right_arm,
]
assert all(not hasattr(device, "calibration") for device in devices)
assert all(not hasattr(device, "calibration_fpath") for device in devices)
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.connect()
teleop.disconnect()
teleop.connect(calibrate=False)
teleop.connect()
try:
assert teleop.arm.enable_calls == 2
finally:
@ -337,51 +332,73 @@ def test_piper_follower_connect_rolls_back_connected_cameras(monkeypatch):
)
robot = PiperFollower(PiperFollowerConfig(port="can0"))
robot.calibration = make_identity_calibration()
with pytest.raises(RuntimeError, match="camera connect failed"):
robot.connect(calibrate=False)
robot.connect()
assert cam_ok.disconnect_calls == 1
assert not cam_ok.is_connected
assert not robot.is_connected
def test_piper_require_calibration_false_allows_uncalibrated_control(monkeypatch):
def test_piper_leader_default_uses_hardware_leader_role(monkeypatch):
patch_fake_sdk(monkeypatch)
teleop = PiperLeader(PiperLeaderConfig(port="can1"))
teleop = PiperLeader(PiperLeaderConfig(port="can1", require_calibration=False))
robot = PiperFollower(PiperFollowerConfig(port="can0", require_calibration=False))
teleop.connect(calibrate=False)
robot.connect(calibrate=False)
teleop.connect()
try:
assert teleop.arm.connect_calls[-1]["piper_init"] is False
assert teleop.arm.role_commands == [(0xFA, 0x00, 0x00, 0x00)]
assert teleop.arm.enable_calls == 0
assert teleop.arm.mode_commands == []
assert teleop.arm.gripper_calls == []
assert teleop.arm.joint_mit_calls == []
action = teleop.get_action()
sent = robot.send_action(action)
assert sent["joint_1.pos"] == 11.0
assert sent["gripper.pos"] == 43.0
assert action["joint_1.pos"] == 10.0
assert action["joint_2.pos"] == 20.0
assert action["gripper.pos"] == 42.0
finally:
teleop.disconnect()
robot.disconnect()
def test_piper_gravity_comp_defaults_are_model_specific():
assert PiperLeaderConfig(port="can1").gravity_comp_tx_ratio == (0.2, 0.2, 0.2, 0.2, 0.2, 0.2)
assert PiperXLeaderConfig(port="can1").gravity_comp_tx_ratio == (1.0, 1.0, 1.0, 1.0, 1.0, 1.0)
def test_piper_leader_manual_control_prefers_feedback_state(monkeypatch):
def test_piper_leader_switches_between_manual_and_policy_control(monkeypatch):
patch_fake_sdk(monkeypatch)
teleop = PiperLeader(PiperLeaderConfig(port="can1"))
teleop.connect()
try:
teleop.arm.command_log.clear()
teleop.set_manual_control(False)
assert teleop.arm.role_commands[-1] == (0xFC, 0x00, 0x00, 0x00)
assert teleop.arm.mode_commands[-1][:3] == (0x01, 0x01, teleop.config.command_speed_ratio)
assert teleop.arm.enable_calls == 1
assert [name for name, _ in teleop.arm.command_log[:3]] == ["role", "motion_mode", "enable"]
teleop = PiperLeader(
PiperLeaderConfig(
port="can1",
manual_control=True,
require_calibration=False,
)
)
role_count = len(teleop.arm.role_commands)
action = {
"joint_1.pos": 1.0,
"joint_2.pos": 2.0,
"joint_3.pos": 3.0,
"joint_4.pos": 4.0,
"joint_5.pos": 5.0,
"joint_6.pos": 6.0,
"gripper.pos": 7.0,
}
teleop.send_feedback(action)
assert len(teleop.arm.role_commands) == role_count
assert teleop.arm.last_joint == (1000, 2000, 3000, 4000, 5000, 6000)
assert teleop.arm.last_gripper[0] == 7000
teleop.connect(calibrate=False)
teleop.set_manual_control(True)
assert teleop.arm.role_commands[-1] == (0xFA, 0x00, 0x00, 0x00)
assert teleop.arm.joint_mit_calls == []
finally:
teleop.disconnect()
def test_piper_leader_policy_mode_reads_feedback(monkeypatch):
patch_fake_sdk(monkeypatch)
teleop = PiperLeader(PiperLeaderConfig(port="can1", manual_control=False))
teleop.connect()
try:
action = teleop.get_action()
assert action["joint_1.pos"] == 11.0
@ -391,134 +408,51 @@ def test_piper_leader_manual_control_prefers_feedback_state(monkeypatch):
teleop.disconnect()
def test_piper_leader_manual_control_waits_for_fresh_feedback(monkeypatch):
def test_piper_leader_returns_empty_action_without_manual_frames(monkeypatch):
patch_fake_sdk(monkeypatch)
teleop = PiperLeader(
PiperLeaderConfig(
port="can1",
manual_control=True,
require_calibration=False,
)
)
teleop.connect(calibrate=False)
teleop = PiperLeader(PiperLeaderConfig(port="can1"))
teleop.connect()
try:
initial = teleop.get_action()
assert initial["joint_1.pos"] == 11.0
def publish_fresh_feedback():
time.sleep(0.01)
teleop.arm._joint_state.time_stamp = 2.0
teleop.arm._joint_state.joint_state.joint_1 = 12000
teleop.arm._joint_state.joint_state.joint_2 = 22000
teleop.arm._joint_state.joint_state.joint_3 = 32000
teleop.arm._joint_state.joint_state.joint_4 = 42000
teleop.arm._joint_state.joint_state.joint_5 = 52000
teleop.arm._joint_state.joint_state.joint_6 = 62000
teleop.arm._gripper_state.time_stamp = 2.0
teleop.arm._gripper_state.gripper_state.grippers_angle = 44000
publisher = threading.Thread(target=publish_fresh_feedback, daemon=True)
publisher.start()
t0 = time.monotonic()
action = teleop.get_action()
publisher.join(timeout=0.1)
assert time.monotonic() - t0 >= 0.008
assert action["joint_1.pos"] == 12.0
assert action["joint_2.pos"] == 22.0
assert action["gripper.pos"] == 44.0
teleop.arm._joint_ctrl.time_stamp = 0.0
assert teleop.get_action() == {}
finally:
teleop.disconnect()
def test_piper_leader_manual_control_does_not_unblock_on_gripper_feedback_only(monkeypatch):
def test_piper_leader_failed_policy_switch_restores_manual_role(monkeypatch):
patch_fake_sdk(monkeypatch)
teleop = PiperLeader(
PiperLeaderConfig(
port="can1",
manual_control=True,
require_calibration=False,
)
)
teleop.connect(calibrate=False)
teleop = PiperLeader(PiperLeaderConfig(port="can1", enable_timeout_s=0.0))
teleop.connect()
try:
initial = teleop.get_action()
assert initial["joint_1.pos"] == 11.0
def publish_feedback():
time.sleep(0.005)
teleop.arm._gripper_state.time_stamp = 2.0
teleop.arm._gripper_state.gripper_state.grippers_angle = 45000
time.sleep(0.01)
teleop.arm._joint_state.time_stamp = 2.0
teleop.arm._joint_state.joint_state.joint_1 = 13000
teleop.arm._joint_state.joint_state.joint_2 = 23000
teleop.arm._joint_state.joint_state.joint_3 = 33000
teleop.arm._joint_state.joint_state.joint_4 = 43000
teleop.arm._joint_state.joint_state.joint_5 = 53000
teleop.arm._joint_state.joint_state.joint_6 = 63000
publisher = threading.Thread(target=publish_feedback, daemon=True)
publisher.start()
t0 = time.monotonic()
action = teleop.get_action()
publisher.join(timeout=0.1)
assert time.monotonic() - t0 >= 0.013
assert action["joint_1.pos"] == 13.0
with pytest.raises(RuntimeError, match="did not enable"):
teleop.set_manual_control(False)
assert teleop._manual_control_enabled is True
assert teleop.arm.role_commands[-1][0] == 0xFA
finally:
teleop.disconnect()
def test_piper_follower_connect_calibrates_then_reenables(monkeypatch, tmp_path):
def test_piper_leader_disconnect_disables_if_role_restore_fails(monkeypatch):
patch_fake_sdk(monkeypatch)
teleop = PiperLeader(PiperLeaderConfig(port="can1", manual_control=False))
teleop.connect()
robot = PiperFollower(
PiperFollowerConfig(
port="can0",
id="connect_reenable_after_calibration",
calibration_dir=tmp_path,
require_calibration=True,
enable_on_connect=True,
)
)
def fail_role_restore(*args):
raise RuntimeError("leader role restore failed")
def fake_calibrate():
# Mirror real calibration's drag-mode behavior.
robot.arm.DisableArm(7)
robot.calibration = make_identity_calibration()
monkeypatch.setattr(teleop.arm, "MasterSlaveConfig", fail_role_restore)
monkeypatch.setattr(robot, "calibrate", fake_calibrate)
robot.connect(calibrate=True)
try:
assert robot.arm.disable_calls == 1
assert robot.arm.enable_calls == 1
assert robot.arm.is_enabled
finally:
robot.disconnect()
with pytest.raises(RuntimeError, match="leader role restore failed"):
teleop.disconnect()
assert teleop.arm.disable_calls == 1
assert not teleop.arm.connected
def test_piper_follower_connect_without_calibration_still_enables(monkeypatch, tmp_path):
def test_piper_follower_connect_enables_arm(monkeypatch):
patch_fake_sdk(monkeypatch)
robot = PiperFollower(PiperFollowerConfig(port="can0", enable_on_connect=True))
robot = PiperFollower(
PiperFollowerConfig(
port="can0",
id="connect_enable_without_calibration",
calibration_dir=tmp_path,
require_calibration=True,
enable_on_connect=True,
)
)
robot.connect(calibrate=False)
robot.connect()
try:
assert robot.arm.enable_calls == 1
assert robot.arm.is_enabled
@ -527,90 +461,17 @@ def test_piper_follower_connect_without_calibration_still_enables(monkeypatch, t
robot.disconnect()
def test_piper_leader_gravity_comp_manual_control_uses_mit(monkeypatch):
def test_piper_follower_connect_sets_follower_role_without_restart(monkeypatch):
patch_fake_sdk(monkeypatch)
class FakeModel:
def __init__(self):
self.nq = 6
self.nv = 6
self.gravity = SimpleNamespace(linear=None)
def createData(self):
return object()
class FakeRobotWrapper:
@staticmethod
def BuildFromURDF(urdf_path, package_dirs):
del urdf_path, package_dirs
return SimpleNamespace(model=FakeModel(), data=object())
class FakePinocchio:
RobotWrapper = FakeRobotWrapper
@staticmethod
def rnea(model, data, q, v, a):
del model, data, q, v, a
return [0.1] * 6
monkeypatch.setitem(sys.modules, "pinocchio", FakePinocchio)
teleop = PiperLeader(
PiperLeaderConfig(
port="can1",
manual_control=True,
gravity_comp_control_hz=200.0,
mode_refresh_interval_s=0.01,
)
)
teleop.calibration = make_identity_calibration()
teleop.connect(calibrate=False)
try:
assert wait_until(lambda: len(teleop.arm.joint_mit_calls) > 0)
assert teleop.arm.disable_calls == 0
assert len(teleop.arm.joint_mit_calls) > 0
assert len(teleop.arm.gripper_calls) > 0
assert teleop.arm.gripper_calls[0][2] == 0x00
teleop.set_manual_control(False)
calls_after_stop = len(teleop.arm.joint_mit_calls)
time.sleep(0.03)
assert len(teleop.arm.joint_mit_calls) == calls_after_stop
assert teleop.arm.gripper_calls[-1][2] == 0x01
finally:
teleop.disconnect()
@pytest.mark.parametrize(
("device_kind", "port", "robot_id"),
[("leader", "can1", None), ("follower", "can0", "piper_role_guard")],
)
def test_piper_connect_fails_and_writes_follower_role_when_in_teach_mode(
monkeypatch, device_kind, port, robot_id
):
patch_fake_sdk(monkeypatch)
if device_kind == "leader":
device = PiperLeader(PiperLeaderConfig(port=port))
else:
device = PiperFollower(PiperFollowerConfig(port=port, id=robot_id))
device = PiperFollower(PiperFollowerConfig(port="can0"))
device.arm._arm_status.arm_status.ctrl_mode = 0x06
with pytest.raises(RuntimeError, match="Follower role command .* sent.*Power-cycle"):
device.connect(calibrate=False)
assert device.arm.role_commands[-1] == (0xFC, 0x00, 0x00, 0x00)
def test_piper_lfs_pointer_urdf_raises_actionable_error(tmp_path):
pointer_file = tmp_path / "lfs_pointer.urdf"
pointer_file.write_text("version https://git-lfs.github.com/spec/v1\noid sha256:deadbeef\nsize 123\n")
with pytest.raises(RuntimeError, match="Git LFS pointer files"):
piper_leader_module._ensure_not_lfs_pointer(
pointer_file, "assets/piper_description/urdf/pointer.urdf"
)
device.connect()
try:
assert device.arm.role_commands == [(0xFC, 0x00, 0x00, 0x00)]
assert [name for name, _ in device.arm.command_log[:3]] == ["role", "motion_mode", "enable"]
finally:
device.disconnect()
@pytest.mark.parametrize(
@ -627,8 +488,8 @@ def test_piper_lfs_pointer_urdf_raises_actionable_error(tmp_path):
[
(
BiPiperLeaderConfig(
left_arm_config=PiperLeaderConfigBase(port="can1", manual_control=False, sync_gripper=True),
right_arm_config=PiperLeaderConfigBase(port="can3", manual_control=False, sync_gripper=True),
left_arm_config=PiperLeaderConfigBase(port="can1", manual_control=True, sync_gripper=True),
right_arm_config=PiperLeaderConfigBase(port="can3", manual_control=True, sync_gripper=True),
process_isolation=False,
),
BiPiperFollowerConfig(
@ -644,8 +505,8 @@ def test_piper_lfs_pointer_urdf_raises_actionable_error(tmp_path):
),
(
BiPiperXLeaderConfig(
left_arm_config=PiperXLeaderConfigBase(port="can1", manual_control=False, sync_gripper=True),
right_arm_config=PiperXLeaderConfigBase(port="can3", manual_control=False, sync_gripper=True),
left_arm_config=PiperXLeaderConfigBase(port="can1", manual_control=True, sync_gripper=True),
right_arm_config=PiperXLeaderConfigBase(port="can3", manual_control=True, sync_gripper=True),
process_isolation=False,
),
BiPiperXFollowerConfig(
@ -684,14 +545,11 @@ def test_bimanual_piper_leader_follower_roundtrip(
assert isinstance(robot.left_arm, left_robot_cls)
assert isinstance(robot.right_arm, right_robot_cls)
teleop.left_arm.calibration = make_identity_calibration()
teleop.right_arm.calibration = make_identity_calibration()
robot.left_arm.calibration = make_identity_calibration()
robot.right_arm.calibration = make_identity_calibration()
teleop.connect(calibrate=False)
robot.connect(calibrate=False)
teleop.connect()
robot.connect()
try:
assert teleop.left_arm.arm.role_commands[-1][0] == 0xFA
assert teleop.right_arm.arm.role_commands[-1][0] == 0xFA
action = teleop.get_action()
assert "left_joint_1.pos" in action
assert "right_joint_1.pos" in action
@ -714,6 +572,8 @@ def test_bimanual_piper_leader_follower_roundtrip(
assert obs["right_gripper.pos"] == 43.0
teleop.send_feedback(action)
assert teleop.left_arm.arm.role_commands[-1][0] == 0xFC
assert teleop.right_arm.arm.role_commands[-1][0] == 0xFC
assert teleop.left_arm.arm.last_joint == (10000, 20000, 30000, 40000, 50000, 60000)
assert teleop.right_arm.arm.last_joint == (10000, 20000, 30000, 40000, 50000, 60000)
assert teleop.left_arm.arm.last_gripper[0] == 42000
@ -721,6 +581,29 @@ def test_bimanual_piper_leader_follower_roundtrip(
finally:
teleop.disconnect()
robot.disconnect()
assert teleop.left_arm.arm.role_commands[-1][0] == 0xFA
assert teleop.right_arm.arm.role_commands[-1][0] == 0xFA
def test_bimanual_piper_get_action_returns_available_side(monkeypatch):
patch_fake_sdk(monkeypatch)
teleop = make_teleoperator_from_config(
BiPiperLeaderConfig(
left_arm_config=PiperLeaderConfigBase(port="can2"),
right_arm_config=PiperLeaderConfigBase(port="can3"),
process_isolation=False,
)
)
teleop.connect()
try:
teleop.right_arm.arm._joint_ctrl.time_stamp = 0.0
action = teleop.get_action()
assert "left_joint_1.pos" in action
assert "right_joint_1.pos" not in action
finally:
teleop.disconnect()
def test_bimanual_piper_follower_action_features_are_available_without_connect(monkeypatch):
@ -746,8 +629,8 @@ def test_bimanual_piper_teleop_loop_smoke(monkeypatch):
teleop = make_teleoperator_from_config(
BiPiperLeaderConfig(
left_arm_config=PiperLeaderConfigBase(port="can0", manual_control=False, sync_gripper=True),
right_arm_config=PiperLeaderConfigBase(port="can1", manual_control=False, sync_gripper=True),
left_arm_config=PiperLeaderConfigBase(port="can0", manual_control=True, sync_gripper=True),
right_arm_config=PiperLeaderConfigBase(port="can1", manual_control=True, sync_gripper=True),
process_isolation=False,
)
)
@ -758,18 +641,13 @@ def test_bimanual_piper_teleop_loop_smoke(monkeypatch):
)
)
teleop.left_arm.calibration = make_identity_calibration()
teleop.right_arm.calibration = make_identity_calibration()
robot.left_arm.calibration = make_identity_calibration()
robot.right_arm.calibration = make_identity_calibration()
teleop_action_processor, robot_action_processor, robot_observation_processor = make_default_processors()
robot.get_observation = lambda: (_ for _ in ()).throw(
AssertionError("teleop_loop should not fetch observation for identity processors without display")
)
teleop.connect(calibrate=False)
robot.connect(calibrate=False)
teleop.connect()
robot.connect()
try:
teleop_loop(
teleop=teleop,
@ -797,18 +675,13 @@ def test_bimanual_piper_leader_uses_process_proxy_by_default(monkeypatch):
self.arm_cls = arm_cls
self.arm_config = arm_config
self.is_connected = False
self.is_calibrated = True
action_features = dict.fromkeys(PIPER_ACTION_KEYS, float)
feedback_features = dict.fromkeys(PIPER_ACTION_KEYS, float)
def connect(self, calibrate=True):
del calibrate
def connect(self):
self.is_connected = True
def calibrate(self):
pass
def configure(self):
pass
@ -838,79 +711,3 @@ def test_bimanual_piper_leader_uses_process_proxy_by_default(monkeypatch):
assert isinstance(teleop.left_arm, DummyProxy)
assert isinstance(teleop.right_arm, DummyProxy)
def test_piper_follower_send_only_mode_skips_sdk_reader_threads(monkeypatch):
patch_fake_sdk(monkeypatch)
robot = make_robot_from_config(
PiperFollowerConfig(port="can3", sync_gripper=True, require_calibration=False)
)
robot.calibration = make_identity_calibration()
robot.set_teleop_send_only_mode(True)
robot.connect(calibrate=False)
try:
assert robot.arm.connect_calls[-1]["start_thread"] is False
with pytest.raises(RuntimeError, match="send-only mode"):
robot.get_observation()
finally:
robot.disconnect()
def test_piper_follower_send_only_mode_skips_camera_connects_and_stays_connected(monkeypatch):
patch_fake_sdk(monkeypatch)
class FakeCamera:
def __init__(self):
self.is_connected = False
self.connect_calls = 0
self.disconnect_calls = 0
def connect(self):
self.connect_calls += 1
self.is_connected = True
def disconnect(self):
self.disconnect_calls += 1
self.is_connected = False
camera = FakeCamera()
monkeypatch.setattr(piper_follower_module, "make_cameras_from_configs", lambda _: {"front": camera})
robot = make_robot_from_config(
PiperFollowerConfig(port="can3", sync_gripper=True, require_calibration=False)
)
robot.calibration = make_identity_calibration()
robot.set_teleop_send_only_mode(True)
robot.connect(calibrate=False)
try:
assert robot.is_connected
assert camera.connect_calls == 0
assert not camera.is_connected
finally:
robot.disconnect()
assert camera.disconnect_calls == 0
def test_bimanual_piper_send_only_mode_propagates_to_both_followers(monkeypatch):
patch_fake_sdk(monkeypatch)
robot = make_robot_from_config(
BiPiperFollowerConfig(
left_arm_config=PiperFollowerConfigBase(port="can3", sync_gripper=True, require_calibration=False),
right_arm_config=PiperFollowerConfigBase(port="can2", sync_gripper=True, require_calibration=False),
)
)
robot.left_arm.calibration = make_identity_calibration()
robot.right_arm.calibration = make_identity_calibration()
robot.set_teleop_send_only_mode(True)
robot.connect(calibrate=False)
try:
assert robot.left_arm.arm.connect_calls[-1]["start_thread"] is False
assert robot.right_arm.arm.connect_calls[-1]["start_thread"] is False
finally:
robot.disconnect()

查看文件

@ -0,0 +1,34 @@
from types import SimpleNamespace
import lerobot.scripts.lerobot_setup_can as setup_can_module
import lerobot.utils.piper_sdk as piper_sdk_utils
def test_resolve_piper_can_interface(monkeypatch, tmp_path):
interface_path = tmp_path / "can2"
interface_path.mkdir()
(interface_path / "type").write_text("280\n")
monkeypatch.setattr(piper_sdk_utils, "_SYS_CLASS_NET", tmp_path)
monkeypatch.setattr(
piper_sdk_utils.subprocess,
"run",
lambda *args, **kwargs: SimpleNamespace(stdout="ID_SERIAL_SHORT=SERIAL_A\n"),
)
assert piper_sdk_utils.resolve_piper_can_interface("SERIAL_A") == "can2"
def test_setup_can_uses_classic_can_for_usb_serials(monkeypatch):
monkeypatch.setattr(setup_can_module, "resolve_piper_can_interface", lambda serial: "can2")
attempts = []
def fake_setup(interface, bitrate, data_bitrate, use_fd):
attempts.append((interface, bitrate, use_fd))
return False, "failed"
monkeypatch.setattr(setup_can_module, "setup_interface", fake_setup)
cfg = setup_can_module.CANSetupConfig(mode="setup", usb_can_serials="SERIAL_A")
setup_can_module.setup_interface_with_fallback(cfg.get_interfaces()[0], cfg)
assert attempts == [("can2", 1000000, False)]