SAP Formulation Notes

刚体的位姿和速度 #

每个刚体有一个位姿 body_q = (p, q)（wp.transform）：

• p：刚体原点在世界系中的位置，原点在刚体自己的坐标系里就是 (0,0,0)
• q：刚体坐标系相对世界系的旋转（四元数 xyzw），对应旋转矩阵 R

质心单独存在 body_com 里，是质心在刚体坐标系下的坐标。

在世界系下：质心 = p + R·body_com

层	顺序	线速度是谁的
对外接口（SapState.joint_qd、body_qd）	[v, ω]	质心 v_com
solver 内部（free motion、耦合求解、积分）	[ω, v]	刚体原点 v_o

内部速度的 v 就是 p 的时间导数 ṗ，在世界系中表示；ω 也在世界系中表示。自由关节的 joint_q = [p, quat] 一般就是这个刚体的 body_q。条件是关节的父系 joint_X_p 和子系 joint_X_c 都是单位变换；不是的话会差一个固定的偏移。所以对自由刚体来说，积分时 p += dt·v_o 直接更新的就是原点位置。

关节的父系 joint_X_p 和子系 joint_X_c 是什么？ #

每个关节连接两个刚体：父刚体 P 和子刚体 C。关节本身在两个刚体上各钉一个坐标系：

• joint_X_p：关节在父刚体上的坐标系 F，相对父刚体坐标系的位姿。根关节的父刚体是世界，所以这时它就是相对世界系的位姿。
• joint_X_c：关节在子刚体上的坐标系 M，相对子刚体坐标系的位姿。

它们是模型常量，不随时间变。随时间变的是 F 到 M 的相对运动 X_j，由 joint_q 决定：

• revolute：绕轴转角度 q
• prismatic：沿轴平移 q
• free：一般刚体变换 (p, quat)

子刚体的世界位姿就是沿着这条链乘下来：

X_WC = X_WP · joint_X_p · X_j(q) · joint_X_c⁻¹

例子：肘关节（revolute）

• joint_X_p：把转轴放在上臂末端，用上臂坐标系表示
• joint_X_c：把转轴放在前臂起点，用前臂坐标系表示
• X_j：绕这根轴转角度 q

变换链路：

1. joint_X_c⁻¹: 一个在子刚体坐标系的点，先变换到子刚体关节坐标系M
2. X_j(q): 严格说，X_j 是 M 相对 F 的位姿，在 F 里表示，即 X_FM。所以点从M变换到坐标系F里面
3. joint_X_p: 从父刚体关节坐标系F变换到父刚体坐标系
4. X_WP: 从父刚体坐标系变换到世界

所以子刚体变换是从父刚体变换递推。

说一下关节和freemotion #

思路是：先用关节变量把每个刚体的速度写出来，再把每个刚体的牛顿-欧拉方程投影到关节空间，得到的就是 free motion 要解的方程。

关节：从 q̇ 到刚体速度 #

例子：单摆。 一个 revolute 关节，F 在世界原点，转轴是 z 轴，转角 θ。子刚体的原点放在转轴上，质心离转轴距离 l。

• 广义坐标 q = θ，广义速度 v = θ̇。

• 刚体速度（内部约定 [ω, v_o]，世界系，在刚体原点处取）：

  • ω = z·θ̇
  • v_o = 0（原点在转轴上，不动）

把”θ̇ = 1 时刚体的速度”记作 S，叫运动子空间。这里 S = [z; 0]，刚体速度 V = S·θ̇。

一般情况：每个关节 \(k\) 有 \(n_k\) 个自由度，每个自由度对应一列 \(S_k \in \mathbb{R}^6\)。\(S_k\) 的含义是：只让这个自由度以单位速度运动（\(\dot q_k = 1\)，其它为 \(0\)）时，子刚体的速度。子刚体的速度在世界系中表示，取在子刚体原点，排列为 \([\omega;\, v_o]\)。

关节	坐标个数	自由度	\(S\) 的列
revolute	1	1	\(\begin{bmatrix} a \\ a \times (p_C - p_F) \end{bmatrix}\)，\(a\) 为世界系转轴
prismatic	1	1	\(\begin{bmatrix} 0 \\ a \end{bmatrix}\)
ball	4（四元数）	3	绕 3 个坐标轴转
free	7	6	3 个转动 + 3 个平移
fixed	0	0	无

\(p_C\)和\(p_F\)是啥 #

这两个都是世界系下的位置：

