dual arm control from sample_udp_sender.py

xr_rm_teleop/setup.py

@@ -1,3 +1,9 @@
+"""xr_rm_teleop 包安装配置。
+
+该包提供基于 XR 相对位移的 RM75 笛卡尔速度遥操作节点，以及把
+手柄 trigger 映射到夹爪力控命令的桥接节点。
+"""
+
 from setuptools import setup
 
 package_name = "xr_rm_teleop"

xr_rm_teleop/xr_rm_teleop/__init__.py

@@ -1 +1,4 @@
+"""XR 到 RM75 遥操作节点包。
 
+核心模块包括单臂速度遥操作、RealMan SDK 适配层和夹爪 trigger 桥接。
+"""

xr_rm_teleop/xr_rm_teleop/gripper_trigger_bridge.py

@@ -1,3 +1,9 @@
+"""手柄 trigger 到夹爪力控指令的桥接节点。
+
+订阅 XR 手柄消息，将 trigger 压力映射为 OmniPicker 可使用的归一化
+闭合力比例，并在消息超时或未握持时自动回到开爪指令。
+"""
+
 from __future__ import annotations
 
 import rclpy
@@ -62,6 +68,7 @@ class GripperTriggerBridge(Node):
     def _publish_command(self) -> None:
         command = self._open_command
 
+        # 手柄消息超时后立即退回开爪，避免夹爪在通信中断时保持闭合力。
         if self._last_msg is not None and self._last_msg_time is not None:
             age = (self.get_clock().now() - self._last_msg_time).nanoseconds * 1e-9
             if age <= self._stale_timeout_sec:

xr_rm_teleop/xr_rm_teleop/realman_adapter.py

@@ -1,5 +1,12 @@
+"""RM75 机械臂适配层。
+
+对上提供统一的当前位姿读取、笛卡尔速度发送和停止接口；对下根据配置
+选择 mock 积分模拟器或睿尔曼 Python API2 真机通信。
+"""
+
 from __future__ import annotations
 
+import time
 from dataclasses import dataclass
 from numbers import Number
 from typing import Any
@@ -72,6 +79,9 @@ 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
@@ -90,7 +100,12 @@ 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._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:
@@ -103,17 +118,21 @@ class RealManAdapter:
         self._arm = RoboticArm(rm_thread_mode_e.RM_TRIPLE_MODE_E)
         handle = self._arm.rm_create_robot_arm(self._robot_ip, self._robot_port)
         self._check_robot_handle(handle)
+        self._log_info(
+            "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}"
+        )
         if self._configure_safety_limits:
             self._apply_safety_limits()
         if self._move_to_initial_pose_on_connect:
             self._move_to_initial_pose()
-        # 速度透传初始化必须和控制循环周期一致，避免真实机械臂出现周期不稳定。
-        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")
+        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()
@@ -125,14 +144,32 @@ class RealManAdapter:
 
     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,
+            self._canfd_trajectory_mode,
+            self._canfd_radio,
+        )
+        self._check_return(ret, "rm_movep_canfd")
+
+    def uses_pose_targets(self) -> bool:
+        return self._command_mode == "pose_canfd"
+
     def stop(self) -> None:
         if self._arm is None:
             return
         try:
-            self._arm.rm_movev_canfd([0.0] * 6, False, 0, 0)
+            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
@@ -151,7 +188,8 @@ class RealManAdapter:
             raise RuntimeError("睿尔曼机械臂尚未连接")
 
     def _apply_safety_limits(self) -> None:
-        self._try_call("rm_set_avoid_singularity_mode", True)
+        # 真机安全限幅尽量下发到控制器；不支持的 SDK 接口会在 _try_call 中降级为警告。
+        self._try_call("rm_set_avoid_singularity_mode", int(self._avoid_singularity))
         self._try_call("rm_set_arm_max_line_speed", self._max_line_speed)
         self._try_call("rm_set_arm_max_angular_speed", self._max_angular_speed)
         self._try_call("rm_set_arm_max_line_acc", self._max_line_acc)
@@ -169,6 +207,46 @@ 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:
@@ -185,18 +263,28 @@ class RealManAdapter:
         if self._logger is not None:
             self._logger.warn(message)
 
-    @staticmethod
-    def _check_robot_handle(handle: Any) -> None:
+    def _log_info(self, message: str) -> None:
+        if self._logger is not None:
+            self._logger.info(message)
+
+    def _check_robot_handle(self, handle: Any) -> None:
         handle_id = getattr(handle, "id", None)
         if handle_id == -1:
-            raise RuntimeError("rm_create_robot_arm failed: socket error or robot unreachable")
+            raise RuntimeError(
+                "rm_create_robot_arm failed: TCP may be reachable, but the RealMan "
+                f"controller did not return robot info from {self._robot_ip}:{self._robot_port}"
+            )
 
     @staticmethod
     def _check_return(ret: Any, name: str) -> None:
-        code = ret[0] if isinstance(ret, tuple) and ret else ret
+        code = RealManAdapter._return_code(ret)
         if isinstance(code, int) and code != 0:
             raise RuntimeError(f"{name} failed with code {code}: {ret!r}")
 
