Pinocchio源码阅读——刚体运动物理量空间变换及求导

1. 记号与基本约定

设位姿

g= $Rp01$ ∈SE(3),g=\begin{bmatrix}R&p\\0&1\end{bmatrix}\in SE(3),g= $R0p1$ ∈SE(3),其中 R∈SO(3)R\in SO(3)R∈SO(3)，p∈R3p\in\mathbb R^3p∈R3。本文约定 ggg 表示从局部/物体坐标系 BBB 到空间/惯性坐标系 AAA 的变换：

xA=RxB+p.x_A=R x_B+p.xA=RxB+p.

6D 旋量顺序按 [linear; angular] 排列。若使用 [angular; linear]，需交换块并相应转置。

对 x∈R3x\in\mathbb R^3x∈R3，定义叉乘矩阵

x∧= $0-x3x2x30-x1-x2x10$ ,x∧y=x×y.x^\wedge= \begin{bmatrix} 0&-x_3&x_2\\ x_3&0&-x_1\\ -x_2&x_1&0 \end{bmatrix}, \qquad x^\wedge y=x\times y.x∧= 0x3−x2−x30x1x2−x10 ,x∧y=x×y.

Motion 写作

v=(vlin,ω)∈se(3),v=(v_{\text{lin}},\omega)\in se(3),v=(vlin,ω)∈se(3),

Force 写作

f=(flin,τ)∈se(3)∗.f=(f_{\text{lin}},\tau)\in se(3)^*.f=(flin,τ)∈se(3)∗.

二者对偶配对为

⟨f,v⟩=flin⋅vlin+τ⋅ω.\langle f,v\rangle =f_{\text{lin}}\cdot v_{\text{lin}} + \tau\cdot \omega.⟨f,v⟩=flin⋅vlin+τ⋅ω.这就是 Pinocchio 中 motion.dot(force) 或 force.dot(motion) 返回的标量。

2. Motion、Force、Inertia 与动量

2.1 Motion：运动旋量

Motion 表示速度旋量，属于李代数 se(3)se(3)se(3)：

v=(vlin,ω).v=(v_{\text{lin}},\omega).v=(vlin,ω).其中：

vlinv_{\text{lin}}vlin：线速度；
ω\omegaω：角速度。

Motion 描述刚体运动速度，是位姿流形切空间中的元素。

2.2 Force：力旋量

Force 表示力/力矩，也用于表示动量/角动量，属于对偶空间 se(3)∗se(3)^*se(3)∗：

f=(flin,τ).f=(f_{\text{lin}},\tau).f=(flin,τ).其中：

flinf_{\text{lin}}flin：力；
τ\tauτ：力矩。

Force 与 Motion 的配对是功率：

P=⟨f,v⟩=flin⋅vlin+τ⋅ω.P=\langle f,v\rangle =f_{\text{lin}}\cdot v_{\text{lin}} + \tau\cdot \omega.P=⟨f,v⟩=flin⋅vlin+τ⋅ω.因此 Motion * Force 不是普通 6D 内积，而是对偶配对，结果是标量。

2.3 动量 hhh

动量不是独立于 Motion/Force 的第四类量，而是惯量作用在 Motion 上的结果：

h=Iv.h=I v.h=Iv.由于 I:se(3)→se(3)∗I:se(3)\to se(3)^*I:se(3)→se(3)∗，所以

h∈se(3)∗,h\in se(3)^*,h∈se(3)∗,即动量与 Force 同属对偶空间，按 Force 的规则变换。

空间动量与物体动量分别写为

hs=IsVs,hb=IbVb.h_s=I_s V_s,\qquad h_b=I_b V_b.hs=IsVs,hb=IbVb.它们满足

hs=Ad⁡g−Thb.h_s=\operatorname{Ad}_g^{-T}h_b.hs=Adg−Thb.动量与 Motion 的配对给出两倍动能：

⟨h,v⟩=2T.\langle h,v\rangle=2T.⟨h,v⟩=2T.

在空间系中，牛顿-欧拉方程可写为