• \(p_C\)：子刚体原点的世界坐标，也就是子刚体位姿 \(X_{WC}\) 的平移部分。
• \(p_F\)：关节坐标系 \(F\) 原点的世界坐标，也就是 \(X_{WP}\, X_{PF}\) 的平移部分。对 revolute 关节来说，\(F\) 原点就在转轴上。

所以ball free fixed的S长啥样 #

三种关节的 \(S\) 如下。记号：

• \(R_{WF}\)：\(F\) 系到世界系的旋转
• \(e_1, e_2, e_3\)：坐标轴单位向量
• \(\hat e_k = R_{WF}\, e_k\)：\(F\) 的第 \(k\) 个轴在世界系下的方向

Ball（3 个转动自由度）

球心就是 \(F\) 原点（也是 \(M\) 原点），子刚体可以绕球心沿任意方向转动。广义速度是 3 维角速度，在 \(F\) 系下表示。每一列等于”绕 \(\hat e_k\) 转的 revolute”：

\[S = \begin{bmatrix} \hat e_1 & \hat e_2 & \hat e_3 \\ \hat e_1 \times (p_C - p_F) & \hat e_2 \times (p_C - p_F) & \hat e_3 \times (p_C - p_F) \end{bmatrix}\]

可以理解为 3 个过同一点、互相垂直的 revolute 叠在一起。

Free（6 个自由度）

广义速度 \([\omega;\, v_M]\) 在 \(F\) 系下表示：\(\omega\) 是角速度，\(v_M\) 是 \(M\) 原点的速度。

• 前 3 列是转动，绕过 \(M\) 原点的轴：

\[\begin{bmatrix} \hat e_k \\ \hat e_k \times (p_C - p_M) \end{bmatrix}\]

• 后 3 列是平移：

\[\begin{bmatrix} 0 \\ \hat e_k \end{bmatrix}\]

最常见的情况：根关节，\(F\) 是世界系，且 \(X_{PF}\) 和 \(X_{CM}\) 都是单位变换。此时 \(p_C = p_M\)，\(\hat e_k = e_k\)，于是

\[S = I_6\]

也就是说，广义速度就是刚体原点的 twist 本身，\(v = V_C\)。这正是前面说 free 关节的 joint_qd 等于刚体内部速度 \([\omega;\, v_o]\) 的原因。这时 \(A\) 中对应的 \(6\times 6\) 块就是这个刚体在原点处的空间惯量（加上 armature）。

Fixed（0 个自由度）

没有列，\(S\) 是空的：子刚体和父刚体焊死，

\[V_C = \begin{bmatrix} \omega_P \\ v_P + \omega_P \times (p_C - p_P) \end{bmatrix}\]

它不贡献广义坐标，但它的质量还在：CRBA 回扫时，它的惯量会累加进父刚体的复合惯量，从而出现在祖先自由度的 \(A\) 里。

所以最后body速度=S * joint速度？ #

不完全是。\(V_C = S\,\dot q\) 只是子刚体相对父刚体的那部分速度：

\[V_C = \underbrace{\begin{bmatrix} \omega_P \\ v_P + \omega_P \times (p_C - p_P) \end{bmatrix}}_{\text{父刚体带过来的}} + \underbrace{S_{\text{joint}(C)}\, \dot q_{\text{joint}(C)}}_{\text{本关节贡献的}}\]

只有父刚体是世界（根关节）时第一项为 \(0\)，才有 \(V_C = S\,\dot q\)。free 根关节又有 \(S = I_6\)，所以 \(V_C = \dot q\)。

一般情况，把递推展开到根，每个祖先关节都有贡献：

\[V_i = \sum_{k \in \text{anc}(i)} S_k^{(i)}\, \dot q_k = J_i\, v\]

• \(\text{anc}(i)\)：从根到 \(i\) 路径上的所有关节，包括 \(i\) 自己的关节
• \(S_k^{(i)}\)：祖先关节 \(k\) 的 \(S\) 平移到刚体 \(i\) 原点后的结果，即线速度部分改成 \(\hat a \times (p_i - p_F)\)
• \(J_i\)：把这些列拼在一起，非祖先的列为 \(0\)

例子：双摆，两个 revolute 关节，转角 \(\theta_1\)、\(\theta_2\)。第二根杆的速度

\[V_2 = S_1^{(2)}\, \dot\theta_1 + S_2\, \dot\theta_2\]

它同时受两个关节影响：第一个关节转动时会把第二根杆整体带着走。

不过代码里不需要显式构造 \(S_k^{(i)}\)，因为直接存的 \(S\) 都取在各自子刚体的原点。另外有两种做法：

