CH-12 Pinocchio 运动学与动力学
夹爪要到达桌上的抓取目标,程序先得知道当前末端在哪里,再计算关节怎样改变,最后求出执行期望运动所需的力矩。上一章用仿真器推进物理状态,这一章转向计算运动学与刚体动力学。Pinocchio 的机械臂样例和 Modern Robotics 的双连杆算例采用了不同模型,所需的 SE(3)、Jacobian 与递归动力学仍来自第 02–05 章。推导中的每一步都将对应到具体 API,并检查缓存时序、参考坐标系和作用点。
Pinocchio 实现多关节系统的刚体动力学算法,也提供主要算法的解析导数,包括 Recursive Newton–Euler Algorithm 和 Articulated Body Algorithm 的导数。它最初面向机器人,也可用于生物力学、计算机图形和视觉等领域。Python 接口便于建立原型,核心计算由 C++ 完成。
1. 运行环境与 Model/Data
1.1 运行环境
Conda 安装的包名为 pinocchio,Python import 名称也是 pinocchio;PyPI 的发行包名则是 pin。本章以 3.8.0 为固定版本。
1 | conda create -n embodied-pin -c conda-forge python=3.11 pinocchio=3.8.0 |
Linux 上也可以在独立环境中使用 PyPI:
1 | python -m venv .venv |
两种安装方式选择其一即可。版本 3.8.0 的 README 将 pip 路径标为 Linux,其他平台应先检查 Conda 包与平台支持,再决定是否使用 pip install pin。
1.2 模型与计算缓存
首先导入库并创建 Model 与 Data 对象:
1 | import numpy as np |
Model 描述机械系统,Data 保存算法的中间结果与输出,每个 Data 都应与创建它的模型一致。neutral 给出合法的中性构型:对这里的简单机械臂,是全零关节向量;遇到更复杂的关节表示时,使用 neutral 可以避免手工拼接出无效构型。
这种分工与第 11 章的 MjModel/MjData 相似。同一机械系统需要在许多状态上计算,将拓扑与计算缓存分开,就能避免每次重新建立模型和分配全部缓存。
缓存中的结果需要由算法更新。修改 q 后,data.oMi 仍保存上次计算的位姿;如果还要保留一次 RNEA 的输出,应在调用其他算法前复制所需数组。并行计算时,各工作线程应使用独立的 Data,共享只读模型,避免不同计算流程相互覆盖结果。
如果已经有 URDF,加载入口为:
1 | model = pin.buildModelFromUrdf('robot.urdf') |
碰撞和可视化几何分别有自己的模型对象。计算运动学与惯性动力学,首先建立机械系统的 Model;需要检查碰撞或显示结果,再加载相应几何数据。
夹爪的工具中心点通常位于末端关节之外,模型需要表达这个安装偏移。joint 定义活动连接,frame 是附着在关节上的坐标系,point 则是指定坐标系中的几何位置。URDF 的固定连接也可以作为附着 frame 处理,不增加速度自由度。若直接把最后一个 joint 的原点当作夹爪位置,就会遗漏工具安装偏移。
构型与速度还可能采用不同维度。浮动根或球关节使 nq 与 nv 不同:构型是流形上的点,速度和加速度位于切空间,因此 integrate 接收的增量为 nv 维。
2. 正向运动学
给定当前关节构型,正向运动学计算末端的位置和朝向。这是将夹爪位姿与抓取目标比较的起点。
2.1 世界位姿
执行正向运动学后,各关节在世界坐标系中的位姿保存在 data.oMi 中。
1 | pin.forwardKinematics(model, data, q) |
oMi 表示从关节坐标系 i 到世界坐标系 o 的刚体变换。translation 读取平移部分,rotation 读取旋转矩阵。这里的关节索引并不直接等于关节向量中的单个位置索引:不同关节可以占用不同数量的构型参数与速度自由度。
若任务使用名为 tool 的 frame,先查找其索引,在本次 forwardKinematics 后调用 updateFramePlacements,再读取 data.oMf 中的工具位姿。3.8.0 的 frame 接口要求先更新关节运动学,工具安装位姿随后与当前关节位姿组合,得到世界位姿。
读取速度也有相应前提:运动学调用需要传入本次速度,仅输入构型不会重新建立当前速度结果。控制周期应从同一时戳的 $q,v$ 开始,按计算依赖更新缓存,再读取结果。
2.2 末端目标
本例末端对应第 6 个关节。目标位姿由单位旋转矩阵与位置 [1, 0, 1] 给出:
1 | JOINT_ID = 6 |
这个目标同时规定末端位置和朝向。SE3 对象把两者放在同一个刚体变换中,后面的残差也会包含平移与转动。
求解前需要检查机器人能够完成怎样的任务。目标应位于可达集合内,局部 Jacobian 也应具有任务所需的秩。例如,平面 2R 机械臂只能沿二维构型集合移动末端,位置与朝向相互关联,任意指定空间六维目标通常会造成过约束。
末端索引同样属于模型约定。本例选择第 6 个关节,换成 URDF 后应重新查找任务 frame,确认它对应实际工具位置。
3. 逆运动学
逆运动学从目标位姿求关节构型。迭代时,关节改变带动当前末端移动,所以需要用一致的坐标系定义误差。这里将固定目标变换到当前末端的局部坐标系,再求六维位姿残差。
3.1 局部位姿误差
每一次迭代先计算当前位姿,再计算当前末端与目标之间的相对变换:
1 | pin.forwardKinematics(model, data, q) |
actInv 对应先求当前变换的逆,再作用于目标变换。因此,
这个误差表达在当前关节坐标系 i 中。log 将 SE(3) 相对变换映射到李代数 se(3),得到六维的局部误差向量。
六维向量中的线性量和角度量具有不同单位,直接计算 Euclidean 范数会混合两者。工程实现可以分别判断位置与姿态误差,或给出明确的度量与权重。这里的简单示例使用统一范数,以便先看清迭代过程。
Pinocchio 将误差向量存为 $[\rho;\phi]$,前三项是平移相关的李代数坐标,后三项是旋转向量。相对旋转较大时,$\rho$ 一般不等于相对变换的平移列,两者通过 SE(3) 指数映射中的矩阵联系。纯平移或小误差近似下,才可以将前三项直接理解为平移差。
报告到达精度时,可以另算世界坐标中的位置差和 SO(3) 旋转角。若需要统一残差尺度,则选择特征长度 $\ell$,用 $[\rho/\ell;\phi]$ 构造无量纲度量。$\ell$ 由任务对位置与朝向的相对要求决定,需要在实现中明确指定。
3.2 误差 Jacobian
计算当前关节的局部 Jacobian:
1 | J = pin.computeJointJacobian(model, data, q, JOINT_ID) |
第一行计算关节运动的 Jacobian,第二行用 Jlog6 转换为位姿误差的 Jacobian。关节运动先改变相对变换,再经过 logarithm 得到 err,求导也必须沿着这条链进行。
转换后的 $J$ 满足局部线性近似 $e(q\oplus\delta q)\approx e(q)+J\delta q$,其中 $\oplus$ 表示构型上的积分。固定目标下,当前末端运动会使相对误差反向变化,所以链式求导包含上面的负号与 logarithm 导数。
中心差分可以检查这条求导链。选取切空间基向量,用 integrate 得到正负微扰构型,分别刷新运动学并计算同一个残差,再比较差分结果。四元数的四个存储分量带有约束,直接对它们差分无法代替对三个旋转自由度求导;未经 Jlog6 转换的关节运动 Jacobian,也不能直接与误差导数比较。
3.3 阻尼伪逆
为了缓解奇异位形附近的问题,使用阻尼伪逆计算速度增量:
代码通过解线性方程组实现这一式子,而不显式构造逆矩阵:
1 | from numpy.linalg import solve |
integrate 将切空间增量作用到当前构型上。在这个简单机械臂中,它相当于相加;对于其他关节类型,它会执行构型空间所需的几何更新。更新之后,下一轮重新计算位姿和误差。
这里采用固定阻尼,便于观察算法的各个环节。实际收敛速度与效果还取决于目标、初始构型和 Jacobian 的条件,需要结合任务调整。
阻尼更新可由带正则项的局部最小二乘得到。求 $\min_v\tfrac12\|Jv+e\|^2+\tfrac\lambda2\|v\|^2$,令梯度为零,便有 $(J^TJ+\lambda I)v=-J^Te$;当 $\lambda>0$,可转写成上面的六维任务空间求解。
借助第 01 章的 SVD,各奇异方向的增益为 $\sigma/(\sigma^2+\lambda)$。无阻尼时,增益 $1/\sigma$ 会在小奇异值方向放大误差和数值噪声;加入阻尼后,接近零的方向被抑制,该方向的误差也就无法在一步中消除。代码中的 damp 对应 $\lambda$。有些文献将同一项记为阻尼系数的平方,比较数值前应统一写法。
取尺度已统一的二维局部任务 $J=\operatorname{diag}(1,0.001)$、$e=(0.1,0.01)$。无阻尼时,更新为 $v=(-0.1,-10)$;令 $\lambda=10^{-4}$,则得到约 $(-0.099990,-0.099010)$。第二个方向的步长显著减小,单位步长的线性预测会留下约 $0.009901$ 的第二项残差。乘以这里的 DT=0.1 后,实际构型增量约为 $(-0.009999,-0.009901)$。
阻尼与动力学复算脚本核对这些解析数值,用来观察奇异值对更新的影响,其中没有 Pinocchio 收敛轨迹。原例的 $10^{-12}$ 阻尼很小,在明显病态的任务中,对步长的限制会弱于这里的 $10^{-4}$。
4. CLIK 迭代
下面的程序实现 closed-loop inverse kinematics。最大迭代次数用于避免在目标不可达时无限循环,DT 调整每次构型更新的幅度,eps 决定停止阈值。
1 | import numpy as np |
下载逆运动学脚本可以运行上游完整循环。得到关节向量之后,还应一起检查收敛标志和最终误差,确认目标是否到达。
1 | python inverse-kinematics.py |
这一迭代处理局部位姿误差,尚未加入碰撞、关节范围、速度或力矩限制。实际抓取若需要这些约束,应继续使用带约束的规划与控制检查候选构型。
4.1 终止条件
输出 Convergence achieved! 时,循环中的 norm(err) 已低于 1e-4;达到最大迭代次数时,脚本会提示未收敛。两种情况都打印关节构型与最终误差,记录结果时应连同终止信息保存。图 1 给出计算顺序,其中没有收敛曲线。
DT 控制这个离线 IK 循环的更新幅度,与真实机器人的控制周期分别设定。较小步长使局部线性近似更可信,也会增加迭代次数;较大步长可能越过收敛区域。候选更新若没有改善误差,可以尝试缩小步长,但这种局部处理无法保证找到全局解。
工程实现还应检查残差长期不降、增量过小、非有限值和迭代预算耗尽,并分别判断位置与角度是否达标。若因约束边界或不可达目标停止,应返回当前残差与失败原因,再交给第 06 章的规划或带约束优化处理。执行器接收构型前,还需要完成这些可执行性检查。
5. 刚体动力学
逆运动学给出候选构型,规划再生成期望关节运动。要执行这段运动,还需要力矩;反过来,给定力矩,也希望预测加速度。RNEA 和 ABA 分别完成这两个计算,CRBA 则提供惯性矩阵。
5.1 RNEA 逆动力学
Recursive Newton–Euler Algorithm 计算逆动力学:给定构型、速度和期望加速度,得到执行该运动所需的关节力矩。
算法先向前递推,完成二阶运动学计算,再向后递推各连杆传递的 wrench,从中提取关节力矩。选用特定输入,还可以单独计算重力等动力学项。
1 | v = np.zeros(model.nv) |
第 04 章的无外力方程为 $\tau=M(q)a+h(q,v)$,其中 $h$ 汇总速度相关项与重力。将速度和加速度都设为零,RNEA 就给出静止维持该构型的重力补偿力矩;模型重力若为零,输出的这一项也消失。
纳入外力时,还需要指定各 wrench 的作用点和表达坐标系,因为改变作用点会改变力矩。RNEA 得到的是模型要求的广义力,驱动饱和、未建模摩擦与状态延迟仍需在控制系统中处理。
5.2 CRBA 惯性矩阵
Composite Rigid Body Algorithm 用于计算机器人在关节空间中的惯性矩阵。该矩阵对应动力学方程中与加速度相乘的 $M(q)$。
1 | M = pin.crba(model, data, q) |
3.8.0 的 CRBA 接口保证更新惯性矩阵的上三角部分。计算动能、特征值或完整矩阵乘法之前,需要从上三角补全对称矩阵,避免使用尚未更新的下三角。可以在基本调用后建立完整副本:
1 | M_full = np.triu(M) + np.triu(M, 1).T |
没有冗余速度坐标且惯性参数合法时,$\tfrac12v^TM_{\mathrm{full}}v$ 给出动能,正定性对应非零速度具有非零动能。若读到非对称矩阵,应先按接口约定补全输出,再检查模型参数。
5.3 ABA 前向动力学
Articulated Body Algorithm 计算无约束前向动力学:给定构型、速度、力矩与外力,计算由此产生的关节加速度。
1 | ddq = pin.aba(model, data, q, v, tau) |
这些算法可以互相检查。对同一无约束模型,先用 RNEA 将给定运动转换为力矩,再将力矩送入 ABA,应能恢复原先的加速度;CRBA 则用于检查其中的惯性响应。
ABA 沿树递推求出 $a$,无需先形成完整矩阵并显式求逆。若想单独检查惯性项,可在相同 $q,v$ 下比较 $\operatorname{rnea}(q,v,a)-\operatorname{rnea}(q,v,0)$ 与 $M_{\mathrm{full}}a$,通过相减消去偏置项。
以教学矩阵 $M=\begin{bmatrix}2&0.5\\0.5&1\end{bmatrix}$、$h=(0.3,-0.2)$、$a=(0.4,-0.6)$ 为例,计算得到力矩 $(0.8,-0.6)$;解 $M a=\tau-h$ 可以恢复原加速度,矩阵行列式为 $1.75>0$。复算脚本核对这个代数例子,Pinocchio 脚本则执行库中同一模型的往返计算。
1 | a = np.linspace(-0.2, 0.2, model.nv) |
此外,Pinocchio 还提供约束前向动力学、冲量动力学、惯性矩阵求逆和质心动力学等算法。存在接触约束时,应当使用对应的约束动力学接口,或者显式把接触力纳入所分析的问题。
5.4 动力学一致性
往返检查脚本把一组给定加速度经 RNEA 转为力矩,再用 ABA 恢复加速度:
1 | python 12-dynamics-check.py |
输出包括库版本、nq/nv、round-trip error 和 close。检查使用 np.allclose 的 1e-9 绝对与相对容差;它用于同一无约束模型上的 API 一致性检查,不包含接触或关节限制。
往返检查验证两个算法在同一模型上的一致性。共享错误惯量的两个算法,也可能精确恢复原加速度,因此还需要独立的物理检查,例如单连杆重力力矩的解析量级、平行轴变换后的动能,或实测力矩与加速度关系。
跨 MuJoCo 与 Pinocchio 比较时,应先统一重力、惯量、浮动根、关节顺序和外力,去掉接触与不同驱动模型的影响,再逐项增加约束。若直接比较含接触的仿真加速度与无约束 ABA 结果,差异可能来自省略的 $J^Tf$。
6. 控制系统接口
把这些计算接入控制循环时,还要考虑重复调用的开销。C++ 实现可以预先分配 Jacobian 并反复使用,阻尼线性系统也可以通过分解求解。这里的 Python 示例采用简洁写法,便于逐步检查模型、误差和更新。
Pinocchio 可以配合 Meshcat、Gepetto Viewer 显示机器人,也可以为 Crocoddyl、Stack-of-Tasks 和运动规划系统提供计算。连接这些组件时,要确认它们使用一致的构型、关节顺序、坐标系和模型参数,再分别检查运动学、动力学、碰撞与可视化结果。
7. POE 几何验证
前面的 API 检查之后,可以用几何更简单的模型核对第 02、03 章的 SE(3)、螺旋轴与 POE。下面用 Modern Robotics(MR)配套代码建立平面 2R 机械臂,两根连杆均长 1 米,分别检查正向运动学、空间 Jacobian 与逆运动学。它便于手工核算几何约定;前面的 Pinocchio 流程则处理完整样例模型或 URDF。
7.1 平面 2R 模型
两个转轴都沿世界坐标系的 $z$ 方向。零位时,第二个轴经过 $(1,0,0)$,末端位于 $(2,0,0)$,且朝向为单位矩阵。空间螺旋轴按列放入 S_mr;第二列的线性部分由 $v=-\omega\times q$ 得到,其中 $q$ 是轴上一点。目标构型选为 $30^\circ,60^\circ$,末端朝向因而旋转 $90^\circ$。
FKinSpace 接收末端零位 M、零位空间螺旋轴矩阵和关节坐标,返回末端相对于空间坐标系的齐次变换。JacobianSpace 返回对应的 $6\times n$ 空间 Jacobian。IKinSpace 接收目标位姿、邻近的初始关节猜测,以及角度和线性误差容差;它使用 Newton–Raphson 迭代,固定版本的最大迭代次数为 20,并返回解与成功标志。
7.2 数值核验
下载并解压 2R 算例包,脚本与 modern_robotics/ 已放在同一目录。也可以分别保存完整算例,把配套的 core.py、初始化文件 和 LICENSE 放到脚本旁的 modern_robotics/ 目录,并将初始化文件命名为 __init__.py。在已有 NumPy 的环境中执行:
1 | python 12-mr-planar-check.py |
第 02–07 章的螺旋位移、POE、递归动力学、计算力矩、时间缩放和抓取核算也收录在系列算例合集中。合集保留对应模块和数值结果;各脚本独立运行,绘图示例另需 Matplotlib。
下面的独立代码包含同一组核心计算,变量后缀 _mr 用来与前面 Pinocchio 的模型区分:
1 | import numpy as np |
完整脚本还用独立三角公式检查末端位置。实际 CPU 运行的关键输出为:
1 | FK position (m): [0.866025404 1.5 0. ] |
目标由已知构型生成,再从 $20^\circ,70^\circ$ 开始求解,因此本例目标位于 2R 机械臂的可达集合内。成功之后的正向计算检查这个目标的恢复误差,尚不能推及任意三维位姿。
有限差分检查需要选择合适的微扰幅度。中心差分的截断误差随 $h^2$ 缩小,浮点相减的舍入误差却大致随机器精度除以 $h$ 放大;持续减小 $h$,最终会让舍入误差占主导。这里取 $10^{-6}$,适用于本例米与弧度的量级。改用毫米坐标或病态构型时,应尝试邻近幅度,检查误差是否有稳定区间。
目标来源也影响能够验证的内容。本例 IK 目标由同一正向模型生成,本来就属于模型的可达集合。换成独立测量的目标后,标定偏差可能使残差无法达到相同容差,此时应分别检查数值求解误差与模型误差。

