Add Placo IK solver and associated tests.

xr_rm_teleop/xr_rm_teleop/realman_adapter.py

@@ -1,21 +1,15 @@
-"""RM75 机械臂适配层。
-
-对上提供统一的当前位姿读取、笛卡尔位姿目标发送和停止接口；对下根据配置
-选择 mock 积分模拟器或睿尔曼 Python API2 真机通信。
-"""
+"""RM75 机械臂关节反馈、关节透传和停止适配层。"""
 
 from __future__ import annotations
 
 import math
+import threading
+import time
 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
@@ -32,55 +26,64 @@ class ArmPose:
         return [self.rx, self.ry, self.rz]
 
 
-class MockRealManAdapter:
-    """无机械臂时使用的运动学模拟器，用于验证 ROS2 遥操链路。"""
+@dataclass(frozen=True)
+class JointStateSnapshot:
+    positions: list[float]
+    received_at: float
 
-    def __init__(self, initial_pose: list[float], dt: float) -> None:
-        self._pose = ArmPose(*initial_pose[:6])
-        self._dt = dt
-        self.last_velocity = [0.0] * 6
+
+class MockRealManAdapter:
+    """不导入厂商 SDK 的关节状态 mock。"""
+
+    def __init__(self, initial_joint_degrees: list[float]) -> None:
+        if len(initial_joint_degrees) != 7 or not all(
+            math.isfinite(value) for value in initial_joint_degrees
+        ):
+            raise ValueError("initial joint pose must contain 7 finite values")
+        self._joint_positions = [
+            math.radians(value) for value in initial_joint_degrees
+        ]
+        self.last_joint_target: list[float] | None = None
         self.last_tool_open: bool | None = None
 
     def connect(self) -> None:
         return
 
-    def get_current_pose(self) -> ArmPose:
-        return self._pose
+    def get_latest_joint_state(self) -> JointStateSnapshot:
+        return JointStateSnapshot(
+            list(self._joint_positions),
+            time.monotonic(),
+        )
 
-    def send_cartesian_target(self, pose: ArmPose, follow: bool) -> None:
+    def send_joint_target(self, joints: list[float], follow: bool) -> None:
         del follow
