Pinocchio 浮动基专题 05：接触约束、基座耦合与质心动力学

为什么机器人悬空时内部关节一动，基座也会动；足端接触地面以后，却又能通过关节力矩支撑身体？答案不是“给基座加上控制器”，而是系统的动力学方程与约束条件发生了变化。

本篇把三个层次连起来：无接触时的基座耦合 → 接触时的约束力求解 → 从整机角度观察质心和动量。它们描述同一个机器人，但未知量、坐标表达和适用假设不同。

阅读路线：专题入口 → 02：RNEA、CRBA 与 ABA → 本篇。03：接口与自检 提供模型及基础调用背景。本文固定 Pinocchio v3.8.0，不是最新版声明；接口按官方该 tag 的源码与 Python 绑定核对。

1. 无接触：先把基座方程真正写出来

假设根 free-flyer 占据广义速度前六维，其余关节全驱动，没有外部接触和推进力。把加速度和质量矩阵分块：

$$\begin{bmatrix}M_{bb}&M_{bj}\\M_{jb}&M_{jj}\end{bmatrix} \begin{bmatrix}a_b\\a_j\end{bmatrix} +\begin{bmatrix}h_b\\h_j\end{bmatrix} =\begin{bmatrix}0\\\tau_j\end{bmatrix}$$

这里 a_b 是六维基座广义加速度，a_j 是内部关节加速度；分块顺序必须与真实模型 idx_v 一致。h 已包含重力及速度相关项，不应再重复添加。

基座没有直接驱动，表示右端基座块为零，绝不表示左端 a_b=0。第一行直接给出：

$$a_b=-M_{bb}^{-1}(M_{bj}a_j+h_b)$$

所以改变内部关节加速度，会通过 M_bj 改变基座响应。固定基模型则从建模时就去掉了这些自由度；它不是把浮动基输入 tau[:6] 设为零之后得到的同一个系统。

Schur 补告诉我们关节看到的等效惯量

将基座解代入第二行：

$$\underbrace{(M_{jj}-M_{jb}M_{bb}^{-1}M_{bj})}_{M_{red}}a_j =\tau_j-h_j+M_{jb}M_{bb}^{-1}h_b$$

M_red 是基座可以自由响应时，内部关节看到的约化惯量，而不是简单截取质量矩阵右下角。只用 M_jj 的计算隐含着另一种基座运动条件。

实际代码不应显式求 Mbb 的逆，而应共享同一个分解来解多个右端项：

# M、h 已通过同一 q、v、armature 的 CRBA/NLE 得到。
Mbb, Mbj = M[:6, :6], M[:6, 6:]
Mjb, Mjj = M[6:, :6], M[6:, 6:]
# 将两个右端合并，让一次 solve 共用分解。
solved = np.linalg.solve(Mbb, np.column_stack((Mbj, h[:6])))
X, y = solved[:, :-1], solved[:, -1]
Mred = Mjj - Mjb @ X
aj = np.linalg.solve(Mred, tau_j - h[6:] + Mjb @ y)
ab = -(X @ aj + y)
a_schur = np.concatenate((ab, aj))

这是一种便于理解和自检的稠密写法，不比 ABA 更高效。应比较 a_schur 与给定同一广义力 [0, tau_j] 的 ABA 输出，再检查完整动力学残差。基础接口见 CRBA 与 ABA。

2. 加上接触：新增的未知量是什么？

设 lambda 为环境作用在机器人上的接触力，J 为与该力坐标表达配套的约束雅可比，则：

$$M a+h=B\tau_{act}+J^{\mathsf T}\lambda$$

相比无接触情形，多出来的不是一个已知输入，而是一组未知约束力。需要同时补充“接触如何运动”的条件才能求解。

若接触的几何约束为 phi(q)=0，速度满足 Jv=0，理想加速度约束为：

$$J a+\dot Jv=0$$

更一般地，把对应坐标表达下的运动偏置、目标加速度和稳定化修正合并写成：

$$J a=b_c$$

理想、固定、无修正接触时，b_c=-dot(J)v。对于位置点约束、空间约束以及不同表达坐标系，构造偏置的公式有区别；不能看到一个六维 data.a 就随手取前三项当作所有约束的偏置。

KKT 方程的负号从哪里来？

令 r=B*tau_act-h，则动力学是 Ma-Jᵀlambda=r。将约束行也乘以负号，得到一个对称鞍点系统：

$$\begin{bmatrix}M&-J^{\mathsf T}\\-J&0\end{bmatrix} \begin{bmatrix}a\\\lambda\end{bmatrix} =\begin{bmatrix}r\\-b_c\end{bmatrix}$$

不同资料可能把未知量定义成负的接触力，或者交换未知量排列；矩阵中的符号随之变化。判断是否正确，要回到两条物理方程，而不是只比对某一块矩阵前面的符号。

