checkpoint8: Mini-MPPI/随机微分方程扩写 + 足式60润色

扩写: 采样MPC 110_Mini-MPPI实战2042(+环境配置/代价模板/warp-kernel/log-sum-exp/ESS自适应)、随机分析 10_SDE2046(+Euler-Maruyama/Milstein/反向SDE/Kalman-Bucy/MPPI关联)。润色: 足式60_QP(补知识导航+目标5→7+棱锥术语,2058)。

Co-Authored-By: Claude Opus 4.8 (1M context) <noreply@anthropic.com>

Author: Michael-Jetson

05_运动控制/10_足式/230_Perceptive_MPC.md

@@ -980,9 +980,47 @@ auto [distance, grad_xy] =
 
 这个设计有一个微妙后果：当查询点移动越过格子边界时，梯度可能跳变（因为参与插值的四个角点变了）。工程上依赖三件事来降低影响：较细的地图分辨率、良好的 warm start，以及对 SDF/惩罚函数的适度平滑。不能把它理解成全局光滑函数。
 
+### 真实接口 2.5：`DistanceTransformInterface` 与 `ComputeDistanceTransform`（源码核实）
+
+在写 `TeachingSignedDistanceTransform` 教学模块之前，必须先把真实接口讲清楚——否则会形成一个根深蒂固的误解："OCS2 的距离场是 2D 的"。核对 `main` 分支源码后，真实的抽象接口签名是：
+
+```cpp
+// 来源：ocs2_perceptive/.../distance_transform/DistanceTransformInterface.h（源码原文）
+class DistanceTransformInterface {
+ public:
+  using vector3_t = Eigen::Matrix<scalar_t, 3, 1>;
+
+  // 给定 3D 点 p，返回有符号距离值
+  virtual scalar_t getValue(const vector3_t& p) const = 0;
+
+  // 给定 3D 点 p，返回其在零等值面（障碍表面）上的投影点
+  virtual vector3_t getProjectedPoint(const vector3_t& p) const = 0;
+
+  // 给定 3D 点 p，返回 {距离值, 距离对 p 的 3D 梯度}
+  virtual std::pair<scalar_t, vector3_t> getLinearApproximation(const vector3_t& p) const = 0;
+};
+```
+
+> 📄 **论文-代码差异（CONFLICT，源码核实）**：本章 67.4 出于教学简化，把 SDF 讲成"2D EDT + 高程图垂直检查"。这在**直觉层面**没错（水平避障是主要矛盾），但在**接口层面**与真实代码不一致——`DistanceTransformInterface` 的三个方法全部接收 `vector3_t`，返回 3D 梯度。Grandia 2023 的 ANYmal 示例（`SegmentedPlanesSignedDistanceField`）从分割平面构建的是**3D 体素 SDF**，沿高度方向也有距离信息。约束侧的 `EndEffectorDistanceConstraintCppAd` 拿到的 `grad` 是完整的 3D 向量，与末端运动学雅可比 `middleRows<3>` 相乘。
+> **判断**：本章前文的 2D 叙述是**降维教学近似**，适合先建立直觉；但读者读源码时会看到 3D 接口，二者必须能对上。正确理解是：*距离场的数学定义和 OCS2 接口都是 3D 的；2D EDT 只是某些轻量实现（或本教学简化）选择的具体计算方式，不是接口契约。*
+
+底层的距离变换由 `ComputeDistanceTransform.h` 提供，它不是一个类，而是一对模板函数——关键设计是**用 lambda 解耦"数据怎么存"**：
+
+```cpp
+// 来源：ocs2_perceptive/.../distance_transform/ComputeDistanceTransform.h（源码原文，简化注释）
+// GetValFunc: Scalar(size_t index)        —— 取第 index 个采样点的当前值
+// SetValFunc: void(size_t index, Scalar)  —— 把结果写回第 index 个采样点
+template <typename GetValFunc, typename SetValFunc, typename Scalar = float>
+void computeDistanceTransform(size_t numSamples, GetValFunc&& getValue, SetValFunc&& setValue,
+                              size_t start, size_t end,
+                              std::vector<size_t>& vBuffer, std::vector<Scalar>& zBuffer);
+```
+
+> 💡 **论文没告诉你的（工程 trick，来源：`ComputeDistanceTransform.h`）**：这个模板函数实现的就是 67.4 讲的 Felzenszwalb & Huttenlocher 一维抛物线下包络算法（`vBuffer` 存抛物线的分界横标、`zBuffer` 存交点）。它故意不接收任何具体的栅格/图像类型，而是用 `getValue`/`setValue` 两个 lambda 抽象"第 $i$ 个采样点怎么读写"。这样同一份 1D 变换代码既能在 $x$ 方向扫，也能在 $y$、$z$ 方向扫（多维 EDT = 沿各维顺序做 1D EDT），还能适配 `grid_map`、稠密 `Eigen::Matrix` 或自定义体素容器——这是论文完全不会提、但工程上极其关键的可复用性设计。
+
 ### 教学简化模块 2：`TeachingSignedDistanceTransform`
 
