Pinocchio 浮动基专题 01：Model、Data 与 SE(3) 状态定义

从一个最容易写错的问题开始

给机器人底座一个向前的速度，为什么它沿世界坐标系的侧面移动？为什么配置向量有 7 个数，速度却只有 6 个数？为什么 q + v * dt 在固定基机械臂中似乎能工作，换成浮动基就不对了？

这些并非调用习惯上的小差别，而是 Pinocchio 对状态、参考系和李群运算的具体约定。先把这些约定读懂，再读 RNEA、CRBA 和 ABA，源码里的变换方向、矩阵维度才不会变成猜谜。

本专题固定阅读官方 v3.8.0，对应提交 655877b314baed68c7e2d4dd56b0a0200bb9f98e。这不是“当前最新版”的声明；不同版本请对照实际源码，不要混用滚动文档里的新接口。固定版本源码

六篇文章按实现链路逐层展开，完整入口在 Pinocchio 专题：

• 本篇：模型组织、浮动基的状态定义，以及 integrate / difference。
• 专题 02：浮动基动力学源码：跟踪 RNEA、CRBA、ABA 的递归与浮动基力。
• 专题 03：数值技巧与接口验证：用可检查的恒等式验证接口、导数与数值实现。
• 专题 04：完整 C++ 工程：从手工建模到缓存、矩阵与导数调用，附可下载源码和 CMake 配置。
• 专题 05：接触与质心动力学：推导欠驱动基座消元、接触约束与动量方程，对照约束求解接口。
• 专题 06：解析导数与控制线性化：追踪递归导数，用流形链式法则构造离散 A/B，并解释位姿 IK 的误差方向。

原来的 ABA 笔记保留，适合作为递归算法的入门背景。本篇中的代码是教学示例，运行环境与实测范围以专题 03 的说明为准；源码核对不等于这些片段已在每个 Docker 镜像里执行通过。

Model 和 Data：结构与计算现场分开

Model 回答“机器人是什么”，Data 回答“这次计算把结果存在哪里”。不是把 Model 理解成当前机器人状态、把 Data 理解成另一个状态副本。

字段定义可对照 model.hpp 和 data.hpp。

典型用法是在模型结构确定后创建工作区：

import pinocchio as pin

model = pin.buildModelFromUrdf(
    "robot.urdf", pin.JointModelFreeFlyer()
)
data = model.createData()

q、v、a 通常是调用者维护的输入数组；不要期待修改某个 q 后 data 会自行变成最新状态。重新建模或增删关节后应重新创建匹配的 Data。共享只读模型、每个并行任务使用独立 Data，是从这种可写工作区设计得到的工程做法，不是说“名字叫 Data 所以天然线程安全”。

Joint、Body 和 Frame 为什么不是一一对应

Model 默认保留编号 0 的 universe。它是树的起点，不是一个需要在 q 中填写数值的实际驱动关节。

一个固定安装的相机可以有自己的 Frame，但不需要额外增加 nq 或 nv。URDF 中的固定连接也不等于可动关节：解析器会处理固定变换与惯量归并，同时保留相应 Frame。因此，URDF 里的 link 数、model.njoints、model.nframes 不应强行对应。URDF 固定关节处理

调试时先打印名称和索引，不要把 Frame ID 当 Joint ID：

for jid in range(1, model.njoints):
    joint = model.joints[jid]
    print(
        jid, model.names[jid], joint.shortname(),
        joint.idx_q, joint.nq, joint.idx_v, joint.nv
    )

frame_id = model.getFrameId("base_link")
if frame_id >= model.nframes:
    raise ValueError("模型中没有 base_link Frame")

未找到名称时不能只检查“返回值非负”。getFrameId 的未找到哨兵是 Frame 数量，源码接口有明确说明。名称查询契约

FreeFlyer：七维存储，六维运动

浮动基不是“给底座硬加六个普通电机关节”。JointModelFreeFlyer 是一个六自由度的关节类型，配置采用平移加单位四元数表示。

对于根关节安装变换为单位阵的常见模型，记底座位姿为：

$${}^WT_B= \begin{bmatrix} R & p\\ 0 & 1 \end{bmatrix}.$$

其中 $R$ 把底座坐标表示的向量旋转到世界坐标，$p$ 是底座原点在世界中的位置。该关节的配置和切向量分别是：

$$q_B=[p_x,p_y,p_z,Q_x,Q_y,Q_z,Q_w]^T, \qquad v_B=[u_x,u_y,u_z,\omega_x,\omega_y,\omega_z]^T.$$

这里用大写 $Q$ 表示四元数，避免与整机配置 $q$ 混淆。源码直接规定四元数顺序为 xyzw，而速度是子坐标系表达的线速度在前、角速度在后。FreeFlyer 类型与约定

如果 free-flyer 不是根关节，或设置了非单位的 jointPlacements，则必须继续考虑父关节与固定安装变换；不能把任意该关节的 q[:3] 都直接叫作世界坐标。

多出来的一维在哪里

单位四元数有四个存储系数，但必须满足：

$$Q_x^2+Q_y^2+Q_z^2+Q_w^2=1.$$

所以旋转只有三个独立自由度；另外 $Q$ 与 $-Q$ 表示同一个旋转。不是“浮动基实际上有七个自由度”，也不是“速度丢了一个分量”。

如果后面接 $n$ 个普通单自由度、单配置坐标的关节，则：

$$n_q=7+n,\qquad n_v=6+n.$$

不要把这个式子当成所有机器人的恒等式。连续转动关节等其他关节类型也可能使用非最小配置表示。通用代码应读取 model.nq、model.nv 和每个关节自己的索引，而不是永远假定 nq = nv + 1。

q 的索引和 v 的索引从底座后就不同了

对上述简单模型，首个普通关节在 q 中从下标 7 开始，在 v 中从下标 6 开始。把 v[7:] 当作全部驱动关节速度，会静悄悄地漏掉第一个关节。

Model::addJoint 分别维护 nq、nv，然后为新关节设置 idx_q、idx_v；并非使用一个统一偏移量。model.hxx 的索引分配

joint_id = model.getJointId("hip_joint")
if joint_id >= model.njoints:
    raise ValueError("模型中没有 hip_joint")

j = model.joints[joint_id]
q_joint = q[j.idx_q:j.idx_q + j.nq]
v_joint = v[j.idx_v:j.idx_v + j.nv]

这是复制任意机器人代码前，最值得加的一组尺寸断言：q.shape == (model.nq,)，v.shape == (model.nv,)，广义加速度和广义力也都是 nv 维。

局部速度不是世界位置的逐元素导数

根浮动基的线速度 $u$ 是底座系表达的原点速度。因此：

$$\dot p=Ru,\qquad \dot R=R[\omega]_\times.$$

其中 $[\omega]_\times x=\omega\times x$。若底座绕世界 z 轴转了 $90^\circ$，其局部 x 轴就指向世界 y 轴。此时输入 v[:3] = [1, 0, 0]，底座沿世界 y 方向移动是正确结果。

同理，v[3:6] 不是 roll、pitch、yaw 的变化率；欧拉角变化率与角速度之间还隔着与当前姿态有关的映射。

四元数导数也需要映射。采用本篇的旋转方向与右侧增量约定，可以写成：

$$\dot Q=\frac{1}{2}Q\otimes(\omega_x,\omega_y,\omega_z,0).$$

因此，整机 $\dot q$ 是配置系数空间中的量，而广义速度 $v$ 是切空间中的量，两者通常并不等同。后续动力学写成 $M(q)a+h(q,v)=\tau$ 更清楚：这里 $a$ 是 nv 维广义加速度，不是直接把 7 维底座数组求两次导数。

integrate：不是数组相加，而是右乘一个运动增量

pin.integrate(model, q, delta) 的第三个参数是切空间增量，不是带单位的时间。若手中拿的是速度，自己构造 delta = dt * v：

q_next = pin.integrate(model, q, dt * v)

FreeFlyer 对应 SpecialEuclideanOperationTpl<3>，从关节类型到李群运算的选择可以在 LieGroupMap 看到。

设六维增量为 $\delta=[\rho^T,\phi^T]^T$，它执行的几何操作是：

$$T_{\mathrm{next}}=T\exp(\widehat\delta),\qquad \widehat\delta= \begin{bmatrix} [\phi]_\times & \rho\\ 0 & 0 \end{bmatrix}.$$

源码先计算 quaternion::exp6，再将其平移旋转到当前坐标中并相加，四元数则按当前姿态乘增量姿态。它不是对平移做世界系相加、对四元数随意补一个归一化。integrate_impl

为什么连平移部分都不能直接相加

记 $\theta=\|\phi\|$，则指数映射包含：

$$R_{\mathrm{next}}=R\exp([\phi]_\times),\qquad p_{\mathrm{next}}=p+R\mathcal J(\phi)\rho,$$

$$\mathcal J(\phi)=I+ \frac{1-\cos\theta}{\theta^2}[\phi]_\times+ \frac{\theta-\sin\theta}{\theta^3}[\phi]_\times^2.$$

$\mathcal J$ 是 SO(3) 的左雅可比；不要与机器人末端的任务雅可比混为一谈。公式来源可对应到源码中的两次叉乘，它没有必须先生成一个通用的 $4\times4$ 矩阵指数。quaternion::exp6

一个能立即区分正确实现与“分量相加”的解析例子是：从单位位姿出发，取 $\rho=(1,0,0)$，$\phi=(0,0,\pi/2)$，则最后平移为：

$$p_{\mathrm{next}}=(2/\pi,\;2/\pi,\;0)^T.$$

这表示在有限时间内同时维持局部前进与旋转，走过一段弧线。它并不等价于“先在世界 x 轴平移 1 米，再原地转 90 度”。

当 $\phi=0$ 时才有 $\mathcal J=I$，此时平移更新退化为 $p+R\rho$。integrate 保证按照流形约定更新配置，但并不因此自动成为高阶动力学积分器：速度随时间变化时，时间离散误差仍需要单独处理。

difference：把两个配置的差放回切空间

delta = pin.difference(model, q0, q1)

对浮动基，其含义为：

$$\delta=\log(T_0^{-1}T_1)^\vee.$$

顺序很重要：这是从 q0 到 q1 的增量。实现先把平移差变到 q0 的局部系，再计算相对四元数，最后调用 log6。difference_impl

所以它的前三项通常也不等于 $p_1-p_0$：既有参考系变换，也有 SE(3) 对数中平移与旋转的耦合。若控制目标只要求世界系的位置误差，应明确计算那个误差，不要因为 difference 名字方便就把它当成世界坐标误差。

在远离旋转对数分支、增量足够小的局部区域，可以验证：

$$\operatorname{difference}(q,\operatorname{integrate}(q,\delta))\approx\delta.$$

不要用超过一圈的旋转增量要求这个等式全局成立。旋转配置并不记录“累计转了几圈”；接近 $\pi$ 的相对旋转还涉及对数分支与轴方向选择。应区分“返回了等价姿态”和“返回同一个切向量”。

还有一个细节：把六维误差平方直接求和，会把米和弧度混在一个标量里。源码的距离工具不替你决定任务权重；轨迹优化通常需要自己定义合理尺度，如分别对平移与转动设置权重。

neutral、normalize 与四元数的两个坑

全零向量不是合法的浮动基初始状态

单位位姿是 [0, 0, 0, 0, 0, 0, 1]，不是七个零。零四元数没有可用的旋转含义，不能靠“之后再归一化”修好。

import numpy as np

q = pin.neutral(model)
v = np.zeros(model.nv)
a = np.zeros(model.nv)

neutral 为整棵树按各自关节类型构造中性配置，并不保证这个配置满足机器人碰撞约束、机械限位或足底接触。SE(3) neutral 定义

