From 0d7e30cb7be1acc9c209fe219bdb57e9033bd6f8 Mon Sep 17 00:00:00 2001 From: YikaiFu-cart Date: Thu, 2 Jul 2026 14:02:44 +0800 Subject: [PATCH] feat: add reset ability in mujoco --- ik_qp/README.md | 11 +++++ ik_qp/src/rm75_ik/mujoco_robot.py | 38 ++++++++++++-- ik_qp/src/rm75_ik/robot_backend.py | 2 + ik_qp/src/rm75_ik/teleop_control.py | 49 +++++++++++++++++++ ik_qp/tests/test_stage3_control.py | 34 +++++++++++++ xr_rm_teleop/package.xml | 1 + .../xr_rm_teleop/dual_arm_qp_teleop.py | 46 ++++++++++++++++- 7 files changed, 176 insertions(+), 5 deletions(-) diff --git a/ik_qp/README.md b/ik_qp/README.md index 5fc77ea..d88d6e5 100644 --- a/ik_qp/README.md +++ b/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 仅用于提供左右 diff --git a/ik_qp/src/rm75_ik/mujoco_robot.py b/ik_qp/src/rm75_ik/mujoco_robot.py index 671a40d..8202a47 100644 --- a/ik_qp/src/rm75_ik/mujoco_robot.py +++ b/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 diff --git a/ik_qp/src/rm75_ik/robot_backend.py b/ik_qp/src/rm75_ik/robot_backend.py index 860a80c..1c11b76 100644 --- a/ik_qp/src/rm75_ik/robot_backend.py +++ b/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: ... diff --git a/ik_qp/src/rm75_ik/teleop_control.py b/ik_qp/src/rm75_ik/teleop_control.py index 691a99e..8c08b4f 100644 --- a/ik_qp/src/rm75_ik/teleop_control.py +++ b/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")) diff --git a/ik_qp/tests/test_stage3_control.py b/ik_qp/tests/test_stage3_control.py index 2f7eebb..b60105e 100644 --- a/ik_qp/tests/test_stage3_control.py +++ b/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", diff --git a/xr_rm_teleop/package.xml b/xr_rm_teleop/package.xml index 73c480d..eaea12b 100755 --- a/xr_rm_teleop/package.xml +++ b/xr_rm_teleop/package.xml @@ -16,6 +16,7 @@ sensor_msgs python3-yaml std_msgs + std_srvs xr_rm_interfaces ament_lint_auto diff --git a/xr_rm_teleop/xr_rm_teleop/dual_arm_qp_teleop.py b/xr_rm_teleop/xr_rm_teleop/dual_arm_qp_teleop.py index 7541a5c..dc6736f 100644 --- a/xr_rm_teleop/xr_rm_teleop/dual_arm_qp_teleop.py +++ b/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()