Unverified Commit 0f221d87 authored by Justin Carpentier's avatar Justin Carpentier Committed by GitHub
Browse files

Merge pull request #1059 from jcarpent/devel

Add example for CRBA codegen
parents 20c63449 e06bf071
......@@ -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)
#
# 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)
//
// 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;
}
......@@ -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;
};
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment