MuJoCo 的计算流程与状态更新

给定某一时刻的关节角和速度，程序还要计算每个刚体的位置、惯量以及接触关系，才能得到加速度。首先分清哪些量描述当前状态、哪些量由状态推得，后面的计算顺序才有明确的输入与输出。

一个长方体的质量、形状和连接方式决定了它采用怎样的动力学方程；它当前位于哪里、运动多快，则决定这一次求值的结果。MuJoCo 用 mjModel 保存编译后的模型参数，用 mjData 保存一份运行实例。同一个模型可以配合多份数据，分别从不同状态出发。

mjData 中既有随时间积分的状态，也有控制输入和已经算出的中间量。三类数据会在同一数组集合中出现，使用时按它们的计算来源区分：

含球关节时，四个四元数分量对应三个独立角速度分量。一个自由关节对 qpos 占七项，对 qvel 占六项。因此配置更新要按关节类型进行：标量关节使用加法，姿态使用第三章的四元数增量。

N(q) 表示把广义速度转成配置表示导数的映射。标量移动关节对应单位映射；球关节对应 \dot q_{\rm quat}=q_{\rm quat}(0,\boldsymbol\omega_b)/2。实际配置积分由 mj_integratePos 按这一几何关系完成。

可以把当前状态理解为下一步求值的起点。例如，给一个自由刚体设置新的位置和四元数后，qpos 已经记录这一改变；碰撞包围盒、世界惯量和接触点还需要根据新配置重新计算。相同地，只改变控制时，几何可能仍可复用，而执行器力和加速度需要更新。数据之间的这种依赖关系决定了后面的计算顺序。

保存与恢复状态

上述变量构成常见刚体和执行器的主要状态。3.10.0 的 mjSTATE_PHYSICS 还包含用于延迟执行器、传感器的 history；完整物理状态再包括 time 和 plugin_state。精确恢复一次运行时，还要保存控制、施加力、等式开关与求解器热启动等输入。mjSTATE_INTEGRATION 将这些积分所需字段一起纳入。

int nstate = mj_stateSize(m, mjSTATE_INTEGRATION);
// buffer 已分配 nstate 个 mjtNum
mj_getState(m, d, buffer, mjSTATE_INTEGRATION);
// 稍后恢复这一份状态
mj_setState(m, d, buffer, mjSTATE_INTEGRATION);
mj_forward(m, d);

最后一次调用使几何、力和加速度与恢复的状态对应。若控制回调已安装，它也会在这次求值中运行。状态分类与接口说明见本地 simulation.rst 的 State components。

连续时间动力学要回答的是：在给定状态和输入下，此刻的速度将怎样改变？MuJoCo 的 mj_forward 依次完成五组计算，主结果为 qacc 和 act_dot。整个过程保持当前仿真时间。

父子位姿沿用式（6.2）的递推，质量矩阵描述式（7.2）中的加速度系数。两者由当前配置确定；速度项随后使用当前位置与当前速度，力和加速度再使用这些已经建立的量。

位置阶段先沿刚体树计算世界位姿，继而组装质量矩阵并分解；碰撞查询建立接触记录，约束装配确定本次的行数和雅可比。执行器传动也在这里求出长度和力臂。速度阶段沿已有结构计算运动量，并用广义加速度取零时的递归 Newton–Euler 计算求出 c(q,v)。

由配置建立本次计算的几何

位置阶段承接第六、八章的递推。父刚体的世界位姿与当前关节变换相乘，得到子刚体位姿；刚体惯量由各自的质心和主轴变换到计算坐标，再沿树形成 M(q)。质量矩阵完成分解后，后续每次给定一列力，都可以通过前代、回代求出一列加速度。一个状态中的平滑加速度求解与约束方向求解共用这一分解。

接触几何也依赖配置。两个表面距离多远、法向指向哪里，先由碰撞查询确定；接着将接触点速度投影到法向和切向，形成相应雅可比行。接触对发生变化时，约束行数也随之改变。由此可见，固定的关节自由度数与本次求值中的约束行数分别由模型拓扑和当前几何决定。

由速度建立偏置与被动力