• 正运动学：用上面的递推，逐层平移父刚体速度再加本关节项。
• CRBA：把所有 \(S\) 统一平移到每棵树的参考点上。同一参考点下 twist 可以直接相加，这时 \(V_i = \sum_{k \in \text{anc}(i)} S_k\, \dot q_k\) 才真正成立。

比如有一条链P3->P2->P1->C #

你的理解是对的。把这条链写完整：链是 \(W \to P_3 \to P_2 \to P_1 \to C\)，每个箭头是一个关节：

• 关节 \(j_3\)：\(W \to P_3\)
• 关节 \(j_2\)：\(P_3 \to P_2\)
• 关节 \(j_1\)：\(P_2 \to P_1\)
• 关节 \(j_0\)：\(P_1 \to C\)

\(S_{j_2}\) 的含义：只让 \(\dot q_{j_2} = 1\) 时，\(P_2\) 在 \(P_2\) 原点处的 twist。

同一个关节对 \(C\) 的影响：只动 \(j_2\)、其它关节都冻住时，\(P_2\)、\(P_1\)、\(C\) 焊成一个整体，一起绕 \(j_2\) 的轴转。所以 \(C\) 得到的是同一个刚体运动，只是参考点换到 \(C\) 的原点：

\[S_{j_2}^{(C)} = \begin{bmatrix} \omega \text{ 部分不变} \\ \text{线速度改在 } p_C \text{ 处取} \end{bmatrix}\]

\(C\) 的 twist 就是链上每个关节的贡献相加（都取在 \(p_C\) 处）：

\[V_C = S_{j_3}^{(C)}\, \dot q_{j_3} + S_{j_2}^{(C)}\, \dot q_{j_2} + S_{j_1}^{(C)}\, \dot q_{j_1} + S_{j_0}\, \dot q_{j_0}\]

最后一项不用平移，因为 \(S_{j_0}\) 本来就取在 \(C\) 原点。

和逐层递推是一回事：

\[V_C = \text{shift}_{P_1 \to C}(V_{P_1}) + S_{j_0}\, \dot q_{j_0}\]

\(V_{P_1}\) 里已经包含了 \(j_3, j_2, j_1\) 的贡献，平移到 \(C\) 后，每一项就变成了上面的 \(S^{(C)}\)。

• 递推写法：每层平移一次，代码的正运动学用这种。
• 展开写法：一次性把所有祖先的 \(S\) 都平移到 \(C\)。

这也解释了 \(M\) 的稀疏性：\(C\) 的速度只依赖 \(j_3, j_2, j_1, j_0\)。如果另有一个分支 \(P_2 \to D\)，它的关节 \(j_D\) 不在 \(C\) 的祖先链上，所以 \(\dot q_{j_D}\) 不影响 \(C\)，\(J_C\) 在那一列是 \(0\)。

讲讲我们现在的CRBA是怎么整的 #

现在的 CRBA 沿用 MuJoCo Warp 的做法，每一步都是并行 kernel。它的核心是：每棵树先选一个公共参考点，所有惯量和 \(S\) 都平移到这个点（世界系表出），然后只需做加法和点积。

第 0 步：选参考点 #

每棵树取整棵树的质心作参考点：

\[r = \frac{\sum_{b \in \text{tree}} m_b\, c_b}{\sum_{b \in \text{tree}} m_b}\]

\(c_b\) 是刚体 \(b\) 质心的世界坐标。

为什么要统一参考点：只有取在同一点、在同一坐标系表出时，twist 才能直接相加，惯量也才能直接相加，这样就不必为每个刚体单独构造 \(S^{(i)}\)。

选质心而不是世界原点，我的理解是为了让力臂 \(c_b - r\) 尽量小，减轻 f32 下的抵消误差（MuJoCo 也是这样选的）。

第 1 步：每个刚体的惯量平移到 \(r\) #

令 \(d = c_b - r\)。刚体 \(b\) 在点 \(r\)、世界系下的空间惯量是

\[I_b = \begin{bmatrix} I_r & [h]_\times \\ -[h]_\times & m\,\mathbf 1 \end{bmatrix}, \qquad I_r = R\, I_{\text{com}} R^\top + m\left(\|d\|^2 \mathbf 1 - d d^\top\right), \qquad h = m\, d\]

• \(I_r\)：平行轴定理
• \([h]_\times\)：\(h\) 的叉乘矩阵

这个矩阵只有 10 个独立数：\(I_r\) 的 6 个、\(h\) 的 3 个、\(m\) 的 1 个。代码就按 10 个数紧凑存储

第 2 步：每个自由度的 \(S\) 平移到 \(r\) #

