refactor(piper): simplify teleop tests and gravity-comp init

- parametrize piper/piperx roundtrip teleop test to remove duplication

- stop gravity-comp loop before disconnect on connect rollback

- avoid repeating URDF/LFS checks after loop initialization
这个提交包含在:
Elvin
2026-03-07 01:50:26 +08:00
父节点 8934fdbabf
当前提交 f68f8eb543
共有 2 个文件被更改,包括 29 次插入 和 75 次删除

查看文件

@ -120,6 +120,7 @@ class PiperLeader(Teleoperator):
)
self.calibrate()
except Exception:
self._stop_gravity_comp_loop_if_needed()
self.arm.DisconnectPort()
self._is_connected = False
raise
@ -229,15 +230,15 @@ class PiperLeader(Teleoperator):
self._manual_control_enabled = False
def _ensure_gravity_comp_loop(self) -> PiperGravityCompensationLoop:
default_urdf = resources.files("lerobot").joinpath(self.gravity_comp_urdf_relpath)
if not default_urdf.is_file():
raise FileNotFoundError(
"Bundled gravity compensation URDF is missing: "
f"{self.gravity_comp_urdf_relpath}. Reinstall the package."
)
_ensure_not_lfs_pointer(default_urdf, self.gravity_comp_urdf_relpath)
urdf_path = str(default_urdf)
if self._gravity_comp_loop is None:
default_urdf = resources.files("lerobot").joinpath(self.gravity_comp_urdf_relpath)
if not default_urdf.is_file():
raise FileNotFoundError(
"Bundled gravity compensation URDF is missing: "
f"{self.gravity_comp_urdf_relpath}. Reinstall the package."
)
_ensure_not_lfs_pointer(default_urdf, self.gravity_comp_urdf_relpath)
urdf_path = str(default_urdf)
self._gravity_comp_loop = PiperGravityCompensationLoop(
arm=self.arm,
urdf_path=urdf_path,

查看文件

@ -177,78 +177,31 @@ def wait_until(predicate, timeout_s: float = 0.2, poll_s: float = 0.005) -> bool
return False
def test_piper_leader_follower_teleop_roundtrip(monkeypatch):
@pytest.mark.parametrize(
("teleop_cfg", "robot_cfg", "teleop_cls", "robot_cls"),
[
(
PiperLeaderConfig(port="can1", manual_control=False, sync_gripper=True),
PiperFollowerConfig(port="can0", sync_gripper=True),
PiperLeader,
PiperFollower,
),
(
PiperXLeaderConfig(port="can1", manual_control=False, sync_gripper=True),
PiperXFollowerConfig(port="can0", sync_gripper=True),
PiperXLeader,
PiperXFollower,
),
],
)
def test_piper_leader_follower_teleop_roundtrip(monkeypatch, teleop_cfg, robot_cfg, teleop_cls, robot_cls):
patch_fake_sdk(monkeypatch)
teleop_cfg = PiperLeaderConfig(
port="can1",
manual_control=False,
sync_gripper=True,
)
robot_cfg = PiperFollowerConfig(
port="can0",
sync_gripper=True,
)
teleop = make_teleoperator_from_config(teleop_cfg)
robot = make_robot_from_config(robot_cfg)
assert isinstance(teleop, PiperLeader)
assert isinstance(robot, PiperFollower)
teleop.calibration = make_identity_calibration()
robot.calibration = make_identity_calibration()
teleop.connect(calibrate=False)
robot.connect(calibrate=False)
try:
action = teleop.get_action()
sent = robot.send_action(action)
obs = robot.get_observation()
assert robot.arm.last_joint == (10000, 20000, 30000, 40000, 50000, 60000)
assert robot.arm.last_gripper == (
42000,
robot_cfg.gripper_effort_default,
robot_cfg.gripper_status_code,
0x00,
)
assert sent["joint_1.pos"] == 10.0
assert sent["gripper.pos"] == 42.0
assert obs["joint_1.pos"] == 11.0
assert obs["gripper.pos"] == 43.0
teleop.send_feedback(action)
assert teleop.arm.last_joint == (10000, 20000, 30000, 40000, 50000, 60000)
assert teleop.arm.last_gripper == (
42000,
teleop_cfg.gripper_effort_default,
teleop_cfg.gripper_status_code,
0x00,
)
finally:
teleop.disconnect()
robot.disconnect()
def test_piperx_leader_follower_teleop_roundtrip(monkeypatch):
patch_fake_sdk(monkeypatch)
teleop_cfg = PiperXLeaderConfig(
port="can1",
manual_control=False,
sync_gripper=True,
)
robot_cfg = PiperXFollowerConfig(
port="can0",
sync_gripper=True,
)
teleop = make_teleoperator_from_config(teleop_cfg)
robot = make_robot_from_config(robot_cfg)
assert isinstance(teleop, PiperXLeader)
assert isinstance(robot, PiperXFollower)
assert isinstance(teleop, teleop_cls)
assert isinstance(robot, robot_cls)
teleop.calibration = make_identity_calibration()
robot.calibration = make_identity_calibration()