+    @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 位姿字段。

xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py

@@ -1,3 +1,9 @@
+"""RM75 单臂 XR 相对位移遥操作节点。
+
+节点订阅左/右手柄位姿，在 grip 按下时锁定手柄和 TCP 起点，把手柄相对位移
+映射到机器人坐标系中的目标 TCP，并通过笛卡尔速度命令跟随该目标。
+"""
+
 from __future__ import annotations
 
 import math
@@ -60,6 +66,9 @@ class SingleArmVelocityTeleop(Node):
         self.declare_parameter("initial_joint_pose", [0.0] * 7)
         self.declare_parameter("initial_tcp_pose", [0.35, 0.0, 0.30, 0.0, 0.0, 0.0])
         self.declare_parameter("init_move_speed", 20)
+        self.declare_parameter("command_mode", "velocity")
+        self.declare_parameter("canfd_trajectory_mode", 2)
+        self.declare_parameter("canfd_radio", 0)
         self.declare_parameter("debug_topic_prefix", "/xr_rm")
 
         self._arm_name = str(self.get_parameter("arm_name").value)
@@ -107,6 +116,7 @@ class SingleArmVelocityTeleop(Node):
         self.get_logger().info(f"{self._arm_name} 速度遥操节点已启动，监听话题：{topic}")
 
     def _make_adapter(self):
+        # mock 和真机共享同一控制逻辑，只在适配层切换运动命令的执行方式。
         if self._bool_parameter("use_mock"):
             return MockRealManAdapter(
                 [float(v) for v in self.get_parameter("mock_initial_pose").value],
@@ -131,6 +141,9 @@ class SingleArmVelocityTeleop(Node):
             initial_joint_pose=self._float_list_parameter("initial_joint_pose", 7),
             initial_tcp_pose=self._float_list_parameter("initial_tcp_pose", 6),
             init_move_speed=int(self.get_parameter("init_move_speed").value),
+            command_mode=str(self.get_parameter("command_mode").value),
+            canfd_trajectory_mode=int(self.get_parameter("canfd_trajectory_mode").value),
+            canfd_radio=int(self.get_parameter("canfd_radio").value),
         )
 
     def _on_controller(self, msg: XrController) -> None:
@@ -206,7 +219,24 @@ class SingleArmVelocityTeleop(Node):
         ]
         cartesian_velocity = self._filtered_velocity + [0.0, 0.0, 0.0]
         self._publish_debug(robot_pose, target, cartesian_velocity)
-        self._send_cartesian_velocity(cartesian_velocity)
+        if self._adapter_uses_pose_targets():
+            step_target = [
+                robot_now[i] + cartesian_velocity[i] * self._dt
+                for i in range(3)
+            ]
+            step_target = self._clamp_workspace(step_target)
+            self._send_cartesian_target(
+                ArmPose(
+                    x=step_target[0],
+                    y=step_target[1],
+                    z=step_target[2],
+                    rx=robot_pose.rx,
+                    ry=robot_pose.ry,
+                    rz=robot_pose.rz,
+                )
+            )
+        else:
+            self._send_cartesian_velocity(cartesian_velocity)
 
     @staticmethod
     def _controller_xyz(msg: XrController) -> list[float]:
@@ -221,6 +251,7 @@ class SingleArmVelocityTeleop(Node):
         ]
 
     def _clamp_workspace(self, target: list[float]) -> list[float]:
+        # 先做盒状工作空间限幅，再叠加底座圆柱半径限制。
         clamped = [
             _clamp(target[i], self._workspace_min[i], self._workspace_max[i])
             for i in range(3)
@@ -259,6 +290,8 @@ class SingleArmVelocityTeleop(Node):
     def _safe_stop(self) -> None:
         # 所有断连、松手、退出路径都走这里，确保发送零速度。
         self._filtered_velocity = [0.0, 0.0, 0.0]
+        if self._adapter_uses_pose_targets():
+            return
         self._send_cartesian_velocity([0.0] * 6)
 
     def _send_cartesian_velocity(self, velocity: list[float]) -> None:
@@ -271,6 +304,20 @@ class SingleArmVelocityTeleop(Node):
             )
             self._active = False
 
+    def _send_cartesian_target(self, pose: ArmPose) -> None:
+        try:
+            self._adapter.send_cartesian_target(pose, self._follow)
+        except Exception as exc:
+            self.get_logger().error(
+                f"{self._arm_name} 发送位姿透传命令失败：{exc}",
+                throttle_duration_sec=1.0,
+            )
+            self._active = False
+
+    def _adapter_uses_pose_targets(self) -> bool:
+        uses_pose_targets = getattr(self._adapter, "uses_pose_targets", None)
+        return bool(uses_pose_targets is not None and uses_pose_targets())
+
     def _publish_debug(self, current_pose: ArmPose, target_xyz: list[float], velocity: list[float]) -> None:
         stamp = self.get_clock().now().to_msg()
         current_msg = self._pose_msg(stamp, current_pose)