Refactor and enhance XR-RM teleoperation functionality

xr_rm_teleop/xr_rm_teleop/realman_adapter.py

@@ -1,12 +1,11 @@
 """RM75 机械臂适配层。
 
-对上提供统一的当前位姿读取、笛卡尔速度发送和停止接口；对下根据配置
+对上提供统一的当前位姿读取、笛卡尔位姿目标发送和停止接口；对下根据配置
 选择 mock 积分模拟器或睿尔曼 Python API2 真机通信。
 """
 
 from __future__ import annotations
 
-import time
 from dataclasses import dataclass
 from numbers import Number
 from typing import Any
@@ -40,16 +39,17 @@ class MockRealManAdapter:
     def get_current_pose(self) -> ArmPose:
         return self._pose
 
-    def send_cartesian_velocity(self, velocity: list[float], follow: bool) -> None:
+    def send_cartesian_target(self, pose: ArmPose, 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
+        self.last_velocity = [
+            (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,
+        ]
+        self._pose = pose
 
     def stop(self) -> None:
         self.last_velocity = [0.0] * 6
@@ -65,13 +65,12 @@ class MockRealManAdapter:
 
 
 class RealManAdapter:
-    """睿尔曼 Python API2 的笛卡尔速度透传适配层。"""
+    """睿尔曼 Python API2 的笛卡尔位姿透传适配层。"""
 
     def __init__(
         self,
         robot_ip: str,
         robot_port: int,
-        dt: float,
         avoid_singularity: int,
         frame_type: int,
         logger: Any | None = None,
@@ -86,13 +85,11 @@ class RealManAdapter:
         initial_joint_pose: list[float] | None = None,
         initial_tcp_pose: list[float] | None = None,
         init_move_speed: int = 20,
-        command_mode: str = "velocity",
         canfd_trajectory_mode: int = 2,
         canfd_radio: int = 0,
     ) -> 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._logger = logger
@@ -107,13 +104,10 @@ class RealManAdapter:
         self._initial_joint_pose = initial_joint_pose
         self._initial_tcp_pose = initial_tcp_pose
         self._init_move_speed = init_move_speed
-        self._command_mode = command_mode
         self._canfd_trajectory_mode = canfd_trajectory_mode
         self._canfd_radio = canfd_radio
         self._scissorgripper: int | None = None
         self._arm: Any | None = None
-        if self._command_mode not in ("velocity", "pose_canfd"):
-            raise ValueError("command_mode must be one of: velocity, pose_canfd")
 
     def connect(self) -> None:
         try:
@@ -130,17 +124,12 @@ 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}, dt_ms={self._dt_ms}, "
-            f"command_mode={self._command_mode}"
+            f"frame_type={self._frame_type}, command=rm_movep_canfd"
         )
         if self._configure_safety_limits:
             self._apply_safety_limits()
         if self._move_to_initial_pose_on_connect:
             self._move_to_initial_pose()
-        if self._command_mode == "velocity":
-            self._init_movev_canfd()
-        else:
-            self._log_info("command_mode=pose_canfd，跳过 rm_set_movev_canfd_init。")
 
     def get_current_pose(self) -> ArmPose:
         self._require_arm()
@@ -150,17 +139,8 @@ class RealManAdapter:
             raise RuntimeError(f"无法从睿尔曼状态中解析当前 TCP 位姿：{state!r}")
         return ArmPose(*pose[:6])
 
-    def send_cartesian_velocity(self, velocity: list[float], follow: bool) -> None:
-        self._require_arm()
-        if self._command_mode == "pose_canfd":
-            return
-        ret = self._arm.rm_movev_canfd(velocity, follow, 0, 0)
-        self._check_return(ret, "rm_movev_canfd")
-
     def send_cartesian_target(self, pose: ArmPose, follow: bool) -> None:
         self._require_arm()
-        if self._command_mode != "pose_canfd":
-            raise RuntimeError("send_cartesian_target requires command_mode=pose_canfd")
         ret = self._arm.rm_movep_canfd(
             [pose.x, pose.y, pose.z, pose.rx, pose.ry, pose.rz],
             follow,
@@ -169,9 +149,6 @@ class RealManAdapter:
         )
         self._check_return(ret, "rm_movep_canfd")
 
-    def uses_pose_targets(self) -> bool:
-        return self._command_mode == "pose_canfd"
-
     def configure_peripheral(self, config_file: str, peripheral_arm: str) -> None:
         self._require_arm()
         from .fun_peripheral import load_peripheral_config, peripheral_cfg
@@ -205,8 +182,6 @@ class RealManAdapter:
         if self._arm is None:
             return
         try:
-            if self._command_mode == "velocity":
-                self._arm.rm_movev_canfd([0.0] * 6, False, 0, 0)
             self._arm.rm_set_arm_slow_stop()
         except Exception:
             pass
@@ -244,46 +219,6 @@ class RealManAdapter:
         ret = self._arm.rm_movel(self._initial_tcp_pose, self._init_move_speed, 0, 0, 1)
         self._check_return(ret, "rm_movel(initial_tcp_pose)")
 
-    def _init_movev_canfd(self) -> None:
-        attempts = 3
-        candidates = [self._avoid_singularity]
-        if self._avoid_singularity != 0:
-            candidates.append(0)
-
-        # 某些现场控制器初始化避奇异模式会超时，失败后自动降级到 0 再重试。
-        last_ret: Any = None
-        for avoid_singularity in candidates:
-            if avoid_singularity != self._avoid_singularity:
-                self._log_warn("rm_set_movev_canfd_init 降级为 avoid_singularity=0 后重试。")
-
-            for attempt in range(1, attempts + 1):
-                ret = self._arm.rm_set_movev_canfd_init(
-                    avoid_singularity,
-                    self._frame_type,
-                    self._dt_ms,
-                )
-                last_ret = ret
-                code = self._return_code(ret)
-                if code == 0:
-                    self._avoid_singularity = avoid_singularity
-                    self._log_info(
-                        "rm_set_movev_canfd_init 成功："
-                        f"avoid_singularity={avoid_singularity}, "
-                        f"frame_type={self._frame_type}, dt_ms={self._dt_ms}"
-                    )
-                    return
-
-                self._log_warn(
-                    "rm_set_movev_canfd_init 失败："
-                    f"avoid_singularity={avoid_singularity}, "
-                    f"frame_type={self._frame_type}, dt_ms={self._dt_ms}, "
-                    f"attempt={attempt}/{attempts}, ret={ret!r}"
-                )
-                if attempt < attempts:
-                    time.sleep(0.2)
-
-        self._check_return(last_ret, "rm_set_movev_canfd_init")
-
     def _try_call(self, name: str, *args: Any) -> None:
         func = getattr(self._arm, name, None)
         if func is None: