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

bench/derivatives: update timings derivatives

parent 18578493
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
benchmark/CMakeLists.txt
+ 7
− 0
View file @ b77322f2
......@@ -69,6 +69,13 @@ ADD_BENCH(timings-cholesky TRUE)
#
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
+ 243
− 48
View file @ b77322f2
......@@ -15,11 +15,6 @@
// Pinocchio If not, see
// <http://www.gnu.org/licenses/>.
#include "pinocchio/spatial/fwd.hpp"
#include "pinocchio/spatial/se3.hpp"
#include "pinocchio/multibody/visitor.hpp"
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/multibody/data.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/algorithm/kinematics.hpp"
#include "pinocchio/algorithm/kinematics-derivatives.hpp"
......@@ -37,14 +32,199 @@
#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,
const Eigen::VectorXd & v,
const Eigen::VectorXd & a,
Eigen::MatrixXd & drnea_dq,
Eigen::MatrixXd & drnea_dv,
Eigen::MatrixXd & drnea_da)
const Eigen::MatrixBase<Matrix1> & _drnea_dq,
const Eigen::MatrixBase<Matrix2> & _drnea_dv,
const Eigen::MatrixBase<Matrix3> & _drnea_da)
{
Matrix1 & drnea_dq = EIGEN_CONST_CAST(Matrix1,_drnea_dq);
Matrix2 & drnea_dv = EIGEN_CONST_CAST(Matrix2,_drnea_dv);
Matrix3 & drnea_da = EIGEN_CONST_CAST(Matrix3,_drnea_da);
using namespace Eigen;
VectorXd v_eps(VectorXd::Zero(model.nv));
VectorXd q_plus(model.nq);
......@@ -77,8 +257,8 @@ void rnea_fd(const se3::Model & model, se3::Data & data_fd,
// dRNEA/da
drnea_da = crba(model,data_fd,q);
drnea_da.triangularView<Eigen::StrictlyLower>()
= drnea_da.transpose().triangularView<Eigen::StrictlyLower>();
drnea_da.template triangularView<Eigen::StrictlyLower>()
= drnea_da.transpose().template triangularView<Eigen::StrictlyLower>();
}
......@@ -88,7 +268,7 @@ void aba_fd(const se3::Model & model, se3::Data & data_fd,
const Eigen::VectorXd & tau,
Eigen::MatrixXd & daba_dq,
Eigen::MatrixXd & daba_dv,
se3::Data::RowMatrixXd & daba_dtau)
se3::Data::RowMatrixXs & daba_dtau)
{
using namespace Eigen;
VectorXd v_eps(VectorXd::Zero(model.nv));
......@@ -157,6 +337,7 @@ int main(int argc, const char ** argv)
else
if(with_ff)
se3::urdf::buildModel(filename,JointModelFreeFlyer(),model);
// se3::urdf::buildModel(filename,JointModelRX(),model);
else
se3::urdf::buildModel(filename,model);
std::cout << "nq = " << model.nq << std::endl;
......@@ -181,28 +362,34 @@ int main(int argc, const char ** argv)
taus[i] = Eigen::VectorXd::Random(model.nv);
}
MatrixXd drnea_dq(MatrixXd::Zero(model.nv,model.nv));
MatrixXd drnea_dv(MatrixXd::Zero(model.nv,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::RowMatrixXd daba_dtau(Data::RowMatrixXd::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);
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)
{
......@@ -218,14 +405,22 @@ int main(int argc, const char ** argv)
}
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)
SMOOTH(NBT/100)
{
rnea_fd(model,data,qs[_smooth],qdots[_smooth],qddots[_smooth],
drnea_dq,drnea_dv,drnea_da);
}
std::cout << "RNEA finite differences= \t\t"; timer.toc(std::cout,NBT);
std::cout << "RNEA finite differences= \t\t"; timer.toc(std::cout,NBT/100);
timer.tic();
SMOOTH(NBT)
{
......@@ -248,23 +443,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