feat: 优化双臂采摘QP稳健性

2026-08-13 10:23:35 +08:00
parent 807374c9fd
commit 7a5c27d6b9
10 changed files with 1078 additions and 43 deletions
@@ -0,0 +1,372 @@
+# 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`、合并分支或连接真机。