199 lines
7.7 KiB
Markdown
199 lines
7.7 KiB
Markdown
# RM75 CANFD UDP 主动反馈设计
|
||
|
||
> 真机验证修正：右臂高跟随首轮测试触发掉使能。离线复现发现静止目标存在
|
||
> `23.582°` 空空间漂移，第一周期约 `3315°/s²`。当前实现已移除
|
||
> manipulability 自运动、增加软件关节加速度限幅和故障 Grip 锁存，并将三份
|
||
> YAML 恢复为 `follow: false` 安全基线；高跟随须在基线验证后单独测试。
|
||
|
||
## 目标
|
||
|
||
按睿尔曼 MovejCANFD 示例，将真机关节反馈从同一 TCP 控制连接上的
|
||
`rm_get_joint_degree()` 周期轮询，替换为控制器 UDP 主动状态推送。
|
||
|
||
控制命令继续通过现有单个 `RoboticArm(RM_TRIPLE_MODE_E)` 句柄发送，不新增
|
||
RealMan 连接，不修改 Placo QP、工作空间/圆柱限位、速度限制、指令超时或安全
|
||
停止条件。
|
||
|
||
## 根因与证据
|
||
|
||
低跟随模式下，125 Hz CANFD 发送与绝对周期 TCP 反馈轮询能够同时工作：
|
||
|
||
- `feedback_interval mean=10.041–10.389 ms`；
|
||
- `feedback_age mean=5.730–6.138 ms`；
|
||
- 控制周期最大值不超过 `8.979 ms`。
|
||
|
||
启用高跟随后，即使 `canfd_trajectory_mode=2`，控制发送仍正常：
|
||
|
||
- `period max=9.275 ms`；
|
||
- `send max=0.235 ms`。
|
||
|
||
但同步反馈退化为：
|
||
|
||
- `feedback_read mean=11.278 ms`、`max=80.320 ms`；
|
||
- `feedback_age max=117.502 ms`。
|
||
|
||
反馈年龄逼近现有 `command_timeout_sec=0.12 s`，触发“关节反馈缺失或过期”
|
||
安全停止，造成 Grip 按住期间控制反复退出和重新锁定。增大超时只会允许 QP
|
||
继续使用更旧的关节状态，不解决 TCP 反馈阻塞。
|
||
|
||
睿尔曼 MovejCANFD 示例使用三线程模式、`rm_set_realtime_push()` 和
|
||
`rm_realtime_arm_state_call_back()`，通过 UDP 回调获取关节状态，而不是在
|
||
CANFD 透传期间同步轮询关节角。
|
||
|
||
## 数据流
|
||
|
||
```text
|
||
PICO -> ROS 125 Hz 控制回调 -> Placo QP -> TCP rm_movej_canfd
|
||
|
||
RM75 控制器 -> UDP 5 ms 主动推送 -> SDK 第三线程回调
|
||
-> JointStateSnapshot 缓存 -> ROS 125 Hz 控制回调
|
||
```
|
||
|
||
TCP 仍承担 CANFD、慢停、安全配置和末端工具命令。UDP 只承担状态反馈，两条
|
||
传输路径共用同一个机械臂句柄。
|
||
|
||
## SDK 连接与反馈生命周期
|
||
|
||
`RealManAdapter.connect()` 保持三线程模式和单次 `rm_create_robot_arm()`：
|
||
|
||
1. 创建并检查机械臂句柄。
|
||
2. 下发已有安全参数和可选初始位姿。
|
||
3. 创建并保存 `rm_realtime_arm_state_callback_ptr`，避免 Python 回调被垃圾
|
||
回收。
|
||
4. 使用 `rm_realtime_push_config_t` 配置 5 ms UDP 主动上报。
|
||
5. 注册 `rm_realtime_arm_state_call_back()`。
|
||
6. 等待第一帧有效 UDP 反馈，最长 2 秒。
|
||
|
||
若配置接口返回非零，或 2 秒内没有有效反馈，连接初始化失败并删除已创建的
|
||
机械臂句柄；不静默回退到 TCP 轮询。
|
||
|
||
连接成功后不再创建反馈线程，也不再调用 `rm_get_joint_degree()`。
|
||
`close()` 先停止接受回调更新，再执行现有慢停和句柄删除。控制器的 UDP 配置
|
||
由下一次启动重新覆盖，不额外增加关闭阶段配置命令。
|
||
|
||
## UDP 回调与缓存
|
||
|
||
回调只执行有界、非阻塞工作：
|
||
|
||
1. 检查回调对象、`errCode` 和来源机械臂 IP。
|
||
2. 读取 7 个 `joint_position`，检查数量、数值类型及 NaN/Inf。
|
||
3. 将厂商反馈的角度转换为弧度。
|
||
4. 使用 `time.monotonic()` 记录接收时刻，并计算与上一帧的更新间隔。
|
||
5. 在现有 `_joint_state_lock` 下替换 `JointStateSnapshot`。
|
||
6. 第一帧有效数据唤醒连接初始化等待。
|
||
|
||
无效 UDP 帧不覆盖上一帧缓存。若后续持续丢包，现有 120 ms 新鲜度检查自然
|
||
触发安全停止。
|
||
|
||
UDP 回调不执行 QP、ROS 发布、停止命令或其他 SDK 调用，避免阻塞 SDK 接收
|
||
线程。
|
||
|
||
## 参数与三份 YAML
|
||
|
||
新增真机参数：
|
||
|
||
- `realtime_push_host_ip`：机械臂可直接访问的上位机地址；
|
||
- `realtime_push_port`：单臂 UDP 接收端口；
|
||
- `realtime_push_cycle_ms`：主动上报周期，默认并配置为 `5`。
|
||
|
||
本次现场配置：
|
||
|
||
| 配置 | 节点 | host | port |
|
||
|---|---|---|---:|
|
||
| `right_arm_rm75.yaml` | 右臂 | `192.168.192.148` | 8090 |
|
||
| `left_arm_rm75.yaml` | 左臂 | `192.168.192.148` | 8089 |
|
||
| `dual_arm_rm75.yaml` | 左臂 | `192.168.192.148` | 8089 |
|
||
| `dual_arm_rm75.yaml` | 右臂 | `192.168.192.148` | 8090 |
|
||
|
||
左右臂端口必须不同。更换上位机或网络后，只需同步修改 YAML 中的
|
||
`realtime_push_host_ip`。
|
||
|
||
Mock 模式不导入厂商 SDK，也不要求 UDP 参数有效。
|
||
|
||
## YAML 与 launch 参数所有权
|
||
|
||
此前 `arm_debug.launch.py` 会用 launch 默认值覆盖 YAML 中的机械臂参数，
|
||
导致 YAML 无法单独控制 `follow` 等行为。
|
||
|
||
用户选择由 YAML 作为机械臂行为和硬件参数的唯一默认来源。
|
||
|
||
以下参数只由 `left_arm_rm75.yaml`、`right_arm_rm75.yaml` 和
|
||
`dual_arm_rm75.yaml` 管理，launch 不再声明或覆盖：
|
||
|
||
- `robot_ip`、`robot_port`；
|
||
- `avoid_singularity`；
|
||
- `control_rate_hz`；
|
||
- `follow`、`canfd_trajectory_mode`、`canfd_radio`；
|
||
- `configure_safety_limits`；
|
||
- `move_to_initial_pose_on_connect`；
|
||
- `enable_tool_control`、`enable_trigger_gripper_control`；
|
||
- `trigger_close_threshold`；
|
||
- `configure_peripheral_on_connect`；
|
||
- 本设计新增的 UDP 主动反馈参数。
|
||
|
||
三份 YAML 补齐工具控制参数；删除其中不再生效的 `use_mock`，避免出现两个
|
||
配置来源。
|
||
|
||
`arm_debug.launch.py` 只保留：
|
||
|
||
- `arm=left|right|both`，选择启动拓扑；
|
||
- `use_mock=true|false`，作为显式安全运行模式，默认仍为 `true`；
|
||
- PICO 输入节点的 `udp_host`、`udp_port`、`udp_timer_hz`；
|
||
- launch 根据安装路径和左右臂生成的 `robot_urdf_path`、
|
||
`peripheral_config_file`、`peripheral_arm` 和 `tool_command_topic`。
|
||
|
||
`launcher_ui.py` 和 README 中的启动命令同步删除机械臂 IP、初始化移动等已
|
||
移交 YAML 的 launch 参数，只保留 `arm`、`use_mock` 和 PICO 输入覆盖。
|
||
|
||
控制模式保持分阶段范围：
|
||
|
||
- 三份 YAML 均使用 `follow: false`、`canfd_trajectory_mode: 2` 完成安全基线；
|
||
- 基线验证通过前不启用高跟随或模式 0，不提高 `max_linear_speed`。
|
||
|
||
## Timing 日志
|
||
|
||
保留：
|
||
|
||
- `period`；
|
||
- `total`；
|
||
- `qp`；
|
||
- `send`；
|
||
- `feedback_age`；
|
||
- `feedback_interval`。
|
||
|
||
`feedback_read` 表示同步 SDK 查询耗时；UDP 架构不存在该查询，因此该字段
|
||
不再产生样本，现有条件日志逻辑会自动省略它，不新增同义统计项。
|
||
|
||
## 测试
|
||
|
||
使用 FakeArm 和伪造 SDK 模块覆盖：
|
||
|
||
- 连接时使用正确 host、port、5 ms 周期配置 UDP 并注册回调；
|
||
- 第一帧有效回调转换 7 个关节角为弧度并解除启动等待；
|
||
- 连续回调正确记录 `feedback_interval`；
|
||
- 错误码、来源 IP、长度或 NaN/Inf 无效帧不覆盖缓存；
|
||
- UDP 配置失败或首帧超时会清理句柄并抛出明确异常；
|
||
- 真机适配器不再启动轮询线程或调用 `rm_get_joint_degree()`；
|
||
- Mock 模式无需厂商 SDK。
|
||
- launch 不再覆盖 YAML 的机械臂行为和硬件参数；
|
||
- `use_mock` 仍由 launch 默认设为 `true`；
|
||
- `launcher_ui.py` 不再传递已经删除的 launch 参数。
|
||
|
||
随后运行：
|
||
|
||
```bash
|
||
cd /home/robot/WS_xr
|
||
source /opt/ros/humble/setup.bash
|
||
python3 -m pytest -q src/xr_rm_teleop/test
|
||
python3 -m pytest -q src/xr_rm_teleop/test/test_orientation_control.py
|
||
colcon build --symlink-install --executor sequential
|
||
```
|
||
|
||
Codex 不连接真机。用户在右臂小范围测试中确认：
|
||
|
||
- 启动日志显示收到 UDP 首帧；
|
||
- 按住 Grip 不再出现反馈过期或 SDK `-2`；
|
||
- 连续四个 timing 窗口 `period max < 10 ms`、`total max < 8 ms`；
|
||
- `feedback_interval mean` 接近 5 ms；
|
||
- `feedback_age mean < 5 ms`，且最大值不触发 120 ms 安全停止。
|