h˙s=fs.\dot h_s=f_s.h˙s=fs.在物体系中，由于基在变化，需要额外叉乘项：

h˙b+ad⁡VbThb=fb.\dot h_b+\operatorname{ad}_{V_b}^T h_b=f_b.h˙b+adVbThb=fb.这也说明：动量 hhh 是对偶量，求导时不能当作普通固定向量处理。

2.4 Inertia：空间惯量

Inertia 是线性映射：

I:se(3)→se(3)∗,f=Iv.I:se(3)\to se(3)^*,\qquad f=I v.I:se(3)→se(3)∗,f=Iv.它不是 Motion 与 Force 的逐分量比值。在一维平动中，质量可理解为力与加速度的比值：

m=Fa.m=\frac{F}{a}.m=aF.但在 6D 空间中，惯量是 6×66\times 66×6 矩阵。

按 [linear; angular] 顺序，若：

质量为 mmm；
质心 CCC 相对原点 OOO 的位置为 ccc；
关于质心的转动惯量为 ICI_CIC；

则关于原点 OOO 的空间惯量为

IO= $mI3-mc\landmc\landIC-mc\landc\land$ .I_O= \begin{bmatrix} m I_3 & -m c^\wedge\\ m c^\wedge & I_C-m c^\wedge c^\wedge \end{bmatrix}.IO= $mI3mc\land-mc\landIC-mc\landc\land$ .

动能写为

T=12⟨Iv,v⟩=12vTIv,T=\frac12\langle I v,v\rangle =\frac12 v^T I v,T=21⟨Iv,v⟩=21vTIv,是标量。

3. SE(3) act：坐标变换公式

设 g=(R,p)g=(R,p)g=(R,p) 从 BBB 变到 AAA。下面讨论同一物理量在 BBB 与 AAA 中的表示。

3.1 Motion 的变换

Motion 是李代数元素，按伴随作用变换：

vA=Ad⁡gvB.v_A=\operatorname{Ad}_g v_B.vA=AdgvB.其中

Ad⁡g= $Rp\landR0R$ .\operatorname{Ad}_g= \begin{bmatrix} R & p^\wedge R\\ 0 & R \end{bmatrix}.Adg= $R0p\landRR$ .展开为

vA,lin=RvB,lin+p×(RωB),ωA=RωB.\begin{aligned} v_{A,\text{lin}} &= R v_{B,\text{lin}}+p\times (R\omega_B),\\ \omega_A &= R\omega_B. \end{aligned}vA,linωA=RvB,lin+p×(RωB),=RωB.

3.2 Force 的变换

Force 是对偶空间元素。为了使配对保持不变：

⟨fA,vA⟩=⟨fB,vB⟩,\langle f_A,v_A\rangle=\langle f_B,v_B\rangle,⟨fA,vA⟩=⟨fB,vB⟩,必须使用逆转置：

fA=Ad⁡g−TfB.f_A=\operatorname{Ad}_g^{-T}f_B.fA=Adg−TfB.展开为

fA,lin=RfB,lin,τA=p×(RfB,lin)+RτB.\begin{aligned} f_{A,\text{lin}} &= R f_{B,\text{lin}},\\ \tau_A &= p\times (R f_{B,\text{lin}})+R\tau_B. \end{aligned}fA,linτA=RfB,lin,=p×(RfB,lin)+RτB.矩阵形式为

Ad⁡g−T= $R0p\landRR$ .\operatorname{Ad}_g^{-T} =\begin{bmatrix} R & 0\\ p^\wedge R & R \end{bmatrix}.Adg−T= $Rp\landR0R$ .

3.3 动量 hhh 的变换

由于 h∈se(3)∗h\in se(3)^*h∈se(3)∗，它与 Force 使用同一变换：

hA=Ad⁡g−ThB.h_A=\operatorname{Ad}_g^{-T}h_B.hA=Adg−ThB.若 hB=IBvBh_B=I_B v_BhB=IBvB，则

hA=Ad⁡g−TIBvB.h_A=\operatorname{Ad}_g^{-T}I_B v_B.hA=Adg−TIBvB.