这个 KKT 矩阵一般不是正定矩阵，不能把用于正定质量矩阵的普通 LLT 不加区分地用于整个鞍点系统。Pinocchio 使用专门的接触分解结构。接触分解接口

Delassus 矩阵：接触力如何改变接触加速度

从第一行解出 a=M⁻¹(r+Jᵀlambda) 并代入约束：

$$\underbrace{J M^{-1}J^{\mathsf T}}_W\lambda=b_c-JM^{-1}r$$

W 把接触力变化映射为接触加速度变化。其尺寸由约束行数决定，不由 nq 决定。重复接触、线性相关的约束或退化几何会使它秩亏；这不是把求解容差设得更小就能解决的问题。

3. 3D 接触与 6D 接触不是“精度不同”

一个理想点接触可传递力，但没有独立的点接触力偶。一个刚性焊接式 6D 约束可以传递完整力旋量，因此通常比“脚底和地面贴着”更强。

真实平足接触还会受到压力中心、支撑多边形、法向力、摩擦以及扭转摩擦限制。使用 CONTACT_6D 并不意味着这些物理边界都已建模。类型定义和约束数据见 contact-info.hpp。

为什么要明确选择 LOCAL 或 LWA？

本文示例统一选 pin.LOCAL：约束运动与接触力在接触 1 的局部坐标表达，参考原点位于接触位置。LOCAL_WORLD_ALIGNED 使用与世界对齐的轴，但参考点仍与相应接触几何有关，不能因此把力矩当成关于世界原点的力矩。

v3.8.0 的 constraintDynamics 明确拒绝 WORLD，只支持 LOCAL 与 LOCAL_WORLD_ALIGNED。即使某个通用枚举列出了 WORLD，也不能推断这个具体算法支持它。约束算法的参考系检查

4. 一套真实接口：把一个 frame 固定在初始世界位姿

以下示例假定已有合法的浮动基 model、配置 q0 和真实存在的 frame_name。它演示一个双边 6D 约束的建立与单步动力学，不是地面碰撞检测器；建议先从 v=0、合法初始接触开始。

import numpy as np
import pinocchio as pin

def make_fixed_frame_contact(model, q0, frame_name):
    fid = model.getFrameId(frame_name)
    if fid >= model.nframes:
        raise ValueError(f"frame 不存在: {frame_name}")
    frame = model.frames[fid]
    kin = model.createData()
    pin.framesForwardKinematics(model, kin, q0)
    world_anchor = kin.oMf[fid].copy()

    # 接触1在机器人关节上，接触2在 universe 上。
    # universe 位姿是世界参考，故其 placement 就是固定世界锚点。
    cm = pin.RigidConstraintModel(
        pin.ContactType.CONTACT_6D,
        model,
        frame.parentJoint,
        frame.placement,
        0,
        world_anchor,
        pin.LOCAL,
    )
    # 第一轮只看理想约束残差，暂不加漂移修正。
    cm.corrector.Kp = np.zeros(6)
    cm.corrector.Kd = np.zeros(6)

    cms = pin.StdVec_RigidConstraintModel()
    cds = pin.StdVec_RigidConstraintData()
    cms.append(cm)
    cds.append(cm.createData())
    dyn = model.createData()
    pin.initConstraintDynamics(model, dyn, cms)
    return fid, dyn, cms, cds

# frame_name 必须替换为自己模型中的实际名称。
# fid, dyn, cms, cds = make_fixed_frame_contact(model, q0, frame_name)
# q = q0.copy()
# v = np.zeros(model.nv)
# tau = np.zeros(model.nv)  # 根六维无直接驱动
# settings = pin.ProximalSettings(1e-10, 0.0, 1)
# a = pin.constraintDynamics(model, dyn, q, v, tau, cms, cds, settings).copy()
# lambda_local = cds[0].contact_force.vector.copy()

七参数 RigidConstraintModel 构造器、标准向量包装与 constraintDynamics 的参数顺序均来自 v3.8.0 的 约束类型 Python 绑定 和 算法 Python 绑定。这里使用标准向量是为了让随后更新的约束数据明确留在可读取的容器中。

world_anchor 应在接触建立时记录，不能每步都重新设成当前足端位置，否则你不断移动约束目标，漂移也会被隐藏。接触集合或尺寸变化后，需要为新集合重新初始化相关接触求解内存。

求解后应该读取哪个力？

本例读取 cds[0].contact_force，其六维顺序为力在前、力矩在后，与选定 LOCAL 约束雅可比配对。data.lambda_c 是所有约束块的堆叠结果；混合 3D/6D 时应按各模型 size() 分段，不要一律每六维截取。

