夹爪要到达桌上的抓取目标,程序先得知道当前末端在哪里,再计算关节怎样改变,最后求出执行期望运动所需的力矩。上一章用仿真器推进物理状态,这一章转向计算运动学与刚体动力学。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
2
3
conda create -n embodied-pin -c conda-forge python=3.11 pinocchio=3.8.0
conda activate embodied-pin
python -c "import pinocchio; print(pinocchio.__version__)"

Linux 上也可以在独立环境中使用 PyPI:

1
2
3
python -m venv .venv
source .venv/bin/activate
python -m pip install pin==3.8.0

两种安装方式选择其一即可。版本 3.8.0 的 README 将 pip 路径标为 Linux,其他平台应先检查 Conda 包与平台支持,再决定是否使用 pip install pin。

1.2 模型与计算缓存

首先导入库并创建 Model 与 Data 对象:

1
2
3
4
5
6
import numpy as np
import pinocchio as pin

model = pin.buildSampleModelManipulator()
data = model.createData()
q = pin.neutral(model)

Model 描述机械系统,Data 保存算法的中间结果与输出,每个 Data 都应与创建它的模型一致。neutral 给出合法的中性构型:对这里的简单机械臂,是全零关节向量;遇到更复杂的关节表示时,使用 neutral 可以避免手工拼接出无效构型。

这种分工与第 11 章的 MjModel/MjData 相似。同一机械系统需要在许多状态上计算,将拓扑与计算缓存分开,就能避免每次重新建立模型和分配全部缓存。

缓存中的结果需要由算法更新。修改 q 后,data.oMi 仍保存上次计算的位姿;如果还要保留一次 RNEA 的输出,应在调用其他算法前复制所需数组。并行计算时,各工作线程应使用独立的 Data,共享只读模型,避免不同计算流程相互覆盖结果。

如果已经有 URDF,加载入口为:

1
2
model = pin.buildModelFromUrdf('robot.urdf')
data = model.createData()

碰撞和可视化几何分别有自己的模型对象。计算运动学与惯性动力学,首先建立机械系统的 Model;需要检查碰撞或显示结果,再加载相应几何数据。

夹爪的工具中心点通常位于末端关节之外,模型需要表达这个安装偏移。joint 定义活动连接,frame 是附着在关节上的坐标系,point 则是指定坐标系中的几何位置。URDF 的固定连接也可以作为附着 frame 处理,不增加速度自由度。若直接把最后一个 joint 的原点当作夹爪位置,就会遗漏工具安装偏移。

构型与速度还可能采用不同维度。浮动根或球关节使 nq 与 nv 不同:构型是流形上的点,速度和加速度位于切空间,因此 integrate 接收的增量为 nv 维。

2. 正向运动学

给定当前关节构型,正向运动学计算末端的位置和朝向。这是将夹爪位姿与抓取目标比较的起点。

2.1 世界位姿

执行正向运动学后,各关节在世界坐标系中的位姿保存在 data.oMi 中。

1
2
3
pin.forwardKinematics(model, data, q)
for name, oMi in zip(model.names, data.oMi):
print(name, oMi.translation)

oMi 表示从关节坐标系 i 到世界坐标系 o 的刚体变换。translation 读取平移部分,rotation 读取旋转矩阵。这里的关节索引并不直接等于关节向量中的单个位置索引:不同关节可以占用不同数量的构型参数与速度自由度。

若任务使用名为 tool 的 frame,先查找其索引,在本次 forwardKinematics 后调用 updateFramePlacements,再读取 data.oMf 中的工具位姿。3.8.0 的 frame 接口要求先更新关节运动学,工具安装位姿随后与当前关节位姿组合,得到世界位姿。

读取速度也有相应前提:运动学调用需要传入本次速度,仅输入构型不会重新建立当前速度结果。控制周期应从同一时戳的 $q,v$ 开始,按计算依赖更新缓存,再读取结果。

2.2 末端目标

本例末端对应第 6 个关节。目标位姿由单位旋转矩阵与位置 [1, 0, 1] 给出:

1
2
JOINT_ID = 6
oMdes = pin.SE3(np.eye(3), np.array([1.0, 0.0, 1.0]))

这个目标同时规定末端位置和朝向。SE3 对象把两者放在同一个刚体变换中,后面的残差也会包含平移与转动。

求解前需要检查机器人能够完成怎样的任务。目标应位于可达集合内,局部 Jacobian 也应具有任务所需的秩。例如,平面 2R 机械臂只能沿二维构型集合移动末端,位置与朝向相互关联,任意指定空间六维目标通常会造成过约束。

末端索引同样属于模型约定。本例选择第 6 个关节,换成 URDF 后应重新查找任务 frame,确认它对应实际工具位置。

3. 逆运动学

逆运动学从目标位姿求关节构型。迭代时,关节改变带动当前末端移动,所以需要用一致的坐标系定义误差。这里将固定目标变换到当前末端的局部坐标系,再求六维位姿残差。

3.1 局部位姿误差

每一次迭代先计算当前位姿,再计算当前末端与目标之间的相对变换:

1
2
3
pin.forwardKinematics(model, data, q)
iMd = data.oMi[JOINT_ID].actInv(oMdes)
err = pin.log(iMd).vector

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
2
J = pin.computeJointJacobian(model, data, q, JOINT_ID)
J = -pin.Jlog6(iMd.inverse()) @ J