速度阶段使用刚才得到的几何，将 v 传到各刚体和执行器。以一条标量关节为例，阻尼产生 -dv，弹簧产生 -k(q-q_*)；这里 q_* 是弹簧的参考位置。这些力可以在约束求解之前确定。偏置力 c(q,v) 则包括重力、向心和科里奥利效应：把广义加速度设为零，运行第八章的递归 Newton–Euler 计算，就得到维持该瞬时零加速度所需的广义力。

例如，单摆取角度 \theta、杆长 l、末端质量 m，有 M=ml^2、c=mgl\sin\theta。位置改变后，世界位置和重力矩需要重算；只改变角速度时，几何保持原值，而阻尼力随之改变。在多连杆中，质量矩阵也通常随配置变化，偏置力中还会出现速度的二次项。

位置、速度和加速度传感器分别在相应结果准备好后求值。控制回调位于速度阶段之后，因而可以读取当前的位姿和速度，用它们生成随后所需的控制输入。

第三至五节分别建立传动、质量求解和约束修正的关系。第六节把同一组力依次代入，并用两质点图展示每一步怎样改变后续计算。

各阶段会读取此前留下的数组。以质量矩阵为例，改变配置后，下一次求解就要先重新组装和分解。第八节将据此说明阶段复用的条件。

考虑一根连接机构的绳索。关节转动会改变绳长；绳索拉力也会沿同一几何关系产生关节力矩。关节、绳索等常用长度传动用标量 l_i(q) 描述，将长度对配置运动的瞬时变化排列成矩阵 P：

f 收集各执行器的标量力，\tau_a 是它们合成的广义力。MuJoCo 将传动的这些系数保存为稀疏的 actuator_moment，长度和速度分别保存在 actuator_length、actuator_velocity。

读矩阵 P 时，每一行对应一路执行器，每一列对应一个广义速度。某个系数表示：该自由度以单位速度运动时，这一路传动长度以多快的速度变化。对铰链，系数中会含力臂；对直接驱动的移动关节，它可以是无量纲传动比。因此传动把“长度变化”和“广义力方向”联系起来，矩阵转置由功率相等确定。

在图中令 v_1=1,v_2=-1/2，第二路传动速度变为零。它仍可对两个质点施力，但这一瞬间两项功率互相抵消。第一路速度保持为一，其功率直接等于第一路标量力。这个状态能同时核对 P 的行含义和 P^{\mathsf T} 的力回传方式。

力的生成

直接力控制可以令 f_i=k_i u_i。位置伺服则根据目标与当前传动长度之差产生力，例如 f_i=k_p(u_i-l_i)-k_d\dot l_i。这时 ctrl 保存目标长度，其单位随传动定义。控制输入的物理含义由执行器模型给定。

启用限幅后，执行器先按控制范围取得有效输入，再计算力，并按执行器力范围限制输出；关节级的执行器总力限制在广义力合成后应用。一个区间的限幅可写为 \operatorname{clip}(x,l,h)=\min(h,\max(l,x))。MuJoCo 的黏附等特殊传动直接从接触几何构造广义力方向，其 actuator_moment 按该传动定义读取。

带内部状态的执行器先由控制驱动激活，再由激活产生力。一阶滤波是一个简单例子：

这里 \tau 是时间常数，控制和激活具有相同单位。即使控制瞬间变化，激活也要沿自身的微分方程演化。MuJoCo 分别用 act 和 act_dot 保存当前激活及其导数。

在一段时间内保持控制恒定，激活与目标的差按指数衰减：

经过一个时间常数，剩余误差约为原来的 e^{-1}\approx0.368。控制从零阶跃到一、初始激活为零时，这就对应激活上升到约 0.632。时间常数因而给出了可以从图上直接辨认的响应快慢。

固定时间常数并减小步长，橙色离散点逐渐靠近绿色解析曲线。Euler 每一步沿当前斜率前进；激活上升时，实际斜率持续减小，所以在图中的正步长范围内，Euler 点通常位于解析曲线上方。这个误差会通过 f=kz 继续影响力，随后进入质量方程。

第七、八章得到的广义动力学方程可以写为

\tau_p 为弹簧、阻尼等被动力，\tau_a 为执行器广义力，\tau_u 为直接施加的用户广义力。用户对刚体施加的笛卡尔力和力矩，经相应雅可比转置投影后组成 \tau_x。最后一项收集尚待求解的约束力。

