codegen-crba.cpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2020 INRIA
3 //
4 
5 #include "pinocchio/codegen/cppadcg.hpp" // this file should be included first before all the others!
6 #include "pinocchio/algorithm/crba.hpp"
7 
8 #include "pinocchio/parsers/urdf.hpp"
9 #include "pinocchio/algorithm/joint-configuration.hpp"
10 #include "pinocchio/codegen/code-generator-algo.hpp"
11 
12 #include <iostream>
13 
14 int main(int argc, const char ** argv)
15 {
16  using namespace pinocchio;
17  using namespace Eigen;
18 
19  std::string filename = PINOCCHIO_MODEL_DIR + std::string("/example-robot-data/robots/ur_description/urdf/ur5_robot.urdf");
20  if(argc>1) filename = argv[1];
21 
22  std::cout << "Opening file: " << filename << std::endl;
23 
24  // Load the model
25  Model model; pinocchio::urdf::buildModel(filename, model);
26 
27  CodeGenCRBA<double> crba_code_gen(model);
28 
29  // Generate the lib if it does not exist and load it afterwards.
30  crba_code_gen.initLib();
31  crba_code_gen.loadLib();
32 
33  // Use it with a random configuration samples in the bounds of the joint limits
35  crba_code_gen.evalFunction(q);
36 
37  // Retrieve the result
38  MatrixXd & M = crba_code_gen.M;
39 
40  // And make it symmetric if needed
41  M.template triangularView<Eigen::StrictlyLower>() = M.transpose().template triangularView<Eigen::StrictlyLower>();
42 
43  // You can check the result with the classic CRBA
44  Data data_check(model);
45  crba(model,data_check,q);
46 
47  data_check.M.triangularView<Eigen::StrictlyLower>() = data_check.M.transpose().triangularView<Eigen::StrictlyLower>();
48 
49  const MatrixXd & M_check = data_check.M;
50  if(M_check.isApprox(M)) {
51  std::cout << "Super! The two results are the same." << std::endl;
52  return 0;
53  }
54  else {
55  std::cout << "Not Super! The results do not match." << std::endl;
56  return -1;
57  }
58 
59 }
int main(int argc, const char **argv)
const DataTpl< Scalar, Options, JointCollectionTpl >::MatrixXs & crba(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q)
Computes the upper triangular part of the joint space inertia matrix M by using the Composite Rigid B...
void randomConfiguration(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorIn1 > &lowerLimits, const Eigen::MatrixBase< ConfigVectorIn2 > &upperLimits, const Eigen::MatrixBase< ReturnType > &qout)
Generate a configuration vector uniformly sampled among provided limits.
#define PINOCCHIO_MODEL_DIR
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > & q
void loadLib(const bool generate_if_not_exist=true)
ModelTpl< Scalar, Options, JointCollectionTpl > & buildModel(const std::string &filename, const typename ModelTpl< Scalar, Options, JointCollectionTpl >::JointModel &rootJoint, ModelTpl< Scalar, Options, JointCollectionTpl > &model, const bool verbose=false)
Build the model from a URDF file with a particular joint as root of the model tree inside the model g...
Main pinocchio namespace.
Definition: timings.cpp:28
filename
M
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > VectorXd
Definition: conversions.cpp:14
void evalFunction(const Eigen::MatrixBase< ConfigVectorType > &q)
JointCollectionTpl & model


pinocchio
Author(s):
autogenerated on Fri Jun 23 2023 02:38:29