Files

T

YikaiFu-cart bbfb306903 docs: 记录 RM75 QP 与 UDP 修复计划

2026-07-30 18:14:47 +08:00

10 KiB

Raw Permalink Blame History

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 执行：

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 测试附近增加：

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)

并在文件顶部加入现有标准库类型：

from types import SimpleNamespace

同时从 placo_ik_solver 导入现有收敛常量：

from xr_rm_teleop.placo_ik_solver import (
    QP_POSITION_TOLERANCE_M,
    PlacoIkSolver,
    _validated_transform,
)

将真实 Placo 可达目标测试的位置断言改为引用同一收敛常量：

assert position_error <= QP_POSITION_TOLERANCE_M

再增加超过 2 mm 时仍拒绝结果的边界测试：

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：运行测试并确认当前实现失败

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 修改现有常量：

QP_POSITION_TOLERANCE_M = 2e-3

保留 QP_MAX_ITERATIONS = 30、姿态阈值、有限值检查、关节位置和速度限制不变。

步骤 4：运行局部测试并确认通过

python3 -m pytest src/xr_rm_teleop/test/test_placo_transforms.py -q

预期：该文件全部测试通过；1.5 mm 残差被接受，2.1 mm 残差仍在 30 次后被拒绝。

步骤 5：提交 QP 修改

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 改为：

@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：运行参数化测试并确认当前实现失败

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 时修改第一个参数：

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：运行适配器测试并确认通过

python3 -m pytest src/xr_rm_teleop/test/test_initial_joint_pose.py -q

预期：该文件全部测试通过；mock 测试仍不依赖厂商 SDK。

步骤 5：提交周期换算修改

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 中创建遥操对象后加入：

warnings = []
teleop.get_logger = lambda: SimpleNamespace(
    warn=lambda message: warnings.append(message)
)

在现有断言末尾加入：

assert warnings == [
    "right_rm75 UDP关节反馈超时（age=200.0 ms），保持最后安全目标。"
]

步骤 2：运行测试并确认当前实现失败

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() 中仅修改现有警告：

self.get_logger().warn(
    f"{self._arm_name} UDP关节反馈超时"
    f"（age={age * 1000.0:.1f} ms），保持最后安全目标。"
)

不改变 _joint_feedback_ready 的一次性日志条件、最后安全目标重发、500 ms 重新同步、Grip 重使能和故障锁存逻辑。

步骤 4：运行关节控制测试并确认通过

python3 -m pytest src/xr_rm_teleop/test/test_joint_control.py -q

预期：该文件全部测试通过。

步骤 5：提交诊断日志修改

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 全部测试

python3 -m pytest src/xr_rm_teleop/test -q

预期：全部通过，无失败或错误。

步骤 2：单独运行姿态控制测试

python3 -m pytest src/xr_rm_teleop/test/test_orientation_control.py -q

预期：全部通过。

步骤 3：构建完整工作空间

colcon build --symlink-install

预期：xr_rm_interfaces、xr_rm_input、xr_rm_teleop、xr_rm_bringup 全部构建成功。

步骤 4：使用右臂 mock 启动统一 launch

timeout 15s ros2 launch xr_rm_bringup arm_debug.launch.py arm:=right use_mock:=true

预期：节点正常启动，无 Python 异常或厂商 SDK 导入错误；因 timeout 主动终止，命令退出码可以是 124。禁止将 use_mock 改为 false。

步骤 5：检查最终差异

git status --short
git diff HEAD~3 --check
git diff HEAD~3 --stat

预期：除本实施计划文档外，代码差异仅包含计划列出的 6 个代码/测试文件；无空白错误，不包含 YAML、launch、UI、依赖或用户其他改动。

真机验收

自动验证完成后，由用户在安全条件下启动真机右臂模式并观察：

feedback_interval mean 从约 25 ms 降到接近 5 ms；
正常遥操不再频繁触发 UDP关节反馈超时；
反馈确实超过 120 ms 时，日志显示实际 age，且仍保持最后安全目标；
位置残差小于 2 mm 时不再出现 QP 未收敛警告；
若仍频繁出现超过 120 ms 的中断，保留当前阈值，依据日志继续排查网络、SDK 回调或控制器固件。

10 KiB Raw Permalink Blame History Unescape Escape

RM75 QP 与 UDP 反馈周期修复实施计划

文件范围

测试环境

任务 1：接受 2 mm 内的 QP 位置残差

任务 2：修正睿尔曼 UDP 实时上报周期单位

任务 3：在 UDP 超时日志中打印反馈年龄

任务 4：完整回归与 mock 启动验证

真机验收

10 KiB

Raw Permalink Blame History