第一行计算关节运动的 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
2
3
4
5
6
from numpy.linalg import solve

damp = 1e-12
DT = 1e-1
v = -J.T @ solve(J @ J.T + damp * np.eye(6), err)
q = pin.integrate(model, q, v * DT)

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}$。

CLIK算法中当前位姿、目标、局部误差、Jacobian和构型积分的计算关系
图1. 目标与当前位姿共同定义局部误差,再通过任务 Jacobian 计算构型更新。

4. CLIK 迭代

下面的程序实现 closed-loop inverse kinematics。最大迭代次数用于避免在目标不可达时无限循环,DT 调整每次构型更新的幅度,eps 决定停止阈值。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
import numpy as np
import pinocchio as pin
from numpy.linalg import norm, solve

model = pin.buildSampleModelManipulator()
data = model.createData()
JOINT_ID = 6
oMdes = pin.SE3(np.eye(3), np.array([1.0, 0.0, 1.0]))
q = pin.neutral(model)
eps, IT_MAX, DT, damp = 1e-4, 1000, 1e-1, 1e-12

for i in range(IT_MAX + 1):
pin.forwardKinematics(model, data, q)
iMd = data.oMi[JOINT_ID].actInv(oMdes)
err = pin.log(iMd).vector
if norm(err) < eps:
success = True
break
if i == IT_MAX:
success = False
break
J = pin.computeJointJacobian(model, data, q, JOINT_ID)
J = -pin.Jlog6(iMd.inverse()) @ J
v = -J.T @ solve(J @ J.T + damp * np.eye(6), err)
q = pin.integrate(model, q, v * DT)

print('success:', success)
print('q:', q)
print('final error:', err)

下载逆运动学脚本可以运行上游完整循环。得到关节向量之后,还应一起检查收敛标志和最终误差,确认目标是否到达。

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
2
3
v = np.zeros(model.nv)
a = np.zeros(model.nv)
tau = pin.rnea(model, data, q, v, a)

第 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
2
3
4
a = np.linspace(-0.2, 0.2, model.nv)
tau = pin.rnea(model, data, q, v, a).copy()
recovered = pin.aba(model, data, q, v, tau).copy()
print('round-trip error:', np.linalg.norm(recovered - a))

此外,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
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
import numpy as np
import modern_robotics as mr

M_mr = np.eye(4)
M_mr[0, 3] = 2.0
S_mr = np.array([[0, 0], [0, 0], [1, 1],
[0, 0], [0, -1], [0, 0]], dtype=float)
theta_mr = np.deg2rad([30.0, 60.0])
T_mr = mr.FKinSpace(M_mr, S_mr, theta_mr)
Js_mr = mr.JacobianSpace(S_mr, theta_mr)
p_mr = T_mr[:3, 3]
Jp_mr = Js_mr[3:] - mr.VecToso3(p_mr) @ Js_mr[:3]
h = 1e-6
Jp_fd = np.column_stack([
(mr.FKinSpace(M_mr, S_mr, theta_mr + h * e)[:3, 3]
- mr.FKinSpace(M_mr, S_mr, theta_mr - h * e)[:3, 3]) / (2 * h)
for e in np.eye(2)
])
theta_ik, ok = mr.IKinSpace(
S_mr, M_mr, T_mr, np.deg2rad([20.0, 70.0]), 1e-8, 1e-8)
T_ik = mr.FKinSpace(M_mr, S_mr, theta_ik)
assert ok and np.allclose(T_ik, T_mr, atol=1e-8, rtol=0)
assert np.allclose(Jp_mr, Jp_fd, atol=1e-9, rtol=0)
print('position:', p_mr)
print('IK success:', ok)
print('angles:', np.rad2deg(theta_ik))

完整脚本还用独立三角公式检查末端位置。实际 CPU 运行的关键输出为:

1
2
3
4
5
FK position (m): [0.866025404 1.5         0.         ]
Finite-difference max error: 9.864e-11
IK success: True
IK angles (deg): [29.999999993 60.000000007]
FK recovery max error: 9.996e-11

目标由已知构型生成,再从 $20^\circ,70^\circ$ 开始求解,因此本例目标位于 2R 机械臂的可达集合内。成功之后的正向计算检查这个目标的恢复误差,尚不能推及任意三维位姿。

有限差分检查需要选择合适的微扰幅度。中心差分的截断误差随 $h^2$ 缩小,浮点相减的舍入误差却大致随机器精度除以 $h$ 放大;持续减小 $h$,最终会让舍入误差占主导。这里取 $10^{-6}$,适用于本例米与弧度的量级。改用毫米坐标或病态构型时,应尝试邻近幅度,检查误差是否有稳定区间。

目标来源也影响能够验证的内容。本例 IK 目标由同一正向模型生成,本来就属于模型的可达集合。换成独立测量的目标后,标定偏差可能使残差无法达到相同容差,此时应分别检查数值求解误差与模型误差。

两根一米连杆的零位、三十度与六十度目标构型,以及逆运动学恢复的末端
图2. 零位与目标构型;绿色圆圈标出实际数值 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. 固定版本的接口约定;阻尼与二阶动力学代数算例为独立教学推导。