Commit d5aeccd0 authored by jcarpent's avatar jcarpent
Browse files

[Bench] Update timings of derivatives

parent 0d800d4b
......@@ -19,8 +19,13 @@
#include "pinocchio/spatial/se3.hpp"
#include "pinocchio/multibody/visitor.hpp"
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/algorithm/rnea-derivatives.hpp"
#include "pinocchio/algorithm/aba-derivatives.hpp"
#include "pinocchio/algorithm/aba.hpp"
#include "pinocchio/algorithm/rnea.hpp"
#include "pinocchio/algorithm/crba.hpp"
#include "pinocchio/algorithm/cholesky.hpp"
#include "pinocchio/parsers/urdf.hpp"
#include "pinocchio/parsers/sample-models.hpp"
#include "pinocchio/container/aligned-vector.hpp"
......@@ -29,6 +34,93 @@
#include "pinocchio/tools/timer.hpp"
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)
{
using namespace Eigen;
VectorXd v_eps(VectorXd::Zero(model.nv));
VectorXd q_plus(model.nq);
VectorXd tau_plus(model.nv);
const double alpha = 1e-8;
VectorXd tau0 = rnea(model,data_fd,q,v,a);
// dRNEA/dq
for(int k = 0; k < model.nv; ++k)
{
v_eps[k] += alpha;
q_plus = integrate(model,q,v_eps);
tau_plus = rnea(model,data_fd,q_plus,v,a);
drnea_dq.col(k) = (tau_plus - tau0)/alpha;
v_eps[k] -= alpha;
}
// dRNEA/dv
VectorXd v_plus(v);
for(int k = 0; k < model.nv; ++k)
{
v_plus[k] += alpha;
tau_plus = rnea(model,data_fd,q,v_plus,a);
drnea_dv.col(k) = (tau_plus - tau0)/alpha;
v_plus[k] -= alpha;
}
// dRNEA/da
drnea_da = crba(model,data_fd,q);
drnea_da.triangularView<Eigen::StrictlyLower>()
= drnea_da.transpose().triangularView<Eigen::StrictlyLower>();
}
void aba_fd(const se3::Model & model, se3::Data & data_fd,
const Eigen::VectorXd & q,
const Eigen::VectorXd & v,
const Eigen::VectorXd & tau,
Eigen::MatrixXd & daba_dq,
Eigen::MatrixXd & daba_dv,
se3::Data::RowMatrixXd & daba_dtau)
{
using namespace Eigen;
VectorXd v_eps(VectorXd::Zero(model.nv));
VectorXd q_plus(model.nq);
VectorXd a_plus(model.nv);
const double alpha = 1e-8;
VectorXd a0 = aba(model,data_fd,q,v,tau);
// dABA/dq
for(int k = 0; k < model.nv; ++k)
{
v_eps[k] += alpha;
q_plus = integrate(model,q,v_eps);
a_plus = rnea(model,data_fd,q_plus,v,tau);
daba_dq.col(k) = (a_plus - a0)/alpha;
v_eps[k] -= alpha;
}
// dABA/dv
VectorXd v_plus(v);
for(int k = 0; k < model.nv; ++k)
{
v_plus[k] += alpha;
a_plus = rnea(model,data_fd,q,v_plus,tau);
daba_dv.col(k) = (a_plus - a0)/alpha;
v_plus[k] -= alpha;
}
// dABA/dtau
daba_dtau = computeMinverse(model,data_fd,q);
}
int main(int argc, const char ** argv)
{
using namespace Eigen;
......@@ -46,12 +138,24 @@ int main(int argc, const char ** argv)
std::string filename = PINOCCHIO_SOURCE_DIR"/models/simple_humanoid.urdf";
if(argc>1) filename = argv[1];
bool with_ff = true;
if(argc>2)
{
const std::string ff_option = argv[2];
if(ff_option == "-no-ff")
with_ff = false;
}
if( filename == "HS")
buildModels::humanoidSimple(model,true);
else if( filename == "H2" )
buildModels::humanoid2d(model);
else
se3::urdf::buildModel(filename,JointModelFreeFlyer(),model);
if(with_ff)
se3::urdf::buildModel(filename,JointModelFreeFlyer(),model);
else
se3::urdf::buildModel(filename,model);
std::cout << "nq = " << model.nq << std::endl;
std::cout << "nv = " << model.nv << std::endl;
......@@ -81,6 +185,13 @@ int main(int argc, const char ** argv)
MatrixXd daba_dv(MatrixXd::Zero(model.nv,model.nv));
Data::RowMatrixXd daba_dtau(Data::RowMatrixXd::Zero(model.nv,model.nv));
timer.tic();
SMOOTH(NBT)
{
rnea(model,data,qs[_smooth],qdots[_smooth],qddots[_smooth]);
}
std::cout << "RNEA= \t\t"; timer.toc(std::cout,NBT);
timer.tic();
SMOOTH(NBT)
{
......@@ -89,6 +200,21 @@ int main(int argc, const char ** argv)
}
std::cout << "RNEA derivatives= \t\t"; timer.toc(std::cout,NBT);
timer.tic();
SMOOTH(NBT)
{
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);
timer.tic();
SMOOTH(NBT)
{
aba(model,data,qs[_smooth],qdots[_smooth],taus[_smooth]);
}
std::cout << "ABA= \t\t"; timer.toc(std::cout,NBT);
timer.tic();
SMOOTH(NBT)
{
......@@ -96,6 +222,31 @@ int main(int argc, const char ** argv)
daba_dq,daba_dv,daba_dtau);
}
std::cout << "ABA derivatives= \t\t"; timer.toc(std::cout,NBT);
timer.tic();
SMOOTH(NBT)
{
aba_fd(model,data,qs[_smooth],qdots[_smooth],taus[_smooth],
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);
std::cout << "--" << std::endl;
return 0;
......
Supports Markdown
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