Pinocchio 浮动基专题 06：解析导数、流形线性化与控制接口

前面已经能算位姿、速度、逆动力学和前向动力学。接下来真正进入控制器时，问题变成：状态或命令改变一点，下一步加速度、位姿误差和离散状态会怎么改变？

这不是把 compute...Derivatives 的返回值塞进一个矩阵就结束了。导数必须和扰动定义、执行器映射、积分器以及误差坐标同时匹配。本篇固定 Pinocchio v3.8.0，提交 655877b314baed68c7e2d4dd56b0a0200bb9f98e；所有代码是教学片段，未在 Pinocchio 环境中执行，不提供虚构的数值结果。

从专题入口进入；需要先理解01：模型与流形、02：动力学源码与03：数值自检。本篇不重复安装步骤，重点完成“源码递归 → 偏导合同 → 离散线性模型 → 控制接口”的连接。

1. 先确定线性化的空间

以 free-flyer 加两个转轴为例，状态存储是 q ∈ R^9 与 v ∈ R^8；其中四元数还有单位约束。若优化器直接把 17 个存储数都当独立状态，会引入没有物理意义的四元数径向自由度。

在参考状态 x_bar=(q_bar,v_bar) 附近，本篇选择：

$\delta x=\begin{bmatrix}\delta q\\\delta v\end{bmatrix}\in\mathbb R^{2n_v},\qquad q=\operatorname{integrate}(\bar q,\delta q),\quad v=\bar v+\delta v.$

这里 δq 是切空间变量，绝不是 q-q_bar。速度部分使用 Pinocchio 所用的局部/平凡化坐标数值之差；这是本篇明确选定的状态图。它不表示两个不同姿态下的局部速度对应同一个世界向量；若你的误差定义要求世界速度相同，就必须加入坐标变换和链式法则。

因此这个模型的误差状态是 16 维，不是 17 维。储存维度、误差维度和执行器数应分别命名，例如 nx_storage=nq+nv、ndx=2*nv、nu=2。

接口返回量	尺寸	求导时固定什么
RNEA 的 `dtau_dq`	`nv × nv`	`v,a` 的数值坐标、模型和外力参数
RNEA 的 `dtau_dv`、`dtau_da`	`nv × nv`	另外两类输入
ABA 的 `ddq_dq`、`ddq_dv`	`nv × nv`	`tau` 及另外一类状态输入
ABA 的 `ddq_dtau`	`nv × nv`	`q,v`
离散控制矩阵 `B`	`2nv × nu`	选定误差图和离散步进器

不要根据变量名中的 q 就推断列数必须等于 nq。这个版本在 RNEA 导数入口明确检查各矩阵的行列都是 model.nv。

2. RNEA 的解析导数沿着树传播什么？

先看单个刚体的概念关系，以下量均表达在同一个刚体坐标系，X_i 是父运动到子运动的变换：

$v_i=X_i v_p+S_i v_{J_i},\qquad a_i=X_i a_p+S_i a_{J_i}+c_i,$

$f_i=I_i a_i+v_i\times^{*}(I_i v_i)-f_i^{ext}.$

c_i 在这里统称关节与速度相关的偏置项。对一个扰动求微分，不能只微分最后的 tau=S.T f：父变换、运动子空间、速度乘积以及向父节点累计的力都会变化。

例如，在固定局部惯量的表示下：

$\delta v_i=(\delta X_i)v_p+X_i\delta v_p +(\delta S_i)v_{J_i}+S_i\delta v_{J_i},$

$\delta f_i=I_i\delta a_i +(\delta v_i)\times^{*}(I_i v_i) +v_i\times^{*}(I_i\delta v_i)-\delta f_i^{ext}.$

这两式用于理解链式法则，不是对源码的逐行翻译。实际 v3.8.0 的导数实现大量在世界表达中共享中间量；当惯量变换到世界表达后，惯量的变化也要算进去，不能再把上式里的局部常量 I_i 直接照搬。

2.1 正向 visitor：为导数准备运动学与惯量变化

打开 rnea-derivatives.hxx 的 ComputeRNEADerivativesForwardStep，重点追踪：

jmodel.calc 产生当前关节变换、运动子空间和关节速度；liMi/oMi 把它们接到整棵树。
v/a 与 ov/oa/oa_gf 分别携带局部、世界表达，以及包含重力偏置的加速度。
J/dJ/dVdq/dAdq/dAdv 保存当前关节列块相关的运动学敏感性。
oYcrb/oinertias/doYcrb 保存世界表达惯量及其变化；oh/of 保存动量与力。

这些 6 × nv 工作区中的关节列块是递归使用的中间量，不是“每个刚体的完整雅可比都简单存在同一块里”。必须结合 jointCols、idx_v 和后续子树操作解释，不能看到一个字段名字就直接拿去做控制矩阵。

2.2 反向 visitor：投影力导数并合并子树

ComputeRNEADerivativesBackwardStep 由叶到根累计复合惯量、力及其变化，通过运动子空间/世界雅可比列块投影到广义力。dFda 相关投影构造加速度导数，dFdq/dFdv 参与配置和速度导数；祖先—子树之间的块由 nvSubtree 等索引写入。

所以解析导数不是“对每个坐标调用一次完整 RNEA”，也不需要选有限差分步长。它复用树结构和动力学中间量。输出完整稠密矩阵仍有存储与计算成本，不能把“RNEA 本身线性复杂度”直接推广成“完整导数矩阵永远也是线性成本”。固定版本递归实现

函数结束还会处理重力相关缓存恢复以及 armature 对力矩和加速度导数对角线的贡献。理解这一点后，dtau_da = M 就不只是一个记忆公式，而是同一惯量投影的结果。

3. 从逆动力学导数得到前向动力学导数

无接触约束时，写成隐式关系：

$g(q,v,a)-\tau=0,\qquad g=\operatorname{RNEA},\quad a=\operatorname{ABA}(q,v,\tau).$

在同一个解 a 上微分，令 G_q=∂g/∂δq、G_v=∂g/∂v，得到：

$G_q\delta q+G_v\delta v+M\delta a-\delta\tau=0.$

若 M 可逆，立即有：

$F_q=-M^{-1}G_q,\qquad F_v=-M^{-1}G_v,\qquad F_\tau=M^{-1}.$

关键是 RNEA 导数必须在 ABA 算出的同一个 a 上求。 任意选一组加速度算 G_q，再与另一个力矩对应的 ABA 导数比较，通常不会成立，因为 G_q 含有 M(q)a 的配置变化。

# 假定 pin,np,model,q,v,tau 已按同一版本和约定准备。
forward_data = model.createData()
a = pin.aba(model, forward_data, q, v, tau).copy()
inverse_derivative_data = model.createData()
Gq, Gv, M = tuple(x.copy() for x in pin.computeRNEADerivatives(
    model, inverse_derivative_data, q, v, a
))
forward_derivative_data = model.createData()
Fq, Fv, Ftau = tuple(x.copy() for x in pin.computeABADerivatives(
    model, forward_derivative_data, q, v, tau
))
checks = {
    "q_implicit": np.max(np.abs(M @ Fq + Gq)),
    "v_implicit": np.max(np.abs(M @ Fv + Gv)),
    "tau_inverse": np.max(np.abs(M @ Ftau - np.eye(model.nv))),
}

这使用残差检验，避免为了比较而额外构造 M.inverse()。若自己从 RNEA 导数计算 Fq/Fv，应复用矩阵分解求解 M Fq=-Gq，而不是逐元素求倒数。

源码并非只给出抽象关系：aba-derivatives.hxx 在构造前向动力学中间量、Minv 和逆动力学导数后，确实有 -Minv * dtau_dq 与 -Minv * dtau_dv 的组合。ABA 导数实现

包装层的第三项不要读错

Python computeABADerivatives(model,data,q,v,tau) 返回的第三项是 data.Minv，也就是 ddq_dtau。v3.8.0 说明字符串末尾把它写成 ddq_da，但实际 make_tuple 内容清楚表明是 Minv；不能按那句笔误解释成“加速度对加速度的导数”。Python 包装

RNEA 与 ABA 导数包装都返回指向 Data 的引用视图。上面保留具名 Data，并立即复制所有返回项；下一次调用之前，不能把仍然共享内存的矩阵当成旧时刻快照。

