From 948d50cab4ddc79770ea2c2f0c4e0402a423d300 Mon Sep 17 00:00:00 2001 From: YikaiFu-cart Date: Sun, 31 May 2026 20:06:07 +0800 Subject: [PATCH] Refactor and enhance XR-RM teleoperation functionality --- CODEX.md | 8 +- README.md | 29 +- docs/pico_udp_sender_ubuntu22_setup.md | 15 +- xr_rm_bringup/config/dual_arm_rm75.yaml | 52 +-- xr_rm_bringup/config/left_arm_rm75.yaml | 78 +--- xr_rm_bringup/config/right_arm_rm75.yaml | 20 +- xr_rm_bringup/launch/arm_debug.launch.py | 26 +- xr_rm_bringup/tools/launcher_ui.py | 10 +- .../xr_rm_input/udp_controller_receiver.py | 9 +- xr_rm_teleop/setup.py | 4 +- xr_rm_teleop/xr_rm_teleop/__init__.py | 2 +- xr_rm_teleop/xr_rm_teleop/realman_adapter.py | 91 +--- .../single_arm_velocity_teleop.py | 419 ++++++++++++------ 13 files changed, 409 insertions(+), 354 deletions(-) diff --git a/CODEX.md b/CODEX.md index 8018e0d..0a7ecb7 100644 --- a/CODEX.md +++ b/CODEX.md @@ -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//current_pose、target_pose、cmd_vel + -> /xr_rm//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 工程。 diff --git a/README.md b/README.md index 7798cd7..cb72d26 100755 --- a/README.md +++ b/README.md @@ -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//current_pose、target_pose、cmd_vel 调试话题 + -> 左右 RM75 笛卡尔相对位姿透传控制 + -> /xr_rm//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//cmd_vel` 归零。 +6. 确认松开 `grip` 后机械臂慢停，`/xr_rm//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//cmd_vel` 是否有非零速度。 -- 确认目标 TCP 没有被工作空间边界或圆柱半径限制夹住。 +- 确认 `/xr_rm//raw_target_pose` 与 `/xr_rm//target_pose` 是否在变化。 +- 确认 `/xr_rm//target_clamped` 是否持续为 `true`，如果是，目标 TCP 可能被工作空间、圆柱半径或单帧步长限制夹住。 - 确认真机 SDK 连接成功，且 RM75 没有报警或急停。 diff --git a/docs/pico_udp_sender_ubuntu22_setup.md b/docs/pico_udp_sender_ubuntu22_setup.md index bb03f0f..b6178e8 100644 --- a/docs/pico_udp_sender_ubuntu22_setup.md +++ b/docs/pico_udp_sender_ubuntu22_setup.md @@ -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//cmd_vel` 为 0。 +- `grip=false` 时机械臂慢停，`/xr_rm//cmd_vel` 为 0。 - `move_to_initial_pose_on_connect` 保持 `false`。 单臂真机： diff --git a/xr_rm_bringup/config/dual_arm_rm75.yaml b/xr_rm_bringup/config/dual_arm_rm75.yaml index 39b82b2..5ea3879 100755 --- a/xr_rm_bringup/config/dual_arm_rm75.yaml +++ b/xr_rm_bringup/config/dual_arm_rm75.yaml @@ -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 diff --git a/xr_rm_bringup/config/left_arm_rm75.yaml b/xr_rm_bringup/config/left_arm_rm75.yaml index ba85edb..c37dabf 100644 --- a/xr_rm_bringup/config/left_arm_rm75.yaml +++ b/xr_rm_bringup/config/left_arm_rm75.yaml @@ -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 diff --git a/xr_rm_bringup/config/right_arm_rm75.yaml b/xr_rm_bringup/config/right_arm_rm75.yaml index e859471..1b80670 100644 --- a/xr_rm_bringup/config/right_arm_rm75.yaml +++ b/xr_rm_bringup/config/right_arm_rm75.yaml @@ -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 diff --git a/xr_rm_bringup/launch/arm_debug.launch.py b/xr_rm_bringup/launch/arm_debug.launch.py index fbe23fe..e60cb31 100644 --- a/xr_rm_bringup/launch/arm_debug.launch.py +++ b/xr_rm_bringup/launch/arm_debug.launch.py @@ -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"), diff --git a/xr_rm_bringup/tools/launcher_ui.py b/xr_rm_bringup/tools/launcher_ui.py index 9b8d61f..cc7a3e9 100755 --- a/xr_rm_bringup/tools/launcher_ui.py +++ b/xr_rm_bringup/tools/launcher_ui.py @@ -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, diff --git a/xr_rm_input/xr_rm_input/udp_controller_receiver.py b/xr_rm_input/xr_rm_input/udp_controller_receiver.py index 6b6a69e..7064dbc 100755 --- a/xr_rm_input/xr_rm_input/udp_controller_receiver.py +++ b/xr_rm_input/xr_rm_input/udp_controller_receiver.py @@ -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__": diff --git a/xr_rm_teleop/setup.py b/xr_rm_teleop/setup.py index 7cc31b1..a8d5352 100755 --- a/xr_rm_teleop/setup.py +++ b/xr_rm_teleop/setup.py @@ -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={ diff --git a/xr_rm_teleop/xr_rm_teleop/__init__.py b/xr_rm_teleop/xr_rm_teleop/__init__.py index f8b6293..8bc1296 100755 --- a/xr_rm_teleop/xr_rm_teleop/__init__.py +++ b/xr_rm_teleop/xr_rm_teleop/__init__.py @@ -1,4 +1,4 @@ """XR 到 RM75 遥操作节点包。 -核心模块包括单臂速度遥操作、RealMan SDK 适配层和夹爪 trigger 桥接。 +核心模块包括单臂位姿透传遥操作、RealMan SDK 适配层和夹爪 trigger 桥接。 """ diff --git a/xr_rm_teleop/xr_rm_teleop/realman_adapter.py b/xr_rm_teleop/xr_rm_teleop/realman_adapter.py index 8a4f142..5de7be4 100755 --- a/xr_rm_teleop/xr_rm_teleop/realman_adapter.py +++ b/xr_rm_teleop/xr_rm_teleop/realman_adapter.py @@ -1,12 +1,11 @@ """RM75 机械臂适配层。 -对上提供统一的当前位姿读取、笛卡尔速度发送和停止接口；对下根据配置 +对上提供统一的当前位姿读取、笛卡尔位姿目标发送和停止接口；对下根据配置 选择 mock 积分模拟器或睿尔曼 Python API2 真机通信。 """ from __future__ import annotations -import time from dataclasses import dataclass from numbers import Number from typing import Any @@ -40,16 +39,17 @@ class MockRealManAdapter: def get_current_pose(self) -> ArmPose: return self._pose - def send_cartesian_velocity(self, velocity: list[float], follow: bool) -> None: + def send_cartesian_target(self, pose: ArmPose, follow: bool) -> None: del follow - self.last_velocity = velocity - # 模拟模式只做简单积分，便于观察控制器是否在按预期更新末端位置。 - self._pose.x += velocity[0] * self._dt - self._pose.y += velocity[1] * self._dt - self._pose.z += velocity[2] * self._dt - self._pose.rx += velocity[3] * self._dt - self._pose.ry += velocity[4] * self._dt - self._pose.rz += velocity[5] * self._dt + self.last_velocity = [ + (pose.x - self._pose.x) / self._dt, + (pose.y - self._pose.y) / self._dt, + (pose.z - self._pose.z) / self._dt, + 0.0, + 0.0, + 0.0, + ] + self._pose = pose def stop(self) -> None: self.last_velocity = [0.0] * 6 @@ -65,13 +65,12 @@ class MockRealManAdapter: class RealManAdapter: - """睿尔曼 Python API2 的笛卡尔速度透传适配层。""" + """睿尔曼 Python API2 的笛卡尔位姿透传适配层。""" def __init__( self, robot_ip: str, robot_port: int, - dt: float, avoid_singularity: int, frame_type: int, logger: Any | None = None, @@ -86,13 +85,11 @@ class RealManAdapter: initial_joint_pose: list[float] | None = None, initial_tcp_pose: list[float] | None = None, init_move_speed: int = 20, - command_mode: str = "velocity", canfd_trajectory_mode: int = 2, canfd_radio: int = 0, ) -> None: self._robot_ip = robot_ip self._robot_port = robot_port - self._dt_ms = int(round(dt * 1000.0)) self._avoid_singularity = avoid_singularity self._frame_type = frame_type self._logger = logger @@ -107,13 +104,10 @@ class RealManAdapter: self._initial_joint_pose = initial_joint_pose self._initial_tcp_pose = initial_tcp_pose self._init_move_speed = init_move_speed - self._command_mode = command_mode self._canfd_trajectory_mode = canfd_trajectory_mode self._canfd_radio = canfd_radio self._scissorgripper: int | None = None self._arm: Any | None = None - if self._command_mode not in ("velocity", "pose_canfd"): - raise ValueError("command_mode must be one of: velocity, pose_canfd") def connect(self) -> None: try: @@ -130,17 +124,12 @@ class RealManAdapter: "RealMan connected: " f"ip={self._robot_ip}, port={self._robot_port}, " f"avoid_singularity={self._avoid_singularity}, " - f"frame_type={self._frame_type}, dt_ms={self._dt_ms}, " - f"command_mode={self._command_mode}" + f"frame_type={self._frame_type}, command=rm_movep_canfd" ) if self._configure_safety_limits: self._apply_safety_limits() if self._move_to_initial_pose_on_connect: self._move_to_initial_pose() - if self._command_mode == "velocity": - self._init_movev_canfd() - else: - self._log_info("command_mode=pose_canfd，跳过 rm_set_movev_canfd_init。") def get_current_pose(self) -> ArmPose: self._require_arm() @@ -150,17 +139,8 @@ class RealManAdapter: raise RuntimeError(f"无法从睿尔曼状态中解析当前 TCP 位姿：{state!r}") return ArmPose(*pose[:6]) - def send_cartesian_velocity(self, velocity: list[float], follow: bool) -> None: - self._require_arm() - if self._command_mode == "pose_canfd": - return - ret = self._arm.rm_movev_canfd(velocity, follow, 0, 0) - self._check_return(ret, "rm_movev_canfd") - def send_cartesian_target(self, pose: ArmPose, follow: bool) -> None: self._require_arm() - if self._command_mode != "pose_canfd": - raise RuntimeError("send_cartesian_target requires command_mode=pose_canfd") ret = self._arm.rm_movep_canfd( [pose.x, pose.y, pose.z, pose.rx, pose.ry, pose.rz], follow, @@ -169,9 +149,6 @@ class RealManAdapter: ) self._check_return(ret, "rm_movep_canfd") - def uses_pose_targets(self) -> bool: - return self._command_mode == "pose_canfd" - def configure_peripheral(self, config_file: str, peripheral_arm: str) -> None: self._require_arm() from .fun_peripheral import load_peripheral_config, peripheral_cfg @@ -205,8 +182,6 @@ class RealManAdapter: if self._arm is None: return try: - if self._command_mode == "velocity": - self._arm.rm_movev_canfd([0.0] * 6, False, 0, 0) self._arm.rm_set_arm_slow_stop() except Exception: pass @@ -244,46 +219,6 @@ class RealManAdapter: ret = self._arm.rm_movel(self._initial_tcp_pose, self._init_move_speed, 0, 0, 1) self._check_return(ret, "rm_movel(initial_tcp_pose)") - def _init_movev_canfd(self) -> None: - attempts = 3 - candidates = [self._avoid_singularity] - if self._avoid_singularity != 0: - candidates.append(0) - - # 某些现场控制器初始化避奇异模式会超时，失败后自动降级到 0 再重试。 - last_ret: Any = None - for avoid_singularity in candidates: - if avoid_singularity != self._avoid_singularity: - self._log_warn("rm_set_movev_canfd_init 降级为 avoid_singularity=0 后重试。") - - for attempt in range(1, attempts + 1): - ret = self._arm.rm_set_movev_canfd_init( - avoid_singularity, - self._frame_type, - self._dt_ms, - ) - last_ret = ret - code = self._return_code(ret) - if code == 0: - self._avoid_singularity = avoid_singularity - self._log_info( - "rm_set_movev_canfd_init 成功：" - f"avoid_singularity={avoid_singularity}, " - f"frame_type={self._frame_type}, dt_ms={self._dt_ms}" - ) - return - - self._log_warn( - "rm_set_movev_canfd_init 失败：" - f"avoid_singularity={avoid_singularity}, " - f"frame_type={self._frame_type}, dt_ms={self._dt_ms}, " - f"attempt={attempt}/{attempts}, ret={ret!r}" - ) - if attempt < attempts: - time.sleep(0.2) - - self._check_return(last_ret, "rm_set_movev_canfd_init") - def _try_call(self, name: str, *args: Any) -> None: func = getattr(self._arm, name, None) if func is None: 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 index 7c568f7..af13548 100755 --- a/xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py +++ b/xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py @@ -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__":