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)