fix(piper): seed manual action from feedback (#34)

Co-authored-by: Elvin-yk <180030703+Elvin-yk@users.noreply.github.com>
这个提交包含在:
Ye Kai
2026-07-15 22:08:53 +08:00
提交者 GitHub
父节点 e439dd12d5
当前提交 6f2db449a2
共有 2 个文件被更改,包括 109 次插入 和 55 次删除

查看文件

@ -53,6 +53,9 @@ class PiperLeader(Teleoperator):
self.config = config self.config = config
self._is_connected = False self._is_connected = False
self._manual_control_enabled: bool | None = None 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 self._last_mode_refresh_t = 0.0
interface_cls, _ = get_piper_sdk() interface_cls, _ = get_piper_sdk()
@ -84,12 +87,14 @@ class PiperLeader(Teleoperator):
self._is_connected = True self._is_connected = True
self._manual_control_enabled = None self._manual_control_enabled = None
self._manual_action = None
try: try:
self.configure() self.configure()
except Exception: except Exception:
self.arm.DisconnectPort() self.arm.DisconnectPort()
self._is_connected = False self._is_connected = False
self._manual_control_enabled = None self._manual_control_enabled = None
self._manual_action = None
raise raise
logger.info("%s connected.", self) logger.info("%s connected.", self)
@ -141,8 +146,27 @@ class PiperLeader(Teleoperator):
self._manual_control_enabled = None self._manual_control_enabled = None
if enabled: if enabled:
set_piper_role(self.arm, PIPER_ROLE_LEADER) 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: else:
self._manual_action = None
set_piper_role(self.arm, PIPER_ROLE_FOLLOWER) set_piper_role(self.arm, PIPER_ROLE_FOLLOWER)
self._send_command_mode() self._send_command_mode()
if not wait_enable_piper(self.arm, self.config.enable_timeout_s): if not wait_enable_piper(self.arm, self.config.enable_timeout_s):
@ -157,15 +181,6 @@ class PiperLeader(Teleoperator):
def configure(self) -> None: def configure(self) -> None:
self.set_manual_control(self.config.manual_control) 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: def _read_joint_from_feedback(self) -> dict[str, float] | None:
message = self.arm.GetArmJointMsgs() message = self.arm.GetArmJointMsgs()
if message.time_stamp <= 0: if message.time_stamp <= 0:
@ -175,22 +190,54 @@ class PiperLeader(Teleoperator):
for joint_name in PIPER_JOINT_NAMES 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: def _read_gripper_from_feedback(self) -> float | None:
message = self.arm.GetArmGripperMsgs() message = self.arm.GetArmGripperMsgs()
if message.time_stamp <= 0: if message.time_stamp <= 0:
return None return None
return abs(milli_to_unit(message.gripper_state.grippers_angle)) 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: def _read_raw_action(self) -> RobotAction:
if self._manual_control_enabled is True: if self._manual_control_enabled is True:
action = self._read_joint_from_ctrl() return self._read_manual_action()
gripper_pos = self._read_gripper_from_ctrl() if self.config.sync_gripper else None
elif self._manual_control_enabled is False: elif self._manual_control_enabled is False:
action = self._read_joint_from_feedback() action = self._read_joint_from_feedback()
gripper_pos = self._read_gripper_from_feedback() if self.config.sync_gripper else None 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.arm.DisconnectPort()
self._is_connected = False self._is_connected = False
self._manual_control_enabled = None self._manual_control_enabled = None
self._manual_action = None
logger.info("%s disconnected.", self) 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) sent = robot.send_action(action)
obs = robot.get_observation() 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 == ( assert robot.arm.last_gripper == (
42000, 43000,
robot_cfg.gripper_effort_default, robot_cfg.gripper_effort_default,
robot_cfg.gripper_status_code, robot_cfg.gripper_status_code,
0x00, 0x00,
) )
assert sent["joint_1.pos"] == 10.0 assert sent["joint_1.pos"] == 11.0
assert sent["gripper.pos"] == 42.0 assert sent["gripper.pos"] == 43.0
assert obs["joint_1.pos"] == 11.0 assert obs["joint_1.pos"] == 11.0
assert obs["gripper.pos"] == 43.0 assert obs["gripper.pos"] == 43.0
teleop.send_feedback(action) teleop.send_feedback(action)
assert teleop.arm.role_commands[-1] == (0xFC, 0x00, 0x00, 0x00) 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 == ( assert teleop.arm.last_gripper == (
42000, 43000,
teleop_cfg.gripper_effort_default, teleop_cfg.gripper_effort_default,
teleop_cfg.gripper_status_code, teleop_cfg.gripper_status_code,
0x00, 0x00,
@ -346,16 +346,18 @@ def test_piper_leader_default_uses_hardware_leader_role(monkeypatch):
teleop.connect() teleop.connect()
try: try:
assert teleop.arm.connect_calls[-1]["piper_init"] is False assert teleop.arm.role_commands == [
assert teleop.arm.role_commands == [(0xFA, 0x00, 0x00, 0x00)] (0xFC, 0x00, 0x00, 0x00),
(0xFA, 0x00, 0x00, 0x00),
]
assert teleop.arm.enable_calls == 0 assert teleop.arm.enable_calls == 0
assert teleop.arm.mode_commands == [] assert teleop.arm.mode_commands == []
assert teleop.arm.gripper_calls == [] assert teleop.arm.gripper_calls == []
assert teleop.arm.joint_mit_calls == [] assert teleop.arm.joint_mit_calls == []
action = teleop.get_action() action = teleop.get_action()
assert action["joint_1.pos"] == 10.0 assert action["joint_1.pos"] == 11.0
assert action["joint_2.pos"] == 20.0 assert action["joint_2.pos"] == 21.0
assert action["gripper.pos"] == 42.0 assert action["gripper.pos"] == 43.0
finally: finally:
teleop.disconnect() teleop.disconnect()
@ -408,31 +410,35 @@ def test_piper_leader_policy_mode_reads_feedback(monkeypatch):
teleop.disconnect() 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) patch_fake_sdk(monkeypatch)
teleop = PiperLeader(PiperLeaderConfig(port="can1")) teleop = PiperLeader(PiperLeaderConfig(port="can1"))
teleop.arm._joint_ctrl.time_stamp = 0.0
teleop.arm._gripper_ctrl.time_stamp = 0.0
teleop.connect() teleop.connect()
try: try:
teleop.arm._joint_ctrl.time_stamp = 0.0 action = teleop.get_action()
assert teleop.get_action() == {} assert action["joint_1.pos"] == 11.0
assert action["joint_2.pos"] == 21.0
assert action["gripper.pos"] == 43.0
finally: finally:
teleop.disconnect() 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) patch_fake_sdk(monkeypatch)
teleop = PiperLeader(PiperLeaderConfig(port="can1", enable_timeout_s=0.0)) teleop = PiperLeader(PiperLeaderConfig(port="can1", enable_timeout_s=0.0))
teleop.connect() teleop.connect()
try: try:
with pytest.raises(RuntimeError, match="did not enable"): with pytest.raises(RuntimeError, match="did not enable"):
teleop.set_manual_control(False) teleop.set_manual_control(False)
assert teleop._manual_control_enabled is True assert teleop._manual_control_enabled is None
assert teleop.arm.role_commands[-1][0] == 0xFA assert teleop.arm.role_commands[-1][0] == 0xFC
finally: finally:
teleop.disconnect() 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) patch_fake_sdk(monkeypatch)
teleop = PiperLeader(PiperLeaderConfig(port="can1", manual_control=False)) teleop = PiperLeader(PiperLeaderConfig(port="can1", manual_control=False))
teleop.connect() 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"): with pytest.raises(RuntimeError, match="leader role restore failed"):
teleop.disconnect() teleop.disconnect()
assert teleop.arm.disable_calls == 1 assert teleop.arm.disable_calls == 0
assert not teleop.arm.connected assert not teleop.arm.connected
@ -557,15 +563,15 @@ def test_bimanual_piper_leader_follower_roundtrip(
sent = robot.send_action(action) sent = robot.send_action(action)
obs = robot.get_observation() obs = robot.get_observation()
assert robot.left_arm.arm.last_joint == (10000, 20000, 30000, 40000, 50000, 60000) assert robot.left_arm.arm.last_joint == (11000, 21000, 31000, 41000, 51000, 61000)
assert robot.right_arm.arm.last_joint == (10000, 20000, 30000, 40000, 50000, 60000) assert robot.right_arm.arm.last_joint == (11000, 21000, 31000, 41000, 51000, 61000)
assert robot.left_arm.arm.last_gripper[0] == 42000 assert robot.left_arm.arm.last_gripper[0] == 43000
assert robot.right_arm.arm.last_gripper[0] == 42000 assert robot.right_arm.arm.last_gripper[0] == 43000
assert sent["left_joint_1.pos"] == 10.0 assert sent["left_joint_1.pos"] == 11.0
assert sent["right_joint_1.pos"] == 10.0 assert sent["right_joint_1.pos"] == 11.0
assert sent["left_gripper.pos"] == 42.0 assert sent["left_gripper.pos"] == 43.0
assert sent["right_gripper.pos"] == 42.0 assert sent["right_gripper.pos"] == 43.0
assert obs["left_joint_1.pos"] == 11.0 assert obs["left_joint_1.pos"] == 11.0
assert obs["right_joint_1.pos"] == 11.0 assert obs["right_joint_1.pos"] == 11.0
assert obs["left_gripper.pos"] == 43.0 assert obs["left_gripper.pos"] == 43.0
@ -574,10 +580,10 @@ def test_bimanual_piper_leader_follower_roundtrip(
teleop.send_feedback(action) teleop.send_feedback(action)
assert teleop.left_arm.arm.role_commands[-1][0] == 0xFC assert teleop.left_arm.arm.role_commands[-1][0] == 0xFC
assert teleop.right_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.left_arm.arm.last_joint == (11000, 21000, 31000, 41000, 51000, 61000)
assert teleop.right_arm.arm.last_joint == (10000, 20000, 30000, 40000, 50000, 60000) assert teleop.right_arm.arm.last_joint == (11000, 21000, 31000, 41000, 51000, 61000)
assert teleop.left_arm.arm.last_gripper[0] == 42000 assert teleop.left_arm.arm.last_gripper[0] == 43000
assert teleop.right_arm.arm.last_gripper[0] == 42000 assert teleop.right_arm.arm.last_gripper[0] == 43000
finally: finally:
teleop.disconnect() teleop.disconnect()
robot.disconnect() robot.disconnect()
@ -585,7 +591,7 @@ def test_bimanual_piper_leader_follower_roundtrip(
assert teleop.right_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): def test_bimanual_piper_get_action_uses_feedback_seed_for_silent_side(monkeypatch):
patch_fake_sdk(monkeypatch) patch_fake_sdk(monkeypatch)
teleop = make_teleoperator_from_config( 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 teleop.right_arm.arm._joint_ctrl.time_stamp = 0.0
action = teleop.get_action() action = teleop.get_action()
assert "left_joint_1.pos" in action assert "left_joint_1.pos" in action
assert "right_joint_1.pos" not in action assert "right_joint_1.pos" in action
finally: finally:
teleop.disconnect() teleop.disconnect()
@ -663,10 +669,10 @@ def test_bimanual_piper_teleop_loop_smoke(monkeypatch):
teleop.disconnect() teleop.disconnect()
robot.disconnect() robot.disconnect()
assert robot.left_arm.arm.last_joint == (10000, 20000, 30000, 40000, 50000, 60000) assert robot.left_arm.arm.last_joint == (11000, 21000, 31000, 41000, 51000, 61000)
assert robot.right_arm.arm.last_joint == (10000, 20000, 30000, 40000, 50000, 60000) assert robot.right_arm.arm.last_joint == (11000, 21000, 31000, 41000, 51000, 61000)
assert robot.left_arm.arm.last_gripper[0] == 42000 assert robot.left_arm.arm.last_gripper[0] == 43000
assert robot.right_arm.arm.last_gripper[0] == 42000 assert robot.right_arm.arm.last_gripper[0] == 43000
def test_bimanual_piper_leader_uses_process_proxy_by_default(monkeypatch): def test_bimanual_piper_leader_uses_process_proxy_by_default(monkeypatch):