-这个教学模块解释从二值可踩性图到 2D SDF 的计算链路。当前 OCS2 main 分支没有 `TeachingSignedDistanceTransform` 这个文件，也没有把下面这些步骤封装成同名类；真实代码中请查看 `ComputeDistanceTransform.h`、`DistanceTransformInterface.h`，以及 perceptive ANYmal 示例中的 `SegmentedPlanesSignedDistanceField` / `grid_map_sdf` 相关链路。下面代码是概念伪代码，用来表达数据流，不是可直接编译的源码。
+这个教学模块解释从二值可踩性图到 SDF 的计算链路。当前 OCS2 main 分支没有 `TeachingSignedDistanceTransform` 这个文件，也没有把下面这些步骤封装成同名类；真实代码中请查看 `ComputeDistanceTransform.h`、`DistanceTransformInterface.h`，以及 perceptive ANYmal 示例中的 `SegmentedPlanesSignedDistanceField` / `grid_map_sdf` 相关链路。下面代码是概念伪代码（按 2D 简化叙述），用来表达数据流，不是可直接编译的源码；真实链路按上面的 3D 接口工作。
 
 ```cpp
 // 概念伪代码：说明 SDF 计算流程，不对应 OCS2 中的真实类名。
@@ -1029,38 +1067,65 @@ private:
 
 ### 真实接口 3：`EndEffectorDistanceConstraint(CppAd)`
 
-`EndEffectorDistanceConstraint.h` 和 `EndEffectorDistanceConstraintCppAd.h` 把距离场查询包装成 OCS2 约束接口。非 CppAD 版本使用普通运动学线性化；CppAD 版本用 `CppAdInterface` 生成末端位置关于状态的雅可比。两者共同点是：距离场本身通过 `set(clearance, distanceTransform)` 在运行时注入。
+`EndEffectorDistanceConstraint.h` 和 `EndEffectorDistanceConstraintCppAd.h` 把距离场查询包装成 OCS2 约束接口。读源码时第一个要注意的差异是**两者的基类不同**——这直接决定了约束依赖哪些 OCP 变量：
+
+| 类 | 基类 | 约束依赖 | 雅可比来源 |
+|----|------|---------|-----------|
+| `EndEffectorDistanceConstraint` | `StateConstraint` | 仅状态 $x$ | `EndEffectorKinematics::getPositionLinearApproximation` 普通运动学线性化 |
+| `EndEffectorDistanceConstraintCppAd` | `StateInputConstraint` | 状态 $x$ 与输入 $u$ | `CppAdInterface` 生成的末端位置雅可比 |
+
+> 📄 **论文-代码差异（源码核实）**：很多二手资料笼统地说"OCS2 用 CppAD 对距离约束求导"。核对 `main` 分支源码后更准确的说法是：**距离场本身不进 AD tape**。CppAd 版本只用 `CppAdInterface` 对"末端正运动学 $p(x)$"求导，距离值 $\phi$ 和空间梯度 $\nabla_p\phi$ 由 `DistanceTransformInterface` 在运行时显式提供，最后两者相乘。非 CppAd 版本连 AD 都不用，直接拿运动学的解析线性化。两个版本共享同一个 `set()` 注入接口。
+
+真实的 `set()` 有多个重载，覆盖"不带 clearance""统一 clearance""逐末端 clearance"三种用法（`EndEffectorDistanceConstraintCppAd` 没有第一个重载，因为它要求显式给出 clearance）：
 
 ```cpp