7.3 接口约定
MR 的六维运动向量按 $[\omega;v]$ 排列,Pinocchio 的 Motion.vector 按 $[v;\omega]$ 排列。调整存储顺序之外,还要确认线性部分对应什么量。对于末端位置 $p$,空间 twist 与点速度满足 $\dot p=v+\omega\times p$,所以代码用 $J_p=J_v-[p]_\times J_\omega$ 核对位置有限差分,空间 Jacobian 的后三行并非直接给出末端点速度。
跨库比较前,应统一参考坐标系和参考点、零位、关节顺序、轴方向、末端安装变换与单位。局部 Jacobian 转为空间 Jacobian 还需要伴随变换,交换上下三行只处理存储顺序。2R 与 Pinocchio 样例机械臂的结果各自对应所用模型;进行跨库数值比较时,还需要先使用同一机器人模型。
Pinocchio 的 frame Jacobian 用三种选项分别规定参考点与表达轴:LOCAL 以目标 frame 原点为参考点,沿局部轴表达;LOCAL_WORLD_ALIGNED 保留该参考点,沿世界轴表达;WORLD 则以世界原点为参考点,也沿世界轴表达。这些选择之间的转换涉及坐标变换,仅交换行次序并不够。
例如,纯绕世界原点旋转的空间 twist 可以具有零线性项,距原点一米的工具点却仍有速度。抓取控制若需要工具点沿世界轴表达的平移速度,应选择与该点及坐标轴一致的 Jacobian。这样,第 03 章的速度映射才能正确接入第 05 章的控制与第 13 章的动作接口。
参考资料
Stack-of-Tasks contributors. Pinocchio README. Version 3.8.0. © 2014–2023 CNRS,© 2018–2025 INRIA;中文节选与官方示例改编,校订 SE(3) 误差表述并增加检查;BSD 2-Clause。
Stack-of-Tasks contributors. Inverse Kinematics (CLIK); Python Example; Dynamics Algorithms. Version 3.8.0.
Stack-of-Tasks contributors. Pinocchio Features. Spatial Algebra; Model and Data; Joints; Robot Model; Lie Group Geometry; Kinematic Algorithms. Version 3.8.0.
Carpentier, Justin, et al. The Pinocchio C++ Library: A Fast and Flexible Implementation of Rigid Body Dynamics Algorithms and Their Analytical Derivatives. SII, 2019.
Lynch, Kevin M., and Frank C. Park. Modern Robotics: Mechanics, Planning, and Control. Cambridge University Press, 2017; updated preprint, 2019. Chapter 4: Forward Kinematics; Chapter 5: Velocity Kinematics and Statics; Chapter 6: Inverse Kinematics. Sections 4.1.1, 5.1.1, and 6.2;概念核对与配套算例。
NxRLab contributors. Modern Robotics Python Code Library. Fixed commit cbd41fdf0bf75dbd5e986d832ae14226964ff0ac; VecTose3, FKinSpace, JacobianSpace, IKinSpace. © 2019 NxRLab;配套代码及其说明采用 MIT License,完整 core.py 保持原样。函数说明作中文节选;2R 参数、数值检查、图 2 与接口衔接为独立教学案例。
Stack-of-Tasks contributors. Pinocchio MotionTpl. Version 3.8.0, LINEAR = 0 and ANGULAR = 3; motion vector storage order.
Stack-of-Tasks contributors. Pinocchio Frame Algorithms; CRBA Interface. Version 3.8.0; frame placement prerequisites, LOCAL/WORLD/LOCAL_WORLD_ALIGNED, upper-triangular inertia output. 固定版本的接口约定;阻尼与二阶动力学代数算例为独立教学推导。

