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",

xr_rm_teleop/package.xml

@@ -16,6 +16,7 @@
   <exec_depend>sensor_msgs</exec_depend>
   <exec_depend>python3-yaml</exec_depend>
   <exec_depend>std_msgs</exec_depend>
+  <exec_depend>std_srvs</exec_depend>
   <exec_depend>xr_rm_interfaces</exec_depend>
 
   <test_depend>ament_lint_auto</test_depend>

xr_rm_teleop/xr_rm_teleop/dual_arm_qp_teleop.py

@@ -3,6 +3,7 @@
 from __future__ import annotations
 
 import os
+import threading
 from pathlib import Path
 
 import rclpy
@@ -12,6 +13,7 @@ from rclpy.executors import ExternalShutdownException
 from rclpy.node import Node
 from sensor_msgs.msg import JointState
 from std_msgs.msg import Bool, String
+from std_srvs.srv import Trigger
 from xr_rm_interfaces.msg import XrController
 
 
@@ -55,9 +57,14 @@ class DualArmQpTeleop(Node):
             self._robot, profiles, control_rate_hz=control_rate_hz
         )
         self._closed = False
+        self._viewer_reset_requested = threading.Event()
+        self._viewer_reset_key_codes: set[int] = set()
         self._show_viewer = bool(self.get_parameter("show_viewer").value)
         if self._show_viewer:
-            self._robot.open_viewer()
+            from mujoco.glfw import glfw
+
+            self._viewer_reset_key_codes = {glfw.KEY_R, glfw.KEY_HOME}
+            self._robot.open_viewer(key_callback=self._on_viewer_key)
 
         prefix = str(self.get_parameter("debug_topic_prefix").value).rstrip("/")
         self._target_publishers = {
@@ -79,6 +86,11 @@ class DualArmQpTeleop(Node):
         self._joint_state_publisher = self.create_publisher(
             JointState, f"{prefix}/joint_states", 10
         )
+        self._reset_service = self.create_service(
+            Trigger,
+            f"{prefix}/reset_to_initial",
+            self._on_reset_service,
+        )
         self._joint_names = [
             f"{arm}_joint_{joint}"
             for arm in ("left", "right")
@@ -106,7 +118,7 @@ class DualArmQpTeleop(Node):
             )
         self.get_logger().info(
             "dual-arm QP teleop ready: backend=mujoco, grip controls each arm, "
-            "any active-arm fault stops both arms"
+            "R/Home resets both arms, any active-arm fault stops both arms"
         )
 
     def _now_sec(self) -> float:
@@ -124,7 +136,37 @@ class DualArmQpTeleop(Node):
             )
             self._controller.reject_input(str(exc))
 
+    def _on_viewer_key(self, keycode: int) -> None:
+        if keycode in self._viewer_reset_key_codes:
+            self._viewer_reset_requested.set()
+
+    def _request_reset(self, source: str):
+        result = self._controller.reset_to_initial()
+        self.get_logger().info(
+            f"dual-arm MuJoCo reset requested by {source}; "
+            "waiting for fresh left/right grip release"
+        )
+        return result
+
+    def _on_reset_service(self, request, response):
+        del request
+        try:
+            result = self._request_reset("ROS service")
+        except Exception as exc:
+            response.success = False
+            response.message = str(exc)
+        else:
+            response.success = True
+            response.message = result.reason
+        return response
+
     def _control_tick(self) -> None:
+        if self._viewer_reset_requested.is_set():
+            self._viewer_reset_requested.clear()
+            try:
+                self._request_reset("MuJoCo viewer key")
+            except Exception as exc:
+                self.get_logger().error(f"dual-arm MuJoCo reset failed: {exc}")
         result = self._controller.step(self._now_sec())
         stamp = self.get_clock().now().to_msg()
         fault = Bool()