ZhengLiu-cart/acRealman_xr_from_Yikai
1
0
Fork 0
forked from YikaiFu-cart/acRealman_xr
Code Pull Requests Activity

Refactor and enhance XR-RM teleoperation functionality

Browse Source
This commit is contained in:
YikaiFu-cart
2026-05-31 20:06:07 +08:00
parent 3f48468f63
commit 948d50cab4
13 changed files with 409 additions and 354 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
CODEX.md
View File

@ -12,10 +12,10 @@ PICO/XR UDP JSON
-> /xr/left_controller 与 /xr/right_controller
-> xr_rm_teleop/single_arm_velocity_teleop
-> MockRealManAdapter 或 RealManAdapter
-> /xr_rm/<arm_name>/current_pose、target_pose、cmd_vel
-> /xr_rm/<arm_name>/current_pose、raw_target_pose、target_pose、cmd_vel、target_clamped
```
核心控制方式是相对位移遥操作：`grip=true` 的第一帧锁定手柄起点和机器人 TCP 起点，之后用手柄位移增量生成目标 TCP；`grip=false`、UDP 超时、异常或节点退出必须停止。
核心控制方式是相对位姿透传遥操作：`grip=true` 的第一帧锁定手柄起点和机器人 TCP 起点，之后用手柄位移增量生成目标 TCP，并通过 `rm_movep_canfd` 下发位姿目标；`grip=false`、UDP 超时、异常或节点退出必须停止。
## 固定工作流
@ -96,6 +96,8 @@ 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 /xr_rm/left_rm75/target_pose
ros2 topic echo /xr_rm/right_rm75/target_pose
ros2 topic echo /xr_rm/left_rm75/cmd_vel
ros2 topic echo /xr_rm/right_rm75/cmd_vel
```
@ -165,7 +167,7 @@ git diff --check
- `xr_rm_interfaces`：定义 `XrController`。
- `xr_rm_input`：接收 UDP controller JSON，发布左右手柄话题，并提供 `sample_udp_sender`。
- `xr_rm_teleop`：把手柄相对位移映射成 RM75 笛卡尔速度命令。
- `xr_rm_teleop`：把手柄相对位移映射成 RM75 笛卡尔位姿透传目标。
- `xr_rm_bringup`：维护 launch、YAML、现场 UI 和运行入口。
- `unity/XR_RM_PICO_UDP_Sender`：PICO 4 Ultra UDP Sender Unity 工程。

29
README.md
View File

@ -7,11 +7,11 @@ PICO/XR 双手柄 UDP JSON
-> xr_rm_input/udp_controller_receiver
-> /xr/left_controller 与 /xr/right_controller
-> xr_rm_teleop/single_arm_velocity_teleop
-> 左右 RM75 笛卡尔相对位移速度控制
-> /xr_rm/<arm_name>/current_pose、target_pose、cmd_vel 调试话题
-> 左右 RM75 笛卡尔相对位姿透传控制
-> /xr_rm/<arm_name>/current_pose、raw_target_pose、target_pose、cmd_vel、target_clamped 调试话题
```
当前控制方式是“手柄相对位移”遥操作：按住 `grip` 时锁定当前手柄位置和机械臂 TCP 位置，之后根据手柄相对位移生成目标 TCP，再用速度命令跟随。松开 `grip`、UDP 超时或节点退出时会发送零速度。
当前控制方式是“手柄相对位姿透传”遥操作：按住 `grip` 时锁定当前手柄位置和机械臂 TCP 位置，之后根据手柄相对位移生成目标 TCP，经过工作空间限幅、目标低通和单帧步长限制后，通过 `rm_movep_canfd` 下发目标位姿。松开 `grip`、UDP 超时或节点退出时会请求机械臂慢停。
## 当前范围
@ -19,7 +19,7 @@ PICO/XR 双手柄 UDP JSON
- PICO/XR 手柄 UDP 数据接收，并分发到左右手柄 ROS2 话题。
- 通过统一的 `arm_debug.launch.py` 支持左臂、右臂、双臂的 mock 调试和真机调试。
- RM75 真机连接适配，包含速度透传初始化、安全速度/加速度配置、可选初始化点位移动。
- RM75 真机连接适配，包含 `rm_movep_canfd` 位姿透传、安全速度/加速度配置、可选初始化点位移动。
- Tkinter 启动面板 `launcher_ui.py`，用于现场快速启动、监控 topic、检查环境和清理进程。
- 自定义 PICO 4 Ultra UDP Sender Unity 工程，负责发送左右手柄 pose、`grip` 和 `trigger`。
@ -126,7 +126,7 @@ ros2 run xr_rm_bringup launcher_ui
每个模式都会附带三个监控入口：
- `Open Controller Topic Monitor`：同时查看 `/xr/left_controller` 和 `/xr/right_controller`。
- `Open Cmd Vel Topic Monitor`：同时查看 `/xr_rm/left_rm75/cmd_vel` 和 `/xr_rm/right_rm75/cmd_vel`。
- `Open Target Velocity Monitor`：同时查看 `/xr_rm/left_rm75/cmd_vel` 和 `/xr_rm/right_rm75/cmd_vel`；该话题表示目标位姿变化率，仅用于调试。
- `Open ROS Topic/Node List Monitor`：每秒刷新 `ros2 topic list` 和 `ros2 node list`。
分屏监控依赖 `x-terminal-emulator` 指向 Terminator。若提示不支持，可安装并切换：
@ -156,6 +156,8 @@ 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 /xr_rm/left_rm75/target_pose
ros2 topic echo /xr_rm/right_rm75/target_pose
ros2 topic echo /xr_rm/left_rm75/cmd_vel
ros2 topic echo /xr_rm/right_rm75/cmd_vel
```
@ -194,9 +196,12 @@ ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false \
- `use_mock`：`true` 不连接真机，`false` 连接 RM75。
- `udp_host`：UDP 监听地址，默认 `0.0.0.0`。
- `udp_port`：UDP 监听端口，默认 `15000`。
- `udp_timer_hz`：UDP receiver 轮询频率，默认 `200.0`。
- `left_robot_ip`：左臂 IP，默认 `192.168.192.18`。
- `right_robot_ip`：右臂 IP，默认 `192.168.192.19`。
- `robot_port`：RM75 TCP 端口，默认 `8080`。
- `control_rate_hz`：`rm_movep_canfd` 目标位姿发送频率，默认 `90.0`。
- `follow`：传给 `rm_movep_canfd` 的跟随标志，默认 `false`。
- `enable_tool_control`：是否在遥操作节点内启用末端工具控制 topic，默认 `true`。
- `configure_peripheral_on_connect`：遥操作节点连接真机后是否配置末端外设，默认 `true`；工具控制会复用同一个 RealMan 连接，避免两个进程同时抢占同一机械臂。
- `move_to_initial_pose_on_connect`：连接后是否执行 `movej`/`movel` 初始化，默认 `false`。
@ -216,11 +221,13 @@ ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false \
- `controller_topic`：订阅的手柄话题。
- `scale`：手柄位移到 TCP 位移的比例。
- `kp_linear`：位置误差到速度命令的比例增益。
- `max_linear_speed`：软件侧最大线速度。
- `target_filter_alpha` / `target_filter_alpha_fast`：目标 TCP 低通滤波系数，快速移动时自动使用更大的系数。
- `target_filter_fast_threshold_m`：进入快速滤波区间的目标变化阈值。
- `max_linear_speed`：目标位姿单帧步长限制对应的最大线速度。
- `workspace_min` / `workspace_max`：笛卡尔工作空间边界。
- `cyl_radius_limit`：基座圆柱半径限制。
- `xr_to_robot_matrix`：PICO/OpenXR 位移到 RM75 base 坐标的映射矩阵。
- `current_pose_poll_hz`：低频读取真机当前 TCP 的频率；控制中不再每帧阻塞读取状态。
- `mock_initial_pose`：mock 模式初始 TCP 位姿。
- `initial_joint_pose` / `initial_tcp_pose`：可选真机初始化点。
@ -300,7 +307,7 @@ PICO 4 Ultra 在 Ubuntu 22.04 下配置 Unity、构建 APK、安装到头显并
3. 单臂启动，`move_to_initial_pose_on_connect:=false`。
4. 手握急停，按住 `grip` 后只做小幅单轴移动。
5. 逐个确认上/下、前/后、左/右方向。
6. 确认松开 `grip` 后 `/xr_rm/<arm>/cmd_vel` 归零。
6. 确认松开 `grip` 后机械臂慢停，`/xr_rm/<arm>/cmd_vel` 回到零。
7. 左右臂都确认后，再进入双臂模式。
当前项目没有双臂碰撞检测。双臂首次联调时，请让两个工作区在物理上分开，低速验证，不要让两臂末端互相靠近。
@ -309,7 +316,7 @@ PICO 4 Ultra 在 Ubuntu 22.04 下配置 Unity、构建 APK、安装到头显并
为了达到“稳定可用的双臂 XR 遥操作/采摘平台”，建议按下面顺序推进：
1. 稳定 PICO 数据链路：固定 UDP JSON 协议和坐标系，增加频率、延迟、丢包统计，记录 `/xr/*_controller`、`/xr_rm/*/cmd_vel`、`/xr_rm/*/current_pose`。
1. 稳定 PICO 数据链路：固定 UDP JSON 协议和坐标系，增加频率、延迟、丢包统计，记录 `/xr/*_controller`、`/xr_rm/*/raw_target_pose`、`/xr_rm/*/target_pose`、`/xr_rm/*/target_clamped`、`/xr_rm/*/current_pose`。
2. 提升真机安全性：增加启动前安全检查、软件急停 topic、UI Stop 状态提示、双臂中间区域互斥边界和速度/加速度限幅。
3. 集成末端执行器：明确夹爪 topic、力控比例、开合方向和安全上限，把 `trigger` 从预留字段变成稳定夹爪输入。
4. 接入视觉和数据记录：加入 D405/D435 相机 launch、TF、内外参和 rosbag2 实验记录。
@ -341,6 +348,6 @@ Controller topic 没有数据：
机械臂不动：
- 确认 `grip=true`。
- 确认 `/xr_rm/<arm>/cmd_vel` 是否有非零速度。
- 确认目标 TCP 没有被工作空间边界或圆柱半径限制夹住。
- 确认 `/xr_rm/<arm>/raw_target_pose` 与 `/xr_rm/<arm>/target_pose` 是否在变化。
- 确认 `/xr_rm/<arm>/target_clamped` 是否持续为 `true`，如果是，目标 TCP 可能被工作空间、圆柱半径或单帧步长限制夹住。
- 确认真机 SDK 连接成功，且 RM75 没有报警或急停。

