feat: add reset ability in mujoco

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()