How to calculate the centroidal composite inertia matrix of a robot

When working on robot control problems, it is often useful to simplify the dynamics in order to perform some sort of optimization. One of the possible simplifications to make is assume that the robot behaves as a single rigid body. Thus the dynamics are described by the total mass at the center of mass (CoM) and the moment of inertia also at the CoM.

Calculating the CoM, total mass and moment of inertia of a robot described with URDFs is not straight forward, even more when you want to describe the moments of inertia with respect to a particular robot state (centroidal composite rigid body inertia).

Since I had to do this task and found little documentation, I’m making a short blog post for someone who struggles with the same problem.

I used the Pinocchio library, which implements some state of the art Rigid Body algorithms. It is written in C++ but has Python bindinds.

Pinocchio uses the concept of model and data. The idea is that when you load a robot, it creates a model. This model contains generic information only, such as how many links and joints the robot has. Then you create the data for each robot you want to spawn, that contains current joint positions and velocities for example. This implementation makes Pinocchio very efficient.

You can find the installation instructions here.

In this post we will write a simple Python script that read a URDF and outputs its equivalent inertia matrix. The first step is to obviously import the package:

from __future__ import print_function    # I am using Python2 on Ubuntu 16.04

import pinocchio
import os

Next, we need to load the URDF into a model and create the data for a single robot:

urdf_filename = 'your_path/robot.urdf'
model = pinocchio.buildModelFromUrdf(urdf_filename, pinocchio.JointModelFreeFlyer())
data = model.createData()
print('model name: ' + model.name)

The pinocchio.JointModelFreeFlyer() parameter is there to tell Pinocchio that our robot is not fixed, thus the base link has to be also taken into account.

Next, we need to define the position of every movable joint. In this case, we assume that we want our joints to be in their home position for the calculation.

q = pinocchio.neutral(model)       # Joint positions
v = pinocchio.utils.zero(model.nv) # Joint velocities

The last step is to execute the algorithm that calculates the Centroidal Composite Rigid Body Inertia. We will use the Composite Rigid Body Algorithm (CRBA), which calculates among other things the Centroidal Momentum. You can see all the functionality of Pinocchio’s bindings with help(pinocchio).

pinocchio.ccrba(model, data, q, v)
print(data.Ig)

This will print the robot’s total mass and centroidal momentum matrix. If you want to calculate the CoM, you can do so with:

pinocchio.centerOfMass(model, data, q)

Edit: changed every ‘joint space inertia matrix’ occurence with ‘centroidal composite rigid body inertia’ since I conflated the two.