什么是 Pinocchio？

Pinocchio 是一个用于高效计算机器人模型动力学及其导数的库，也可用于任何你想要处理的多刚体系统，例如仿真器中的角色、用于生物力学的骨骼模型等。 Pinocchio 是计算多刚体动力学最有效的库之一。它实现了 Featherstone 2008 年著作中描述的经典算法 book （感谢他做出的贡献）。它还在这些算法之上提供了高效变体和一些新算法，尤其包含一整套用于计算核心算法导数的实现。

Pinocchio 是开源项目，主体以 C++ 编写并提供 Python 绑定，遵循 BSD 许可证发布。欢迎贡献代码和改进。

在本文档中，你会看到库功能的常规介绍、帮助理解实现背后数学原理的快速教程、一些关于如何实现经典应用的示例（逆运动学、接触动力学、碰撞检测等），以及一组面向初学者的实践练习。

如何安装 Pinocchio？

Pinocchio 最推荐通过我们的仓库在 Ubuntu 14.04、16.04 和 18.04 上的 APT 包安装。在 Mac OS X 上，我们支持通过 Homebrew 安装 Pinocchio。如果你只需要 Python 绑定，可以直接通过 Conda 使用它们。对于没有提供二进制包的系统，从源码安装应该也比较直接。每个版本都会在主流 Linux 发行版和 Mac OS X 上进行验证。

完整安装流程可以在项目的 Github Pages 上找到： http://stack-of-tasks.github.io/pinocchio/download.html。

最简单的示例与编译命令

我们先从一个计算机器人逆动力学的简单程序开始。下面同时给出了 C++ 和 Python 版本。

overview-simple.cpp

overview-simple.py

 
#include <Eigen/Core>
 
#include <boost/core/ref.hpp>
#include <boost/fusion/algorithm.hpp>
#include <boost/fusion/functional.hpp>
#include <boost/variant.hpp>
 
#include <iostream>
 
#include "pinocchio/algorithm/check-data.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/algorithm/rnea.hpp"
#include "pinocchio/eigen-common.hpp"
#include "pinocchio/math.hpp"
#include "pinocchio/multibody.hpp"
#include "pinocchio/multibody/joint.hpp"
#include "pinocchio/multibody/sample-models.hpp"
#include "pinocchio/spatial.hpp"
#include "pinocchio/utils/check.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)

编译并运行程序

你可以使用下面这个最小的 CMakeLists.txt 来编译 C++ 程序：

cmake_minimum_required(VERSION 3.22)
 
project(overview CXX)
 
find_package(pinocchio REQUIRED)
 
add_executable(overview-simple overview-simple.cpp)
target_link_libraries(overview-simple PUBLIC pinocchio::pinocchio)

然后运行下面的 CMake 命令：

cmake -B build

cmake --build build

编译完成后，可以这样运行它：

./build/overview-simple

在 Python 中，直接运行即可：

python overview-simple.py

程序说明

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

我们首先包含正确的头文件。C++ 目前还没有统一整理 include 路径，因此这里需要包含示例模型头文件（其中定义了测试所用模型的参数），以及定义机器人零位姿和 RNEA 函数的头文件。在 Python 中，只需直接导入 pinocchio 即可。

第一段定义模型。在 C++ 中，我们先定义对象，再把它赋值为样例模型（一个简单的内置机械臂，非常适合小测试）。在 Python 中，模型只需通过一个命令创建。模型类包含机器人的常量参数（质量、连杆长度、物体名称等），也就是不会被算法修改的参数。由于大多数算法都需要额外的内部存储空间，我们还必须为模型分配一个专用的 Data 结构。Pinocchio 严格区分 Model 中的常量参数和 Data 中的计算缓冲区。

逆动力学会根据关节构型、速度和加速度计算跟踪轨迹所需的力矩。第二段中，我们定义这三个量。速度和加速度都是普通向量，可以初始化为任意值。它们的维度是 model.nv，对应机器人瞬时自由度的数量。构型可能比这更复杂，例如它可能包含四元数。当前示例中使用的是简单机械臂，因此不会遇到这种情况，但对于更复杂的模型，比如人形机器人，可能会带来麻烦。因此，库提供了若干辅助函数，用于将构型初始化为零值（零位姿）或随机值，或者用于保证构型有效（归一化）。

最后，我们调用 rnea 函数，传入机器人模型、对应的数据，以及当前的构型、速度和加速度。返回结果是一个维度为 model.nv 的向量。它也会存储在 data.tau 中，以便后续使用。对于机械臂而言，这个值对应于实现给定运动所需的关节力矩。我们这里只是把它打印出来。注意，在 Python 中，我们使用 numpy 来表示矩阵和向量。因此 tau 是一个 numpy 矩阵对象，q、v 和 a 也都是如此。

在继续之前给一个小提示：在示例中，为了清晰起见，我们通常直接使用完整命名空间 pinocchio。不过如果你觉得 "pinocchio" 输入太长，推荐的简写方式是

namespace pin = pinocchio;

pinocchio

Main pinocchio namespace.

Definition: treeview.dox:11

in C++ and

import pinocchio as pin

in Python.

更复杂的 C++ 与 Python 示例

overview-urdf.cpp

overview-urdf.py

#include <iostream>
 
#include <Eigen/Core>
 
#include <boost/core/ref.hpp>
#include <boost/fusion/algorithm.hpp>
#include <boost/fusion/functional.hpp>
#include <boost/variant.hpp>
 
#include <iostream>
#include <string>
#include <vector>
 
#include "pinocchio/algorithm/fwd.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/algorithm/kinematics.hpp"
#include "pinocchio/eigen-common.hpp"
#include "pinocchio/math.hpp"
#include "pinocchio/multibody.hpp"
#include "pinocchio/multibody/joint.hpp"
#include "pinocchio/parsers/urdf.hpp"
#include "pinocchio/spatial.hpp"
#include "pinocchio/utils/check.hpp"
 
// EXAMPLE_ROBOT_DATA_MODEL_DIR is defined by the CMake but you can define your own directory here.
#ifndef EXAMPLE_ROBOT_DATA_MODEL_DIR
  #define EXAMPLE_ROBOT_DATA_MODEL_DIR "path_to_the_model_dir"
#endif
 
int main(int argc, char ** argv)
{
  using namespace pinocchio;
 
  // You should change here to set up your own URDF file or just pass it as an argument of this
  // example.
  const std::string urdf_filename =
    (argc <= 1) ? EXAMPLE_ROBOT_DATA_MODEL_DIR + std::string("/ur_description/urdf/ur5_robot.urdf")
                : argv[1];
 
  // Load the urdf model
  Model model;
  pinocchio::urdf::buildModel(urdf_filename, model);
  std::cout << "model name: " << model.name << std::endl;
 
  // Create data required by the algorithms
  Data data(model);
 
  // Sample a random configuration
  Eigen::VectorXd q = randomConfiguration(model);
  std::cout << "q: " << q.transpose() << std::endl;
 
  // Perform the forward kinematics over the kinematic tree
  forwardKinematics(model, data, q);
 
  // Print out the placement of each joint of the kinematic tree
  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;
}

import os
from pathlib import Path
from sys import argv
 
import pinocchio
 
# This path refers to Pinocchio source code but you can define your own directory here.
model_dir = Path(os.environ.get("EXAMPLE_ROBOT_DATA_MODEL_DIR"))
 
# You should change here to set up your own URDF file or just pass it as an argument of
# this example.
urdf_filename = (
    model_dir / "ur_description/urdf/ur5_robot.urdf" if len(argv) < 2 else argv[1]
)
 
# Load the urdf model
model = pinocchio.buildModelFromUrdf(urdf_filename)
print("model name: " + model.name)
 
# Create data required by the algorithms
data = model.createData()
 
# Sample a random configuration
q = pinocchio.randomConfiguration(model)
print(f"q: {q.T}")
 
# Perform the forward kinematics over the kinematic tree
pinocchio.forwardKinematics(model, data, q)
 
# Print out the placement of each joint of the kinematic tree
for name, oMi in zip(model.names, data.oMi):
    print("{:<24} : {: .2f} {: .2f} {: .2f}".format(name, *oMi.translation.T.flat))

