acRealman_xr/docs/superpowers/specs/2026-07-28-rm75-feedback-scheduling-follow-speed-design.md

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 等待串联起来：

当前更新间隔 = rm_get_joint_degree 调用耗时 + 8 ms 固定等待

控制回调、QP 和发送均有充足余量，不是当前反馈慢的原因。

阶段一：反馈线程绝对周期调度

调度语义

将当前“读取完成后固定等待 8 ms”改为“读取起始时间之间以 8 ms 为目标”：

读取耗时 < 8 ms：只等待剩余时间
读取耗时 ≥ 8 ms：不再额外等待，从当前时间重新建立周期基准

调度不补跑已经错过的历史周期。一次长阻塞结束后最多立即开始下一次读取， 不会为了追赶多个旧截止点而密集补调用 SDK。

等价的目标启动间隔为：

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