3.4 Inertia 的变换

Inertia 是 se(3)→se(3)∗se(3)\to se(3)^*se(3)→se(3)∗ 的线性映射。若

fB=IBvB,vB=Ad⁡g−1vA,fA=Ad⁡g−TfB,f_B=I_B v_B,\qquad v_B=\operatorname{Ad}_g^{-1}v_A,\qquad f_A=\operatorname{Ad}_g^{-T}f_B,fB=IBvB,vB=Adg−1vA,fA=Adg−TfB,

则

fA=Ad⁡g−TIBAd⁡g−1vA.f_A =\operatorname{Ad}_g^{-T}I_B\operatorname{Ad}_g^{-1}v_A.fA=Adg−TIBAdg−1vA.

因此

IA=Ad⁡g−TIBAd⁡g−1.I_A= \operatorname{Ad}_g^{-T}I_B\operatorname{Ad}_g^{-1}.IA=Adg−TIBAdg−1.

这是合同变换，不是简单的 Ad⁡gIB\operatorname{Ad}_g I_BAdgIB。

3.5 为什么三者公式不同

三者不同的根本原因在于它们属于不同数学空间：

物理量	数学空间	变换方式
`Motion`	se(3)se(3)se(3) 中的向量	vA=Ad⁡gvBv_A=\operatorname{Ad}_g v_BvA=AdgvB
`Force`	se(3)∗se(3)^*se(3)∗ 中的对偶向量	fA=Ad⁡g−TfBf_A=\operatorname{Ad}_g^{-T}f_BfA=Adg−TfB
动量 hhh	se(3)∗se(3)^*se(3)∗ 中的对偶向量	hA=Ad⁡g−ThBh_A=\operatorname{Ad}_g^{-T}h_BhA=Adg−ThB
`Inertia`	se(3)→se(3)∗se(3)\to se(3)^*se(3)→se(3)∗ 的线性映射	IA=Ad⁡g−TIBAd⁡g−1I_A=\operatorname{Ad}_g^{-T}I_B\operatorname{Ad}_g^{-1}IA=Adg−TIBAdg−1

Pinocchio 的 SE3::act 对三种类型有不同重载，本质就是上述公式。

4. 时间导数、叉乘项与科氏力

4.1 空间速度与物体速度

定义

Vs=g˙g−1,Vb=g−1g˙.V_s=\dot g g^{-1},\qquad V_b=g^{-1}\dot g.Vs=g˙g−1,Vb=g−1g˙.其中：

VsV_sVs：空间/惯性系速度；
VbV_bVb：物体/局部系速度。

两者满足

Vs=Ad⁡gVb.V_s=\operatorname{Ad}_g V_b.Vs=AdgVb.令

X=Ad⁡g,X=\operatorname{Ad}_g,X=Adg,则

Vs=XVb.V_s=X V_b.Vs=XVb.

4.2 X 的导数

有恒等式

X˙=Ad⁡gad⁡Vb=ad⁡VsX.\dot X=\operatorname{Ad}g\operatorname{ad}{V_b} =\operatorname{ad}_{V_s}X.X˙=AdgadVb=adVsX.

其中 ad⁡V\operatorname{ad}_VadV 是李括号矩阵。按 [linear; angular] 顺序：

ad⁡(v,ω)= $ω\landv\land0ω\land$ .\operatorname{ad}_{(v,\omega)} =\begin{bmatrix} \omega^\wedge & v^\wedge\\ 0 & \omega^\wedge \end{bmatrix}.ad(v,ω)= $ω\land0v\landω\land$ .

由此可得

X˙−1=−ad⁡VbX−1=−X−1ad⁡Vs,\dot X^{-1} =-\operatorname{ad}{V_b}X^{-1} =-X^{-1}\operatorname{ad}{V_s},X˙−1=−adVbX−1=−X−1adVs,

以及

