feat: add reset ability in mujoco

ik_qp/README.md

@@ -208,6 +208,17 @@ ros2 launch xr_rm_bringup dual_arm_qp_sim.launch.py \
 `initial_tcp_pose` 初始化的残差，最后出现 `dual-arm QP teleop ready`。第三阶段结果见
 [STAGE3_VALIDATION.md](STAGE3_VALIDATION.md)。
 
+运行过程中可在 MuJoCo viewer 中按 `R` 或 `Home`，将双臂恢复到本次启动时求得的
+初始关节状态；也可调用：
+
+```bash
+ros2 service call /xr_rm/qp/reset_to_initial std_srvs/srv/Trigger "{}"
+```
+
+Reset 会清除旧的 grip 相对位姿和控制故障。为避免旧输入在复位后立即重新驱动机械臂，
+控制器必须收到左右手柄新的 `grip=false` 消息后才恢复为 `idle`。MuJoCo passive viewer
+自带的 `Reload` 按钮仍为灰色；它用于重新加载模型，不用于控制本项目的运行时状态。
+
 ## 模型说明
 
 单臂 URDF 是 RM75-B 运动链几何参数的唯一来源。导入的双臂 URDF 仅用于提供左右

ik_qp/src/rm75_ik/mujoco_robot.py

@@ -1,8 +1,9 @@
 from __future__ import annotations
 
+from contextlib import nullcontext
 from dataclasses import dataclass
 from time import sleep
-from typing import Collection, Dict, Mapping, Optional
+from typing import Callable, Collection, Dict, Mapping, Optional
 
 import numpy as np
 import pinocchio as pin
@@ -47,6 +48,7 @@ class MujocoRobot:
         self._viewer = None
         self._closed = False
         self._stopped_arms: set[ArmName] = set()
+        self._initial_joint_positions: Optional[Dict[ArmName, np.ndarray]] = None
         self.initial_pose_diagnostics: Dict[ArmName, InitialPoseDiagnostic] = {}
         if initialize_from_tcp:
             self._initialize_from_configured_tcp()
@@ -93,6 +95,9 @@ class MujocoRobot:
                 configured_joint_orientation_error_rad=configured_error[1],
             )
         self.scene.set_dual_configuration(solved["left"], solved["right"])
+        self._initial_joint_positions = {
+            arm: solved[arm].copy() for arm in ("left", "right")
+        }
         for arm in ("left", "right"):
             self.set_target_tcp_pose(arm, self.profiles[arm].initial_tcp)
 
@@ -140,6 +145,23 @@ class MujocoRobot:
             q, self.profiles[arm].tool_from_flange
         )
 
+    def reset_to_initial(self) -> None:
+        """Restore the solved startup state without recreating the MuJoCo model."""
+
+        self._require_open()
+        if self._initial_joint_positions is None:
+            raise RuntimeError("MuJoCo initial joint positions are unavailable")
+        viewer_lock = self._viewer.lock() if self._viewer is not None else nullcontext()
+        with viewer_lock:
+            self.scene.set_dual_configuration(
+                self._initial_joint_positions["left"],
+                self._initial_joint_positions["right"],
+            )
+            self.scene.data.qvel[:] = 0.0
+            for arm in ("left", "right"):
+                self.set_target_tcp_pose(arm, self.profiles[arm].initial_tcp)
+        self._stopped_arms.update(("left", "right"))
+
     def stop(self, arms: Collection[ArmName] = ("left", "right")) -> None:
         self._require_open()
         selected = set(arms)
@@ -148,13 +170,23 @@ class MujocoRobot:
         self.scene.data.qvel[:] = 0.0
         self._stopped_arms.update(selected)
 
-    def open_viewer(self):
+    def open_viewer(self, key_callback: Optional[Callable[[int], None]] = None):
         self._require_open()
         if self._viewer is None:
             import mujoco.viewer
 
             self._viewer = mujoco.viewer.launch_passive(
-                self.scene.model, self.scene.data
+                self.scene.model,
+                self.scene.data,
+                key_callback=key_callback,
+            )
+            self._viewer.set_texts(
+                (
+                    None,
+                    mujoco.mjtGridPos.mjGRID_BOTTOMLEFT,
+                    "Reset dual arms",
+                    "R / Home",
+                )
             )
         return self._viewer
 

ik_qp/src/rm75_ik/robot_backend.py

@@ -38,6 +38,8 @@ class RobotBackend(Protocol):
 
     def set_target_tcp_pose(self, arm: ArmName, target_tcp: pin.SE3) -> None: ...
 
+    def reset_to_initial(self) -> None: ...
+
     def stop(self, arms: Collection[ArmName] = ("left", "right")) -> None: ...
 
     def close(self) -> None: ...

ik_qp/src/rm75_ik/teleop_control.py

@@ -17,6 +17,7 @@ from .types import IkOptions, teleop_joint_limits
 class SafetyState(str, Enum):
     IDLE = "idle"
     ACTIVE = "active"
+    RESETTING = "resetting"
     FAULT = "fault"
 
 
@@ -297,6 +298,7 @@ class DualArmQpTeleopController:
         }
         self._latest: Dict[ArmName, tuple[ControllerSample, float]] = {}
         self._fault_reason = ""
