【Pinocchio】概述

最新推荐文章于 2025-11-17 09:40:38 发布

原创最新推荐文章于 2025-11-17 09:40:38 发布 · 1.6k 阅读

CC 4.0 BY-SA版权

什么是 Pinocchio？

Pinocchio 是一个用于高效计算机器人模型或任何你想要的铰接刚体模型（如模拟器中的虚拟角色、生物力学中的骨骼模型等）动力学（及其导数）的库。Pinocchio 是计算铰接体动力学最高效的库之一。它实现了遵循 Featherstone 2008 年著作中描述方法的经典算法（非常感谢他）。它还引入了一些算法的高效变体，以及一些新算法，特别是包括一套完整的算法来计算主要算法的导数。

Pinocchio 是开源的，主要用 C++ 编写并带有 Python 绑定，遵循 BSD 许可证发布。欢迎贡献。

在本文档中，您将找到库功能的常规描述、快速教程以了解实现背后的数学原理、一系列关于如何实现经典应用（如逆运动学、接触动力学、碰撞检测等）的示例，以及一组面向初学者的实践练习。

如何安装 Pinocchio？

Pinocchio 最好从我们的仓库通过 APT 包管理在 Ubuntu 14.04、16.04 和 18.04 上安装。在 Mac OS X 上，我们支持通过 Homebrew 包管理器安装 Pinocchio。如果您只需要 Python 绑定，可以直接通过 Conda 获取。对于未提供二进制文件的系统，从源码安装应该很简单。每个版本都在主要的 Linux 发行版和 Mac OS X 上进行了验证。

完整的安装步骤可在项目的 Github 页面上找到：http://stack-of-tasks.github.io/pinocchio/download.html。

最简单的编译命令示例

我们从一个简单的程序开始，用于计算机器人逆动力学。它提供了 C++和 Python 两个版本。

overview-simple.cpp	overview-simple.py
#include <iostream> #include "pinocchio/multibody/sample-models.hpp" #include "pinocchio/algorithm/joint-configuration.hpp" #include "pinocchio/algorithm/rnea.hpp" int main() { pinocchio::Model model; pinocchio::buildModels::manipulator(model); pinocchio::Data data(model); Eigen::VectorXd q = pinocchio::neutral(model); Eigen::VectorXd v = Eigen::VectorXd::Zero(model.nv); Eigen::VectorXd a = Eigen::VectorXd::Zero(model.nv); const Eigen::VectorXd & tau = pinocchio::rnea(model, data, q, v, a); std::cout << "tau = " << tau.transpose() << std::endl; }	import pinocchio model = pinocchio.buildSampleModelManipulator() data = model.createData() q = pinocchio.neutral(model) v = pinocchio.utils.zero(model.nv) a = pinocchio.utils.zero(model.nv) tau = pinocchio.rnea(model, data, q, v, a) print("tau = ", tau.T)

overview-simple.cpp

overview-simple.py

#include <iostream>

#include "pinocchio/multibody/sample-models.hpp"

#include "pinocchio/algorithm/joint-configuration.hpp"

#include "pinocchio/algorithm/rnea.hpp"

int main()

{

pinocchio::Model model;

pinocchio::buildModels::manipulator(model);

pinocchio::Data data(model);

Eigen::VectorXd q = pinocchio::neutral(model);

Eigen::VectorXd v = Eigen::VectorXd::Zero(model.nv);

Eigen::VectorXd a = Eigen::VectorXd::Zero(model.nv);

const Eigen::VectorXd & tau = pinocchio::rnea(model, data, q, v, a);

std::cout << "tau = " << tau.transpose() << std::endl;

}

import pinocchio

model = pinocchio.buildSampleModelManipulator()

data = model.createData()

q = pinocchio.neutral(model)

v = pinocchio.utils.zero(model.nv)

a = pinocchio.utils.zero(model.nv)

tau = pinocchio.rnea(model, data, q, v, a)

print("tau = ", tau.T)

编译并运行你的程序

你可以通过包含 Pinocchio 和 Eigen 头文件目录来编译 C++版本。

g++ -std=c++11 overview-simple.cpp -o overview-simple $(pkg-config --cflags --libs pinocchio)

一旦你的代码编译完成，就可以使用它来运行了

./overview-simple

在 Python 中，直接运行它：

python overview-simple.py

程序说明

该程序加载机器人模型，创建用于算法缓冲的数据结构，并运行递归牛顿-欧拉算法（RNEA）以计算机器人逆动力学。

