acRealman_xr/docs/superpowers/plans/2026-08-13-rm75-qp-robustness.md

RM75 双臂采摘 QP 稳健性优化实施计划

For agentic workers: REQUIRED SUB-SKILL: Use superpowers:subagent-driven-development (recommended) or superpowers:executing-plans to implement this plan task-by-task. Steps use checkbox (- [ ]) syntax for tracking.

Goal: 在当前双臂严格六维遥操作链路中实现 QP 失败参考状态保持、J3 初始姿态软引导、J4 硬下限与软缓冲，以及按六维奇异值动态启用的可操作度任务。

Architecture: 保留 Placo 相对六维位姿主任务和下游关节速度/加速度限制。遥操作层将滤波结果作为候选值，只有 QP 求解和关节发送都成功后才提交；QP 求解器复用 Placo 现有 joints、half-space 和 manipulability 任务，不新增求解框架或依赖。

Tech Stack: Python 3.10、ROS2 Humble、Placo 0.9.4、NumPy、pytest、ament/colcon。

---

文件结构

• 修改 xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py：QP 失败状态和笛卡尔参考状态提交。
• 修改 xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py：J3、J4、动态六维可操作度和失败状态恢复。
• 修改 xr_rm_teleop/test/test_joint_control.py：失败不发送、不提交和发送失败保持测试。
• 修改 xr_rm_teleop/test/test_placo_transforms.py：辅助任务参数、激活函数和真实模型测试。
• 修改 xr_rm_teleop/test/test_initial_joint_pose.py：三份 YAML 的 QP 参数一致性测试。
• 修改 xr_rm_bringup/config/dual_arm_rm75.yaml：左右臂独立 QP 参数。
• 修改 xr_rm_bringup/config/left_arm_rm75.yaml：左臂 QP 参数。
• 修改 xr_rm_bringup/config/right_arm_rm75.yaml：右臂 QP 参数。

Task 1：QP 失败时不提交笛卡尔参考状态

Files:

• Modify: xr_rm_teleop/test/test_joint_control.py

• Modify: xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py

• Step 1：修改 QP 失败测试并增加候选滤波测试

把现有失败测试改为要求 _solve_joint_target() 返回 None，同时增加位置和姿态滤波只计算候选、不直接修改已提交状态的断言：

def test_qp_failure_returns_none_and_keeps_last_known_good_target() -> None:
    ...
    target = teleop._solve_joint_target(np.eye(4))
    assert target is None
    assert teleop._last_valid_joint_target == pytest.approx([0.1] * 7)


def test_target_filters_do_not_commit_candidate_state() -> None:
    teleop = object.__new__(SingleArmVelocityTeleop)
    teleop._filtered_target = [0.0, 0.0, 0.0]
    teleop._filtered_orientation_target = np.eye(3)
    teleop._target_filter_alpha = 0.5
    teleop._target_filter_alpha_fast = 0.5
    teleop._target_filter_fast_threshold_m = 1.0
    teleop._orientation_filter_alpha = 0.5

    position = teleop._filter_target([0.2, 0.0, 0.0])
    orientation = teleop._filter_orientation_target(
        _so3_exp(np.asarray([0.0, 0.0, 0.2]))
    )

    assert position == pytest.approx([0.1, 0.0, 0.0])
    assert teleop._filtered_target == pytest.approx([0.0, 0.0, 0.0])
    assert teleop._filtered_orientation_target == pytest.approx(np.eye(3))
    assert np.linalg.norm(_so3_log(orientation)) == pytest.approx(0.1)

• Step 2：运行新测试并确认按预期失败

Run:

cd /home/robot/WS_xr
source /opt/ros/humble/setup.bash
PYTEST_DISABLE_PLUGIN_AUTOLOAD=1 PYTHONPATH=src/xr_rm_teleop:$PYTHONPATH \
  /home/robot/miniconda3/envs/xr/bin/python -m pytest \
  src/xr_rm_teleop/test/test_joint_control.py \
  -k 'qp_failure or target_filters_do_not_commit' -q

Expected: FAIL；当前失败路径仍返回旧关节数组，滤波函数会立即修改成员状态。

• Step 3：实现最小失败保持逻辑

修改 _filter_target() 和 _filter_orientation_target() 只返回候选值，不直接写成员。 修改 _solve_joint_target() 在异常时返回 None，成功时也不提前更新 _last_valid_joint_target。控制周期只在结果非空时发送，并在发送成功后统一提交：

joint_target = self._solve_joint_target(target_pose)
sent = (
    joint_target is not None
    and self._send_joint_target(joint_target)
)
if sent:
    self._last_valid_joint_target = list(joint_target)
    self._filtered_target = list(filtered_target)
    self._filtered_orientation_target = filtered_orientation.copy()
    self._last_sent_target = sent_target
    self._last_sent_orientation = sent_orientation.copy()
    self._last_command_time = now
    self._stop_sent = False

