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