+        self._reset_waiting_for_release = False
         self._closed = False
         self.robot.connect()
 
@@ -304,6 +306,8 @@ class DualArmQpTeleopController:
     def safety_state(self) -> SafetyState:
         if self._fault_reason:
             return SafetyState.FAULT
+        if self._reset_waiting_for_release:
+            return SafetyState.RESETTING
         if any(mapper.active for mapper in self.mappers.values()):
             return SafetyState.ACTIVE
         return SafetyState.IDLE
@@ -326,6 +330,27 @@ class DualArmQpTeleopController:
     def reject_input(self, reason: str) -> None:
         self._trip_fault(f"invalid controller input: {reason}")
 
+    def reset_to_initial(self) -> ControlCycleResult:
+        """Reset the backend and require fresh dual-grip release before rearming."""
+
+        if self._closed:
+            raise RuntimeError("controller is closed")
+        for mapper in self.mappers.values():
+            mapper.reset()
+        try:
+            self.robot.stop(("left", "right"))
+            self.robot.reset_to_initial()
+        except Exception as exc:
+            self._trip_fault(f"reset/backend failure: {exc}")
+            raise RuntimeError(f"failed to reset robot backend: {exc}") from exc
+        self._latest.clear()
+        self._fault_reason = ""
+        self._reset_waiting_for_release = True
+        return ControlCycleResult(
+            SafetyState.RESETTING,
+            reason="reset complete; waiting for fresh dual-grip release",
+        )
+
     def step(self, timestamp_sec: float) -> ControlCycleResult:
         if self._closed:
             raise RuntimeError("controller is closed")
@@ -336,6 +361,15 @@ class DualArmQpTeleopController:
                 self._fault_reason = ""
                 return ControlCycleResult(SafetyState.IDLE, reason="fault cleared")
             return ControlCycleResult(SafetyState.FAULT, reason=self._fault_reason)
+        if self._reset_waiting_for_release:
+            if self._can_finish_reset(timestamp_sec):
+                self._reset_waiting_for_release = False
+                self._latest.clear()
+                return ControlCycleResult(SafetyState.IDLE, reason="reset rearmed")
+            return ControlCycleResult(
+                SafetyState.RESETTING,
+                reason="waiting for fresh dual-grip release",
+            )
 
         try:
             state = self.robot.read_joint_positions()
@@ -404,8 +438,22 @@ class DualArmQpTeleopController:
             mapper.reset()
         return True
 
+    def _can_finish_reset(self, timestamp_sec: float) -> bool:
+        if set(self._latest) != {"left", "right"}:
+            return False
+        for arm, (sample, sample_time) in self._latest.items():
+            if sample.grip:
+                return False
+            age = timestamp_sec - sample_time
+            if age < -1e-6 or age > self.profiles[arm].command_timeout_sec:
+                return False
+        for mapper in self.mappers.values():
+            mapper.reset()
+        return True
+
     def _trip_fault(self, reason: str) -> ControlCycleResult:
         self._fault_reason = str(reason)
+        self._reset_waiting_for_release = False
         for mapper in self.mappers.values():
             mapper.reset()
         try:
@@ -415,6 +463,7 @@ class DualArmQpTeleopController:
         return ControlCycleResult(SafetyState.FAULT, reason=self._fault_reason)
 
     def stop(self) -> None:
+        self._reset_waiting_for_release = False
         for mapper in self.mappers.values():
             mapper.reset()
         self.robot.stop(("left", "right"))

ik_qp/tests/test_stage3_control.py

@@ -159,6 +159,40 @@ def test_active_arm_timeout_faults_both_and_requires_release(profiles):
         controller.close()
 
 
+def test_reset_restores_initial_state_and_requires_fresh_dual_release(profiles):
+    robot = MujocoRobot(profiles)
+    controller = DualArmQpTeleopController(robot, profiles)
+    try:
+        initial = robot.read_joint_positions().positions_rad
+        for arm in ("left", "right"):
+            controller.update_sample(sample(arm, True), 0.0)
+        assert controller.step(0.0).state is SafetyState.ACTIVE
+
+        for arm in ("left", "right"):
+            controller.update_sample(sample(arm, True, [0, 0.005, 0]), 0.01)
+        assert controller.step(0.01).state is SafetyState.ACTIVE
+        moved = robot.read_joint_positions().positions_rad
+        assert any(
+            np.max(np.abs(moved[arm] - initial[arm])) > 1e-5
+            for arm in ("left", "right")
+        )
+
+        reset = controller.reset_to_initial()
+        assert reset.state is SafetyState.RESETTING
+        restored = robot.read_joint_positions().positions_rad
+        for arm in ("left", "right"):
+            np.testing.assert_array_equal(restored[arm], initial[arm])
+
+        assert controller.step(0.02).state is SafetyState.RESETTING
+        controller.update_sample(sample("left", False), 0.03)
+        controller.update_sample(sample("right", True), 0.03)
+        assert controller.step(0.03).state is SafetyState.RESETTING
+        controller.update_sample(sample("right", False), 0.04)
+        assert controller.step(0.04).state is SafetyState.IDLE
+    finally:
+        controller.close()
+
+
 def test_message_compatible_adapter_rejects_wrong_hand():
     message = SimpleNamespace(
         hand="right",