YikaiFu-cart/acRealman_xr
1
1
Fork 1
Code Issues Pull Requests Packages Projects Releases Wiki Activity
Files
7b2caf301241b98b97a0a9e2ac5bf2b8d20692a1
acRealman_xr/docs/superpowers/plans/2026-07-29-rm75-qp-convergence.md
T

13 KiB
Raw Blame History

RM75 QP 收敛优化实施计划

执行要求： 使用 superpowers:executing-plans 逐项执行。用户未授权 subagent、独立worktree或本地分支，因此本计划只允许当前会话内联实施。所有 步骤使用复选框跟踪。

目标： 将当前每周期单步QP改为有界迭代QP，使低跟随RM75在手柄移动10 cm 后约1秒内稳定到位，并消除由近距离台阶目标造成的持续轻微晃动。

实现方式： 每个正常控制周期仍先用UDP实际关节角同步Placo，然后在一次 PlacoIkSolver.solve()内部最多迭代30次，提前达到1 mm位置误差和0.005 rad 姿态误差即返回。最终关节解继续经过现有90 Hz关节速度与加速度限幅后，以 follow=false发送；不增加预测状态、线程、连接、依赖或配置参数。

技术栈： Python 3.10、ROS2 Humble、Placo 0.9.4、NumPy、pytest、 ament/colcon。

设计文档： docs/superpowers/specs/2026-07-29-rm75-qp-convergence-design.md

仓库与安全约束

  • 构建、测试和启动命令在 /home/robot/WS_xr 执行。
  • Git命令在 /home/robot/WS_xr/src 执行。
  • 每次构建、测试或启动前执行 source /opt/ros/humble/setup.bash。
  • 不自动提交、推送、创建分支或worktree。
  • 不连接真机，不发送真实CANFD，不移动机械臂，不操作夹爪。
  • 只通过 arm_debug.launch.py arm:=right use_mock:=true进行启动验证。
  • 不修改三份机械臂YAML、RealMan适配器、launch、UI、依赖或公开入口。
  • 保留工作空间、圆柱、TCP速度、姿态速度、关节速度、关节加速度、反馈超时、 CANFD恢复、Grip重新使能和安全停止逻辑。

文件范围

  • 修改 xr_rm_teleop/test/test_placo_transforms.py
    • 增加真实Placo 7 cm目标收敛回归测试。
  • 修改 xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py
    • 增加固定上限、提前收敛和逐步安全校验。

不需要修改 single_arm_velocity_teleop.py；现有 _solve_joint_target() 已负责 QP异常时打印限频警告并保持上一组安全关节目标，现有 _limit_joint_command_step() 已负责最终90 Hz真实命令限速。

任务一：用真实Placo复现单步QP不收敛

修改文件：

  • xr_rm_teleop/test/test_placo_transforms.py

  • 步骤1：增加测试辅助函数

在文件顶部增加：

import math

在现有URDF结构测试之后增加：

def _rm75_placo_solver() -> tuple[PlacoIkSolver, list[float]]:
    pytest.importorskip("placo")
    urdf_path = (
        Path(__file__).resolve().parents[1]
        / "models"
        / "rm75_omnipicker"
        / "urdf"
        / "RM75-B_OmniPicker_fixed.urdf"
    )
    joints = [
        math.radians(value)
        for value in [
            -90.14,
            3.76,
            -86.89,
            87.89,
            -96.53,
            -79.62,
            -90.04,
        ]
    ]
    return PlacoIkSolver(str(urdf_path), 1.0 / 90.0), joints

importorskip()只让没有Placo的普通系统Python跳过真模型用例；下面的RED/GREEN 命令会显式加入项目现有Placo 0.9.4路径，因此该用例必须实际执行而不能跳过。

  • 步骤2：增加7 cm目标收敛测试

增加：

def test_qp_solve_converges_to_reachable_tcp_target() -> None:
    solver, joints = _rm75_placo_solver()
    start_pose = solver.update_joint_state(joints)
    target_pose = start_pose.copy()
    target_pose[0, 3] += 0.07

    result = solver.solve(target_pose)
    reached_pose = solver.update_joint_state(result)
    position_error = np.linalg.norm(
        target_pose[:3, 3] - reached_pose[:3, 3]
    )
    rotation_delta = (
        target_pose[:3, :3] @ reached_pose[:3, :3].T
    )
    orientation_error = math.acos(
        float(
            np.clip(
                (np.trace(rotation_delta) - 1.0) * 0.5,
                -1.0,
                1.0,
            )
        )
    )

    assert position_error <= 1e-3
    assert orientation_error <= 5e-3