Python 与 C++ 的 normalize 调用习惯不同

这个版本 Python 的 normalize 包装先复制输入，再返回归一化结果，因此应接住返回值：

q = pin.normalize(model, q)

对应的 C++ 配置归一化接口是修改传入配置。不要把一种语言的副作用习惯带到另一种语言。Python normalize_proxy

归一化只解决表示的范数条件，不负责验证 NaN、零范数输入、碰撞、关节限位或姿态来源是否正确。对外部日志或状态估计输入，要先检查有限数与非零范数，再做归一化。

为什么源码要检查两个四元数的点积

integrate_impl 得到新四元数后，会根据它与原四元数的点积决定是否整体翻转符号。因为 $Q$ 和 $-Q$ 代表相同旋转，选择与上一步更接近的符号可避免系数日志无故跳变。

这不是强行令 w >= 0，也不是改变机器人姿态。若比较两个配置是否相同，使用 pin.isSameConfiguration(model, q0, q1) 这类按关节语义比较的接口，不要仅依赖四元数数组逐元素相等。

源码里值得停下来看的数值技巧

小角度时不直接计算两个几乎相等的数之差

上面的 $\mathcal J$ 包含 $(1-\cos\theta)/\theta^2$ 和 $(\theta-\sin\theta)/\theta^3$。当角度非常小时，分子相减会损失有效数字，零点又有形式上的除零。

源码在小角度区域使用展开式，例如：

$$\frac{1-\cos\theta}{\theta^2}\approx\frac12-\frac{\theta^2}{24},\qquad \frac{\theta-\sin\theta}{\theta^3}\approx\frac16-\frac{\theta^2}{120}.$$

它还使用与标量类型精度相关的阈值，并在某些范数计算中加入机器精度量级的平方项，避免中间式直接落在零分母上。不要把这一细节理解成“所有角度都人为加一个固定工程阈值”。指数映射的小角度分支

只修正舍入误差时，不必每步都做一次完整归一化

乘法得到的四元数本来就应该在单位球面附近。内部 firstOrderNormalize 采用：

$$Q_{\mathrm{out}}=Q\frac{3-\|Q\|^2}{2}.$$

令 $\epsilon_Q=\|Q\|^2-1$，这个系数是 $(1+\epsilon_Q)^{-1/2}$ 的一阶近似。结果把小的范数误差压到更高阶，主计算只需要加法和乘法，不需要完整的平方根与变量除法。firstOrderNormalize

它的前提是输入已经很接近单位四元数。不要复制这个内部优化去处理任意用户输入，更不能拿它修复零四元数。区别在于：内部是在维护已知不变量，输入层是在验证不可信数据。

从关节变换到 Frame 查询：别读旧缓存

对于关节 $i$，固定安装变换与当前关节运动变换先组合，再沿树累乘：

$${}^{\mathrm{parent}}T_i= \texttt{jointPlacements}[i]\,T_{J_i}(q_i),\qquad {}^WT_i={}^WT_{\mathrm{parent}}\,{}^{\mathrm{parent}}T_i.$$

这分别对应 data.liMi[i] 与 data.oMi[i]。JointModelFreeFlyer::calc 负责把这一关节的平移、四元数和六维速度装进关节临时量；整棵树的世界传播由算法完成，不是每种关节自己遍历全模型。关节计算、运动学前向传播

如果关心 Frame 的速度和加速度，可以显式写出调用顺序：

pin.forwardKinematics(model, data, q, v, a)
pin.updateFramePlacements(model, data)

placement = data.oMf[frame_id]
velocity = pin.getFrameVelocity(
    model, data, frame_id, pin.LOCAL_WORLD_ALIGNED
)
acceleration = pin.getFrameClassicalAcceleration(
    model, data, frame_id, pin.LOCAL_WORLD_ALIGNED
)

只调用 forwardKinematics(model, data, q) 是位置级更新，不能期待旧的速度与加速度同时变新。updateFramePlacements 则把父关节的世界位姿乘上 Frame 固定安装位姿；它也不负责重新计算速度或动力学。Frame 更新实现

