# 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 窗口返回后再进入下一阶段。