Enhance orientation control for RM75 arms in XR teleoperation

xr_rm_teleop/xr_rm_teleop/realman_adapter.py

@@ -6,11 +6,16 @@
 
 from __future__ import annotations
 
+import math
 from dataclasses import dataclass
 from numbers import Number
 from typing import Any
 
 
+def _angle_delta(target: float, current: float) -> float:
+    return math.atan2(math.sin(target - current), math.cos(target - current))
+
+
 @dataclass
 class ArmPose:
     x: float
@@ -23,6 +28,9 @@ class ArmPose:
     def xyz(self) -> list[float]:
         return [self.x, self.y, self.z]
 
+    def rpy(self) -> list[float]:
+        return [self.rx, self.ry, self.rz]
+
 
 class MockRealManAdapter:
     """无机械臂时使用的运动学模拟器，用于验证 ROS2 遥操链路。"""
@@ -45,9 +53,9 @@ class MockRealManAdapter:
             (pose.x - self._pose.x) / self._dt,
             (pose.y - self._pose.y) / self._dt,
             (pose.z - self._pose.z) / self._dt,
-            0.0,
-            0.0,
-            0.0,
+            _angle_delta(pose.rx, self._pose.rx) / self._dt,
+            _angle_delta(pose.ry, self._pose.ry) / self._dt,
+            _angle_delta(pose.rz, self._pose.rz) / self._dt,
         ]
         self._pose = pose