Program Listing for File code-generator-base.hpp
↰ Return to documentation for file (include/pinocchio/codegen/code-generator-base.hpp
)
//
// Copyright (c) 2018-2020 CNRS INRIA
//
#ifndef __pinocchio_utils_code_generator_base_hpp__
#define __pinocchio_utils_code_generator_base_hpp__
#include "pinocchio/codegen/cppadcg.hpp"
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/multibody/data.hpp"
namespace pinocchio
{
template<typename _Scalar>
struct CodeGenBase
{
typedef _Scalar Scalar;
typedef CppAD::cg::CG<Scalar> CGScalar;
typedef CppAD::AD<CGScalar> ADScalar;
enum { Options = 0 };
typedef pinocchio::ModelTpl<Scalar,Options> Model;
typedef pinocchio::DataTpl<Scalar,Options> Data;
typedef pinocchio::ModelTpl<CGScalar,Options> CGModel;
typedef pinocchio::DataTpl<CGScalar,Options> CGData;
typedef pinocchio::ModelTpl<ADScalar,Options> ADModel;
typedef pinocchio::DataTpl<ADScalar,Options> ADData;
typedef Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic,Options> MatrixXs;
typedef Eigen::Matrix<Scalar,Eigen::Dynamic,1,Options> VectorXs;
typedef Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic,Options|Eigen::RowMajor> RowMatrixXs;
typedef Eigen::Matrix<ADScalar,Eigen::Dynamic,1,Options> ADVectorXs;
typedef Eigen::Matrix<ADScalar,Eigen::Dynamic,Eigen::Dynamic,Options> ADMatrixXs;
typedef typename Model::ConfigVectorType ConfigVectorType;
typedef typename Model::TangentVectorType TangentVectorType;
typedef typename ADModel::ConfigVectorType ADConfigVectorType;
typedef typename ADModel::TangentVectorType ADTangentVectorType;
typedef CppAD::ADFun<CGScalar> ADFun;
CodeGenBase(const Model & model,
const Eigen::DenseIndex dim_input,
const Eigen::DenseIndex dim_output,
const std::string & function_name,
const std::string & library_name)
: ad_model(model.template cast<ADScalar>())
, ad_data(ad_model)
, function_name(function_name)
, library_name(library_name + "_" + model.name)
, build_forward(true)
, build_jacobian(true)
{
ad_X = ADVectorXs(dim_input);
ad_Y = ADVectorXs(dim_output);
y = VectorXs(ad_Y.size());
jac = RowMatrixXs(ad_Y.size(),ad_X.size());
}
virtual void buildMap() = 0;
void initLib()
{
buildMap();
// generates source code
cgen_ptr = std::unique_ptr<CppAD::cg::ModelCSourceGen<Scalar> >(new CppAD::cg::ModelCSourceGen<Scalar>(ad_fun, function_name));
cgen_ptr->setCreateForwardZero(build_forward);
cgen_ptr->setCreateJacobian(build_jacobian);
libcgen_ptr = std::unique_ptr<CppAD::cg::ModelLibraryCSourceGen<Scalar> >(new CppAD::cg::ModelLibraryCSourceGen<Scalar>(*cgen_ptr));
dynamicLibManager_ptr
= std::unique_ptr<CppAD::cg::DynamicModelLibraryProcessor<Scalar> >(new CppAD::cg::DynamicModelLibraryProcessor<Scalar>(*libcgen_ptr,library_name));
}
CppAD::cg::ModelCSourceGen<Scalar> & codeGenerator()
{ return *cgen_ptr; }
void compileLib(const std::string& gccPath = "/usr/bin/gcc")
{
CppAD::cg::GccCompiler<Scalar> compiler(gccPath);
std::vector<std::string> compile_options = compiler.getCompileFlags();
compile_options[0] = "-Ofast";
compiler.setCompileFlags(compile_options);
dynamicLibManager_ptr->createDynamicLibrary(compiler,false);
}
bool existLib() const
{
const std::string filename = dynamicLibManager_ptr->getLibraryName() + CppAD::cg::system::SystemInfo<>::DYNAMIC_LIB_EXTENSION;
std::ifstream file(filename.c_str());
return file.good();
}
void compileAndLoadLib(const std::string& gccPath)
{
compileLib(gccPath);
loadLib(false);
}
void loadLib(const bool generate_if_not_exist = true)
{
if(!existLib() && generate_if_not_exist)
compileLib();
const auto it = dynamicLibManager_ptr->getOptions().find("dlOpenMode");
if (it == dynamicLibManager_ptr->getOptions().end())
{
dynamicLib_ptr.reset(new CppAD::cg::LinuxDynamicLib<Scalar>(dynamicLibManager_ptr->getLibraryName() + CppAD::cg::system::SystemInfo<>::DYNAMIC_LIB_EXTENSION));
}
else
{
int dlOpenMode = std::stoi(it->second);
dynamicLib_ptr.reset(new CppAD::cg::LinuxDynamicLib<Scalar>(dynamicLibManager_ptr->getLibraryName() + CppAD::cg::system::SystemInfo<>::DYNAMIC_LIB_EXTENSION, dlOpenMode));
}
generatedFun_ptr = dynamicLib_ptr->model(function_name.c_str());
}
template<typename Vector>
void evalFunction(const Eigen::MatrixBase<Vector> & x)
{
assert(build_forward);
generatedFun_ptr->ForwardZero(PINOCCHIO_EIGEN_CONST_CAST(Vector,x),y);
}
template<typename Vector>
void evalJacobian(const Eigen::MatrixBase<Vector> & x)
{
assert(build_jacobian);
CppAD::cg::ArrayView<const Scalar> x_(PINOCCHIO_EIGEN_CONST_CAST(Vector,x).data(),(size_t)x.size());
CppAD::cg::ArrayView<Scalar> jac_(jac.data(),(size_t)jac.size());
generatedFun_ptr->Jacobian(x_,jac_);
}
Eigen::DenseIndex getInputDimension() const { return ad_X.size(); }
Eigen::DenseIndex getOutputDimension() const { return ad_Y.size(); }
protected:
ADModel ad_model;
ADData ad_data;
const std::string function_name;
const std::string library_name;
bool build_forward;
bool build_jacobian;
ADVectorXs ad_X, ad_Y;
ADFun ad_fun;
ADConfigVectorType ad_q, ad_q_plus;
ADTangentVectorType ad_dq, ad_v, ad_a;
VectorXs y;
RowMatrixXs jac;
std::unique_ptr<CppAD::cg::ModelCSourceGen<Scalar> > cgen_ptr;
std::unique_ptr<CppAD::cg::ModelLibraryCSourceGen<Scalar> > libcgen_ptr;
std::unique_ptr<CppAD::cg::DynamicModelLibraryProcessor<Scalar> > dynamicLibManager_ptr;
std::unique_ptr<CppAD::cg::DynamicLib<Scalar> > dynamicLib_ptr;
std::unique_ptr<CppAD::cg::GenericModel<Scalar> > generatedFun_ptr;
}; // struct CodeGenBase
} // namespace pinocchio
#endif // ifndef __pinocchio_utils_code_generator_base_hpp__