实际实现将内部 primal-dual 解中的接触首块取负，得到 lambda_c 与 contact_force。因此调试内部缓存时，不能直接拿内部首块当作本文方程中的正接触力。结果提取源码

5. 不靠动画验收：检查两条独立残差

对上面的静止初始接触、零修正、固定世界 6D 约束，可建立独立审计数据：

# 接在上节一次成功调用之后；所有量对应同一 q、v、tau。
audit = model.createData()
M = pin.crba(model, audit, q).copy()
h = pin.nonLinearEffects(model, audit, q, v).copy()
J = pin.computeFrameJacobian(model, audit, q, fid, pin.LOCAL).copy()

pin.forwardKinematics(model, audit, q, v, np.zeros(model.nv))
drift = pin.getFrameAcceleration(model, audit, fid, pin.LOCAL).vector.copy()

dynamic_residual = M @ a + h - tau - J.T @ lambda_local
constraint_residual = J @ a + drift
print("动力学残差:", np.linalg.norm(dynamic_residual, ord=np.inf))
print("约束残差:", np.linalg.norm(constraint_residual, ord=np.inf))
print("迭代次数:", settings.iter)

两条残差用途不同：动力学残差检查力的符号、坐标与方程；约束残差检查运动条件。只验证一条可能漏掉另一类错误。进一步测试非零速度时，应先构造满足约束的速度，或明确将速度误差及修正项纳入所比较的约束右端。

这段偏置调用针对 6D 空间约束。改成 3D 点位置约束时，要使用与约束一致的经典线加速度偏置，例如固定世界目标的合适表达下查询 getFrameClassicalAcceleration，不能只把本段 drift 的前三项照搬。源码对 3D 分支显式加入了角速度与线速度相关项。

本篇代码是可执行的接口与验收思路，不预先声明某个机器人已得到特定误差值；运行环境、实际输入和结果应与测试报告一起记录。

6. 约束漂移：加速度满足了，位置为什么还会偏？

数值积分只近似满足连续时间约束。即使每步解出的加速度残差很小，离散误差仍会积累到 Jv 和接触位姿误差；初始配置或速度不满足约束时，单靠理想加速度约束也不会自动修复它们。

常见稳定化思想是对约束误差加入反馈：

$$J a+\dot Jv=-K_p e-K_d\dot e$$

对于标量线性化误差，若取 Kp=omega_n²、Kd=2*zeta*omega_n，可类比二阶误差系统。它提供调参直觉，但完整 SE(3) 姿态误差不是全局线性变量，不宜对大姿态偏差机械使用同一个线性解释。

在该版本中，contact_model.corrector.Kp/Kd 控制修正；6D 误差用 log6 构造，而实现没有把大误差下所有流形微分因素都当成精确全局约束线性化。应先建立几何一致的初始接触，再使用适度增益控制小漂移。误差及修正实现

例如建立约束前可选择 Kp=100、Kd=20 作为一组需要结合步长验证的起点，不是所有机器人通用的稳定参数。增益过大可能使积分变得刚性、接触力出现尖峰；减小步长或进行位置/速度投影也是不同的数值手段，不能混称为同一种修复。

ProximalSettings 的 mu 不是摩擦系数

三参数构造器是 ProximalSettings(accuracy, mu, max_iter)。这里 mu 是算法正则化参数，不是库仑摩擦系数；mu=0 的单次求解适合先理解独立、良态的理想约束，退化或冗余约束可能需要不同处理。

正则化与更多迭代可以影响求解行为，但不会自动使相互矛盾的接触几何变得可行。检查 absolute_residual、relative_residual、iter，并独立检查物理残差。设置定义

7. 双边约束不等于真实地面接触

理想约束求解允许法向力为负，这在数学上相当于地面可以把脚吸住。真实非黏附接触通常至少要求：

$$f_n\ge0,\qquad \lVert f_t\rVert\le\mu_f f_n$$

若考虑接触间隙 d，还涉及单边互补条件：

$$d\ge0,\qquad f_n\ge0,\qquad d f_n=0$$

constraintDynamics 的双边刚性约束不会自动检查这些条件，也不会替你发现碰撞、决定接触建立/脱离或选择摩擦模式。接触切换发生冲击时，速度跃变还需要冲量层面的处理，不能只用连续加速度乘步长代替。

因此，“脚的位置一直没动”不是完整验收。还应检查法向力方向、切向摩擦可行性、6D 力矩是否位于实际足底可实现范围，以及关节力矩限制。无法满足时，应改变控制目标、接触模式或引入相应的不等式求解，而不是简单接受任何 lambda。

8. 从关节方程转到质心：Ag 映射了什么？

整机质心动量由线动量和关于质心的角动量组成：