将已知各项相加，得到平滑广义力，再解一次质量矩阵方程：

a_u 称为平滑加速度，对应 qacc_smooth。实际求值使用已经分解的质量矩阵，通过回代求解。第八章给出的稀疏分解可以在这一位置直接接入。

为便于计算，式（9.4）的四类已知力在求和后只保留一个合力记号 \tau_0。后面的推导只需记住 Ma_u=\tau_0；各数组名称用于最后的源码对照。这里的“平滑”表示约束修正之前的运动响应。接触法向、摩擦和关节限位等力，将在下一阶段依照各自的力学条件共同确定。

若外力作用在刚体上的某一点，点速度雅可比 J_x 将广义速度变成该点速度；自由力矩对应角速度雅可比 J_\omega。它们对已知力的贡献为 \tau_x=J_x^{\mathsf T}\boldsymbol F+J_\omega^{\mathsf T}\boldsymbol T。各雅可比与力都采用同一个世界坐标系，才能直接按下式核对功率。

在源码中，qfrc_passive - qfrc_bias + qfrc_applied + qfrc_actuator 先组成 qfrc_smooth；mj_xfrcAccumulate 再加入 xfrc_applied 的投影，随后由 mj_solveLD 求得 qacc_smooth。

两条滑轨上的质点若由保持相对位移的理想连接件约束，就需要 q_2-q_1=d。对时间求导，它要求两者速度相同；再求导，要求两者加速度相同。约束力承担了将自由运动改为共同运动的作用。

这个式子由式（9.4）减去式（9.5）得到。J^{\mathsf T}\lambda 是约束广义力，质量矩阵的求解将它转成加速度修正。

一条理想等式约束可以分成“运动要求”与“力的响应”两部分。运动要求写为 Ja=b；b 给出几何曲率要求的加速度。单位约束力经过质量矩阵后产生一列加速度，记为 y。将它再投影回约束方向，得到标量 A：

分子是约束方向上尚缺少的加速度，分母是单位约束力能造成的加速度；两者相除就是需要施加的力。这个顺序把嵌套乘积拆为一次质量求解、一次投影和一次标量除法，每一步都有明确的量纲。

MuJoCo 把接触、关节限位、等式与摩擦等约束装配成行，并结合软约束和摩擦模型求解。约束阶段计算 efc_b = J*qacc_smooth - efc_aref，将当前平滑运动与参考目标作比较；最后产生 efc_force、qfrc_constraint 和 qacc。第十一至十四章将分别建立这些参考量和力的条件。这里的理想等式算例用于展示质量求解与力回传的完整关系。

取两个独立的直线移动坐标。在本次状态上，质量、传动与力为

常量偏置可以由线性势能 V(q)=2q_1+3q_2 对应的恒定载荷产生，系数单位为牛顿；被动力与用户力采用上述本次取值。约束为 q_2-q_1=0，因此 J=(-1,1)，且 \dot J=0。

这组数据的完整结果为：传动后的执行器力 (3,-2)^{\mathsf T}\,\mathrm N，平滑加速度 (1,-2)^{\mathsf T}\,\mathrm{m/s^2}，约束力大小 2\,\mathrm N，最终两者加速度均为零。下面按同一顺序展开每个中间量；图中的每个阶段都对应其中的一步。

下面的实验 03 和配套内核使用这组数据。关闭等式约束后，两者分别按 1 与 -2\,\mathrm{m/s^2} 加速；打开约束后，求解器通过一对等大反向的力使加速度相同。

初始数据下，质点一的平滑合力为正，质点二为负，两者会相向加速。约束施加负向和正向各 2\,\mathrm N 的力，恰好抵消这两项。把施加比例记为 \alpha，图中的中间加速度为 a(\alpha)=a_u+\alpha y\lambda，相对加速度为 Ja(\alpha)=-3(1-\alpha)\,\mathrm{m/s^2}。滑块展示同一个约束修正的组成过程；完整约束解位于 \alpha=1。

再把两路执行器力改为 (2,2)^{\mathsf T}\,\mathrm N，两个质点的平滑加速度变为 1 与 2\,\mathrm{m/s^2}，共同加速度为 4/3\,\mathrm{m/s^2}。约束使它们共同运动，系统整体是否加速则由总外力决定。连接件的一对内力在求整体合力时互相抵消。

