TechOverflow Logo
TechOverflow

How to use libfranka without FCI: Compute mass matrix example

The following example uses libfranka to compute the mass matrix of a Franka Emika Panda robot without using the Franka Control Interface (FCI).

In this example, we will initialize a RobotModel instance from a URDF directly.

This example has fr3.urdf hard-coded, see How to create URDF models for Franka robots for details on how to generate it.

#include <array>
#include <fstream>
#include <string>
#include <iostream>
#include <sstream>

#include "franka/robot_model.h"

### `franka_mass_matrix.cpp`

int main() {
    // Read "fr3.urdf" into a string
    std::ifstream urdf_file("/home/uli/dev/Versatile/SFunctionTesting/fr3.urdf");
    std::stringstream urdf_buffer;
    urdf_buffer << urdf_file.rdbuf();
    std::string urdf_string = urdf_buffer.str();

    // Initialize inputs
    std::array<double, 7> q = {0, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6};
    std::array<double, 9> eeInertia = {0};
    double eeMass = 0.0;
    std::array<double, 3> eeInertialOrigin = {0};

    // Initialize outputs
    std::array<double, 49> massMatrix = {0};

    // Compute mass matrix from joint position vecftor
    franka::RobotModel model(urdf_string);
    model.mass(q, eeInertia, eeMass, eeInertialOrigin, massMatrix);

    // Print mass matrix
    for (int i = 0; i < 7; i++) {
        for (int j = 0; j < 7; j++) {
            std::cout << massMatrix[i * 7 + j] << " ";
        }
        std::cout << std::endl;
    }

}

How to compile

g++ -o franka_mass_matrix franka_mass_matrix.cpp -lfranka -I/opt/openrobots/include -I/usr/include/eigen3 -lpinocchio_default -lpinocchio_parsers -L/opt/openrobots/lib

How to run

./franka_mass_matrix

Example output

0.143555 -0.051297 0.104378 0.0184172 0.0378107 0.00118233 -4.59035e-05 
-0.051297 2.63713 -0.0549108 -1.11478 -0.0981895 -0.0508712 -0.00307348 
0.104378 -0.0549108 0.123903 0.0410319 0.0288785 0.00424617 0.000121731 
0.0184172 -1.11478 0.0410319 0.706659 0.0392072 0.020443 0.00240753 
0.0378107 -0.0981895 0.0288785 0.0392072 0.0484374 -0.00102917 -6.08268e-05 
0.00118233 -0.0508712 0.00424617 0.020443 -0.00102917 0.0306795 0.000195995 
-4.59035e-05 -0.00307348 0.000121731 0.00240753 -6.08268e-05 0.000195995 0.000119426