mink¶

难度：⭐⭐ - ⭐⭐⭐（MJPC 体验 ⭐⭐，iLQG 数学 ⭐⭐⭐，QP IK ⭐⭐⭐）时间：2 周（20-28 小时）前置：S01 MuJoCo 核心（mjModel / mjData / mj_step / Menagerie）

前置自测 ⭐¶

开始本章前，请确认能回答以下三个问题。答不上来请先回顾对应章节。

#	问题	检验知识点	回顾
1	MPC 把控制问题转化为什么形式？它与 PID 的本质区别是什么？	MPC = 滚动优化；PID 无预测	04_移动机器人规控/20_采样式MPC
2	iLQG 的全称是什么？为什么它常被说成 DDP 的 Gauss-Newton 近似？	iterative Linear Quadratic Gaussian；省略动力学二阶张量	足式 DDP/Crocoddyl 章节
3	机械臂的几何 Jacobian \(J \in \mathbb{R}^{6 \times n}\) 把什么映射到什么？当 \(n < 6\) 时会怎样？	关节速度 \(\dot{q}\) 映射到末端速度 \(\dot{x}\)；欠驱动，不存在精确逆	机械臂 M01/M03 或李群雅可比章节

本章目标 ⭐¶

学完本章，你应当能够：

用 MJPC 的 GUI 实时可视化 MPC 行为——拖动目标位姿，观察轨迹如何变化，切换 planner 对比效果
解释 iLQG 的反向传播公式——写出 \(Q_{xx}, Q_{ux}, Q_{uu}\) 的表达式，说出正则化项 \(\mu I\) 的作用
区分 Predictive Sampling 与 iLQG 的适用场景——前者适合接触丰富/非光滑，后者适合需要精确跟踪的光滑动力学
**用 mjctrl 的差分 IK、零空间差分 IK 和操作空间控制示例**实现机械臂末端位姿跟踪，理解伪逆、阻尼和零空间项在奇异点附近的行为差异
用 mink 的 QP IK 求解带约束的差分 IK——关节限位 + 速度限制同时满足

S2.1 MJPC——MPC 最佳交互教具 ⭐⭐⭐¶

动机：MPC 理论太抽象，怎么办？ ⭐⭐¶

MPC 的教科书写法是一堆公式：代价函数、预测时域、约束、求解器。学生看完能写出公式，但对"MPC 到底在做什么"缺乏直觉。

反面案例：很多学生学完 MPC 课程后，仍然认为"MPC 就是在每步求解一个优化问题"。这个理解没错但太空泛——他们不理解**预测时域的长度如何影响行为**，不理解**代价权重 Q/R 的变化如何改变轨迹形状**，不理解**不同求解器面对同一个问题为什么给出截然不同的解**。

MJPC 解决这个问题的方式非常直接：给你一个 GUI，让你用鼠标拖动目标球，实时看到 MPC 规划出的轨迹。当你把目标球拖远一点，轨迹变了；把时域拉长一点，轨迹又变了；切换求解器，轨迹风格完全不同。这种即时反馈建立的直觉，是公式推导无法替代的。

项目：google-deepmind/mujoco_mpc。仓库活跃度、许可证和安装方式会随时间变化，工程使用前以项目主页与官方文档为准。

三种 Planner 对比 ⭐⭐⭐¶

MJPC 当前公开文档中的核心 planner 覆盖了三类轨迹优化范式：

求解器	算法范式	最优性	计算开销	适用场景
iLQG	二阶方法（Gauss-Newton 近似 Hessian）	局部最优	中-高	光滑动力学、精确跟踪
Gradient	一阶梯度下降 + Armijo 线搜索	收敛慢，局部最优	低	Baseline / 简单系统
Predictive Sampling	MPPI 变体：随机采样 + 指数加权	全局探索能力	高（可 GPU 并行）	非光滑 / 接触丰富

一句话区分：iLQG 用二阶近似"精雕细琢"一条轨迹；Gradient 用一阶方向做保守下降；Predictive Sampling 用大量随机轨迹"投票选出"更好的控制序列。CEM 也是采样 MPC 中常见的方法，但不要把它误认为 MJPC 当前公开文档中的内置 planner。

iLQG 核心公式 ⭐⭐⭐¶

iLQG 的数学结构是**前向 rollout + 反向 Riccati 递推**。理解这两步，就理解了整个算法。

在正式推导前，先把它和 GUI 里的现象对应起来。

当你拖动目标球时，MJPC 不是直接把目标位置变成下一步动作。

它会先沿着当前控制序列预测一条未来轨迹。

这条轨迹通常不完美：末端可能没到目标，力矩可能太大，姿态可能偏离。

这些偏差会被代价函数累计成一个标量。

前向 rollout 的作用就是回答：“如果继续执行这串控制，未来会付出多少代价？”

反向 Riccati 递推的作用则是回答：“为了降低这个代价，每个时刻的控制应该朝哪个方向改？”

所以 iLQG 不是“凭空求一个动作”。

它是在已有轨迹附近做局部二次近似。

这个近似越可信，更新越像牛顿法，收敛越快。

接触切换越剧烈、模型越非光滑，近似越容易失真，正则化和线搜索就越重要。

这也是为什么 MJPC 同时保留 Predictive Sampling：当梯度局部信息不可靠时，随机 rollout 反而更稳。

下面的公式不需要一次背下来。

先记住数据流：状态和控制经过动力学产生下一步状态，下一步 value function 再通过 \(A_k,B_k\) 回传到当前步。

所有矩阵项都只是这条数据流的偏导数。

第一步：前向传播。沿着当前名义轨迹 \(\tau = \{x_0, u_0, \ldots, u_{N-1}\}\) 做 rollout，得到状态序列。

第二步：反向传播。从最后一步 \(k = N-1\) 向前递推，对每步计算 Q 函数的二阶展开：

