# 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`，同时增加位置和姿态滤波只计算候选、不直接修改已提交状态的断言：

```python
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:

```bash
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`。控制周期只在结果非空时发送，并在发送成功后统一提交：

```python
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:

```bash
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：写辅助任务激活和参数失败测试**

增加纯激活函数测试：

```python
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
```

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

```python
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:

```bash
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 原生接口：

```python
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:

```bash
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 参数化测试，断言左右臂分别为：

```python
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:

```bash
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:

```bash
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:

```bash
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:

```bash
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:

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

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

- [ ] **Step 4：检查差异与安全配置**

Run:

```bash
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：创建本地提交**

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

```bash
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`、合并分支或连接真机。