X˙−T=−ad⁡VsTX−T=−X−Tad⁡VbT.\dot X^{-T}=-\operatorname{ad}{V_s}^T X^{-T} =-X^{-T}\operatorname{ad}{V_b}^T.X˙−T=−adVsTX−T=−X−TadVbT.

若记：

X=Ad⁡gX=\operatorname{Ad}_gX=Adg；
X∗=Ad⁡g−TX^*=\operatorname{Ad}_g^{-T}X∗=Adg−T，用于 Force 和动量变换；
X−∗=(X−1)−T=XTX^{-*}=(X^{-1})^{-T}=X^TX−∗=(X−1)−T=XT，用于反向 Force 变换；

则

X˙=ad⁡VsX,X˙∗=−ad⁡VsTX∗.\dot X=\operatorname{ad}{V_s}X, \qquad \dot X^*=-\operatorname{ad}{V_s}^T X^*.X˙=adVsX,X˙∗=−adVsTX∗.

4.3 Motion 的导数

若

vA=XvB,v_A=X v_B,vA=XvB,则

v˙A=X˙vB+Xv˙B=ad⁡VsvA+Xv˙B.\dot v_A=\dot X v_B+X\dot v_B=\operatorname{ad}_{V_s}v_A+X\dot v_B.v˙A=X˙vB+Xv˙B=adVsvA+Xv˙B.

如果 vBv_BvB 在物体坐标中的分量不变，即 v˙B=0\dot v_B=0v˙B=0，则

v˙A=ad⁡VsvA.\dot v_A=\operatorname{ad}_{V_s}v_A.v˙A=adVsvA.

展开为

v˙A,lin=ωs×vA,lin+vs×ωA,\dot v_{A,\text{lin}}=\omega_s\times v_{A,\text{lin}}+v_s\times \omega_A,v˙A,lin=ωs×vA,lin+vs×ωA,ω˙A=ωs×ωA.\dot \omega_A =\omega_s\times \omega_A.ω˙A=ωs×ωA.

这就是"一求导就出现 ω×\omega\timesω×"的直接来源。

4.4 Force 的导数

若

fA=X−TfB,f_A=X^{-T}f_B,fA=X−TfB,则

f˙A=−ad⁡VsTfA+X−Tf˙B.\dot f_A =-\operatorname{ad}_{V_s}^T f_A +X^{-T}\dot f_B.f˙A=−adVsTfA+X−Tf˙B.

如果 fBf_BfB 在物体坐标中的分量不变，即 f˙B=0\dot f_B=0f˙B=0，则

f˙A=−ad⁡VsTfA.\dot f_A=-\operatorname{ad}_{V_s}^T f_A.f˙A=−adVsTfA.展开为

f˙A,lin=ωs×fA,lin,\dot f_{A,\text{lin}} =\omega_s\times f_{A,\text{lin}},f˙A,lin=ωs×fA,lin,τ˙A=vs×fA,lin+ωs×τA.\dot \tau_A =v_s\times f_{A,\text{lin}} +\omega_s\times \tau_A.τ˙A=vs×fA,lin+ωs×τA.

动量 hA=X−ThBh_A=X^{-T}h_BhA=X−ThB 的导数与 Force 完全一致：

h˙A=−ad⁡VsThA.\dot h_A=-\operatorname{ad}_{V_s}^T h_A.h˙A=−adVsThA.

4.5 为什么等价于"先变到空间系求导，再变回来"

设物体系量为 bbb，空间系量为

s=Xb.s=X b.s=Xb.在空间系中求普通时间导数：

s˙=X˙b+Xb˙=ad⁡Vss+Xb˙.\dot s =\dot X b+X\dot b =\operatorname{ad}_{V_s}s+X\dot b.s˙=X˙b+Xb˙=adVss+Xb˙.

再变回物体系：

X−1s˙=ad⁡Vbb+b˙.X^{-1}\dot s=\operatorname{ad}_{V_b}b+\dot b.X−1s˙=adVbb+b˙.

因此，物体系中的物理导数不是简单的 b˙\dot bb˙，而是

