feat: 支持 MuJoCo 双臂即时复位

xr_rm_teleop/test/test_joint_control.py

@@ -105,7 +105,7 @@ def test_send_joint_target_publishes_limited_command() -> None:
     assert message.position == pytest.approx(teleop._last_joint_command_target)
 
 
-def _primary_button_teleop(*, move_error=None):
+def _primary_button_teleop(*, use_mock=False, move_error=None):
     events = []
     errors = []
     snapshot = JointStateSnapshot([0.2] * 7, time.monotonic())
@@ -122,6 +122,7 @@ def _primary_button_teleop(*, move_error=None):
 
     teleop = object.__new__(SingleArmVelocityTeleop)
     teleop._arm_name = "right_rm75"
+    teleop._use_mock = use_mock
     teleop._adapter = Adapter()
     teleop._last_primary_pressed = None
     teleop._grip_rearm_required = False
@@ -175,6 +176,34 @@ def test_primary_button_move_failure_logs_and_stays_stopped() -> None:
     ]
 
 
+def test_mock_primary_reset_can_reanchor_without_grip_release() -> None:
+    teleop, events, _, snapshot = _primary_button_teleop(use_mock=True)
+
+    teleop._on_controller(SimpleNamespace(primary=False))
+    teleop._on_controller(SimpleNamespace(primary=True))
+
+    assert events == [
+        ("stop", True),
+        "move",
+        "read",
+        ("sync", snapshot),
+    ]
+    assert not teleop._grip_rearm_required
+
+
+def test_failed_mock_primary_reset_still_requires_grip_release() -> None:
+    failure = RuntimeError("mock reset failed")
+    teleop, _, _, _ = _primary_button_teleop(
+        use_mock=True,
+        move_error=failure,
+    )
+
+    teleop._on_controller(SimpleNamespace(primary=False))
+    teleop._on_controller(SimpleNamespace(primary=True))
+
+    assert teleop._grip_rearm_required
+
+
 def test_startup_joint_query_initializes_qp_and_command_history() -> None:
     positions = [0.1] * 7
     pose = np.eye(4)

xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py

@@ -259,6 +259,7 @@ class SingleArmVelocityTeleop(Node):
         self._low_z_threshold = float(self.get_parameter("low_z_threshold").value)
         self._low_z_min_radius = float(self.get_parameter("low_z_min_radius").value)
         self._xr_to_robot_matrix = self._float_list_parameter("xr_to_robot_matrix", 9)
+        self._use_mock = self._bool_parameter("use_mock")
         self._follow = self._bool_parameter("follow")
         self._enable_tool_control = self._bool_parameter("enable_tool_control")
         self._enable_trigger_gripper_control = self._bool_parameter("enable_trigger_gripper_control")
@@ -363,7 +364,7 @@ class SingleArmVelocityTeleop(Node):
             "initial_joint_pose",
             7,
         )
-        if self._bool_parameter("use_mock"):
+        if self._use_mock:
             return MockRealManAdapter(initial_joint_pose)
 
         return RealManAdapter(
@@ -559,6 +560,8 @@ class SingleArmVelocityTeleop(Node):
             )
             return
 
+        if self._use_mock:
+            self._grip_rearm_required = False
         self.get_logger().info(f"{self._arm_name} 已回到初始位姿。")
 
     def _handle_trigger_gripper(self, msg: XrController) -> None: