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