b˙+ad⁡Vbb.\dot b+\operatorname{ad}_{V_b}b.b˙+adVbb.这就是协变导数。若直接在物体系对分量求导，会漏掉基变化项；先变到空间系求导，再变回来，就自动补上了这一项。

换句话说：

空间系是惯性系，空间系中的普通时间导数有明确的绝对变化率含义；
物体系的基随刚体运动，分量导数只描述分量变化，不描述物理量的绝对变化；
先变到空间系求导，再变回物体系，等价于在物体系中求协变导数。

4.6 为什么不在惯性系下直接做：空间惯量变化与能量守恒

在空间系中，动能写为

T=12⟨hs,Vs⟩=12VsTIsVs,hs=IsVs.T=\frac12\langle h_s,V_s\rangle= \frac12 V_s^T I_s V_s, \qquad h_s=I_s V_s.T=21⟨hs,Vs⟩=21VsTIsVs,hs=IsVs.

其中

Is=Ad⁡g−TIbAd⁡g−1.I_s=\operatorname{Ad}_g^{-T}I_b\operatorname{Ad}_g^{-1}.Is=Adg−TIbAdg−1.

虽然 IbI_bIb 在物体系中是常数，但 IsI_sIs 在空间系中随位姿 ggg 变化，不是常数：

I˙s=−ad⁡VsTIs−Isad⁡Vs.\dot I_s=-\operatorname{ad}{V_s}^T I_s-I_s\operatorname{ad}{V_s}.I˙s=−adVsTIs−IsadVs.

因此

h˙s=I˙sVs+IsV˙s=IsV˙s−ad⁡VsThs.\dot h_s=\dot I_s V_s+I_s\dot V_s=I_s\dot V_s-\operatorname{ad}_{V_s}^T h_s.h˙s=I˙sVs+IsV˙s=IsV˙s−adVsThs.

正确的空间系动力学是

h˙s=fs,\dot h_s=f_s,h˙s=fs,

即

fs=IsV˙s−ad⁡VsThs.f_s=I_s\dot V_s-\operatorname{ad}_{V_s}^T h_s.fs=IsV˙s−adVsThs.

如果误以为"在惯性系下 IsI_sIs 是常数"，直接写成

fs=IsV˙s,f_s=I_s\dot V_s,fs=IsV˙s,就丢掉了 −ad⁡VsThs-\operatorname{ad}_{V_s}^T h_s−adVsThs 项。该项与 VsV_sVs 的配对为零，不直接贡献瞬时功率，但它决定动量方向的变化。数值积分时，若长期忽略该项，会破坏能量一致性，表现为能量漂移。

更稳妥的做法是：

在物体系中保持 IbI_bIb 常数；
用协变导数处理 Motion、Force、动量；
若在空间系中计算，则必须使用 h˙s=fs\dot h_s=f_sh˙s=fs，并正确更新 IsI_sIs，而不是把 IsI_sIs 当常数。

Pinocchio 的 act 和 cross 正是为了避免手动丢掉这些项。

4.7 与理论力学科氏力的关系

在旋转坐标系中，位置满足

rA=p+RrB.r_A=p+R r_B.rA=p+RrB.若 rBr_BrB

随时间变化，则速度

vA=p˙+ω∧RrB+Rr˙B.v_A=\dot p+\omega^\wedge R r_B+R\dot r_B.vA=p˙+ω∧RrB+Rr˙B.

再求导会出现

aA=a相对+2ω×v相对+ω×(ω×r)+ω˙×r.a_A=a_{\text{相对}}+2\omega\times v_{\text{相对}}+\omega\times(\omega\times r)+\dot\omega\times r.aA=a相对+2ω×v相对+ω×(ω×r)+ω˙×r.

其中

2ω×v相对2\omega\times v_{\text{相对}}2ω×v相对就是科氏加速度。李括号 ad⁡V\operatorname{ad}_VadV 正是这一套叉乘项的旋量形式。因此，旋量求导中的 ω×\omega\timesω× 与理论力学中的科氏力、离心力、欧拉力是同一件事的不同层级表述。

5. Inertia 的质心/原点转换与组合

5.1 质心惯量与原点惯量

