177 lines
5.7 KiB
Markdown
177 lines
5.7 KiB
Markdown
# 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 或依赖。
|