4. 外力、执行器和缓存改变的是哪个合同？

带外力重载为 computeRNEADerivatives(model,data,q,v,a,fext) 与 computeABADerivatives(model,data,q,v,tau,fext)；fext 是 model.njoints 个局部关节 wrench。上述隐式关系仍成立，但两端必须使用同一份外力定义，并在偏导中固定同样的外力数值。

若真实外力由 fext(q,v,u) 给出，例如弹簧或按世界坐标给定的载荷，API 的固定参数偏导不是闭环总导数。需要把 ∂a/∂fext 与外力模型的导数接上。已表达在世界原点的固定 wrench，变到关节局部应使用完整力变换 oMi.actInv；不同作用点还需要力臂，不能只旋转线性三分量。

令执行器映射为 tau = S u，其中 S ∈ R^(nv×nu)。本专题自由基两关节模型中，S 的前六行是零，后两行是单位矩阵，所以 F_u=F_tau S，不是把 F_tau 全部列直接交给两维控制输入。

如果 S 随配置变化，F_q 还需要加入 F_tau * ∂(S(q)u)/∂δq。若电机命令经 PD、饱和、延迟或滤波才形成力矩，也要对那一层建模；动力学库不会替你线性化整个控制链。

还有接受更少参数、复用已有 ABA 缓存的导数重载。不要在一次默认 LOCAL ABA 后盲目调用最短重载：固定版本的上游缓存复用测试先调用 aba(..., Convention::WORLD)，再求缓存版导数。初学和回归基线优先使用含 q,v,tau 的完整重载；需要优化时逐项确认缓存生产者、表达约定和外力一致性。上游复用测试

5. `dIntegrate` 与 `dDifference`：把两处切空间接起来

定义 r(q,w)=integrate(q,w)。对输入配置和切空间增量分别求导：

D0 = pin.dIntegrate(model, q, w, pin.ArgumentPosition.ARG0)
D1 = pin.dIntegrate(model, q, w, pin.ArgumentPosition.ARG1)
# 也可以：D0, D1 = pin.dIntegrate(model, q, w)

ARG0 指配置参数 q，ARG1 指增量参数 w；不是 free-flyer 的平移段和旋转段。两个矩阵都是 nv × nv，输出扰动用 r(q,w) 附近的切空间表达：

$\operatorname{difference}(r(q,w),r(\operatorname{integrate}(q,\eta),w)) =D_0\eta+o(\|\eta\|).$

对第二参数，则把右边改为 D1 eta，内层输入改为 w+eta。返回的不是 nq × nv 的四元数存储数组导数。 此解释来自 joint-configuration.hpp，Python 的两个重载可从 expose-joints.cpp 核对。

再定义 d(q0,q1)=difference(q0,q1)，H0/H1=dDifference(...) 对两个配置参数分别求切空间导数。若 q1=integrate(q0,w) 且没有跨越对数分支，局部恒等式 d(q0,r(q0,w))=w 给出：

$H_1D_1=I,\qquad H_0+H_1D_0=0.$

它们是很好的接口自检：一条检验增量变化，另一条检验同时移动积分起点的链式法则。在 w=0 时得到 D0=D1=I、H0=-I、H1=I；不能把这个零增量特例推广到有限旋转增量。

差分验证时，输入配置用 integrate 扰动，输出配置用 difference 比较；只有输入、输出都已是同一线性空间里的数值，才使用普通减法。

6. 明确一步映射后，才能构造离散 A、B

选用与第 03 篇一致的一阶步进，令时间步长 h>0：

$a=f(q,v,Su),\qquad v^+=v+h a,\qquad q^+=\operatorname{integrate}(q,hv^+).$

这里速度先更新。若实际仿真器用的是 integrate(q,hv) 或多阶段方法，下面的矩阵必须重推；不能以“反正都是 Euler”混用。

令 Fq,Fv,Fu 是加速度对配置、速度、输入的偏导，D0,D1 在 (q,hv_plus) 处计算。链式法则给出速度块：

$V_q=hF_q,\qquad V_v=I+hF_v,\qquad V_u=hF_u.$

再把速度变化传进 integrate 的第二参数，得到：

$A=\begin{bmatrix} D_0+h^2D_1F_q & hD_1(I+hF_v)\\ hF_q & I+hF_v \end{bmatrix},\qquad B=\begin{bmatrix}h^2D_1F_u\\hF_u\end{bmatrix}.$

上式输出误差参考点是名义映射自身产生的 (q_plus,v_plus)，状态误差图采用第 1 节的约定。位置部分不是 I+hFq；h² 来自“加速度先改变速度，再改变本步配置”的两次时间缩放。

def linearize_step(model, q, v, u, S, h):
    # 教学片段：调用方须检查有限性、尺寸、版本和 h>0。
    # 实时使用时应把 Data/矩阵移到循环外复用。
    tau = S @ u
    aba_data = model.createData()
    acc = pin.aba(model, aba_data, q, v, tau).copy()
    derivative_data = model.createData()
    Fq, Fv, Ftau = tuple(x.copy() for x in pin.computeABADerivatives(
        model, derivative_data, q, v, tau
    ))
    Fu = Ftau @ S
    vp = v + h * acc
    qp = pin.integrate(model, q, h * vp).copy()
    D0, D1 = pin.dIntegrate(model, q, h * vp)
    Vq, Vv, Vu = h * Fq, np.eye(model.nv) + h * Fv, h * Fu
    A = np.block([[D0 + h * D1 @ Vq, h * D1 @ Vv], [Vq, Vv]])
    B = np.vstack((h * D1 @ Vu, Vu))
    return qp, vp, A, B

对本专题模型，A.shape==(16,16)、B.shape==(16,2)。没有出现一个四元数径向误差状态，也没有给自由基凭空增加六个执行器。

参考下一状态不是预测下一状态时

多重打靶中，优化变量 q_ref_next 不一定等于动力学产生的 q_plus。缺陷应写成 difference(q_ref_next,q_plus)，还要加上速度部分 v_plus-v_ref_next 的残差。

此时配置行的雅可比需要左乘 H1=dDifference(model,q_ref_next,q_plus,ARG1)；对参考下一配置这个独立优化变量求导，则使用 H0。在不满足动力学的初值上直接使用上面的 A，同时忽略缺陷和 H1，优化器得到的就不是实际残差的线性化。

7. 验证 A、B 要差分完整一步，而不只差分 ABA

先运行名义一步得到 (q_nom_next,v_nom_next)。对每个配置扰动列使用 integrate(q,±eps e_j)；对速度和输入列分别使用 v±eps e_j、u±eps e_j。每一侧完整重跑选定的一步映射。

把两侧结果分别映射到同一个输出误差图：

$z_\pm=\begin{bmatrix} \operatorname{difference}(q_{nom}^{+},q_\pm^{+})\\ v_\pm^{+}-v_{nom}^{+} \end{bmatrix}.$

然后用 (z_plus-z_minus)/(2eps) 比较 A 或 B 的对应列。不要直接减 q_plus 的四元数系数，也不要为了图省事，把输出误差参考点分别设成不同的扰动结果。

应同时扫描 eps 与时间步长，先检验单状态隐式导数关系，再检验 dIntegrate/dDifference 恒等式，最后检验整步 A/B。若只有配置行失败而速度行正确，优先检查 D0/D1、步进顺序与输出误差图，而不是立即怀疑 ABA。

8. `Jlog6` 的负号：从误差定义推到一步 IK

设末端位姿 X(q) 和目标 Xd 都从末端/目标坐标映射到世界，选取：

$E=X(q)^{-1}X_d,\qquad e=\log_6(E).$

使用末端 LOCAL 雅可比 J_L，小配置扰动对应右乘运动：X(q⊕δ) ≈ X(q) exp6(J_L δ)。因此：

$E(q\oplus\delta)\approx\exp_6(-J_L\delta)E =E\exp_6(-\operatorname{Ad}_{E^{-1}}J_L\delta).$

Pinocchio 的 Jlog6(E) 定义为右扰动 log6(E exp6(xi)) 对 xi 的导数。于是：

$J_e=-Jlog_6(E)\operatorname{Ad}_{E^{-1}}J_L =-Jlog_6(E^{-1})J_L.$