\[\bar S_k = \begin{bmatrix} \omega_k \\ v_k + \omega_k \times (r - p_{C_k}) \end{bmatrix}\]

\(p_{C_k}\) 是关节 \(k\) 子刚体的原点，正运动学存下来的 \(S_k\) 就取在这里。平移以后，同一棵树里所有自由度的 \(\bar S\) 都取在同一点 \(r\)。

第 3 步：复合惯量，叶 → 根累加 #

\[I^c_b = I_b + \sum_{b' \in \text{children}(b)} I^c_{b'}\]

因为参考点和坐标系都相同，这里就是 10 个数逐项相加，不需要任何平移。按深度分层，从最深一层往上，每层并行。

• fixed 关节下面的刚体也参与累加，所以它的质量会进入祖先自由度。
• 树根以上的固定刚体不在任何树里，不参与累加。

用你那条链 \(P_3 \to P_2 \to P_1 \to C\) 举例： \(\begin{aligned} I^c_C &= I_C \\ I^c_{P_1} &= I_{P_1} + I^c_C = I_{P_1} + I_C \\ I^c_{P_2} &= I_{P_2} + I^c_{P_1} = I_{P_2} + I_{P_1} + I_C \\ I^c_{P_3} &= I_{P_3} + I_{P_2} + I_{P_1} + I_C \end{aligned}\) 它在 \(M\) 里的意义：只让关节 \(j_2\)（\(P_3 \to P_2\)）以 \(\dot q\) 转动时，\(P_2\)、\(P_1\)、\(C\) 一起以 twist \(\bar S_{j_2}\dot q\) 运动。这部分的动能是

\[T = \tfrac12\, \dot q\, \bar S_{j_2}^\top\, I^c_{P_2}\, \bar S_{j_2}\, \dot q\]

所以 \(M_{j_2 j_2} = \bar S_{j_2}^\top I^c_{P_2} \bar S_{j_2}\)，就是这个关节”感受到的”惯量。直观例子：转肩膀时，只要肘和腕锁住，你要带动的是整条手臂（上臂 + 前臂 + 手），而不只是上臂。

第 4 步：填 \(M\) 的一行 #

对自由度 \(i\)，记 \(b(i)\) 为它的子刚体：

1. 先算一次 wrench：

   \[F_i = I^c_{b(i)}\, \bar S_i\]

2. 再沿祖先链 \(j = i,\ \text{parent}(i),\ \dots\) 往上走：

   \[M_{ij} = \bar S_j^\top F_i\]

3. 对角元再加上 armature。

为什么用 \(b(i)\) 的复合惯量：

\[M_{ij} = \sum_b \bar S_i^\top I_b \bar S_j\]

求和只对同时被 \(i\) 和 \(j\) 带动的刚体有贡献。\(j\) 是 \(i\) 的祖先，所以同时被两者带动的刚体正好是 \(b(i)\) 的整棵子树，合起来就是 \(I^c_{b(i)}\)。

• 并行方式：一个线程负责一行（一个自由度）。
• 存储：结果按稀疏下三角存，第 \(i\) 行只存祖先链上的列。
• v* 写法的额外输出：SolverSAP 需要 v* 时，同一个 kernel 还会额外写出每棵树的 dense 块，供 Cholesky 分解；SAP-VBD 不写这份。

Mij怎么理解 #

看动能

\[T = \tfrac12\, \dot q^\top M \dot q = \tfrac12 \sum_i M_{ii}\, \dot q_i^2 + \sum_{i<j} M_{ij}\, \dot q_i \dot q_j\]

\(M_{ij}\) 衡量关节 \(i\) 和关节 \(j\) 同时运动时，二者运动的耦合有多强。

展开：每个刚体 \(b\) 的速度是 \(V_b = \sum_k \bar S_k \dot q_k\)，求和只对 \(b\) 的祖先自由度。代入 \(T = \tfrac12\sum_b V_b^\top I_b V_b\)，\(\dot q_i \dot q_j\) 前的系数是

\[M_{ij} = \sum_{b\ \text{同时被}\ i,\,j\ \text{带动}} \bar S_j^\top I_b\, \bar S_i\]

• 设 \(j\) 是 \(i\) 的祖先，同时被两者带动的刚体就是 \(i\) 的子刚体 \(b(i)\) 的整棵子树，所以 \(M_{ij} = \bar S_j^\top I^c_{b(i)} \bar S_i\)。
• 如果 \(i\)、\(j\) 都不是对方的祖先（两个分支），没有刚体同时被二者带动，所以 \(M_{ij} = 0\)。

