镜像自地址
https://github.com/MINT-SJTU/Evo-RL.git
已同步 2026-11-20 18:17:06 +00:00
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
这个提交包含在:
@ -120,6 +120,7 @@ class PiperLeader(Teleoperator):
|
|||||||
)
|
)
|
||||||
self.calibrate()
|
self.calibrate()
|
||||||
except Exception:
|
except Exception:
|
||||||
|
self._stop_gravity_comp_loop_if_needed()
|
||||||
self.arm.DisconnectPort()
|
self.arm.DisconnectPort()
|
||||||
self._is_connected = False
|
self._is_connected = False
|
||||||
raise
|
raise
|
||||||
@ -229,15 +230,15 @@ class PiperLeader(Teleoperator):
|
|||||||
self._manual_control_enabled = False
|
self._manual_control_enabled = False
|
||||||
|
|
||||||
def _ensure_gravity_comp_loop(self) -> PiperGravityCompensationLoop:
|
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:
|
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(
|
self._gravity_comp_loop = PiperGravityCompensationLoop(
|
||||||
arm=self.arm,
|
arm=self.arm,
|
||||||
urdf_path=urdf_path,
|
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
|
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)
|
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)
|
teleop = make_teleoperator_from_config(teleop_cfg)
|
||||||
robot = make_robot_from_config(robot_cfg)
|
robot = make_robot_from_config(robot_cfg)
|
||||||
|
|
||||||
assert isinstance(teleop, PiperLeader)
|
assert isinstance(teleop, teleop_cls)
|
||||||
assert isinstance(robot, PiperFollower)
|
assert isinstance(robot, robot_cls)
|
||||||
|
|
||||||
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)
|
|
||||||
|
|
||||||
teleop.calibration = make_identity_calibration()
|
teleop.calibration = make_identity_calibration()
|
||||||
robot.calibration = make_identity_calibration()
|
robot.calibration = make_identity_calibration()
|
||||||
|
|||||||
在新工单中引用
屏蔽一个用户