132 lines
4.8 KiB
Markdown
132 lines
4.8 KiB
Markdown
# RM75 反馈调度与跟随速度优化设计
|
||
|
||
## 目标
|
||
|
||
在保留 Placo 单步 QP、工作空间与圆柱限位、关节速度限制、指令超时和安全
|
||
停止的前提下，分阶段解决 PICO 遥操机械臂跟随速度很慢的问题。
|
||
|
||
每阶段只改变一个控制因素，真机验证通过后才进入下一阶段：
|
||
|
||
1. 提高关节反馈的新鲜度；
|
||
2. 启用 `rm_movej_canfd` 高跟随完全透传；
|
||
3. 将右臂单独调试 TCP 速度提高到 `0.2 m/s`。
|
||
|
||
## 真机证据
|
||
|
||
右臂在 125 Hz、低跟随模式下连续四个约 5 秒窗口的结果为：
|
||
|
||
- `period mean=8.000 ms`，四个窗口最大值为 `9.023–9.424 ms`；
|
||
- `total mean=1.908–2.004 ms`，最大值不超过 `4.109 ms`；
|
||
- `qp mean=0.230–0.245 ms`；
|
||
- `send mean=0.122–0.127 ms`；
|
||
- `feedback_read mean=9.101–9.630 ms`；
|
||
- `feedback_interval mean=17.368–17.918 ms`；
|
||
- `feedback_age mean=9.079–9.632 ms`，最差达到 `46.480 ms`。
|
||
|
||
每个窗口只有 `279–288` 次新反馈，即实际反馈频率约为 `56–58 Hz`。
|
||
`feedback_interval - feedback_read` 在四个窗口中稳定为 `8.27–8.39 ms`，
|
||
确认当前反馈线程把一次 SDK 查询耗时和完整的 8 ms 等待串联起来：
|
||
|
||
```text
|
||
当前更新间隔 = rm_get_joint_degree 调用耗时 + 8 ms 固定等待
|
||
```
|
||
|
||
控制回调、QP 和发送均有充足余量，不是当前反馈慢的原因。
|
||
|
||
## 阶段一：反馈线程绝对周期调度
|
||
|
||
### 调度语义
|
||
|
||
将当前“读取完成后固定等待 8 ms”改为“读取起始时间之间以 8 ms 为目标”：
|
||
|
||
```text
|
||
读取耗时 < 8 ms：只等待剩余时间
|
||
读取耗时 ≥ 8 ms：不再额外等待，从当前时间重新建立周期基准
|
||
```
|
||
|
||
调度不补跑已经错过的历史周期。一次长阻塞结束后最多立即开始下一次读取，
|
||
不会为了追赶多个旧截止点而密集补调用 SDK。
|
||
|
||
等价的目标启动间隔为：
|
||
|
||
```text
|
||
max(8 ms, 本次反馈读取耗时)
|
||
```
|
||
|
||
当前读取平均约 9.3 ms，因此预期反馈频率接近 `100 Hz`，但不强求达到
|
||
`125 Hz`。
|
||
|
||
### 范围
|
||
|
||
本阶段只修改 `RealManAdapter._feedback_loop()` 的等待计算。以下内容保持不变：
|
||
|
||
- `follow=false`；
|
||
- `canfd_trajectory_mode=2`；
|
||
- 右臂单独调试 `max_linear_speed=0.15 m/s`；
|
||
- 125 Hz ROS 控制定时器和 Placo QP；
|
||
- 同一个 RealMan 连接承担反馈与发送，不新增连接；
|
||
- 所有安全限位、超时和停止逻辑。
|
||
|
||
现有 `feedback_read`、`feedback_interval` 和其他 timing 指标继续保留。
|
||
|
||
### 验收
|
||
|
||
右臂真机连续采集四个 timing 窗口，全部满足：
|
||
|
||
- `feedback_interval mean ≤ 12 ms`；
|
||
- `feedback_interval mean - feedback_read mean ≤ 2 ms`；
|
||
- `feedback_age mean ≤ 7 ms`；
|
||
- `period max < 10 ms`；
|
||
- `total max < 8 ms`；
|
||
- 无反馈超时、异常停止或 SDK 发送错误。
|
||
|
||
若更密集的反馈查询使 `period max` 达到或超过 10 ms，或明显增加发送耗时，
|
||
停止后续高跟随阶段，继续定位同一 SDK 连接的读写竞争。
|
||
|
||
## 阶段二：高跟随完全透传
|
||
|
||
只有阶段一通过后才验证：
|
||
|
||
- `follow=true`；
|
||
- `canfd_trajectory_mode=0`；
|
||
- 右臂单独调试速度仍为 `0.15 m/s`。
|
||
|
||
先通过现有 launch 参数显式启用高跟随完成右臂单机验证；验证通过后，再把
|
||
`arm_debug.launch.py` 默认值和 `dual_arm_rm75.yaml`、`left_arm_rm75.yaml`、
|
||
`right_arm_rm75.yaml` 同步为高跟随完全透传。
|
||
|
||
验收条件：
|
||
|
||
- 快速移动手柄约 10 cm 后，机械臂追赶不超过 1 秒；
|
||
- 连续四个 timing 窗口 `period max < 10 ms`；
|
||
- 无明显振荡、跳动、反馈超时或异常停止。
|
||
|
||
若仍追赶超过 1 秒，不进入加速阶段；先增加关节目标与实测关节误差统计，
|
||
确认慢速来自 QP 单步目标还是控制器执行。
|
||
|
||
## 阶段三：右臂速度提高到 0.2 m/s
|
||
|
||
只有阶段二通过后，将 `right_arm_rm75.yaml` 中右臂单独调试的
|
||
`max_linear_speed` 从 `0.15` 提高到 `0.2 m/s`。
|
||
|
||
- `dual_arm_rm75.yaml` 的左右臂已经是 `0.2 m/s`，无需修改；
|
||
- 左臂单独调试速度保持现状；
|
||
- 右臂真机 `max_line_speed=0.25 m/s` 安全上限保持不变。
|
||
|
||
右臂 `scale=0.7` 时，手柄移动 10 cm 对应约 7 cm TCP 目标，理论限速时间约
|
||
0.35 秒。验收追赶时间不超过 0.7 秒，并确认没有明显振荡或限位异常。
|
||
|
||
## 测试与交付
|
||
|
||
阶段一实现采用测试先行：
|
||
|
||
- 用确定性时钟和停止事件验证短读取只等待剩余时间；
|
||
- 验证读取超期后不额外等待，也不补跑多个历史周期；
|
||
- 运行 `xr_rm_teleop` 全部 pytest；
|
||
- 运行 `test_orientation_control.py`；
|
||
- 在 `/home/robot/WS_xr` 运行 `colcon build --symlink-install`。
|
||
|
||
Codex 不连接真机、不移动机械臂。每个阶段的真机验证由用户通过
|
||
`xr_rm_bringup/launch/arm_debug.launch.py` 在右臂、小范围动作下完成，并把连续
|
||
四个完整 timing 窗口返回后再进入下一阶段。
|