失败时不调用 _send_joint_target()，因此不会把旧关节保持动作伪装成新 QP 成功；已 存在的指令超时和反馈故障保持逻辑不改变。

• Step 4：运行关节控制测试

Run:

cd /home/robot/WS_xr
source /opt/ros/humble/setup.bash
PYTEST_DISABLE_PLUGIN_AUTOLOAD=1 PYTHONPATH=src/xr_rm_teleop:$PYTHONPATH \
  /home/robot/miniconda3/envs/xr/bin/python -m pytest \
  src/xr_rm_teleop/test/test_joint_control.py -q

Expected: PASS。

Task 2：J3、J4 与动态六维可操作度

Files:

• Modify: xr_rm_teleop/test/test_placo_transforms.py

• Modify: xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py

• Step 1：写辅助任务激活和参数失败测试

增加纯激活函数测试：

def test_lower_margin_activation_is_clamped_and_linear() -> None:
    assert _lower_margin_activation(0.05, 0.01, 0.04) == 0.0
    assert _lower_margin_activation(0.025, 0.01, 0.04) == pytest.approx(0.5)
    assert _lower_margin_activation(0.005, 0.01, 0.04) == 1.0

增加真实左右臂求解器测试，构造时传入：

solver = PlacoIkSolver(
    str(DUAL_URDF_PATH),
    1.0 / 90.0,
    arm,
    j3_reference_deg=j3_reference_deg,
    j3_weight=1e-5,
    j4_min_deg=10.0,
    j4_warn_deg=25.0,
    j4_weight=1e-4,
    manipulability_sigma_stop=0.01,
    manipulability_sigma_warn=0.04,
    manipulability_weight=1e-4,
)

断言 J3 任务目标等于该侧参考角、J4 half-space 为 -q4 <= -10°，六维雅可比为 6x7 且奇异值有限。

• Step 2：运行新测试并确认按预期失败

Run:

cd /home/robot/WS_xr
source /opt/ros/humble/setup.bash
PYTEST_DISABLE_PLUGIN_AUTOLOAD=1 PYTHONPATH=src/xr_rm_teleop:$PYTHONPATH \
  /home/robot/miniconda3/envs/xr/bin/python -m pytest \
  src/xr_rm_teleop/test/test_placo_transforms.py \
  -k 'lower_margin_activation or auxiliary_qp_tasks' -q

Expected: FAIL；激活函数和构造参数尚不存在。

• Step 3：实现 Placo 辅助任务

新增 _lower_margin_activation(value, stop, warn)，并在构造器中验证有限参数及 j4_warn > j4_min、sigma_warn > sigma_stop > 0。复用 Placo 原生接口：

self._j3_task = self._solver.add_joints_task()
self._j3_task.set_joints({self._joint_names[2]: np.deg2rad(j3_reference_deg)})
self._j3_task.configure("j3_reference", "soft", j3_weight)

self._j4_task = self._solver.add_joints_task()
self._j4_task.set_joints({self._joint_names[3]: np.deg2rad(j4_warn_deg)})

matrix = np.zeros((1, self._robot.state.q.size))
matrix[0, self._q_offsets[3]] = -1.0
self._j4_constraint = self._solver.add_joint_space_half_spaces_constraint(
    matrix,
    np.asarray([-np.deg2rad(j4_min_deg)]),
)
self._j4_constraint.configure("j4_lower_bound", "hard")

self._manipulability_task = self._solver.add_manipulability_task(
    self._tcp_frame,
    "both",
    1.0,
)

每次数值迭代前，从 frame_jacobian(..., "local_world_aligned") 的当前臂 6x7 雅可比计算 sigma_min。J4 和可操作度任务分别使用线性夹紧激活系数重新配置软权重； J3 权重使用节点传入的左右臂独立配置。启用 Placo 原生关节限位，保留现有速度限位 和结果校验。

• Step 4：失败时恢复 Placo 到实际关节反馈

在 solve() 入口保存实际关节状态；任何求解异常或 30 次未收敛时，将活动臂关节 恢复到 _actual_joints 并更新运动学后重新抛出异常。测试制造不收敛，断言内部活动 关节未停留在失败迭代结果。

• Step 5：运行 Placo 测试

Run:

cd /home/robot/WS_xr
source /opt/ros/humble/setup.bash
PYTEST_DISABLE_PLUGIN_AUTOLOAD=1 PYTHONPATH=src/xr_rm_teleop:$PYTHONPATH \
  /home/robot/miniconda3/envs/xr/bin/python -m pytest \
  src/xr_rm_teleop/test/test_placo_transforms.py -q

Expected: PASS。

Task 3：同步节点和三份控制配置

Files:

• Modify: xr_rm_teleop/test/test_initial_joint_pose.py

• Modify: xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py

• Modify: xr_rm_bringup/config/dual_arm_rm75.yaml