完成一次动力学求值后，积分器把状态推进到下一时刻。对本章标量坐标模型，半隐式 Euler 先更新速度，再用新速度更新位置：

含姿态时，第二步调用配置积分，用新角速度构造四元数增量。有状态执行器还需要推进激活；其离散公式由所选激活动力学决定。MuJoCo 的 Euler 分支也包含关节阻尼的隐式处理，因此具体更新所用加速度可能经过阻尼求解修正。第十五章推导这些积分器。

积分后的缓存时间层

以本章没有隐式阻尼修正的单步示例为例，先在 t_k 求位姿、力与加速度，再把 qpos、qvel 和 time 推进到 t_{k+1}。这时配置已经改变，先前计算的世界位姿与接触仍保存着旧计算结果。

调用 mj_step 后需要立即读取新状态上的派生量时，可以再调用 mj_forward。它保持新状态的时间，同时计算与新配置、速度对应的几何、力和加速度。经典四阶 Runge–Kutta 方法（Runge–Kutta method of order four, RK4）会在一步内对多个临时状态求值，其返回缓存可能来自内部阶段；统一的判据是：积分之后，所需派生量应在最终状态上重新计算。

在第三阶段，绿色圆点已经移动，橙色方框仍停在旧位置；进入第四阶段后，方框追上新配置，但仿真时间保持原值。两次变化分别对应积分和重新求值。由此可以判断一个读数的时间含义：配置由积分推进，几何缓存由相应计算阶段刷新。

本地 engine_forward.c 第 1920 行起实现 mj_forwardSkip。它按位置、速度、执行器、平滑加速度、约束的次序调用各阶段。mj_step 先运行完整前向计算，再根据 opt.integrator 选择积分器。

在当前观测之后设置控制

mj_step1 完成位置和速度相关计算，让控制器读取这些新结果。随后可以写入控制和外力，再由 mj_step2 完成力、加速度与积分：

mj_step1(m, d);
// 使用本次已计算的位姿与速度，写入 ctrl 或施加力
set_control(m, d);
mj_step2(m, d);

在两次调用之间保持配置、速度和模型一致，使前两阶段结果继续有效。已安装的 mjcb_control 会在 mj_step1 末尾调用一次，随后手动写入的控制会成为 mj_step2 的输入。

本地第 2062 行的分支表明：implicit 和 implicitfast 在 mj_step2 中使用隐式积分，其余选项使用 Euler；RK4 选项在这一路径下也进入 Euler。使用 RK4 并使反馈控制随内部状态更新时，将控制律放入 mjcb_control，由完整 mj_step 在各求值阶段调用。

复用已经计算好的阶段

研究同一配置附近的控制响应时，可以保留质量矩阵和雅可比。mj_forwardSkip 的参数指定已经有效的前段结果：

例如，先在一个状态上完整求值，只修改 ctrl，再用 mjSTAGE_VEL 重新求加速度；改变速度而保持位置阶段的全部输入时，可从速度阶段继续。复用的判断依据是缓存的全部依赖保持一致，其中包括模型、运动捕捉位姿、约束启用状态、休眠状态与相关回调输入。每次正常时间推进之后，通常从完整前向计算开始。

最后一个参数 skipsensor 控制是否重算传感器。选择跳过时，传感器数组保留已有值。控制回调仍按函数中的位置调用，因此比较不同控制样本时，要让回调与手动设置输入采用一致的策略。

官方说明见前向动力学与阶段复用、数据一致性。具体版本行为以本章链接的本地 3.10.0 源码为准。

pipeline.py 将两质点模型写成 Model 与 Data，实现传动、质量求解、单行理想等式、阶段缓存和半隐式更新。其 forward 保持当前状态，step 推进状态并保留本步派生量；缓存键检查展示复用所需条件。激活函数单独实现恒定输入解析更新。

from pipeline import Model, Data, forward, step
m, d = Model(), Data()
forward(m, d)
print(d.cache["smooth"], d.cache["acceleration"])
step(m, d, 0.1, constrained=False)
print(d.q, d.cache["position"])
forward(m, d)
print(d.q, d.cache["position"])

在笔记目录运行独立核对，检查手算结果、传动功率、约束动力学残差、阶段复用及时间层：

python3 examples/check_pipeline.py