Skip to content
Snippets Groups Projects
Verified Commit 9bccdfd8 authored by Justin Carpentier's avatar Justin Carpentier
Browse files

bench: correct timings-derivatives to work without CPPAD

parent 3226dd91
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
benchmark/CMakeLists.txt
+ 10
− 10
View file @ 9bccdfd8
......@@ -66,16 +66,16 @@ ENDIF(CPPADCG_FOUND)
ADD_BENCH(timings-cholesky TRUE)
# timings derivatives
#
#ADD_BENCH(timings-derivatives TRUE)
#TARGET_LINK_LIBRARIES(timings-derivatives ${PROJECT_NAME})
#IF(CPPAD_FOUND)
# PKG_CONFIG_USE_DEPENDENCY(timings-derivatives "cppad")
#ENDIF(CPPAD_FOUND)
#IF(CPPADCG_FOUND)
# SET_PROPERTY(TARGET timings-derivatives PROPERTY CXX_STANDARD 11)
# PKG_CONFIG_USE_DEPENDENCY(timings-derivatives "cppadcg")
#ENDIF(CPPADCG_FOUND)
ADD_BENCH(timings-derivatives TRUE)
TARGET_LINK_LIBRARIES(timings-derivatives ${PROJECT_NAME})
IF(CPPAD_FOUND)
PKG_CONFIG_USE_DEPENDENCY(timings-derivatives "cppad")
ENDIF(CPPAD_FOUND)
IF(CPPADCG_FOUND)
SET_PROPERTY(TARGET timings-derivatives PROPERTY CXX_STANDARD 11)
PKG_CONFIG_USE_DEPENDENCY(timings-derivatives "cppadcg")
ENDIF(CPPADCG_FOUND)
# timings-eigen
#
......
benchmark/timings-derivatives.cpp
+ 30
− 226
View file @ 9bccdfd8
......@@ -32,186 +32,6 @@
#include "pinocchio/utils/timer.hpp"
template<typename _Scalar, typename _EvalScalar = _Scalar>
struct AutoDiffRNEA
{
typedef _Scalar Scalar;
typedef CppAD::AD<Scalar> ADScalar;
typedef _EvalScalar EvalScalar;
enum { Options = 0 };
typedef se3::ModelTpl<Scalar,Options> Model;
typedef se3::DataTpl<Scalar,Options> Data;
typedef se3::ModelTpl<EvalScalar,Options> EvalModel;
typedef se3::DataTpl<EvalScalar,Options> EvalData;
typedef se3::ModelTpl<ADScalar,Options> ADModel;
typedef se3::DataTpl<ADScalar,Options> ADData;
typedef Eigen::Matrix<EvalScalar,Eigen::Dynamic,Eigen::Dynamic,Options> MatrixXs;
typedef Eigen::Matrix<EvalScalar,Eigen::Dynamic,1,Options> VectorXs;
typedef Eigen::Matrix<ADScalar,Eigen::Dynamic,1,Options> ADVectorXs;
typedef typename ADModel::ConfigVectorType ADCongigVectorType;
typedef typename ADModel::TangentVectorType ADTangentVectorType;
typedef CppAD::ADFun<Scalar> ADFun;
template<typename OtherScalar>
AutoDiffRNEA(const se3::ModelTpl<OtherScalar,Options> & model)
: drnea_dq(model.nv,model.nv)
, drnea_dv(model.nv,model.nv)
, drnea_da(model.nv,model.nv)
, ad_model(model.template cast<ADScalar>())
, ad_data(ad_model)
{
ad_q = ADCongigVectorType(model.nq);
ad_q_plus = ADCongigVectorType(model.nq);
ad_dq = ADTangentVectorType(model.nv); ad_dq.setZero();
ad_v = ADTangentVectorType(model.nv); ad_v.setZero();
ad_a = ADTangentVectorType(model.nv); ad_a.setZero();
ad_X = ADVectorXs(model.nv*3);
ad_Y = ADVectorXs(model.nv);
}
void compute()
{
ad_X.head(ad_model.nv).setZero();
size_t abort_op_index = 0;
bool record_compare = false;
CppAD::Independent(ad_X,abort_op_index,record_compare,ad_q);
Eigen::DenseIndex it = 0;
ad_dq = ad_X.head(ad_model.nv); it += ad_model.nv;
ad_v = ad_X.segment(it,ad_model.nv); it += ad_model.nv;
ad_a = ad_X.segment(it,ad_model.nv); it += ad_model.nv;
ad_q_plus = se3::integrate(ad_model,ad_q,ad_dq);
se3::rnea(ad_model,ad_data,ad_q_plus,ad_v,ad_a);
ad_Y = ad_data.tau;
ad_fun.Dependent(ad_X,ad_Y);
ad_fun.optimize("no_compare_op");
}
template<typename ConfigVectorType, typename TangentVector1, typename TangentVector2>
void eval(const Eigen::MatrixBase<ConfigVectorType> & q,
const Eigen::MatrixBase<TangentVector1> & v,
const Eigen::MatrixBase<TangentVector2> & a)
{
ad_fun.new_dynamic(q.derived());
CPPAD_TESTVECTOR(EvalScalar) x((size_t)(ad_X.size()));
Eigen::DenseIndex it = 0;
Eigen::Map<typename Data::TangentVectorType>(x.data(),ad_model.nv,1).setZero();
it += ad_model.nv;
Eigen::Map<typename Data::TangentVectorType>(x.data()+it,ad_model.nv,1) = v;
it += ad_model.nv;
Eigen::Map<typename Data::TangentVectorType>(x.data()+it,ad_model.nv,1) = a;
it += ad_model.nv;
CPPAD_TESTVECTOR(EvalScalar) dy_dx = ad_fun.Jacobian(x);
it = 0;
drnea_dq = Eigen::Map<typename EIGEN_PLAIN_ROW_MAJOR_TYPE(typename EvalData::MatrixXs)>(dy_dx.data(),ad_model.nv,ad_model.nv);
it += ad_model.nv * ad_model.nv;
drnea_dv = Eigen::Map<typename EIGEN_PLAIN_ROW_MAJOR_TYPE(typename EvalData::MatrixXs)>(dy_dx.data()+it,ad_model.nv,ad_model.nv);
it += ad_model.nv * ad_model.nv;
drnea_da = Eigen::Map<typename EIGEN_PLAIN_ROW_MAJOR_TYPE(typename EvalData::MatrixXs)>(dy_dx.data()+it,ad_model.nv,ad_model.nv);
it += ad_model.nv * ad_model.nv;
}
// Results of eval
MatrixXs drnea_dq, drnea_dv, drnea_da;
ADCongigVectorType ad_q, ad_q_plus;
ADTangentVectorType ad_dq, ad_v, ad_a;
ADVectorXs ad_X, ad_Y;
ADFun ad_fun;
protected:
ADModel ad_model;
ADData ad_data;
};
template<typename _Scalar>
struct CodeGenRNEA : AutoDiffRNEA<CppAD::cg::CG<_Scalar>,_Scalar>
{
typedef _Scalar Scalar;
typedef CppAD::cg::CG<Scalar> CGScalar;
typedef AutoDiffRNEA<CGScalar,Scalar> Base;
typedef typename Base::ADScalar ADScalar;
typedef typename Base::EvalScalar EvalScalar;
enum { Options = Base::Options };
typedef se3::ModelTpl<Scalar,Options> Model;
using Base::eval;
using Base::compute;
CodeGenRNEA(const Model & model,
const std::string & function_name = "rnea",
const std::string & library_name = "cg_rnea")
: Base(model.template cast<ADScalar>())
, dynamicLib_ptr(nullptr)
{
compute();
// generates source code
cgen_ptr = std::unique_ptr<CppAD::cg::ModelCSourceGen<Scalar> >(new CppAD::cg::ModelCSourceGen<Scalar>(Base::ad_fun, function_name));
cgen_ptr->setCreateJacobian(true);
cgen_ptr->setCreateForwardZero(true);
// cgen.setCreateForwardOne(true);
// cgen.setCreateReverseOne(true);
// cgen.setCreateReverseTwo(true);
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));
}
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 generateLib()
{
CppAD::cg::GccCompiler<Scalar> compiler;
dynamicLibManager_ptr->createDynamicLibrary(compiler,false);
}
void loadLib(const bool generate_if_not_exist = true)
{
if(not existLib() && generate_if_not_exist)
generateLib();
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));
}
}
protected:
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;
};
template<typename Matrix1, typename Matrix2, typename Matrix3>
void rnea_fd(const se3::Model & model, se3::Data & data_fd,
const Eigen::VectorXd & q,
......@@ -332,8 +152,6 @@ int main(int argc, const char ** argv)
if( filename == "HS")
buildModels::humanoidRandom(model,true);
else if( filename == "H2" )
buildModels::humanoid2d(model);
else
if(with_ff)
se3::urdf::buildModel(filename,JointModelFreeFlyer(),model);
......@@ -361,34 +179,28 @@ int main(int argc, const char ** argv)
qddots[i] = Eigen::VectorXd::Random(model.nv);
taus[i] = Eigen::VectorXd::Random(model.nv);
}
// AutoDiffRNEA<double> ad_rnea(model);
// CodeGenRNEA<double> cg_rnea(model);
// cg_rnea.compute();
// cg_rnea.loadLib();
EIGEN_PLAIN_ROW_MAJOR_TYPE(MatrixXd) drnea_dq(MatrixXd::Zero(model.nv,model.nv));
EIGEN_PLAIN_ROW_MAJOR_TYPE(MatrixXd) drnea_dv(MatrixXd::Zero(model.nv,model.nv));
// EIGEN_PLAIN_ROW_MAJOR_TYPE(MatrixXd)
MatrixXd drnea_da(MatrixXd::Zero(model.nv,model.nv));
MatrixXd daba_dq(MatrixXd::Zero(model.nv,model.nv));
MatrixXd daba_dv(MatrixXd::Zero(model.nv,model.nv));
Data::RowMatrixXs daba_dtau(Data::RowMatrixXs::Zero(model.nv,model.nv));
// timer.tic();
// SMOOTH(NBT)
// {
// forwardKinematics(model,data,qs[_smooth],qdots[_smooth],qddots[_smooth]);
// }
// std::cout << "FK= \t\t"; timer.toc(std::cout,NBT);
//
// timer.tic();
// SMOOTH(NBT)
// {
// computeForwardKinematicsDerivatives(model,data,qs[_smooth],qdots[_smooth],qddots[_smooth]);
// }
// std::cout << "FK derivatives= \t\t"; timer.toc(std::cout,NBT);
timer.tic();
SMOOTH(NBT)
{
forwardKinematics(model,data,qs[_smooth],qdots[_smooth],qddots[_smooth]);
}
std::cout << "FK= \t\t"; timer.toc(std::cout,NBT);
timer.tic();
SMOOTH(NBT)
{
computeForwardKinematicsDerivatives(model,data,qs[_smooth],qdots[_smooth],qddots[_smooth]);
}
std::cout << "FK derivatives= \t\t"; timer.toc(std::cout,NBT);
timer.tic();
SMOOTH(NBT)
......@@ -404,14 +216,6 @@ int main(int argc, const char ** argv)
drnea_dq,drnea_dv,drnea_da);
}
std::cout << "RNEA derivatives= \t\t"; timer.toc(std::cout,NBT);
// ad_rnea.compute();
// timer.tic();
// SMOOTH(NBT/100)
// {
// ad_rnea.eval(qs[_smooth],qdots[_smooth],qddots[_smooth]);
// }
// std::cout << "RNEA auto diff= \t\t"; timer.toc(std::cout,NBT/100);
timer.tic();
SMOOTH(NBT/100)
......@@ -443,23 +247,23 @@ int main(int argc, const char ** argv)
daba_dq,daba_dv,daba_dtau);
}
std::cout << "ABA finite differences= \t\t"; timer.toc(std::cout,NBT);
//
// timer.tic();
// SMOOTH(NBT)
// {
// computeMinverse(model,data,qs[_smooth]);
// }
// std::cout << "M.inverse() from ABA = \t\t"; timer.toc(std::cout,NBT);
//
// MatrixXd Minv(model.nv,model.nv); Minv.setZero();
// timer.tic();
// SMOOTH(NBT)
// {
// crba(model,data,qs[_smooth]);
// cholesky::decompose(model,data);
// cholesky::computeMinv(model,data,Minv);
// }
// std::cout << "Minv from Cholesky = \t\t"; timer.toc(std::cout,NBT);
timer.tic();
SMOOTH(NBT)
{
computeMinverse(model,data,qs[_smooth]);
}
std::cout << "M.inverse() from ABA = \t\t"; timer.toc(std::cout,NBT);
MatrixXd Minv(model.nv,model.nv); Minv.setZero();
timer.tic();
SMOOTH(NBT)
{
crba(model,data,qs[_smooth]);
cholesky::decompose(model,data);
cholesky::computeMinv(model,data,Minv);
}
std::cout << "Minv from Cholesky = \t\t"; timer.toc(std::cout,NBT);
std::cout << "--" << std::endl;
return 0;
......
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