Commit 3f30814a authored by Justin Carpentier's avatar Justin Carpentier Committed by GitHub

Merge pull request #521 from nim65s/topic/doc-v2

[Doc] Implementation of doc/outline.txt, fix #514
parents 1e7ca61d de574cce
Pipeline #1512 passed with stage
in 111 minutes and 57 seconds
......@@ -574,4 +574,4 @@ INCLUDE_PATH = @PROJECT_SOURCE_DIR@/src
......@@ -19,47 +19,42 @@
// <>.
## Abstract
This is the documentation of Pinocchio, the Eigen-like library for Rigid Body Dynamics computations.
This library implements highly efficient kinematic and dynamic algorithms for multi-body systems making Pinocchio a versatile framework for robotics applications.
We introduce Pinocchio, an open-source software framework that implements rigid body dynamics algorithms and their
analytical derivatives. Pinocchio does not only include standard algorithms employed in robotics (e.g. forward and
inverse dynamics) but provides additional features essential for the control, the planning and the simulation of
robots. In this paper, we describe these features and detail the programming patterns and design which make Pinocchio
efficient. We also offer a short tutorial for easy handling of the framework.
## Installation
The full installation procedure can be found on the Github Pages of the project:
## Introduction
## Simplest example with compilation command
One of the main features is the separation between models (constant values representing an object) and data (used for intermediate computations).
The two main classes are:
- se3::Model that represents a kinematic chain composed of joints that move links with mass and inertia,
- se3::Data that stores intermediate data for kinematic and dynamic computations.
TODO: load urdf & run rnea
### Spatial Algebra
## More complex example with C++ & Python
TODO: load model and run loop for an IK with 3D task
## About Python wrapings
TODO philosophy and example
## How to cite Pinocchio
TODO: ref
## Presentation of the content of the documentation
The library provides some classes to represent spatial quantities at the root of kinematic and dynamic computations. This main classes are reported below:
- se3::SE3Tpl represents a rigid placement, a mathematical representation of \f$ SE(3) \f$.
- se3::MotionTpl represents a spatial motion (linear and angular velocities), e.g. a spatial velocity or acceleration associated to a frame or a body. The spatial motion is the mathematical representation of \f$ se(3) \f$.
- se3::ForceTpl represents a spatial force, e.g. a spatial impulse or force associated to a body. The spatial force is the mathematical representation of \f$ se^{*}(3) \f$, the dual of \f$ se(3) \f$.
- se3::InertiaTpl represents a spatial inertia characterizing a rigid body and expressed in a given frame. This inertia is composed of mass, the position of the center of mass regarding to the frame and a rotational inertia.
### Main Algorithms
### Collision checking and distance computation
Collision checking between two bodies of the kinematic chain and external
obstacles is implemented using library hpp-fcl (a modified version of
Flexible Collision Library). The computation of the position of objects in 3D
space for a given configuration is performed by the following classes:
- se3::GeometryModel that represents the collision objects associated to a joint stored in a se3::Model,
- se3::GeometryData that stores intermediate data like the position of objects in a given configuration of the kinematic chain.
## Further reading
This documentation starts with a preview of the [Mathematical Formulation of operations in
After this, we explain [how to use](md_doc_b-usage_intro.html) this library in different use cases, and we provide
[quick tutorials](md_doc_c-tutorials_intro.html) for classical tasks.
If you need an in-depth understanding of the topic, we also have [Labs](md_doc_d-labs_intro.html) for you.
# Spatial Algebra module
# Model and data
TODO: just explain the concept of model and data and what they contain. The explanation on how to build and/or load a
model is delayed to the two following pages (explicitely say so from the start). Just use
buildModels::humanoidSimple(model) in the snippets [pending v2.0]
# Joints
TODO: Or: Building the model: joints and bodies. Maybe merge this section with previous one?
@nmansard : I prefer this way of separing models of joint models.
# Loading the model
# Dealing with Lie group geometry
# Kinematic algorithms
# Operational frames
TODO: as frames are not necessary to understand the previous sections, it could be a good idea to treat them
separately. Introduce them here together with the related algorithms.
# Geometric models
TODO: with related algos
# Main features of Pinocchio
Pinocchio has been written in C++ for efficiency reasons and uses the
Eigen library for linear algebra routines. It comes with
Python bindings for easy code prototyping. We describe here the main
features implemented in Pinocchio.
## Spatial algebra
Spatial algebra is a mathematical notation
commonly employed in rigid body dynamics to represent and manipulate
physical quantities such as velocities, accelerations and forces.
Pinocchio is based on this mathematical notation. Dedicated classes are
provided to represent coordinate transformations in the 3D Euclidean
space (named SE3), spatial motion vectors (Motion), spatial force
vectors (Force), and spatial inertias (Inertia). Along with the
available methods, this endows Pinocchio with an efficient software
library for spatial algebra calculations.
## Model and data
A fundamental paradigm of Pinocchio is the strict separation between
*model* and *data*. By *model*, we mean the physical description of the
robot, including kinematic and possibly inertial parameters defining its
structure. This information is held by a dedicated class which, once
created, is never modified by the algorithms of Pinocchio. By *data*, we
mean all values which are the result of a computation. *Data* vary
according to the joint configuration, velocity, etc\... of the system.
It contains for instance the velocity and the acceleration of each link.
It also stores intermediate computations and final results of the
algorithms in order to prevent memory allocation. With this splitting,
all the algorithms in Pinocchio follow the signature:
algorithm(model, data, arg1, arg2, ...)
where are the arguments of the function (e.g. configuration or
velocity). Keeping model and data separated reduces memory footprint
when performing several different tasks on the same robot, notably when
this involves parallel computation. Each process can employ its own data
object, while sharing the same model object. The fact that a model
object never changes within an algorithm of Pinocchio enhances the
predictability of the code.
A model can be created using the C++ API or loaded from an external
file, which can be either URDF, Lua (following the RBDL standard) or
## Supported kinematic models
Within a model, a robot is represented as a kinematic tree, containing a
collection of all the joints, information about their connectivity, and,
optionally, the inertial quantities associated to each link. In
Pinocchio a joint can have one or several degrees of freedom, and it
belongs to one of the following categories:
- **Revolute** joints, rotating around a fixed axis, either one of \f$X,Y,Z\f$ or a custom one;
- **Prismatic** joints, translating along any fixed axis, as in the revolute case;
- **Spherical** joints, free rotations in the 3D space;
- **Translation** joint, for free translations in the 3D space;
- **Planar** joints, for free movements in the 2D space;
- **Free-floating** joints, for free movements in the 3D space. Planar and free-floating joints are meant to be
employed as the basis of kinematic tree of mobile robots (humanoids, automated vehicles, or objects in manipulation
- More complex joints can be created as a collection of ordinary ones through the concept of **Composite** joint.
## Dealing with Lie group geometry
Each type of joints is characterized by its own specific configuration
and tangent spaces. For instance, the configuration and tangent spaces
of a revolute joint are both the real axis line \f$\mathbb{R}\f$, while for
a Spherical joint the configuration space corresponds to the set of
rotation matrices of dimension 3 and its tangent space is the space of
3-dimensional real vectors \f$\mathbb{R}^{3}\f$. Some configuration spaces
might not behave as a vector space, but have to be endowed with the
corresponding integration (exp) and differentiation (log) operators.
Pinocchio implements all these specific integration and differentiation
## Geometric models
Aside the kinematic model, Pinocchio defines a geometric model, i.e. the
volumes attached to the kinematic tree. This model can be used for
displaying the robot and computing quantities associated to collisions.
Like the kinematic model, the fixed quantities (placement and shape of
the volumes) are stored in a *GeometricModel* object, while buffers and
quantities used by associated algorithms are defined in a object. The
volumes are represented using the FCL library . Bodies
of the robot are attached to each joint, while obstacles of the
environment are defined in the world frame. Collision and distance
algorithms for the kinematic trees are implemented, based on FCL
## Main algorithms
The implementation of the basic algorithms, including all those listed
in this section, is recursive. The recursive formulation allows the
software to avoid repeated computations and to exploit the sparsity
induced by the kinematic tree. For the dynamics algorithms, we largely
drew inspiration from Featherstone algorithms, with slight
### Forward kinematics
Pinocchio implements direct kinematic computations up to the second
order. When a robot configuration is given, a forward pass is performed
to compute the spatial placements of each joint and to store them as
coordinate transformations. If the velocity is given, it also computes
the spatial velocities of each joint (expressed in local frame), and
similarly for accelerations.
### Kinematic Jacobian
the spatial Jacobian of each joint can be easily computed with a single
forward pass, either expressed locally or in the world frame.
### Inverse dynamics
the Recursive Newton-Euler Algorithm (RNEA) computes the
inverse dynamics: given a desired robot configuration, velocity and
acceleration, the torques required to execute this motion are computed
and stored. The algorithm first performs a forward pass (equivalent to
second-order kinematics). It then performs a backward pass, computing
the wrenches transmitted along the structure and extracting the joint
torques needed to obtain the computed link motions. With the appropriate
inputs, this algorithm can also be employed to compute specific terms of
the dynamic model, such as the gravity effects.
### Joint space inertia matrix
the Composite Rigid Body Algorithm (CRBA) is
employed to compute the joint space inertia matrix of the robot. We have
implemented some slight modifications of the original algorithm that
improve the computational efficiency.
### Forward dynamics
the Articulated Body Algorithm (ABA)
computes the unconstrained forward dynamics: given a robot
configuration, velocity, torque and external forces, the resulting joint
accelerations are computed.
### Additional algorithms
beside the algorithms above, other methods are provided, most notably
for constrained forward dynamics, impulse dynamics, inverse of the joint
space inertia and centroidal
## Analytical derivatives
Beside proposing standard forward and inverse dynamics algorithms,
Pinocchio also provides efficient implementations of their analytical
derivatives. These derivatives are for
instance of primary importance in the context of whole-body trajectory
optimization or more largely, for numerical optimal control. To the best
of our knowledge, Pinocchio is the first rigid body framework which
implements this feature natively.
## Automatic differentiation and source code generation
In addition to analytical derivatives, Pinocchio supports automatic
differentiation. This is made possible through the full *scalar*
templatization of the whole C++ code and the use of any external library
that does automatic differentiation: ADOL-C, CasADi, CppAD and others. It is
important to keep in mind that these automatic derivatives are often
much slower than the analytical ones.
Another unique but central feature of Pinocchio is its ability to
generate code both at compile time and at runtime. This is achieved by
using another external toolbox called CppADCodeGen built on top of
CppAD. From any function using Pinocchio, CppADCodeGen is
able to generate on the fly its code in various languages: C, Latex,
etc. and to make some simplifications of the math expressions. Thanks to
this procedure, a code tailored for a specific robot model can be
generated and used externally to Pinocchio.
# Automatic differenctiation and source code generation
# Geometry
## Special Euclidian Group
## Direct Geometry
## Inverse Geometry
# Kinematics
## Direct Kinematics
## Inverse Kinematics
# Loading the model
## Python
## C++
\include a-model.cpp
# Computing the dynamics RNEA, bias forces b(q,v), gravity g(q), CRBA M(q)
## Python
## C++
\include b-dynamics.cpp
# Contact dynamics
## Python
## C++
\include c-contact.cpp
# Loading and displaying the model
## Python
## C++
\include d-display.cpp
# Collision detection and distances
## Python
## C++
\include e-collisions.cpp
# Derivatives of the dynamics (with finite diff checking)
## Python
## C++
\include f-derivatives.cpp
# Code generation
## Python
## C++
\include g-code-generation.cpp
# Inverse geometry
## Python
## C++
\include h-inverse-geometry.cpp
# Inverse kinematics (clik)
## Python
## C++
\include i-inverse-kinematics.cpp
# Task space inverse dynamics
## Python
## C++
\include j-tsid.cpp
# QP (normal forces) unilateral contact dynamics (if we can write it concise enough)
## Python
## C++
\include k-qp.cpp
# Posture generation using derivatives (if we can write it concise enough)
## Python
## C++
\include l-postures.cpp
# A RRT planner for a robot arm (if we can write it concise enough)
## Python
## C++
\include m-rrt.cpp
# A RL tensorflow example (if we can write it concise enough)
## Python
## C++
\include n-rl.cpp
# Creating models
// Copyright (c) 2016, 2018 CNRS
// Author: Florent Lamiraux, Justin Carpentier, Florian Valenza, Guilhem Saurel
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
// Pinocchio is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// Pinocchio If not, see
// <>.
Here you can find the differents ways to create a model ( simple models, python/lua/urdf loading, Joint after Joint)
## Introduction
In Pinocchio you can create Models in many ways. We have built-in parsers for different kind of format ( urdf, python, Lua)
but you can also create a Model from scratch and fill it with the Joints you want.
## Supported formats
### Format urdf
To load an urdf file in C++ code, copy the following lines:
#include <pinocchio/multibody/model.hpp>
#include <pinocchio/multibody/parser/urdf.hpp>
bool verbose = false;
const std::string filename = "path/to/file/model.urdf";
se3::Model model
se3::urdf::buildModel (filename, model, verbose);
se3::Data data (model);
### Format Python
To load a python file in C++ code, copy the following lines:
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/parsers/python.hpp"
bool verbose = false;
const std::string filename = "path/to/file/";
const std::string model_name = "python_variable_name_in_filename"
se3::Model model = se3::python::buildModel(filename, model_name, verbose);
se3::Data data (model);
### Format lua
To load an lua file in C++ code, copy the following lines:
#include <pinocchio/multibody/model.hpp>
#include <pinocchio/multibody/parser/lua.hpp>
bool verbose = false;
bool freeflyer = true;
const std::string filename = "path/to/file/model.lua";
se3::Model model = se3::lua::buildModel (filename, freeflyer, verbose);
se3::Data data (model);
# Overview
## Rigid Dynamics
## Joint Dynamics
## Articulated Dynamics
## Algorithms
# Rigid Bodies
## Geometry
## Kinematics
## Dynamics
# Joint dynamics
## Geometry
## Configuration Space
## Kinematics
## Tangent Space
## List of Joints
# Articulated Dynamics
## Kinematic Tree
## Configuration and Velocity
## Geometry
## Kinematics and Jacobian
## Inverse and Direct Dynamics
# Going further
## Templatization
## Auto-differenciation
## Code Generation
# Main algorithm
TODO: list taken from SII
# Direct Geometry
## Load the robot
from pinocchio.robot_wrapper import RobotWrapper
from os.path import join
PKG = '/opt/openrobots/share'
URDF = join(PKG, 'ur5_description/urdf/ur5_gripper.urdf'@
robot = RobotWrapper(URDF, [PKG])
## Visualize the model
launch `gepetto-gui`, and then
## Put the robot in a particular position
import numpy as np
q = np.matrix([-.5, -1, 1.5, -.5, -.5, 0]).T
# Tutorials
These tutorials contains quick exemples showing how Pinocchio can be used.
If you need more detailed exercises, go to [Labs](md_doc_labs_intro)
# Inverse Kinematics
## Get the robot
import pinocchio
from os.path import join
PKG = '/opt/openrobots/share'
URDF = join(PKG, 'ur5_description/urdf/ur5_gripper.urdf')
robot = pinocchio.robot_wrapper.RobotWrapper(URDF, [PKG])
NQ = robot.model.nq
q = pinocchio.utils.rand(NQ)
## Get current and desired position of end effector
import numpy as np
Mtool =[IDX_TOOL]
Tgoal = [.2, -.4, .7]
Qgoal = [.4, .02, -.5, .7]
Qgoal = pinocchio.Quaternion(*Qgoal)
Mgoal = pinocchio.SE3(Qgoal.matrix(), np.matrix(Tgoal).T)
## Show goal
robot.viewer.gui.addXYZaxis('world/goal', [1, 0, 0, 1], .015, .2)
Vgoal = pinocchio.utils.XYZQUATToViewerConfiguration(pinocchio.utils.se3ToXYZQUAT(Mgoal))
robot.viewer.gui.applyConfiguration('world/goal', Vgoal)
## Reach it
import time
from numpy.linalg import norm, pinv
DT = .001
nu = + 1
while norm(nu) > .01:
pinocchio.forwardKinematics(robot.model,, q)
Mtool =[IDX_TOOL]
nu = pinocchio.log(Mtool.inverse() * Mgoal).vector
J = pinocchio.frameJacobian(robot.model,, IDX_TOOL, q)
vq = pinv(J) * nu
robot.increment(q, vq * DT)
# Labs
# Practical Exercises
This section contains complete exercices to understand Pinocchio.