diff --git a/docs/superpowers/specs/2026-07-30-rm75-qp-udp-feedback-fix-design.md b/docs/superpowers/specs/2026-07-30-rm75-qp-udp-feedback-fix-design.md new file mode 100644 index 0000000..d957a45 --- /dev/null +++ b/docs/superpowers/specs/2026-07-30-rm75-qp-udp-feedback-fix-design.md @@ -0,0 +1,176 @@ +# RM75 QP 阈值与 UDP 反馈周期修复设计 + +## 背景 + +右臂真机在 90 Hz、`follow: false` 遥操过程中频繁出现两类警告: + +```text +QP did not converge after 30 iterations: +position_error=0.001245~0.001609 m + +UDP关节反馈超时,保持最后安全目标。 +``` + +现场 timing 日志同时表明: + +- 控制回调最大约 5.1 ms,没有耗尽 11.111 ms 周期; +- UDP 反馈间隔均值约 25 ms,即实际约 40 Hz; +- UDP 间隔存在 37~71 ms 的明显抖动; +- UDP 短暂超时后约 44 ms 收到新帧,但现有安全状态机要求先松开 Grip。 + +因此,控制线程计算量不是这两类警告的原因。 + +## 根因 + +### QP 近阈值失败 + +当前 QP 最多迭代 30 次,并要求: + +```text +位置误差 <= 1 mm +姿态误差 <= 0.005 rad +``` + +现场失败时姿态误差约 0.0007 rad,已经满足要求;位置误差仅比 1 mm 高 +0.245~0.609 mm。30 次迭代中的每一步已经通过关节有限值、关节位置限制和单步 +速度限制校验,但最终结果仍因严格的 1 mm 判定被整体丢弃。 + +用户已明确确认 2 mm 位置残差可接受;该数值也与现有 1 mm 手柄位置死区处于 +同一量级。将位置收敛阈值改为 2 mm,可以接收现场这类安全的近收敛 +结果,同时继续拒绝此前出现过的 7.5 mm 等明显未收敛结果。 + +### UDP 周期单位错误 + +项目参数 `realtime_push_cycle_ms` 的单位是毫秒,三份机械臂配置均填写 `5`。 +当前适配器把这个值原样传给: + +```python +rm_realtime_push_config_t(cycle, ...) +``` + +但睿尔曼 SDK 的 `cycle` 单位不是毫秒,而是 5 ms 的倍数。因此: + +```text +当前传入 cycle=5 +实际周期 = 5 × 5 ms = 25 ms +实际频率 = 40 Hz +``` + +这与现场 `feedback_interval mean≈25 ms` 完全一致。期望 5 ms 上报时,SDK +参数应为 `cycle=1`。 + +在 120 ms 反馈超时窗口内,25 ms 上报只有约 5 次发送机会;修正为 5 ms 后有 +约 24 次发送机会,能显著提高对偶发丢包和调度抖动的容忍度。若网络或 SDK +回调整体停顿超过 120 ms,仍应触发现有安全超时。 + +## 目标 + +- QP 位置收敛阈值由 1 mm 调整为 2 mm; +- `realtime_push_cycle_ms: 5` 实际配置成 SDK `cycle=1`,恢复 5 ms 上报; +- UDP 超时警告打印触发时的实际反馈年龄; +- 保持控制频率 90 Hz 和 `follow: false`; +- 保持 UDP 短超时保持、持续超时重同步、Grip 重使能、CANFD 恢复和故障锁存; +- 保留工作空间、圆柱、TCP、姿态和关节安全限制。 + +## 不在本次范围 + +- 不增加 QP 最大迭代次数; +- 不修改姿态收敛阈值; +- 不放宽 `command_timeout_sec=0.12`; +- 不修改 `feedback_resync_timeout_sec=0.5`; +- 不允许短超时后自动恢复 Grip; +- 不修改三份机械臂 YAML; +- 不修改 UDP 目标 IP、端口、线程模式或增加新连接; +- 不处理网卡、交换机或控制器固件问题; +- 不修改用户当前未提交的 `right_arm_rm75.yaml` 参数调整。 + +## 修改方案 + +### QP 收敛 + +在 `placo_ik_solver.py` 中只修改: + +```python +QP_POSITION_TOLERANCE_M = 2e-3 +``` + +30 次迭代、逐步关节安全校验、姿态阈值和未收敛异常格式保持不变。 + +### UDP 周期换算 + +保留公开参数 `realtime_push_cycle_ms` 的毫秒语义和“正数且为 5 ms 倍数”的现有 +校验。构造 SDK 配置时执行: + +```python +sdk_cycle = self._realtime_push_cycle_ms // 5 +``` + +示例: + +| 项目参数 | SDK `cycle` | 实际周期 | +|---:|---:|---:| +| 5 ms | 1 | 5 ms | +| 10 ms | 2 | 10 ms | +| 25 ms | 5 | 25 ms | + +启动日志仍打印毫秒值,避免把 SDK 内部单位暴露为用户配置。 + +### UDP 超时日志 + +首次进入短暂超时时打印: + +```text +right_rm75 UDP关节反馈超时(age=xxx.x ms),保持最后安全目标。 +``` + +日志只增加诊断值,不改变节流、保持目标、QP 停止和 Grip 重使能行为。 + +## 数据流与安全 + +正常路径: + +```text +YAML 5 ms +→ 适配器换算 SDK cycle=1 +→ 控制器约每 5 ms UDP 上报 +→ 回调校验并缓存 joint_position +→ 90 Hz 控制读取最新实际反馈 +→ 最多 30 次 QP,位置阈值 2 mm +→ 现有关节提前制动限幅 +→ rm_movej_canfd(..., follow=false) +``` + +反馈年龄超过 120 ms 时仍停止生成新目标和 QP,以 90 Hz 重发最后安全目标,并 +要求 Grip 松开后重新使能。达到 500 ms 时仍只尝试一次 +`rm_get_joint_degree()`;失败时仍停止并锁存故障。 + +## 测试与验证 + +自动测试: + +1. 增加 QP 1.5 mm 位置残差的行为测试,验证其被 2 mm 阈值接受; +2. 保留明显未收敛结果抛出异常的行为; +3. 修改 SDK 配置测试,验证项目 5 ms 参数传入 SDK 时为 `cycle=1`; +4. 增加 10 ms 到 `cycle=2` 的换算覆盖; +5. 验证 UDP 超时日志包含实际反馈年龄; +6. 运行 `xr_rm_teleop` 全部测试和姿态控制测试; +7. 运行 `colcon build --symlink-install`; +8. 使用 `arm_debug.launch.py arm:=right use_mock:=true`验证启动路径。 + +真机由用户验证: + +- 连续 timing 窗口中 `feedback_interval mean` 从约 25 ms 降到接近 5 ms; +- 正常遥操不再频繁出现 UDP 超时; +- 位置残差小于 2 mm 时不再出现 QP 未收敛警告; +- 真正超过 120 ms 的反馈中断仍打印带 `age` 的警告并执行现有安全保持; +- 若修正后仍频繁出现超过 120 ms 的中断,再依据 `age` 和间隔数据排查网络、 + SDK 回调或控制器固件,不继续盲目放宽超时。 + +## 文件范围 + +- 修改 `xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py`; +- 修改 `xr_rm_teleop/xr_rm_teleop/realman_adapter.py`; +- 修改 `xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py`; +- 修改相关现有测试; +- 新增本中文设计文档和后续中文实施计划; +- 不修改 YAML、launch、UI 或依赖。