-        self.last_velocity = [
-            (pose.x - self._pose.x) / self._dt,
-            (pose.y - self._pose.y) / self._dt,
-            (pose.z - self._pose.z) / self._dt,
-            _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
+        if len(joints) != 7 or not all(math.isfinite(value) for value in joints):
+            raise ValueError("joint target must contain 7 finite values")
+        self._joint_positions = list(joints)
+        self.last_joint_target = list(joints)
 
     def stop(self) -> None:
-        self.last_velocity = [0.0] * 6
+        return
 
     def close(self) -> None:
         self.stop()
 
-    def configure_peripheral(self, config_file: str, peripheral_arm: str) -> None:
-        del config_file, peripheral_arm
+    def configure_peripheral(self, config: Any, peripheral_arm: str) -> None:
+        del config, peripheral_arm
 
     def set_tool_enabled(self, open_tool: bool) -> None:
         self.last_tool_open = open_tool
 
 
 class RealManAdapter:
-    """睿尔曼 Python API2 的笛卡尔位姿透传适配层。"""
+    """复用一个睿尔曼 Python API2 连接的关节适配层。"""
 
     def __init__(
         self,
         robot_ip: str,
         robot_port: int,
         avoid_singularity: int,
-        frame_type: int,
+        feedback_period: float,
         logger: Any | None = None,
         configure_safety_limits: bool = True,
         max_line_speed: float = 1.0,
@@ -98,7 +101,9 @@ class RealManAdapter:
         self._robot_ip = robot_ip
         self._robot_port = robot_port
         self._avoid_singularity = avoid_singularity
-        self._frame_type = frame_type
+        if feedback_period <= 0.0:
+            raise ValueError("feedback_period must be positive")
+        self._feedback_period = feedback_period
         self._logger = logger
         self._configure_safety_limits = configure_safety_limits
         self._max_line_speed = max_line_speed
@@ -114,6 +119,11 @@ class RealManAdapter:
         self._canfd_radio = canfd_radio
         self._scissorgripper: int | None = None
         self._arm: Any | None = None
+        self._joint_state_lock = threading.Lock()
+        self._latest_joint_state: JointStateSnapshot | None = None
+        self._feedback_stop = threading.Event()
+        self._feedback_thread: threading.Thread | None = None
+        self._feedback_fault_logged = False
 
     def connect(self) -> None:
         try:
@@ -130,38 +140,48 @@ class RealManAdapter:
             "RealMan connected: "
             f"ip={self._robot_ip}, port={self._robot_port}, "
             f"avoid_singularity={self._avoid_singularity}, "
-            f"frame_type={self._frame_type}, command=rm_movep_canfd"
+            "command=rm_movej_canfd"
         )
         if self._configure_safety_limits:
             self._apply_safety_limits()
         if self._move_to_initial_pose_on_connect:
             self._move_to_initial_pose()
+        self._feedback_stop.clear()
+        self._feedback_thread = threading.Thread(
+            target=self._feedback_loop,
+            name=f"rm75_feedback_{self._robot_ip}",
+            daemon=True,
+        )
+        self._feedback_thread.start()
 
-    def get_current_pose(self) -> ArmPose:
-        self._require_arm()
-        state = self._arm.rm_get_current_arm_state()
-        pose = self._find_pose(state)
-        if pose is None:
-            raise RuntimeError(f"无法从睿尔曼状态中解析当前 TCP 位姿：{state!r}")
-        return ArmPose(*pose[:6])
+    def get_latest_joint_state(self) -> JointStateSnapshot | None:
+        with self._joint_state_lock:
+            if self._latest_joint_state is None:
+                return None
+            return JointStateSnapshot(
+                list(self._latest_joint_state.positions),
+                self._latest_joint_state.received_at,
+            )
 
-    def send_cartesian_target(self, pose: ArmPose, follow: bool) -> None:
+    def send_joint_target(self, joints: list[float], follow: bool) -> None:
         self._require_arm()
-        ret = self._arm.rm_movep_canfd(
-            [pose.x, pose.y, pose.z, pose.rx, pose.ry, pose.rz],
+        if len(joints) != 7 or not all(math.isfinite(value) for value in joints):
+            raise ValueError("joint target must contain 7 finite values")
+        ret = self._arm.rm_movej_canfd(
+            [math.degrees(value) for value in joints],
             follow,
+            0,
             self._canfd_trajectory_mode,
             self._canfd_radio,
         )
-        self._check_return(ret, "rm_movep_canfd")
+        self._check_return(ret, "rm_movej_canfd")
 
-    def configure_peripheral(self, config_file: str, peripheral_arm: str) -> None:
+    def configure_peripheral(self, config: Any, peripheral_arm: str) -> None:
         self._require_arm()
-        from .fun_peripheral import load_peripheral_config, peripheral_cfg
+        from .fun_peripheral import peripheral_cfg
 
-        config = load_peripheral_config(config_file, peripheral_arm)
         self._scissorgripper = config.scissorgripper
-        tool_name = list(config.tools_in_ee.keys())[config.scissorgripper]
+        tool_name = config.tool_name
         self._log_info(
             "开始配置 RealMan 末端外设："
             f"arm={peripheral_arm}, scissorgripper={config.scissorgripper}, "
@@ -196,6 +216,12 @@ class RealManAdapter:
         if self._arm is None:
             return
         self.stop()
+        self._feedback_stop.set()
+        if self._feedback_thread is not None:
+            self._feedback_thread.join(timeout=3.0)
+            if self._feedback_thread.is_alive():
+                self._log_warn("RealMan 关节反馈线程未在 3 秒内退出。")
+            self._feedback_thread = None
         try:
             self._arm.rm_delete_robot_arm()
         finally:
@@ -205,6 +231,37 @@ class RealManAdapter:
         if self._arm is None:
             raise RuntimeError("睿尔曼机械臂尚未连接")
 
+    def _feedback_loop(self) -> None:
+        while not self._feedback_stop.is_set():
+            try:
+                self._read_joint_state_once()
+                self._feedback_fault_logged = False
+            except Exception as exc:
+                if not self._feedback_fault_logged:
+                    self._log_warn(f"RealMan 关节反馈读取失败：{exc}")
+                    self._feedback_fault_logged = True
+            self._feedback_stop.wait(self._feedback_period)
+
+    def _read_joint_state_once(self) -> None:
+        self._require_arm()
+        result = self._arm.rm_get_joint_degree()
+        self._check_return(result, "rm_get_joint_degree")
+        if not isinstance(result, tuple) or len(result) < 2:
+            raise RuntimeError(f"rm_get_joint_degree 返回格式错误：{result!r}")
+        degrees = result[1]
+        if (
+            not isinstance(degrees, (list, tuple))
+            or len(degrees) != 7
+            or not all(isinstance(value, Number) for value in degrees)
+        ):
+            raise RuntimeError(f"RM75 关节反馈必须包含 7 个数值：{degrees!r}")
+        positions = [math.radians(float(value)) for value in degrees]
+        if not all(math.isfinite(value) for value in positions):
+            raise RuntimeError("RM75 关节反馈包含 NaN/Inf")
+        snapshot = JointStateSnapshot(positions, time.monotonic())
+        with self._joint_state_lock:
+            self._latest_joint_state = snapshot
+
     def _apply_safety_limits(self) -> None:
         # 真机安全限幅尽量下发到控制器；不支持的 SDK 接口会在 _try_call 中降级为警告。
         self._try_call("rm_set_avoid_singularity_mode", int(self._avoid_singularity))
@@ -260,74 +317,3 @@ class RealManAdapter:
     @staticmethod
     def _return_code(ret: Any) -> Any:
         return ret[0] if isinstance(ret, tuple) and ret else ret
-
-    @classmethod
-    def _find_pose(cls, obj: Any) -> list[float] | None:
-        # 不同 SDK 版本返回字段可能略有差异，因此递归查找常见 TCP 位姿字段。
-        if isinstance(obj, dict):
-            for key in ("pose", "tool_pose", "tcp_pose", "current_pose"):
-                pose = cls._as_pose(obj.get(key))
-                if pose is not None:
-                    return pose
-            for value in obj.values():
-                pose = cls._find_pose(value)
-                if pose is not None:
-                    return pose
-        elif isinstance(obj, (list, tuple)):
-            pose = cls._as_pose(obj)
-            if pose is not None:
-                return pose
-            for value in obj:
-                pose = cls._find_pose(value)
-                if pose is not None:
-                    return pose
-        elif hasattr(obj, "to_dictionary"):
-            try:
-                return cls._find_pose(obj.to_dictionary(7))
-            except TypeError:
-                return cls._find_pose(obj.to_dictionary())
-        elif hasattr(obj, "to_dict"):
-            return cls._find_pose(obj.to_dict())
-        else:
-            for key in ("pose", "tool_pose", "tcp_pose", "current_pose"):
-                if hasattr(obj, key):
-                    pose = cls._as_pose(getattr(obj, key))
-                    if pose is not None:
-                        return pose
-        return None
-
-    @staticmethod
-    def _as_pose(value: Any) -> list[float] | None:
-        if isinstance(value, (list, tuple)) and len(value) >= 6:
-            if all(isinstance(item, Number) for item in value[:6]):
-                return [float(item) for item in value[:6]]
-        if isinstance(value, dict):
-            position = value.get("position")
-            euler = value.get("euler")
-            if isinstance(position, dict) and isinstance(euler, dict):
-                keys = ("x", "y", "z")
-                rpy_keys = ("rx", "ry", "rz")
-                if all(key in position for key in keys) and all(key in euler for key in rpy_keys):
-                    return [
-                        float(position["x"]),
-                        float(position["y"]),
-                        float(position["z"]),
-                        float(euler["rx"]),
-                        float(euler["ry"]),
-                        float(euler["rz"]),
-                    ]
-        if all(hasattr(value, attr) for attr in ("position", "euler")):
-            position = getattr(value, "position")
-            euler = getattr(value, "euler")
-            if all(hasattr(position, key) for key in ("x", "y", "z")) and all(
-                hasattr(euler, key) for key in ("rx", "ry", "rz")
-            ):
-                return [
-                    float(position.x),
-                    float(position.y),
-                    float(position.z),
-                    float(euler.rx),
-                    float(euler.ry),
-                    float(euler.rz),
-                ]
-        return None