TechOverflow Logo
TechOverflow

Pinocchio: How to compute Coriolis Matrix in C++

The following example computes the DOFxDOF coriolis matrix for a given robot configuration based on an URDF.

coriolis.cpp

You possibly need to change the URDF path just after main()

#include <iostream>
#include <pinocchio/parsers/urdf.hpp>
#include <pinocchio/multibody/data.hpp>
#include <pinocchio/multibody/model.hpp>
#include <pinocchio/algorithm/kinematics.hpp>
#include <pinocchio/algorithm/rnea.hpp>
#include <pinocchio/algorithm/crba.hpp>

int main() {
    // Load the model from a URDF file
    std::string urdf_path = "fr3.urdf";
    pinocchio::Model model;
    pinocchio::urdf::buildModel(urdf_path, model);
    pinocchio::Data data(model);

    // Print model information
    std::cout << "Model loaded with " << model.nq << " DOFs." << std::endl;

    // Define configuration and velocity vectors
    Eigen::VectorXd q = Eigen::VectorXd::Zero(model.nq);  // Joint configuration
    Eigen::VectorXd v = Eigen::VectorXd::Zero(model.nv);  // Joint velocity

    // Example: Set random joint values (you can replace this with specific values)
    q.setRandom();
    v.setRandom();

    // Compute the Coriolis matrix
    Eigen::MatrixXd C = pinocchio::computeCoriolisMatrix(model, data, q, v);

    // Print the Coriolis matrix
    std::cout << "Coriolis matrix:" << std::endl;
    std::cout << C << std::endl;

    return 0;
}

Makefile

all:
	g++ -o coriolis coriolis.cpp -I/opt/openrobots/include/ -I/usr/include/eigen3/ -lpinocchio_default -lpinocchio_parsers -L/opt/openrobots/lib

Example output

Model loaded with 7 DOFs.
Coriolis matrix:
    0.292825    -0.394254   -0.0339698     0.186623     0.018169 -0.000738816 -0.000608633
    0.404033  -0.00617059     0.236754     0.436426    0.0381023   -0.0720541   0.00183273
    0.150394    -0.189039  -0.00631174     0.132594   0.00888995  0.000474864 -0.000305944
   0.0057268    -0.385487    -0.138959  -0.00364853   0.00921516    0.0268936  -0.00145881
 -0.00686805   -0.0103909   -0.0048069   -0.0152075  -0.00313176    0.0112708 -0.000218298
  -0.0304723    0.0611898  -0.00675759     -0.02542   -0.0113946 -1.14965e-05  0.000108948
  0.00145708  -0.00106328    0.0001119  0.000819975  0.000275052 -0.000105111  2.03288e-17