-// 当前 OCS2 结构的简化版
-class EndEffectorDistanceConstraintCppAd : public StateInputConstraint {
-public:
-  void set(vector_t clearances,
-           const DistanceTransformInterface& distanceTransform) {
+// 来源：ocs2_perceptive/.../EndEffectorDistanceConstraint.h（源码原文）
+void set(const DistanceTransformInterface& distanceTransform);                       // clearance 全 0
+void set(scalar_t clearance, const DistanceTransformInterface& distanceTransform);   // 统一 clearance
+void set(const scalar_array_t& clearances, const DistanceTransformInterface&);       // 逐末端 clearance
+```
+
+下面是对齐 `main` 分支结构的 CppAd 版本骨架。相比早期教学稿，这里补上了真实存在的 `Config`（权重 + 是否生成模型 + 是否打印日志）和 `getQuadraticApproximation`：
+
+```cpp
+// 教学简化（结构对齐 main 分支，省略构造细节）
+class EndEffectorDistanceConstraintCppAd final : public ocs2::StateInputConstraint {
+ public:
+  struct Config {                       // 源码原文：默认 weight=1, generateModel=true, verbose=true
+    scalar_t weight;
+    bool generateModel;
+    bool verbose;
+  };
+
+  void set(vector_t clearances, const DistanceTransformInterface& distanceTransform) {
     clearances_ = std::move(clearances);
     distanceTransformPtr_ = &distanceTransform;
   }
 
   VectorFunctionLinearApproximation getLinearApproximation(
       scalar_t t, const vector_t& state, const vector_t& input,
       const PreComputation& preComp) const override {
+    // 末端位置与其对 (x) 的雅可比：CppAD 生成的部分仅限这一步
     auto eePositions = kinematicsModelPtr_->getFunctionValue(state);
     auto eeJacobians = kinematicsModelPtr_->getJacobian(state);
 
+    const size_t numEEs = clearances_.size();
     VectorFunctionLinearApproximation approx =
         VectorFunctionLinearApproximation::Zero(numEEs, stateDim_, inputDim_);
     for (size_t i = 0; i < numEEs; ++i) {
+      // 距离值 + 3D 空间梯度：运行时由距离场提供，不在 AD tape 内
       auto [distance, grad] =
           distanceTransformPtr_->getLinearApproximation(
               eePositions.segment<3>(3 * i));
-      approx.f(i) = weight * (distance - clearances_(i));
-      approx.dfdx.row(i) = weight * grad.transpose()
-                          * eeJacobians.middleRows<3>(3 * i);
+      approx.f(i) = config_.weight * (distance - clearances_(i));
+      approx.dfdx.row(i) = config_.weight * grad.transpose()
+                          * eeJacobians.middleRows<3>(3 * i);   // 链式法则的显式相乘
     }
     return approx;
   }
 
-private:
+ private:
+  Config config_;
   std::unique_ptr<CppAdInterface> kinematicsModelPtr_;
   const DistanceTransformInterface* distanceTransformPtr_ = nullptr;
   vector_t clearances_;

05_运动控制/10_足式/60_QP_NLP建模.md

@@ -29,9 +29,62 @@
 
 1. **区分** QP 与 NLP 的问题结构差异，判断一个机器人优化问题应该建模为 QP 还是 NLP
 2. **手写** KKT 条件并解释互补松弛条件的物理含义——为什么"顶在约束边界上"的约束对应非零对偶变量
-3. **使用 OSQP / ProxQP** 独立求解一个包含动力学等式约束和摩擦锥不等式约束的 QP，并理解 warm-start 对实时性的关键作用
-4. **使用 Ifopt / CasADi** 建模并求解一个非线性轨迹优化问题，理解 SQP 和 Interior-Point 两种 NLP 求解策略的区别
-5. **做出选型决策**：根据问题规模、实时性要求和约束类型，在 OSQP / ProxQP / HPIPM / Ipopt / acados 中选择合适的求解器
+3. **定位凸优化的锥层次**（$\text{LP} \subset \text{QP} \subset \text{QCQP} \subset \text{SOCP} \subset \text{SDP}$），理解每一层"用更难的求解换更真实的约束"的权衡，并能就摩擦锥做出"QP 线性化"还是"SOCP 精确建模"的选择
+4. **使用 OSQP / ProxQP** 独立求解一个包含动力学等式约束和摩擦锥不等式约束的 QP，并理解 warm-start 对实时性的关键作用
+5. **诊断 QP 的数值问题**：识别病态、用缩放（Ruiz 均衡）与无量纲化改善条件数，并读懂不可行性证书来定位冲突约束——而不是盲目调高迭代次数
+6. **使用 Ifopt / CasADi** 建模并求解一个非线性轨迹优化问题，理解 SQP 和 Interior-Point 两种 NLP 求解策略的区别
+7. **做出选型决策**：根据问题规模、实时性要求和约束类型，在 OSQP / ProxQP / HPIPM / Ipopt / acados 中选择合适的求解器
+
+---
+
+## 50.0.2 知识导航
+
+本章要解决的根问题只有一句话：**机器人控制为什么不能直接复用 SLAM 那套无约束最小二乘，以及该用什么工具替代它**。围绕这个问题，全章的知识树分为四个主干、一条贯穿始终的工程主线。
+
+```
+QP/NLP 建模（本章）
+│
+├─ 主干一：范式与理论地基（§50.1–50.2）
+│    ├─ 从 SLAM 无约束到规控有约束的范式跨越      §50.1
+│    ├─ KKT 条件：有约束最优性的语言（充要于凸 QP）  §50.1.4 / §50.2.3
+│    ├─ QP 标准形式 · 对偶变量物理意义 · Active Set   §50.2.1–50.2.5
+│    └─ 凸优化锥层次 LP→QP→QCQP→SOCP→SDP，
+│         摩擦锥：QP 线性化 vs SOCP，QP→SOCP 转化     §50.2.6–50.2.8
+│
+├─ 主干二：QP 求解器与工程细节（§50.3–50.5）
+│    ├─ 求解器版图与按结构选型（含 2025 腿足综述）    §50.3
+│    ├─ OSQP 精读：ADMM 推导 · rho · warm-start ·
+│    │    WBC 集成 · CSC 稀疏格式 · 数值鲁棒性         §50.4
+│    └─ ProxQP（密集 QP 最快）· HPIPM（MPC 结构王者）  §50.5
+│
+├─ 主干三：NLP 求解器与建模框架（§50.6–50.8）
+│    ├─ Ipopt（工业标准）· acados（实时 SQP-RTI）      §50.6
+│    ├─ CasADi 符号框架与代码生成                      §50.7
+│    └─ Ifopt（TOWR 的极简 C++ 建模）                 §50.8
+│
+└─ 主干四：选型与承上启下（§50.9–50.10）
+     ├─ 决策树 · 按机器人类型推荐 · 常见错误选型       §50.9
+     └─ 通往接触力学/WBC/DDP/OCS2 等下游章节           §50.10
+
+工程主线（横切全章）：每一次"问题类升级 / 求解器更换"都是同一个判断——
+                      我愿意为多一点真实性或精度，付出多少计算代价？
+```
+
+**阅读路径建议**：
+
+- **理论优先**（想吃透"为什么"）：§50.1 → §50.2（尤其 50.2.3 KKT 推导、50.2.6 锥层次）→ §50.6.1（QP 与 NLP 的全局-局部之别）。
+- **工程优先**（想尽快跑通 WBC-QP）：§50.2.1 → §50.4（OSQP 全流程，重点 50.4.4 集成、50.4.6 CSC、50.4.7 数值鲁棒性）→ §50.5.1–50.5.3（ProxQP）→ §50.9 选型。
+- **MPC 方向**：在工程路径基础上加读 §50.5.4–50.5.6（HPIPM/BLASFEO）→ §50.6.3–50.6.6（acados/SQP-RTI）→ §50.7（CasADi 建模）。
+
+## 50.0.3 预计阅读时间
+
+| 模式 | 时间 | 覆盖范围 | 适合人群 |
+|------|------|---------|---------|
+| **精读** | 18-25 小时（约 1 周） | 全章 + 全部练习与代码实操 | 首次系统学习有约束优化、要落地 WBC/MPC 的工程师 |
+| **速读** | 4-6 小时 | §50.1–50.2（理论）+ §50.3 选型 + §50.4.1–50.4.4（OSQP 主线）+ §50.9 | 有凸优化基础、想快速建立"机器人优化"全景的读者 |
+| **速查** | 30-60 分钟 | 50.0 前置自测 + §50.3.1/50.9 选型表 + 本章常见误解汇总 + API 速查表 | 已掌握、回来查求解器选型或某个陷阱的读者 |
+
+> 章末附有详细的"一周学习计划"（见本章小结），把精读模式拆解到每一天。
 
 ---
 
@@ -218,7 +271,7 @@ $$\begin{bmatrix} H & A_{\text{eq}}^T & A_{\mathcal{A}}^T \\ A_{\text{eq}} & 0 &
 
 | 约束类型 | 对偶变量含义 | 例子 |
 |---------|------------|------|
-| 动力学等式 $M\ddot{q}+h=\tau+J^Tf$ | 约束力 | 关节约束反力 |
+| 动力学等式 $M\ddot{q}+h=S^T\tau+J^Tf$ | 约束力 | 关节约束反力 |
 | 关节限位 $q \le q_{\max}$ | 限位反力 | 碰到机械止挡时的力 |
 | 摩擦锥 $\|f_t\| \le \mu f_n$ | 滑动趋势 | 接近滑动时乘子增大 |
 | 力矩限制 $\|\tau\| \le \tau_{\max}$ | 性能降级指标 | 乘子大 → 接近饱和 |
@@ -359,6 +412,8 @@ $$\min_{x,t}\ t + f^Tx \quad \text{s.t.}\quad \frac{1}{2}\|L^Tx\|_2^2 \le t,\qua
 
 ## 50.3 QP求解器全景 ⭐⭐⭐
 
+§50.1–50.2 把"有约束优化是什么、QP 的 KKT 结构长什么样、摩擦锥该线性化还是上 SOCP"这些**理论与建模**问题讲清楚了。但建模只是上半场——把模型真正求解出来、而且在 1kHz 控制周期内求解出来，靠的是一整个生态的 QP 求解器。本节从"版图全景"切入：先看清 2025-2026 年有哪些主流求解器、各自押注哪种算法，再用一篇腿足专项综述纠正"唯速度论"的选型误区。这是从"会建模"走向"会落地"的关键一跳。
+
 ### 50.3.1 2025-2026 年 QP 求解器版图 ⭐
 
 基于 qpsolvers v4.11.0（2026年3月）和 Simple-Robotics/proxqp_benchmark 的实测数据：
@@ -911,7 +966,7 @@ private:
         // 足式/90_WBC分层优化与TSID WBC 章节将详细展开这一构建过程：H 矩阵由力矩跟踪权重
         // 和接触力正则化权重组成，A_eq 的前 6 行对应浮动基座的
         // Newton-Euler 方程（欠驱动约束），C_ineq 编码每只接触脚的
-        // 线性化摩擦锥（4 或 8 面体近似）。
+        // 线性化摩擦锥（$k=4$ 或 $k=8$ 棱棱锥近似，见 50.2.7）。
     }
 };
 ```
@@ -1785,7 +1840,7 @@ acados:     性能 ⭐⭐⭐  开发效率 ⭐⭐  维护性 ⭐⭐
     │
     ├──→ 足式/80_接触力学与约束优化 接触力学与约束优化
     │      摩擦锥约束 → QP 的不等式约束 (本章 50.2)
-    │      线性化摩擦锥 → 4 面体近似写入约束矩阵
+    │      线性化摩擦锥 → 四棱锥（$k=4$）近似写入约束矩阵
     │
     ├──→ 足式/90_WBC分层优化与TSID WBC 分层优化与 TSID
     │      分层 QP → 多个 QP 级联求解 (ProxQP/OSQP)