该测试验证一次公开 solve() 调用返回当前TCP目标对应的收敛关节解，而不是验证 内部迭代次数。

  • 步骤3：运行测试并确认RED
source /opt/ros/humble/setup.bash
export RM75_PLACO_TEST_PATH="/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="${RM75_PLACO_TEST_PATH}:${PYTHONPATH:-}" \
  python3 -m pytest \
  src/xr_rm_teleop/test/test_placo_transforms.py::test_qp_solve_converges_to_reachable_tcp_target \
  -v

预期：测试以位置误差约0.063 m大于0.001 m失败，证明当前单步QP确实不能在一次 调用内给出收敛关节目标。测试不得因导入错误或跳过而结束。

任务二：实现有界迭代QP

修改文件：

  • xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py

  • 步骤1：增加固定收敛常量

把模块说明改为：

"""RM75 的 Placo 0.9.4 有界迭代 QP 逆解。"""

在现有常量后增加：

QP_MAX_ITERATIONS = 30
QP_POSITION_TOLERANCE_M = 1e-3
QP_ORIENTATION_TOLERANCE_RAD = 5e-3

这些值是本次已确认的算法边界，不新增ROS参数。

  • 步骤2：增加任务误差读取

在 solve() 前增加：

def _target_errors(self) -> tuple[float, float]:
    position_task = self._frame_task.position()
    orientation_task = self._frame_task.orientation()
    position_task.update()
    orientation_task.update()
    return (
        float(position_task.error_norm()),
        float(orientation_task.error_norm()),
    )

Placo在 solve(True) 后只更新关节状态；先更新机器人运动学，再显式更新两个任务， 确保 error_norm()对应当前迭代后的状态而不是前一迭代。

  • 步骤3：把单步求解改为最多30次且提前收敛

用以下实现替换现有 solve()：

def solve(self, target_tool_pose: np.ndarray) -> list[float]:
    if self._actual_joints is None:
        raise RuntimeError(
            "joint state must be initialized before QP solve"
        )
    self._frame_task.T_world_frame = _validated_transform(
        target_tool_pose
    )
    result = np.asarray(
        self._robot.state.q[RM75_Q_SLICE],
        dtype=float,
    ).copy()
    position_error, orientation_error = self._target_errors()
    if (
        position_error <= QP_POSITION_TOLERANCE_M
        and orientation_error <= QP_ORIENTATION_TOLERANCE_RAD
    ):
        return result.tolist()

    for _ in range(QP_MAX_ITERATIONS):
        previous = result
        self._solver.solve(True)
        self._robot.update_kinematics()
        result = np.asarray(
            self._robot.state.q[RM75_Q_SLICE],
            dtype=float,
        ).copy()
        self._validate_result(result, previous)
        position_error, orientation_error = self._target_errors()
        if (
            position_error <= QP_POSITION_TOLERANCE_M
            and orientation_error <= QP_ORIENTATION_TOLERANCE_RAD
        ):
            return result.tolist()

    raise RuntimeError(
        "QP did not converge after "
        f"{QP_MAX_ITERATIONS} iterations: "
        f"position_error={position_error:.6f} m, "
        f"orientation_error={orientation_error:.6f} rad"
    )

目标已到达时直接返回当前关节角，避免静止时进行不必要的数值迭代。

  • 步骤4：让速度校验针对每次数值迭代

把 _validate_result() 签名改为：

def _validate_result(
    self,
    result: np.ndarray,
    reference: np.ndarray | None = None,
) -> None:

保留现有有限值和关节位置检查，把速度检查替换为：

if reference is None:
    reference = self._actual_joints
if reference is None:
    raise RuntimeError("joint state has not been initialized")
reference = np.asarray(reference, dtype=float)
if reference.shape != (7,) or not np.isfinite(reference).all():
    raise ValueError("QP reference must contain 7 finite values")
max_step = self._velocity_limits * self._dt + 1e-9
if np.any(np.abs(result - reference) > max_step):
    raise ValueError(
        "QP result violates RM75 one-cycle velocity limits"
    )

这样每次内部数值迭代继续满足Placo的URDF关节速度边界；最终收敛解仍由节点现有 _limit_joint_command_step()按真实90 Hz周期限制后才发送。

  • 步骤5：运行目标测试并确认GREEN