LOCAL、WORLD、LOCAL_WORLD_ALIGNED 到底差在哪

WORLD 不只是“把三维线速度旋转到世界系”。六维运动变换还涉及参考点的改变，线速度分量会出现由平移与角速度产生的项。要观察末端原点在世界方向上的实际线速度，通常选 LOCAL_WORLD_ALIGNED 更直接。对同一末端而言，不能把 WORLD 雅可比的前三行无条件叫作该点的位置雅可比。三种速度返回分支

空间加速度和经典加速度不要混读

getFrameAcceleration 返回空间加速度；getFrameClassicalAcceleration 在所选表示下额外给线性部分加上：

$$a_{\mathrm{classical,lin}}= a_{\mathrm{spatial,lin}}+\omega\times u.$$

这一步在 frames.hxx 中只有几行，却会影响“接口结果怎么和运动轨迹对上”的理解。

以前面的局部前进并旋转为例：恒定局部 twist 可以有零的局部系速度系数导数，但路径是弯的，原点的经典加速度并不为零。旋转坐标轴带来的 $\omega\times u$ 正是不能漏掉的项。

要与某个 Frame 原点的世界位置二阶差分比较，使用 LOCAL_WORLD_ALIGNED 下的经典线加速度。还需注意数值差分步长、轨迹积分精度和时间对齐。IMU 原始加速度计通常测的是比力，不是直接的世界位置二阶导；重力、传感器安装位置、安装朝向与传感器约定还要单独处理，不能仅换个 API 就直接相减。

再深入一层：Motion 和 Force 为什么必须成对理解

到这里，六维速度不应再被看成“任意六个数”。它有线分量与角分量、有参考点和坐标轴；它的对偶对象是六维力，而不是另一种六维速度。

本节把同一参考点、同一组坐标轴下的量记为：

$$V=\begin{bmatrix}u\\\omega\end{bmatrix},\qquad F=\begin{bmatrix}f\\n\end{bmatrix},\qquad P=F^TV=f^Tu+n^T\omega.$$

$f$ 的单位是 N，$n$ 是关于指定参考点的力矩，单位 N·m；线速度用 m/s，角速度用 rad/s，配对结果是机械功率。Pinocchio 的 Motion.linear() / angular() 与 Force.linear() / angular() 恰好按这个顺序组织。后一组名字中的 linear 表示力，不表示“力的线速度”。Motion 功率配对

同一个 SE3，作用在运动和力上却不是同一个矩阵

设 $T={}^AT_B=(R,p)$ 把 B 系点坐标转换到 A 系，即 $x_A=Rx_B+p$。定义运动变换矩阵：

$$X(T)= \begin{bmatrix} R &[p]_\times R\\ 0 &R \end{bmatrix}.$$

那么完整空间量的变换是：

$$V_A=X(T)V_B,\qquad F_A=X(T)^{-T}F_B.$$

第一式展开为 $u_A=Ru_B+p\times R\omega_B$、$\omega_A=R\omega_B$；第二式展开为 $f_A=Rf_B$、$n_A=Rn_B+p\times Rf_B$。运动变换的交叉项在线分量，力变换的交叉项在力矩分量，不能把同一个六乘六矩阵直接乘两者。运动 act 实现、力 act 实现

下面是有明确类型与变换方向的 C++ 片段，aMb、v_B、f_B 已由调用者构造：

pinocchio::Motion v_A = aMb.act(v_B);
pinocchio::Force f_A = aMb.act(f_B);
pinocchio::Motion v_B_back = aMb.actInv(v_A);
pinocchio::Force f_B_back = aMb.actInv(f_A);

double power_A = f_A.toVector().dot(v_A.toVector());
double power_B = f_B.toVector().dot(v_B.toVector());
// 检查 power_A 与 power_B 的差，以及两次往返的误差。

这里方法都叫 act，但 C++ 根据输入类型选取正确的作用。actInv 表示用逆变换作用，而不是逐分量取倒数，更不是把空间力简单变号。

