From f5790a8c7739e67b596a4b6e6698d85ca140058d Mon Sep 17 00:00:00 2001
From: YikaiFu-cart <yikai.fu@nicecart.ai>
Date: Tue, 4 Aug 2026 13:34:27 +0800
Subject: [PATCH] =?UTF-8?q?feat:=20=E6=94=AF=E6=8C=81=20MuJoCo=20=E5=8F=8C?=
 =?UTF-8?q?=E8=87=82=E5=8D=B3=E6=97=B6=E5=A4=8D=E4=BD=8D?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 xr_rm_teleop/test/test_joint_control.py       | 31 ++++++++++++++++++-
 .../single_arm_velocity_teleop.py             |  5 ++-
 2 files changed, 34 insertions(+), 2 deletions(-)

diff --git a/xr_rm_teleop/test/test_joint_control.py b/xr_rm_teleop/test/test_joint_control.py
index 0332452..ae5d924 100644
--- a/xr_rm_teleop/test/test_joint_control.py
+++ b/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)
diff --git a/xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py b/xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py
index 597ab88..1531331 100755
--- a/xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py
+++ b/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: