 pinocchio  2.6.8 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

title={Analytical derivatives of rigid body dynamics algorithms},
author={Justin Carpentier and Nicolas Mansard},
year={2018}
}

# Where to go from here?

This documentation is mostly composed of several examples and tutorials for newcomers, along with a technical documentation and a reference guide. If you want to make sure Pinocchio matches your needs, you may first want to check the list of features. Several examples in C++ and Python will then directly give you the keys to implement the most classical applications based on a rigid-body library. For nonexperts, we also provide the main mathematical fundamentals, based on Featherstone's formulations (you may prefer to buy and read the original book if you never did). A long tutorial in Python contains everything you need if you are not a Python expert and want to start with Pinocchio. This tutorial was first written as course material for a Master class about robotics.

That's it for beginners. We then give an overview of the technical choices we made to write the library and make it efficient. Read this section if you want to extend the C++ library core, in particular if you are not familiar with the curiously-recursive template pattern (used for example in Eigen). A description of the benchmarks we did to test the library efficiency is also provided.

By the way, Pinocchio has been deeply inspired by Eigen (hence the structure of this page and the title of this last section). The API of our library is Eigen-based. If you are not comfortable with Eigen and want to use Pinocchio in C++, you better have to follow the basic Eigen tutorials first.

pinocchio::DataTpl
Definition: data.hpp:29
pinocchio::forwardKinematics
void forwardKinematics(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q)
Update the joint placements according to the current joint configuration.
pinocchio::randomConfiguration
void randomConfiguration(const LieGroupGenericTpl< LieGroupCollection > &lg, const Eigen::MatrixBase< ConfigL_t > &q0, const Eigen::MatrixBase< ConfigR_t > &q1, const Eigen::MatrixBase< ConfigOut_t > &qout)
pinocchio::forwardKinematics
void forwardKinematics(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType1 > &v, const Eigen::MatrixBase< TangentVectorType2 > &a)
Update the joint placements, spatial velocities and spatial accelerations according to the current jo...
pinocchio::buildModels::manipulator
void manipulator(ModelTpl< Scalar, Options, JointCollectionTpl > &model)
Create a 6-DOF kinematic chain shoulder-elbow-wrist.
pinocchio::randomConfiguration
void randomConfiguration(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorIn1 > &lowerLimits, const Eigen::MatrixBase< ConfigVectorIn2 > &upperLimits, const Eigen::MatrixBase< ReturnType > &qout)
Generate a configuration vector uniformly sampled among provided limits.
Definition: joint-configuration.hpp:224
pinocchio::neutral
Eigen::Matrix< typename LieGroupCollection::Scalar, Eigen::Dynamic, 1, LieGroupCollection::Options > neutral(const LieGroupGenericTpl< LieGroupCollection > &lg)
Visit a LieGroupVariant to get the neutral element of it.
pinocchio::rnea
void rnea(const int num_threads, ModelPoolTpl< Scalar, Options, JointCollectionTpl > &pool, const Eigen::MatrixBase< ConfigVectorPool > &q, const Eigen::MatrixBase< TangentVectorPool1 > &v, const Eigen::MatrixBase< TangentVectorPool2 > &a, const Eigen::MatrixBase< TangentVectorPool3 > &tau)
The Recursive Newton-Euler algorithm. It computes the inverse dynamics, aka the joint torques accordi...
Definition: rnea.hpp:32
pinocchio::urdf::buildModel
ModelTpl< Scalar, Options, JointCollectionTpl > & buildModel(const std::string &filename, const typename ModelTpl< Scalar, Options, JointCollectionTpl >::JointModel &rootJoint, ModelTpl< Scalar, Options, JointCollectionTpl > &model, const bool verbose=false)
Build the model from a URDF file with a particular joint as root of the model tree inside the model g...
pinocchio::rnea
const DataTpl< Scalar, Options, JointCollectionTpl >::TangentVectorType & rnea(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType1 > &v, const Eigen::MatrixBase< TangentVectorType2 > &a, const container::aligned_vector< ForceDerived > &fext)
The Recursive Newton-Euler algorithm. It computes the inverse dynamics, aka the joint torques accordi...
pinocchio::neutral
void neutral(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ReturnType > &qout)
Return the neutral configuration element related to the model configuration space.
Definition: joint-configuration.hpp:257
pinocchio::ModelTpl
Definition: fwd.hpp:23
pinocchio
Main pinocchio namespace.
Definition: treeview.dox:11