15
docs/pico_udp_sender_ubuntu22_setup.md
View File

@ -265,8 +265,8 @@ sudo ufw status
- 面板中 `L ok / R ok` 表示 Unity 能读到左右手柄。
- `UDP Sending ON` 后，ROS2 的 `/xr/left_controller` 与 `/xr/right_controller` 应持续刷新。
- HUD 显示包计数、追踪状态、grip 和 KeepAwake 状态。
- 按住 `grip` 并移动手柄时，mock 模式下 `/xr_rm/*/cmd_vel` 应出现非零速度。
- 松开 `grip` 后，`cmd_vel` 应回到零速度。
- 按住 `grip` 并移动手柄时，mock 模式下 `/xr_rm/*/target_pose` 应连续变化，`/xr_rm/*/cmd_vel` 会显示目标位姿变化率。
- 松开 `grip` 后，机械臂慢停，`cmd_vel` 应回到零。
## 9. ROS2 端验证
@ -335,16 +335,19 @@ source /opt/ros/humble/setup.bash
source install/setup.bash
ros2 topic echo /xr_rm/left_rm75/cmd_vel
ros2 topic echo /xr_rm/right_rm75/cmd_vel
# 需要看实际位姿目标时另开终端：
ros2 topic echo /xr_rm/left_rm75/target_pose
ros2 topic echo /xr_rm/right_rm75/target_pose
```
流程：
1. 启动 PICO Unity 应用。
2. 确认 `/xr/left_controller` 和 `/xr/right_controller` 正常刷新。
3. 左手按住 `grip`，只移动左手一小段，观察 `/xr_rm/left_rm75/cmd_vel`。
3. 左手按住 `grip`，只移动左手一小段，观察 `/xr_rm/left_rm75/target_pose` 和 `/xr_rm/left_rm75/cmd_vel`。
4. 松开左手 `grip`，确认 `cmd_vel` 回到 0。
5. 右手重复同样流程。
6. 用 `ros2 topic hz` 确认频率稳定，建议 `50 Hz` 以上。
6. 用 `ros2 topic hz` 确认频率稳定，建议接近 PICO 端配置的发送频率。
如果要排除 PICO 端问题，可用本机 sample sender 验证 ROS 端：
@ -427,7 +430,7 @@ payload.quat[3] = rotation.w;
1. 只启动 `use_mock:=true`。
2. 按住左手 `grip`，沿 PICO 的 `+X/-X`、`+Y/-Y`、`+Z/-Z` 每次只动一个轴。
3. 记录 `/xr/left_controller.pose.position` 的变化方向。
4. 记录 `/xr_rm/left_rm75/cmd_vel` 的方向。
4. 记录 `/xr_rm/left_rm75/target_pose` 的方向。
5. 右手重复。
如果两个手柄在 ROS topic 里的某个轴都反了，优先检查 Unity 的坐标转换。如果 ROS topic 正确，但某一只机械臂运动方向不符合现场坐标，只改对应 YAML 的 `xr_to_robot_matrix`。
@ -439,7 +442,7 @@ payload.quat[3] = rotation.w;
- 急停可用。
- 机械臂工作区清空。
- PICO topic 在 mock 下已经稳定。
- `grip=false` 时 `/xr_rm/<arm>/cmd_vel` 为 0。
- `grip=false` 时机械臂慢停，`/xr_rm/<arm>/cmd_vel` 为 0。
- `move_to_initial_pose_on_connect` 保持 `false`。
单臂真机：

52
xr_rm_bringup/config/dual_arm_rm75.yaml
View File

@ -1,34 +1,28 @@
# 阶段一：PICO 遥操作双 RM75 平台配置。
#
# 参数来源：acDual-arm-YikaiFuTest/realman_pkg/core/arg_cfg.py
# - 左臂 IP：192.168.192.18
# - 右臂 IP：192.168.192.19
# - 左右臂工作空间、圆柱半径约束、初始化点位和 IP 沿用最新 acRealman 配置。
# - PICO 手柄位移映射根据论文 OpenXR 坐标和现场双臂 base 坐标图单独设置。
#
# 注意：当前项目仍然采用“手柄相对位移”控制方式。
# 按下握持键时锁定当前手柄位姿和 TCP 位姿，之后只跟随相对位移。
# acDual-arm 项目使用的是戴盟绝对 PoseStamped 重映射，因此这里只迁移
# 坐标标定和安全参数，不迁移其绝对位姿控制链路。
# 当前控制方式是“相对位姿透传”：
# 按下 grip 时锁定当前手柄位姿和 TCP 位姿，之后将手柄相对位移映射为目标
# TCP 位姿，经过工作空间限幅、目标低通和单帧步长限制后，通过 rm_movep_canfd
# 下发。cmd_vel 仅作为目标位姿变化率调试话题，不是机械臂执行命令。
# 末端外设由 peripherals_rm75.yaml 配置，launch 只在真机连接阶段初始化。
left_arm_teleop:
ros__parameters:
arm_name: left_rm75
controller_topic: /xr/left_controller
control_rate_hz: 50.0
control_rate_hz: 90.0
command_timeout_sec: 0.12
# 实验台番茄测试先采用保守参数；完成三轴方向检查和实体急停测试后再提高速度。
# 位姿目标生成与平滑参数。
scale: 0.75
kp_linear: 1.8
deadband_m: 0.002
low_pass_alpha: 0.35
deadband_m: 0.001
target_filter_alpha: 0.65
target_filter_alpha_fast: 0.9
target_filter_fast_threshold_m: 0.03
max_linear_speed: 0.2
enable_position_axes: [true, true, true]
current_pose_poll_hz: 10.0
# 来自 acDual-arm 的 bounds_p[left]：
# x[-0.50, 0.50]，y[-0.60, -0.20]，z[0.10, 0.50]。
workspace_min: [-0.70, -0.60, 0.10]
workspace_max: [0.70, 0.40, 0.70]
cyl_radius_limit: [0.20, 0.60]
@ -36,8 +30,6 @@ left_arm_teleop:
low_z_min_radius: 0.21
# PICO/OpenXR 位置坐标：+X 向右，+Y 向上，+Z 向后。
# 左臂 base 坐标：+X 向下，+Z 向右，前方工作区对应 -Y。
# 现场模拟 XR 调试确认 X/Z 轴符号与原标定相反。
# 映射关系：机器人位移增量 = [-手柄y, 手柄z, -手柄x]。
xr_to_robot_matrix: [0.0, -1.0, 0.0,
0.0, 0.0, 1.0,
@ -50,6 +42,8 @@ left_arm_teleop:
avoid_singularity: 0
frame_type: 1
follow: false
canfd_trajectory_mode: 2
canfd_radio: 0
configure_safety_limits: true
max_line_speed: 1.0
max_angular_speed: 1.5
@ -57,32 +51,28 @@ left_arm_teleop:
max_angular_acc: 2.0
joint_max_speed: 180.0
joint_max_acc: 180.0
# 真机 launch 默认不自动移动；需要沿用 acRealman 上电初始化时再显式打开。
move_to_initial_pose_on_connect: false
initial_joint_pose: [-167.21, 28.48, 28.21, 61.35, -14.40, 84.49, -124.51]
initial_tcp_pose: [-0.2562, -0.2765, 0.1489, -3.0190, -0.1010, 3.1400]
init_move_speed: 20
command_mode: pose_canfd
canfd_trajectory_mode: 2
canfd_radio: 0
debug_topic_prefix: /xr_rm
right_arm_teleop:
ros__parameters:
arm_name: right_rm75
controller_topic: /xr/right_controller
control_rate_hz: 50.0
control_rate_hz: 90.0
command_timeout_sec: 0.12
scale: 0.75
kp_linear: 1.8
deadband_m: 0.002
low_pass_alpha: 0.35
deadband_m: 0.001
target_filter_alpha: 0.65
target_filter_alpha_fast: 0.9
target_filter_fast_threshold_m: 0.03
max_linear_speed: 0.2
enable_position_axes: [true, true, true]
current_pose_poll_hz: 10.0
# 来自 acDual-arm 的 bounds_p[right]：
# x[-0.70, 0.50]，y[-0.60, 0.40]，z[0.10, 0.70]。
workspace_min: [-0.70, -0.60, 0.10]
workspace_max: [0.70, 0.40, 0.70]
cyl_radius_limit: [0.20, 0.60]
@ -90,8 +80,6 @@ right_arm_teleop:
low_z_min_radius: 0.21
# PICO/OpenXR 位置坐标：+X 向右，+Y 向上，+Z 向后。
# 右臂 base 坐标：+X 向上，+Z 向左，前方工作区对应 -Y。
# 现场模拟 XR 调试确认 X/Z 轴符号与原标定相反。
# 映射关系：机器人位移增量 = [手柄y, 手柄z, 手柄x]。
xr_to_robot_matrix: [0.0, 1.0, 0.0,
0.0, 0.0, 1.0,
@ -104,6 +92,8 @@ right_arm_teleop:
avoid_singularity: 1
frame_type: 1
follow: false
canfd_trajectory_mode: 2
canfd_radio: 0
configure_safety_limits: true
max_line_speed: 1.0
max_angular_speed: 1.5

78
xr_rm_bringup/config/left_arm_rm75.yaml
View File

@ -1,90 +1,52 @@
# 左臂单独调试配置：XR 相对位移控制 RM75 TCP。
# 左臂单独调试配置：XR 相对位姿透传控制 RM75 TCP。
# 末端外设由 peripherals_rm75.yaml 配置，launch 只在真机连接阶段初始化。
# single_arm_velocity_teleop 是单臂遥操作节点名；单臂 launch 会按这个节点名读取参数。
single_arm_velocity_teleop:
# ros__parameters 是 ROS2 YAML 参数入口，下面的字段会加载为节点参数。
ros__parameters:
# 机械臂逻辑名称，用于日志、调试话题命名和区分左右臂。
arm_name: left_rm75
# 订阅的 XR 左手柄话题，由 udp_controller_receiver 发布。
controller_topic: /xr/left_controller
# 控制循环频率，单位 Hz；越高响应越细，但对通信和 CPU 要求更高。
control_rate_hz: 50.0
# 手柄消息超时时间，单位秒；超过该时间没有新数据就停止输出速度。
control_rate_hz: 90.0
command_timeout_sec: 0.12
# XR 手柄相对位移到机器人目标位移的比例系数。
# 手柄相对位移 -> 目标 TCP 位姿；随后做目标低通和单帧步长限制。
scale: 1.0
# 目标 TCP 位置误差转换为速度命令的线性比例增益。
kp_linear: 1.8
# 位置误差死区，单位米；小于该值时认为已到位，不再输出速度。
deadband_m: 0.002
# 速度一阶低通滤波系数，范围 0~1；越大响应越快，越小越平滑。
low_pass_alpha: 0.35
# 软件侧最大 TCP 线速度，单位 m/s；限制遥操作移动速度。
deadband_m: 0.001
target_filter_alpha: 0.65
target_filter_alpha_fast: 0.9
target_filter_fast_threshold_m: 0.03
max_linear_speed: 0.3
# 是否启用机器人 X/Y/Z 三个位置轴；false 的轴不会输出位移控制。
enable_position_axes: [true, true, true]
current_pose_poll_hz: 10.0
# TCP 工作空间下限 [x, y, z]，单位米；目标点会被限制在该范围内。
workspace_min: [-0.70, -0.60, 0.10]
# TCP 工作空间上限 [x, y, z]，单位米；目标点会被限制在该范围内。
workspace_max: [0.70, 0.40, 0.70]
# TCP 到机器人底座轴线的圆柱半径限制 [最小半径, 最大半径]，单位米。
cyl_radius_limit: [0.20, 0.60]
# 低高度判定阈值，单位米；低于该 Z 值时会启用更保守的最小半径。
low_z_threshold: 0.20
# 低高度区域内的最小圆柱半径，单位米，用于避免 TCP 过靠近底座。
low_z_min_radius: 0.21
# 现场模拟 XR 调试确认 X/Z 轴符号与原标定相反，因此翻转手柄 X、Z 两列。
# 3x3 坐标映射矩阵，按行展开；robot_delta = matrix * xr_delta。
# 映射关系：机器人位移增量 = [-手柄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]
# 是否使用 mock 机械臂；true 不连接真机，只在软件里积分模拟运动。
use_mock: false
# mock 模式下的初始 TCP 位姿 [x, y, z, rx, ry, rz]。
mock_initial_pose: [-0.2562, -0.2765, 0.1489, -3.0190, -0.1010, 3.1400]
# 左臂 RM75 控制器 IP 地址。
robot_ip: 192.168.192.18
# RM75 控制器 TCP 端口。
robot_port: 8080
# 厂商 SDK 速度透传避奇异参数；0 表示关闭硬件避奇异初始化。
avoid_singularity: 0
# 厂商 SDK 速度透传参考坐标系类型；当前使用 1。
frame_type: 1
# 速度透传 follow 标志，传给 rm_movev_canfd；false 表示不启用跟随模式。
follow: false
# 是否在连接真机后尝试下发硬件安全限速、限加速度等配置。
configure_safety_limits: true
# 下发给控制器的最大直线速度，单位 m/s。
max_line_speed: 1.0
# 下发给控制器的最大角速度，单位 rad/s 或 SDK 定义单位。
max_angular_speed: 1.5
# 下发给控制器的最大直线加速度，单位 m/s^2。
max_line_acc: 1.0
# 下发给控制器的最大角加速度，单位 rad/s^2 或 SDK 定义单位。
max_angular_acc: 2.0
# 下发给控制器的单关节最大速度，单位通常为 deg/s。
joint_max_speed: 180.0
# 下发给控制器的单关节最大加速度，单位通常为 deg/s^2。
joint_max_acc: 180.0
# 连接真机后是否自动移动到 initial_joint_pose 和 initial_tcp_pose。
move_to_initial_pose_on_connect: false
# 自动初始化时先执行的关节目标位姿，单位通常为度，共 7 轴。
initial_joint_pose: [-167.21, 28.48, 28.21, 61.35, -14.40, 84.49, -124.51]
# 自动初始化时再执行的 TCP 目标位姿 [x, y, z, rx, ry, rz]。
initial_tcp_pose: [-0.2562, -0.2765, 0.1489, -3.0190, -0.1010, 3.1400]
# 初始化移动速度，传给厂商 movej/movel 接口。
init_move_speed: 20
# 真机命令模式；pose_canfd 表示通过 CANFD 位置目标方式输出控制。
command_mode: pose_canfd
# CANFD 轨迹模式参数，传给厂商 SDK，用于控制位置目标执行方式。
canfd_trajectory_mode: 2
# CANFD 透传平滑/滤波参数，传给厂商 SDK。
canfd_radio: 0
# 调试话题前缀，节点会发布当前位姿、目标位姿和速度到该命名空间下。
configure_safety_limits: true
max_line_speed: 1.0
max_angular_speed: 1.5
max_line_acc: 1.0
max_angular_acc: 2.0
joint_max_speed: 180.0
joint_max_acc: 180.0
move_to_initial_pose_on_connect: false
initial_joint_pose: [-167.21, 28.48, 28.21, 61.35, -14.40, 84.49, -124.51]
initial_tcp_pose: [-0.2562, -0.2765, 0.1489, -3.0190, -0.1010, 3.1400]
init_move_speed: 20
debug_topic_prefix: /xr_rm

20
xr_rm_bringup/config/right_arm_rm75.yaml
View File

@ -1,19 +1,21 @@
# 右臂单独调试配置：XR 相对位移控制 RM75 TCP。
# 右臂单独调试配置：XR 相对位姿透传控制 RM75 TCP。
# 末端外设由 peripherals_rm75.yaml 配置，launch 只在真机连接阶段初始化。
single_arm_velocity_teleop:
ros__parameters:
arm_name: right_rm75
controller_topic: /xr/right_controller
control_rate_hz: 50.0
control_rate_hz: 90.0
command_timeout_sec: 0.12
scale: 1.0
kp_linear: 1.8
deadband_m: 0.002
low_pass_alpha: 0.7
max_linear_speed: 0.5
scale: 0.75
deadband_m: 0.001
target_filter_alpha: 0.65
target_filter_alpha_fast: 0.9
target_filter_fast_threshold_m: 0.03
max_linear_speed: 0.3
enable_position_axes: [true, true, true]
current_pose_poll_hz: 10.0
workspace_min: [-0.70, -0.60, 0.10]
workspace_max: [0.70, 0.40, 0.70]
@ -21,7 +23,7 @@ single_arm_velocity_teleop:
low_z_threshold: 0.1
low_z_min_radius: 0.1
# 现场模拟 XR 调试确认 X/Z 轴符号与原标定相反，因此翻转手柄 X、Z 两列。
# 映射关系：机器人位移增量 = [手柄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]
@ -33,6 +35,8 @@ single_arm_velocity_teleop:
avoid_singularity: 1
frame_type: 1
follow: false
canfd_trajectory_mode: 2
canfd_radio: 0
configure_safety_limits: true
max_line_speed: 1.0
max_angular_speed: 1.5

26
xr_rm_bringup/launch/arm_debug.launch.py
View File

@ -36,6 +36,7 @@ def _udp_receiver_node() -> Node:
parameters=[{
"udp_host": LaunchConfiguration("udp_host"),
"udp_port": LaunchConfiguration("udp_port"),
"timer_hz": LaunchConfiguration("udp_timer_hz"),
"left_topic": "/xr/left_controller",
"right_topic": "/xr/right_controller",
}],
@ -48,6 +49,8 @@ def _single_arm_node(
move_to_initial_pose: bool,
avoid_singularity: int,
frame_type: int,
control_rate_hz: float,
follow: bool,
configure_safety_limits: bool,
enable_tool_control: bool,
configure_peripheral_on_connect: bool,
@ -69,6 +72,8 @@ def _single_arm_node(
"robot_port": LaunchConfiguration("robot_port"),
"avoid_singularity": avoid_singularity,
"frame_type": frame_type,
"control_rate_hz": control_rate_hz,
"follow": follow,
"configure_safety_limits": configure_safety_limits,
"move_to_initial_pose_on_connect": move_to_initial_pose,
"enable_tool_control": enable_tool_control,
@ -91,6 +96,8 @@ def _dual_arm_nodes(
left_avoid_singularity: int,
right_avoid_singularity: int,
frame_type: int,
control_rate_hz: float,
follow: bool,
configure_safety_limits: bool,
enable_tool_control: bool,
configure_peripheral_on_connect: bool,
@ -111,6 +118,8 @@ def _dual_arm_nodes(
"robot_port": LaunchConfiguration("robot_port"),
"avoid_singularity": left_avoid_singularity,
"frame_type": frame_type,
"control_rate_hz": control_rate_hz,
"follow": follow,
"configure_safety_limits": configure_safety_limits,
"move_to_initial_pose_on_connect": move_to_initial_pose,
"enable_tool_control": enable_tool_control,
@ -134,6 +143,8 @@ def _dual_arm_nodes(
"robot_port": LaunchConfiguration("robot_port"),
"avoid_singularity": right_avoid_singularity,
"frame_type": frame_type,
"control_rate_hz": control_rate_hz,
"follow": follow,
"configure_safety_limits": configure_safety_limits,
"move_to_initial_pose_on_connect": move_to_initial_pose,
"enable_tool_control": enable_tool_control,
@ -163,6 +174,8 @@ def _launch_setup(context, *args, **kwargs):
avoid_override or LaunchConfiguration("right_avoid_singularity").perform(context)
)
frame_type = int(LaunchConfiguration("frame_type").perform(context))
control_rate_hz = float(LaunchConfiguration("control_rate_hz").perform(context))
follow = _as_bool(LaunchConfiguration("follow").perform(context))
configure_safety_limits = _as_bool(
LaunchConfiguration("configure_safety_limits").perform(context)
)
@ -185,6 +198,8 @@ def _launch_setup(context, *args, **kwargs):
left_avoid_singularity,
right_avoid_singularity,
frame_type,
control_rate_hz,
follow,
configure_safety_limits,
enable_tool_control,
configure_peripheral_on_connect,
@ -199,6 +214,8 @@ def _launch_setup(context, *args, **kwargs):
move_to_initial_pose,
avoid_singularity,
frame_type,
control_rate_hz,
follow,
configure_safety_limits,
enable_tool_control,
configure_peripheral_on_connect,
@ -216,17 +233,22 @@ def generate_launch_description() -> LaunchDescription:
# UDP 监听参数，需要与 PICO 端或 sample_udp_sender 保持一致。
DeclareLaunchArgument("udp_host", default_value="0.0.0.0"),
DeclareLaunchArgument("udp_port", default_value="15000"),
# UDP receiver 轮询频率高于 PICO 发送频率，减少 socket 中等待时间。
DeclareLaunchArgument("udp_timer_hz", default_value="200.0"),
# 左右 RM75 默认 IP，可在命令行中按现场网络覆盖。
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("left_avoid_singularity", default_value="0"),
DeclareLaunchArgument("right_avoid_singularity", default_value="1"),
# 非空时作为左右臂全局覆盖，例如 avoid_singularity:=0。
DeclareLaunchArgument("avoid_singularity", default_value=""),
DeclareLaunchArgument("frame_type", default_value="1"),
# 现场调参入口：默认按 PICO 90Hz 输入节奏发送 rm_movep_canfd。
DeclareLaunchArgument("control_rate_hz", default_value="90.0"),
# 默认低跟随；高跟随请确认控制器和网络能稳定满足厂商周期要求后再打开。
DeclareLaunchArgument("follow", default_value="false"),
DeclareLaunchArgument("configure_safety_limits", default_value="true"),
# 工具控制通过遥操作节点复用同一个 RealMan 连接，避免两个进程抢同一机械臂连接。
DeclareLaunchArgument("enable_tool_control", default_value="true"),

10
xr_rm_bringup/tools/launcher_ui.py
View File

@ -41,7 +41,7 @@ SAMPLE_SENDER_ARGS = (
TERMINAL_TITLE_PREFIX = "XR-RM Terminal - "
TOPIC_MONITOR_TITLE = "XR-RM Topic Monitor"
TOPIC_MONITOR_ACTION = "__xr_rm_topic_monitor__"
CMD_VEL_MONITOR_TITLE = "XR-RM Cmd Vel Monitor"
CMD_VEL_MONITOR_TITLE = "XR-RM Target Velocity Monitor"
CMD_VEL_MONITOR_ACTION = "__xr_rm_cmd_vel_monitor__"
ROS_GRAPH_MONITOR_TITLE = "XR-RM ROS Graph Monitor"
ROS_GRAPH_MONITOR_ACTION = "__xr_rm_ros_graph_monitor__"
@ -52,8 +52,8 @@ TOPIC_MONITORS = [
]
CMD_VEL_MONITORS = [
("Left Cmd Vel", "/xr_rm/left_rm75/cmd_vel"),
("Right Cmd Vel", "/xr_rm/right_rm75/cmd_vel"),
("Left Target Vel", "/xr_rm/left_rm75/cmd_vel"),
("Right Target Vel", "/xr_rm/right_rm75/cmd_vel"),
]
ROS_GRAPH_MONITORS = [
@ -167,7 +167,7 @@ def _topic_monitor_item() -> tuple[str, str]:
def _cmd_vel_monitor_item() -> tuple[str, str]:
return ("Open Cmd Vel Topic Monitor", CMD_VEL_MONITOR_ACTION)
return ("Open Target Velocity Monitor", CMD_VEL_MONITOR_ACTION)
def _is_topic_monitor_action(action: str) -> bool:
@ -181,7 +181,7 @@ def _topic_monitor_spec(action: str) -> tuple[str, list[tuple[str, str]], str, s
CMD_VEL_MONITORS,
"xr_rm_cmd_vel_monitor",
"xr_rm_cmd_vel_monitor_",
"cmd_vel topic",
"target velocity topic",
)
return (
TOPIC_MONITOR_TITLE,

9
xr_rm_input/xr_rm_input/udp_controller_receiver.py
View File

@ -44,7 +44,7 @@ class UdpControllerReceiver(Node):
self._quat_order = str(self.get_parameter("quat_order").value).lower()
# UDP 接收使用非阻塞模式，避免没有手柄数据时卡住 ROS2 定时器。
self._publishers = {
self._controller_publishers = {
"left": self.create_publisher(XrController, left_topic, 10),
"right": self.create_publisher(XrController, right_topic, 10),
}
@ -78,7 +78,7 @@ class UdpControllerReceiver(Node):
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)
self._controller_publishers[msg.hand].publish(msg)
def _iter_controller_payloads(self, payload: Any) -> Iterable[tuple[str, Mapping[str, Any]]]:
if isinstance(payload, list):
@ -214,9 +214,12 @@ def main(args: list[str] | None = None) -> None:
node = UdpControllerReceiver()
try:
rclpy.spin(node)
except KeyboardInterrupt:
pass
finally:
node.destroy_node()
rclpy.shutdown()
if rclpy.ok():
rclpy.shutdown()
if __name__ == "__main__":

4
xr_rm_teleop/setup.py
View File

@ -1,6 +1,6 @@
"""xr_rm_teleop 包安装配置。
该包提供基于 XR 相对位移的 RM75 笛卡尔速度遥操作节点。
该包提供基于 XR 相对位移的 RM75 笛卡尔位姿透传遥操作节点。
"""
from setuptools import setup
@ -19,7 +19,7 @@ setup(
zip_safe=True,
maintainer="Yikai Fu",
maintainer_email="user@example.com",
description="XR relative-motion teleoperation controllers for RealMan RM75 arms.",
description="XR relative-pose teleoperation controllers for RealMan RM75 arms.",
license="Apache-2.0",
tests_require=["pytest"],
entry_points={

2
xr_rm_teleop/xr_rm_teleop/__init__.py
View File

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

91
xr_rm_teleop/xr_rm_teleop/realman_adapter.py
View File

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

419
xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py
View File

@ -1,7 +1,7 @@
"""RM75 单臂 XR 相对位移遥操作节点。
"""RM75 单臂 XR 相对位姿透传遥操作节点。
节点订阅左/右手柄位姿，在 grip 按下时锁定手柄和 TCP 起点，把手柄相对位移
映射到机器人坐标系中的目标 TCP，并通过笛卡尔速度命令跟随该目标。
映射成机器人坐标系中的目标 TCP，并通过 rm_movep_canfd 持续下发目标位姿。
"""
from __future__ import annotations
@ -12,6 +12,7 @@ from typing import Iterable
import rclpy
from geometry_msgs.msg import PoseStamped, TwistStamped
from rclpy.node import Node
from rclpy.time import Time
from std_msgs.msg import Bool
from xr_rm_interfaces.msg import XrController
@ -28,21 +29,24 @@ def _clamp(value: float, low: float, high: float) -> float:
class SingleArmVelocityTeleop(Node):
"""基于 XR 手柄相对位移的 RM75 单臂笛卡尔速度遥操节点。"""
"""基于 XR 手柄相对位移的 RM75 单臂位姿透传遥操节点。
类名和可执行入口沿用旧名称，避免已有 launch、UI 和命令失效。
"""
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("control_rate_hz", 90.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("velocity_zero_epsilon", 1e-6)
self.declare_parameter("max_linear_speed", 0.05)
self.declare_parameter("deadband_m", 0.001)
self.declare_parameter("target_filter_alpha", 0.65)
self.declare_parameter("target_filter_alpha_fast", 0.9)
self.declare_parameter("target_filter_fast_threshold_m", 0.03)
self.declare_parameter("max_linear_speed", 0.25)
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])
@ -50,6 +54,7 @@ class SingleArmVelocityTeleop(Node):
self.declare_parameter("low_z_threshold", 0.20)
self.declare_parameter("low_z_min_radius", 0.21)
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("current_pose_poll_hz", 10.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")
@ -68,7 +73,6 @@ 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("enable_tool_control", True)
@ -86,10 +90,12 @@ class SingleArmVelocityTeleop(Node):
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._velocity_zero_epsilon = float(self.get_parameter("velocity_zero_epsilon").value)
self._target_filter_alpha = float(self.get_parameter("target_filter_alpha").value)
self._target_filter_alpha_fast = float(self.get_parameter("target_filter_alpha_fast").value)
self._target_filter_fast_threshold_m = float(
self.get_parameter("target_filter_fast_threshold_m").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)
@ -98,6 +104,7 @@ class SingleArmVelocityTeleop(Node):
self._low_z_threshold = float(self.get_parameter("low_z_threshold").value)
self._low_z_min_radius = float(self.get_parameter("low_z_min_radius").value)
self._xr_to_robot_matrix = self._float_list_parameter("xr_to_robot_matrix", 9)
self._current_pose_poll_hz = float(self.get_parameter("current_pose_poll_hz").value)
self._follow = self._bool_parameter("follow")
self._debug_topic_prefix = str(self.get_parameter("debug_topic_prefix").value).rstrip("/")
if not self._debug_topic_prefix:
@ -105,11 +112,16 @@ class SingleArmVelocityTeleop(Node):
self._validate_parameters()
self._last_msg: XrController | None = None
self._last_msg_time = None
self._last_msg_time: Time | None = 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._robot_start_pose: ArmPose | None = None
self._filtered_target: list[float] | None = None
self._last_sent_target: list[float] | None = None
self._last_command_time: Time | None = None
self._last_current_pose: ArmPose | None = None
self._last_current_pose_time: Time | None = None
self._stop_sent = True
self._adapter = self._make_adapter()
self._adapter.connect()
@ -117,15 +129,20 @@ class SingleArmVelocityTeleop(Node):
debug_ns = f"{self._debug_topic_prefix}/{self._arm_name}"
self._current_pose_pub = self.create_publisher(PoseStamped, f"{debug_ns}/current_pose", 10)
self._raw_target_pose_pub = self.create_publisher(PoseStamped, f"{debug_ns}/raw_target_pose", 10)
self._target_pose_pub = self.create_publisher(PoseStamped, f"{debug_ns}/target_pose", 10)
self._cmd_vel_pub = self.create_publisher(TwistStamped, f"{debug_ns}/cmd_vel", 10)
self._target_clamped_pub = self.create_publisher(Bool, f"{debug_ns}/target_clamped", 10)
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}")
self.get_logger().info(
f"{self._arm_name} 位姿透传遥操节点已启动，监听话题：{topic}, "
f"dt={self._dt:.4f}s, follow={self._follow}"
)
def _make_adapter(self):
# mock 和真机共享同一控制逻辑，只在适配层切换运动命令的执行方式。
# mock 和真机共享同一位姿目标链路，只在适配层切换执行方式。
if self._bool_parameter("use_mock"):
return MockRealManAdapter(
[float(v) for v in self.get_parameter("mock_initial_pose").value],
@ -135,7 +152,6 @@ class SingleArmVelocityTeleop(Node):
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),
logger=self.get_logger(),
@ -150,7 +166,6 @@ 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),
)
@ -196,98 +211,100 @@ class SingleArmVelocityTeleop(Node):
self._last_msg_time = self.get_clock().now()
def _control_tick(self) -> None:
now = self.get_clock().now()
if self._last_msg is None or self._last_msg_time is None:
self._safe_stop()
self._safe_stop(reset_active=True)
return
age = (self.get_clock().now() - self._last_msg_time).nanoseconds * 1e-9
age = (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
if self._active or not self._stop_sent:
self.get_logger().warn(
f"{self._arm_name} XR 手柄数据超时，机械臂停止。",
throttle_duration_sec=1.0,
)
self._safe_stop(reset_active=True)
self._last_msg = None
self._last_msg_time = None
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
self.get_logger().info(f"{self._arm_name} Grip 松开，退出相对位姿遥操。")
self._safe_stop(reset_active=True)
return
controller_now = self._controller_xyz(self._last_msg)
if not self._active:
self._enter_active_control(controller_now, now)
return
assert self._controller_start is not None
assert self._robot_start_pose is not None
self._maybe_refresh_current_pose(now)
raw_target = self._raw_target_from_controller(controller_now)
workspace_target, workspace_clamped = self._clamp_workspace_with_flag(raw_target)
desired_target = self._apply_deadband(workspace_target)
filtered_target = self._filter_target(desired_target)
sent_target, step_limited = self._limit_target_step(filtered_target)
sent_target, final_clamped = self._clamp_workspace_with_flag(sent_target)
velocity = self._estimate_command_velocity(sent_target, now)
target_clamped = workspace_clamped or step_limited or final_clamped
target_pose = ArmPose(
x=sent_target[0],
y=sent_target[1],
z=sent_target[2],
rx=self._robot_start_pose.rx,
ry=self._robot_start_pose.ry,
rz=self._robot_start_pose.rz,
)
self._publish_debug(raw_target, sent_target, velocity, target_clamped)
if self._send_cartesian_target(target_pose):
self._last_sent_target = sent_target
self._last_command_time = now
self._stop_sent = False
def _enter_active_control(self, controller_now: list[float], now: Time) -> None:
try:
robot_pose = self._adapter.get_current_pose()
robot_now = robot_pose.xyz()
robot_pose = self._read_current_pose_for_control(now)
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
self._safe_stop(reset_active=True)
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._filtered_velocity = self._zero_tiny_velocity(self._filtered_velocity)
cartesian_velocity = self._filtered_velocity + [0.0, 0.0, 0.0]
self._publish_debug(robot_pose, target, 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)
robot_xyz = robot_pose.xyz()
self._active = True
self._controller_start = controller_now
self._robot_start_pose = robot_pose
self._filtered_target = robot_xyz
self._last_sent_target = robot_xyz
self._last_command_time = now
self._stop_sent = True
self.get_logger().info(f"{self._arm_name} Grip 按下，已锁定手柄和机械臂初始位姿。")
self._publish_debug(robot_xyz, robot_xyz, [0.0] * 6, False)
@staticmethod
def _controller_xyz(msg: XrController) -> list[float]:
return [msg.pose.position.x, msg.pose.position.y, msg.pose.position.z]
def _raw_target_from_controller(self, controller_now: list[float]) -> list[float]:
assert self._controller_start is not None
assert self._robot_start_pose is not None
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_pose.x + self._scale * robot_delta[0],
self._robot_start_pose.y + self._scale * robot_delta[1],
self._robot_start_pose.z + self._scale * robot_delta[2],
]
return [target[i] if self._enable_position_axes[i] else self._robot_start_pose.xyz()[i] for i in range(3)]
def _map_xr_delta_to_robot(self, delta: list[float]) -> list[float]:
matrix = self._xr_to_robot_matrix
return [
@ -296,13 +313,61 @@ class SingleArmVelocityTeleop(Node):
matrix[6] * delta[0] + matrix[7] * delta[1] + matrix[8] * delta[2],
]
def _clamp_workspace(self, target: list[float]) -> list[float]:
# 先做盒状工作空间限幅，再叠加底座圆柱半径限制。
def _apply_deadband(self, target: list[float]) -> list[float]:
if self._deadband_m <= 0.0 or self._last_sent_target is None:
return target
delta = [target[i] - self._last_sent_target[i] for i in range(3)]
if _norm(delta) < self._deadband_m:
return list(self._last_sent_target)
return target
def _filter_target(self, target: list[float]) -> list[float]:
if self._filtered_target is None:
self._filtered_target = list(target)
return list(target)
delta = [target[i] - self._filtered_target[i] for i in range(3)]
distance = _norm(delta)
alpha = self._adaptive_filter_alpha(distance)
self._filtered_target = [
alpha * target[i] + (1.0 - alpha) * self._filtered_target[i]
for i in range(3)
]
return list(self._filtered_target)
def _adaptive_filter_alpha(self, distance: float) -> float:
if self._target_filter_fast_threshold_m <= 1e-9:
return self._target_filter_alpha_fast
ratio = _clamp(distance / self._target_filter_fast_threshold_m, 0.0, 1.0)
return (
self._target_filter_alpha
+ (self._target_filter_alpha_fast - self._target_filter_alpha) * ratio
)
def _limit_target_step(self, target: list[float]) -> tuple[list[float], bool]:
if self._last_sent_target is None:
return target, False
delta = [target[i] - self._last_sent_target[i] for i in range(3)]
distance = _norm(delta)
max_step = self._max_linear_speed * self._dt
if distance <= max_step or distance <= 1e-9:
return target, False
scale = max_step / distance
return [
self._last_sent_target[i] + delta[i] * scale
for i in range(3)
], True
def _clamp_workspace_with_flag(self, target: list[float]) -> tuple[list[float], bool]:
clamped = [
_clamp(target[i], self._workspace_min[i], self._workspace_max[i])
for i in range(3)
]
return self._clamp_cylinder_radius(clamped)
clamped = self._clamp_cylinder_radius(clamped)
was_clamped = any(abs(clamped[i] - target[i]) > 1e-9 for i in range(3))
return clamped, was_clamped
def _clamp_cylinder_radius(self, target: list[float]) -> list[float]:
min_radius, max_radius = self._cyl_radius_limit
@ -325,40 +390,80 @@ class SingleArmVelocityTeleop(Node):
return target
@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 _estimate_command_velocity(self, target: list[float], now: Time) -> list[float]:
if self._last_sent_target is None:
return [0.0] * 6
def _safe_stop(self) -> None:
# 所有断连、松手、退出路径都走这里，确保发送零速度。
self._filtered_velocity = [0.0, 0.0, 0.0]
self._publish_stop_debug()
if self._adapter_uses_pose_targets():
dt = self._dt
if self._last_command_time is not None:
measured_dt = (now - self._last_command_time).nanoseconds * 1e-9
if measured_dt > 1e-6:
dt = measured_dt
return [
(target[i] - self._last_sent_target[i]) / dt
for i in range(3)
] + [0.0, 0.0, 0.0]
def _read_current_pose_for_control(self, now: Time) -> ArmPose:
pose = self._adapter.get_current_pose()
self._last_current_pose = pose
self._last_current_pose_time = now
return pose
def _maybe_refresh_current_pose(self, now: Time) -> None:
if self._current_pose_poll_hz <= 0.0:
return
self._send_cartesian_velocity([0.0] * 6)
if self._last_current_pose_time is not None:
age = (now - self._last_current_pose_time).nanoseconds * 1e-9
if age < 1.0 / self._current_pose_poll_hz:
return
def _publish_stop_debug(self) -> None:
try:
current_pose = self._adapter.get_current_pose()
except Exception:
return
self._publish_debug(current_pose, current_pose.xyz(), [0.0] * 6)
def _send_cartesian_velocity(self, velocity: list[float]) -> None:
try:
self._adapter.send_cartesian_velocity(velocity, self._follow)
self._read_current_pose_for_control(now)
except Exception as exc:
self.get_logger().error(
f"{self._arm_name} 发送速度命令失败：{exc}",
self.get_logger().warn(
f"{self._arm_name} 低频读取 TCP 位姿失败，继续透传目标：{exc}",
throttle_duration_sec=1.0,
)
self._active = False
def _send_cartesian_target(self, pose: ArmPose) -> None:
def _safe_stop(self, reset_active: bool) -> None:
if not self._stop_sent:
self._send_stop_once()
if reset_active:
self._active = False
self._controller_start = None
self._robot_start_pose = None
self._filtered_target = None
self._last_sent_target = None
self._last_command_time = None
self._publish_stop_debug()
def _send_stop_once(self) -> None:
try:
self._adapter.stop()
except Exception as exc:
self.get_logger().error(
f"{self._arm_name} 停止机械臂失败：{exc}",
throttle_duration_sec=1.0,
)
self._stop_sent = True
def _publish_stop_debug(self) -> None:
pose = self._debug_pose_fallback()
if pose is None:
return
self._publish_debug(pose.xyz(), pose.xyz(), [0.0] * 6, False)
def _debug_pose_fallback(self) -> ArmPose | None:
if self._last_current_pose is not None:
return self._last_current_pose
if self._robot_start_pose is not None:
return self._robot_start_pose
if self._last_sent_target is not None:
return ArmPose(*self._last_sent_target)
return None
def _send_cartesian_target(self, pose: ArmPose) -> bool:
try:
self._adapter.send_cartesian_target(pose, self._follow)
except Exception as exc:
@ -367,22 +472,33 @@ class SingleArmVelocityTeleop(Node):
throttle_duration_sec=1.0,
)
self._active = False
self._send_stop_once()
return False
return True
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 _zero_tiny_velocity(self, velocity: list[float]) -> list[float]:
if self._velocity_zero_epsilon <= 0.0:
return velocity
return [
0.0 if abs(value) < self._velocity_zero_epsilon else value
for value in velocity
]
def _publish_debug(self, current_pose: ArmPose, target_xyz: list[float], velocity: list[float]) -> None:
def _publish_debug(
self,
raw_target_xyz: list[float],
target_xyz: list[float],
command_velocity: list[float],
target_clamped: bool,
) -> None:
stamp = self.get_clock().now().to_msg()
current_msg = self._pose_msg(stamp, current_pose)
current_pose = self._debug_pose_fallback()
if current_pose is None:
current_pose = ArmPose(*target_xyz)
raw_pose = self._pose_msg(
stamp,
ArmPose(
x=raw_target_xyz[0],
y=raw_target_xyz[1],
z=raw_target_xyz[2],
rx=current_pose.rx,
ry=current_pose.ry,
rz=current_pose.rz,
),
)
target_msg = self._pose_msg(
stamp,
ArmPose(
@ -394,20 +510,24 @@ class SingleArmVelocityTeleop(Node):
rz=current_pose.rz,
),
)
velocity = self._zero_tiny_velocity(velocity)
velocity_msg = TwistStamped()
velocity_msg.header.stamp = stamp
velocity_msg.header.frame_id = "rm_base"
velocity_msg.twist.linear.x = float(velocity[0])
velocity_msg.twist.linear.y = float(velocity[1])
velocity_msg.twist.linear.z = float(velocity[2])
velocity_msg.twist.angular.x = float(velocity[3])
velocity_msg.twist.angular.y = float(velocity[4])
velocity_msg.twist.angular.z = float(velocity[5])
velocity_msg.twist.linear.x = float(command_velocity[0])
velocity_msg.twist.linear.y = float(command_velocity[1])
velocity_msg.twist.linear.z = float(command_velocity[2])
velocity_msg.twist.angular.x = float(command_velocity[3])
velocity_msg.twist.angular.y = float(command_velocity[4])
velocity_msg.twist.angular.z = float(command_velocity[5])
self._current_pose_pub.publish(current_msg)
clamped_msg = Bool()
clamped_msg.data = bool(target_clamped)
self._current_pose_pub.publish(self._pose_msg(stamp, current_pose))
self._raw_target_pose_pub.publish(raw_pose)
self._target_pose_pub.publish(target_msg)
self._cmd_vel_pub.publish(velocity_msg)
self._target_clamped_pub.publish(clamped_msg)
@staticmethod
def _pose_msg(stamp, pose: ArmPose) -> PoseStamped:
@ -467,12 +587,18 @@ class SingleArmVelocityTeleop(Node):
raise ValueError("command_timeout_sec must be > 0")
if self._deadband_m < 0.0:
raise ValueError("deadband_m must be >= 0")
if not 0.0 <= self._low_pass_alpha <= 1.0:
raise ValueError("low_pass_alpha must be in [0, 1]")
if self._velocity_zero_epsilon < 0.0:
raise ValueError("velocity_zero_epsilon must be >= 0")
if self._max_linear_speed < 0.0:
raise ValueError("max_linear_speed must be >= 0")
if not 0.0 <= self._target_filter_alpha <= 1.0:
raise ValueError("target_filter_alpha must be in [0, 1]")
if not 0.0 <= self._target_filter_alpha_fast <= 1.0:
raise ValueError("target_filter_alpha_fast must be in [0, 1]")
if self._target_filter_alpha_fast < self._target_filter_alpha:
raise ValueError("target_filter_alpha_fast must be >= target_filter_alpha")
if self._target_filter_fast_threshold_m < 0.0:
raise ValueError("target_filter_fast_threshold_m must be >= 0")
if self._max_linear_speed <= 0.0:
raise ValueError("max_linear_speed must be > 0")
if self._current_pose_poll_hz < 0.0:
raise ValueError("current_pose_poll_hz must be >= 0")
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}]")
@ -498,7 +624,8 @@ def main(args: list[str] | None = None) -> None:
pass
finally:
node.destroy_node()
rclpy.shutdown()
if rclpy.ok():
rclpy.shutdown()
if __name__ == "__main__":