镜像自地址
https://github.com/MINT-SJTU/Evo-RL.git
已同步 2026-11-20 18:17:06 +00:00
fix(piper): seed manual action from feedback (#34)
Co-authored-by: Elvin-yk <180030703+Elvin-yk@users.noreply.github.com>
这个提交包含在:
@ -53,6 +53,9 @@ class PiperLeader(Teleoperator):
|
||||
self.config = config
|
||||
self._is_connected = False
|
||||
self._manual_control_enabled: bool | None = None
|
||||
self._manual_action: RobotAction | None = None
|
||||
self._last_control_joint_timestamp = 0.0
|
||||
self._last_control_gripper_timestamp = 0.0
|
||||
self._last_mode_refresh_t = 0.0
|
||||
|
||||
interface_cls, _ = get_piper_sdk()
|
||||
@ -84,12 +87,14 @@ class PiperLeader(Teleoperator):
|
||||
|
||||
self._is_connected = True
|
||||
self._manual_control_enabled = None
|
||||
self._manual_action = None
|
||||
try:
|
||||
self.configure()
|
||||
except Exception:
|
||||
self.arm.DisconnectPort()
|
||||
self._is_connected = False
|
||||
self._manual_control_enabled = None
|
||||
self._manual_action = None
|
||||
raise
|
||||
|
||||
logger.info("%s connected.", self)
|
||||
@ -141,8 +146,27 @@ class PiperLeader(Teleoperator):
|
||||
|
||||
self._manual_control_enabled = None
|
||||
if enabled:
|
||||
self._manual_action = None
|
||||
seed_action = None
|
||||
try:
|
||||
set_piper_role(self.arm, PIPER_ROLE_FOLLOWER)
|
||||
seed_action = self._wait_for_feedback_action()
|
||||
finally:
|
||||
set_piper_role(self.arm, PIPER_ROLE_LEADER)
|
||||
|
||||
if seed_action is None:
|
||||
raise RuntimeError(
|
||||
f"[{self.config.port}] no complete Piper feedback received while initializing "
|
||||
"manual control."
|
||||
)
|
||||
|
||||
self._manual_action = seed_action
|
||||
self._last_control_joint_timestamp = self.arm.GetArmJointCtrl().time_stamp
|
||||
self._last_control_gripper_timestamp = (
|
||||
self.arm.GetArmGripperCtrl().time_stamp if self.config.sync_gripper else 0.0
|
||||
)
|
||||
else:
|
||||
self._manual_action = None
|
||||
set_piper_role(self.arm, PIPER_ROLE_FOLLOWER)
|
||||
self._send_command_mode()
|
||||
if not wait_enable_piper(self.arm, self.config.enable_timeout_s):
|
||||
@ -157,15 +181,6 @@ class PiperLeader(Teleoperator):
|
||||
def configure(self) -> None:
|
||||
self.set_manual_control(self.config.manual_control)
|
||||
|
||||
def _read_joint_from_ctrl(self) -> dict[str, float] | None:
|
||||
message = self.arm.GetArmJointCtrl()
|
||||
if message.time_stamp <= 0:
|
||||
return None
|
||||
return {
|
||||
f"{joint_name}.pos": milli_to_unit(getattr(message.joint_ctrl, joint_name))
|
||||
for joint_name in PIPER_JOINT_NAMES
|
||||
}
|
||||
|
||||
def _read_joint_from_feedback(self) -> dict[str, float] | None:
|
||||
message = self.arm.GetArmJointMsgs()
|
||||
if message.time_stamp <= 0:
|
||||
@ -175,22 +190,54 @@ class PiperLeader(Teleoperator):
|
||||
for joint_name in PIPER_JOINT_NAMES
|
||||
}
|
||||
|
||||
def _read_gripper_from_ctrl(self) -> float | None:
|
||||
message = self.arm.GetArmGripperCtrl()
|
||||
if message.time_stamp <= 0:
|
||||
return None
|
||||
return abs(milli_to_unit(message.gripper_ctrl.grippers_angle))
|
||||
|
||||
def _read_gripper_from_feedback(self) -> float | None:
|
||||
message = self.arm.GetArmGripperMsgs()
|
||||
if message.time_stamp <= 0:
|
||||
return None
|
||||
return abs(milli_to_unit(message.gripper_state.grippers_angle))
|
||||
|
||||
def _wait_for_feedback_action(self) -> RobotAction | None:
|
||||
deadline = time.monotonic() + self.config.enable_timeout_s
|
||||
while True:
|
||||
action = self._read_joint_from_feedback()
|
||||
gripper_pos = self._read_gripper_from_feedback() if self.config.sync_gripper else 0.0
|
||||
if action is not None and gripper_pos is not None:
|
||||
action["gripper.pos"] = gripper_pos
|
||||
return action
|
||||
if time.monotonic() >= deadline:
|
||||
return None
|
||||
time.sleep(0.01)
|
||||
|
||||
def _read_manual_action(self) -> RobotAction:
|
||||
if self._manual_action is None:
|
||||
return {}
|
||||
|
||||
joint_message = self.arm.GetArmJointCtrl()
|
||||
if joint_message.time_stamp > 0 and joint_message.time_stamp != self._last_control_joint_timestamp:
|
||||
self._manual_action.update(
|
||||
{
|
||||
f"{joint_name}.pos": milli_to_unit(getattr(joint_message.joint_ctrl, joint_name))
|
||||
for joint_name in PIPER_JOINT_NAMES
|
||||
}
|
||||
)
|
||||
self._last_control_joint_timestamp = joint_message.time_stamp
|
||||
|
||||
if self.config.sync_gripper:
|
||||
gripper_message = self.arm.GetArmGripperCtrl()
|
||||
if (
|
||||
gripper_message.time_stamp > 0
|
||||
and gripper_message.time_stamp != self._last_control_gripper_timestamp
|
||||
):
|
||||
self._manual_action["gripper.pos"] = abs(
|
||||
milli_to_unit(gripper_message.gripper_ctrl.grippers_angle)
|
||||
)
|
||||
self._last_control_gripper_timestamp = gripper_message.time_stamp
|
||||
|
||||
return dict(self._manual_action)
|
||||
|
||||
def _read_raw_action(self) -> RobotAction:
|
||||
if self._manual_control_enabled is True:
|
||||
action = self._read_joint_from_ctrl()
|
||||
gripper_pos = self._read_gripper_from_ctrl() if self.config.sync_gripper else None
|
||||
return self._read_manual_action()
|
||||
elif self._manual_control_enabled is False:
|
||||
action = self._read_joint_from_feedback()
|
||||
gripper_pos = self._read_gripper_from_feedback() if self.config.sync_gripper else None
|
||||
@ -237,6 +284,7 @@ class PiperLeader(Teleoperator):
|
||||
self.arm.DisconnectPort()
|
||||
self._is_connected = False
|
||||
self._manual_control_enabled = None
|
||||
self._manual_action = None
|
||||
logger.info("%s disconnected.", self)
|
||||
|
||||
|
||||
|
||||
@ -229,23 +229,23 @@ def test_piper_leader_follower_teleop_roundtrip(monkeypatch, teleop_cfg, robot_c
|
||||
sent = robot.send_action(action)
|
||||
obs = robot.get_observation()
|
||||
|
||||
assert robot.arm.last_joint == (10000, 20000, 30000, 40000, 50000, 60000)
|
||||
assert robot.arm.last_joint == (11000, 21000, 31000, 41000, 51000, 61000)
|
||||
assert robot.arm.last_gripper == (
|
||||
42000,
|
||||
43000,
|
||||
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 sent["joint_1.pos"] == 11.0
|
||||
assert sent["gripper.pos"] == 43.0
|
||||
assert obs["joint_1.pos"] == 11.0
|
||||
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_joint == (11000, 21000, 31000, 41000, 51000, 61000)
|
||||
assert teleop.arm.last_gripper == (
|
||||
42000,
|
||||
43000,
|
||||
teleop_cfg.gripper_effort_default,
|
||||
teleop_cfg.gripper_status_code,
|
||||
0x00,
|
||||
@ -346,16 +346,18 @@ def test_piper_leader_default_uses_hardware_leader_role(monkeypatch):
|
||||
|
||||
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.role_commands == [
|
||||
(0xFC, 0x00, 0x00, 0x00),
|
||||
(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()
|
||||
assert action["joint_1.pos"] == 10.0
|
||||
assert action["joint_2.pos"] == 20.0
|
||||
assert action["gripper.pos"] == 42.0
|
||||
assert action["joint_1.pos"] == 11.0
|
||||
assert action["joint_2.pos"] == 21.0
|
||||
assert action["gripper.pos"] == 43.0
|
||||
finally:
|
||||
teleop.disconnect()
|
||||
|
||||
@ -408,31 +410,35 @@ def test_piper_leader_policy_mode_reads_feedback(monkeypatch):
|
||||
teleop.disconnect()
|
||||
|
||||
|
||||
def test_piper_leader_returns_empty_action_without_manual_frames(monkeypatch):
|
||||
def test_piper_leader_uses_feedback_seed_without_manual_frames(monkeypatch):
|
||||
patch_fake_sdk(monkeypatch)
|
||||
teleop = PiperLeader(PiperLeaderConfig(port="can1"))
|
||||
teleop.arm._joint_ctrl.time_stamp = 0.0
|
||||
teleop.arm._gripper_ctrl.time_stamp = 0.0
|
||||
teleop.connect()
|
||||
try:
|
||||
teleop.arm._joint_ctrl.time_stamp = 0.0
|
||||
assert teleop.get_action() == {}
|
||||
action = teleop.get_action()
|
||||
assert action["joint_1.pos"] == 11.0
|
||||
assert action["joint_2.pos"] == 21.0
|
||||
assert action["gripper.pos"] == 43.0
|
||||
finally:
|
||||
teleop.disconnect()
|
||||
|
||||
|
||||
def test_piper_leader_failed_policy_switch_restores_manual_role(monkeypatch):
|
||||
def test_piper_leader_failed_policy_switch_leaves_mode_unknown(monkeypatch):
|
||||
patch_fake_sdk(monkeypatch)
|
||||
teleop = PiperLeader(PiperLeaderConfig(port="can1", enable_timeout_s=0.0))
|
||||
teleop.connect()
|
||||
try:
|
||||
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
|
||||
assert teleop._manual_control_enabled is None
|
||||
assert teleop.arm.role_commands[-1][0] == 0xFC
|
||||
finally:
|
||||
teleop.disconnect()
|
||||
|
||||
|
||||
def test_piper_leader_disconnect_disables_if_role_restore_fails(monkeypatch):
|
||||
def test_piper_leader_disconnect_cleans_up_if_role_restore_fails(monkeypatch):
|
||||
patch_fake_sdk(monkeypatch)
|
||||
teleop = PiperLeader(PiperLeaderConfig(port="can1", manual_control=False))
|
||||
teleop.connect()
|
||||
@ -444,7 +450,7 @@ def test_piper_leader_disconnect_disables_if_role_restore_fails(monkeypatch):
|
||||
|
||||
with pytest.raises(RuntimeError, match="leader role restore failed"):
|
||||
teleop.disconnect()
|
||||
assert teleop.arm.disable_calls == 1
|
||||
assert teleop.arm.disable_calls == 0
|
||||
assert not teleop.arm.connected
|
||||
|
||||
|
||||
@ -557,15 +563,15 @@ def test_bimanual_piper_leader_follower_roundtrip(
|
||||
sent = robot.send_action(action)
|
||||
obs = robot.get_observation()
|
||||
|
||||
assert robot.left_arm.arm.last_joint == (10000, 20000, 30000, 40000, 50000, 60000)
|
||||
assert robot.right_arm.arm.last_joint == (10000, 20000, 30000, 40000, 50000, 60000)
|
||||
assert robot.left_arm.arm.last_gripper[0] == 42000
|
||||
assert robot.right_arm.arm.last_gripper[0] == 42000
|
||||
assert robot.left_arm.arm.last_joint == (11000, 21000, 31000, 41000, 51000, 61000)
|
||||
assert robot.right_arm.arm.last_joint == (11000, 21000, 31000, 41000, 51000, 61000)
|
||||
assert robot.left_arm.arm.last_gripper[0] == 43000
|
||||
assert robot.right_arm.arm.last_gripper[0] == 43000
|
||||
|
||||
assert sent["left_joint_1.pos"] == 10.0
|
||||
assert sent["right_joint_1.pos"] == 10.0
|
||||
assert sent["left_gripper.pos"] == 42.0
|
||||
assert sent["right_gripper.pos"] == 42.0
|
||||
assert sent["left_joint_1.pos"] == 11.0
|
||||
assert sent["right_joint_1.pos"] == 11.0
|
||||
assert sent["left_gripper.pos"] == 43.0
|
||||
assert sent["right_gripper.pos"] == 43.0
|
||||
assert obs["left_joint_1.pos"] == 11.0
|
||||
assert obs["right_joint_1.pos"] == 11.0
|
||||
assert obs["left_gripper.pos"] == 43.0
|
||||
@ -574,10 +580,10 @@ def test_bimanual_piper_leader_follower_roundtrip(
|
||||
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
|
||||
assert teleop.right_arm.arm.last_gripper[0] == 42000
|
||||
assert teleop.left_arm.arm.last_joint == (11000, 21000, 31000, 41000, 51000, 61000)
|
||||
assert teleop.right_arm.arm.last_joint == (11000, 21000, 31000, 41000, 51000, 61000)
|
||||
assert teleop.left_arm.arm.last_gripper[0] == 43000
|
||||
assert teleop.right_arm.arm.last_gripper[0] == 43000
|
||||
finally:
|
||||
teleop.disconnect()
|
||||
robot.disconnect()
|
||||
@ -585,7 +591,7 @@ def test_bimanual_piper_leader_follower_roundtrip(
|
||||
assert teleop.right_arm.arm.role_commands[-1][0] == 0xFA
|
||||
|
||||
|
||||
def test_bimanual_piper_get_action_returns_available_side(monkeypatch):
|
||||
def test_bimanual_piper_get_action_uses_feedback_seed_for_silent_side(monkeypatch):
|
||||
patch_fake_sdk(monkeypatch)
|
||||
|
||||
teleop = make_teleoperator_from_config(
|
||||
@ -601,7 +607,7 @@ def test_bimanual_piper_get_action_returns_available_side(monkeypatch):
|
||||
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
|
||||
assert "right_joint_1.pos" in action
|
||||
finally:
|
||||
teleop.disconnect()
|
||||
|
||||
@ -663,10 +669,10 @@ def test_bimanual_piper_teleop_loop_smoke(monkeypatch):
|
||||
teleop.disconnect()
|
||||
robot.disconnect()
|
||||
|
||||
assert robot.left_arm.arm.last_joint == (10000, 20000, 30000, 40000, 50000, 60000)
|
||||
assert robot.right_arm.arm.last_joint == (10000, 20000, 30000, 40000, 50000, 60000)
|
||||
assert robot.left_arm.arm.last_gripper[0] == 42000
|
||||
assert robot.right_arm.arm.last_gripper[0] == 42000
|
||||
assert robot.left_arm.arm.last_joint == (11000, 21000, 31000, 41000, 51000, 61000)
|
||||
assert robot.right_arm.arm.last_joint == (11000, 21000, 31000, 41000, 51000, 61000)
|
||||
assert robot.left_arm.arm.last_gripper[0] == 43000
|
||||
assert robot.right_arm.arm.last_gripper[0] == 43000
|
||||
|
||||
|
||||
def test_bimanual_piper_leader_uses_process_proxy_by_default(monkeypatch):
|
||||
|
||||
在新工单中引用
屏蔽一个用户