initial

xr_rm_teleop/config/single_arm.yaml

@@ -0,0 +1,34 @@
+# 单臂模拟/默认配置。
+# 默认按最新 acDual-arm 配置中的右臂参数运行，使模拟和真机单臂测试使用同一套坐标约定。
+
+single_arm_velocity_teleop:
+  ros__parameters:
+    arm_name: right_rm75
+    controller_topic: /xr/right_controller
+    control_rate_hz: 50.0
+    command_timeout_sec: 0.12
+
+    scale: 0.75
+    kp_linear: 2.0
+    deadband_m: 0.002
+    low_pass_alpha: 0.35
+    max_linear_speed: 0.04
+    enable_position_axes: [true, true, true]
+
+    workspace_min: [-0.50, -0.60, 0.10]
+    workspace_max: [0.50, -0.20, 0.50]
+
+    # 右臂 PICO/OpenXR 相对位移到 RM75 base 坐标的初始映射：
+    # 机器人位移增量 = [手柄y, -手柄z, -手柄x]。
+    xr_to_robot_matrix: [0.0, 1.0, 0.0,
+                         0.0, 0.0, -1.0,
+                         -1.0, 0.0, 0.0]
+
+    use_mock: true
+    mock_initial_pose: [0.1924, -0.3025, 0.2923, 3.0560, 0.1310, 0.9070]
+
+    robot_ip: 192.168.192.19
+    robot_port: 8080
+    avoid_singularity: 1
+    frame_type: 1
+    follow: false

xr_rm_teleop/package.xml

@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>xr_rm_teleop</name>
+  <version>0.1.0</version>
+  <description>XR relative-motion teleoperation controllers for RealMan RM75 arms.</description>
+  <maintainer email="user@example.com">Yikai Fu</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_python</buildtool_depend>
+
+  <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>rclpy</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>xr_rm_interfaces</exec_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

xr_rm_teleop/resource/xr_rm_teleop

@@ -0,0 +1 @@
+

xr_rm_teleop/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/xr_rm_teleop
+[install]
+install_scripts=$base/lib/xr_rm_teleop

xr_rm_teleop/setup.py

@@ -0,0 +1,28 @@
+from glob import glob
+from setuptools import setup
+
+package_name = "xr_rm_teleop"
+
+setup(
+    name=package_name,
+    version="0.1.0",
+    packages=[package_name],
+    data_files=[
+        ("share/ament_index/resource_index/packages", [f"resource/{package_name}"]),
+        (f"share/{package_name}", ["package.xml"]),
+        (f"share/{package_name}/config", glob("config/*.yaml")),
+    ],
+    install_requires=["setuptools"],
+    zip_safe=True,
+    maintainer="Yikai Fu",
+    maintainer_email="user@example.com",
+    description="XR relative-motion teleoperation controllers for RealMan RM75 arms.",
+    license="Apache-2.0",
+    tests_require=["pytest"],
+    entry_points={
+        "console_scripts": [
+            "gripper_trigger_bridge = xr_rm_teleop.gripper_trigger_bridge:main",
+            "single_arm_velocity_teleop = xr_rm_teleop.single_arm_velocity_teleop:main",
+        ],
+    },
+)

xr_rm_teleop/xr_rm_teleop/__init__.py

@@ -0,0 +1 @@
+

xr_rm_teleop/xr_rm_teleop/gripper_trigger_bridge.py

