diff --git a/.gitignore b/.gitignore index d1ec0e1..823387d 100644 --- a/.gitignore +++ b/.gitignore @@ -31,11 +31,6 @@ AMENT_IGNORE # ---> VisualStudioCode .vscode/* -!.vscode/settings.json -!.vscode/tasks.json -!.vscode/launch.json -!.vscode/extensions.json -!.vscode/*.code-snippets # Local History for Visual Studio Code .history/ diff --git a/LICENSE b/LICENSE deleted file mode 100644 index 9a22ea1..0000000 --- a/LICENSE +++ /dev/null @@ -1,18 +0,0 @@ -MIT License - -Copyright (c) 2026 YikaiFu-cart - -Permission is hereby granted, free of charge, to any person obtaining a copy of this software and -associated documentation files (the "Software"), to deal in the Software without restriction, including -without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the -following conditions: - -The above copyright notice and this permission notice shall be included in all copies or substantial -portions of the Software. - -THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT -LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO -EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER -IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE -USE OR OTHER DEALINGS IN THE SOFTWARE. diff --git a/README.md b/README.md old mode 100644 new mode 100755 index 2fa9f4d..b30af26 --- a/README.md +++ b/README.md @@ -1,2 +1,155 @@ -# acRealman_xr +# XR-RM75 双臂阶段一遥操作 ROS2 工作空间 +这个 `src/` 目录面向 **Ubuntu 22.04 + ROS2 Humble**，用于先跑通阶段一闭环： + +```text +PICO 4 Ultra 双手柄 UDP 位姿/按键 + -> /xr/left_controller 与 /xr/right_controller + -> 左右 RM75 相对位移速度遥操 + -> 右手 trigger 输出 OmniPicker 归一化力控指令 + -> PC 端查看 D405/D405/D435 视频，后续再接记录节点 +``` + +当前配置参考了最新 `acDual-arm-YikaiFuTest` 中的双臂 acRealman 参数，但本项目仍然采用 PICO 手柄相对位移遥操，不直接搬用戴盟的绝对 `PoseStamped` retarget 链路。 + +## 当前范围 + +- 已纳入：PICO 双手柄输入、左右 RM75 相对位移控制、右手 OmniPicker trigger 指令出口。 +- 暂不纳入：AUBO 移动底盘、升降柱、剪刀手、XR 视频流、自动采摘算法。 + +## 关键配置来源 + +来自 `acDual-arm-YikaiFuTest/realman_pkg/core/arg_cfg.py`： + +- 左臂 IP：`192.168.192.18` +- 右臂 IP：`192.168.192.19` +- 左右臂 TCP 端口：`8080` +- 左右臂工作空间：`x[-0.50, 0.50] y[-0.60, -0.20] z[0.10, 0.50]` +- 左右臂工作空间和 IP 沿用最新 acDual-arm 配置。 + +`dual_arm_rm75.yaml` 里的左右 `xr_to_robot_matrix` 依据论文中的 OpenXR/PICO 手柄坐标和现场双臂 base 坐标图设置： + +- PICO/OpenXR：`+X` 向右，`+Y` 向上，`+Z` 向后。 +- 左臂：`+X` 向下，`+Z` 向右，前方工作区对应 `-Y`。 +- 右臂：`+X` 向上，`+Z` 向左，前方工作区对应 `-Y`。 + +真机上仍需低速确认方向。 + +## 构建 + +在包含 `src/` 的工作空间根目录执行： + +```bash +source /opt/ros/humble/setup.bash +rosdep update +rosdep install --from-paths src -y --ignore-src +colcon build --symlink-install +source install/setup.bash +``` + +## 优先验证：双臂 mock + +mock 模式不会连接真实 RM75，适合先验证 UDP 输入、左右手柄路由、相对位移控制和 trigger 夹爪指令。 + +终端 1： + +```bash +source /opt/ros/humble/setup.bash +source install/setup.bash +ros2 launch xr_rm_bringup dual_arm_mock.launch.py +``` + +终端 2： + +```bash +source /opt/ros/humble/setup.bash +source install/setup.bash +ros2 run xr_rm_input sample_udp_sender --hand both --host 127.0.0.1 --port 15000 +``` + +检查话题： + +```bash +ros2 topic echo /xr/left_controller +ros2 topic echo /xr/right_controller +ros2 topic echo /omnipicker/right/force_ratio +``` + +## 连接双 RM75 + +先安装睿尔曼 Python API2，并确认两台控制器网络可达。 + +```bash +source /opt/ros/humble/setup.bash +source install/setup.bash +ros2 launch xr_rm_bringup dual_arm_realman.launch.py \ + left_robot_ip:=192.168.192.18 \ + right_robot_ip:=192.168.192.19 +``` + +单臂入口仍保留，默认按右臂配置： + +```bash +ros2 launch xr_rm_bringup single_arm_mock.launch.py +ros2 launch xr_rm_bringup single_arm_realman.launch.py robot_ip:=192.168.192.19 +``` + +## UDP JSON 格式 + +单个手柄数据： + +```json +{ + "hand": "right", + "grip": true, + "trigger": 0.2, + "pos": [0.12, 1.05, -0.32], + "quat": [0.0, 0.0, 0.0, 1.0] +} +``` + +也支持一次发送左右手柄： + +```json +{ + "controllers": { + "left": { + "grip": true, + "trigger": 0.0, + "pos": [-0.12, 1.05, -0.32], + "quat": [0.0, 0.0, 0.0, 1.0] + }, + "right": { + "grip": true, + "trigger": 0.4, + "pos": [0.12, 1.05, -0.32], + "quat": [0.0, 0.0, 0.0, 1.0] + } + } +} +``` + +`grip` 是机械臂运动使能：按下时锁定当前手柄和 TCP 起点，之后只跟随相对位移；松开、超时或节点退出都会发送零速度。 + +## OmniPicker 接口 + +`gripper_trigger_bridge` 会把右手 `trigger` 映射为： + +```text +/omnipicker/right/force_ratio std_msgs/Float32 +``` + +默认范围是 `0.0` 开爪、`0.15-0.45` 柔性闭合/保持。实际接 OmniPicker 驱动前，请根据番茄压伤测试收紧 `dual_arm_rm75.yaml` 里的 `max_close_command`。 + +## 真机前方向验证 + +第一轮保持 `max_linear_speed <= 0.04`，先只上右臂： + +1. 按住右手 `grip`，手柄缓慢上移，确认 TCP 方向。 +2. 手柄缓慢前推/后拉，确认 TCP 方向。 +3. 手柄缓慢左移/右移，确认 TCP 方向。 +4. 若任一方向相反，只改 `dual_arm_rm75.yaml` 中对应臂的 `xr_to_robot_matrix` 符号。 +5. 右臂确认后再开放左臂。 +6. 最后接 OmniPicker，并从低力控上限开始抓取番茄。 + +当前双臂是两个独立控制节点，没有跨臂碰撞模型；首次测试请让两臂工作区物理分离。 diff --git a/xr_rm_bringup/CMakeLists.txt b/xr_rm_bringup/CMakeLists.txt new file mode 100755 index 0000000..6fa71e8 --- /dev/null +++ b/xr_rm_bringup/CMakeLists.txt @@ -0,0 +1,12 @@ +cmake_minimum_required(VERSION 3.8) +project(xr_rm_bringup) + +find_package(ament_cmake REQUIRED) + +install(DIRECTORY + config + launch + DESTINATION share/${PROJECT_NAME} +) + +ament_package() diff --git a/xr_rm_bringup/config/dual_arm_rm75.yaml b/xr_rm_bringup/config/dual_arm_rm75.yaml new file mode 100755 index 0000000..e699765 --- /dev/null +++ b/xr_rm_bringup/config/dual_arm_rm75.yaml @@ -0,0 +1,94 @@ +# 阶段一：PICO 遥操作双 RM75 平台配置。 +# +# 参数来源：acDual-arm-YikaiFuTest/realman_pkg/core/arg_cfg.py +# - 左臂 IP：192.168.192.18 +# - 右臂 IP：192.168.192.19 +# - 左右臂工作空间和 IP 沿用最新 acDual-arm 配置。 +# - PICO 手柄位移映射根据论文 OpenXR 坐标和现场双臂 base 坐标图单独设置。 +# +# 注意：当前项目仍然采用“手柄相对位移”控制方式。 +# 按下握持键时锁定当前手柄位姿和 TCP 位姿，之后只跟随相对位移。 +# acDual-arm 项目使用的是戴盟绝对 PoseStamped 重映射，因此这里只迁移 +# 坐标标定和安全参数，不迁移其绝对位姿控制链路。 + +left_arm_teleop: + ros__parameters: + arm_name: left_rm75 + controller_topic: /xr/left_controller + control_rate_hz: 50.0 + command_timeout_sec: 0.12 + + # 实验台番茄测试先采用保守参数；完成三轴方向检查和实体急停测试后再提高速度。 + scale: 0.75 + kp_linear: 1.8 + deadband_m: 0.002 + low_pass_alpha: 0.35 + max_linear_speed: 0.04 + enable_position_axes: [true, true, true] + + # 来自 acDual-arm 的 bounds_p[left]： + # x[-0.50, 0.50]，y[-0.60, -0.20]，z[0.10, 0.50]。 + workspace_min: [-0.50, -0.60, 0.10] + workspace_max: [0.50, -0.20, 0.50] + + # PICO/OpenXR 位置坐标：+X 向右，+Y 向上，+Z 向后。 + # 左臂 base 坐标：+X 向下，+Z 向右，前方工作区对应 -Y。 + # 映射关系：机器人位移增量 = [-手柄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: false + mock_initial_pose: [-0.1560, -0.3476, 0.2689, -3.1100, -0.2840, -1.3180] + robot_ip: 192.168.192.18 + robot_port: 8080 + avoid_singularity: 1 + frame_type: 1 + follow: false + +right_arm_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: 1.8 + deadband_m: 0.002 + low_pass_alpha: 0.35 + max_linear_speed: 0.04 + enable_position_axes: [true, true, true] + + # 来自 acDual-arm 的 bounds_p[right]。 + workspace_min: [-0.50, -0.60, 0.10] + workspace_max: [0.50, -0.20, 0.50] + + # PICO/OpenXR 位置坐标：+X 向右，+Y 向上，+Z 向后。 + # 右臂 base 坐标：+X 向上，+Z 向左，前方工作区对应 -Y。 + # 映射关系：机器人位移增量 = [手柄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: false + 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 + +right_omnipicker_trigger_bridge: + ros__parameters: + # 阶段一只使用右手两指夹爪；右手扳机键输出归一化力控指令。 + controller_topic: /xr/right_controller + command_topic: /omnipicker/right/force_ratio + publish_rate_hz: 30.0 + stale_timeout_sec: 0.25 + require_grip: true + trigger_deadband: 0.05 + open_command: 0.0 + min_close_command: 0.15 + max_close_command: 0.45 + low_pass_alpha: 0.5 diff --git a/xr_rm_bringup/config/single_arm_rm75.yaml b/xr_rm_bringup/config/single_arm_rm75.yaml new file mode 100755 index 0000000..99efd0c --- /dev/null +++ b/xr_rm_bringup/config/single_arm_rm75.yaml @@ -0,0 +1,33 @@ +# 单臂真机 RM75 入口配置。 +# 默认按最新 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: false + 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 diff --git a/xr_rm_bringup/launch/dual_arm_mock.launch.py b/xr_rm_bringup/launch/dual_arm_mock.launch.py new file mode 100755 index 0000000..8816064 --- /dev/null +++ b/xr_rm_bringup/launch/dual_arm_mock.launch.py @@ -0,0 +1,55 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description() -> LaunchDescription: + config_file = PathJoinSubstitution([ + FindPackageShare("xr_rm_bringup"), + "config", + "dual_arm_rm75.yaml", + ]) + + return LaunchDescription([ + DeclareLaunchArgument("udp_host", default_value="0.0.0.0"), + DeclareLaunchArgument("udp_port", default_value="15000"), + # 接收 PICO/XR 侧 UDP 数据，并按左右手字段分发到对应手柄话题。 + Node( + package="xr_rm_input", + executable="udp_controller_receiver", + name="udp_controller_receiver", + output="screen", + parameters=[{ + "udp_host": LaunchConfiguration("udp_host"), + "udp_port": LaunchConfiguration("udp_port"), + "left_topic": "/xr/left_controller", + "right_topic": "/xr/right_controller", + }], + ), + # 左臂模拟节点：不连接真实 RM75，只积分速度命令用于验证坐标方向。 + Node( + package="xr_rm_teleop", + executable="single_arm_velocity_teleop", + name="left_arm_teleop", + output="screen", + parameters=[config_file, {"use_mock": True}], + ), + # 右臂模拟节点：与左臂共用同一配置文件，但读取右手柄话题。 + Node( + package="xr_rm_teleop", + executable="single_arm_velocity_teleop", + name="right_arm_teleop", + output="screen", + parameters=[config_file, {"use_mock": True}], + ), + # 右手扳机键到 OmniPicker 归一化力控指令的桥接，模拟阶段用于检查话题输出。 + Node( + package="xr_rm_teleop", + executable="gripper_trigger_bridge", + name="right_omnipicker_trigger_bridge", + output="screen", + parameters=[config_file], + ), + ]) diff --git a/xr_rm_bringup/launch/dual_arm_realman.launch.py b/xr_rm_bringup/launch/dual_arm_realman.launch.py new file mode 100755 index 0000000..6fe4549 --- /dev/null +++ b/xr_rm_bringup/launch/dual_arm_realman.launch.py @@ -0,0 +1,69 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description() -> LaunchDescription: + config_file = PathJoinSubstitution([ + FindPackageShare("xr_rm_bringup"), + "config", + LaunchConfiguration("robot_config"), + ]) + + return LaunchDescription([ + DeclareLaunchArgument("udp_host", default_value="0.0.0.0"), + DeclareLaunchArgument("udp_port", default_value="15000"), + DeclareLaunchArgument("left_robot_ip", default_value="192.168.192.18"), + DeclareLaunchArgument("right_robot_ip", default_value="192.168.192.19"), + DeclareLaunchArgument("robot_port", default_value="8080"), + DeclareLaunchArgument("robot_config", default_value="dual_arm_rm75.yaml"), + Node( + package="xr_rm_input", + executable="udp_controller_receiver", + name="udp_controller_receiver", + output="screen", + parameters=[{ + "udp_host": LaunchConfiguration("udp_host"), + "udp_port": LaunchConfiguration("udp_port"), + "left_topic": "/xr/left_controller", + "right_topic": "/xr/right_controller", + }], + ), + Node( + package="xr_rm_teleop", + executable="single_arm_velocity_teleop", + name="left_arm_teleop", + output="screen", + parameters=[ + config_file, + { + "use_mock": False, + "robot_ip": LaunchConfiguration("left_robot_ip"), + "robot_port": LaunchConfiguration("robot_port"), + }, + ], + ), + Node( + package="xr_rm_teleop", + executable="single_arm_velocity_teleop", + name="right_arm_teleop", + output="screen", + parameters=[ + config_file, + { + "use_mock": False, + "robot_ip": LaunchConfiguration("right_robot_ip"), + "robot_port": LaunchConfiguration("robot_port"), + }, + ], + ), + Node( + package="xr_rm_teleop", + executable="gripper_trigger_bridge", + name="right_omnipicker_trigger_bridge", + output="screen", + parameters=[config_file], + ), + ]) diff --git a/xr_rm_bringup/launch/single_arm_mock.launch.py b/xr_rm_bringup/launch/single_arm_mock.launch.py new file mode 100755 index 0000000..eee3ec6 --- /dev/null +++ b/xr_rm_bringup/launch/single_arm_mock.launch.py @@ -0,0 +1,36 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description() -> LaunchDescription: + config_file = PathJoinSubstitution([ + FindPackageShare("xr_rm_teleop"), + "config", + "single_arm.yaml", + ]) + + return LaunchDescription([ + DeclareLaunchArgument("udp_host", default_value="0.0.0.0"), + DeclareLaunchArgument("udp_port", default_value="15000"), + Node( + package="xr_rm_input", + executable="udp_controller_receiver", + name="udp_controller_receiver", + output="screen", + parameters=[{ + "udp_host": LaunchConfiguration("udp_host"), + "udp_port": LaunchConfiguration("udp_port"), + "topic": "/xr/right_controller", + }], + ), + Node( + package="xr_rm_teleop", + executable="single_arm_velocity_teleop", + name="single_arm_velocity_teleop", + output="screen", + parameters=[config_file, {"use_mock": True}], + ), + ]) diff --git a/xr_rm_bringup/launch/single_arm_realman.launch.py b/xr_rm_bringup/launch/single_arm_realman.launch.py new file mode 100755 index 0000000..28a416d --- /dev/null +++ b/xr_rm_bringup/launch/single_arm_realman.launch.py @@ -0,0 +1,46 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description() -> LaunchDescription: + config_file = PathJoinSubstitution([ + FindPackageShare("xr_rm_bringup"), + "config", + LaunchConfiguration("robot_config"), + ]) + + return LaunchDescription([ + DeclareLaunchArgument("udp_host", default_value="0.0.0.0"), + DeclareLaunchArgument("udp_port", default_value="15000"), + DeclareLaunchArgument("robot_ip", default_value="192.168.192.19"), + DeclareLaunchArgument("robot_port", default_value="8080"), + DeclareLaunchArgument("robot_config", default_value="single_arm_rm75.yaml"), + Node( + package="xr_rm_input", + executable="udp_controller_receiver", + name="udp_controller_receiver", + output="screen", + parameters=[{ + "udp_host": LaunchConfiguration("udp_host"), + "udp_port": LaunchConfiguration("udp_port"), + "topic": "/xr/right_controller", + }], + ), + Node( + package="xr_rm_teleop", + executable="single_arm_velocity_teleop", + name="single_arm_velocity_teleop", + output="screen", + parameters=[ + config_file, + { + "use_mock": False, + "robot_ip": LaunchConfiguration("robot_ip"), + "robot_port": LaunchConfiguration("robot_port"), + }, + ], + ), + ]) diff --git a/xr_rm_bringup/package.xml b/xr_rm_bringup/package.xml new file mode 100755 index 0000000..701b0db --- /dev/null +++ b/xr_rm_bringup/package.xml @@ -0,0 +1,18 @@ + + + + xr_rm_bringup + 0.1.0 + Launch and configuration package for XR RealMan teleoperation. + Yikai Fu + Apache-2.0 + + ament_cmake + + xr_rm_input + xr_rm_teleop + + + ament_cmake + + diff --git a/xr_rm_input/launch/udp_receiver.launch.py b/xr_rm_input/launch/udp_receiver.launch.py new file mode 100755 index 0000000..b1a85b5 --- /dev/null +++ b/xr_rm_input/launch/udp_receiver.launch.py @@ -0,0 +1,23 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description() -> LaunchDescription: + return LaunchDescription([ + DeclareLaunchArgument("udp_host", default_value="0.0.0.0"), + DeclareLaunchArgument("udp_port", default_value="15000"), + DeclareLaunchArgument("topic", default_value="/xr/right_controller"), + Node( + package="xr_rm_input", + executable="udp_controller_receiver", + name="udp_controller_receiver", + output="screen", + parameters=[{ + "udp_host": LaunchConfiguration("udp_host"), + "udp_port": LaunchConfiguration("udp_port"), + "topic": LaunchConfiguration("topic"), + }], + ), + ]) diff --git a/xr_rm_input/package.xml b/xr_rm_input/package.xml new file mode 100755 index 0000000..9efde9c --- /dev/null +++ b/xr_rm_input/package.xml @@ -0,0 +1,23 @@ + + + + xr_rm_input + 0.1.0 + Receives XR controller pose packets and publishes left/right ROS2 controller messages. + Yikai Fu + Apache-2.0 + + ament_python + + geometry_msgs + rclpy + std_msgs + xr_rm_interfaces + + ament_lint_auto + ament_lint_common + + + ament_python + + diff --git a/xr_rm_input/resource/xr_rm_input b/xr_rm_input/resource/xr_rm_input new file mode 100755 index 0000000..8b13789 --- /dev/null +++ b/xr_rm_input/resource/xr_rm_input @@ -0,0 +1 @@ + diff --git a/xr_rm_input/setup.cfg b/xr_rm_input/setup.cfg new file mode 100755 index 0000000..c347a30 --- /dev/null +++ b/xr_rm_input/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/xr_rm_input +[install] +install_scripts=$base/lib/xr_rm_input diff --git a/xr_rm_input/setup.py b/xr_rm_input/setup.py new file mode 100755 index 0000000..13b0df3 --- /dev/null +++ b/xr_rm_input/setup.py @@ -0,0 +1,28 @@ +from glob import glob +from setuptools import setup + +package_name = "xr_rm_input" + +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}/launch", glob("launch/*.launch.py")), + ], + install_requires=["setuptools"], + zip_safe=True, + maintainer="Yikai Fu", + maintainer_email="user@example.com", + description="Receives XR controller pose packets and publishes left/right ROS2 messages.", + license="Apache-2.0", + tests_require=["pytest"], + entry_points={ + "console_scripts": [ + "udp_controller_receiver = xr_rm_input.udp_controller_receiver:main", + "sample_udp_sender = xr_rm_input.sample_udp_sender:main", + ], + }, +) diff --git a/xr_rm_input/xr_rm_input/__init__.py b/xr_rm_input/xr_rm_input/__init__.py new file mode 100755 index 0000000..8b13789 --- /dev/null +++ b/xr_rm_input/xr_rm_input/__init__.py @@ -0,0 +1 @@ + diff --git a/xr_rm_input/xr_rm_input/sample_udp_sender.py b/xr_rm_input/xr_rm_input/sample_udp_sender.py new file mode 100755 index 0000000..8419079 --- /dev/null +++ b/xr_rm_input/xr_rm_input/sample_udp_sender.py @@ -0,0 +1,58 @@ +import argparse +import json +import math +import socket +import time + + +def main() -> None: + parser = argparse.ArgumentParser(description="通过 UDP 发送一段模拟 XR 左/右手柄数据。") + parser.add_argument("--host", default="127.0.0.1") + parser.add_argument("--port", type=int, default=15000) + parser.add_argument("--seconds", type=float, default=5.0) + parser.add_argument("--hz", type=float, default=60.0) + parser.add_argument("--amplitude", type=float, default=0.04) + parser.add_argument("--hand", choices=("left", "right", "both"), default="right") + args = parser.parse_args() + + sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + dt = 1.0 / args.hz + t0 = time.monotonic() + + hands = ("left", "right") if args.hand == "both" else (args.hand,) + packets = { + hand: { + "t": 0.0, + "hand": hand, + "grip": False, + "trigger": 0.0, + "pos": [0.0, 1.0, 0.0], + "quat": [0.0, 0.0, 0.0, 1.0], + } + for hand in hands + } + + while time.monotonic() - t0 < args.seconds: + t = time.monotonic() - t0 + # 模拟按住握持键后缓慢移动手柄，用来测试相对位移控制链路。 + for index, packet in enumerate(packets.values()): + phase = index * math.pi + packet["t"] = t + packet["grip"] = True + packet["trigger"] = max(0.0, math.sin(2.0 * math.pi * 0.1 * t)) + packet["pos"] = [ + args.amplitude * math.sin(2.0 * math.pi * 0.2 * t + phase), + 1.0, + 0.0, + ] + sock.sendto(json.dumps(packet).encode("utf-8"), (args.host, args.port)) + time.sleep(dt) + + for packet in packets.values(): + packet["grip"] = False + packet["trigger"] = 0.0 + sock.sendto(json.dumps(packet).encode("utf-8"), (args.host, args.port)) + + +if __name__ == "__main__": + main() diff --git a/xr_rm_input/xr_rm_input/udp_controller_receiver.py b/xr_rm_input/xr_rm_input/udp_controller_receiver.py new file mode 100755 index 0000000..df677c0 --- /dev/null +++ b/xr_rm_input/xr_rm_input/udp_controller_receiver.py @@ -0,0 +1,215 @@ +import json +import socket +from collections.abc import Iterable, Mapping +from typing import Any + +import rclpy +from rclpy.node import Node + +from xr_rm_interfaces.msg import XrController + + +class UdpControllerReceiver(Node): + """将轻量级 XR UDP 数据包转换成 ROS2 左/右手柄消息。""" + + def __init__(self) -> None: + super().__init__("udp_controller_receiver") + + self.declare_parameter("udp_host", "0.0.0.0") + self.declare_parameter("udp_port", 15000) + self.declare_parameter("topic", "/xr/right_controller") + self.declare_parameter("left_topic", "/xr/left_controller") + self.declare_parameter("right_topic", "") + self.declare_parameter("default_hand", "right") + self.declare_parameter("timer_hz", 100.0) + self.declare_parameter("quat_order", "xyzw") + + udp_host = self.get_parameter("udp_host").value + udp_port = int(self.get_parameter("udp_port").value) + legacy_topic = str(self.get_parameter("topic").value) + left_topic = str(self.get_parameter("left_topic").value) + right_topic = str(self.get_parameter("right_topic").value) or legacy_topic + timer_hz = float(self.get_parameter("timer_hz").value) + + self._default_hand = self._normalize_hand( + self.get_parameter("default_hand").value, + fallback="right", + ) + self._quat_order = str(self.get_parameter("quat_order").value).lower() + + # UDP 接收使用非阻塞模式，避免没有手柄数据时卡住 ROS2 定时器。 + self._publishers = { + "left": self.create_publisher(XrController, left_topic, 10), + "right": self.create_publisher(XrController, right_topic, 10), + } + self._socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + self._socket.bind((udp_host, udp_port)) + self._socket.setblocking(False) + + self.create_timer(1.0 / timer_hz, self._poll_socket) + self.get_logger().info( + "正在监听 XR UDP 数据：" + f"{udp_host}:{udp_port} -> left={left_topic}, right={right_topic}" + ) + + def _poll_socket(self) -> None: + while True: + try: + data, _ = self._socket.recvfrom(4096) + except BlockingIOError: + return + + try: + payload = json.loads(data.decode("utf-8")) + controller_payloads = list(self._iter_controller_payloads(payload)) + except (UnicodeDecodeError, json.JSONDecodeError, KeyError, TypeError, ValueError) as exc: + self.get_logger().warn(f"XR 数据包格式错误：{exc}", throttle_duration_sec=1.0) + continue + + for hand, controller_payload in controller_payloads: + try: + msg = self._payload_to_msg(controller_payload, hand) + except (KeyError, TypeError, ValueError) as exc: + self.get_logger().warn(f"XR 手柄字段错误：{exc}", throttle_duration_sec=1.0) + continue + self._publishers[msg.hand].publish(msg) + + def _iter_controller_payloads(self, payload: Any) -> Iterable[tuple[str, Mapping[str, Any]]]: + if isinstance(payload, list): + for item in payload: + yield from self._iter_controller_payloads(item) + return + + if not isinstance(payload, Mapping): + raise TypeError("expected JSON object or array") + + if self._contains_pose(payload): + hand = self._normalize_hand(payload.get("hand", self._default_hand)) + yield hand, payload + return + + controllers = payload.get("controllers", payload.get("controller")) + if isinstance(controllers, Mapping): + if self._contains_pose(controllers): + hand = self._normalize_hand(controllers.get("hand", self._default_hand)) + yield hand, controllers + return + for hand_key, controller_payload in controllers.items(): + if isinstance(controller_payload, Mapping): + hand = self._normalize_hand(controller_payload.get("hand", hand_key)) + yield hand, controller_payload + return + + if isinstance(controllers, list): + for controller_payload in controllers: + yield from self._iter_controller_payloads(controller_payload) + return + + for hand_key in ("left", "right", "left_controller", "right_controller"): + controller_payload = payload.get(hand_key) + if isinstance(controller_payload, Mapping): + hand = self._normalize_hand(controller_payload.get("hand", hand_key)) + yield hand, controller_payload + + def _payload_to_msg(self, payload: Mapping[str, Any], hand: str) -> XrController: + pos, quat = self._extract_pose(payload) + if len(pos) != 3 or len(quat) != 4: + raise ValueError("expected pos[3] and quat[4]") + + msg = XrController() + msg.header.stamp = self.get_clock().now().to_msg() + msg.header.frame_id = str(payload.get("frame_id", "xr_world")) + msg.hand = hand + msg.grip = self._as_bool(payload.get("grip", payload.get("grip_button", False))) + msg.trigger = self._clamp_float(payload.get("trigger", 0.0), 0.0, 1.0) + msg.pose.position.x = float(pos[0]) + msg.pose.position.y = float(pos[1]) + msg.pose.position.z = float(pos[2]) + + # Unity/OpenXR 项目里的四元数顺序可能不同，这里用参数统一转换成 ROS 的 xyzw。 + if self._quat_order == "wxyz": + w, x, y, z = quat + else: + x, y, z, w = quat + msg.pose.orientation.x = float(x) + msg.pose.orientation.y = float(y) + msg.pose.orientation.z = float(z) + msg.pose.orientation.w = float(w) + return msg + + @staticmethod + def _contains_pose(payload: Mapping[str, Any]) -> bool: + return any(key in payload for key in ("pos", "position", "p", "pose")) + + def _extract_pose(self, payload: Mapping[str, Any]) -> tuple[list[float], list[float]]: + pose = payload.get("pose") + if isinstance(pose, Mapping): + pos = pose.get("position", pose.get("pos", pose.get("p"))) + quat = pose.get("orientation", pose.get("quat", pose.get("q", [0.0, 0.0, 0.0, 1.0]))) + elif isinstance(pose, list) and len(pose) >= 7: + pos = pose[:3] + quat = pose[3:7] + else: + pos = payload.get("pos", payload.get("position", payload.get("p"))) + quat = payload.get("quat", payload.get("orientation", payload.get("q", [0.0, 0.0, 0.0, 1.0]))) + + if pos is None: + raise KeyError("missing pos/position/p") + return self._vector3(pos), self._quaternion(quat) + + @staticmethod + def _vector3(value: Any) -> list[float]: + if isinstance(value, Mapping): + return [float(value[key]) for key in ("x", "y", "z")] + vector = list(value) + if len(vector) != 3: + raise ValueError("expected 3D position") + return [float(item) for item in vector] + + def _quaternion(self, value: Any) -> list[float]: + if isinstance(value, Mapping): + if self._quat_order == "wxyz": + return [float(value[key]) for key in ("w", "x", "y", "z")] + return [float(value[key]) for key in ("x", "y", "z", "w")] + quat = list(value) + if len(quat) != 4: + raise ValueError("expected quaternion") + return [float(item) for item in quat] + + def _normalize_hand(self, hand: Any, fallback: str | None = None) -> str: + value = str(hand).lower() + if value in ("left", "l", "left_controller"): + return "left" + if value in ("right", "r", "right_controller"): + return "right" + fallback_hand = fallback or self._default_hand + self.get_logger().warn( + f"未知手柄标识 {hand!r}，使用默认 {fallback_hand}", + throttle_duration_sec=1.0, + ) + return fallback_hand + + @staticmethod + def _as_bool(value: Any) -> bool: + if isinstance(value, str): + return value.lower() in ("1", "true", "yes", "on") + return bool(value) + + @staticmethod + def _clamp_float(value: Any, low: float, high: float) -> float: + number = float(value) + return min(max(number, low), high) + + +def main(args: list[str] | None = None) -> None: + rclpy.init(args=args) + node = UdpControllerReceiver() + try: + rclpy.spin(node) + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/xr_rm_interfaces/CMakeLists.txt b/xr_rm_interfaces/CMakeLists.txt new file mode 100755 index 0000000..3f70846 --- /dev/null +++ b/xr_rm_interfaces/CMakeLists.txt @@ -0,0 +1,19 @@ +cmake_minimum_required(VERSION 3.8) +project(xr_rm_interfaces) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(std_msgs REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/XrController.msg" + DEPENDENCIES geometry_msgs std_msgs +) + +ament_export_dependencies(rosidl_default_runtime) +ament_package() diff --git a/xr_rm_interfaces/msg/XrController.msg b/xr_rm_interfaces/msg/XrController.msg new file mode 100755 index 0000000..bb9d6fd --- /dev/null +++ b/xr_rm_interfaces/msg/XrController.msg @@ -0,0 +1,5 @@ +std_msgs/Header header +string hand +bool grip +float32 trigger +geometry_msgs/Pose pose diff --git a/xr_rm_interfaces/package.xml b/xr_rm_interfaces/package.xml new file mode 100755 index 0000000..0d2d77b --- /dev/null +++ b/xr_rm_interfaces/package.xml @@ -0,0 +1,24 @@ + + + + xr_rm_interfaces + 0.1.0 + ROS2 interfaces for XR controller based RealMan teleoperation. + Yikai Fu + Apache-2.0 + + ament_cmake + geometry_msgs + rosidl_default_generators + std_msgs + + geometry_msgs + rosidl_default_runtime + std_msgs + + rosidl_interface_packages + + + ament_cmake + + diff --git a/xr_rm_teleop/config/single_arm.yaml b/xr_rm_teleop/config/single_arm.yaml new file mode 100755 index 0000000..c9720cb --- /dev/null +++ b/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 diff --git a/xr_rm_teleop/package.xml b/xr_rm_teleop/package.xml new file mode 100755 index 0000000..ef25793 --- /dev/null +++ b/xr_rm_teleop/package.xml @@ -0,0 +1,23 @@ + + + + xr_rm_teleop + 0.1.0 + XR relative-motion teleoperation controllers for RealMan RM75 arms. + Yikai Fu + Apache-2.0 + + ament_python + + geometry_msgs + rclpy + std_msgs + xr_rm_interfaces + + ament_lint_auto + ament_lint_common + + + ament_python + + diff --git a/xr_rm_teleop/resource/xr_rm_teleop b/xr_rm_teleop/resource/xr_rm_teleop new file mode 100755 index 0000000..8b13789 --- /dev/null +++ b/xr_rm_teleop/resource/xr_rm_teleop @@ -0,0 +1 @@ + diff --git a/xr_rm_teleop/setup.cfg b/xr_rm_teleop/setup.cfg new file mode 100755 index 0000000..12d13a8 --- /dev/null +++ b/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 diff --git a/xr_rm_teleop/setup.py b/xr_rm_teleop/setup.py new file mode 100755 index 0000000..d0fe314 --- /dev/null +++ b/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", + ], + }, +) diff --git a/xr_rm_teleop/xr_rm_teleop/__init__.py b/xr_rm_teleop/xr_rm_teleop/__init__.py new file mode 100755 index 0000000..8b13789 --- /dev/null +++ b/xr_rm_teleop/xr_rm_teleop/__init__.py @@ -0,0 +1 @@ + diff --git a/xr_rm_teleop/xr_rm_teleop/gripper_trigger_bridge.py b/xr_rm_teleop/xr_rm_teleop/gripper_trigger_bridge.py new file mode 100755 index 0000000..46d6e5b --- /dev/null +++ b/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() diff --git a/xr_rm_teleop/xr_rm_teleop/realman_adapter.py b/xr_rm_teleop/xr_rm_teleop/realman_adapter.py new file mode 100755 index 0000000..b4ee73d --- /dev/null +++ b/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 diff --git a/xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py b/xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py new file mode 100755 index 0000000..608cb52 --- /dev/null +++ b/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()