\[Q_{xx} = \ell_{xx} + A_k^T V'_{xx} A_k\]

这是状态的二阶代价。\(\ell_{xx}\) 是瞬时代价对状态的 Hessian，\(A_k = \partial f / \partial x\) 是系统动力学对状态的 Jacobian，\(V'_{xx}\) 是下一步的 value function Hessian。

\[Q_{ux} = \ell_{ux} + B_k^T V'_{xx} A_k\]

这是控制-状态的交叉项。\(B_k = \partial f / \partial u\) 是系统动力学对控制的 Jacobian。

\[Q_{uu} = \ell_{uu} + B_k^T V'_{xx} B_k + \mu I\]

这是控制的二阶代价。关键：\(\mu I\) 是正则化项。当 \(Q_{uu}\) 接近奇异时（Hessian 信息不足），增大 \(\mu\) 使求解稳定。这与 Levenberg-Marquardt 优化中的阻尼项是完全相同的思想。

有了 Q 展开，最优控制增量的闭式解为：

\[k_k = -Q_{uu}^{-1} q_u \quad \text{（前馈增益——修正名义控制）}\]

\[K_k = -Q_{uu}^{-1} Q_{ux} \quad \text{（反馈增益——修正状态偏差）}\]

最终控制律：\(\delta u_k = \alpha \, k_k + K_k \, \delta x_k\)，其中 \(\alpha \in (0, 1]\) 是线搜索步长，用于保证代价下降。

**Riccati 更新**收起来做下一步的递推：

\[V_{xx} = Q_{xx} - K_k^T Q_{uu} K_k\]

这就是一轮完整的 iLQG 迭代。整个过程重复直到收敛。

⚠️ Pitfall：初学者常问"为什么叫 Gauss-Newton 近似"。答案是：完整的 DDP 需要计算 \(f_{xx}\)（动力学的二阶导数），这是一个三阶张量，计算代价极高。iLQG 把 \(f_{xx}\) 项丢掉，只保留 \(B^T V'_{xx} B\) 这个 Gauss-Newton 近似——代价是失去部分二阶信息，好处是计算量大幅降低且实践中收敛几乎一样快。

Predictive Sampling = MPPI 变体 ⭐⭐⭐¶

Predictive Sampling（Howell 2022）的数学核心只有四步：

采样：在名义轨迹周围产生 \(N\) 条随机轨迹

\[u^{(i)} = u_{\text{nominal}} + \varepsilon^{(i)}, \quad \varepsilon^{(i)} \sim \mathcal{N}(0, \Sigma)\]

评估：对每条轨迹计算总代价

\[S^{(i)} = \sum_{k=0}^{N-1} \ell(x_k^{(i)}, u_k^{(i)}) + \ell_f(x_N^{(i)})\]

加权：用指数核把代价转化为权重

\[w^{(i)} = \frac{\exp(-S^{(i)} / \lambda)}{\sum_j \exp(-S^{(j)} / \lambda)}\]

更新：加权平均得到新的名义轨迹

\[u_{\text{new}} = \sum_i w^{(i)} \cdot u^{(i)}\]

温度参数 \(\lambda\) 控制"贪婪程度"：\(\lambda \to 0\) 时只取代价最低的那条轨迹（纯 exploit）；\(\lambda \to \infty\) 时所有轨迹等权混合（纯 explore）。

这就是 MPPI（Model Predictive Path Integral）的核心数学。如果你学过 04_移动机器人规控/20_采样式MPC/30_MPPI核心算法与GPU.md，会发现**两者的数学完全同构**——只是物理后端不同：MJPC 用 MuJoCo 做前向模型，Nav2/移动机器人场景通常用运动学模型、costmap 和局部代价做前向评估。

💡 跨章连接：OCS2 的 SLQ 求解器是 iLQG 的**连续时间版本 + 约束增广**。数学结构完全同构：离散 Riccati 变成连续 Riccati ODE，\(Q_{uu}\) 的正则化变成连续正则化。在 MJPC 中先理解 iLQG，再看 OCS2 SLQ 会容易很多。

⚠️ Pitfall：Predictive Sampling 在 GUI 中看起来会比 iLQG "抖动"得多，尤其是样本数少的时候。这不是 bug——采样方法的本质就是高方差。解决办法：增大采样数 \(N\)（如 128 → 1024）并适当降低温度 \(\lambda\)（如 0.1 → 0.01）。但注意，\(\lambda\) 太小会丢失探索能力，行为会陷入局部最优。

GUI 教学流程 ⭐⭐¶

# 1. 编译
cd mujoco_mpc && mkdir build && cd build
cmake .. -DCMAKE_BUILD_TYPE=Release && make -j$(nproc)

# 2. 启动 GUI
./bin/mjpc

在 GUI 中按以下顺序操作：

选择 Task：humanoid/stand 或 cartpole（推荐先用 cartpole，状态维度低，容易观察）
选择 Planner：先选 iLQG
拖动目标：鼠标拖动绿色目标球，观察 MPC 实时规划的蓝色预测轨迹如何跟随
调整权重：在 Cost 面板调大/调小 Q（状态代价）和 R（控制代价），观察行为变化——Q 大则跟踪激进，R 大则控制保守
切换 Planner：切到 Predictive Sampling，立刻感受差异——轨迹更"毛糙"但探索能力更强
观察收敛：Plots 面板的代价曲线——iLQG 单调下降，Sampling 振荡后稳定

这六步操作下来，学生对"预测时域""代价权重""求解器差异"的理解，比看一天公式都深。

S2.2 mjctrl——教材级单文件控制器 ⭐⭐¶

动机：IK 算法太多，哪个才对？ ⭐⭐¶

逆运动学（IK）算法的教科书通常罗列一大堆方法，但不告诉你**什么时候该用哪个**。更糟的是，多数 IK 库的实现被框架代码淹没，核心算法藏在几千行代码里。

Kevin Zakka 的 mjctrl 是这个问题的最佳解药：每个算法一个 Python 文件，不超过 200 行，无框架依赖，直接用 MuJoCo + NumPy。

项目：kevinzakka/mjctrl。项目结构和依赖版本以仓库说明为准，本章关注其控制算法组织方式。

mjctrl 中的三个核心控制器文件 ⭐⭐¶

文件	算法	核心公式	何时用
`diffik.py`	差分 IK（速度级）	\(\dot{q}=J^\dagger \dot{x}_d\)	实时末端跟踪、理解阻尼伪逆
`diffik_nullspace.py`	带零空间项的差分 IK	\(\dot{q}=J^\dagger \dot{x}_d+(I-J^\dagger J)\dot{q}_{null}\)	冗余臂、姿态保持、关节限位远离
`opspace.py`	Operational Space Control	\(\tau=J^T F+\tau_{bias}+\tau_{null}\)	从运动学 IK 过渡到力/阻抗控制

梯度下降、Gauss-Newton 和 Levenberg-Marquardt 仍然是理解 IK 的重要数学台阶，但它们在这里作为理论变体讲解，不再假装是 mjctrl 仓库中的文件名。学习顺序可以是：梯度下降直觉 → 阻尼最小二乘 → diffik.py → diffik_nullspace.py → opspace.py。

差分 IK 核心代码详解 ⭐⭐¶

差分 IK 是实时控制中最常用的 IK 方法，与 MoveIt2 的 Servo 模块在数学上等价。下面逐行解释核心逻辑：

def diffik_step(m, d, target_pos, target_quat, dt, site_id, damping=1e-2):
    # ---- 第 1 步：读取当前末端位姿 ----
    ee_pos = d.site('attachment_site').xpos        # 位置 (3,)
    ee_mat = d.site('attachment_site').xmat.reshape(3, 3)  # 旋转矩阵 (3,3)

    # ---- 第 2 步：计算 6D 误差 ----
    pos_err = target_pos - ee_pos                  # 位置误差 (3,)
    target_mat = quat2mat(target_quat)
    err_mat = target_mat @ ee_mat.T                # 相对旋转 R_err = R_target R_current^T
    ori_err = mat2axisangle(err_mat)               # 轴角表示 (3,)
    error = np.concatenate([pos_err, ori_err])     # 6D 误差 (6,)

    # ---- 第 3 步：计算几何 Jacobian ----
    jac = np.zeros((6, m.nv))
    mujoco.mj_jacSite(m, d, jac[:3], jac[3:], site_id)
    # jac[:3] = 位置 Jacobian Jv (3, nv)
    # jac[3:] = 角速度 Jacobian Jw (3, nv)

    # ---- 第 4 步：阻尼伪逆求解（最关键的一行） ----
    jac_pinv = jac.T @ np.linalg.inv(jac @ jac.T + damping**2 * np.eye(6))
    # 数学含义：J^T (J J^T + lambda^2 I)^{-1}
    # 这是阻尼最小二乘的右伪逆，而非 Moore-Penrose 伪逆

    # ---- 第 5 步：生成关节速度指令 ----
    dq = jac_pinv @ (error / dt)                   # 广义速度 (nv,)
    return dq  # 仍需按 actuator-joint 映射写入 velocity actuator

为什么用阻尼伪逆而不是 Moore-Penrose 伪逆 \(J^{\dagger} = J^T(JJ^T)^{-1}\)？

因为当机械臂接近奇异构型时，\(JJ^T\) 的最小特征值趋近于零，\(J^{\dagger}\) 的元素会爆炸到极大值，产生疯狂的关节速度。阻尼项 \(\lambda^2 I\) 的作用是给最小特征值"兜底"——即使特征值为零，\(JJ^T + \lambda^2 I\) 仍然可逆，关节速度被限制在合理范围内。

⚠️ Pitfall：damping 参数的典型值在 \(10^{-3}\) 到 \(10^{-1}\) 之间。太小则奇异点附近仍然爆炸；太大则正常构型下跟踪精度下降（因为阻尼项引入了系统性误差）。实践中常用自适应阻尼：根据 \(JJ^T\) 的条件数动态调整 \(\lambda\)。

与 MoveIt2 Servo 的对比 ⭐⭐¶

维度	mjctrl diffIK	MoveIt2 Servo
数学本质	阻尼伪逆差分 IK	阻尼伪逆差分 IK（相同）
代码量	~100 行 Python	~5000 行 C++
安装	`pip install mujoco` 即可	ROS2 + MoveIt2 + colcon build
约束处理	无	内置 collision checking
适用场景	算法学习、快速原型	工业部署、ROS2 生态集成

选择策略：学习阶段用 mjctrl 理解算法本质；部署阶段用 MoveIt2 Servo 或 mink。

S2.3 mink——生产级 QP IK ⭐⭐¶

动机：差分 IK 不够用的时候 ⭐⭐¶

mjctrl 的差分 IK 简洁优美，但有一个根本缺陷：它不处理约束。当关节角度接近限位、当末端路径上有障碍物、当你需要同时满足多个任务（位置跟踪 + 姿态跟踪 + 冗余关节正则）时，伪逆方法束手无策——它只能给出无约束最小二乘的闭式解，对约束视而不见。

mink 的思路是：把差分 IK 改写为一个**二次规划（QP）**问题，约束以显式不等式的形式加入。

项目：kevinzakka/mink。求解器后端和 API 细节可能更新，实际工程以官方文档为准。

QP 公式化 ⭐⭐⭐¶

mink 求解的核心优化问题：

\[\min_{\dot{q}} \quad \frac{1}{2} \| J \dot{q} - \dot{x}_d \|^2 + \frac{\alpha}{2} \| \dot{q} \|^2\]

\[\text{s.t.} \quad q_{\min} \leq q + \dot{q} \cdot \Delta t \leq q_{\max} \quad \text{（关节限位）}\]

\[\quad \dot{q}_{\min} \leq \dot{q} \leq \dot{q}_{\max} \quad \text{（速度限制）}\]

\[\quad A_{\text{col}} \dot{q} \leq b_{\text{col}} \quad \text{（碰撞避免，可选）}\]

第一项是跟踪误差（与差分 IK 的目标相同），第二项 \(\frac{\alpha}{2}\|\dot{q}\|^2\) 是正则化（等价于伪逆中的阻尼）。关键区别在于：mjctrl 的阻尼伪逆是无约束最小二乘的闭式解；mink 的 QP 是有约束优化的数值解。前者快但可能违反约束，后者慢一点但保证可行性。

mink 通过 qpsolvers 后端接入多种 QP 求解器，推荐使用 DAQP（默认依赖，pip install mink 时自动安装）或 OSQP。对于 7-DOF 机械臂典型的 QP 规模（7 个决策变量，几十个约束），求解时间在亚毫秒级，完全满足实时要求。

代码示例：多任务带约束 IK ⭐⭐¶

import mink

configuration = mink.Configuration(model)  # Configuration 内部自动创建 MjData

# 任务定义（优先级由 cost 权重隐式表达）
tasks = [
    mink.FrameTask(
        frame_name="ee_site",
        frame_type="site",
        position_cost=1.0,      # 位置跟踪权重
        orientation_cost=0.5,   # 姿态跟踪权重
    ),
    mink.PostureTask(model=model, cost=0.01),  # 冗余关节正则化
]

# 约束定义
limits = [
    mink.ConfigurationLimit(model),       # 从 MJCF 自动读取关节限位
    mink.VelocityLimit(model, velocities={"joint1": 2.0, "joint2": 2.0}),  # 按关节名指定速度限制 (rad/s)
]

# 求解——QP 通过 qpsolvers 后端求解（推荐 DAQP）
vel = mink.solve_ik(configuration, tasks, dt, solver="daqp", limits=limits)

这段代码的数学含义：求解一个 QP，使末端位姿误差最小，同时满足关节限位和速度限制，冗余自由度用 PostureTask 正则化。与腿足方向 TSID/WBC 的数学结构完全同构——后者多了动力学方程作为等式约束和接触力作为决策变量。

与 Pink（Pinocchio）的关系 ⭐⭐¶

mink 从 Stephane Caron 的 Pink 库移植而来。两者的数学和 API 完全同构，区别仅在物理后端：

	mink	Pink
物理后端	MuJoCo	Pinocchio
生态	MuJoCo / dm_control / Menagerie	ROS2 / Pinocchio / example-robot-data
安装	`pip install mink`	`pip install pin pink`

如果你的仿真环境是 MuJoCo，用 mink；如果是 Pinocchio/ROS2 生态，用 Pink。算法层面没有任何区别。

何时用 mjctrl，何时用 mink？ ⭐⭐¶

场景	推荐	理由
学习 IK 算法原理	mjctrl	单文件、无黑盒
快速原型（10 分钟搭 demo）	mjctrl	无需安装额外包
需要关节限位 / 碰撞约束	mink	QP 显式约束
多任务优先级 IK	mink	加权 QP 标准做法
工业部署到真机	MoveIt2 Servo	ROS2 生态、safety layer

S2.4 viewer 与 GUI：交互式控制的外壳不是装饰 ⭐¶

为什么 viewer 是控制系统的一部分 ⭐¶

在很多仿真教程里，viewer 被当成"看动画的窗口"。这种理解会让调试效率很低。交互式控制中的 viewer 至少承担四个工程角色：

角色	具体作用	对控制调试的价值
状态观察器	显示机器人姿态、接触、坐标轴、传感器方向	快速判断模型是否按预期运动
输入设备	键盘切换模式，鼠标拖动物体或目标	把离线脚本变成可实验系统
安全面板	暂停、单步、重置、关闭控制器	防止错误增益把系统打飞
数据标注器	在事件发生时打标记，如"撞桌面""失稳"	让日志回放能对应到实际现象

从控制角度看，viewer 的本质是一个**低带宽的人机闭环**。控制器的高频闭环是 \(q,\dot{q}\rightarrow \tau\)，频率可以是 500 Hz 或 1 kHz；人的交互闭环是"看到现象→按键/拖动→改变参数或目标"，频率通常只有 1-10 Hz。二者频率差异巨大，但必须共享同一个物理状态 mjData。这就是为什么 viewer 编程中最容易出错的点不是画面，而是**线程、同步和输入事件的时序**。

回顾 S01：mjModel 是编译后的模型常量，mjData 是每一步变化的状态容器。交互式控制正是围绕这对结构展开：控制器读 data.qpos/qvel/sensordata，写 data.ctrl/qfrc_applied/xfrc_applied；viewer 读同一个 data 做渲染，同时把鼠标扰动、GUI 选项和键盘事件同步回脚本。

本质洞察：viewer 不是控制器之外的"显示层"，而是一个带线程、事件和同步语义的输入输出设备。把 viewer 当作显示层，容易忽略锁和同步；把它当作输入输出设备，才会自然地设计暂停、单步、日志标记和安全开关。

managed viewer 与 passive viewer ⭐¶

MuJoCo Python 中常用的 viewer 模式有两类。具体函数签名和平台要求以官方文档为准，本章只讲工程语义。

模式	典型入口	谁推进物理	适合场景	主要风险
Managed viewer	`viewer.launch(model, data)`	viewer 内部或 MuJoCo 回调	只想看模型，控制写在插件/回调中	用户脚本被阻塞，不方便写自定义循环
Passive viewer	`viewer.launch_passive(model, data)`	用户脚本显式调用 `mj_step`	自己写控制循环、调参、记录数据	需要自己处理定时、锁、同步

本章之后的代码都使用 passive viewer，因为交互式控制最重要的是**脚本掌握主循环**。主循环掌握在脚本手里，才能实现下面这些能力：

以固定频率运行控制器，而不是被渲染帧率牵着走。
在 mj_step 前精确写入 data.ctrl 或外力。
每个控制周期记录同样字段，保证回放可复现。
键盘事件只改变"指令状态"，不直接执行重计算。
viewer 关闭时干净退出，不留下后台线程。

反事实看，如果直接把控制逻辑塞进渲染循环，常见后果是：电脑性能好时控制器看似稳定，打开录屏或移动窗口后帧率下降，控制周期变长，PD 增益等效改变，机器人突然抖动。这类问题表面像"物理引擎不稳定"，根因却是**把渲染频率误当成控制频率**。

最小 passive viewer 控制循环 ⭐¶

下面的例子展示一个最小交互闭环：空格暂停，R 重置，[ 和 ] 调整 PD 刚度，D 开关接触点显示。代码中的模型路径需要替换为本地 MJCF；其余逻辑是完整可运行结构。

import time
from dataclasses import dataclass

import mujoco
import mujoco.viewer
import numpy as np


@dataclass
class UiState:
    """由键盘事件修改的轻量状态。

    键盘回调只改这些标志位，不在回调里调用 mj_step。
    这样可以避免 GUI 线程和控制线程同时改 data。
    """
    paused: bool = False
    reset_requested: bool = False
    show_contact: bool = False
    kp_scale: float = 1.0
    step_once: bool = False


def make_key_callback(ui: UiState):
    """创建键盘回调函数。

    MuJoCo viewer 传入的是 keycode；常用 ASCII 键可用 chr 转换。
    不同平台的特殊键编码可能不同，正式工程要用日志打印确认。
    """
    def key_callback(keycode: int) -> None:
        try:
            key = chr(keycode).lower()
        except ValueError:
            return

        if key == " ":
            ui.paused = not ui.paused
        elif key == "r":
            ui.reset_requested = True
        elif key == "d":
            ui.show_contact = not ui.show_contact
        elif key == "]":
            ui.kp_scale *= 1.1
        elif key == "[":
            ui.kp_scale /= 1.1
        elif key == ".":
            ui.step_once = True

    return key_callback


def actuated_joint_state(model: mujoco.MjModel,
                         data: mujoco.MjData) -> tuple[np.ndarray, np.ndarray]:
    """按 actuator-joint 映射读取主动 hinge/slide 关节状态。

    不要用 data.qpos[-model.nu:] 猜主动关节位置：nu 是 actuator 数量，
    qpos/qvel 的排列还会受到 free joint、ball joint 和模型拓扑影响。
    """
    q = np.zeros(model.nu)
    v = np.zeros(model.nu)
    for i in range(model.nu):
        joint_id = int(model.actuator_trnid[i, 0])
        if joint_id < 0:
            raise ValueError(f"actuator {i} is not attached to a joint")

        joint_type = model.jnt_type[joint_id]
        if joint_type not in (mujoco.mjtJoint.mjJNT_HINGE,
                              mujoco.mjtJoint.mjJNT_SLIDE):
            raise ValueError("this minimal PD example only supports hinge/slide joint actuators")

        qadr = model.jnt_qposadr[joint_id]
        dadr = model.jnt_dofadr[joint_id]
        q[i] = data.qpos[qadr]
        v[i] = data.qvel[dadr]
    return q, v


def joint_pd(model: mujoco.MjModel,
             data: mujoco.MjData,
             q_des: np.ndarray,
             kp: float,
             kd: float) -> np.ndarray:
    """最小关节 PD，输出与 actuator 一一对应的 ctrl。

    这里假设每个 actuator 直接作用在一个 hinge/slide joint 上。
    更复杂的 tendon、site 或耦合执行器需要单独写映射。
    """
    q, v = actuated_joint_state(model, data)
    tau = kp * (q_des - q) - kd * v
    return np.clip(tau, -20.0, 20.0)


def run_interactive_pd(xml_path: str) -> None:
    model = mujoco.MjModel.from_xml_path(xml_path)
    data = mujoco.MjData(model)
    ui = UiState()
    q_home, _ = actuated_joint_state(model, data)

    key_callback = make_key_callback(ui)

    with mujoco.viewer.launch_passive(
        model,
        data,
        key_callback=key_callback,
        show_left_ui=True,
        show_right_ui=True,
    ) as viewer:
        while viewer.is_running():
            tic = time.perf_counter()

            if ui.reset_requested:
                mujoco.mj_resetData(model, data)
                q_home, _ = actuated_joint_state(model, data)
                ui.reset_requested = False

            should_step = (not ui.paused) or ui.step_once
            if should_step:
                kp = 30.0 * ui.kp_scale
                kd = 2.0 * np.sqrt(kp)
                data.ctrl[:] = joint_pd(model, data, q_home, kp, kd)
                mujoco.mj_step(model, data)
                ui.step_once = False

            # viewer 有独立线程，改可视化选项时持有锁。
            with viewer.lock():
                viewer.opt.flags[mujoco.mjtVisFlag.mjVIS_CONTACTPOINT] = int(ui.show_contact)

            # sync 会把物理状态推给窗口，也会把 GUI 中的扰动和选项同步回脚本。
            viewer.sync()

            # 使用仿真步长做基础节拍；下一节会讲更精确的频率分层。
            sleep_time = model.opt.timestep - (time.perf_counter() - tic)
            if sleep_time > 0.0:
                time.sleep(sleep_time)

这个例子有几个细节值得反复强调。

代码位置	设计理由	如果不这样做
键盘回调只改 `UiState`	回调可能来自 GUI 线程，保持轻量	回调里重置仿真可能与主循环同时写 `data`
读取 `qpos/qvel` 后 `.copy()`	Python 绑定暴露的是底层内存视图	日志或变量会随下一步 `mj_step` 自动变化
改 `viewer.opt` 前 `viewer.lock()`	viewer 自身也会访问渲染状态	偶发闪烁、崩溃或状态竞争
每步调用 `viewer.sync()`	同步物理、GUI 选项和扰动	鼠标拖动和 GUI 开关不会进入脚本状态

其中最值得养成习惯的是 actuator-joint 映射。

nu 表示执行器数量。

nq 表示广义坐标数量。

nv 表示广义速度数量。

三者没有理由天然相等。

一个 free joint 会贡献 7 个 qpos 和 6 个 qvel，但通常没有直接 actuator。

一个 ball joint 会贡献 4 个 qpos 和 3 个 qvel。

一个 tendon actuator 可能同时牵动多个关节。

一个 site actuator 甚至不直接对应某个单关节坐标。

因此“主动关节在数组尾部”只能作为最小玩具模型的假设，不能写进主路径。

正确习惯是从 actuator 查到它作用的 joint，再从 joint 查 jnt_qposadr 和 jnt_dofadr。

这个映射一旦写清楚，后面做日志、限幅、PD、力矩回放和真机部署都会少掉一整类错误。

如果策略在仿真中表现像“关节顺序随机打乱”，第一件事就应该打印这张映射表。

不要先调网络结构，也不要先改 reward。

动作接口错位时，学习算法只能在错误物理上做优化，训练曲线再漂亮也没有意义。

这个原则同样适用于 Isaac Lab、MJLab 和真实机器人 SDK。

只要 action 是一个向量，就必须能回答每一维到底进入哪个执行器、单位是什么、默认位置是多少、限幅在哪里发生。

鼠标交互：内置扰动优先，自定义回调谨慎 ⭐¶

键盘事件通常是离散命令：暂停、重置、切模式、调增益。鼠标事件更复杂，因为它可以表达连续目标：拖动目标球、旋转相机、施加外力、移动 mocap body。MuJoCo viewer 已经内置了相机和扰动交互。对教学和控制调参来说，优先使用内置机制有三个好处：

相机操作、选中 body、拖拽扰动的行为与官方 simulate 一致。
鼠标扰动会通过 viewer.sync() 和 data.xfrc_applied 相关机制进入物理循环。
不需要自己处理 GLFW 的鼠标按下、移动、滚轮、拾取和坐标反投影。

如果确实需要完全自定义鼠标回调，例如"左键拖动一个红色目标点，右键拖动末端期望姿态"，可以走两条路线：

路线	适合场景	代价
使用 mocap body + viewer 内置拖动	拖动末端目标、移动虚拟物体	需要在 MJCF 中定义 mocap body
自建 GLFW 窗口或使用 C/C++ viewer 回调	自定义拾取、复杂 GUI、实验平台	需要维护渲染循环和输入回调

对本章的学习目标，最推荐的方式是：键盘负责模式，鼠标负责扰动和目标，控制循环负责解释这些输入。不要让鼠标回调直接改控制器内部矩阵；它应该只改变目标位姿或扰动力大小。

用 mocap body 做可拖动目标 ⭐¶

交互式 IK、阻抗控制和 MPC 常常需要一个"可拖动目标"。在 MJCF 中可以定义一个 mocap body，再让控制器跟踪它。下面是一个最小片段：

<mujoco model="interactive_target">
  <worldbody>
    <body name="target" mocap="true" pos="0.4 0.0 0.4">
      <site name="target_site" type="sphere" size="0.025" rgba="1 0 0 0.7"/>
    </body>
  </worldbody>
</mujoco>

Python 中读取目标：

def read_mocap_target(model: mujoco.MjModel, data: mujoco.MjData, mocap_id: int):
    """读取可拖动目标位姿。

    mocap_pos/mocap_quat 是用户输入字段，适合表示外部目标。
    控制器只读取它们，不应该把机器人真实状态写进去。
    """
    pos = data.mocap_pos[mocap_id].copy()
    quat = data.mocap_quat[mocap_id].copy()
    return pos, quat

mocap body 的优势是语义清楚：它不是机器人状态，不参与动力学积分，却可以实时作为目标输入。机械臂跟踪红球、四足跟随目标速度、人形手掌跟踪手柄，都可以归结为"控制器读取一个外部目标，再把它变成 data.ctrl"。

S2.5 实时循环、控制频率与仿真步长 ⭐¶

三个时间尺度 ⭐¶

交互式控制最常见的错误是把所有东西都放在一个 while 循环里，然后假设"每次循环就是一个控制周期"。真实系统至少有三个时间尺度：

时间尺度	记号	典型频率	谁决定	作用
物理积分步长	\(\Delta t_{\text{sim}}\)	0.5-2 ms	`model.opt.timestep`	接触求解和状态积分
控制周期	\(\Delta t_{\text{ctrl}}\)	1-10 ms	控制器设计	更新 `data.ctrl`
渲染周期	\(\Delta t_{\text{render}}\)	16-33 ms	显示器/窗口	画面刷新和交互反馈

这三个频率不应该强行相等。仿真步长要足够小，以保证接触和高速运动稳定；控制频率要符合算法预算，例如 WBC 500 Hz、MPC 50-100 Hz；渲染频率只要人眼看得顺即可，通常 30-60 Hz。

**零阶保持**是连接控制周期和仿真步长的标准方法：控制器每 \(\Delta t_{\text{ctrl}}\) 更新一次力矩，其间多次 mj_step 复用同一条 data.ctrl。如果 \(\Delta t_{\text{ctrl}} = 0.002\)，\(\Delta t_{\text{sim}} = 0.001\)，则每次控制更新后推进 2 个物理步。

本质洞察：仿真步长解决"物理积分能不能稳定"，控制周期解决"反馈来不来得及"，渲染周期解决"人能不能看清"。三者共享一个循环，但不是同一个概念。

频率分层调度器 ⭐¶

下面的代码把 1 kHz 仿真、500 Hz PD、100 Hz 高层目标更新、60 Hz 渲染分开。它的价值不在于复杂，而在于每个任务都有明确节拍。

import time
from dataclasses import dataclass

import mujoco
import mujoco.viewer
import numpy as np


@dataclass
class RateDivider:
    """用整数分频表达多频率控制。

    sim_dt 必须能较好整除各控制周期；如果不能整除，应使用时间累加器。
    """
    sim_dt: float
    ctrl_dt: float
    slow_dt: float
    render_dt: float
    step_count: int = 0
    last_render_time: float = 0.0

    @property
    def ctrl_div(self) -> int:
        return max(1, round(self.ctrl_dt / self.sim_dt))

    @property
    def slow_div(self) -> int:
        return max(1, round(self.slow_dt / self.sim_dt))

    def do_ctrl(self) -> bool:
        return self.step_count % self.ctrl_div == 0

    def do_slow(self) -> bool:
        return self.step_count % self.slow_div == 0

    def do_render(self, now: float) -> bool:
        return now - self.last_render_time >= self.render_dt


def run_multirate_loop(xml_path: str) -> None:
    model = mujoco.MjModel.from_xml_path(xml_path)
    data = mujoco.MjData(model)
    sim_dt = model.opt.timestep
    rate = RateDivider(sim_dt=sim_dt, ctrl_dt=0.002, slow_dt=0.010, render_dt=1.0 / 60.0)

    q_target, _ = actuated_joint_state(model, data)
    tau_cmd = np.zeros(model.nu)

    with mujoco.viewer.launch_passive(model, data) as viewer:
        wall_start = time.perf_counter()
        sim_start = data.time

        while viewer.is_running():
            loop_start = time.perf_counter()

            if rate.do_slow():
                # 慢速层：更新目标。实际工程可在这里运行 MPC、轨迹生成或键盘命令解析。
                t = data.time
                if model.nu > 0:
                    q_target, _ = actuated_joint_state(model, data)
                    q_target[0] += 0.2 * np.sin(2.0 * np.pi * 0.2 * t)

            if rate.do_ctrl():
                # 快速层：根据当前状态计算力矩。这里用关节 PD 演示。
                q, v = actuated_joint_state(model, data)
                kp = 40.0
                kd = 2.0 * np.sqrt(kp)
                tau_cmd = kp * (q_target - q) - kd * v
                tau_cmd = np.clip(tau_cmd, -30.0, 30.0)

            data.ctrl[:] = tau_cmd
            mujoco.mj_step(model, data)
            rate.step_count += 1

            now = time.perf_counter()
            if rate.do_render(now):
                viewer.sync()
                rate.last_render_time = now

            # 用仿真时间对齐墙钟时间，避免循环耗时累积成系统性漂移。
            sim_elapsed = data.time - sim_start
            wall_elapsed = time.perf_counter() - wall_start
            sleep_time = sim_elapsed - wall_elapsed
            if sleep_time > 0.0:
                time.sleep(sleep_time)

控制频率怎么选 ⭐¶

频率选择不能只看"越高越好"。频率越高，延迟越小，但计算预算越紧；频率越低，计算更宽松，但等效相位滞后变大。一个实用的选型表如下：

控制层	推荐范围	主要计算	太低的症状	太高的症状
关节 PD/阻抗	500 Hz - 2 kHz	少量矩阵和限幅	抖动、接触时发软、相位滞后	CPU 忙等，日志过大
差分 IK / OSC	200 Hz - 1 kHz	Jacobian、质量矩阵、线性代数	轨迹跟踪迟钝	奇异点附近噪声被放大
WBC / TSID	200 Hz - 500 Hz	QP、接触约束	支撑切换不稳	求解器偶发超时
MPC / iLQG	20 Hz - 100 Hz	rollout、线性化、优化	预测落后，行为滞后	每周期迭代不足
GUI 渲染	30 Hz - 60 Hz	OpenGL 渲染	看不清瞬态	与控制抢 CPU/GPU

与足式方向的“腿足简化模型理论”联系在这里很直接：LIPM、SRBD、Centroidal 这些低维模型之所以适合 MPC，是因为它们把高频物理细节交给 WBC/PD 层，只在较低频率规划质心、动量和接触力。复合机器人方向“多模态 MPC”中的分层结构也是同一个逻辑：高层决定模式和目标，低层保证每个小时间隔的稳定执行。

易错陷阱：步长、子步和增益耦合 ⭐¶

现象	常见原因	排查方法
同一组 PD 增益在 1 ms 稳定，5 ms 爆炸	控制周期变长，离散系统阻尼不足	固定 `model.opt.timestep`，只改变控制分频做对比
画面一卡顿机器人就抖	控制循环被渲染阻塞	降低渲染频率，记录每步 wall time
调大 `kp` 后接触抖动明显	执行器过硬，接触求解器承受高频冲击	降低 `kp`，增加 `kd`，检查积分器与接触参数
MPC 看起来"反应慢半拍"	MPC 周期过长或使用旧状态求解	日志记录 `solve_start_time` 和应用控制的时间

S2.6 PD、阻抗与外力扰动 ⭐⭐¶

从 `data.ctrl` 到真实力矩 ⭐⭐¶

MuJoCo 中控制输入的含义由 MJCF 的 <actuator> 定义决定。data.ctrl[i] 本身只是第 \(i\) 个 actuator 的控制量；它可能表示直接力矩、位置目标、速度目标、肌肉激活或其他执行器输入。官方 XML 文档中明确区分了 motor、position、velocity 等执行器快捷写法；具体属性以官方文档为准。

actuator 类型	`data.ctrl` 的直觉含义	适合用途	注意点
`motor`	直接驱动力/力矩	自己实现 PD、WBC、MPC 力矩输出	需要手动限幅和重力补偿
`position`	位置伺服目标	快速搭建位置控制 demo	PD 参数在 XML 中，不在 Python 中
`velocity`	速度伺服目标	差分 IK 输出关节速度	创建 PD 往往需要 position+velocity 两个 actuator
`general`	自定义增益、偏置、动态	复杂执行器建模	参数多，必须逐项验证

这意味着一句 data.ctrl[:] = tau 并不总是"发送力矩"。如果 MJCF 中 actuator 是 position servo，那么 data.ctrl 是目标位置；如果是 motor，它才更接近力矩命令。交互调参时要先确认 actuator 语义，否则会出现"我以为在调 PD，其实在调位置目标"的错位。

关节 PD：最小、最常用、最容易误解 ⭐⭐¶

关节 PD 的连续时间形式是：

\[\tau = K_p(q_d - q) + K_d(\dot{q}_d - \dot{q}) + \tau_{\text{ff}}\]

其中 \(\tau_{\text{ff}}\) 可以是重力补偿、WBC 输出或 MPC 前馈力矩。若只做位置保持，常用 \(\dot{q}_d=0\)。临界阻尼的二阶系统直觉给出一个常用起点：

\[K_d \approx 2 \zeta \sqrt{K_p I_{\text{eff}}}, \quad \zeta \in [0.7, 1.2]\]

单关节近似中 \(I_{\text{eff}}\) 是等效惯量。很多教学代码用 \(K_d=2\sqrt{K_p}\)，隐含假设是 \(I_{\text{eff}}\approx 1\)。真实机器人或复杂 MJCF 中，这个假设未必成立，所以交互调参必须观察阶跃响应，而不能盲目套公式。

def torque_pd_with_gravity(model: mujoco.MjModel,
                           data: mujoco.MjData,
                           q_des: np.ndarray,
                           qd_des: np.ndarray,
                           kp: np.ndarray,
                           kd: np.ndarray,
                           torque_limit: np.ndarray) -> np.ndarray:
    """带偏置力补偿的关节 PD。

    data.qfrc_bias 包含 MuJoCo 当前计算出的科氏、离心和重力等偏置项。
    对固定基机械臂，取主动关节对应的尾部即可；浮动基机器人要按 actuator 映射检查。
    """
    q, qd = actuated_joint_state(model, data)

    tau_fb = kp * (q_des - q) + kd * (qd_des - qd)
    tau_bias = np.zeros(model.nu)
    for i in range(model.nu):
        joint_id = int(model.actuator_trnid[i, 0])
        dadr = model.jnt_dofadr[joint_id]
        tau_bias[i] = data.qfrc_bias[dadr]
    tau = tau_fb + tau_bias
    return np.clip(tau, -torque_limit, torque_limit)

阻抗控制：不是"更软的 PD" ⭐⭐¶

阻抗控制的目标不是简单降低刚度，而是让末端呈现一个期望动力学：

\[M_d(\ddot{x}_d - \ddot{x}) + D_d(\dot{x}_d - \dot{x}) + K_d(x_d - x) = F_{\text{ext}}\]

在常见的笛卡尔阻抗实现中，先计算期望末端力：

\[F = K_x(x_d - x) + D_x(\dot{x}_d - \dot{x})\]

再用 Jacobian 转置映射到关节：

\[\tau = J^T F + \tau_{\text{bias}}\]

与复合机器人多模态控制中的力跟踪和阻抗模式相连，这里应当形成一个判断：位置控制追求"到哪儿去"，阻抗控制追求"遇到外界时表现得像什么"。如果任务包含接触，如推门、擦桌子、插接件，阻抗比纯位置控制更自然。

def cartesian_impedance(model: mujoco.MjModel,
                        data: mujoco.MjData,
                        site_name: str,
                        x_des: np.ndarray,
                        xd_des: np.ndarray,
                        k_xyz: np.ndarray,
                        d_xyz: np.ndarray,
                        torque_limit: np.ndarray) -> np.ndarray:
    """三维位置阻抗控制，演示从末端力到关节力矩。

    为了聚焦主线，这里只控制位置三维，不控制姿态。
    姿态阻抗需要使用 SO(3) 误差和角速度误差。
    """
    site_id = model.site(site_name).id
    x = data.site_xpos[site_id].copy()

    jacp = np.zeros((3, model.nv))
    mujoco.mj_jacSite(model, data, jacp, None, site_id)
    xd = jacp @ data.qvel

    force = k_xyz * (x_des - x) + d_xyz * (xd_des - xd)
    tau_full = jacp.T @ force + data.qfrc_bias
    tau = tau_full[-model.nu:]
    return np.clip(tau, -torque_limit, torque_limit)

外力扰动：`xfrc_applied` 与 `qfrc_applied` ⭐⭐¶

交互式控制一定要主动做扰动实验。一个控制器在静态目标下稳定，不代表它能承受推、拉、碰撞和目标突变。MuJoCo 提供两类直接外部输入：

字段	维度	含义	典型用途
`data.qfrc_applied`	`nv`	广义坐标力	给某个关节加干扰力矩
`data.xfrc_applied`	`nbody x 6`	作用在 body 质心的 6D 空间力	鼠标拖动、推机器人、施加外力矩

下面的例子用键盘 p 触发一个短脉冲，向指定 body 施加水平推力。

@dataclass
class PushState:
    push_requested: bool = False
    push_until: float = 0.0
    force_scale: float = 80.0


def apply_body_push(model: mujoco.MjModel,
                    data: mujoco.MjData,
                    body_name: str,
                    push: PushState,
                    duration: float = 0.15) -> None:
    """给 body 施加短时外力。

    xfrc_applied 每步都会被物理使用，但不会自动清成零。
    因此每个仿真步都先清空，再按条件写入。
    """
    data.xfrc_applied[:] = 0.0

    if push.push_requested:
        push.push_until = data.time + duration
        push.push_requested = False

    if data.time <= push.push_until:
        body_id = model.body(body_name).id
        data.xfrc_applied[body_id, 0] = push.force_scale  # 世界系 x 方向力
        data.xfrc_applied[body_id, 5] = 0.0               # 绕 z 的力矩，此处不用

外力扰动实验的关键不是"把机器人推倒"，而是建立扰动到响应的因果链：

推力多大、持续多久。
作用在哪个 body、哪个方向。
控制器检测到什么状态变化。
力矩、接触力、末端误差如何变化。
多长时间恢复，是否有残余振荡。

S2.7 数据记录、回放与交互调参方法 ⭐⭐¶

为什么交互调参必须记录数据 ⭐⭐¶

交互式调参很容易给人一种错觉：看画面觉得好了，就认为控制器变好了。但肉眼只擅长捕捉大趋势，不擅长比较 5% 的误差变化，更不擅长记住 20 分钟前某组参数的瞬态响应。工程调参必须把"看起来不错"变成可比较的数据。

建议每次实验至少记录以下字段：

类别	字段	用途
状态	`time, qpos, qvel`	回放和误差分析
控制	`ctrl, qfrc_applied, xfrc_applied`	判断控制器是否饱和或受扰
目标	`target_pos, target_quat, q_des`	区分目标突变和控制失败
误差	`ee_error, joint_error, contact_count`	直接画指标曲线
事件	`key_event, gain_scale, mode`	对齐 GUI 操作和曲线变化

官方 Python 绑定暴露很多数组为底层内存视图，所以记录时必须 .copy()。不复制的结果是日志中所有条目都指向同一块内存，最后看起来每一帧都等于最终状态。

class EpisodeLogger:
    """轻量日志器，适合教学实验。

    大规模数据建议写 HDF5；本章用 npz 保持依赖最少。
    """
    def __init__(self):
        self.rows = []

    def append(self,
               model: mujoco.MjModel,
               data: mujoco.MjData,
               ctrl: np.ndarray,
               target_pos: np.ndarray,
               mode: str,
               gain_scale: float) -> None:
        contact_count = int(data.ncon)
        self.rows.append({
            "time": float(data.time),
            "qpos": data.qpos.copy(),
            "qvel": data.qvel.copy(),
            "ctrl": ctrl.copy(),
            "target_pos": target_pos.copy(),
            "mode": mode,
            "gain_scale": float(gain_scale),
            "contact_count": contact_count,
        })

    def save_npz(self, path: str) -> None:
        np.savez_compressed(
            path,
            time=np.array([r["time"] for r in self.rows]),
            qpos=np.stack([r["qpos"] for r in self.rows]),
            qvel=np.stack([r["qvel"] for r in self.rows]),
            ctrl=np.stack([r["ctrl"] for r in self.rows]),
            target_pos=np.stack([r["target_pos"] for r in self.rows]),
            gain_scale=np.array([r["gain_scale"] for r in self.rows]),
            contact_count=np.array([r["contact_count"] for r in self.rows]),
            mode=np.array([r["mode"] for r in self.rows]),
        )

回放：复现现象，而不是复现猜测 ⭐⭐¶

回放分两种：

回放方式	保存内容	优点	局限
状态回放	每帧 `qpos/qvel`	完全复现画面，适合展示	不能验证控制器
控制回放	初始状态 + 每帧 `ctrl/target`	能验证物理和控制链	对模型、步长、扰动一致性敏感

状态回放最简单：逐帧把 qpos/qvel 写回 data，调用 mj_forward 更新派生量，再 viewer.sync()。

def replay_state_log(xml_path: str, log_path: str, realtime: bool = True) -> None:
    model = mujoco.MjModel.from_xml_path(xml_path)
    data = mujoco.MjData(model)
    log = np.load(log_path, allow_pickle=True)
    qpos_log = log["qpos"]
    qvel_log = log["qvel"]
    time_log = log["time"]

    with mujoco.viewer.launch_passive(model, data) as viewer:
        wall_start = time.perf_counter()
        sim_start = float(time_log[0])

        for i in range(len(time_log)):
            if not viewer.is_running():
                break

            data.qpos[:] = qpos_log[i]
            data.qvel[:] = qvel_log[i]
            data.time = float(time_log[i])
            mujoco.mj_forward(model, data)
            viewer.sync()

            if realtime:
                sim_elapsed = float(time_log[i]) - sim_start
                wall_elapsed = time.perf_counter() - wall_start
                sleep_time = sim_elapsed - wall_elapsed
                if sleep_time > 0.0:
                    time.sleep(sleep_time)

控制回放则更像单元测试：重置到同一个初始状态，每一步写入当时记录的 ctrl 和扰动，再调用 mj_step。如果回放轨迹偏离记录，说明某些输入没有记录完整，例如外力、mocap 目标、随机数种子、求解器 warmstart 或模型参数。

交互调参的工程流程 ⭐⭐¶

调参不是随机旋钮。一个可靠流程如下：

步骤	操作	通过标准
1	固定模型、步长、初始状态，只调一个参数	每次实验只有一个变量改变
2	从低增益开始，逐步上调	无饱和、无高频振荡
3	记录阶跃响应：超调、上升时间、稳态误差	指标能解释画面现象
4	加入外力脉冲和目标突变	恢复时间可接受
5	加入接触、限位、障碍物	不依赖"刚好没撞上"
6	保存参数、日志和场景	后续能回放同一现象

对 PD/阻抗最实用的调参顺序是：

关闭积分项和复杂前馈，只保留最小反馈。
调 \(K_p\) 到响应足够快但还没有明显振荡。
增加 \(K_d\) 到超调可接受。
加入力矩限幅，观察是否长期饱和。
开启重力补偿或 WBC 前馈，重新微调 \(K_d\)。
加外力扰动，记录恢复时间。

反事实看，如果先打开所有模块，系统一旦振荡，很难判断是 PD 过硬、阻抗姿态误差错、外力没有清零、还是 MPC 目标跳变。交互式控制要追求的不是一次把系统调好，而是让每个现象都有可定位的原因。

调参指标表 ⭐⭐¶

指标	计算方式	说明
RMS 跟踪误差	\(\sqrt{\frac{1}{T}\sum_t \\|e_t\\|^2}\)	衡量整体精度
峰值误差	\(\max_t \\|e_t\\|\)	捕捉目标突变和扰动瞬间
控制饱和率	`mean(abs(ctrl) > 0.95 * limit)`	判断是否靠限幅硬撑
恢复时间	扰动后误差回到阈值内的时间	衡量鲁棒性
高频能量	控制差分的 RMS	衡量抖动和执行器压力
接触异常次数	`data.ncon` 或接触力阈值统计	发现模型碰撞和地面问题

把这些指标与 viewer 画面对应起来，调参才会从"凭感觉"变成"有证据"。

阶段练习：先建立交互直觉 ⭐⭐¶

练习 1：MJPC Planner 对比实验 ⭐⭐¶

编译并运行 MJPC。选择 cartpole 任务，分别用 iLQG 和 Predictive Sampling 求解。

记录两种 planner 的代价曲线（截图 Plots 面板）
在 Predictive Sampling 下，将采样数从 128 增加到 1024，观察轨迹"抖动"是否减少
将温度 \(\lambda\) 从 0.1 降到 0.01，观察行为变化
提交：两组截图 + 一段文字解释"为什么 Sampling 的轨迹比 iLQG 毛糙"

练习 2：mjctrl 奇异点实验 ⭐⭐¶

运行 mjctrl 的 diffik.py，让 Panda 机械臂跟踪一个目标位姿。

找到一个接近奇异构型的目标（提示：让末端完全伸直，使肘关节角接近 0）
将 damping 设为 \(10^{-6}\)，观察关节速度爆炸
逐步增大 damping（\(10^{-4}, 10^{-2}, 10^{-1}\)），记录跟踪精度与稳定性的 tradeoff
提交：damping vs 稳态误差的表格 + 解释"阻尼项在数学上做了什么"

练习 3：mink 约束 IK vs 无约束 IK ⭐⭐⭐¶

用 mink 实现带关节限位的差分 IK，设置一个接近关节限位边界的目标位姿。

对比 mink（有约束）和 mjctrl diffIK（无约束）——后者会违反关节限位
在 mink 中添加 VelocityLimit，将最大速度限制为正常值的 50%，观察跟踪速度变化
提交：关节角度轨迹对比图（标注限位线）

练习 4：源码精读——iLQG 反向传播 ⭐⭐⭐¶

精读 mjpc/planners/ilqg/planner.cc 的 BackwardPass 函数（约 150 行 C++）。

在代码中标注 \(Q_{xx}, Q_{ux}, Q_{uu}, k_k, K_k, V_{xx}\) 分别对应哪些变量
找到正则化项 \(\mu I\) 的代码位置，说明 \(\mu\) 何时增大、何时减小
对比足式方向 DDP/Crocoddyl 章节中的 DDP——两者数学结构相同，但 MJPC 的 iLQG 近似丢掉了 \(f_{xx}\) 项
提交：带标注的代码截图 + 一段对比分析

S2.8 iLQG 的完整推导与物理含义 ⭐⭐⭐¶

来龙去脉：从 Bellman 到 MJPC ⭐⭐⭐¶

iLQG 不是凭空出现的算法。要真正理解它，需要追溯整条演化链。

1970 年，Jacobson & Mayne 提出 DDP（Differential Dynamic Programming）。他们的出发点是：对于非线性系统 \(x_{k+1} = f(x_k, u_k)\)，直接求解 Bellman 方程在连续状态空间中不可行（维数灾难）。DDP 的核心思想是：不去求全局 value function，而是沿着一条名义轨迹做局部二阶展开，把非线性优化问题**局部近似为 LQR 子问题**。

2005 年，Todorov & Li 提出 iLQG（iterative Linear-Quadratic-Gaussian）。他们观察到 DDP 的反向传播需要计算 \(f_{xx}\)——动力学对状态的二阶导数，这是一个 \(n_x \times n_x \times n_x\) 的三阶张量。对于 \(n_x = 30\)（一个人形机器人的典型状态维度），这个张量有 \(30^3 = 27000\) 个元素，计算和存储代价极高。iLQG 的关键改进是：用 Gauss-Newton 近似丢掉 \(f_{xx}\) 项，只保留一阶动力学信息 \(A_k, B_k\) 与 value function 的二阶信息 \(V_{xx}\) 的组合。这个近似在实践中几乎不损失收敛性能，但计算量从 \(O(n_x^3)\) 降到 \(O(n_x^2)\)。

2012 年，Tassa et al. 发表 "Synthesis and Stabilization of Complex Behaviors"，将 iLQG 应用于 MuJoCo 仿真中的复杂任务（人形站立、行走），证明了 iLQG 在高维非线性系统上的实用性。

2022-2024 年，Howell, Tassa et al. 发布 MJPC，把 iLQG、Gradient Descent、Predictive Sampling 等实时 planner 统一在一个交互式 MPC 框架中。MJPC 的 iLQG 实现直接继承了 Tassa 的数学框架；CEM 属于采样 MPC 中常见的外部方法，不应写成 MJPC 当前公开文档里的内置 planner。

这条线清晰地展示了一个工程现实：理论上最优雅的算法（DDP 完整二阶展开）往往不是实践中最好用的；适当的近似（iLQG 的 Gauss-Newton）才是工程与理论的平衡点。

与 Pontryagin 最大值原理的联系：iLQG 的反向传播本质上是对 Pontryagin 最大值原理（Pontryagin's Maximum Principle, PMP）的离散化近似。PMP 通过伴随变量（adjoint / costate）\(\lambda_k\) 将终端代价的梯度信息反向传播到每个时刻，iLQG 的 \(V_x\) 正是 \(\lambda_k\) 的离散对应物。如果你在连续时间框架下学过 PMP 的 Hamiltonian \(H(x, u, \lambda) = \ell(x,u) + \lambda^T f(x,u)\)，那么 iLQG 的 Q 函数 \(Q_k(\delta x, \delta u)\) 就是 \(H\) 在名义轨迹附近的二阶离散展开。

从 Bellman 方程出发的完整推导 ⭐⭐⭐¶

下面从最基本的离散时间最优控制问题开始，一步步推导出 iLQG 的反向传播公式。每一步都解释"为什么要这样做"。

问题定义。给定非线性离散动力学 \(x_{k+1} = f(x_k, u_k)\) 和代价函数：

\[J = \ell_N(x_N) + \sum_{k=0}^{N-1} \ell(x_k, u_k)\]

目标是找到控制序列 \(\{u_0, u_1, \ldots, u_{N-1}\}\) 使 \(J\) 最小。

第一步：定义 value function。从终端开始，定义从第 \(k\) 步到终点的最优代价：

\[V_k(x) = \min_{u_k, \ldots, u_{N-1}} \left[ \sum_{t=k}^{N-1} \ell(x_t, u_t) + \ell_N(x_N) \right]\]

终端条件：\(V_N(x) = \ell_N(x)\)。这就是 Bellman 的 value function，但我们无法对所有 \(x\) 求解它（连续状态空间）。

第二步：定义 Q 函数。在第 \(k\) 步，定义 action-value function：

\[Q_k(x, u) = \ell(x_k, u_k) + V_{k+1}(f(x_k, u_k))\]

Q 函数的含义是：在状态 \(x\) 执行动作 \(u\)，然后从下一步开始最优行动，所获得的总代价。最优控制满足 \(u_k^* = \arg\min_u Q_k(x_k, u)\)。

第三步：沿名义轨迹做二阶展开。这是 DDP/iLQG 的核心技巧。假设已有一条名义轨迹 \((\bar{x}_k, \bar{u}_k)\)，定义偏差 \(\delta x_k = x_k - \bar{x}_k\)，\(\delta u_k = u_k - \bar{u}_k\)。对 Q 函数做二阶 Taylor 展开：

\[Q_k \approx Q_k(\bar{x}, \bar{u}) + \begin{pmatrix} q_x \\ q_u \end{pmatrix}^T \begin{pmatrix} \delta x \\ \delta u \end{pmatrix} + \frac{1}{2} \begin{pmatrix} \delta x \\ \delta u \end{pmatrix}^T \begin{pmatrix} Q_{xx} & Q_{xu} \\ Q_{ux} & Q_{uu} \end{pmatrix} \begin{pmatrix} \delta x \\ \delta u \end{pmatrix}\]

其中一阶项和二阶项需要逐个计算。这一步的动机是：二阶展开把非线性优化问题变成了一个二次型最小化——而二次型有闭式解。

第四步：计算各展开系数。利用链式法则，将 \(Q\) 的导数用代价函数导数和动力学导数表示。

动力学的一阶线性化：

\[\delta x_{k+1} = A_k \delta x_k + B_k \delta u_k, \quad A_k = \frac{\partial f}{\partial x}\bigg|_{\bar{x}_k, \bar{u}_k}, \quad B_k = \frac{\partial f}{\partial u}\bigg|_{\bar{x}_k, \bar{u}_k}\]

\(A_k\) 和 \(B_k\) 在 MuJoCo 中可以通过 mjd_transitionFD 或有限差分获得。MJPC 具体版本如何缓存、差分或组合这些导数，应以当前源码为准；教学上先把它理解成“沿名义轨迹获得离散动力学 Jacobian”。

一阶项：

\[q_x = \ell_x + A_k^T V'_x\]

这一项的含义：当前状态偏差的代价影响 = 当前步瞬时代价的梯度 + 未来代价通过动力学传播回来的梯度。\(V'_x\) 是下一步 value function 的梯度，\(A_k^T\) 把它从下一步状态空间"搬运"回当前状态空间。

\[q_u = \ell_u + B_k^T V'_x\]

含义类似：控制偏差的代价影响 = 控制代价的梯度 + 未来代价通过 \(B_k^T\) 传播回来的梯度。

二阶项（这里是 iLQG 与 DDP 分道扬镳之处）：

\[Q_{xx} = \ell_{xx} + A_k^T V'_{xx} A_k \quad \textcolor{red}{+ \sum_i (V'_x)_i \cdot (f_{xx})_i \quad \text{（DDP 保留，iLQG 丢弃）}}\]

DDP 保留红色项：\(f_{xx}\) 是动力学的二阶导数（三阶张量），\((V'_x)_i\) 是 value function 梯度的第 \(i\) 个分量。这一项刻画了"动力学的非线性弯曲对代价的影响"。iLQG 丢弃红色项，理由是 Gauss-Newton 近似——类似于非线性最小二乘中用 \(J^T J\) 近似 Hessian、丢弃残差的二阶项。

\[Q_{uu} = \ell_{uu} + B_k^T V'_{xx} B_k \quad \textcolor{red}{+ \sum_i (V'_x)_i \cdot (f_{uu})_i \quad \text{（DDP 保留，iLQG 丢弃）}}\]

\[Q_{ux} = \ell_{ux} + B_k^T V'_{xx} A_k \quad \textcolor{red}{+ \sum_i (V'_x)_i \cdot (f_{ux})_i \quad \text{（DDP 保留，iLQG 丢弃）}}\]

丢弃红色项之后，iLQG 的二阶项只依赖 \(A_k, B_k\)（一阶动力学导数）和 \(V'_{xx}\)（value function 的 Hessian），不再需要三阶张量。这就是"Gauss-Newton 近似"的核心内容。

第五步：正则化。实际计算中，\(Q_{uu}\) 可能不是正定的——特别是在优化早期，名义轨迹离最优还很远时。如果 \(Q_{uu}\) 有负特征值，对它求逆会产生"向代价增大的方向走"的荒谬结果。

解决办法是加正则化：

\[\tilde{Q}_{uu} = Q_{uu} + \mu I\]

\(\mu > 0\) 保证 \(\tilde{Q}_{uu}\) 正定。\(\mu\) 的调节策略与 Levenberg-Marquardt 完全一致：

如果本次迭代代价下降了：\(\mu \leftarrow \mu / \nu\)（减小正则化，信任二阶信息）
如果本次迭代代价没有下降：\(\mu \leftarrow \mu \cdot \nu\)（增大正则化，更像梯度下降）

典型初始值 \(\mu_0 = 1.0\)，缩放因子 \(\nu = 10\)。MJPC 源码中的 regularization_ 变量对应 \(\mu\)。

第六步：求解最优控制增量。对展开后的二次型求极值，令 \(\partial Q / \partial (\delta u) = 0\)：

\[q_u + Q_{ux} \delta x + Q_{uu} \delta u = 0\]

\[\delta u = -Q_{uu}^{-1}(q_u + Q_{ux} \delta x)\]

分离常数项（与 \(\delta x\) 无关）和反馈项（与 \(\delta x\) 成正比）：

\[\delta u = \underbrace{-Q_{uu}^{-1} q_u}_{k_k \text{（前馈增益）}} + \underbrace{(-Q_{uu}^{-1} Q_{ux})}_{K_k \text{（反馈增益）}} \delta x\]

前馈增益 \(k_k\) 的物理含义：即使状态没有偏离名义轨迹（\(\delta x = 0\)），也要修正控制。为什么？因为名义控制序列还不是最优的——\(k_k\) 就是"名义控制需要修正的量"。随着迭代进行，名义轨迹越来越接近最优，\(k_k \to 0\)。

反馈增益 \(K_k\) 的物理含义：当状态偏离了名义轨迹（\(\delta x \neq 0\)），如何修正控制以补偿偏差。\(K_k\) 是一个 \(n_u \times n_x\) 的矩阵，它的每个元素 \((K_k)_{ij}\) 表示"状态第 \(j\) 维偏离 1 个单位时，控制第 \(i\) 维应该修正多少"。这与 LQR 的增益矩阵在物理含义上完全一致——区别在于 LQR 的 \(K\) 是全局最优的（线性系统），iLQG 的 \(K_k\) 只在名义轨迹附近局部最优。

第七步：更新 value function 参数。将最优 \(\delta u\) 代回 Q 的二阶展开，得到 value function 的递推：

\[V_x = q_x + K_k^T Q_{uu} k_k + K_k^T q_u + Q_{xu} k_k\]

\[V_{xx} = Q_{xx} + K_k^T Q_{uu} K_k + K_k^T Q_{ux} + Q_{xu} K_k\]

简化形式（利用 \(K_k = -Q_{uu}^{-1} Q_{ux}\)，\(k_k = -Q_{uu}^{-1} q_u\) 代入）：

\[V_x = q_x - Q_{xu} Q_{uu}^{-1} q_u\]

\[V_{xx} = Q_{xx} - Q_{xu} Q_{uu}^{-1} Q_{ux}\]

这就是 Riccati 方程的离散形式。整个反向传播从 \(k = N-1\) 向 \(k = 0\) 递推，得到每步的 \((k_k, K_k)\)。

第八步：前向传播（线搜索）。拿到所有 \((k_k, K_k)\) 之后，执行新的前向 rollout：

\[u_k^{\text{new}} = \bar{u}_k + \alpha \, k_k + K_k (x_k^{\text{new}} - \bar{x}_k)\]

步长 \(\alpha \in (0, 1]\) 通过回溯线搜索（backtracking line search）确定。Armijo 条件的直觉是：如果 \(\alpha = 1\)（完整步长）导致代价没有充分下降，就缩小 \(\alpha\)（例如乘以 0.5），直到代价下降满足一个最小要求。"充分下降"的定义是：实际代价下降量 \(\geq c_1 \times\) 预测下降量，其中 \(c_1 \in (0, 1)\) 是一个宽松的阈值（典型值 \(10^{-4}\)）。

需要线搜索的原因：Q 函数的二阶展开只在名义轨迹附近有效。如果名义轨迹离最优很远，完整步长 \(\alpha = 1\) 可能"冲过头"，导致代价反而增大。线搜索保证每次迭代都在改善。

与 DDP/Crocoddyl 章节的关系 ⭐⭐⭐¶

特性	DDP（完整）	iLQG	MJPC 中的 iLQG
\(f_{xx}\) 项	保留	丢弃	丢弃
Hessian 近似	精确（需三阶张量）	Gauss-Newton	Gauss-Newton
正则化	可选	必须 \((\mu I)\)	自适应 \(\mu\)
收敛阶	二次（靠近最优点）	超线性	超线性
计算复杂度/步	\(O(n_x^3 + n_x^2 n_u)\)	\(O(n_x^2 n_u + n_u^3)\)	同左 + MuJoCo 导数
实现复杂度	高（三阶张量存储）	中	低（MuJoCo 提供 \(A,B\)）

关键结论：iLQG 是 DDP 在 Gauss-Newton 意义下的"降阶版本"。如果你在足式方向 DDP/Crocoddyl 章节中学过 Crocoddyl 的 DDP，那么 MJPC 的 iLQG 就是把 Crocoddyl 的 calc 和 calcDiff 中的二阶动力学项设为零。

⚠️ Pitfall: iLQG 不是 LQR。初学者容易把 iLQG 等同于 LQR，因为两者都有 Riccati 递推。区别是本质的：LQR 假设动力学是线性的（\(f(x,u) = Ax + Bu\)）、代价是二次的，一次 Riccati 递推就给出全局最优解。iLQG 面对的是非线性动力学，它在名义轨迹附近做线性化、在每次迭代中求解一个 LQR 子问题，然后更新名义轨迹、重新线性化、再求解——这个外层迭代是 iLQG 与 LQR 的本质区别。类比：LQR 相当于一步 Newton 迭代（因为问题本身是二次的，一步就到最优），iLQG 相当于多步 Newton 迭代。

S2.9 Predictive Sampling 深入与 MPPI 统一视角 ⭐⭐⭐¶

从信息论推导 MPPI ⭐⭐⭐¶

前面给出了 Predictive Sampling 的四步公式，但没有解释**指数加权为什么是最优的**。这个问题的答案来自信息论。

Williams et al. (2017) 的 MPPI（Model Predictive Path Integral）从 KL 散度最小化出发推导出指数加权公式。推导过程如下：

目标：找到一个控制分布 \(q(u)\)，使得它与某个"理想分布" \(p^*(u) \propto \exp(-S(u)/\lambda)\) 的 KL 散度最小，同时不偏离初始分布 \(p_0(u)\) 太远。数学表述：

\[\min_q \; \mathbb{E}_q[S(u)] + \lambda \, \text{KL}(q \| p_0)\]

第一项是"代价要低"，第二项是"不要偏离先验太远"（信息论意义上的保守性）。\(\lambda\) 控制这两项的平衡——这就是"温度"的来源。

对这个优化问题求变分极值（令泛函导数为零），得到最优分布的闭式解：

\[q^*(u) = \frac{p_0(u) \exp(-S(u)/\lambda)}{\int p_0(u') \exp(-S(u')/\lambda) \, du'}\]

这就是指数加权的理论来源。用蒙特卡洛近似这个积分（从 \(p_0\) 中采 \(N\) 条轨迹），就得到了离散版本的权重公式：

\[w^{(i)} = \frac{\exp(-S^{(i)}/\lambda)}{\sum_j \exp(-S^{(j)}/\lambda)}\]

这与 S2.1 正文中的公式完全一致。现在我们知道了这个公式不是启发式的，而是 KL 散度最小化的精确解。

温度 \(\lambda\) 的物理含义 ⭐⭐⭐¶

\(\lambda\) 是"风险敏感度的倒数"（inverse risk sensitivity）：

\(\lambda \to 0\)（零温度）：\(q^*(u)\) 退化为 Dirac delta 函数，集中在代价最低的那条轨迹上。这相当于纯贪心，完全不考虑分布的多样性。优点是精确；缺点是极端——如果代价函数有多个局部最优，永远只能看到一个。
\(\lambda \to \infty\)（高温度）：\(\exp(-S/\lambda) \to 1\)，所有轨迹等权。此时 \(q^* = p_0\)，更新完全无效。
实用范围 \(\lambda \in [0.01, 1.0]\)：MJPC 默认 \(\lambda = 0.1\)，这是经验中比较好的起点。

与 softmax 的联系：权重公式就是代价的 softmax（加了负号，因为要最小化代价）。温度 \(\lambda\) 与 softmax 中的温度参数 \(T\) 含义完全相同。如果你理解了 LLM 中温度如何影响 token 采样的"尖锐/平坦"，那么 MPPI 中温度的效果直觉上是一样的。

协方差适应：为什么固定 \(\Sigma\) 在高维不够 ⭐⭐⭐¶

Predictive Sampling 的采样分布是 \(u^{(i)} = u_{\text{nom}} + \varepsilon^{(i)}\)，\(\varepsilon \sim \mathcal{N}(0, \Sigma)\)。如果 \(\Sigma\) 是固定的对角矩阵（每个控制维度独立采样），在高维空间中会遇到严重的效率问题。

问题根源：假设控制维度 \(n_u = 20\)（人形机器人的典型值）。每条轨迹有 \(N = 50\) 个时间步，总的控制空间维度是 \(20 \times 50 = 1000\)。在 1000 维空间中，从各向同性高斯分布采样的点几乎全部落在一个薄壳上（高维高斯分布的集中现象），距离名义轨迹的偏差大致相等。这意味着：大多数采样轨迹都在探索"远处的垃圾区域"，真正有用的近邻轨迹极少被采到。

解决方案一：自适应协方差。用上一轮的优秀轨迹估计新的协方差矩阵 \(\Sigma\)，使采样集中在代价较低的区域。这就是 CMA-ES（Covariance Matrix Adaptation Evolution Strategy）的思想，MPPI 的一些变体借鉴了它。

解决方案二：着色噪声（Colored Noise）。标准 MPPI 在每个时间步独立采样噪声 \(\varepsilon_k\)，导致生成的控制序列在时间上锯齿状跳动（高频噪声）。Smooth-MPPI（Treven et al., 2023）用 \(\beta\)-着色噪声替代白噪声——在频域上压制高频分量，使采样轨迹在时间上平滑。直觉：如果名义轨迹是一条平滑曲线，那么"好的扰动"也应该是平滑的，而不是随机锯齿。

与移动机器人 MPPI 章节的连接 ⭐⭐⭐¶

如果你已经学习了 04_移动机器人规控/20_采样式MPC/30_MPPI核心算法与GPU.md，以下对应关系可以帮助迁移理解：

概念	Nav2 MPPI	MJPC Predictive Sampling
前向模型	costmap + 运动学模型	MuJoCo 物理引擎（完整动力学）
状态维度	\((x, y, \theta)\)，3 维	全状态（位置 + 速度 + 关节角），数十维
控制维度	\((v, \omega)\)，2 维	全关节力矩，数十维
采样数 \(N\)	1000-4000	32-1024
温度 \(\lambda\)	Nav2 中叫 `temperature`	MJPC 中叫 `lambda`
约束处理	代价函数惩罚	代价函数惩罚（相同方式）
GPU 加速	不支持	以 MuJoCo 官方文档列出的并行后端为准

两者数学完全同构。差异只在物理后端和维度规模上。在 Nav2 中理解的所有调参直觉（温度太高导致轨迹发散、采样数不够导致抖动、代价函数设计影响行为风格）在 MJPC 中完全适用。

GPU 并行化的潜力 ⭐⭐⭐¶

Predictive Sampling 天然适合 GPU 并行：\(N\) 条轨迹的前向仿真彼此独立，可以同时在 \(N\) 个线程上执行。MuJoCo 的并行仿真和 GPU 相关接口仍在持续演进，具体能力、后端名称和安装方式以官方文档为准。

在 NVIDIA RTX 4090 上，MuJoCo 可以同时仿真数千个环境实例。如果采样数 \(N = 4096\)，每条轨迹 50 步，总计 \(4096 \times 50 = 204800\) 次 mj_step 调用——但在 GPU 上这些是并行的，实际耗时只相当于单条轨迹的 50 次 mj_step。这意味着 Predictive Sampling 的计算瓶颈从"仿真"转移到了"权重计算和加权平均"——后者的代价几乎可以忽略。

对比 iLQG：iLQG 的反向传播是严格串行的（每步依赖下一步的 \(V_{xx}\)），无法并行化。这使得 Predictive Sampling 在 GPU 时代的吸引力大幅增加——即使每条轨迹的质量不如 iLQG 的局部最优解，数量优势可以弥补质量差距。

S2.10 IK 算法族的数学统一与推导 ⭐⭐¶

统一框架：所有 IK 都是同一个最小二乘的变体 ⭐⭐¶

mjctrl 的差分 IK 示例背后，仍然可以追溯到同一个最小二乘框架。梯度下降、Gauss-Newton、LM 和 diffIK 看似各不相同，本质上都是在不同近似和阻尼下求解：

基本问题：给定当前末端位姿 \(x_{\text{current}}\) 和目标位姿 \(x_{\text{target}}\)，找到关节增量 \(\Delta q\) 使得：

\[\min_{\Delta q} \; \frac{1}{2} \| J \Delta q - e \|^2 + R(\Delta q)\]

其中 \(e = x_{\text{target}} - x_{\text{current}}\) 是 6D 误差（3D 位置 + 3D 姿态），\(J \in \mathbb{R}^{6 \times n}\) 是几何 Jacobian，\(R(\Delta q)\) 是正则化项。不同的 \(R\) 选择产生不同的算法。

从无正则化开始：正规方程 ⭐⭐¶

如果不加正则化（\(R = 0\)），对 \(\frac{1}{2}\|J\Delta q - e\|^2\) 求导令其为零：

\[\frac{\partial}{\partial \Delta q} \frac{1}{2}(J\Delta q - e)^T(J\Delta q - e) = J^T(J\Delta q - e) = 0\]

\[J^T J \, \Delta q = J^T e\]

这就是**正规方程**（normal equations）。如果 \(J^T J\) 可逆（\(J\) 列满秩，即 \(n \leq 6\) 且不在奇异构型），解为：

\[\Delta q = (J^T J)^{-1} J^T e = J^{\dagger} e\]

其中 \(J^{\dagger} = (J^T J)^{-1} J^T\) 是左伪逆（left pseudoinverse）。

对于冗余机械臂（\(n > 6\)），\(J^T J \in \mathbb{R}^{n \times n}\) 是奇异的（秩最多为 6），不可逆。此时用右伪逆 \(J^{\dagger} = J^T(JJ^T)^{-1}\)，它给出所有满足 \(J\Delta q = e\) 的解中范数最小的那个。

梯度下降 IK：最保守的选择 ⭐⭐¶

正则化选择：\(R(\Delta q) = \frac{1}{2\alpha}\|\Delta q\|^2\)（强 L2 正则化），其中 \(\alpha\) 是步长。

展开目标函数并求导：

\[\frac{\partial}{\partial \Delta q}\left[\frac{1}{2}\|J\Delta q - e\|^2 + \frac{1}{2\alpha}\|\Delta q\|^2\right] = J^T(J\Delta q - e) + \frac{1}{\alpha}\Delta q = 0\]

当正则化极强时（\(\alpha\) 很小），\(\frac{1}{\alpha}\Delta q\) 项主导，解趋向于：

\[\Delta q \approx \alpha \cdot J^T e\]

这就是梯度下降的公式。物理含义：\(J^T e\) 是误差函数 \(\frac{1}{2}\|e\|^2\) 对关节角的梯度方向（因为 \(\frac{\partial}{\partial q}\frac{1}{2}\|x_{\text{target}} - f(q)\|^2 = -J^T e\)，取负号得到下降方向 \(J^T e\)）。步长 \(\alpha\) 控制每步走多远。

优点：永远不会爆炸，即使在奇异点附近也只是步长变小。

缺点：收敛极慢。在接近目标时，\(e\) 很小导致步长也很小，需要大量迭代才能收敛到精度要求。

Gauss-Newton IK：快速但脆弱 ⭐⭐¶

正则化选择：\(R(\Delta q) = \frac{\mu}{2}\|\Delta q\|^2\)（弱 L2 正则化，\(\mu\) 很小或为零）。

对正规方程加入正则化：

\[(J^T J + \mu I)\Delta q = J^T e\]

\[\Delta q = (J^T J + \mu I)^{-1} J^T e\]

当 \(\mu = 0\) 时，这就是纯 Gauss-Newton 步。物理含义：在当前关节角附近建立一个局部线性模型 \(\delta x = J \delta q\)，然后在这个线性模型下精确求解"哪个 \(\Delta q\) 使残差为零"。

优点：在远离奇异点时，收敛速度是二次的——每次迭代误差平方级减少。

缺点：在奇异点附近，\(J^T J\) 的最小特征值趋于零，\((J^T J)^{-1}\) 的对应特征值趋于无穷，\(\Delta q\) 爆炸。这不仅是数值问题，更是物理问题——在奇异构型中，末端某些方向的运动需要无穷大的关节速度。

Levenberg-Marquardt IK：自适应的平衡 ⭐⭐¶

正则化选择：\(R(\Delta q) = \frac{\lambda}{2}\Delta q^T \text{diag}(J^T J) \, \Delta q\)（自适应 L2 正则化）。

求解：

\[[J^T J + \lambda \cdot \text{diag}(J^T J)] \, \Delta q = J^T e\]

与 Gauss-Newton 的区别：正则化矩阵不再是标量 \(\mu\) 乘单位矩阵，而是 \(\lambda\) 乘以 \(J^T J\) 的对角线。这意味着**沿 \(J^T J\) 特征值大的方向（信息丰富的方向）正则化强，沿特征值小的方向（信息贫乏的方向）正则化弱**。直觉：在有信息的方向信任 Gauss-Newton 步，在信息不足的方向自动退化为梯度下降。

\(\lambda\) 的调节逻辑（与 iLQG 中的 \(\mu\) 完全一致）：

当残差下降：\(\lambda \leftarrow \lambda / \nu\)（减小阻尼，更信任 Gauss-Newton）
当残差上升：\(\lambda \leftarrow \lambda \cdot \nu\)（增大阻尼，更保守）

LM 的另一种等价理解是**信赖域方法**（trust region）：\(\lambda\) 对应信赖域的半径。\(\lambda\) 大 = 信赖域小 = 只允许小步（接近梯度下降）；\(\lambda\) 小 = 信赖域大 = 允许大步（接近 Gauss-Newton）。

奇异性分析：三种算法在奇异点附近的行为 ⭐⭐¶

设 Jacobian 的 SVD 分解为 \(J = U \Sigma_{\text{sv}} V^T\)，其中 \(\Sigma_{\text{sv}} = \text{diag}(\sigma_1, \ldots, \sigma_m)\)，\(\sigma_1 \geq \cdots \geq \sigma_m \geq 0\)。奇异构型意味着 \(\sigma_m \to 0\)。

无阻尼伪逆 \(J^{\dagger} = V \Sigma_{\text{sv}}^{-1} U^T\)：

\[\Delta q = J^{\dagger} e = \sum_{i=1}^m \frac{u_i^T e}{\sigma_i} v_i\]

当 \(\sigma_m \to 0\) 时，第 \(m\) 项 \(\propto 1/\sigma_m \to \infty\)——关节速度在 \(v_m\) 方向上爆炸。

阻尼伪逆 \(J_{\text{damped}}^{\dagger} = J^T(JJ^T + \lambda^2 I)^{-1}\)：

通过 SVD 展开：

\[\Delta q = J_{\text{damped}}^{\dagger} e = \sum_{i=1}^m \frac{\sigma_i}{\sigma_i^2 + \lambda^2} (u_i^T e) \, v_i\]

当 \(\sigma_m \to 0\) 时，第 \(m\) 项 \(\approx \frac{\sigma_m}{\lambda^2}(u_m^T e) \, v_m \to 0\)——奇异方向的关节速度被压制为零，而非爆炸。代价是：即使 \(\sigma_m\) 不为零但很小，阻尼也会在该方向引入跟踪误差。

阻尼参数的选择（定量分析）：

damping \(\lambda\)	奇异点附近行为	远离奇异点的精度	适用场景
\(10^{-6}\)	仍然爆炸（阻尼不足）	极高精度	仅用于条件数很好的场景
\(10^{-4}\)	基本稳定，偶有尖峰	高精度	非冗余臂的普通任务
\(10^{-2}\)	完全稳定	中等精度（稳态误差 mm 级）	通用默认值
\(10^{-1}\)	非常稳定但迟钝	低精度（稳态误差 cm 级）	仅在极端奇异场景

⚠️ Pitfall：阻尼参数不是"设一次就好"的常数。在实际的轨迹跟踪中，机械臂不断进出奇异构型附近。固定阻尼意味着：远离奇异点时精度白白损失，进入奇异点时阻尼可能仍然不够。正确做法是**自适应阻尼**——根据 \(JJ^T\) 的最小特征值（或条件数）动态调整 \(\lambda\)。mjctrl 的 LM 实现通过 \(\lambda\) 的自适应调节近似实现了这一点。

阻尼伪逆的完整推导（从优化视角） ⭐⭐¶

阻尼伪逆 \(J_{\text{damped}}^{\dagger} = J^T(JJ^T + \lambda^2 I)^{-1}\) 并非凭空写出，它有严格的优化推导路径。

目标：在关节空间中找到最小范数的速度，使末端速度误差在 \(\lambda\) 容忍范围内尽可能小。形式化为：

\[\min_{\Delta q} \; \frac{1}{2}\|J\Delta q - e\|^2 + \frac{\lambda^2}{2}\|\Delta q\|^2\]

对 \(\Delta q\) 求导令其为零：

\[J^T(J\Delta q - e) + \lambda^2 \Delta q = 0\]

\[(J^T J + \lambda^2 I)\Delta q = J^T e\]

\[\Delta q = (J^T J + \lambda^2 I)^{-1} J^T e\]

利用矩阵恒等式（push-through identity）：\((J^T J + \lambda^2 I)^{-1}J^T = J^T(JJ^T + \lambda^2 I)^{-1}\)，得到等价形式：

\[\Delta q = J^T(JJ^T + \lambda^2 I)^{-1} e\]

第一种形式在 \(n < m\) 时计算效率更高（逆矩阵大小 \(n \times n\)），第二种形式在 \(n > m\)（冗余臂）时更高效（逆矩阵大小 \(m \times m = 6 \times 6\)）。mjctrl 的 diffik.py 使用第二种形式，因为 7-DOF Panda 臂是冗余的（\(n = 7 > m = 6\)），计算 \(6 \times 6\) 矩阵的逆比 \(7 \times 7\) 更快。

S2.11 Operational Space Control (OSC) 深入 ⭐⭐⭐¶

从 IK 到 OSC 的概念跨越 ⭐⭐⭐¶

前面的四种算法（gradient, GN, LM, diffIK）都工作在**运动学层面**：它们计算的是"关节角度应该怎么变"（位置级）或"关节速度应该是多少"（速度级），然后依赖底层控制器（位置控制器/速度控制器）去执行。

OSC 的根本不同在于：它直接计算关节力矩 \(\tau\)，把动力学（质量、惯性、科氏力、重力）纳入考量。这使得机械臂在笛卡尔空间表现出期望的动力学行为——例如，你可以让末端像一个弹簧-阻尼系统一样响应偏差，而不是单纯地追踪位置。

三个层级的对比：

层级	方法	输出	考虑动力学？	典型延迟要求
位置级	gradient/GN/LM IK	\(q_{\text{target}}\)	否	10-100 ms
速度级	diffIK	\(\dot{q}_{\text{cmd}}\)	否	1-10 ms
力级	OSC	\(\tau\)	是	< 1 ms

为什么需要力级控制？ 两个典型场景：(1) 机械臂需要与环境接触（擦桌子、插插头），纯位置/速度控制在接触时会产生巨大的冲击力；(2) 需要精确的动态行为（画圆时不要因为惯性而偏离轨迹）。OSC 通过补偿动力学效应，让末端的行为"像在理想世界中一样"。

Khatib 1987 框架 ⭐⭐⭐¶

Oussama Khatib 在 1987 年发表了奠基性论文 "A Unified Approach for Motion and Force Control of Robot Manipulators"，提出了操作空间控制的完整数学框架。

目标：让末端在笛卡尔空间中表现出期望的动力学：

\[F_{\text{cmd}} = \Lambda(q) \, \ddot{x}_{\text{des}} + \mu(q, \dot{q}) + p(q)\]

其中 \(\Lambda\) 是操作空间惯性矩阵，\(\mu(q, \dot{q})\) 是操作空间科氏力/离心力向量（注意：\(\mu\) 本身已经是力向量，不需要再乘以 \(\dot{x}\)），\(p(q)\) 是操作空间重力向量，\(F_{\text{cmd}}\) 是施加在末端的操作空间力。

实现方式：将期望的操作空间力通过 Jacobian 转置映射到关节力矩。有两种等价的实现路径：

路径 A（完整操作空间补偿）：在操作空间完成所有动力学补偿，不再额外加关节空间非线性项：

\[\tau = J^T F_{\text{cmd}} + (I - J^T \bar{J}^T) \tau_0\]

其中 \(F_{\text{cmd}}\) 包含完整的操作空间动力学补偿：

\[F_{\text{cmd}} = \Lambda(K_p e_x + K_d \dot{e}_x) + \mu(q, \dot{q}) + p(q)\]

路径 B（MuJoCo 捷径）：利用 MuJoCo 的 qfrc_bias（包含关节空间科氏力+重力），在关节空间做补偿，操作空间只算 PD + 惯性：

\[\tau = J^T \Lambda(K_p e_x + K_d \dot{e}_x) + \text{qfrc\_bias} + (I - J^T \bar{J}^T) \tau_0\]

⚠️ Pitfall：切勿同时在 \(F_{\text{cmd}}\) 中加入操作空间补偿 \((\mu + p)\) 又在关节力矩中额外加 \(c(q,\dot{q}) + g(q)\)——这会导致非线性项被双重补偿，产生严重的力矩偏差。两条路径选一条即可。

逐项解释（以路径 A 为例）：

\(J^T F_{\text{cmd}}\)：主要的操作空间力矩。\(F_{\text{cmd}}\) 是在笛卡尔空间设计的控制力，包含 PD 控制和动力学补偿：

其中 \(e_x = x_{\text{target}} - x_{\text{current}}\) 是末端位姿误差，\(\dot{e}_x = \dot{x}_{\text{target}} - \dot{x}_{\text{current}}\) 是末端速度误差。\(K_p\) 是刚度矩阵（类比弹簧常数），\(K_d\) 是阻尼矩阵（类比阻尼系数）。

\(\Lambda = (J M^{-1} J^T)^{-1}\)：操作空间惯性矩阵。这是整个 OSC 中最重要的量。它的含义是：在末端施加单位力时，末端产生多大的加速度。\(M\) 是关节空间质量矩阵（从 MuJoCo 的 mj_fullM 获得），\(J\) 是几何 Jacobian。通过 \(J M^{-1} J^T\) 的逆，我们把关节空间的惯性"投影"到操作空间。

为什么 \(\Lambda\) 如此重要？因为没有它，PD 控制器在笛卡尔空间的行为会**依赖构型**——在惯性大的构型中响应慢，在惯性小的构型中响应快。\(\Lambda\) 的作用是"补偿"这种变化，使末端在所有构型中表现一致。

\((I - J^T \bar{J}^T) \tau_0\)：零空间项。对于冗余机械臂（\(n > 6\)），满足末端任务的关节力矩不唯一——有一个"零空间"可以自由利用。\(\tau_0\) 是零空间中期望的力矩（例如将关节推向舒适位置），\((I - J^T \bar{J}^T)\) 是零空间投影矩阵，确保 \(\tau_0\) 不影响末端行为。

\(\bar{J} = M^{-1} J^T \Lambda\) 是动力学一致的广义逆（dynamically consistent generalized inverse），它保证零空间投影不引入操作空间加速度。这与差分 IK 中的 \((I - J^{\dagger} J)\) 类似，但后者是运动学一致的，不考虑质量矩阵。

\(c(q, \dot{q}) + g(q)\)：科氏力 + 离心力 + 重力补偿。在 MuJoCo 中，这些项可以通过 data.qfrc_bias 直接获得（MuJoCo 在每步已经计算好了）。

mjctrl opspace.py 关键代码解读 ⭐⭐⭐¶

mjctrl 的 opspace.py 大约 150 行，核心逻辑集中在以下几个步骤：

def osc_step(m, d, target_pos, target_quat, Kp, Kd):
    # ---- 关键步骤 1：获取质量矩阵 M 及其逆 ----
    M = np.zeros((m.nv, m.nv))
    mujoco.mj_fullM(m, M, d.qM)          # 从紧凑格式展开为完整矩阵
    M_inv = np.linalg.inv(M)              # 关节空间惯性的逆

    # ---- 关键步骤 2：获取 Jacobian ----
    J = np.zeros((6, m.nv))
    mujoco.mj_jacSite(m, d, J[:3], J[3:], site_id)

    # ---- 关键步骤 3：计算操作空间惯性 Lambda ----
    Lambda_inv = J @ M_inv @ J.T           # Lambda^{-1} = J M^{-1} J^T
    Lambda = np.linalg.inv(Lambda_inv)     # Lambda = (J M^{-1} J^T)^{-1}

    # ---- 关键步骤 4：计算笛卡尔空间 PD 力（路径 B：仅 PD + 惯性） ----
    pos_err = target_pos - d.site('ee').xpos
    ori_err = orientation_error(target_quat, d.site('ee').xmat)
    error = np.concatenate([pos_err, ori_err])
    dx = J @ d.qvel                        # 末端速度 = J * 关节速度
    F = Lambda @ (Kp @ error - Kd @ dx)    # 只含 PD + 惯性补偿，不含 mu/p

    # ---- 关键步骤 5：映射到关节力矩（路径 B：用 qfrc_bias 做关节空间补偿） ----
    # 注意：F 中不含操作空间 mu/p，所以这里用 qfrc_bias 补偿科氏力+重力
    # 如果 F 已包含操作空间 mu+p（路径 A），则不应再加 qfrc_bias，否则双重补偿
    tau = J.T @ F + d.qfrc_bias            # tau = J^T (Λ·PD) + qfrc_bias
    return tau

注意步骤 3 和步骤 4 的区别：如果去掉步骤 3，直接 F = Kp @ error - Kd @ dx（不乘 \(\Lambda\)），得到的就是普通的笛卡尔空间 PD 控制。它也能工作，但末端的动态响应会随构型变化——在完全伸展（惯性大）和弯曲（惯性小）构型下，同样的 \(K_p\) 产生截然不同的加速度。乘上 \(\Lambda\) 之后，若同时正确处理 \(Jdot\dot{q}\)、任务空间 bias、重力/科氏补偿和奇异点阻尼，末端响应可以近似呈现 \(\ddot{x} = K_p e - K_d \dot{x}\) 的二阶行为。没有这些补偿时，它只是比普通笛卡尔 PD 更接近构型一致，并不是严格的构型无关系统。

⚠️ Pitfall：在实际实现中，\(\Lambda = (J M^{-1} J^T)^{-1}\) 在奇异构型附近不可逆，因为此时 \(J\) 不满秩。处理方式与 IK 相同：加阻尼，使用 \((J M^{-1} J^T + \epsilon I)^{-1}\)。另外，\(M\) 的求逆在高自由度系统中代价很高。MuJoCo 的 mj_solveM 可以利用 \(M\) 的稀疏 LDLT 分解高效求解 \(M^{-1}x\)，避免显式求逆。

S2.12 mink QP 求解的数学细节 ⭐⭐⭐¶

QP 标准形式与 IK 的映射 ⭐⭐⭐¶

二次规划（Quadratic Programming）的标准形式是：

\[\min_x \; \frac{1}{2} x^T H x + g^T x\]

\[\text{s.t.} \quad A_{\text{ineq}} x \leq b_{\text{ineq}}, \quad A_{\text{eq}} x = b_{\text{eq}}\]

其中 \(x\) 是决策变量，\(H\) 是正半定矩阵（Hessian），\(g\) 是线性代价项，\(A_{\text{ineq}}, b_{\text{ineq}}\) 是不等式约束，\(A_{\text{eq}}, b_{\text{eq}}\) 是等式约束。

mink 的差分 IK 如何映射到这个标准形式？

决策变量：\(x = \dot{q} \in \mathbb{R}^n\)，即关节速度。

Hessian 矩阵。展开 mink 的目标函数 \(\frac{1}{2}\|J\dot{q} - \dot{x}_d\|^2 + \frac{\alpha}{2}\|\dot{q}\|^2\)：

\[= \frac{1}{2}\dot{q}^T J^T J \dot{q} - \dot{x}_d^T J \dot{q} + \frac{1}{2}\dot{x}_d^T \dot{x}_d + \frac{\alpha}{2}\dot{q}^T \dot{q}\]

丢掉常数项 \(\frac{1}{2}\dot{x}_d^T\dot{x}_d\)（不影响 \(\dot{q}\) 的最优值），得到：

\[H = J^T J + \alpha I, \quad g = -J^T \dot{x}_d\]

\(H\) 的物理含义：\(J^T J\) 是跟踪项的 Hessian——它衡量关节速度在各方向上对末端误差的影响。\(\alpha I\) 是正则化项的 Hessian——它均匀地惩罚所有方向的关节速度。两者之和 \(H = J^T J + \alpha I\) 保证了正定性（因为 \(\alpha > 0\)），使 QP 有唯一解。

当 \(\alpha\) 很大时，\(H \approx \alpha I\)，解趋向于 \(\dot{q} \approx 0\)（不动）。当 \(\alpha\) 很小时，\(H \approx J^T J\)，解趋向于纯跟踪（可能关节速度很大）。这与差分 IK 中阻尼参数 \(\lambda\) 的角色完全一致——实际上 \(\alpha = \lambda^2\)。

关节限位约束的构造 ⭐⭐⭐¶

关节限位 \(q_{\min} \leq q \leq q_{\max}\) 是关节角度的约束，但 QP 的决策变量是关节速度 \(\dot{q}\)。需要做一步转化：

\[q + \dot{q} \cdot \Delta t \leq q_{\max} \quad \Rightarrow \quad \dot{q} \leq \frac{q_{\max} - q}{\Delta t}\]

\[q + \dot{q} \cdot \Delta t \geq q_{\min} \quad \Rightarrow \quad \dot{q} \geq \frac{q_{\min} - q}{\Delta t}\]

写成标准不等式形式 \(A_{\text{ineq}} \dot{q} \leq b_{\text{ineq}}\)：

\[\begin{pmatrix} I \\ -I \end{pmatrix} \dot{q} \leq \begin{pmatrix} (q_{\max} - q) / \Delta t \\ (q - q_{\min}) / \Delta t \end{pmatrix}\]

其中 \(I\) 是 \(n \times n\) 单位矩阵。这产生 \(2n\) 个不等式约束（每个关节的上下限各一个）。

直觉：如果某个关节已经接近上限（\(q_j\) 接近 \(q_{\max,j}\)），那么允许的正向速度 \(\dot{q}_j \leq (q_{\max,j} - q_j)/\Delta t\) 就很小——关节被"刹住"了。这就是约束 IK 的核心价值：它在**数学层面保证不违反限位**，而不是依赖事后裁剪（clamping，会导致末端轨迹偏离目标）。

碰撞避免约束 ⭐⭐⭐¶

碰撞避免比关节限位更复杂。核心思想是：对机器人身体上的每一对可能碰撞的几何体，计算它们之间的最短距离 \(d\)，并要求 \(d \geq d_{\text{safe}}\)。

将碰撞约束线性化为关节速度的约束：

\[\frac{\partial d}{\partial q} \dot{q} \geq \frac{d_{\text{safe}} - d}{\Delta t}\]

其中 \(\frac{\partial d}{\partial q}\) 是距离对关节角的梯度。这个梯度可以通过碰撞检测得到——MuJoCo 的 mj_collision 提供了碰撞对的信息，从中可以计算接触点的 Jacobian，进而得到距离梯度。

写成标准形式：

\[-\frac{\partial d}{\partial q} \dot{q} \leq \frac{d - d_{\text{safe}}}{\Delta t}\]

注意：线性化只在当前构型附近有效。如果 \(\Delta t\) 太大，实际运动可能已经进入碰撞区域而线性化的约束没有预见到。因此碰撞约束 IK 需要较小的 \(\Delta t\)（高控制频率）。

多任务优先级：加权 QP 与零空间 ⭐⭐⭐¶

当有多个任务时（例如末端跟踪 + 姿态正则 + 肘部避障），有两种处理方式：

方式一：加权 QP（mink 的默认方式）。把所有任务的代价加权求和：

\[\min_{\dot{q}} \; w_1 \|J_1 \dot{q} - e_1\|^2 + w_2 \|J_2 \dot{q} - e_2\|^2 + \cdots + \frac{\alpha}{2}\|\dot{q}\|^2\]

权重 \(w_i\) 隐式表达优先级。这等价于一个 Hessian 为 \(H = \sum_i w_i J_i^T J_i + \alpha I\) 的 QP。

优点：实现简单，一个 QP 搞定。

缺点：低优先级任务可能干扰高优先级任务（因为所有任务在同一个优化中竞争）。权重的绝对值和相对值都需要调参。

方式二：严格优先级（零空间投影）。先解最高优先级任务，再在零空间中解次高优先级任务：

\[\dot{q}_1 = J_1^{\dagger} e_1\]

\[\dot{q}_2 = \dot{q}_1 + (I - J_1^{\dagger} J_1) J_2^{\dagger} e_2\]

\((I - J_1^{\dagger} J_1)\) 是 \(J_1\) 的零空间投影矩阵。第二个任务的解只利用第一个任务"用不完"的自由度。

优点：严格保证高优先级任务不受影响。

缺点：需要多次求解，且低优先级任务的性能严重依赖剩余自由度。

mink 采用方式一，通过 cost 参数调节权重。在大多数机械臂应用中，加权 QP 的效果足够好。

DAQP / OSQP / ProxQP：求解器选择 ⭐⭐⭐¶

mink 通过 qpsolvers 后端支持多种 QP 求解器。对于 IK 规模的问题（\(n = 7\) 决策变量，约 20 个约束），任何现代求解器都能在亚毫秒级完成。但理解它们的区别有助于选择：

求解器	算法	热启动	精度	开源？
DAQP	双活动集（Dual Active Set）	是	高	是（MIT）
OSQP	ADMM（交替方向乘子法）	是	中等（\(\epsilon \sim 10^{-4}\)）	是（Apache-2.0）
ProxQP	近端增广 Lagrangian	是	高（\(\epsilon \sim 10^{-8}\)）	是（BSD-2）
qpSWIFT	内点法	否	高	是

DAQP 是 mink 的默认依赖（pip install mink 时自动安装），基于双活动集方法，在小规模 QP 上精度高且速度快，推荐作为首选。

OSQP 的优势在于热启动——上一步的解作为下一步的初始猜测，在轨迹跟踪场景中（相邻时刻的 QP 非常相似），热启动可以把迭代次数从几十次降到几次。

ProxQP（Bambade et al., 2022）在精度和稳定性上优于 OSQP，特别是当约束接近 active（恰好满足）时。

对于 S2.3 的教学目的，用 DAQP 即可——它随 mink 默认安装，对于 7-DOF IK 的规模性能绰绰有余。如果需要 OSQP 或其他求解器，可通过 pip install qpsolvers[osqp] 额外安装。

代码示例：带关节限位约束的 mink IK ⭐⭐⭐¶

import mink
import mujoco
import numpy as np

# 加载模型
model = mujoco.MjModel.from_xml_path("panda.xml")

# 创建 mink 配置（Configuration 内部自动创建 MjData）
configuration = mink.Configuration(model)
data = configuration.data  # 后续渲染和 mj_forward 都使用同一份 MjData，避免状态脱节

# 定义主任务：末端位姿跟踪
ee_task = mink.FrameTask(
    frame_name="attachment_site",
    frame_type="site",
    position_cost=1.0,
    orientation_cost=1.0,
)

# 定义次要任务：姿态正则化（将关节推向默认位姿）
posture_task = mink.PostureTask(model=model, cost=1e-2)

tasks = [ee_task, posture_task]

# 定义约束——VelocityLimit 使用 dict 按关节名指定速度上限
velocity_dict = {model.joint(i).name: 2.0 for i in range(model.njnt)}
limits = [
    mink.ConfigurationLimit(model),                            # 关节角度限位
    mink.VelocityLimit(model, velocities=velocity_dict),       # 关节速度限制 (rad/s)
]

# 设置目标
ee_task.set_target_from_configuration(configuration)

# 控制循环
dt = 0.002  # 500 Hz
for step in range(10000):
    # 更新目标位姿（跟踪圆形轨迹）
    t = step * dt
    target_pos = np.array([0.4 + 0.1*np.cos(t), 0.1*np.sin(t), 0.5])
    ee_task.set_target(mink.SE3.from_translation(target_pos))

    # 求解 QP（solver 参数必须显式指定，推荐 "daqp"）
    vel = mink.solve_ik(
        configuration, tasks, dt,
        solver="daqp",
        limits=limits,
    )

    # 积分并更新配置内部状态；随后用同一份 data 刷新运动学量。
    configuration.integrate_inplace(vel, dt)
    mujoco.mj_forward(model, data)

与 mjctrl 的关键区别：

mink.ConfigurationLimit 自动从 MJCF 文件的 <joint range=...> 读取限位，不需要手动指定
mink.solve_ik 内部构建并求解 QP，返回的 vel 保证满足所有约束
如果目标不可达且会违反约束，mink 返回"尽量接近目标但不违反约束"的解——这是 QP 的可行性保证

综合练习 ⭐⭐⭐¶

练习 5：从零实现 iLQG 求解 cartpole ⭐⭐⭐¶

目标：不依赖 MJPC，从零用 Python 实现 iLQG，求解 cartpole 的摆起（swing-up）问题。

系统定义：

状态：\(x = [p, \theta, \dot{p}, \dot{\theta}]^T\)，\(p\) 是小车位置，\(\theta\) 是摆角（0 = 朝下）
控制：\(u \in \mathbb{R}\)，施加在小车上的力
目标：\(\theta = \pi\)（倒立），\(p = 0\)（回到原点），\(\dot{p} = \dot{\theta} = 0\)

最小实现骨架（保留关键函数边界，便于逐段验证）：

import numpy as np

# ---- 系统参数 ----
mc, mp, l, g_acc = 1.0, 0.3, 0.5, 9.81  # 小车质量, 摆质量, 摆长, 重力
dt, N = 0.02, 100                          # 时间步长, 时域长度

# ---- 动力学 f(x, u) -> x_next ----
def dynamics(x, u):
    p, th, pd, thd = x
    s, c = np.sin(th), np.cos(th)
    force = float(np.asarray(u).reshape(-1)[0])
    total = mc + mp

    # 连续动力学：先计算角加速度，再计算小车加速度。
    temp = (force + mp * l * thd**2 * s) / total
    thdd = (g_acc * s - c * temp) / (l * (4.0 / 3.0 - mp * c**2 / total))
    pdd = temp - mp * l * thdd * c / total

    # 半隐式 Euler：先更新速度，再用新速度更新位置。
    pd_next = pd + dt * pdd
    thd_next = thd + dt * thdd
    p_next = p + dt * pd_next
    th_next = th + dt * thd_next
    x_next = np.array([p_next, th_next, pd_next, thd_next])
    return x_next

# ---- 线性化 A = df/dx, B = df/du ----
def linearize(x, u, eps=1e-5):
    n_state, m_ctrl = len(x), 1
    A = np.zeros((n_state, n_state))
    B = np.zeros((n_state, m_ctrl))
    # 中心差分比前向差分多一次函数调用，但误差阶更高。
    for j in range(n_state):
        dx = np.zeros(n_state)
        dx[j] = eps
        A[:, j] = (dynamics(x + dx, u) - dynamics(x - dx, u)) / (2.0 * eps)

    du = np.array([eps])
    B[:, 0] = (dynamics(x, u + du) - dynamics(x, u - du)) / (2.0 * eps)
    return A, B

# ---- 代价函数 ----
Q = np.diag([1.0, 10.0, 0.1, 0.1])    # 状态代价权重
R = np.array([[0.01]])                  # 控制代价权重
Qf = 10 * Q                            # 终端代价权重

def stage_cost(x, u):
    x_err = x.copy(); x_err[1] -= np.pi  # 目标: theta = pi
    return 0.5 * x_err @ Q @ x_err + 0.5 * u @ R @ u

def terminal_cost(x):
    x_err = x.copy(); x_err[1] -= np.pi
    return 0.5 * x_err @ Qf @ x_err

def rollout(x0, us):
    """前向 rollout，返回状态序列 (N+1, n)"""
    xs = [x0]
    for k in range(N):
        xs.append(dynamics(xs[-1], us[k]))
    return np.array(xs)

# ---- iLQG 主循环 ----
def ilqg(x0, u_init, max_iter=50, mu_init=1.0):
    n, m = 4, 1
    us = u_init.copy()                    # (N, m) 名义控制序列
    xs = rollout(x0, us)                  # (N+1, n) 名义状态序列

    mu = mu_init
    for iteration in range(max_iter):
        # ---- 反向传播 ----
        ks = np.zeros((N, m))             # 前馈增益
        Ks = np.zeros((N, m, n))          # 反馈增益

        # 终端条件
        x_err = xs[-1].copy(); x_err[1] -= np.pi
        Vx = Qf @ x_err
        Vxx = Qf.copy()

        for k in range(N-1, -1, -1):
            A, B = linearize(xs[k], us[k])
            x_err = xs[k].copy(); x_err[1] -= np.pi

            # Q 函数的局部二阶展开。
            qx = Q @ x_err + A.T @ Vx
            qu = R @ us[k] + B.T @ Vx
            Qxx = Q + A.T @ Vxx @ A
            Quu = R + B.T @ Vxx @ B + mu * np.eye(m)
            Qux = B.T @ Vxx @ A

            # 前馈增益修正名义控制，反馈增益修正状态偏差。
            ks[k] = -np.linalg.solve(Quu, qu).reshape(-1)
            Ks[k] = -np.linalg.solve(Quu, Qux)

            # Riccati 递推。这里使用 Schur complement 形式，维度最清晰。
            Quu_inv_qu = np.linalg.solve(Quu, qu)
            Quu_inv_Qux = np.linalg.solve(Quu, Qux)
            Vx = qx - Qux.T @ Quu_inv_qu
            Vxx = Qxx - Qux.T @ Quu_inv_Qux
            Vxx = 0.5 * (Vxx + Vxx.T)  # 抑制数值误差导致的非对称

        # ---- 前向传播（线搜索） ----
        nominal_cost = sum(stage_cost(xs[k], us[k]) for k in range(N)) + terminal_cost(xs[-1])
        best_cost = float('inf')
        best_xs, best_us = xs, us
        for alpha in [1.0, 0.5, 0.25, 0.125]:
            x_new = x0.copy()
            xs_candidate = [x_new]
            us_candidate = []
            cost = 0.0

            for k in range(N):
                # 新控制 = 名义控制 + 线搜索前馈 + 状态反馈修正。
                du = alpha * ks[k] + Ks[k] @ (x_new - xs[k])
                u_new = us[k] + du
                us_candidate.append(u_new)
                cost += stage_cost(x_new, u_new)
                x_new = dynamics(x_new, u_new)
                xs_candidate.append(x_new)

            cost += terminal_cost(xs_candidate[-1])
            if cost < best_cost:
                best_cost = cost
                best_xs = np.array(xs_candidate)
                best_us = np.array(us_candidate)

        # 更新名义轨迹
        xs, us = best_xs, best_us

        # 代价下降则信任二阶模型；否则增大阻尼，下一轮更保守。
        if best_cost < nominal_cost:
            mu = max(mu / 10.0, 1e-6)
        else:
            mu = min(mu * 10.0, 1e6)

    return xs, us

提交要求：

完整的可运行代码
代价收敛曲线图
将你的结果与 MJPC 的 cartpole iLQG 对比（代价值、轨迹形状）
回答：你的实现需要多少次迭代收敛？MJPC 在实时 MPC 中每个控制周期只做 1-3 次迭代，为什么仍然有效？

练习 6：LM-IK 阻尼参数扫描 ⭐⭐¶

目标：定量理解阻尼参数 \(\lambda\) 对 IK 性能的影响。

实验设置：

加载 Panda 机械臂，设定一个接近奇异构型的目标（肘关节接近完全伸展）
对 \(\lambda \in \{10^{-6}, 10^{-5}, 10^{-4}, 10^{-3}, 10^{-2}, 10^{-1}, 1.0\}\) 分别运行 LM-IK
记录每个 \(\lambda\) 下的：(a) 收敛步数，(b) 最终末端误差，(c) 最大关节速度

import numpy as np

dampings = [1e-6, 1e-5, 1e-4, 1e-3, 1e-2, 1e-1, 1.0]



def scan_damping(run_lm_ik_once):
    """扫描阻尼参数。

    run_lm_ik_once 是你已有的 LM-IK 单次实验函数，
    输入 damping，输出 steps/error/max_vel 三个标量。
    这样实验框架与具体机器人模型解耦。
    """
    results = []
    for lam in dampings:
        steps, final_error, max_joint_velocity = run_lm_ik_once(damping=lam)
        results.append({
            "damping": lam,
            "steps": steps,
            "error": final_error,
            "max_vel": max_joint_velocity,
        })
    return results


def print_scan_table(results):
    """先打印表格，再画图；表格更容易发现异常点。"""
    print("damping, steps, final_error, max_joint_velocity")
    for row in results:
        print(row["damping"], row["steps"], row["error"], row["max_vel"])

提交要求：

两张图 + 一段分析："最优阻尼"在什么范围？为什么？
额外实验：在远离奇异点的目标上重复实验，比较"最优阻尼"是否变化
回答：自适应阻尼（LM 算法）相比固定阻尼（diffIK）的核心优势是什么？

练习 7：OSC 零空间行为演示 ⭐⭐⭐¶

目标：让末端跟踪一个圆形轨迹，同时利用零空间使肘部远离一个虚拟障碍物。

实验步骤：

在 MuJoCo 中设定一个球形障碍物（仅可视化，不参与碰撞）
实现 OSC 的主任务：末端跟踪半径 0.1m 的圆
实现零空间任务 \(\tau_0\)：将肘部关节推离障碍物

def null_space_torque(data, obstacle_pos, J, M_inv, Lambda):
    """计算零空间避障力矩"""
    # 获取肘部位置
    elbow_pos = data.site('elbow_site').xpos

    # 计算排斥力（远离障碍物）
    diff = elbow_pos - obstacle_pos
    dist = np.linalg.norm(diff)
    if dist < 0.3:  # 只在距离 < 0.3m 时激活
        # 距离越近，排斥力越大；除以 dist 得到单位方向。
        k_rep = 10.0
        repulsion = k_rep * (1.0/dist - 1.0/0.3) * diff / dist
    else:
        repulsion = np.zeros(3)

    # 将排斥力通过肘部 Jacobian 映射到关节力矩。
    J_elbow = np.zeros((3, model.nv))
    mujoco.mj_jacSite(model, data, J_elbow, None, elbow_site_id)
    tau_elbow = J_elbow.T @ repulsion

    # 用零空间投影矩阵 (I - J^T @ J_bar^T) 投影，避免干扰末端主任务。
    J_bar = M_inv @ J.T @ Lambda
    N_proj = np.eye(model.nv) - J.T @ J_bar.T
    tau_null = N_proj @ tau_elbow
    return tau_null

提交要求：

末端轨迹图（应为圆形）
肘部轨迹图（应避开障碍物）
对比实验：去掉零空间项后，肘部轨迹如何变化？
回答：零空间投影为什么需要使用动力学一致的广义逆 \(\bar{J} = M^{-1}J^T\Lambda\) 而不是运动学伪逆 \(J^{\dagger}\)？

练习 8：mink 约束 IK 综合实验 ⭐⭐⭐¶

目标：用 mink 实现带关节限位的 IK，并与无约束 IK 对比。

实验设置：

人为收紧 Panda 臂的关节 4 限位：\(q_4 \in [-1.0, 1.0]\)（原始限位约 \([-2.9, 2.9]\)）
设定一个需要关节 4 超出收紧限位才能到达的目标
分别用 mink（有约束）和 mjctrl diffIK（无约束）求解

import mink
import numpy as np


def run_mink_limited(model, steps=1000, dt=0.002):
    """运行带关节限位的 mink IK，并记录关节轨迹。"""
    model.jnt_range[4] = [-1.0, 1.0]  # 收紧第 4 个关节限位
    configuration = mink.Configuration(model)
    tasks = [
        mink.FrameTask("ee_site", "site", position_cost=1.0, orientation_cost=1.0)
    ]
    limits = [mink.ConfigurationLimit(model)]

    q_history = []
    for _ in range(steps):
        vel = mink.solve_ik(configuration, tasks, dt=dt, solver="daqp", limits=limits)
        configuration.integrate_inplace(vel, dt)
        q_history.append(configuration.q.copy())
    return np.asarray(q_history)


def compare_joint_limit(q_history_constrained, q_history_unconstrained):
    """返回关节 4 是否越界的布尔统计，画图可在外部脚本中完成。"""
    q_min, q_max = -1.0, 1.0
    q4_constrained = q_history_constrained[:, 4]
    q4_unconstrained = q_history_unconstrained[:, 4]
    return {
        "mink_violate": bool(np.any((q4_constrained < q_min) | (q4_constrained > q_max))),
        "diffik_violate": bool(np.any((q4_unconstrained < q_min) | (q4_unconstrained > q_max))),
        "q4_constrained": q4_constrained,
        "q4_unconstrained": q4_unconstrained,
    }

提交要求：

关节 4 角度对比图（标注限位线）
末端位姿误差对比图
回答以下问题：
mink 在约束激活时（关节角触及限位），末端误差是否增大？为什么？
如果同时添加 VelocityLimit(model, velocities={...}) 并将各关节速度上限设为 0.5 rad/s，轨迹跟踪速度如何变化？
mink 的 QP 求解耗时是多少？（用 time.time() 测量 solve_ik 的平均耗时）

学习检验：完成练习 5-8 后，你应当能够回答：(1) iLQG 的反向传播为什么从终端开始？ (2) 阻尼伪逆的阻尼参数在什么情况下最关键？ (3) OSC 比 IK 多考虑了什么信息？ (4) 约束 IK 在什么场景下是必须的？如果这四个问题都能清晰回答，本章的补充内容已经掌握。

章末回看：把交互式控制读成四条因果链 ⭐¶

前面已经分别讲了 MJPC、mjctrl、mink、viewer、实时循环、阻抗、扰动和日志。把这些内容放在一起看，会发现交互式控制不是一堆工具，而是四条因果链。

第一条因果链是**目标如何变成力**。

第二条因果链是**频率如何改变稳定性**。

第三条因果链是**扰动如何暴露控制器弱点**。

第四条因果链是**日志如何把现象变成证据**。

这四条因果链是本章最重要的收束。只会运行 demo，还不能算掌握交互式控制；能够沿着因果链定位问题，才算进入工程层面的理解。

因果链一：目标如何变成力 ⭐⭐¶

一个可拖动目标从 viewer 进入控制器后，并不会直接改变机器人。它通常经历以下路径：

环节	数据对象	数学含义	常见错误
目标输入	`mocap_pos/mocap_quat` 或 GUI task target	人给出的期望位姿	目标坐标系与机器人坐标系混淆
误差计算	\(e = x_d - x\) 或 \(\log(T^{-1}T_d)\)	当前状态到目标的偏差	姿态误差直接用欧拉角相减
运动学映射	\(J\) 或 \(J^\dagger\)	末端误差到关节速度	奇异点附近无阻尼
动力学映射	\(J^T F\) 或 QP/WBC	末端力到关节力矩	忽略质量矩阵和偏置力
执行器写入	`data.ctrl`	actuator 的控制输入	把 position actuator 当 motor 用
物理积分	`mj_step`	求解接触、约束和积分	步长过大或外力未清零

这张表告诉我们，"目标没跟上"不是一个足够具体的问题。它可能发生在任一环节。

如果目标坐标系错了，误差一开始就是错的。

如果 Jacobian 的 frame 错了，误差方向会被映射到错误关节。

如果 actuator 语义错了，力矩控制器会变成位置目标写入器。

如果接触参数过硬，正确力矩也可能在地面接触处变成抖动。

调试时应当沿链路逐项检查，而不是直接改增益。

一个实用检查顺序如下：

先冻结机器人，只拖动目标，打印目标位姿是否连续。
再不写 data.ctrl，只计算误差，确认误差方向与画面一致。
再计算 Jacobian，做一次小速度积分，确认末端朝目标移动。
再打开 PD 或阻抗，使用很小增益，确认方向正确。
最后逐步加大增益和限幅，观察误差、速度、力矩是否同步变化。

本质洞察：控制器的输出不是从公式直接到机器人，中间有坐标系、Jacobian、动力学、执行器和积分器五道门。任何一道门的语义错了，后面的公式越精确，错误传播越快。

因果链二：频率如何改变稳定性 ⭐⭐¶

频率问题的迷惑性在于：它经常伪装成参数问题。

同一组 \(K_p,K_d\)，在 1 kHz 下稳定，在 100 Hz 下可能振荡。

同一个 MPC，在 50 Hz 下自然，在 10 Hz 下可能显得迟钝。

同一个 viewer，在 60 Hz 渲染下顺滑，在录屏时掉到 20 Hz 后可能拖慢主循环。

这些现象的共同原因是：离散系统看到的不是连续公式，而是采样后的闭环。

变化	表面现象	真正变化
增大仿真步长	接触发散或能量漂移	积分误差变大
降低控制频率	反馈变慢、超调增大	相位滞后变大
降低 MPC 频率	预测目标落后	优化结果应用到旧状态
降低渲染频率	画面不顺	不应影响物理，除非循环耦合
增加日志写盘频率	偶发卡顿	IO 阻塞进入控制周期

因此，频率调试要先问三个问题：

data.time 是否按固定步长增长？
控制更新是否按仿真时间触发？
渲染和写盘是否会阻塞控制计算？

如果答案不清楚，任何增益调节都不可靠。

从系统设计上，交互式控制推荐使用"仿真时间驱动控制，墙钟时间驱动播放"。

控制器何时更新，由 data.time 或 step count 决定。

画面是否睡眠等待，由 wall time 决定。

日志是否落盘，最好由缓冲区决定，而不是每步同步写文件。

这样即使机器临时卡顿，仿真内部的控制节拍仍然可解释。

因果链三：扰动如何暴露控制器弱点 ⭐⭐¶

没有扰动的稳定，常常只是任务太简单。

一个站立控制器在没有外力时稳定，并不说明它有恢复能力。

一个 IK 控制器在目标缓慢移动时稳定，并不说明它能处理目标突变。

一个阻抗控制器在自由空间稳定，并不说明它接触桌面时安全。

扰动实验的价值是把隐藏问题显性化。

扰动类型	暴露的问题	推荐指标
目标阶跃	响应速度、超调、力矩饱和	上升时间、峰值误差、饱和率
外力脉冲	鲁棒性、阻尼、恢复策略	恢复时间、残余振荡
接触切换	接触参数、阻抗方向、限幅	接触力峰值、穿透量
关节限位逼近	IK 约束处理	限位距离、末端误差
传感器噪声	速度估计和微分项敏感性	控制高频能量
帧率下降	循环耦合问题	控制周期抖动

扰动实验必须可重复。可重复至少包含四个条件：

初始状态相同。
扰动时刻相同。
扰动幅值和方向相同。
控制参数和模型参数相同。

这就是为什么本章强调日志。没有日志，扰动实验只是一次表演；有日志，扰动实验才是一次测量。

对外力扰动尤其要注意一个细节：xfrc_applied 和 qfrc_applied 是输入字段，不是一次性命令。

如果写入后不清零，外力会持续存在。

如果每步先清零再由 viewer 同步扰动，可能覆盖鼠标输入。

如果同时使用鼠标扰动和脚本扰动，应明确优先级。

最稳妥的方式是把扰动源也当作一个模块：它读取键盘、鼠标或脚本事件，输出当前步的外力数组，并在日志中记录外力来源。

因果链四：日志如何把现象变成证据 ⭐⭐¶

日志不是为了"保存数据"，而是为了回答问题。

好的日志能回答：

当时目标是什么？
当前状态是什么？
控制器输出了什么？
执行器有没有饱和？
外界扰动是什么？
接触发生在哪里？
参数在什么时候被改变？

差的日志只能回答："机器人当时好像动了一下。"

问题	必须记录的字段	没记录会怎样
为什么末端没到目标	目标位姿、末端位姿、误差	无法区分目标错和控制错
为什么力矩突然变大	`ctrl`、`qfrc_bias`、外力	无法区分补偿项和反馈项
为什么接触后抖动	接触数量、接触力、末端速度	只能凭画面猜测
为什么回放不一致	初始状态、外力、mocap、随机种子	控制回放会漂移
哪组参数更好	参数事件、误差指标	调参依赖主观记忆

日志还应该分层。

高频日志记录 qpos/qvel/ctrl/error。

中频日志记录目标、模式、接触摘要。

低频日志记录参数变化、按键事件、实验标签。

全部字段都用最高频率记录会浪费空间，也会增加 IO 压力。

全部字段都低频记录又会丢掉瞬态。

一个经验做法是：控制频率 500 Hz 时，高频状态按 500 Hz 记录，渲染用 30-60 Hz，参数事件按发生时记录。

从四条因果链回到本章工具 ⭐¶

MJPC 主要训练第一条和第二条因果链。

拖动目标、改变权重、切换 planner，会让你看到目标、代价、频率和优化器如何改变行为。

mjctrl 主要训练第一条因果链。

它把 IK、diffIK 和 OSC 写成短代码，让你看清误差如何变成速度或力矩。

mink 主要训练第一条和第三条因果链。

它让你看到约束激活时，目标跟踪必须让位于可行性。

viewer 主要训练第一条和第三条因果链。

它把人的输入和物理扰动引入闭环，让控制器暴露真实弱点。

日志与回放主要训练第四条因果链。

它们把一次交互实验变成可以复查、比较和复现实验。

这就是本章为什么把 GUI、控制频率、扰动和数据记录放在同一章。它们不是外设知识，而是交互式控制的工程骨架。

十个调试提问 ⭐¶

遇到控制问题时，可以按下面十个问题自查：

当前 data.ctrl 对应的 actuator 语义是什么？
目标位姿在哪个坐标系下表达？
Jacobian 的线速度和角速度顺序是否与误差向量一致？
控制器实际更新频率是多少？
渲染或日志是否阻塞了主循环？
外力字段是否每步按预期清零或写入？
力矩是否长期处于限幅边界？
约束是否已经激活？
日志是否保存了参数变化事件？
状态回放和控制回放的第一处偏离在哪里？

这十个问题覆盖了本章大多数故障。先按它们排查，再考虑更复杂的物理建模问题。

和后续章节的连接 ⭐¶

S03 的 GPU 生态会把单环境变成大批量环境。

如果单环境中的控制频率、日志字段和目标语义没有理清，批量环境只会把错误放大。

S04 的可微分仿真会让梯度穿过物理过程。

如果你还不能解释一次前向交互实验中的因果链，反向梯度更难解释。

S05 的可微分 MPC 会把优化器放进学习循环。

如果你没有记录求解器状态、约束激活和控制饱和，训练失败时很难判断是学习问题还是控制问题。

因此，本章是后续高级主题的地基。

术语速查：一句话辨析 ⭐¶

术语	一句话辨析
`mjModel`	编译后的模型常量，描述结构、惯量、执行器和传感器布局
`mjData`	每一步变化的运行状态，包含位置、速度、控制、接触和派生量
`mj_step`	推进一个完整物理步，读取当前输入并写入下一状态
`mj_forward`	不积分，只根据当前状态重新计算派生物理量
`data.ctrl`	actuator 输入数组，含义由 MJCF actuator 类型决定
`qfrc_applied`	直接施加在广义坐标上的外力或力矩
`xfrc_applied`	施加在 body 质心的空间力和空间力矩
`qfrc_bias`	当前状态下的偏置广义力，常用于重力与科氏补偿
`launch_passive`	让脚本掌握物理主循环的 viewer 模式
`viewer.sync`	同步物理状态、GUI 选项、扰动和渲染状态
`viewer.lock`	修改 viewer 共享状态前使用的互斥保护
`key_callback`	键盘事件入口，适合写入命令状态而非直接改物理
mocap body	不参与动力学积分、适合表达外部目标的可拖动对象
零阶保持	控制周期之间保持上一条控制命令不变
控制分频	用多个仿真步对应一个控制更新周期
渲染分频	降低画面刷新频率，避免渲染抢占控制预算
阶跃目标	用突变目标测试响应速度和超调
外力脉冲	用短时推力测试恢复能力和阻尼设计
控制饱和率	控制命令贴近限幅的时间比例
RMS 误差	衡量全程平均跟踪质量的指标
峰值误差	衡量最坏瞬态表现的指标
约束激活	QP 中某个限位或不等式达到边界
warmstart	用上一时刻求解结果初始化当前求解器
状态回放	直接播放记录的 `qpos/qvel`，用于复现画面
控制回放	重放控制输入和扰动，用于验证闭环一致性
diffIK	速度级逆运动学，求关节速度而非关节位置
OSC	直接在操作空间设计力，再映射到关节力矩
阻尼伪逆	用阻尼项抑制奇异点附近的速度爆炸
QP IK	把 IK 写成带限位和速度约束的二次规划
iLQG	反复线性化动力学、二次化代价，并用 Riccati 递推更新控制
MPPI	用采样轨迹和指数加权更新控制分布的预测控制方法
CEM	用精英样本逐轮收缩采样分布的随机优化方法；属于采样 MPC 常见外部方法，不写作 MJPC 当前内置 planner
代价权重	决定控制器偏好精度、平滑性、能耗或约束余量的数值
目标坐标系	目标表达所在参考系，错误时会造成系统性方向偏差
actuator 语义	`data.ctrl` 与实际力、位置或速度之间的转换规则
接触可视化	用接触点和接触力显示定位碰撞与穿透问题
参数事件	记录按键调参的时间点和新参数值
实验标签	标记场景、目标、扰动和参数组合，便于后续比较
单环境闭环	在扩展到批量训练前，先验证一个环境中的完整因果链

本章小结 ⭐¶

本章的主线可以用一句话概括：MuJoCo 交互式控制不是把控制器接到仿真器上那么简单，而是把物理状态、控制频率、GUI 输入、扰动实验和数据记录组织成一个可观察、可复现、可调参的闭环系统。

模块	本章核心结论	工程判断
MJPC	用 GUI 建立 MPC 直觉，观察预测时域、权重、求解器如何改变行为	教学和研究原型优先，部署前要评估约束和实时性
iLQG	前向 rollout + 反向 Riccati 递推，本质是沿名义轨迹反复求 LQR 子问题	适合光滑动力学和精确跟踪，接触切换处要谨慎
Predictive Sampling	MPPI 式采样、指数加权、滚动更新	适合非光滑和多模态代价，代价是方差和采样预算
mjctrl	单文件展示 gradient/GN/LM/diffIK/OSC 的算法本质	学算法、查错误、做小原型非常合适
mink	把差分 IK 写成 QP，显式处理关节限位、速度限制和任务权重	约束一旦重要，就应优先考虑 QP IK
viewer	passive viewer 让脚本掌握主循环，键盘和鼠标输入变成控制命令	viewer 是输入输出设备，不只是显示窗口
多频率循环	仿真步长、控制周期、渲染周期必须分离	不要把渲染帧率当成控制频率
PD/阻抗	PD 追踪目标，阻抗定义接触行为	有接触任务时，"软硬"只是表象，期望动力学才是本质
外力扰动	`qfrc_applied/xfrc_applied` 让鲁棒性实验可控	每步清零外力，记录扰动时刻和幅值
数据记录	`.copy()` 保存状态，日志要包含目标、控制、事件和参数	没有日志的调参只能依赖记忆

本章应形成的判断力 ⭐¶

看到一个 MuJoCo 控制脚本，先判断 data.ctrl 的物理含义，而不是直接假设它是力矩。
看到 viewer 卡顿或画面掉帧，先检查控制周期是否也被拖慢。
看到 IK 在奇异点附近爆炸，先看 Jacobian 条件数和阻尼，而不是只调步长。
看到约束 IK 跟踪误差变大，先判断约束是否已经激活，而不是认为 QP 求解失败。
看到接触抖动，先检查刚度、阻尼、步长、积分器和接触参数的组合，而不是只怪模型。
看到某组参数"看起来更好"，必须用日志指标验证 RMS 误差、峰值误差、饱和率和恢复时间。

累积项目：本章新增模块——交互式 MuJoCo 控制实验台 ⭐⭐¶

累积项目进度：

章节	新增模块	累积能力
S01	模型诊断脚手架 `mj_diagnose`	模型加载、字段检查、力分解、接触诊断
S02（本章）	交互式控制实验台	在诊断脚手架基础上增加 viewer、控制器、日志、回放、调参
S03（下章预告）	GPU 并行训练环境	在实验台基础上增加批量仿真和 RL 训练接口

本章建议把零散代码整理成一个小实验台。目录可以按下面的责任划分：

interactive_control_lab/
  models/
    panda_scene.xml
  controllers/
    joint_pd.py
    diffik.py
    impedance.py
  tools/
    logger.py
    replay.py
    rate.py
  run_viewer_pd.py
  run_impedance_push.py
  run_mink_ik.py

最低可用能力如下：

能力	验收方式
passive viewer 主循环	空格暂停、单步、重置稳定工作
键盘调参	`[`/`]` 改变刚度，日志记录参数变化
鼠标或 mocap 目标	拖动目标后末端或机器人行为跟随变化
多频率调度	控制频率和渲染频率可以独立设置
外力扰动	按键触发短脉冲，日志记录扰动窗口
数据记录	保存 `time/qpos/qvel/ctrl/target/mode`
回放	状态回放画面与原实验一致

完成这个实验台后，再学习 S03 的 GPU 生态或 S04 的可微分仿真会更顺畅，因为你已经具备了"先把单环境闭环看清楚"的工程基本功。

延伸阅读 ⭐¶

资源	难度	阅读重点
MuJoCo Python 文档：https://mujoco.readthedocs.io/en/stable/python.html	⭐⭐	`mujoco.viewer`、`launch_passive`、named access、数组视图与 `.copy()`
MuJoCo Computation 文档：https://mujoco.readthedocs.io/en/stable/computation/	⭐⭐⭐	`mjData` 状态、`ctrl/qfrc_applied/xfrc_applied`、确定性计算管线
MuJoCo XML Reference：https://mujoco.readthedocs.io/en/stable/XMLreference.html	⭐⭐⭐	actuator 的 motor/position/velocity/general 语义
MJPC 项目仓库：`google-deepmind/mujoco_mpc`	⭐⭐⭐	iLQG、sampling planner、task XML 与 GUI 操作
Kevin Zakka 的 `mjctrl`	⭐⭐	梯度 IK、LM IK、diffIK、OSC 的最小实现
Kevin Zakka 的 `mink`	⭐⭐⭐	QP IK 的任务、限制和求解器接口
Khatib 1987 Operational Space Control	⭐⭐⭐⭐	操作空间惯性、动力学一致零空间和力控统一框架
Tassa, Erez, Todorov 2012/2014 iLQG/DDP 系列	⭐⭐⭐⭐	在线轨迹优化、控制受限 DDP、MuJoCo 行为合成
Williams et al. 2017 MPPI	⭐⭐⭐	指数加权、温度参数、采样控制与信息论推导

涉及 API、安装命令、平台要求和版本差异时，以官方文档和项目主页为准。

故障排查手册 ⭐⭐¶

故障现象	高概率原因	排查步骤	修正方向
viewer 开了但机器人不动	passive viewer 中没有调用 `mj_step`，或暂停标志一直为真	打印 `data.time` 是否增长，检查主循环是否进入 step 分支	明确 `paused/step_once` 状态，确保每个仿真步调用 `mj_step`
鼠标拖动扰动没有效果	没有调用 `viewer.sync()`，或脚本覆盖了扰动字段	在每步 `mj_step` 后调用 `viewer.sync()`，检查是否每步清零 `xfrc_applied`	只在自定义外力前清零，保留 viewer 同步时序
键盘调参偶发崩溃	回调里直接修改 `mjData` 或复杂控制器对象	让回调只改轻量状态，主循环统一读取状态并执行	使用 `UiState` 这类命令缓冲
同一控制器在不同电脑表现不同	控制周期跟随 wall time 或渲染帧率漂移	记录每步 wall time、`data.time` 和控制更新间隔	用仿真时间调度控制，用 wall time 只做实时播放
PD 控制高频抖动	`kp` 过大、`kd` 不足、力矩限幅太高或步长过大	扫描 `kp/kd`，记录控制饱和率和高频能量	降低刚度、增加阻尼、缩小控制周期或调整 actuator
阻抗控制接触时冲击大	把位置误差直接映射成过大末端力，没有限幅	记录 `J.T @ F` 和 `data.ctrl` 峰值	对末端力和关节力矩双重限幅，降低接触方向刚度
IK 靠近奇异点速度爆炸	使用无阻尼伪逆或阻尼太小	记录 Jacobian 最小奇异值和最大关节速度	使用阻尼伪逆或 LM，自适应调阻尼
mink 跟踪误差突然变大	关节限位或速度限制变成 active	画关节角、速度和限位线，检查 QP active set	接受误差增大，或重新设计目标和姿态正则
日志中每一帧状态都一样	保存了 NumPy 视图，没有 `.copy()`	检查 `qpos_history[0] is qpos_history[-1]` 的等价问题	记录所有 MuJoCo 数组时立即 `.copy()`
控制回放不能复现实验	没记录外力、mocap 目标、随机数或 warmstart	对比状态回放与控制回放的第一处偏离	扩展日志字段，保存初始状态和全部用户输入

章末练习 ⭐⭐¶

viewer 主循环实验 ⭐⭐：把 S2.4 的 passive viewer 代码改成三种模式：暂停、单步、实时运行。记录 data.time 与 wall time 的误差曲线，解释误差来自哪里。
频率分层实验 ⭐⭐：固定 model.opt.timestep=0.001，分别用 1000 Hz、500 Hz、100 Hz 更新同一个 PD 控制器。比较 RMS 误差、峰值力矩和抖动。
外力恢复实验 ⭐⭐⭐：给机械臂末端或四足基座施加 0.15 s 外力脉冲，比较不同阻尼下的恢复时间和超调。
日志回放实验 ⭐⭐：记录一次拖动目标的 IK 实验，分别实现状态回放和控制回放。解释两种回放方式的差异。
跨章综合题 ⭐⭐⭐⭐：结合足式方向 SRBD/Centroidal 低维模型、复合机器人方向多模态 MPC 的分层思想，以及本章 viewer/日志工具，设计一个"四足基座受推后恢复站立"实验。要求给出高层状态、低层控制频率、扰动方式、记录字段和验收指标。

S02 MuJoCo 交互式控制——MJPC / mjctrl / mink¶

前置自测 ⭐¶

本章目标 ⭐¶

S2.1 MJPC——MPC 最佳交互教具 ⭐⭐⭐¶

动机：MPC 理论太抽象，怎么办？ ⭐⭐¶

三种 Planner 对比 ⭐⭐⭐¶

iLQG 核心公式 ⭐⭐⭐¶

Predictive Sampling = MPPI 变体 ⭐⭐⭐¶

GUI 教学流程 ⭐⭐¶

S2.2 mjctrl——教材级单文件控制器 ⭐⭐¶

动机：IK 算法太多，哪个才对？ ⭐⭐¶

mjctrl 中的三个核心控制器文件 ⭐⭐¶

差分 IK 核心代码详解 ⭐⭐¶

与 MoveIt2 Servo 的对比 ⭐⭐¶

S2.3 mink——生产级 QP IK ⭐⭐¶

动机：差分 IK 不够用的时候 ⭐⭐¶

QP 公式化 ⭐⭐⭐¶

代码示例：多任务带约束 IK ⭐⭐¶

与 Pink（Pinocchio）的关系 ⭐⭐¶

何时用 mjctrl，何时用 mink？ ⭐⭐¶

S2.4 viewer 与 GUI：交互式控制的外壳不是装饰 ⭐¶

为什么 viewer 是控制系统的一部分 ⭐¶

managed viewer 与 passive viewer ⭐¶

最小 passive viewer 控制循环 ⭐¶

鼠标交互：内置扰动优先，自定义回调谨慎 ⭐¶

用 mocap body 做可拖动目标 ⭐¶

S2.5 实时循环、控制频率与仿真步长 ⭐¶

三个时间尺度 ⭐¶

频率分层调度器 ⭐¶

控制频率怎么选 ⭐¶

易错陷阱：步长、子步和增益耦合 ⭐¶

S2.6 PD、阻抗与外力扰动 ⭐⭐¶

从 data.ctrl 到真实力矩 ⭐⭐¶

关节 PD：最小、最常用、最容易误解 ⭐⭐¶

阻抗控制：不是"更软的 PD" ⭐⭐¶

外力扰动：xfrc_applied 与 qfrc_applied ⭐⭐¶

S2.7 数据记录、回放与交互调参方法 ⭐⭐¶

为什么交互调参必须记录数据 ⭐⭐¶

回放：复现现象，而不是复现猜测 ⭐⭐¶

交互调参的工程流程 ⭐⭐¶

调参指标表 ⭐⭐¶

阶段练习：先建立交互直觉 ⭐⭐¶

练习 1：MJPC Planner 对比实验 ⭐⭐¶

练习 2：mjctrl 奇异点实验 ⭐⭐¶

练习 3：mink 约束 IK vs 无约束 IK ⭐⭐⭐¶

练习 4：源码精读——iLQG 反向传播 ⭐⭐⭐¶

S2.8 iLQG 的完整推导与物理含义 ⭐⭐⭐¶

来龙去脉：从 Bellman 到 MJPC ⭐⭐⭐¶

从 Bellman 方程出发的完整推导 ⭐⭐⭐¶

与 DDP/Crocoddyl 章节的关系 ⭐⭐⭐¶

S2.9 Predictive Sampling 深入与 MPPI 统一视角 ⭐⭐⭐¶

从信息论推导 MPPI ⭐⭐⭐¶

温度 \(\lambda\) 的物理含义 ⭐⭐⭐¶

协方差适应：为什么固定 \(\Sigma\) 在高维不够 ⭐⭐⭐¶

与移动机器人 MPPI 章节的连接 ⭐⭐⭐¶

GPU 并行化的潜力 ⭐⭐⭐¶

S2.10 IK 算法族的数学统一与推导 ⭐⭐¶

统一框架：所有 IK 都是同一个最小二乘的变体 ⭐⭐¶

从无正则化开始：正规方程 ⭐⭐¶

梯度下降 IK：最保守的选择 ⭐⭐¶

Gauss-Newton IK：快速但脆弱 ⭐⭐¶

Levenberg-Marquardt IK：自适应的平衡 ⭐⭐¶

奇异性分析：三种算法在奇异点附近的行为 ⭐⭐¶

阻尼伪逆的完整推导（从优化视角） ⭐⭐¶

S2.11 Operational Space Control (OSC) 深入 ⭐⭐⭐¶

从 IK 到 OSC 的概念跨越 ⭐⭐⭐¶

Khatib 1987 框架 ⭐⭐⭐¶

mjctrl opspace.py 关键代码解读 ⭐⭐⭐¶

S2.12 mink QP 求解的数学细节 ⭐⭐⭐¶

QP 标准形式与 IK 的映射 ⭐⭐⭐¶

关节限位约束的构造 ⭐⭐⭐¶

碰撞避免约束 ⭐⭐⭐¶

多任务优先级：加权 QP 与零空间 ⭐⭐⭐¶

DAQP / OSQP / ProxQP：求解器选择 ⭐⭐⭐¶

代码示例：带关节限位约束的 mink IK ⭐⭐⭐¶

综合练习 ⭐⭐⭐¶

练习 5：从零实现 iLQG 求解 cartpole ⭐⭐⭐¶

练习 6：LM-IK 阻尼参数扫描 ⭐⭐¶

练习 7：OSC 零空间行为演示 ⭐⭐⭐¶

练习 8：mink 约束 IK 综合实验 ⭐⭐⭐¶

从 `data.ctrl` 到真实力矩 ⭐⭐¶

外力扰动：`xfrc_applied` 与 `qfrc_applied` ⭐⭐¶