docs: 记录 RM75 QP 与 UDP 周期修复设计

2026-07-30 18:04:49 +08:00
parent 4f6981d08b
commit 0df9e9bcfc
1 changed files with 176 additions and 0 deletions
@@ -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 或依赖。