Merge pull request #1059 from jcarpent/devel

Add example for CRBA codegen

examples/CMakeLists.txt

@@ -2,6 +2,23 @@
 # Copyright (c) 2015-2020 CNRS INRIA
 #
 
+FUNCTION(ADD_PINOCCHIO_CPP_EXAMPLE EXAMPLE)
+  GET_FILENAME_COMPONENT(EXAMPLE_NAME ${EXAMPLE} NAME)
+  SET(EXAMPLE_NAME "example-cpp-${EXAMPLE_NAME}")
+  ADD_UNIT_TEST(${EXAMPLE_NAME} ${EXAMPLE})
+  TARGET_LINK_LIBRARIES(${EXAMPLE_NAME} ${PROJECT_NAME})
+  PKG_CONFIG_USE_DEPENDENCY(${EXAMPLE_NAME} eigen3)
+  IF(URDFDOM_FOUND)
+    PKG_CONFIG_USE_DEPENDENCY(${EXAMPLE_NAME} urdfdom)
+  ENDIF(URDFDOM_FOUND)
+  IF(HPP_FCL_FOUND)
+    PKG_CONFIG_USE_DEPENDENCY(${EXAMPLE_NAME} hpp-fcl)
+  ENDIF(HPP_FCL_FOUND)
+  IF(CPPAD_FOUND)
+    PKG_CONFIG_USE_DEPENDENCY(${EXAMPLE_NAME} "cppad")
+  ENDIF(CPPAD_FOUND)
+ENDFUNCTION()
+
 SET(${PROJECT_NAME}_EXAMPLES
   inverse-kinematics
   overview-simple
@@ -26,19 +43,7 @@ ENDIF(HPP_FCL_FOUND)
 ADD_DEFINITIONS(-DPINOCCHIO_MODEL_DIR="${PINOCCHIO_MODEL_DIR}")
 
 FOREACH(EXAMPLE ${${PROJECT_NAME}_EXAMPLES})
-  SET(EXAMPLE_NAME "example-cpp-${EXAMPLE}")
-  ADD_UNIT_TEST(${EXAMPLE_NAME} ${EXAMPLE})
-  TARGET_LINK_LIBRARIES(${EXAMPLE_NAME} ${PROJECT_NAME})
-  PKG_CONFIG_USE_DEPENDENCY(${EXAMPLE_NAME} eigen3)
-  IF(URDFDOM_FOUND)
-    PKG_CONFIG_USE_DEPENDENCY(${EXAMPLE_NAME} urdfdom)
-  ENDIF(URDFDOM_FOUND)
-  IF(HPP_FCL_FOUND)
-    PKG_CONFIG_USE_DEPENDENCY(${EXAMPLE_NAME} hpp-fcl)
-  ENDIF(HPP_FCL_FOUND)
-  IF(CPPAD_FOUND)
-    PKG_CONFIG_USE_DEPENDENCY(${EXAMPLE_NAME} "cppad")
-  ENDIF(CPPAD_FOUND)
+  ADD_PINOCCHIO_CPP_EXAMPLE(${EXAMPLE})
 ENDFOREACH(EXAMPLE ${${PROJECT_NAME}_EXAMPLES})
 
 IF(BUILD_PYTHON_INTERFACE)
@@ -68,3 +73,5 @@ IF(BUILD_PYTHON_INTERFACE)
     ADD_PYTHON_UNIT_TEST("example-py-${EXAMPLE}" "examples/${EXAMPLE}.py" "bindings/python")
   ENDFOREACH(EXAMPLE ${${PROJECT_NAME}_PYTHON_EXAMPLES})
 ENDIF(BUILD_PYTHON_INTERFACE)
+
+ADD_SUBDIRECTORY(codegen)

examples/codegen/CMakeLists.txt

+#
+# Copyright (c) 2020 INRIA
+#
+
+IF(CPPADCG_FOUND AND BUILD_WITH_CODEGEN_SUPPORT)
+  ADD_PINOCCHIO_CPP_EXAMPLE(codegen-crba)
+  SET_PROPERTY(TARGET example-cpp-codegen-crba PROPERTY CXX_STANDARD 11) 
+  TARGET_LINK_LIBRARIES(example-cpp-codegen-crba ${CMAKE_DL_LIBS})
+ENDIF(CPPADCG_FOUND AND BUILD_WITH_CODEGEN_SUPPORT)
+

examples/codegen/codegen-crba.cpp

+//
+// 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("/others/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.loadLib();
+
+  // 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);
+
+  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;
+  else
+    std::cout << "Not Super! The results do not match." << std::endl;
+
+  return -1;
+}

src/codegen/code-generator-algo.hpp

@@ -98,6 +98,8 @@ namespace pinocchio
       dtau_da = Base::jac.middleCols(it,ad_model.nv); it += ad_model.nv;
     }
     
+    MatrixXs dtau_dq, dtau_dv, dtau_da;
+    
   protected:
     
     using Base::ad_model;
@@ -110,7 +112,6 @@ namespace pinocchio
     
     VectorXs x;
     VectorXs res;
-    MatrixXs dtau_dq, dtau_dv, dtau_da;
     
     ADCongigVectorType ad_q, ad_q_plus;
     ADTangentVectorType ad_dq, ad_v, ad_a;
@@ -196,6 +197,8 @@ namespace pinocchio
       da_dtau = Base::jac.middleCols(it,ad_model.nv); it += ad_model.nv;
     }
     
+    MatrixXs da_dq,da_dv,da_dtau;
+    
   protected:
     
     using Base::ad_model;
@@ -208,7 +211,6 @@ namespace pinocchio
     
     VectorXs x;
     VectorXs res;
-    MatrixXs da_dq,da_dv,da_dtau;
     
     ADCongigVectorType ad_q, ad_q_plus;
     ADTangentVectorType ad_dq, ad_v, ad_tau;
@@ -286,6 +288,8 @@ namespace pinocchio
       assert(it_Y == Base::getOutputDimension());
     }
     
+    MatrixXs M;
+    
   protected:
     
     using Base::ad_model;
@@ -297,7 +301,6 @@ namespace pinocchio
     
     VectorXs x;
     VectorXs res;
-    MatrixXs M;
     
     ADCongigVectorType ad_q;
   };
@@ -371,6 +374,8 @@ namespace pinocchio
       }
     }
     
+    MatrixXs Minv;
+    
   protected:
     
     using Base::ad_model;
@@ -382,7 +387,6 @@ namespace pinocchio
     
     VectorXs x;
     VectorXs res;
-    MatrixXs Minv;
     
     ADCongigVectorType ad_q;
   };