第一个负号来自对当前位姿求逆；第二个表达利用左右扰动转换。它与官方 IK 示例一致，右导数定义见 explog.hpp。换成 log6(Xd.inverse()*X)，就不能继续照抄这一套符号。

下面将关节示例改成一般 frame 的误差查询，显式准备雅可比和 frame 位姿：

def pose_error_jacobian(model, q, frame_id, target):
    data = model.createData()
    pin.computeJointJacobians(model, data, q)
    pin.updateFramePlacements(model, data)
    E = data.oMf[frame_id].actInv(target)
    err = pin.log6(E).vector.copy()
    Jlocal = pin.getFrameJacobian(
        model, data, frame_id, pin.ReferenceFrame.LOCAL
    ).copy()
    Jerr = -pin.Jlog6(E.inverse()) @ Jlocal
    return err, Jerr

# 单次阻尼最小二乘 + 回溯；不是完整碰撞/限位约束求解器。
err, Jerr = pose_error_jacobian(model, q, frame_id, target)
W = np.diag([1 / 0.5] * 3 + [1 / 1.0] * 3)  # 特征长度0.5m、角度1rad
ew, Jw = W @ err, W @ Jerr
damping = 1e-3
delta = -Jw.T @ np.linalg.solve(Jw @ Jw.T + damping**2 * np.eye(6), ew)
accepted = False
for alpha in (1.0, 0.5, 0.25, 0.125, 0.0625):
    candidate = pin.integrate(model, q, alpha * delta).copy()
    candidate_err, _ = pose_error_jacobian(model, candidate, frame_id, target)
    if np.linalg.norm(W @ candidate_err) < np.linalg.norm(ew):
        q = candidate
        accepted = True
        break

外层循环必须先检查平移/旋转容差，再执行这一步；若 accepted 为假，报告无下降步并停止或调整阻尼，不能假装已经收敛。这里 alpha 是配置更新步长，不必解释为真实经过的秒数；若改成速度控制，还需采样时间、速度限制和低层执行器闭环。

自由基 IK 允许调整基座位姿；几何上找到 q 不代表欠驱动系统能到达它。把基座列删掉可用于“固定基座几何 IK”的另一个问题，但不能替代浮动基动力学约束。接近旋转对数分支、不可达目标、碰撞或关节限位时，这个无约束下降法没有成功保证。

9. 连续导数不等于完整控制系统的导数

Pinocchio 提供刚体算法的导数，不直接替你生成 NMPC/OCS2 的完整模型。还需明确参考轨迹、执行器选择、约束、代价权重、离散步进、延迟和状态估计误差。传给控制器的状态维度、切空间维度、输入维度要在接口边界分别校验。

例如，以欧氏写法机械套用 A_cont=[[0,I],[Fq,Fv]]，会忽略本篇配置误差图在运动参考轨迹上的变化。离散链式推导中的 D0 正是在处理这种几何关系；不要在运动的 free-flyer 上先假定配置误差导数就是速度误差。

质心动量的导数涉及 h_g=A_g(q)v、质心位置和所选原点；它不是从 Fq 随便截取六行。接触动力学的导数还依赖接触集合、约束雅可比、求解器与正则化；一旦发生落地、离地或摩擦模式切换，光滑无接触 ABA 导数不足以描述整个切换。相关模型边界见05：接触约束与质心动力学。

10. 建议的检查顺序与当前边界

在无接触固定模型上，先完成 03 的八项基线，确认原有坐标与动力学合同。
对同一 a=ABA(q,v,tau) 检查 M Fq+Gq、M Fv+Gv、M Ftau-I。
以小而非零的 w 检查 H1 D1=I、H0+H1 D0=0，再检验完整离散 A/B。
对 IK 的 Jerr 做切空间差分，记录误差定义、参考系、eps 扫描和分支距离。
最后才引入外力模型、执行器非线性、接触或优化器，不要同时改变全部假设。

这些是待执行的深化实验，不是下载脚本已经覆盖的全部项目。本次核对的是固定版本的实现与绑定，并检查文章/代码片段的静态结构；未运行导数数值、IK 收敛、离散控制器或 Docker 仿真。真正补运行报告时，应像第 03 篇那样保存模型与输入快照、版本、每项残差及失败退出状态。