镜像自地址
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.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):
|
||||||
|
|||||||
在新工单中引用
屏蔽一个用户