Pinocchio: Cómo calcular la matriz de Coriolis en C++

El siguiente ejemplo calcula la matriz de coriolis DOFxDOF para una configuración de robot dada basada en un URDF.

`coriolis.cpp`

Posiblemente necesites cambiar la ruta del URDF justo después de main()

coriolis.cpp

#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() {
    // Cargar el modelo desde un archivo URDF
    std::string urdf_path = "fr3.urdf";
    pinocchio::Model model;
    pinocchio::urdf::buildModel(urdf_path, model);
    pinocchio::Fecha data(model);

    // Imprimir información del modelo
    std::cout << "Model loaded with " << model.nq << " DOFs." << std::endl;

    // Definir vectores de configuración y velocidad
    Eigen::VectorXd q = Eigen::VectorXd::Zero(model.nq);  // Joint configuration
    Eigen::VectorXd v = Eigen::VectorXd::Zero(model.nv);  // Joint velocity

    // Ejemplo: Establecer valores aleatorios de las articulaciones (puedes reemplazar esto con valores específicos)
    q.setRandom();
    v.setRandom();

    // Calcular la matriz de Coriolis
    Eigen::MatrixXd C = pinocchio::computeCoriolisMatrix(model, data, q, v);

    // Imprimir la matriz de Coriolis
    std::cout << "Coriolis matrix:" << std::endl;
    std::cout << C << std::endl;

    return 0;
}

`Makefile`

Makefile

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

Ejemplo de salida

coriolis_output.txt

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