@@ -0,0 +1,112 @@
+from __future__ import annotations
+
+import rclpy
+from rclpy.node import Node
+from std_msgs.msg import Float32
+
+from xr_rm_interfaces.msg import XrController
+
+
+def _clamp(value: float, low: float, high: float) -> float:
+    return min(max(value, low), high)
+
+
+class GripperTriggerBridge(Node):
+    """将 PICO 扳机键映射为 OmniPicker 可订阅的归一化力控指令。"""
+
+    def __init__(self) -> None:
+        super().__init__("gripper_trigger_bridge")
+
+        self.declare_parameter("controller_topic", "/xr/right_controller")
+        self.declare_parameter("command_topic", "/omnipicker/right/force_ratio")
+        self.declare_parameter("publish_rate_hz", 30.0)
+        self.declare_parameter("stale_timeout_sec", 0.25)
+        self.declare_parameter("require_grip", True)
+        # 扳机键输入小于死区时保持开爪，避免手柄轻微误触导致夹爪闭合。
+        self.declare_parameter("trigger_deadband", 0.05)
+        self.declare_parameter("open_command", 0.0)
+        # 阶段一以保护番茄为先，闭合力控上限应从低值开始实测。
+        self.declare_parameter("min_close_command", 0.15)
+        self.declare_parameter("max_close_command", 0.45)
+        self.declare_parameter("low_pass_alpha", 0.5)
+
+        controller_topic = str(self.get_parameter("controller_topic").value)
+        command_topic = str(self.get_parameter("command_topic").value)
+        publish_rate_hz = float(self.get_parameter("publish_rate_hz").value)
+
+        self._stale_timeout_sec = float(self.get_parameter("stale_timeout_sec").value)
+        self._require_grip = bool(self.get_parameter("require_grip").value)
+        self._trigger_deadband = float(self.get_parameter("trigger_deadband").value)
+        self._open_command = float(self.get_parameter("open_command").value)
+        self._min_close_command = float(self.get_parameter("min_close_command").value)
+        self._max_close_command = float(self.get_parameter("max_close_command").value)
+        self._low_pass_alpha = float(self.get_parameter("low_pass_alpha").value)
+        self._validate_parameters()
+
+        self._last_msg: XrController | None = None
+        self._last_msg_time = None
+        self._filtered_command = self._open_command
+
+        self._publisher = self.create_publisher(Float32, command_topic, 10)
+        self.create_subscription(XrController, controller_topic, self._on_controller, 10)
+        self.create_timer(1.0 / publish_rate_hz, self._publish_command)
+
+        self.get_logger().info(
+            f"夹爪 trigger 桥接已启动：{controller_topic} -> {command_topic}"
+        )
+
+    def _on_controller(self, msg: XrController) -> None:
+        self._last_msg = msg
+        self._last_msg_time = self.get_clock().now()
+
+    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:
+                command = self._command_from_trigger(self._last_msg)
+
+        # 对夹爪指令做一阶平滑，避免扳机键抖动直接传到力控命令。
+        alpha = self._low_pass_alpha
+        self._filtered_command = alpha * command + (1.0 - alpha) * self._filtered_command
+        msg = Float32()
+        msg.data = float(self._filtered_command)
+        self._publisher.publish(msg)
+
+    def _command_from_trigger(self, msg: XrController) -> float:
+        if self._require_grip and not msg.grip:
+            return self._open_command
+
+        trigger = _clamp(float(msg.trigger), 0.0, 1.0)
+        if trigger <= self._trigger_deadband:
+            return self._open_command
+
+        ratio = (trigger - self._trigger_deadband) / (1.0 - self._trigger_deadband)
+        return self._min_close_command + ratio * (self._max_close_command - self._min_close_command)
+
+    def _validate_parameters(self) -> None:
+        if not 0.0 <= self._trigger_deadband < 1.0:
+            raise ValueError("trigger_deadband must be in [0, 1)")
+        if self._min_close_command < self._open_command:
+            raise ValueError("min_close_command must be >= open_command")
+        if self._max_close_command < self._min_close_command:
+            raise ValueError("max_close_command must be >= min_close_command")
+        if not 0.0 <= self._low_pass_alpha <= 1.0:
+            raise ValueError("low_pass_alpha must be in [0, 1]")
+
+
+def main(args: list[str] | None = None) -> None:
+    rclpy.init(args=args)
+    node = GripperTriggerBridge()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

xr_rm_teleop/xr_rm_teleop/realman_adapter.py

@@ -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

xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py

