5.7 KiB
RM75 QP 阈值与 UDP 反馈周期修复设计
背景
右臂真机在 90 Hz、follow: false 遥操过程中频繁出现两类警告：
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 次，并要求：
位置误差 <= 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。
当前适配器把这个值原样传给：
rm_realtime_push_config_t(cycle, ...)
但睿尔曼 SDK 的 cycle 单位不是毫秒，而是 5 ms 的倍数。因此：
当前传入 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实际配置成 SDKcycle=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 中只修改：
QP_POSITION_TOLERANCE_M = 2e-3
30 次迭代、逐步关节安全校验、姿态阈值和未收敛异常格式保持不变。
UDP 周期换算
保留公开参数 realtime_push_cycle_ms 的毫秒语义和“正数且为 5 ms 倍数”的现有
校验。构造 SDK 配置时执行：
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 超时日志
首次进入短暂超时时打印：
right_rm75 UDP关节反馈超时（age=xxx.x ms），保持最后安全目标。
日志只增加诊断值，不改变节流、保持目标、QP 停止和 Grip 重使能行为。
数据流与安全
正常路径：
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()；失败时仍停止并锁存故障。
测试与验证
自动测试：
- 增加 QP 1.5 mm 位置残差的行为测试，验证其被 2 mm 阈值接受；
- 保留明显未收敛结果抛出异常的行为；
- 修改 SDK 配置测试，验证项目 5 ms 参数传入 SDK 时为
cycle=1； - 增加 10 ms 到
cycle=2的换算覆盖； - 验证 UDP 超时日志包含实际反馈年龄；
- 运行
xr_rm_teleop全部测试和姿态控制测试； - 运行
colcon build --symlink-install； - 使用
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 或依赖。