编译并运行程序

和前一个示例类似，你只需要：

cmake_minimum_required(VERSION 3.22)
 
project(overview-urdf CXX)
 
find_package(pinocchio REQUIRED)
 
add_executable(overview-urdf overview-urdf.cpp)
target_link_libraries(overview-urdf PUBLIC pinocchio::pinocchio)

Then run the following CMake command:

cmake -B build

cmake --build build

这个程序通常配合 UR5 的 URDF 描述一起运行，你可以在例如这个仓库中找到它：https://github.com/humanoid-path-planner/ur_description

现在可以启动程序：

./build/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++，并通过绑定保持一份可在 Python 中继续扩展的镜像 API。

如何引用 Pinocchio

如果你喜欢 Pinocchio，请按以下格式引用我们。

简单方式：引用我们的开放获取论文

这是引用 Pinocchio 的推荐方式。论文可在 HAL 上公开获取 (ref 01866228).

@inproceedings{carpentier-sii19,
  title={The {P}inocchio {C}++ library -- {A} fast and flexible implementation of rigid body dynamics algorithms and their analytical derivatives},
  author={J. Carpentier and G. Saurel and G. Buondonno and J. Mirabel and F. Lamiraux and O. Stasse and N. Mansard},
  booktitle={International Symposium on System Integration (SII)}
  year={2019}
}

引用软件包

另外，如果你想引用软件包本身，可以使用下面的引用格式。

@misc{pinocchioweb,
  author = {Justin Carpentier and Joseph Mirabel and Nicolas Mansard and the Pinocchio Development Team},
  title = {Pinocchio: fast forward and inverse dynamics for poly-articulated systems},
  howpublished = {https://stack-of-tasks.github.io/pinocchio},
  year = {2015--2018}
}

引用导数算法

Pinocchio 的一大创新在于，它首次公开提供了刚体动力学算法的导数实现。如果你想引用这些新算法，但不想明确提到 Pinocchio，可以引用

@inproceedings{carpentier-rss18,
  title={Analytical derivatives of rigid body dynamics algorithms},
  author={Justin Carpentier and Nicolas Mansard},
  booktitle={Robotics: Science and Systems (RSS)},
  year={2018}
}

实现细节

Pinocchio 的算法实现之所以高效，得益于多项设计选择。其中之一就是 CRTP（Curiously Recurring Template Pattern，递归模板模式）。和 Eigen 类似，Pinocchio 大量使用所谓的 CRTP 设计模式。这样做是出于性能考虑，用于实现静态多态，避免动态转换和虚函数调用。总的来说，CRTP 在 Pinocchio 的性能中起着核心作用。

更多说明可参考：https://en.wikipedia.org/wiki/Curiously_recurring_template_pattern。

其他提升性能的因素还包括对矩阵稀疏性的合理处理、模板化、自动微分和代码生成。更详细的技术信息请参考上面的相关论文。

接下来去哪里？

本文档主要由面向新手的若干示例和教程，以及技术文档和参考手册组成。如果你想确认 Pinocchio 是否符合你的需求，建议先查看功能列表。随后，若干 C++ 和 Python 示例会直接告诉你如何实现基于刚体库的经典应用。对于非专业读者，我们还提供了基于 Featherstone 表述的主要数学基础（如果你从未读过原著，可能更适合买并阅读那本书）。如果你不是 Python 专家，但希望开始使用 Pinocchio，还有一份较长的 Python 教程，包含你所需要的一切。这份教程最初是为机器人学硕士课程准备的教学材料。

初学者到这里就可以了。接下来我们会概述编写这个库并使其高效的技术选择。如果你想扩展 C++ 库核心，尤其是不熟悉递归模板模式（例如 Eigen 中也在使用），建议阅读这一节。文档还提供了我们用于测试库性能的基准说明。

顺带一提，Pinocchio 深受 Eigen 的启发，因此本页的结构以及最后一节的标题都带有这种影子。我们的库 API 也是基于 Eigen 的。如果你对 Eigen 不熟悉，但想在 C++ 中使用 Pinocchio，最好先阅读 Eigen 的基础教程。

	pinocchio 4.0.0 A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives