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

# What is Pinocchio?

Pinocchio is a library for efficiently computing the dynamics (and derivatives) of a robot model, or of any articulated rigid-body model you would like (avatars in a simulator, skeletal models for biomechanics, etc.). Pinocchio is one of the most efficient libraries for computing the dynamics of articulated bodies. It implements the classical algorithms following the methods described in Featherstone's 2008 book (many thanks to him). It also introduces efficient variations of some of them, plus some new ones, notably including a full set of algorithms to compute the derivatives of the main ones.

Pinocchio is open-source, mostly written in C++ with Python bindings, and distributed under the BSD licence. Contributions are welcome.

In this doc, you will find the usual description of the library functionalities, a quick tutorial to catch over the mathematics behind the implementation, a bunch of examples about how to implement classical applications (inverse kinematics, contact dynamics, collision detection, etc) and a set of practical exercices for beginners.

# How to install Pinocchio?

Pinocchio is best installed from APT packaging on Ubuntu 14.04, 16.04 and 18.04, from our repository. On Mac OS X, we support the installation of Pinocchio through the Homebrew package manager. If you just need the Python bindings, you can directly have access to them through Conda. On systems for which binaries are not provided, installation from source should be straightforward. Every release is validated in the main Linux distributions and Mac OS X.

# Simplest example with compilation command

We start with a simple program to compute the robot inverse dynamics. It is given in both C++ and Python version.

overview-simple.cpp overview-simple.py
#include "pinocchio/parsers/sample-models.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/algorithm/rnea.hpp"
int main()
{
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;
}
from __future__ import print_function
import pinocchio
model = pinocchio.buildSampleModelManipulator()
data = model.createData()
v = pinocchio.utils.zero(model.nv)
a = pinocchio.utils.zero(model.nv)
tau = pinocchio.rnea(model,data,q,v,a)
print('tau = ', tau.T)

## Compiling and running your program

You can compile the C++ version by including Pinocchio and Eigen header directories.

g++ -std=c++11 overview-simple.cpp -o overview-simple $(pkg-config --cflags --libs pinocchio) Once your code is compiled, you might then run it using ./overview-simple In Python, just run it: python overview-simple.py ## Explanation of the program This program loads a robot model, creates a data structure for algorithm buffering, and runs the Recursive Newton-Euler Algorithm (RNEA) to compute the robot inverse dynamics. We first include the proper files. In C++, there is not yet a rationalization of the include files. Here we have to include the sample model (where the parameters of the model we are using in the test are defined), and the header files defining the robot neutral position and the RNEA function. In Python, the library is easier to include by just importing pinocchio. The first paragraph defines the model. In C++, we first define the object, then allocate it to be our sample model (a simple built-in manipulator, very useful for small tests). In Python, the model is created by calling a single command. The model class contains the constant parameters of the robot (masses, segment length, body names, etc), i.e. the parameters that are not to be modified by the algorithms. As most algorithms need extra room for storage of internal values, we also have to allocate a Data structure, dedicated to the model. Pinocchio relies on a strict separation between constant parameters, in Model, and computation buffer, in Data. Inverse dynamics computes the needed torque to track a trajectory defined by the joint configuration, velocity and acceleration. In the second paragraph, we define these three quantities. Velocity and acceleration are plain vectors, and can be initialized to any value. Their dimension is model.nv, corresponding to the number of instantaneous degrees of freedom of the robot. The configuration might be more complex than this (e.g. it can contain quaternions). This is not the case in the current example, where we have a simple manipulator, but it might cause difficulties for more complicated models, such as humanoids. Consequently, helper functions are provided to initialize a configuration to zero (neutral) or to a random value, or to make sure a configuration is valid (normalize). Finally, we call the rnea function, by providing the robot model, the corresponding data, and the current configuration, velocity and acceleration. The result is a vector of dimension model.nv. It is also stored in data.tau, in case it's needed. For a manipulator, this value corresponds to the joint torque required to achieve the given motion. We just print it. Notice that, in Python, we use numpy to represent matrices and vectors. Therefore tau is numpy matrix object, and so are q, v and a. Before we move on, a little tip: in our examples, we generaly include the whole namespace pinocchio for clarity. However, if you feel like "pinocchio" is too long to type, the recommended shorthands are namespace pin = pinocchio; in C++ and import pinocchio as pin in Python. # More complex example with C++ and 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 is defined by the CMake but you can define your own directory here. #ifndef PINOCCHIO_MODEL_DIR #define PINOCCHIO_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) ? PINOCCHIO_MODEL_DIR + std::string("/example-robot-data/robots/ur_description/urdf/ur5_robot.urdf") : argv; // 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 pinocchio from sys import argv from os.path import dirname, join, abspath # This path refers to Pinocchio source code but you can define your own directory here. pinocchio_model_dir = join(dirname(dirname(str(abspath(__file__)))), "models") # You should change here to set up your own URDF file or just pass it as an argument of this example. urdf_filename = pinocchio_model_dir + '/example-robot-data/robots/ur_description/urdf/ur5_robot.urdf' if len(argv)<2 else argv # 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 print('q: %s' % q.T) # Perform the forward kinematics over the kinematic tree # 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 ))) ## Compiling and running your program In similar way than the previous example, you simple need to do: g++ -std=c++11 overview-urdf.cpp -o overview-urdf$(pkg-config --cflags --libs pinocchio)

