353 lines
10 KiB
Markdown
353 lines
10 KiB
Markdown
# RM75 QP 与 UDP 反馈周期修复实施计划
|
||
|
||
> **供代理执行：** 必须使用 `superpowers:subagent-driven-development`（推荐）或
|
||
> `superpowers:executing-plans` 子技能，按任务逐项实施。步骤使用复选框
|
||
>（`- [ ]`）跟踪。
|
||
|
||
**目标：** 将 QP 位置收敛阈值调整为 2 mm，并把项目配置的毫秒周期正确换算为
|
||
睿尔曼 SDK 的 5 ms 周期单位，从根因上降低误触发 UDP 反馈超时的概率。
|
||
|
||
**架构：** 保持 ROS 参数和 YAML 中 `realtime_push_cycle_ms` 的毫秒语义，仅在
|
||
`RealManAdapter.connect()` 的 SDK 边界执行单位换算。QP 只调整现有位置收敛常量；
|
||
反馈超时状态机保持不变，只在首次超时日志中增加实际反馈年龄。
|
||
|
||
**技术栈：** Python 3.10、ROS2 Humble、pytest、ament/colcon、睿尔曼 Python
|
||
API2、Placo 0.9.4。
|
||
|
||
---
|
||
|
||
## 文件范围
|
||
|
||
- 修改 `xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py`：QP 位置收敛阈值。
|
||
- 修改 `xr_rm_teleop/test/test_placo_transforms.py`：1.5 mm 近收敛结果测试。
|
||
- 修改 `xr_rm_teleop/xr_rm_teleop/realman_adapter.py`：毫秒到 SDK 周期单位换算。
|
||
- 修改 `xr_rm_teleop/test/test_initial_joint_pose.py`：5 ms、10 ms 换算测试。
|
||
- 修改 `xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py`：超时日志增加年龄。
|
||
- 修改 `xr_rm_teleop/test/test_joint_control.py`：超时年龄日志测试。
|
||
- 不修改 YAML、launch、UI、消息定义或依赖。
|
||
|
||
## 测试环境
|
||
|
||
所有命令从工作空间根目录 `/home/robot/WS_xr` 执行：
|
||
|
||
```bash
|
||
source /opt/ros/humble/setup.bash
|
||
export PYTHONPATH=/home/robot/WS_xr/src/xr_rm_teleop:/home/robot/miniconda3/envs/xr/lib/python3.10/site-packages:/home/robot/miniconda3/envs/xr/lib/python3.10/site-packages/cmeel.prefix/lib/python3.10/site-packages:${PYTHONPATH}
|
||
```
|
||
|
||
### 任务 1：接受 2 mm 内的 QP 位置残差
|
||
|
||
**文件：**
|
||
|
||
- 修改：`xr_rm_teleop/test/test_placo_transforms.py`
|
||
- 修改：`xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py:14`
|
||
|
||
- [ ] **步骤 1：编写 1.5 mm 近收敛结果的失败测试**
|
||
|
||
在 `test_placo_transforms.py` 的 QP 测试附近增加：
|
||
|
||
```python
|
||
def test_qp_solve_accepts_position_error_within_two_millimeters() -> None:
|
||
solver = object.__new__(PlacoIkSolver)
|
||
solver._actual_joints = np.zeros(7)
|
||
solver._robot = SimpleNamespace(
|
||
state=SimpleNamespace(q=np.zeros(14))
|
||
)
|
||
solver._frame_task = SimpleNamespace(T_world_frame=None)
|
||
solver._target_errors = lambda: (1.5e-3, 0.0)
|
||
|
||
result = solver.solve(np.eye(4))
|
||
|
||
assert result == pytest.approx([0.0] * 7)
|
||
```
|
||
|
||
并在文件顶部加入现有标准库类型：
|
||
|
||
```python
|
||
from types import SimpleNamespace
|
||
```
|
||
|
||
同时从 `placo_ik_solver` 导入现有收敛常量：
|
||
|
||
```python
|
||
from xr_rm_teleop.placo_ik_solver import (
|
||
QP_POSITION_TOLERANCE_M,
|
||
PlacoIkSolver,
|
||
_validated_transform,
|
||
)
|
||
```
|
||
|
||
将真实 Placo 可达目标测试的位置断言改为引用同一收敛常量：
|
||
|
||
```python
|
||
assert position_error <= QP_POSITION_TOLERANCE_M
|
||
```
|
||
|
||
再增加超过 2 mm 时仍拒绝结果的边界测试：
|
||
|
||
```python
|
||
def test_qp_solve_rejects_position_error_above_two_millimeters() -> None:
|
||
solver = object.__new__(PlacoIkSolver)
|
||
solver._actual_joints = np.zeros(7)
|
||
solver._robot = SimpleNamespace(
|
||
state=SimpleNamespace(q=np.zeros(14)),
|
||
update_kinematics=lambda: None,
|
||
)
|
||
solver._frame_task = SimpleNamespace(T_world_frame=None)
|
||
solver._solver = SimpleNamespace(solve=lambda update: None)
|
||
solver._validate_result = lambda result, previous: None
|
||
solver._target_errors = lambda: (2.1e-3, 0.0)
|
||
|
||
with pytest.raises(RuntimeError, match="QP did not converge after 30"):
|
||
solver.solve(np.eye(4))
|
||
```
|
||
|
||
- [ ] **步骤 2：运行测试并确认当前实现失败**
|
||
|
||
```bash
|
||
python3 -m pytest src/xr_rm_teleop/test/test_placo_transforms.py::test_qp_solve_accepts_position_error_within_two_millimeters -q
|
||
```
|
||
|
||
预期：失败；当前 1 mm 阈值不会直接接收 1.5 mm 残差，测试对象缺少后续 QP
|
||
求解器。
|
||
|
||
- [ ] **步骤 3：最小修改 QP 位置阈值**
|
||
|
||
在 `placo_ik_solver.py` 修改现有常量：
|
||
|
||
```python
|
||
QP_POSITION_TOLERANCE_M = 2e-3
|
||
```
|
||
|
||
保留 `QP_MAX_ITERATIONS = 30`、姿态阈值、有限值检查、关节位置和速度限制不变。
|
||
|
||
- [ ] **步骤 4：运行局部测试并确认通过**
|
||
|
||
```bash
|
||
python3 -m pytest src/xr_rm_teleop/test/test_placo_transforms.py -q
|
||
```
|
||
|
||
预期：该文件全部测试通过；1.5 mm 残差被接受，2.1 mm 残差仍在 30 次后被拒绝。
|
||
|
||
- [ ] **步骤 5：提交 QP 修改**
|
||
|
||
```bash
|
||
git add src/xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py src/xr_rm_teleop/test/test_placo_transforms.py
|
||
git commit -m "fix: 放宽 RM75 QP 位置收敛阈值"
|
||
```
|
||
|
||
### 任务 2：修正睿尔曼 UDP 实时上报周期单位
|
||
|
||
**文件：**
|
||
|
||
- 修改：`xr_rm_teleop/test/test_initial_joint_pose.py`
|
||
- 修改：`xr_rm_teleop/xr_rm_teleop/realman_adapter.py:190-196`
|
||
|
||
- [ ] **步骤 1：将现有连接测试改为周期换算参数化测试**
|
||
|
||
将
|
||
`test_connect_configures_udp_feedback_and_waits_for_first_frame` 改为：
|
||
|
||
```python
|
||
@pytest.mark.parametrize(
|
||
("cycle_ms", "sdk_cycle"),
|
||
[(5, 1), (10, 2)],
|
||
)
|
||
def test_connect_converts_udp_feedback_cycle_to_sdk_units(
|
||
monkeypatch,
|
||
cycle_ms,
|
||
sdk_cycle,
|
||
) -> None:
|
||
fake_sdk = _install_fake_sdk(monkeypatch)
|
||
adapter = RealManAdapter(
|
||
"127.0.0.1",
|
||
8080,
|
||
0,
|
||
"192.168.192.148",
|
||
8090,
|
||
realtime_push_cycle_ms=cycle_ms,
|
||
configure_safety_limits=False,
|
||
)
|
||
|
||
adapter.connect()
|
||
|
||
arm = fake_sdk.RoboticArm.instance
|
||
assert arm is not None
|
||
assert arm.config.args == (
|
||
sdk_cycle,
|
||
True,
|
||
8090,
|
||
0,
|
||
"192.168.192.148",
|
||
)
|
||
assert arm.callback is adapter._realtime_callback
|
||
assert adapter.get_latest_joint_state() is not None
|
||
assert not hasattr(adapter, "_feedback_thread")
|
||
```
|
||
|
||
- [ ] **步骤 2：运行参数化测试并确认当前实现失败**
|
||
|
||
```bash
|
||
python3 -m pytest src/xr_rm_teleop/test/test_initial_joint_pose.py::test_connect_converts_udp_feedback_cycle_to_sdk_units -q
|
||
```
|
||
|
||
预期：两个用例均失败；当前代码把 5 和 10 直接传给 SDK，而预期分别为 1 和 2。
|
||
|
||
- [ ] **步骤 3：在 SDK 边界执行单位换算**
|
||
|
||
在 `RealManAdapter.connect()` 创建 `rm_realtime_push_config_t` 时修改第一个参数：
|
||
|
||
```python
|
||
config = rm_realtime_push_config_t(
|
||
self._realtime_push_cycle_ms // 5,
|
||
True,
|
||
self._realtime_push_port,
|
||
0,
|
||
self._realtime_push_host_ip,
|
||
)
|
||
```
|
||
|
||
构造函数已有“正的 5 ms 整数倍”校验，因此不新增辅助函数或重复校验。启动日志继续
|
||
输出项目参数的真实毫秒值。
|
||
|
||
- [ ] **步骤 4：运行适配器测试并确认通过**
|
||
|
||
```bash
|
||
python3 -m pytest src/xr_rm_teleop/test/test_initial_joint_pose.py -q
|
||
```
|
||
|
||
预期：该文件全部测试通过；mock 测试仍不依赖厂商 SDK。
|
||
|
||
- [ ] **步骤 5：提交周期换算修改**
|
||
|
||
```bash
|
||
git add src/xr_rm_teleop/xr_rm_teleop/realman_adapter.py src/xr_rm_teleop/test/test_initial_joint_pose.py
|
||
git commit -m "fix: 修正 RM75 UDP 上报周期单位"
|
||
```
|
||
|
||
### 任务 3：在 UDP 超时日志中打印反馈年龄
|
||
|
||
**文件：**
|
||
|
||
- 修改：`xr_rm_teleop/test/test_joint_control.py`
|
||
- 修改：`xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py:1036-1044`
|
||
|
||
- [ ] **步骤 1：扩展现有短暂超时测试，捕获并断言日志**
|
||
|
||
在
|
||
`test_short_udp_timeout_repeats_last_limited_target_without_query` 中创建遥操对象后加入：
|
||
|
||
```python
|
||
warnings = []
|
||
teleop.get_logger = lambda: SimpleNamespace(
|
||
warn=lambda message: warnings.append(message)
|
||
)
|
||
```
|
||
|
||
在现有断言末尾加入：
|
||
|
||
```python
|
||
assert warnings == [
|
||
"right_rm75 UDP关节反馈超时（age=200.0 ms），保持最后安全目标。"
|
||
]
|
||
```
|
||
|
||
- [ ] **步骤 2：运行测试并确认当前实现失败**
|
||
|
||
```bash
|
||
python3 -m pytest src/xr_rm_teleop/test/test_joint_control.py::test_short_udp_timeout_repeats_last_limited_target_without_query -q
|
||
```
|
||
|
||
预期：失败；当前日志中没有 `age=200.0 ms`。
|
||
|
||
- [ ] **步骤 3：给首次超时警告增加实际反馈年龄**
|
||
|
||
在 `_handle_stale_joint_feedback()` 中仅修改现有警告：
|
||
|
||
```python
|
||
self.get_logger().warn(
|
||
f"{self._arm_name} UDP关节反馈超时"
|
||
f"（age={age * 1000.0:.1f} ms），保持最后安全目标。"
|
||
)
|
||
```
|
||
|
||
不改变 `_joint_feedback_ready` 的一次性日志条件、最后安全目标重发、500 ms
|
||
重新同步、Grip 重使能和故障锁存逻辑。
|
||
|
||
- [ ] **步骤 4：运行关节控制测试并确认通过**
|
||
|
||
```bash
|
||
python3 -m pytest src/xr_rm_teleop/test/test_joint_control.py -q
|
||
```
|
||
|
||
预期：该文件全部测试通过。
|
||
|
||
- [ ] **步骤 5：提交诊断日志修改**
|
||
|
||
```bash
|
||
git add src/xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py src/xr_rm_teleop/test/test_joint_control.py
|
||
git commit -m "fix: 补充 RM75 UDP 超时反馈年龄"
|
||
```
|
||
|
||
### 任务 4：完整回归与 mock 启动验证
|
||
|
||
**文件：**
|
||
|
||
- 不修改生产代码。
|
||
|
||
- [ ] **步骤 1：运行 `xr_rm_teleop` 全部测试**
|
||
|
||
```bash
|
||
python3 -m pytest src/xr_rm_teleop/test -q
|
||
```
|
||
|
||
预期：全部通过，无失败或错误。
|
||
|
||
- [ ] **步骤 2：单独运行姿态控制测试**
|
||
|
||
```bash
|
||
python3 -m pytest src/xr_rm_teleop/test/test_orientation_control.py -q
|
||
```
|
||
|
||
预期：全部通过。
|
||
|
||
- [ ] **步骤 3：构建完整工作空间**
|
||
|
||
```bash
|
||
colcon build --symlink-install
|
||
```
|
||
|
||
预期：`xr_rm_interfaces`、`xr_rm_input`、`xr_rm_teleop`、`xr_rm_bringup`
|
||
全部构建成功。
|
||
|
||
- [ ] **步骤 4：使用右臂 mock 启动统一 launch**
|
||
|
||
```bash
|
||
timeout 15s ros2 launch xr_rm_bringup arm_debug.launch.py arm:=right use_mock:=true
|
||
```
|
||
|
||
预期：节点正常启动，无 Python 异常或厂商 SDK 导入错误；因 `timeout` 主动终止，
|
||
命令退出码可以是 124。禁止将 `use_mock` 改为 `false`。
|
||
|
||
- [ ] **步骤 5：检查最终差异**
|
||
|
||
```bash
|
||
git status --short
|
||
git diff HEAD~3 --check
|
||
git diff HEAD~3 --stat
|
||
```
|
||
|
||
预期：除本实施计划文档外，代码差异仅包含计划列出的 6 个代码/测试文件；无空白
|
||
错误，不包含 YAML、launch、UI、依赖或用户其他改动。
|
||
|
||
## 真机验收
|
||
|
||
自动验证完成后，由用户在安全条件下启动真机右臂模式并观察：
|
||
|
||
1. `feedback_interval mean` 从约 25 ms 降到接近 5 ms；
|
||
2. 正常遥操不再频繁触发 `UDP关节反馈超时`；
|
||
3. 反馈确实超过 120 ms 时，日志显示实际 `age`，且仍保持最后安全目标；
|
||
4. 位置残差小于 2 mm 时不再出现 QP 未收敛警告；
|
||
5. 若仍频繁出现超过 120 ms 的中断，保留当前阈值，依据日志继续排查网络、SDK
|
||
回调或控制器固件。
|