@@ -0,0 +1,243 @@
+from __future__ import annotations
+
+import math
+from typing import Iterable
+
+import rclpy
+from rclpy.node import Node
+
+from xr_rm_interfaces.msg import XrController
+
+from .realman_adapter import MockRealManAdapter, RealManAdapter
+
+
+def _norm(values: Iterable[float]) -> float:
+    return math.sqrt(sum(value * value for value in values))
+
+
+def _clamp(value: float, low: float, high: float) -> float:
+    return min(max(value, low), high)
+
+
+class SingleArmVelocityTeleop(Node):
+    """基于 XR 手柄相对位移的 RM75 单臂笛卡尔速度遥操节点。"""
+
+    def __init__(self) -> None:
+        super().__init__("single_arm_velocity_teleop")
+
+        self.declare_parameter("arm_name", "rm75")
+        self.declare_parameter("controller_topic", "/xr/right_controller")
+        self.declare_parameter("control_rate_hz", 50.0)
+        self.declare_parameter("command_timeout_sec", 0.12)
+        self.declare_parameter("scale", 1.0)
+        self.declare_parameter("kp_linear", 2.0)
+        self.declare_parameter("deadband_m", 0.002)
+        self.declare_parameter("low_pass_alpha", 0.35)
+        self.declare_parameter("max_linear_speed", 0.05)
+        self.declare_parameter("enable_position_axes", [True, True, True])
+        self.declare_parameter("workspace_min", [0.20, -0.35, 0.10])
+        self.declare_parameter("workspace_max", [0.65, 0.35, 0.60])
+        self.declare_parameter("xr_to_robot_matrix", [0.0, 0.0, -1.0, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0])
+        self.declare_parameter("use_mock", True)
+        self.declare_parameter("mock_initial_pose", [0.35, 0.0, 0.30, 0.0, 0.0, 0.0])
+        self.declare_parameter("robot_ip", "192.168.1.18")
+        self.declare_parameter("robot_port", 8080)
+        self.declare_parameter("avoid_singularity", 1)
+        self.declare_parameter("frame_type", 1)
+        self.declare_parameter("follow", False)
+
+        self._arm_name = str(self.get_parameter("arm_name").value)
+        topic = self.get_parameter("controller_topic").value
+        control_rate_hz = float(self.get_parameter("control_rate_hz").value)
+        self._dt = 1.0 / control_rate_hz
+        self._command_timeout_sec = float(self.get_parameter("command_timeout_sec").value)
+        self._scale = float(self.get_parameter("scale").value)
+        self._kp_linear = float(self.get_parameter("kp_linear").value)
+        self._deadband_m = float(self.get_parameter("deadband_m").value)
+        self._low_pass_alpha = float(self.get_parameter("low_pass_alpha").value)
+        self._max_linear_speed = float(self.get_parameter("max_linear_speed").value)
+        self._enable_position_axes = self._bool_list_parameter("enable_position_axes", 3)
+        self._workspace_min = self._float_list_parameter("workspace_min", 3)
+        self._workspace_max = self._float_list_parameter("workspace_max", 3)
+        self._xr_to_robot_matrix = self._float_list_parameter("xr_to_robot_matrix", 9)
+        self._follow = bool(self.get_parameter("follow").value)
+        self._validate_workspace()
+
+        self._last_msg: XrController | None = None
+        self._last_msg_time = None
+        self._active = False
+        self._controller_start: list[float] | None = None
+        self._robot_start: list[float] | None = None
+        self._filtered_velocity = [0.0, 0.0, 0.0]
+
+        self._adapter = self._make_adapter()
+        self._adapter.connect()
+
+        self.create_subscription(XrController, topic, self._on_controller, 10)
+        self.create_timer(self._dt, self._control_tick)
+        self.get_logger().info(f"{self._arm_name} 速度遥操节点已启动，监听话题：{topic}")
+
+    def _make_adapter(self):
+        if bool(self.get_parameter("use_mock").value):
+            return MockRealManAdapter(
+                [float(v) for v in self.get_parameter("mock_initial_pose").value],
+                self._dt,
+            )
+
+        return RealManAdapter(
+            robot_ip=self.get_parameter("robot_ip").value,
+            robot_port=int(self.get_parameter("robot_port").value),
+            dt=self._dt,
+            avoid_singularity=int(self.get_parameter("avoid_singularity").value),
+            frame_type=int(self.get_parameter("frame_type").value),
+        )
+
+    def _on_controller(self, msg: XrController) -> None:
+        self._last_msg = msg
+        self._last_msg_time = self.get_clock().now()
+
+    def _control_tick(self) -> None:
+        if self._last_msg is None or self._last_msg_time is None:
+            self._safe_stop()
+            return
+
+        age = (self.get_clock().now() - self._last_msg_time).nanoseconds * 1e-9
+        if age > self._command_timeout_sec:
+            self.get_logger().warn(f"{self._arm_name} XR 手柄数据超时，机械臂停止。", throttle_duration_sec=1.0)
+            self._safe_stop()
+            self._active = False
+            return
+
+        if not self._last_msg.grip:
+            if self._active:
+                self.get_logger().info(f"{self._arm_name} Grip 松开，退出相对位移遥操。")
+            self._safe_stop()
+            self._active = False
+            self._controller_start = None
+            self._robot_start = None
+            return
+
+        controller_now = self._controller_xyz(self._last_msg)
+        try:
+            robot_now = self._adapter.get_current_pose().xyz()
+        except Exception as exc:
+            self.get_logger().error(
+                f"{self._arm_name} 读取 TCP 位姿失败，停止输出：{exc}",
+                throttle_duration_sec=1.0,
+            )
+            self._safe_stop()
+            self._active = False
+            return
+
+        if not self._active:
+            # 握持键按下的第一帧只锁定手柄和 TCP 起点，不立即运动，避免启停跳变。
+            self._active = True
+            self._controller_start = controller_now
+            self._robot_start = robot_now
+            self._filtered_velocity = [0.0, 0.0, 0.0]
+            self.get_logger().info(f"{self._arm_name} Grip 按下，已锁定手柄和机械臂初始位姿。")
+            self._safe_stop()
+            return
+
+        assert self._controller_start is not None
+        assert self._robot_start is not None
+
+        # 核心控制：手柄相对位移 -> 机器人坐标系相对位移 -> 目标 TCP -> 速度命令。
+        controller_delta = [controller_now[i] - self._controller_start[i] for i in range(3)]
+        robot_delta = self._map_xr_delta_to_robot(controller_delta)
+        target = [self._robot_start[i] + self._scale * robot_delta[i] for i in range(3)]
+        target = self._clamp_workspace(target)
+        error = [target[i] - robot_now[i] for i in range(3)]
+        error = [error[i] if self._enable_position_axes[i] else 0.0 for i in range(3)]
+
+        if _norm(error) < self._deadband_m:
+            velocity = [0.0, 0.0, 0.0]
+        else:
+            velocity = [self._kp_linear * value for value in error]
+            velocity = self._clamp_vector_norm(velocity, self._max_linear_speed)
+
+        # 低通滤波可以削弱 XR 追踪抖动，真实机械臂测试时不要关掉。
+        alpha = self._low_pass_alpha
+        self._filtered_velocity = [
+            alpha * velocity[i] + (1.0 - alpha) * self._filtered_velocity[i]
+            for i in range(3)
+        ]
+        self._send_cartesian_velocity(self._filtered_velocity + [0.0, 0.0, 0.0])
+
+    @staticmethod
+    def _controller_xyz(msg: XrController) -> list[float]:
+        return [msg.pose.position.x, msg.pose.position.y, msg.pose.position.z]
+
+    def _map_xr_delta_to_robot(self, delta: list[float]) -> list[float]:
+        matrix = self._xr_to_robot_matrix
+        return [
+            matrix[0] * delta[0] + matrix[1] * delta[1] + matrix[2] * delta[2],
+            matrix[3] * delta[0] + matrix[4] * delta[1] + matrix[5] * delta[2],
+            matrix[6] * delta[0] + matrix[7] * delta[1] + matrix[8] * delta[2],
+        ]
+
+    def _clamp_workspace(self, target: list[float]) -> list[float]:
+        return [
+            _clamp(target[i], self._workspace_min[i], self._workspace_max[i])
+            for i in range(3)
+        ]
+
+    @staticmethod
+    def _clamp_vector_norm(vector: list[float], max_norm: float) -> list[float]:
+        norm = _norm(vector)
+        if norm <= max_norm or norm <= 1e-9:
+            return vector
+        scale = max_norm / norm
+        return [value * scale for value in vector]
+
+    def _safe_stop(self) -> None:
+        # 所有断连、松手、退出路径都走这里，确保发送零速度。
+        self._filtered_velocity = [0.0, 0.0, 0.0]
+        self._send_cartesian_velocity([0.0] * 6)
+
+    def _send_cartesian_velocity(self, velocity: list[float]) -> None:
+        try:
+            self._adapter.send_cartesian_velocity(velocity, self._follow)
+        except Exception as exc:
+            self.get_logger().error(
+                f"{self._arm_name} 发送速度命令失败：{exc}",
+                throttle_duration_sec=1.0,
+            )
+            self._active = False
+
+    def _float_list_parameter(self, name: str, expected_length: int) -> list[float]:
+        values = [float(value) for value in self.get_parameter(name).value]
+        if len(values) != expected_length:
+            raise ValueError(f"{name} must contain {expected_length} values")
+        return values
+
+    def _bool_list_parameter(self, name: str, expected_length: int) -> list[bool]:
+        values = [bool(value) for value in self.get_parameter(name).value]
+        if len(values) != expected_length:
+            raise ValueError(f"{name} must contain {expected_length} values")
+        return values
+
+    def _validate_workspace(self) -> None:
+        for axis, (low, high) in enumerate(zip(self._workspace_min, self._workspace_max)):
+            if low >= high:
+                raise ValueError(f"workspace_min[{axis}] must be smaller than workspace_max[{axis}]")
+
+    def destroy_node(self) -> bool:
+        self._adapter.close()
+        return super().destroy_node()
+
+
+def main(args: list[str] | None = None) -> None:
+    rclpy.init(args=args)
+    node = SingleArmVelocityTeleop()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()