我们首先包含适当的文件。在 C++中，包含文件尚未实现合理化。这里我们需要包含样本模型（其中定义了测试中使用的模型参数），以及定义机器人中立位置和 RNEA 函数的头文件。在 Python 中，只需导入 pinocchio 即可更轻松地包含库。

第一段定义了模型。在 C++中，我们首先定义对象，然后将其分配为我们的样本模型（一个简单的内置操作器，对于小型测试非常有用）。在 Python 中，模型通过调用单个命令创建。模型类包含机器人的常量参数（质量、段长度、身体名称等），即不会被算法修改的参数。由于大多数算法需要额外的空间来存储内部值，我们还必须分配一个专门用于模型的数据结构。Pinocchio 依赖于 Model 中的常量参数和 Data 中的计算缓冲区之间的严格分离。

逆动力学计算跟踪由关节配置、速度和加速度定义的轨迹所需的扭矩。在第二段中，我们定义了这三个量。速度和加速度是普通向量，可以初始化为任何值。它们的维度为 model.nv ，对应于机器人的瞬时自由度数量。配置可能比这更复杂（例如，它可能包含四元数）。在当前示例中并非如此，因为我们有一个简单的机械臂，但对于更复杂的模型（如人形机器人），这可能会带来困难。因此，提供了辅助函数来将配置初始化为零（中性）或随机值，或确保配置有效（归一化）。

最后，我们通过提供机器人模型、相应数据以及当前配置、速度和加速度来调用 rnea函数。结果是一个维度为model.nv的向量。它也被存储在data.tau中，以备需要时使用。对于机械臂而言，该值对应于实现给定运动所需的关节扭矩。我们仅将其打印出来。注意，在 Python 中，我们使用 numpy 来表示矩阵和向量。因此，tau是numpy矩阵对象，q、v和a 也是如此。

在我们继续之前，一个小提示：在我们的示例中，为了清晰起见，我们通常包含完整的命名空间 pinocchio 。然而，如果您觉得“pinocchio”太长而不便输入，推荐的简写形式是

namespace pin = pinocchio;

在 C++ 中

import pinocchio as pin

在 Python 中。

更复杂的 C++和 Python 示例

overview-urdf.cpp	overview-urdf.py
#include "pinocchio/parsers/urdf.hpp" #include "pinocchio/algorithm/joint-configuration.hpp" #include "pinocchio/algorithm/kinematics.hpp" #include <iostream> // PINOCCHIO_MODEL_DIR 由 CMake 定义，但你也可以在这里定义你自己的目录。 #ifndef PINOCCHIO_MODEL_DIR #define PINOCCHIO_MODEL_DIR "模型目录路径" #endif int main(int argc, char ** argv) { using namespace pinocchio; // 你应该在这里更改以设置你自己的 URDF 文件，或者直接将其作为此函数的参数传递 // example. const std::string urdf_filename = (argc <= 1) ? PINOCCHIO_MODEL_DIR + std::string("/example-robot-data/robots/ur_description/urdf/ur5_robot.urdf") : argv[1]; // 加载 urdf 模型 Model model; pinocchio::urdf::buildModel(urdf_filename, model); std::cout << "模型名称: " << model.name << std::endl; // 创建算法所需的数据数据数据(模型); // 随机采样一个配置 Eigen::VectorXd q = randomConfiguration(model); std::cout << "q: " << q.transpose() << std::endl; // 在运动树上执行正向运动学 forwardKinematics(model, data, q); // 打印出运动树每个关节的位置 for (JointIndex joint_id = 0; joint_id < (JointIndex)model.njoints; ++joint_id) std::cout << std::setw(24) << std::left << model.names[joint_id] << ": " << std::fixed << std::setprecision(2) << data.oMi[joint_id].translation().transpose() << std::endl; }	from pathlib import Path from sys import argv import pinocchio # 此路径指向 Pinocchio 源代码，但你可以在此定义自己的目录。 pinocchio_model_dir = Path(__file__).parent.parent / "models" # 你应该在这里更改以设置你自己的 URDF 文件，或者直接将其作为参数传递 # 这个例子。 urdf_filename = ( pinocchio_model_dir / "example-robot-data/robots/ur_description/urdf/ur5_robot.urdf" 如果 len(argv) < 2 否则 argv[1] ) # 加载 urdf 模型 model = pinocchio.buildModelFromUrdf(urdf_filename) print("模型名称: " + model.name) # 创建算法所需的数据 data = model.createData() # 随机采样一个配置 q = pinocchio.randomConfiguration(model) 打印(f"q: {q.T}") # 在运动树上执行正向运动学 pinocchio.forwardKinematics(model, data, q) # 打印出运动树中每个关节的位置 for name, oMi in zip(model.names, data.oMi): print("{:<24} : {: .2f} {: .2f} {: .2f}".format(name, *oMi.translation.T.flat))

overview-urdf.cpp

overview-urdf.py

#include "pinocchio/parsers/urdf.hpp"

#include "pinocchio/algorithm/joint-configuration.hpp"

#include "pinocchio/algorithm/kinematics.hpp"

#include <iostream>

// PINOCCHIO_MODEL_DIR 由 CMake 定义，但你也可以在这里定义你自己的目录。

#ifndef PINOCCHIO_MODEL_DIR

#define PINOCCHIO_MODEL_DIR "模型目录路径"

#endif

int main(int argc, char ** argv)

{

using namespace pinocchio;

// 你应该在这里更改以设置你自己的 URDF 文件，或者直接将其作为此函数的参数传递

// example.

const std::string urdf_filename =

(argc <= 1) ? PINOCCHIO_MODEL_DIR

+ std::string("/example-robot-data/robots/ur_description/urdf/ur5_robot.urdf")

: argv[1];

// 加载 urdf 模型

Model model;

pinocchio::urdf::buildModel(urdf_filename, model);

std::cout << "模型名称: " << model.name << std::endl;

// 创建算法所需的数据

数据数据(模型);

// 随机采样一个配置

Eigen::VectorXd q = randomConfiguration(model);

std::cout << "q: " << q.transpose() << std::endl;

// 在运动树上执行正向运动学

forwardKinematics(model, data, q);

// 打印出运动树每个关节的位置

for (JointIndex joint_id = 0; joint_id < (JointIndex)model.njoints; ++joint_id)

std::cout << std::setw(24) << std::left << model.names[joint_id] << ": " << std::fixed

<< std::setprecision(2) << data.oMi[joint_id].translation().transpose() << std::endl;

}

from pathlib import Path

from sys import argv

import pinocchio

# 此路径指向 Pinocchio 源代码，但你可以在此定义自己的目录。

pinocchio_model_dir = Path(__file__).parent.parent / "models"

# 你应该在这里更改以设置你自己的 URDF 文件，或者直接将其作为参数传递

# 这个例子。

urdf_filename = (

pinocchio_model_dir / "example-robot-data/robots/ur_description/urdf/ur5_robot.urdf"

如果 len(argv) < 2

否则 argv[1]

)

# 加载 urdf 模型

model = pinocchio.buildModelFromUrdf(urdf_filename)

print("模型名称: " + model.name)

# 创建算法所需的数据

data = model.createData()

# 随机采样一个配置

q = pinocchio.randomConfiguration(model)

打印(f"q: {q.T}")

# 在运动树上执行正向运动学

pinocchio.forwardKinematics(model, data, q)

# 打印出运动树中每个关节的位置

for name, oMi in zip(model.names, data.oMi):

print("{:<24} : {: .2f} {: .2f} {: .2f}".format(name, *oMi.translation.T.flat))

编译并运行你的程序

与前面的例子类似，你只需执行：

g++ -std=c++11 overview-urdf.cpp -o overview-urdf $(pkg-config --cflags --libs pinocchio)

该程序通常使用 UR5 URDF 描述运行，例如可以在该仓库中找到：https://github.com/humanoid-path-planner/ur_description

现在你可以启动程序：

./overview-urdf /path/to/ur5.urdf

在 Python 中，直接运行它：

python overview-urdf.py /path/to/ur5.urdf

程序说明

该程序从 URDF 文件加载机械臂机器人模型，计算随机初始配置下的正向运动学，并显示每个机器人关节的位置及其名称。

在 C++中，我们需要模型的头文件、urdf 解析器、随机配置和正运动学算法。在 Python 中，仅需 Pinocchio 模块。

模型通过解析 urdf 模型构建：URDF 文件应直接提供给解析器。这里我们（暂时）不解析几何体，即物体的形状，而仅解析带有名称、长度和惯性属性的运动树。然后，像前一个示例一样，从 Model 对象构建 Data 对象。接下来，我们计算一个随机配置并打印它。