$$h_G=\begin{bmatrix}p\\L_G\end{bmatrix}=A_G(q)v$$

A_G 是六乘 nv 的质心动量矩阵，不是六乘 nq。它把机器人已有的广义速度坐标映射为世界轴表达、关于质心原点的动量；输入仍然是 Pinocchio 模型的速度坐标，并未要求将根速度预先改成世界系。

cent = model.createData()
Ag = pin.computeCentroidalMap(model, cent, q).copy()
momentum_from_map = Ag @ v
Ag_again = pin.ccrba(model, cent, q, v).copy()
momentum = cent.hg.vector.copy()
com_world = cent.com[0].copy()
assert np.allclose(Ag_again, Ag)
assert np.allclose(momentum, momentum_from_map)

接口与返回值见 质心算法绑定；中间结果与更新范围可直接对照 centroidal.hxx。使用 Ag@v 自检时，应确保没有被别的算法覆盖同一个缓存。

为什么源码会把角动量列加上一个叉乘？

先把所有连杆动量汇总到世界原点，得到 p 和 L_O，再把角动量参考点移到质心 c：

$$L_G=L_O-c\times p=L_O+p\times c$$

这解释了源码中角动量映射列加上“线动量列 cross 质心位置”的操作。它是参考原点变换，不是额外添加了一种物理角动量。

不要把“世界轴表达”自动等同于“关于世界原点”。同理，某足端的 LWA 雅可比虽然也使用世界轴，原点却在足端；Ag 的角动量原点在整机质心。二者的列意义、物理量与原点都不同，不能直接相互替代。

9. 质心方程把接触力和整机运动接起来

设总质量为 m，接触位置为世界点 r_i，接触力和关于该接触点的力矩已转换为世界轴表达，则：

$$\dot p=m\ddot c=mg+\sum_i f_i$$

$$\dot L_G=\sum_i\big((r_i-c)\times f_i+\tau_i\big)$$

在均匀重力场中，重力关于整机质心的总力矩为零；因此第二式没有额外的 c cross mg 项。这里的 tau_i 是外部接触力矩，不是每个关节电机力矩的直接相加。

对本篇 LOCAL 接触力，可用接触世界旋转分别旋转力与关于接触点的力矩，再用 (r_i-c) cross f_i 移到质心。若已经用完整 SE(3) 变换把力矩移到世界原点，就不要再次把它误当成关于接触点的力矩，否则会重复或错误添加力臂项。

内部电机为什么不能直接改变总动量？

理想刚体系统中的内部关节作用力成对出现，在总动量平衡中相互抵消。它们可以重新分配各连杆的运动，也可以通过接触改变外部反力；但在无外力、无外力矩的悬空系统中，不能凭内部动作使整机总动量任意变化。

这一论断针对闭合的机械系统。若模型用理想 armature 近似转子而质心动量只按刚体连杆惯量统计，应先明确所统计系统的边界；守恒自检的入门模型宜设 armature 为零，避免把未显式建模的转子动量和连杆动量混为一谈。

10. 两个能写出解析期望的实验

实验 A：无重力、无接触的悬空关节运动

设模型重力为零、armature 为零，无外力，基座直接驱动力为零，只给内部关节施加力矩。初始总动量为 h0，理论上整机总线动量和关于质心的角动量保持不变。基座和关节都可能运动，不能要求每个连杆动量各自不变。

执行思路是每步用 ABA 求 a，按流形方法积分，再用 ccrba 记录 hg。比较 hg-h0 随步长减小的变化，并同时记录动力学残差。时间离散不一定严格守恒，误差不应被虚构为精确零。

实验 B：仅受均匀重力的自由落体

撤掉接触和其他外力，保留均匀重力。以世界系质心位置、初始质心速度为 c0,vc0，解析关系为：

$$c(t)=c_0+v_{c0}t+\tfrac12gt^2,\qquad p(t)=p_0+mgt$$

关于质心的角动量仍应守恒。即使内部关节运动，质心的抛物线规律也不会因此失效；但根关节原点通常不等于整机质心，所以不能拿根平移直接替代 c(t) 做验证。

如果程序显示机器人不下落，优先检查是否仍保留接触约束、是否误加载固定基模型、是否把基座加速度手动清零，而不是立即怀疑重力参数。

11. 从故障现象反查代码

学习这部分的闭环不是“让脚看起来贴地”，而是把动力学残差、约束残差、接触可行性和整机动量平衡同时串起来。四者对应不同错误来源；当它们都说得通，才有理由进一步做控制器和长期仿真。

下一篇 06：解析导数、流形线性化与控制接口 先在光滑、无接触的模型上建立可核对的 A/B，再解释为什么接触切换不能直接沿用这一套导数。需要对照建模和缓存代码时，回到 04：完整 C++ 工程。