initial

2026-05-20 14:17:43 +08:00
parent 1c83dfb033
commit 5a48619599
31 changed files with 1522 additions and 24 deletions
@@ -0,0 +1,158 @@
+from __future__ import annotations
+
+from dataclasses import dataclass
+from numbers import Number
+from typing import Any
+
+
+@dataclass
+class ArmPose:
+    x: float
+    y: float
+    z: float
+    rx: float = 0.0
+    ry: float = 0.0
+    rz: float = 0.0
+
+    def xyz(self) -> list[float]:
+        return [self.x, self.y, self.z]
+
+
+class MockRealManAdapter:
+    """无机械臂时使用的运动学模拟器，用于验证 ROS2 遥操链路。"""
+
+    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
+
+    def connect(self) -> None:
+        return
+
+    def get_current_pose(self) -> ArmPose:
+        return self._pose
+
+    def send_cartesian_velocity(self, velocity: list[float], follow: bool) -> None:
+        del follow
+        self.last_velocity = velocity
+        # 模拟模式只做简单积分，便于观察控制器是否在按预期更新末端位置。
+        self._pose.x += velocity[0] * self._dt
+        self._pose.y += velocity[1] * self._dt
+        self._pose.z += velocity[2] * self._dt
+        self._pose.rx += velocity[3] * self._dt
+        self._pose.ry += velocity[4] * self._dt
+        self._pose.rz += velocity[5] * self._dt
+
+    def stop(self) -> None:
+        self.last_velocity = [0.0] * 6
+
+    def close(self) -> None:
+        self.stop()
+
+
+class RealManAdapter:
+    """睿尔曼 Python API2 的笛卡尔速度透传适配层。"""
+
+    def __init__(
+        self,
+        robot_ip: str,
+        robot_port: int,
+        dt: float,
+        avoid_singularity: int,
+        frame_type: int,
+    ) -> None:
+        self._robot_ip = robot_ip
+        self._robot_port = robot_port
+        self._dt_ms = int(round(dt * 1000.0))
+        self._avoid_singularity = avoid_singularity
+        self._frame_type = frame_type
+        self._arm: Any | None = None
+
+    def connect(self) -> None:
+        try:
+            from Robotic_Arm.rm_robot_interface import RoboticArm, rm_thread_mode_e
+        except ImportError as exc:
+            raise RuntimeError(
+                "未安装睿尔曼 Python API2。请安装厂商 SDK，或用 use_mock:=true 先跑模拟模式。"
+            ) from exc
+
+        self._arm = RoboticArm(rm_thread_mode_e.RM_TRIPLE_MODE_E)
+        ret = self._arm.rm_create_robot_arm(self._robot_ip, self._robot_port)
+        self._check_return(ret, "rm_create_robot_arm")
+        # 速度透传初始化必须和控制循环周期一致，避免真实机械臂出现周期不稳定。
+        ret = self._arm.rm_set_movev_canfd_init(
+            self._avoid_singularity,
+            self._frame_type,
+            self._dt_ms,
+        )
+        self._check_return(ret, "rm_set_movev_canfd_init")
+
+    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 send_cartesian_velocity(self, velocity: list[float], follow: bool) -> None:
+        self._require_arm()
+        ret = self._arm.rm_movev_canfd(velocity, follow, 0, 0)
+        self._check_return(ret, "rm_movev_canfd")
+
+    def stop(self) -> None:
+        if self._arm is None:
+            return
+        try:
+            self._arm.rm_movev_canfd([0.0] * 6, False, 0, 0)
+            self._arm.rm_set_arm_slow_stop()
+        except Exception:
+            pass
+
+    def close(self) -> None:
+        if self._arm is None:
+            return
+        self.stop()
+        try:
+            self._arm.rm_delete_robot_arm()
+        finally:
+            self._arm = None
+
+    def _require_arm(self) -> None:
+        if self._arm is None:
+            raise RuntimeError("睿尔曼机械臂尚未连接")
+
+    @staticmethod
+    def _check_return(ret: Any, name: str) -> None:
+        code = ret[0] if isinstance(ret, tuple) and ret else ret
+        if isinstance(code, int) and code != 0:
+            raise RuntimeError(f"{name} failed with code {code}: {ret!r}")
+
+    @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
+        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]]
+        return None