给定：

质量 mmm；
质心 CCC 相对原点 OOO 的位置 ccc；
关于质心的转动惯量 ICI_CIC。

关于原点 OOO 的空间惯量为

IO= $mI3-mc\landmc\landIC-mc\landc\land$ .I_O= \begin{bmatrix} m I_3 & -m c^\wedge\\ m c^\wedge & I_C-m c^\wedge c^\wedge \end{bmatrix}.IO= $mI3mc\land-mc\landIC-mc\landc\land$ .其角-角块满足平行轴定理：

IO,ang=IC−mc∧c∧=IC+m(∥c∥2I3−ccT).I_{O,\text{ang}}=I_C-m c^\wedge c^\wedge=I_C+m(\|c\|^2 I_3-c c^T).IO,ang=IC−mc∧c∧=IC+m(∥c∥2I3−ccT).

5.2 从原点惯量提取参数

反过来，从 IOI_OIO 可提取：

m=13tr⁡(IO $0:3,0:3$ ),m=\frac{1}{3}\operatorname{tr}(I_O $0:3,0:3$ ),m=31tr(IO $0:3,0:3$ ),c∧=−1mIO $0:3,3:6$ ,c^\wedge=-\frac{1}{m}I_O $0:3,3:6$ ,c∧=−m1IO $0:3,3:6$ ,IC=IO $3:6,3:6$ +mc∧c∧.I_C=I_O $3:6,3:6$ +m c^\wedge c^\wedge.IC=IO $3:6,3:6$ +mc∧c∧.

5.3 坐标变换

若已知 BBB 系中的惯量 IBI_BIB，要变到 OOO 系：

IO=Ad⁡gOB−TIBAd⁡gOB−1.I_O= \operatorname{Ad}{g{OB}}^{-T} I_B \operatorname{Ad}{g{OB}}^{-1}.IO=AdgOB−TIBAdgOB−1.在 Pinocchio 中即：

cpp 复制代码

I_O = g_OB.act(I_B);

前提是 g_OB 表示从 BBB 到 OOO 的位姿。

5.4 两个固定连接刚体的组合惯量

若刚体 1、2 固定连接，则组合体仍是一个刚体。空间惯量在同一坐标系、同一原点下可直接相加：

IcombO=I1O+I2O.I_{\text{comb}}^O=I_1^O+I_2^O.IcombO=I1O+I2O.步骤：

选组合体参考系 OOO；
把每个刚体的惯量变到 OOO：
IiO=Ad⁡gOi−TIiBiAd⁡gOi−1; I_i^O= \operatorname{Ad}{g{Oi}}^{-T} I_i^{B_i} \operatorname{Ad}{g{Oi}}^{-1};IiO=AdgOi−TIiBiAdgOi−1;
相加：
IcombO=∑iIiO. I_{\text{comb}}^O=\sum_i I_i^O.IcombO=i∑IiO.

若用质心参数合并，则

m=∑imi,m=\sum_i m_i,m=i∑mi,c=∑imicim,c=\frac{\sum_i m_i c_i}{m},c=m∑imici,IC=∑i $ICi+mi(∥ci-c∥2I3-(ci-c)(ci-c)T)$ .I_C= \sum_i\left $I_{C_i}+m_i\\left(\\\|c_i-c\\\|\^2 I_3- (c_i-c)(c_i-c)\^T\\right)\\right$ .IC=i∑ $ICi+mi(∥ci-c∥2I3-(ci-c)(ci-c)T)$ .

注意：各 ICiI_{C_i}ICi 和 cic_ici 必须先旋转/平移到同一个方向与原点约定下。

6. Pinocchio 工程注意事项与速查表

6.1 常见错误