正向运动学通过提供模型、相关数据和机器人配置来简单计算。然后，循环遍历运动学树（从根到叶），并显示每个关节的名称及其在世界坐标系中的位置。对于 6 自由度机械臂 UR5，共有 7 个关节需要显示，其中第一个关节是“universe”，即表达所有内容的参考世界坐标系。

关于 Python 封装

Pinocchio 是用 C++ 编写的，具有完整的基于模板的 C++ API，以提高效率。所有功能都可在 C++ 中使用。库的扩展最好使用 C++。

然而，C++的高效性伴随着更高的工作成本，尤其对新手而言。因此，所有接口都通过 Python 公开。我们努力使 Python API 尽可能成为 C++接口的镜像。最大的区别在于，C++接口使用 Eigen 对象表示矩阵和向量，而在 Python 中这些被暴露为 NumPy 矩阵。

在使用 Pinocchio 时，我们通常建议先用 Python 对你的想法进行原型设计。自动类型和脚本功能使得开发速度大大加快。一旦你对原型感到满意，就可以将其翻译成 C++，同时绑定 API，以便在 Python 中拥有一个镜像，你可以用它来扩展你的想法。

如何引用 Pinocchio

对 Pinocchio 满意吗？请使用以下格式引用我们。

简单解决方案：引用我们的开放获取论文

以下是引用 Pinocchio 的首选方式。该论文可在 HAL（参考号 01866228）公开获取。

@inproceedings{carpentier-sii19,title={Pinocchio C++库——刚体动力学算法及其解析导数的快速灵活实现}作者={J. Carpentier, G. Saurel, G. Buondonno, J. Mirabel, F. Lamiraux, O. Stasse, N. Mansard}booktitle={国际系统集成研讨会 (SII)}  年份={2019}}

引用软件包

此外，如果您想引用该软件包，您可能需要参考以下引用。

@misc{pinocchioweb,作者 = {Justin Carpentier、Joseph Mirabel、Nicolas Mansard 和 Pinocchio 开发团队}title = {Pinocchio：多关节系统的快速正向和逆向动力学}howpublished = {https://stack-of-tasks.github.io/pinocchio},年份 = {2015--2018}}

引用派生算法

Pinocchio 的一大创新在于首次公开了刚体动力学算法的导数。若想引用这些新算法而不明确提及 Pinocchio，可引用

@inproceedings{carpentier-rss18,title={刚体动力学算法的解析导数}作者={Justin Carpentier 和 Nicolas Mansard},booktitle={机器人学：科学与系统（RSS）},  年份={2018}}

实现细节

几个设计选择有助于使 Pinocchio 中的算法实现高效。其中之一是奇异递归模板模式（CRTP）。与 Eigen 类似，Pinocchio 库大量使用了所谓的 CRTP 设计模式。这种模式在静态多态的实现中出于性能考虑，避免了动态类型转换和虚方法的调用。总的来说，CRTP 在 Pinocchio 的性能中扮演了核心角色。

我们参考 https://en.wikipedia.org/wiki/Curiously_recurring_template_pattern 以获取进一步解释。

使 Pinocchio 高效的其他因素包括正确处理矩阵稀疏性、模板化、自动微分和代码生成。查看上面的相关论文以获取更多技术细节。

接下来该何去何从？

本文档主要由多个示例和教程组成，面向新手，同时包含技术文档和参考指南。如果您想确认 Pinocchio 是否满足您的需求，可以先查看功能列表。随后，C++和 Python 中的多个示例将直接为您提供基于刚体库实现最经典应用的关键。对于非专家用户，我们还提供了基于 Featherstone 公式的主要数学基础（如果您从未读过，可能更愿意购买并阅读原书）。如果您不是 Python 专家并希望从 Pinocchio 开始，一个详细的 Python 教程包含了您所需的一切。该教程最初是作为机器人学硕士课程的教材编写的。

以上就是为初学者准备的内容。接着，我们概述了编写该库并使其高效所做出的技术选择。如果您想扩展 C++库核心，特别是如果您不熟悉奇异递归模板模式（例如在 Eigen 中使用），请阅读本节。我们还提供了用于测试库效率的基准测试描述。

顺便说一下，Pinocchio 深受 Eigen 的启发（因此本页的结构和最后一节的标题都与之相关）。我们库的 API 基于 Eigen。如果你对 Eigen 不熟悉，但又想在 C++中使用 Pinocchio，最好先学习一些基本的 Eigen 教程。