feat: 复用适配器初始位姿运动

xr_rm_teleop/test/test_initial_joint_pose.py

@@ -30,11 +30,23 @@ def test_initial_pose_uses_joint_move_only() -> None:
     )
     adapter._arm = FakeArm()
 
-    adapter._move_to_initial_pose()
+    adapter.move_to_initial_pose()
 
     assert adapter._arm.calls == [(joints, 20, 0, 0, 1)]
 
 
+def test_mock_initial_pose_restores_configured_joints() -> None:
+    initial_degrees = [-78.81, 3.22, 67.96, 97.12, 95.08, -81.11, -74.55]
+    adapter = MockRealManAdapter(initial_degrees)
+    adapter.send_joint_target([0.0] * 7, follow=False)
+
+    adapter.move_to_initial_pose()
+
+    assert adapter.read_joint_state().positions == pytest.approx(
+        [math.radians(value) for value in initial_degrees]
+    )
+
+
 def test_peripheral_config_exposes_selected_tool() -> None:
     config = PeripheralConfig(
         scissorgripper=1,

xr_rm_teleop/xr_rm_teleop/realman_adapter.py

@@ -44,9 +44,10 @@ class MockRealManAdapter:
             math.isfinite(value) for value in initial_joint_degrees
         ):
             raise ValueError("initial joint pose must contain 7 finite values")
-        self._joint_positions = [
+        self._initial_joint_positions = [
             math.radians(value) for value in initial_joint_degrees
         ]
+        self._joint_positions = list(self._initial_joint_positions)
         self.last_joint_target: list[float] | None = None
         self.last_tool_open: bool | None = None
 
@@ -69,6 +70,10 @@ class MockRealManAdapter:
         self._joint_positions = list(joints)
         self.last_joint_target = list(joints)
 
+    def move_to_initial_pose(self) -> None:
+        self._joint_positions = list(self._initial_joint_positions)
+        self.last_joint_target = list(self._joint_positions)
+
     def stop(self) -> None:
         return
 
@@ -178,7 +183,7 @@ class RealManAdapter:
             if self._configure_safety_limits:
                 self._apply_safety_limits()
             if self._move_to_initial_pose_on_connect:
-                self._move_to_initial_pose()
+                self.move_to_initial_pose()
             self._feedback_ready.clear()
             self._accept_realtime_feedback = True
             self._realtime_callback = rm_realtime_arm_state_callback_ptr(
@@ -451,11 +456,18 @@ class RealManAdapter:
             self._try_call("rm_set_joint_max_speed", joint_index, self._joint_max_speed)
             self._try_call("rm_set_joint_max_acc", joint_index, self._joint_max_acc)
 
-    def _move_to_initial_pose(self) -> None:
+    def move_to_initial_pose(self) -> None:
+        self._require_arm()
         if self._initial_joint_pose is None:
             raise RuntimeError("启用初始位姿移动时必须配置 initial_joint_pose")
 
-        ret = self._arm.rm_movej(self._initial_joint_pose, self._init_move_speed, 0, 0, 1)
+        ret = self._arm.rm_movej(
+            self._initial_joint_pose,
+            self._init_move_speed,
+            0,
+            0,
+            1,
+        )
         self._check_return(ret, "rm_movej(initial_joint_pose)")
 
     def _try_call(self, name: str, *args: Any) -> None: