codegen-crba.cpp

Go to the documentation of this file.

//
// Copyright (c) 2020 INRIA
//
 
#include "pinocchio/codegen/cppadcg.hpp" // this file should be included first before all the others!
#include "pinocchio/algorithm/crba.hpp"
 
#include "pinocchio/parsers/urdf.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/codegen/code-generator-algo.hpp"
 
#include <iostream>
 
int main(int argc, const char ** argv)
{
  using namespace pinocchio;
  using namespace Eigen;
 
  std::string filename =
    PINOCCHIO_MODEL_DIR
    + std::string("/example-robot-data/robots/ur_description/urdf/ur5_robot.urdf");
  if (argc > 1)
    filename = argv[1];
 
  std::cout << "Opening file: " << filename << std::endl;
 
  // Load the model
  Model model;
  pinocchio::urdf::buildModel(filename, model);
 
  CodeGenCRBA<double> crba_code_gen(model);
 
  // Generate the lib if it does not exist and load it afterwards.
  crba_code_gen.initLib();
  crba_code_gen.compileAndLoadLib(PINOCCHIO_CXX_COMPILER);
 
  // Use it with a random configuration samples in the bounds of the joint limits
  VectorXd q = randomConfiguration(model);
  crba_code_gen.evalFunction(q);
 
  // Retrieve the result
  MatrixXd & M = crba_code_gen.M;
 
  // And make it symmetric if needed
  M.template triangularView<Eigen::StrictlyLower>() =
    M.transpose().template triangularView<Eigen::StrictlyLower>();
 
  // You can check the result with the classic CRBA
  Data data_check(model);
  crba(model, data_check, q, Convention::WORLD);
 
  data_check.M.triangularView<Eigen::StrictlyLower>() =
    data_check.M.transpose().triangularView<Eigen::StrictlyLower>();
 
  const MatrixXd & M_check = data_check.M;
  if (M_check.isApprox(M))
  {
    std::cout << "Super! The two results are the same." << std::endl;
    return 0;
  }
  else
  {
    std::cout << "Not Super! The results do not match." << std::endl;
    return -1;
  }
}