source /opt/ros/humble/setup.bash
export RM75_PLACO_TEST_PATH="/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="${RM75_PLACO_TEST_PATH}:${PYTHONPATH:-}" \
  python3 -m pytest \
  src/xr_rm_teleop/test/test_placo_transforms.py::test_qp_solve_converges_to_reachable_tcp_target \
  src/xr_rm_teleop/test/test_placo_transforms.py::test_qp_result_rejects_nan_position_and_velocity_violations \
  -v

预期：两个测试通过；真实Placo用例不被跳过。

  • 步骤6：运行Placo变换测试文件
source /opt/ros/humble/setup.bash
export RM75_PLACO_TEST_PATH="/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="${RM75_PLACO_TEST_PATH}:${PYTHONPATH:-}" \
  python3 -m pytest \
  src/xr_rm_teleop/test/test_placo_transforms.py \
  -v

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

任务三：回归、安全和mock验证

验证范围：

  • xr_rm_teleop全部测试；

  • ROS2工作空间构建；

  • 统一launch的右臂mock启动；

  • 最终差异与安全配置审计。

  • 步骤1：运行遥操作包全部测试

source /opt/ros/humble/setup.bash
export RM75_PLACO_TEST_PATH="/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="${RM75_PLACO_TEST_PATH}:${PYTHONPATH:-}" \
  python3 -m pytest src/xr_rm_teleop/test -v

预期：全部测试通过，真实Placo收敛用例被执行。

  • 步骤2：按项目规则单独运行姿态控制测试
source /opt/ros/humble/setup.bash
python3 -m pytest \
  src/xr_rm_teleop/test/test_orientation_control.py \
  -v

预期：全部通过。

  • 步骤3：构建ROS2工作空间
source /opt/ros/humble/setup.bash
colcon build --symlink-install

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

  • 步骤4：通过统一入口进行右臂mock启动验证
source /opt/ros/humble/setup.bash
source install/setup.bash
if timeout --signal=INT 10s ros2 launch \
  xr_rm_bringup arm_debug.launch.py \
  arm:=right use_mock:=true udp_port:=15123
then
  true
else
  launch_status=$?
  test "$launch_status" -eq 124
fi

预期：

  • udp_controller_receiver和single_arm_velocity_teleop正常启动；

  • 节点报告 dt=0.0111s、follow=False；

  • mock关节初始化成功；

  • 不导入RealMan SDK，不建立真机连接，不发送CANFD；

  • 10秒后仅由 timeout结束。

  • 步骤5：最终差异和安全审计

在 /home/robot/WS_xr/src 执行：

git diff --check
git status --short
git diff -- \
  xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py \
  xr_rm_teleop/test/test_placo_transforms.py
rg -n \
  "control_rate_hz|follow:|configure_safety_limits|move_to_initial_pose_on_connect" \
  xr_rm_bringup/config/dual_arm_rm75.yaml \
  xr_rm_bringup/config/left_arm_rm75.yaml \
  xr_rm_bringup/config/right_arm_rm75.yaml

预期：

  • 生产代码只修改Placo求解器；
  • 测试只增加真实模型收敛验证；
  • 三份配置继续使用90 Hz、follow: false、 configure_safety_limits: true和 move_to_initial_pose_on_connect: false；
  • 不改变此前由用户保留的 AGENTS.md 修改；
  • 不自动提交或推送。

真机交接验收

Codex不执行本节。自动验证全部通过后，由用户在安全工作区使用：

ros2 launch xr_rm_bringup arm_debug.launch.py \
  arm:=right use_mock:=false

验收步骤：

  1. 急停可用、Grip松开、工作区无人后启动。
  2. 按住Grip，快速移动手柄约10 cm后保持不动。
  3. 机械臂应在约1秒内稳定到位，无持续肉眼可见晃动。
  4. 连续观察四个5秒 timing 窗口，total max均低于11.111 ms。
  5. 不应出现QP未收敛、反馈超时、CANFD错误或故障锁存日志。
  6. 松开Grip后机械臂按现有逻辑安全停止。

若任一窗口 total max达到或超过11.111 ms，或机械臂出现明显振荡，立即松开 Grip并停止测试，把完整timing和错误日志返回后再调整；不得直接提高控制频率、 关闭限速或改成高跟随。

Reference in New Issue View Git Blame Copy Permalink