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`、合并分支或连接真机。
@@ -0,0 +1,175 @@
+# RM75 双臂采摘 QP 稳健性优化方案概述
+
+## 1. 目标与边界
+
+本方案面向当前双臂机器人从初始位姿向机器人公共坐标系 `+Y` 前方采摘，再移动到
+本侧机械臂初始 TCP 正下方约 40 cm、位于底盘车上的收集筐这一流程。首要目标是：
+
+- 保持手柄给出的 TCP 位置和姿态严格参与六维逆解；
+- 减少奇异点附近的构型恶化、QP 不收敛和连续失败；
+- QP 失败时保持上一安全关节解，并且不提交本周期笛卡尔参考状态；
+- 保留现有工作空间、速度、加速度、指令超时和安全停止限制。
+
+本轮不加入自动采摘状态机、自动放松姿态或真机自动运动，不取消现有安全限制。
+
+此前离线扫描中所有候选均未通过完整轨迹硬门槛，因此不能把扫描得到的左臂 34°、
+右臂 0°写成“最优 J3”。仿真只能说明：两臂 `q4 >= 10°` 均保持正余量，J4 的 10°
+硬下限不是当次 QP 失败的直接原因。
+
+## 2. 更新后的 QP 目标
+
+主任务和辅助任务写为：
+
+\[
+\begin{aligned}
+\min_{\Delta q}\quad
+&\left\|J_p\Delta q-e_p\right\|_{W_p}^2
+\left\|J_R\Delta q-e_R\right\|_{W_R}^2 \\
+&+\lambda\left\|\Delta q\right\|^2
+-w_m\alpha_m(\sigma)\nabla m_6(q)^T\Delta q \\
+&+w_3\left(q_3+\Delta q_3-q_{3,\mathrm{ref}}\right)^2 \\
+&+w_4\alpha_4(q_4)
+  \left[q_{4,\mathrm{warn}}-(q_4+\Delta q_4)\right]_+^2，
+\end{aligned}
+\]
+
+其中：
+
+- `e = 目标位姿 - 当前位姿`，因此主任务使用 `JΔq - e`。如果误差定义相反，公式
+  才写成加号；当前 Placo 代码不翻转误差符号。
+- 前两项是严格六维 TCP 位置和姿态任务，始终保持最高权重。
+- `λ||Δq||²` 是现有动能正则，用于抑制过大的关节增量和数值抖动。
+- `m6` 使用 Placo 支持的 `both` 类型六维可操作度；`αm` 只在完整六维雅可比的
+  最小奇异值进入预警区时逐渐激活，正常区域为零。
+- J3 是低权重软引导，不属于可行性硬门槛。
+- `[x]+ = max(0, x)`；J4 软项只在进入预警区后产生作用，提前远离 10° 硬下限。
+
+辅助项不能通过提高权重来抵消六维 TCP 跟踪。第一版复用 Placo 现有任务接口，不
+引入新的分层 QP 框架或外部依赖。
+
+## 3. 四处修改
+
+### 3.1 六维主任务、可操作度与数值迭代
+
+严格六维 TCP 主任务保持不变，可操作度由“全程恒定启用的位置任务”改为“接近奇异
+区才启用的六维任务”：
+
+- `sigma_min >= sigma_warn`：`αm = 0`，不干扰正常遥操作；
+- `sigma_stop < sigma_min < sigma_warn`：`αm` 从 0 平滑增加到 1；
+- `sigma_min <= sigma_stop`：保持最大辅助权重，但仍不降低六维 TCP 权重。
+
+`sigma_warn`、`sigma_stop` 和最大辅助权重先保留为仿真可调参数，根据现有完整轨迹
+日志确定；不直接沿用此前效果不明显的恒定位置可操作度权重。
+
+当前 30 次求解是同一目标的数值迭代，不是 30 个真实控制周期。生产控制链路仍由
+现有关节速度和加速度限制器约束实际运动，因此不把“单个物理周期到达完整目标”作为
+收敛要求。求解器逐次检查六维误差；只有达到当前位置和姿态阈值的结果才允许发送。
+30 次内未收敛则恢复到本周期实际关节反馈，不发送未收敛的中间结果。
+
+### 3.2 J3 初始姿态软参考
+
+取消继续扫描 J3 最优角，使用当前 YAML 初始姿态作为第一版参考：
+
+\[
+q_{3,\mathrm{ref}}^L=67.96^\circ，\qquad
+q_{3,\mathrm{ref}}^R=-89.57^\circ。
+\]
+
+J3 只使用低权重软任务，不设置 J3 硬限位，不因追踪参考角而放松 TCP 位姿任务。
+代表性严格六维 mock 路径显示：左臂使用 `1e-5` 可完成路径，提高到 `1e-4` 会提前
+触及关节限位；右臂使用 `1e-5` 时 J6 到达 URDF 下限，提高到 `1e-4` 后保留约
+23° J6 余量并完成前伸段。因此第一版分别取：
+
+\[
+w_3^L=10^{-5}，\qquad w_3^R=10^{-4}。
+\]
+
+左右臂参数分别配置，后续只在完整 mock 轨迹明显改善时再调整，不把参考角本身当作
+成功保证。
+
+### 3.3 J4 硬下限与软缓冲区
+
+左右臂暂时保持相同硬约束：
+
+\[
+q_4\geq q_{4,\min}=10^\circ。
+\]
+
+在硬下限上方增加预警区，第一版取 `q4_warn = 25°`：
+
+- `q4 >= 25°`：J4 软项关闭；
+- `10° < q4 < 25°`：软项随接近 10°逐渐增强；
+- `q4 <= 10°`：由硬约束禁止继续向下。
+
+这样保留收集筐下降阶段所需的可达空间，同时避免 QP 到达 10°附近才突然遇到约束
+边界。`25°` 是待仿真验证的缓冲起点，不是新的硬下限；左右臂允许分别调整预警角，
+但除非轨迹数据证明有必要，不增加更多参数。
+
+### 3.4 QP 失败恢复与笛卡尔参考状态提交
+
+这是除目标函数外最关键的修复。当前风险流程为：
+
+```text
+QP 失败
+→ 关节指令保持不动
+→ 笛卡尔目标历史仍向前更新
+→ 下一周期误差进一步增大
+→ 连续失败或恢复时突跳
+```
+
+修改后，QP 求解结果、关节目标和笛卡尔参考状态按同一周期提交。
+
+QP 成功时：
+
+```text
+QP 成功
+→ 发送新关节目标
+→ 关节目标发送成功
+→ 提交新的笛卡尔参考状态
+```
+
+QP 失败或关节目标发送失败时：
+
+```text
+QP 失败
+→ 丢弃失败后的 Placo 内部迭代结果
+→ 保持上一有效关节目标
+→ 不提交本周期笛卡尔参考状态
+→ 操作者把手柄移回可行区域后继续求解
+```
+
+“不提交笛卡尔参考状态”包括不更新本周期候选的滤波状态、
+`_last_sent_target`、`_last_sent_orientation` 和命令时间。下一周期仍从上一已提交的
+笛卡尔参考状态以及实际关节反馈出发计算，防止 QP 误差在机械臂不动时继续累积。
+
+手柄原始输入仍正常接收，不会被程序改写，也不会自动改变操作者给出的末端姿态。
+失败时机械臂不会为了恢复而自行移动；操作者主动将手柄移回可行区域后，QP 使用新的
+手柄输入重新求解。收集筐到达和松开夹爪仍以实际 TCP 反馈及位置、姿态容差为判据。
+
+## 4. 保留约束
+
+QP 和下游控制继续保留：
+
+- URDF 关节位置限制和 J4 的 10°额外硬下限；
+- 现有关节速度、关节加速度、TCP 线速度和角速度限制；
+- 工作空间/圆柱限位、指令超时和安全停止；
+- `configure_safety_limits` 默认启用；
+- `move_to_initial_pose_on_connect` 默认关闭；
+- mock 模式不依赖睿尔曼真机 SDK。
+
+## 5. 验证顺序与通过标准
+
+实施按以下顺序进行：
+
+1. 先实现 QP 失败保持，以及关节目标与笛卡尔参考状态的成功后统一提交；
+2. 加入 J4 的 10°硬下限与 25°软缓冲区；
+3. 加入左右臂 J3 初始姿态软参考；
+4. 加入按六维最小奇异值激活的 `both` 可操作度任务；
+5. 在 `use_mock:=true` 下运行初始位姿、前方 30～50 cm 采摘、本侧下方 40 cm 收集
+   筐和返回初始位姿的完整严格六维轨迹。
+
+至少记录并比较修改前后的：QP 成功/失败周期数、连续失败长度、失败周期参考状态是否
+保持不变、恢复时的关节跳变量、完整轨迹成功数、
+六维最小奇异值、最大位置/姿态误差、J4 最小余量、最大关节速度以及目标历史与实际
+TCP 的偏差。只有失败周期下降、完整轨迹成功率不降低、严格六维误差和全部安全约束
+仍满足时，辅助项才保留；否则首先回退可操作度或 J4 软项，不回退失败状态修复。