• Modify: xr_rm_bringup/config/left_arm_rm75.yaml

• Modify: xr_rm_bringup/config/right_arm_rm75.yaml

• Step 1：写三份 YAML 一致性失败测试

扩展现有 YAML 参数化测试，断言左右臂分别为：

expected = {
    "left": {
        "qp_j3_reference_deg": 67.96,
        "qp_j3_weight": 1e-5,
    },
    "right": {
        "qp_j3_reference_deg": -89.57,
        "qp_j3_weight": 1e-4,
    },
}
shared = {
    "qp_j4_min_deg": 10.0,
    "qp_j4_warn_deg": 25.0,
    "qp_j4_weight": 1e-4,
    "qp_manipulability_sigma_stop": 0.01,
    "qp_manipulability_sigma_warn": 0.04,
    "qp_manipulability_weight": 1e-4,
}

同时断言单臂 YAML 与双臂同侧节点值一致。

• Step 2：运行配置测试并确认按预期失败

Run:

cd /home/robot/WS_xr
source /opt/ros/humble/setup.bash
PYTEST_DISABLE_PLUGIN_AUTOLOAD=1 PYTHONPATH=src/xr_rm_teleop:$PYTHONPATH \
  /home/robot/miniconda3/envs/xr/bin/python -m pytest \
  src/xr_rm_teleop/test/test_initial_joint_pose.py -q

Expected: FAIL；QP 参数尚未写入 YAML。

• Step 3：声明、读取并传入 QP 参数

节点声明上述八个 qp_* 参数，进行有限性和大小关系验证，并作为关键字参数传入 PlacoIkSolver。三份 YAML 同步写入相同共享参数，J3 只按左右臂设置不同参考角； 不修改 configure_safety_limits 和 move_to_initial_pose_on_connect。

• Step 4：运行配置和遥操作姿态测试

Run:

cd /home/robot/WS_xr
source /opt/ros/humble/setup.bash
PYTEST_DISABLE_PLUGIN_AUTOLOAD=1 PYTHONPATH=src/xr_rm_teleop:$PYTHONPATH \
  /home/robot/miniconda3/envs/xr/bin/python -m pytest \
  src/xr_rm_teleop/test/test_initial_joint_pose.py \
  src/xr_rm_teleop/test/test_orientation_control.py -q

Expected: PASS。

Task 4：完整验证和本地提交

Files:

• Verify all modified files.

• Step 1：运行遥操作包测试

Run:

cd /home/robot/WS_xr
source /opt/ros/humble/setup.bash
PYTEST_DISABLE_PLUGIN_AUTOLOAD=1 PYTHONPATH=src/xr_rm_teleop:$PYTHONPATH \
  /home/robot/miniconda3/envs/xr/bin/python -m pytest \
  src/xr_rm_teleop/test -q

Expected: 全部 PASS，无失败。

• Step 2：运行真实 URDF 左右臂 QP 冒烟测试

Run:

cd /home/robot/WS_xr
source /opt/ros/humble/setup.bash
PYTHONPATH=src/xr_rm_teleop:$PYTHONPATH /home/robot/miniconda3/envs/xr/bin/python \
  src/xr_rm_teleop/test/placo_ik_smoke.py \
  src/xr_rm_teleop/models/dual_rm75/Dual_arm.urdf

Expected: 左右臂保持位姿漂移和 1 cm 六维 QP 冒烟断言均通过。

• Step 3：构建 ROS2 工作空间

Run:

cd /home/robot/WS_xr
source /opt/ros/humble/setup.bash
colcon build --symlink-install

Expected: 所有工作空间包构建成功。

• Step 4：检查差异与安全配置

Run:

cd /home/robot/WS_xr/src
git diff --check
git diff --stat
rg -n "configure_safety_limits: true|move_to_initial_pose_on_connect: false" \
  xr_rm_bringup/config/{dual_arm_rm75,left_arm_rm75,right_arm_rm75}.yaml

Expected: 无空白错误，三份配置继续保留安全设置。

• Step 5：创建本地提交

规格文档和实施计划必须在同一个本地提交中；实现与测试一并纳入该提交，避免文档和 代码版本不一致：

git add \
  docs/superpowers/specs/2026-08-13-rm75-qp-robustness-design.md \
  docs/superpowers/plans/2026-08-13-rm75-qp-robustness.md \
  xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py \
  xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py \
  xr_rm_teleop/test/test_joint_control.py \
  xr_rm_teleop/test/test_placo_transforms.py \
  xr_rm_teleop/test/test_initial_joint_pose.py \
  xr_rm_bringup/config/dual_arm_rm75.yaml \
  xr_rm_bringup/config/left_arm_rm75.yaml \
  xr_rm_bringup/config/right_arm_rm75.yaml
git commit -m "feat: 优化双臂采摘QP稳健性"

禁止 git push、合并分支或连接真机。