功率不变来自代数恒等式：

$$F_A^TV_A=(X^{-T}F_B)^T(XV_B)=F_B^TV_B.$$

这给调试提供了比“箭头看起来对”更强的检查。若外力坐标变换后，配对功率在合理误差之外发生变化，首先检查参考点、力矩平移项和变换方向。

一个手算例子：为什么有力矩却仍然没有功率

令 $R=I$、$p=(1,0,0)$，B 系有 $f_B=(0,10,0)$、$n_B=0$，并令 $u_B=0$、$\omega_B=(0,0,2)$。在 A 表示中：

$$f_A=(0,10,0),\quad n_A=(0,0,10),\quad u_A=(0,-2,0),\quad\omega_A=(0,0,2).$$

此时 $P_A=10\times(-2)+10\times2=0=P_B$。不是“一个力矩没有做功所以公式错了”，而是换参考点后，线性功率和角功率都变化，二者的和才是同一个物理功率。若只旋转三维力、却漏掉力矩平移项，这个例子会立刻暴露错误。

运动叉乘和力叉乘也不同

在“线在前、角在后”的排列下，运动的李括号矩阵是：

$$\operatorname{ad}_V= \begin{bmatrix} [\omega]_\times &[u]_\times\\ 0 &[\omega]_\times \end{bmatrix},\qquad \operatorname{ad}_V^*=-\operatorname{ad}_V^T.$$

它们分别作用于运动和力。动力学里的 $V\times^*(IV)$ 不是对两个六维数组调用普通三维 cross；是这个对偶代数作用。Pinocchio 使用 Motion、Force 类型把两种重载分开，而不是让调用者到处记忆块矩阵符号。运动作用矩阵

空间惯量不是把三维转动惯量塞进六维对角线

pinocchio::Inertia(mass, com, rotational_inertia) 保存的核心参数是质量 $m$、参考原点指向质心的向量 $c$，以及关于质心、但用当前坐标轴表达的转动惯量 $I_C$。质心位置不为零时，平移与旋转必然耦合。

在当前的线角排列下，六维空间惯量为：

$$\mathcal I_O= \begin{bmatrix} mI_3 &-m[c]_\times\\ m[c]_\times &I_C-m[c]_\times^2 \end{bmatrix}.$$

右下角就是关于参考原点 O 的转动惯量：

$$I_O=I_C+m(\|c\|^2I_3-cc^T).$$

上右块与下左块互为转置，所以整体对称。源码用质量、质心和紧凑对称惯量存储，在需要矩阵时按这些块展开；不是每个刚体永久保存一个任意稠密的六乘六矩阵。Inertia 构造、matrix_impl

两千克刚体的平行轴手算

取 $m=2$ kg，$c=(0.3,0,0)$ m，质心惯量 $I_C=\operatorname{diag}(0.02,0.05,0.06)$ kg·m²。平行轴项为 $\operatorname{diag}(0,0.18,0.18)$，因此：

$$I_O=\operatorname{diag}(0.02,0.23,0.24).$$

若参考原点速度为零、角速度为 $\omega=(0,0,2)$ rad/s，则质心速度为 $\omega\times c=(0,0.6,0)$ m/s。动能可以通过两条独立路径计算：

$$E=\tfrac12m\|u_C\|^2+\tfrac12\omega^TI_C\omega =0.36+0.12=0.48\;\mathrm J,$$

$$E=\tfrac12 V^T\mathcal I_OV =\tfrac12\times0.24\times2^2=0.48\;\mathrm J.$$

这时线动量为 $(0,1.2,0)$，关于 O 的角动量为 $(0,0,0.48)$。如果构造 Inertia 时已经把 $I_C$ 手工换成 $I_O$，却仍把 com 填为 $(0.3,0,0)$，库会再计入一次平行轴贡献；上述动能核对就会失败。

两种“坐标转换”不要重复做