The program typically runs with a UR5 URDF description, that can be found for example in this repository https://github.com/humanoid-path-planner/ur_description

Now you can launch the program:

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

In Python, just run it:

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

## Explanation of the program

This program loads a model of a manipulator robot from a URDF file, computes the forward kinematics at a random initial configuration and displays the position of each robot joint with its name.

In C++, we need the headers of the model, the urdf parser, the random configuration and the forward-kinematics algorithms. In Python, only the Pinocchio module is needed.

The model is built by parsing the urdf model: the URDF file should be directly provided to the parser. Here we do not parse the geometries (yet), i.e. the shape of the bodies, but only the kinematic tree with names, lengths, and inertial properties. The Data object is then built from the Model object, like in the previous example. Next, we compute a random configuration and we print it.

The forward kinematics is simply computed by providing the model, the associated data and the robot configuration. Then, a loop goes through the kinematic tree (from root to leaves) and displays for each joint its name and position in the world frame. For the 6-DOF manipulator UR5, there are 7 joints to be displayed, with the first joint being the "universe", i.e. the reference world frame from which everything is expressed.

Pinocchio is written in C++, with a full template-based C++ API, for efficiency purposes. All the functionalities are available in C++. Extension of the library should be preferably in C++.

However, C++ efficiency comes with a higher work cost, especially for newcomers. For this reason, all the interface is exposed in Python. We tried to build the Python API as much as possible as a mirror of the C++ interface. The greatest difference is that the C++ interface is proposed using Eigen objects for matrices and vectors, that are exposed as NumPy matrices in Python.

When working with Pinocchio, we often suggest to first prototype your ideas in Python. Both the auto-typing and the scripting make it much faster to develop. Once you are happy with your prototype, then translate it in C++ while binding the API to have a mirror in Python that you can use to extend your idea.

# How to cite Pinocchio

Happy with Pinocchio? Please cite us with the following format.

### Easy solution: cite our open access paper

The following is the preferred way to cite Pinocchio. The paper is publicly available in 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}
}

### Citing the software package

Additionally, if you want to cite the software package, you might want to resort to the following citation.

@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},
year = {2015--2018}
}

### Citing the derivative algorithms

A great novelty of Pinocchio is that the derivatives of rigid body dynamics algorithms are made publicly available for the first time. If you want to refer to the new algorithms, without explicitley mentioning Pinocchio, you can cite