# 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 回调或控制器固件。