摘要
本文介绍了 Pinocchio，这是一个开源软件框架，实现了刚体动力学算法及其解析导数。Pinocchio 不仅包括机器人领域常用的标准算法（例如，前向和反向动力学），还提供了对机器人控制、规划和仿真至关重要的额外功能。
刚体动力学
刚体动力学是机器人技术中一个非常有用的工具。它们允许以高效的方式计算刚体系统的逆动力学和正向动力学。从 20 世纪 80 年代末开始，最初的实现大多基于代码生成，但与此同时，这种方法也存在着缺乏灵活性的问题：如果机器人模型发生任何细微的修改，代码就必须从头开始重新生成。
随着桌面计算机上计算资源的日益丰富，一种新的范式也应运而生，即一次性编译一个库，该库能够在运行时加载机器人模型（运动链、质量分布等）的描述文件。比如一些框架实现了这种范式：RBDL [9]、OpenHRP [10]、SymBody [11]、RigidBodyDynamics.jl [12]、Drake [13]、Bullet [14]、DART [15] 等。另外，更加灵活也是一个特性，比如修改描述文件的动力学属性，或者模型中添加新的功能，那么可以立即重新计算与生成。
在这篇论文中，我们介绍了一种名为 Pinocchio 的新刚性体动力学框架。Pinocchio 是一个动态库，能够在运行时加载任何机器人模型。
主要特性
C++编写的，出于效率考虑，并使用了 Eigen 库[16]进行线性代数运算。它附带 Python 绑定，便于代码原型设计。
A 空间代数
空间代数[2]是一种在刚体动力学中常用的数学符号，用于表示和操作物理量，如速度、加速度和力。Pinocchio 基于这种数学符号。提供了专门的类来表示 3D 欧几里得空间（称为 SE3）中的坐标变换、空间运动向量（Motion）、空间力向量（Force）和空间惯性（Inertia）。结合提供的方法，这使 Pinocchio 拥有一个高效的空间代数计算软件库。
B 模型与数据
Pinocchio 的一个基本范式是严格区分模型和数据。
所谓模型，指的是机器人的物理描述，包括定义其结构的运动学和可能的惯性参数。此类信息由一个专门的类持有，一旦创建，其内容就不再被皮诺曹的算法修改。
所谓数据，指的是所有计算结果。数据根据系统的关节配置、速度等变化。例如，它包含每个连杆的速度和加速度。它还存储中间计算和算法的最终结果，以防止动态内存分配。
将模型和数据分离可以减少在同一个机器人上执行多个不同任务时的内存占用，尤其是在涉及并行计算时。每个进程都可以使用自己的数据对象，同时共享相同的模型对象。在 Pinocchio 算法中，模型对象始终保持不变，这提高了代码的可预测性。
使用 C++ API 可以创建模型，或者从外部文件加载，该文件可以是 URDF、Lua（遵循 RBDL 标准）或 Python。
C 支持的运动学模型
模型中，机器人被表示为一个运动学树，包含所有关节、它们之间的连接信息，以及每个连杆可选的惯性量。在 Pinocchio 中，一个关节可以有一个或多个自由度，并且属于以下类别之一：
旋转关节，围绕固定轴旋转，可以是 X、Y、Z 轴或自定义轴；
滑动关节，沿任意固定轴平移，类似于旋转关节；
球面关节，在 3D 空间中的自由旋转；
平移关节，用于 3D 空间中的自由平移；
平面关节，用于 2D 空间中的自由运动；
自由浮动机关，用于 3D 空间中的自由运动。
平面和自由浮动机关旨在作为移动机器人（类人机器人、自动驾驶车辆或操作规划中的物体）运动学树的基础。更复杂的关节可以通过组合普通关节的方式，通过组合关节的概念来创建。
D 处理李群几何
每种类型的关节都有其独特的配置和切线空间。例如，旋转关节的配置和切线空间都是实轴线 R，而对于球面关节，其配置空间对应于 3 维旋转矩阵的集合，其切线空间则是 3 维实向量空间 R。某些配置空间可能不具备向量空间的性质。但是必须具备相应的积分（exp）和微分（log）运算符。Pinocchio 实现了所有这些特定的积分和微分运算符。
E 几何模型
除了运动学模型之外，Pinocchio 还定义了一个几何模型，即附着在运动学树上的体积。这个模型可以用于显示机器人并计算与碰撞相关的量。与运动学模型一样，固定量（体积的放置和形状）存储在几何模型对象中，而由相关算法使用的缓冲区和量定义在几何数据对象中。体积使用 FCL 库[17]进行表示。机器人的各个部分都连接到每个关节上，而环境中的障碍物则定义在世界坐标系中。基于 FCL 方法实现了运动学树的碰撞和距离算法。
F 主要算法
a) 前向运动学：Pinocchio 实现了直到二阶的直接运动学计算。当给定一个机器人配置时，将执行前向传递以计算每个关节的空间位置，并将它们存储为坐标变换。如果给定速度，它还会计算每个关节的空间速度（以局部坐标系表示），同样也适用于加速度。
b) 运动雅可比矩阵：每个关节的空间雅可比矩阵可以通过单次前向传递轻松计算，既可以表示为局部坐标系，也可以表示为世界坐标系。
c) 逆动力学：递归牛顿-欧拉算法（RNEA）[18]计算逆动力学：给定期望的机器人配置、速度和加速度，计算执行此运动所需的扭矩，并将其存储。该算法首先执行正向传递（相当于二阶运动学），然后执行反向传递，计算沿结构传递的力矩，并提取所需的关节扭矩以获得计算出的连杆运动。在适当的输入下，此算法还可以用于计算动态模型的特定项，例如重力效应。
d) 联合空间惯性矩阵：机器人采用体算法（CRBA）[19]计算关节空间惯性矩阵。我们对原始算法进行了一些细微的修改，以提高计算效率。
e) 前向动力学：关节体算法（ABA）[20]计算无约束的前向动力学：给定一个机器人配置、速度、扭矩和外部力，计算得到的关节加速度。
f) 其他算法：除了上述算法外，还提供了其他方法，尤其是针对约束前向动力学、冲量动力学、关节空间惯量的逆[21]和质心动力学。
G 分析导数
除了提出标准的正向和逆向动力学算法外，Pinocchio 还提供了这些算法的解析导数的有效实现[22]。这些导数在全身轨迹优化或更广泛地说，在数值最优控制方面至关重要。据我们所知，Pinocchio 是第一个原生实现这一功能的刚体框架。
后续计划
作为后续的里程碑，我们计划在 Pinocchio 中整合传动模型（齿轮、皮带轮、腱等）和驱动模型（气动肌肉、生物肌肉、电动马达等），以便例如在运动方程的动态中考虑它们的物理效应。这将使 Pinocchio 不仅能处理机器人系统，还能模拟生物系统，这对于理解例如拟人化运动的基础至关重要。
使用案例
from __future__ import division
import pinocchio as pin, math
import numpy as np
from matplotlib import pyplot as plt
# load the urdf file of your robot model
model = pin.buildModelFromUrdf('iiwa7_description.urdf')
print('model name: ' + model.name)
# Create data required by the algorithms
data = model.createData()
NQ = model.nq
NV = model.nv
print('Dimension of the configuration vector representation: ' + str(NQ))
print('Dimension of the velocity: ' + str(NV))
# calculate the total mass of the model, put it in data.mass[0] and return it
total_Mass = pin.computeTotalMass(model,data)
print('Total mass of the model: ', data.mass[0])
# Generating joint position, angular velocity and angular acceleration using quintic polynomials
# 1. Generating position q, initial: 0 deg, end: 60 deg, ouput:rad
def mypoly_p(t):
p = np.poly1d([12*60*math.pi/180/(2*3**5),-30*60*math.pi/180/(2*3**4),20*60*math.pi/180/(2*3**3),0,0,0])
return p(t)
q = np.zeros((61, 7))
for i in range(0, 61):
for j in range(7):
q[i, j] = mypoly_p(0 + 3 / 60 * i) # 61*7 matrix, each column represents a sequence of joint
# 2. Generating angular velocity qdot, initial: 0 rad/s, end: rad/s, ouput:rad/s
def mypoly_v(t):
p = np.poly1d([5*12*60*math.pi/180/(2*3**5),-4*30*60*math.pi/180/(2*3**4),3*20*60*math.pi/180/(2*3**3),0,0])
return p(t)
qdot = np.zeros((61, 7))
for i in range(0, 61):
for j in range(7):
qdot[i, j] = mypoly_v(0 + 3 / 60 * i) # 61*7 matrix, each column represents a sequence of joint
# 3. Generating angular acceleration qddot, initial: 0 rad/s^2, end: rad/s^2, ouput:rad/s^2
def mypoly_a(t):
p = np.poly1d([4*5*12*60*math.pi/180/(2*3**5),-3*4*30*60*math.pi/180/(2*3**4),2*3*20*60*math.pi/180/(2*3**3),0])
return p(t)
qddot = np.zeros((61, 7))
for i in range(0, 61):
for j in range(7):
qddot[i, j] = mypoly_a(0 + 3 / 60 * i) # 61*7 matrix, each column represents a sequence of joint
# Calculates the torque of each joint, return 1*7 vector
Torque = np.zeros((61, 7))
for i in range(0,61):
tau = pin.rnea(model,data,q[i],qdot[i],qddot[i]) # 1*7 vector
Torque[i][:] = tau.T
print('The ' + str(i) + 'th Torque is: ',i,tau.T)
# Computes the generalized gravity contribution G(q), stored in data.g
G_Torque = np.zeros((61, 7))
for i in range(0,61):
G_Tau = pin.computeGeneralizedGravity(model,data,q[i])
G_Torque[i][:] = G_Tau
print('The ' + str(i) + 'th G_Tau is: ', G_Tau)
# Computes the upper triangular part of the joint space inertia matrix M, stored in data.M
M_Matrix = np.zeros((61,7,7))
for i in range(0,61):
M_Temp = pin.crba(model,data,q[i])
M_Matrix[i,:,:] = M_Temp
print('The ' + str(i) + 'th M_Matrix is: ', M_Temp)
#Computes the Coriolis Matrix C
C_Matrix = np.zeros((61,7,7))
for i in range(0,61):
C_Temp = pin.computeCoriolisMatrix(model,data,q[i], qdot[i])
C_Matrix[i,:,:] = C_Temp
print('The ' + str(i) + 'th C_Matrix is: ', C_Temp)
# Verify the anti-symmetric property of dM/dt - 2* C, take the fifth sequence as example
M = pin.crba(model,data,q[5])
dt = 1e-8
q_plus = pin.integrate(model,q[5],qdot[5]*dt)
M_plus = pin.crba(model,data,q_plus)
dM = (M_plus - M)/dt
C = pin.computeCoriolisMatrix(model,data,q[5],qdot[5])
print('The ' + str(5) + 'th C_Matrix is: ', C)
A = dM - 2*C
print('A is: ', A)
res = A + A.T
print('res is: ', res)