把并行粒度说准确 #

一共五步，所有步骤还会在 world 这一维上并行，下面不再重复写：

步	并行粒度	做什么
0	每棵树一个线程块	求这棵树的质心，作参考点 \(r\)
1	每个刚体一个线程	把自己的惯量平移到所属树的 \(r\)，得到 \(I_b\)
2	每个自由度一个线程	把 \(S_k\) 平移到 \(r\)，得到 \(\bar S_k\)
3	按深度分层，层间串行、层内每个刚体并行	从最深一层开始，每个刚体把 \(I^c_b\) 原子加到父刚体上，得到复合惯量
4	每个自由度一个线程	先算 \(F_i = I^c_{b(i)} \bar S_i\)，再沿祖先链往上走，逐个算 \(M_{ij} = \bar S_j^\top F_i\)，填满第 \(i\) 行

\(S_k\) 在 CRBA 之前就算好了，是在正运动学那一步里顺带算的，也就是组装动力学的第一步。那一步一个线程沿一条从根到叶的分支往下走，每经过一个关节就做几件事：

1. 用父刚体位姿和 \(q\) 算出子刚体位姿 \(X_{WC}\)；
2. 用世界系的转轴、\(p_F\)、\(p_C\) 算出这个关节每个自由度的 \(S_k\)（取在子刚体原点，世界系表出），存下来；
3. 用 \(S_k \dot q_k\) 递推子刚体的 twist 和偏置加速度，供 RNE 使用。

\(S_k\) 依赖 \(X_{WC}\)，而 \(X_{WC}\) 又要用父刚体的位姿，所以必须在根到叶的递推里一起算。CRBA 第 2 步并不重新计算 \(S_k\)，只是把已经存好的 \(S_k\) 从子刚体原点平移到树的参考点 \(r\)：

\[\bar S_k = \begin{bmatrix} \omega_k \\ v_k + \omega_k \times (r - p_{C_k}) \end{bmatrix}\]

整体顺序是：

1. 正运动学：得到位姿、\(S_k\)、twist、偏置加速度；
2. RNE：得到 \(\tau\)；
3. CRBA：得到 \(A\)。

每步开始时我们有的只有：

• 每个自由度的位置 \(q_0\) 和速度 \(v_0\)

没有现成的加速度，加速度是这一步算出来的。 流程如下。

1. 汇总所有已知的力，得到 \(\tau\)

   （每个自由度一个数）：

   • 每个自由度上直接施加的力 joint_f（就是你说的”每个自由度可以施加外力”）
   • 作用在刚体上的外力 \(f^{\text{ext}}\)，通过 \(J^\top\) 换算到各自由度
   • 减去重力和科氏/陀螺项 \(b(q_0, v_0)\)，这一项由 RNE 算出
   • 不包括 PD、限位、接触

2. 算质量矩阵 \(A\)（CRBA）。

3. free motion：

   \[\dot v = A^{-1}\tau, \qquad v^* = v_0 + \Delta t\,\dot v\]

   这里各自由度是耦合的：\(A\) 不是对角矩阵，所以给一个自由度加力，其它自由度的速度也会变（推肘，肩也会动）。不能逐个自由度单独算”力 → 加速度 → 速度”。

4. 加上 PD、限位、接触，一起求解，得到最终的 \(v\)。在 SAP-VBD 里，第 3 步并不单独做，而是并入这一步一起解。

5. 用 \(v\) 更新 \(q\)。

使用v而非v* #

在 SolverSAPVBD 里，刚体的 free motion 就到这三步为止，后面没有单独的求解。

前三步得到 $A$ 和 $\tau$ 之后：

• 不分解 $A$，也不算 $v^* = v_0 + \Delta t\,A^{-1}\tau$。
• 直接把 $v_0$ 和 $\tau$ 交给耦合求解，写成目标里的刚体项：

\[\tfrac12 (v - v_0)^\top A (v - v_0) - \Delta t\,\tau^\top (v - v_0)\]

• 它的极小点就是 $v^$。所以”求 free motion”这件事，是在耦合 Newton-PCG 里和接触一起顺带完成的；没有接触时，解出来就是 $v^$。

对比：显式算 $v^\star$ 的路径（SolverSAP 的非动量模式、诊断快照）会在三步之后多做一步：

• 逐棵树分解 $A$：
  • 每棵树都不超过 64 个自由度时，用 dense Cholesky；
  • 否则用按层并行的稀疏 LDLᵀ。
• 解 $A\dot v = \tau$，再得到 $v^* = v_0 + \Delta t\,\dot v$。