Motion 和 Force 都是 6D 向量，但属于不同空间，不能随意相加。
Motion.dot(Force) 是对偶配对，结果是功率/标量。
动量 h=Ivh=I vh=Iv 属于 Force 空间，不能按 Motion 变换。
Inertia 不是 6D 向量，而是 6×66\times 66×6 矩阵；可用 Inertia::matrix() 获取。
SE3::act 对 Motion、Force、Inertia 有不同重载，不要手写普通矩阵乘法。
方向约定很关键：g.act(x) 通常把局部表示变到世界表示；反向用 g.actInv(x)。
Motion::cross 对 Motion 用 ad⁡\operatorname{ad}ad，对 Force 用 −ad⁡T-\operatorname{ad}^T−adT，对应叉乘项。
使用惯量矩阵时确认 [linear; angular] 顺序，否则块位置会变。

6.2 公式速查表

对象	变换	导数项
`Motion`	vA=Ad⁡gvBv_A=\operatorname{Ad}_g v_BvA=AdgvB	ad⁡VsvA\operatorname{ad}_{V_s}v_AadVsvA
`Force`	fA=Ad⁡g−TfBf_A=\operatorname{Ad}_g^{-T}f_BfA=Adg−TfB	−ad⁡VsTfA-\operatorname{ad}_{V_s}^T f_A−adVsTfA
动量 hhh	hA=Ad⁡g−ThBh_A=\operatorname{Ad}_g^{-T}h_BhA=Adg−ThB	−ad⁡VsThA-\operatorname{ad}_{V_s}^T h_A−adVsThA
`Inertia`	IA=Ad⁡g−TIBAd⁡g−1I_A=\operatorname{Ad}_g^{-T}I_B\operatorname{Ad}_g^{-1}IA=Adg−TIBAdg−1	−ad⁡TI−Iad⁡-\operatorname{ad}^T I-I\operatorname{ad}−adTI−Iad

6.3 核心公式

X=Ad⁡g,X∗=Ad⁡g−T,X−∗=XT.X=\operatorname{Ad}_g,\qquad X^=\operatorname{Ad}_g^{-T},\qquad X^{-}=X^T.X=Adg,X∗=Adg−T,X−∗=XT.

X˙=ad⁡VsX,X˙∗=−ad⁡VsTX∗.\dot X=\operatorname{ad}{V_s}X,\qquad \dot X^*=-\operatorname{ad}{V_s}^T X^*.X˙=adVsX,X˙∗=−adVsTX∗.
IO= $mI3-mc\landmc\landIC-mc\landc\land$ .I_O= \begin{bmatrix} m I_3 & -m c^\wedge\\ m c^\wedge & I_C-m c^\wedge c^\wedge \end{bmatrix}.IO= $mI3mc\land-mc\landIC-mc\landc\land$ .
IcombO=∑iAd⁡gOi−TIiBiAd⁡gOi−1.I_{\text{comb}}^O=\sum_i \operatorname{Ad}{g{Oi}}^{-T} I_i^{B_i} \operatorname{Ad}{g{Oi}}^{-1}.IcombO=i∑AdgOi−TIiBiAdgOi−1.
h˙s=fs,hs=IsVs.\dot h_s=f_s,\qquad h_s=I_s V_s.h˙s=fs,hs=IsVs.

7. 总结

刚体旋量在 Pinocchio 中的核心逻辑是：

Motion 属于 se(3)se(3)se(3)，按 Ad⁡g\operatorname{Ad}_gAdg 变换；
Force 和动量 hhh 属于 se(3)∗se(3)^*se(3)∗，按 Ad⁡g−T\operatorname{Ad}_g^{-T}Adg−T 变换；
Inertia 是 se(3)→se(3)∗se(3)\to se(3)^*se(3)→se(3)∗ 的映射，按合同变换
IA=Ad⁡g−TIBAd⁡g−1 I_A=\operatorname{Ad}_g^{-T}I_B\operatorname{Ad}_g^{-1}IA=Adg−TIBAdg−1
Motion 与 Force 的配对是标量功率；
求导中的叉乘项来自李括号，与理论力学中的科氏力同源；
在惯性系中直接求导时，空间惯量 IsI_sIs 随位姿变化，不能把它当常数，否则会破坏能量一致性；
惯量的质心/原点转换和固定连接组合，都在同一坐标系、同一原点下按合同变换后相加。