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

docs/superpowers/specs/2026-08-13-rm75-qp-robustness-design.md

@@ -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 软项，不回退失败状态修复。