第一种是把一个刚体的质心惯量与质心坐标整理到同一组轴；第二种是通过 appendBodyToJoint(jid, inertia, joint_M_body) 把这个刚体整体表达变到支承关节坐标系。

如果 inertia 已用关节原点和关节坐标轴描述，就应传与之相符的安装变换，常见情况是单位阵。若其数据仍用独立 body 坐标系描述，才让 joint_M_body 完成那一次转换。不能先手工变换一遍，然后把同一变换又传给接口。

一般空间惯量的变换满足：

$$\mathcal I_A=X(T)^{-T}\mathcal I_BX(T)^{-1}.$$

因此 $F=\mathcal IV$ 的关系与动能在坐标改变后保持一致。SE3.act(Inertia) 把这个结构保留下来；它不是普通矩阵的 $RIR^T$ 这么简单。最后还要检查质量正、中心惯量对称、主惯量非负及三角不等式；“矩阵元素都正”不是物理有效性的判断规则。

右扰动如何变成 dIntegrate：先分清导数的输出空间

配置采用七维存储，不意味着每个导数都应该有七行。令 $F(T,\delta)=T\exp(\widehat\delta)$。如果比较两次输出配置时使用 difference，输出误差本身仍是六维切向量。

保持 $\delta$ 不变，对输入姿态做右扰动 $T\exp(\widehat\epsilon)$，在基准输出处比较差异：

$$F(T,\delta)^{-1}F(T\exp(\widehat\epsilon),\delta) =\exp(-\widehat\delta)\exp(\widehat\epsilon)\exp(\widehat\delta).$$

一阶误差因此为 $\operatorname{Ad}_{\exp(-\widehat\delta)}\epsilon$。这正是浮动基 dIntegrate(..., ARG0) 的几何含义：输入处的扰动方向必须搬到输出处，不能因为“都是六维”就直接用单位阵。dIntegrate 的接口契约、SE(3) ARG0 实现

如果扰动的是增量 $\delta$ 本身，则 ARG1 使用与右侧误差表示配套的指数映射雅可比。它也不是“指数映射逐元素求导”，因为误差是先投回流形切空间再比较。SE(3) ARG1 实现

与之不同，integrateCoeffWiseJacobian 回答“微小切向量如何改变配置存储系数”。它的整机尺寸是 nq × nv，单浮动基是 7 × 6。在零增量处，平移块为 $R$，四元数的四乘三块可写为：

$$\frac12\begin{bmatrix}Q_wI_3+[Q_{xyz}]_\times\\-Q_{xyz}^T\end{bmatrix}.$$

这个映射用于理解 $\dot q$ 与 $v$ 的联系，不应拿来替代 nv × nv 的 dIntegrate。判断该调用哪个接口时，先问最终误差是 q1 - q0 这种系数差，还是 difference(model, q0, q1) 这种流形差。配置系数雅可比实现

四个小练习，把定义变成自己的直觉

1. 建立仅包含一个 FreeFlyer 的模型，检查 nq == 7、nv == 6、neutral 的最后一项为 1。接入实际 URDF 后再打印每个关节的两个偏移量。
2. 在 yaw 为 $\pi/2$ 时积分一个局部 x 方向的纯平移增量，检查世界 y 方向变化。把结果与直接给 q[:3] 相加比较。
3. 在单位位姿积分 $[1,0,0,0,0,\pi/2]$，检查平移是否接近 $[2/\pi,2/\pi,0]$；然后用小增量验证 difference 的局部逆关系。
4. 只把配置中的四元数四项全部取负，确认姿态等价。分别观察逐系数误差和流形误差，再考虑优化器应该使用哪一种。

这些是解析预期与检查方法，不是伪造的运行日志。配套验证脚本、固定版本环境说明和实际运行状态见 专题 03。

读完本篇，再看到动力学源码中的 S、actInv、nv 维矩阵和六维基座力，应该能先回答三个问题：这个量在哪个参考点、用哪组坐标轴表达、属于配置空间还是切空间。下一篇就沿这些约定进入 RNEA、CRBA 与 ABA。