Skip to content
Snippets Groups Projects
Commit 03cd2ead authored by Guilhem Saurel's avatar Guilhem Saurel
Browse files

use std::chrono, remove .urdf from filename, and gather timings of all NBT tests

parent 34f72c4a
No related branches found
No related tags found
1 merge request!3Update the repository to the version used for the first submission of the article
#include <chrono>
#include <iostream>
#include <fstream>
......@@ -6,8 +7,6 @@
#include <pinocchio/algorithm/rnea.hpp>
#include <pinocchio/algorithm/aba.hpp>
#include <pinocchio/utils/timer.hpp>
#include <Eigen/StdVector>
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::VectorXd)
......@@ -15,12 +14,10 @@ EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::VectorXd)
#define NBT 100 * 1000
void benchmark_pinocchio_rnea(std::string model_file)
void benchmark_pinocchio_rnea(std::string model_file, std::string filename)
{
PinocchioTicToc timer(PinocchioTicToc::NS);
se3::Model model;
se3::urdf::buildModel(pinocchio_benchmarks::path + model_file,
se3::urdf::buildModel(pinocchio_benchmarks::path + model_file + ".urdf",
se3::JointModelFreeFlyer(), model);
se3::Data data(model);
......@@ -38,18 +35,23 @@ void benchmark_pinocchio_rnea(std::string model_file)
taus[i] = Eigen::VectorXd::Random(model.nq);
}
timer.tic();
for(size_t i=0; i<NBT; i++) se3::rnea(model, data, qs[i], qdots[i], taus[i]);
std::cout << "Pinocchio RNEA " << model_file << " \t ";
timer.toc(std::cout, NBT);
std::ofstream file;
file.open(filename);
for(size_t i=0; i<NBT; i++)
{
auto start = std::chrono::high_resolution_clock::now();
se3::rnea(model, data, qs[i], qdots[i], taus[i]);
auto end = std::chrono::high_resolution_clock::now();
std::chrono::nanoseconds time = end - start;
file << time.count() << std::endl;
}
file.close();
}
void benchmark_pinocchio_aba(std::string model_file)
void benchmark_pinocchio_aba(std::string model_file, std::string filename)
{
PinocchioTicToc timer(PinocchioTicToc::NS);
se3::Model model;
se3::urdf::buildModel(pinocchio_benchmarks::path + model_file,
se3::urdf::buildModel(pinocchio_benchmarks::path + model_file + ".urdf",
se3::JointModelFreeFlyer(), model);
se3::Data data(model);
......@@ -67,18 +69,27 @@ void benchmark_pinocchio_aba(std::string model_file)
taus[i] = Eigen::VectorXd::Random(model.nq);
}
timer.tic();
for(size_t i=0; i<NBT; i++) se3::aba(model, data, qs[i], qdots[i], taus[i]);
std::cout << "Pinocchio ABA " << model_file << " \t ";
timer.toc(std::cout, NBT);
std::ofstream file;
file.open(filename);
for(size_t i=0; i<NBT; i++)
{
auto start = std::chrono::high_resolution_clock::now();
se3::aba(model, data, qs[i], qdots[i], taus[i]);
auto end = std::chrono::high_resolution_clock::now();
std::chrono::nanoseconds time = end - start;
file << time.count() << std::endl;
}
file.close();
}
int main()
{
for (auto& model : pinocchio_benchmarks::models)
benchmark_pinocchio_rnea(model);
benchmark_pinocchio_rnea(model, pinocchio_benchmarks::get_filename(
"Pinocchio", "RNEA", model));
for (auto& model : pinocchio_benchmarks::models)
benchmark_pinocchio_aba(model);
benchmark_pinocchio_aba(model, pinocchio_benchmarks::get_filename(
"Pinocchio", "ABA", model));
return 0;
}
#include <chrono>
#include <iostream>
#include <fstream>
#include <rbdl/rbdl.h>
#include <rbdl/addons/urdfreader/urdfreader.h>
#include <pinocchio/utils/timer.hpp>
#include <Eigen/StdVector>
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(RigidBodyDynamics::Math::VectorNd)
......@@ -13,13 +12,11 @@ EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(RigidBodyDynamics::Math::VectorNd)
#define NBT 100 * 1000
void benchmark_rbdl_rnea(std::string model_file)
void benchmark_rbdl_rnea(std::string model_file, std::string filename)
{
PinocchioTicToc timer(PinocchioTicToc::NS);
RigidBodyDynamics::Model* model = new RigidBodyDynamics::Model();
RigidBodyDynamics::Addons::URDFReadFromFile((pinocchio_benchmarks::path +
model_file).c_str(), model, true);
model_file + ".urdf").c_str(), model, true);
std::vector<RigidBodyDynamics::Math::VectorNd> qs(NBT);
std::vector<RigidBodyDynamics::Math::VectorNd> qdots(NBT);
......@@ -35,22 +32,26 @@ void benchmark_rbdl_rnea(std::string model_file)
taus[i] = RigidBodyDynamics::Math::VectorNd::Random(model->qdot_size);
}
timer.tic();
std::ofstream file;
file.open(filename);
for(size_t i=0; i<NBT; i++)
{
auto start = std::chrono::high_resolution_clock::now();
RigidBodyDynamics::InverseDynamics(*model, qs[i], qdots[i], qddots[i], taus[i]);
std::cout << "RBDL RNEA " << model_file << " \t ";
timer.toc(std::cout, NBT);
auto end = std::chrono::high_resolution_clock::now();
std::chrono::nanoseconds time = end - start;
file << time.count() << std::endl;
}
file.close();
delete model;
}
void benchmark_rbdl_aba(std::string model_file)
void benchmark_rbdl_aba(std::string model_file, std::string filename)
{
PinocchioTicToc timer(PinocchioTicToc::NS);
RigidBodyDynamics::Model* model = new RigidBodyDynamics::Model();
RigidBodyDynamics::Addons::URDFReadFromFile((pinocchio_benchmarks::path +
model_file).c_str(), model, true);
model_file + ".urdf").c_str(), model, true);
std::vector<RigidBodyDynamics::Math::VectorNd> qs(NBT);
std::vector<RigidBodyDynamics::Math::VectorNd> qdots(NBT);
......@@ -66,11 +67,17 @@ void benchmark_rbdl_aba(std::string model_file)
taus[i] = RigidBodyDynamics::Math::VectorNd::Random(model->qdot_size);
}
timer.tic();
std::ofstream file;
file.open(filename);
for(size_t i=0; i<NBT; i++)
{
auto start = std::chrono::high_resolution_clock::now();
RigidBodyDynamics::ForwardDynamics(*model, qs[i], qdots[i], taus[i], qddots[i]);
std::cout << "RBDL ABA " << model_file << " \t ";
timer.toc(std::cout, NBT);
auto end = std::chrono::high_resolution_clock::now();
std::chrono::nanoseconds time = end - start;
file << time.count() << std::endl;
}
file.close();
delete model;
}
......@@ -78,9 +85,11 @@ void benchmark_rbdl_aba(std::string model_file)
int main()
{
for (auto& model : pinocchio_benchmarks::models)
benchmark_rbdl_rnea(model);
benchmark_rbdl_rnea(model, pinocchio_benchmarks::get_filename(
"RBDL", "RNEA", model));
for (auto& model : pinocchio_benchmarks::models)
benchmark_rbdl_aba(model);
benchmark_rbdl_aba(model, pinocchio_benchmarks::get_filename(
"RBDL", "ABA", model));
return 0;
}
#ifndef __pinocchio_benchmarks_models__
#define __pinocchio_benchmarks_models__
#include <unistd.h>
#include <boost/array.hpp>
namespace pinocchio_benchmarks
......@@ -9,14 +11,22 @@ namespace pinocchio_benchmarks
static const boost::array<std::string, 7> models =
{
"simple.urdf",
"romeo.urdf",
"nao.urdf",
"poppy.urdf",
"lwr.urdf",
"atlas.urdf",
"hyq.urdf"
"simple",
"romeo",
"nao",
"poppy",
"lwr",
"atlas",
"hyq"
};
inline static std::string get_filename(std::string lib, std::string algo, std::string model)
{
char hostname_c[50];
gethostname(hostname_c, sizeof(hostname_c));
std::string hostname(hostname_c);
return "data/" + hostname + "_" + lib + "_" + algo + "_" + model + ".txt";
}
}
#endif
......@@ -14,7 +14,7 @@ static void BM_Pinocchio_RNEA(benchmark::State& state)
{
se3::Model model;
se3::urdf::buildModel(pinocchio_benchmarks::path +
pinocchio_benchmarks::models[(unsigned long)state.range(0)],
pinocchio_benchmarks::models[(unsigned long)state.range(0)] + ".urdf",
se3::JointModelFreeFlyer(), model);
se3::Data data(model);
......@@ -40,7 +40,7 @@ static void BM_Pinocchio_ABA(benchmark::State& state)
{
se3::Model model;
se3::urdf::buildModel(pinocchio_benchmarks::path +
pinocchio_benchmarks::models[(unsigned long)state.range(0)],
pinocchio_benchmarks::models[(unsigned long)state.range(0)] + ".urdf",
se3::JointModelFreeFlyer(), model);
se3::Data data(model);
......
......@@ -11,7 +11,7 @@
void pinocchio_test(std::string model_file)
{
se3::Model model;
se3::urdf::buildModel(pinocchio_benchmarks::path + model_file,
se3::urdf::buildModel(pinocchio_benchmarks::path + model_file + ".urdf",
se3::JointModelFreeFlyer(), model);
std::cout << "Pinocchio Test" << std::endl;
......
......@@ -12,7 +12,7 @@ static void BM_RBDL_RNEA(benchmark::State& state)
{
RigidBodyDynamics::Model* model = new RigidBodyDynamics::Model();
RigidBodyDynamics::Addons::URDFReadFromFile((pinocchio_benchmarks::path +
pinocchio_benchmarks::models[(unsigned long)state.range(0)]).c_str(),
pinocchio_benchmarks::models[(unsigned long)state.range(0)] + ".urdf").c_str(),
model, true);
RigidBodyDynamics::Math::VectorNd q =
......@@ -43,7 +43,7 @@ static void BM_RBDL_ABA(benchmark::State& state)
{
RigidBodyDynamics::Model* model = new RigidBodyDynamics::Model();
RigidBodyDynamics::Addons::URDFReadFromFile((pinocchio_benchmarks::path +
pinocchio_benchmarks::models[(unsigned long)state.range(0)]).c_str(),
pinocchio_benchmarks::models[(unsigned long)state.range(0)] + ".urdf").c_str(),
model, true);
RigidBodyDynamics::Math::VectorNd q =
......@@ -70,7 +70,6 @@ static void BM_RBDL_ABA(benchmark::State& state)
delete model;
}
BENCHMARK(BM_RBDL_RNEA)->Arg(0)->Arg(1)->Arg(2)->Arg(3)->Arg(4)->Arg(5)->Arg(6);
BENCHMARK(BM_RBDL_ABA)->Arg(0)->Arg(1)->Arg(2)->Arg(3)->Arg(4)->Arg(5)->Arg(6);
......
......@@ -10,7 +10,7 @@ void rbdl_test(std::string model_file)
{
RigidBodyDynamics::Model* model = new RigidBodyDynamics::Model();
RigidBodyDynamics::Addons::URDFReadFromFile((pinocchio_benchmarks::path +
model_file).c_str(), model, true);
model_file + ".urdf").c_str(), model, true);
std::cout << "RBDL Test" << std::endl;
std::cout << " model: " << model_file << std::endl;
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment