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

clean *-benchmark.cpp

parent a63f9235
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
......@@ -11,28 +11,51 @@
static void BM_Pinocchio_RNEA(benchmark::State& state)
{
se3::Model model;
// Load an urdf file provided by the user
se3::urdf::buildModel("models/simple_humanoid.urdf", se3::JointModelFreeFlyer(), model);
std::cout << "Pinocchio Benchmark" << std::endl;
std::cout << " model: " << "models/simple_humanoid.urdf" << std::endl;
std::cout << " nq: " << model.nq << std::endl;
std::cout << " nv: " << model.nv << std::endl;
se3::Data data(model);
Eigen::VectorXd q = Eigen::VectorXd::Zero(model.nq);
Eigen::VectorXd qdot = Eigen::VectorXd::Zero(model.nv);
Eigen::VectorXd tau = Eigen::VectorXd::Zero(model.nv);
Eigen::VectorXd qddot = Eigen::VectorXd::Zero(model.nv);
for (auto _: state)
{
state.PauseTiming();
q = Eigen::VectorXd::Random(model.nq);
qdot = Eigen::VectorXd::Random(model.nv);
qddot = Eigen::VectorXd::Random(model.nv);
state.ResumeTiming();
se3::rnea(model, data, q, qdot, tau);
}
}
static void BM_Pinocchio_ABA(benchmark::State& state)
{
se3::Model model;
se3::urdf::buildModel("models/simple_humanoid.urdf", se3::JointModelFreeFlyer(), model);
se3::Data data(model);
Eigen::VectorXd q = Eigen::VectorXd::Zero(model.nq);
Eigen::VectorXd qdot = Eigen::VectorXd::Zero(model.nv);
Eigen::VectorXd tau = Eigen::VectorXd::Zero(model.nv);
Eigen::VectorXd qddot = Eigen::VectorXd::Zero(model.nv);
for (auto _: state)
{
Eigen::VectorXd q = Eigen::VectorXd::Random(model.nq);
Eigen::VectorXd qdot = Eigen::VectorXd::Random(model.nv);
Eigen::VectorXd tau = Eigen::VectorXd::Random(model.nv);
Eigen::VectorXd qddot = Eigen::VectorXd::Random(model.nv);
state.PauseTiming();
q = Eigen::VectorXd::Random(model.nq);
qdot = Eigen::VectorXd::Random(model.nv);
tau = Eigen::VectorXd::Random(model.nv);
state.ResumeTiming();
se3::aba(model, data, q, qdot, tau);
//std::cout << "qddot after ABA: " << data.ddq.transpose() << std::endl;
}
}
BENCHMARK(BM_Pinocchio_RNEA);
BENCHMARK(BM_Pinocchio_ABA);
BENCHMARK_MAIN();
......@@ -9,35 +9,57 @@
static void BM_RBDL_RNEA(benchmark::State& state)
{
RigidBodyDynamics::Model* model = new RigidBodyDynamics::Model();
RigidBodyDynamics::Addons::URDFReadFromFile("models/simple_humanoid.urdf", model, true);
RigidBodyDynamics::Math::VectorNd q =
RigidBodyDynamics::Math::VectorNd::Zero(model->q_size);
RigidBodyDynamics::Math::VectorNd qdot =
RigidBodyDynamics::Math::VectorNd::Zero(model->qdot_size);
RigidBodyDynamics::Math::VectorNd tau =
RigidBodyDynamics::Math::VectorNd::Zero(model->qdot_size);
RigidBodyDynamics::Math::VectorNd qddot =
RigidBodyDynamics::Math::VectorNd::Zero(model->qdot_size);
// Load an urdf file provided by the user
for (auto _: state)
{
state.PauseTiming();
q = RigidBodyDynamics::Math::VectorNd::Random(model->q_size);
qdot = RigidBodyDynamics::Math::VectorNd::Random(model->qdot_size);
qddot = RigidBodyDynamics::Math::VectorNd::Random(model->qdot_size);
state.ResumeTiming();
RigidBodyDynamics::InverseDynamics(*model, q, qdot, tau, qddot);
}
}
static void BM_RBDL_ABA(benchmark::State& state)
{
RigidBodyDynamics::Model* model = new RigidBodyDynamics::Model();
RigidBodyDynamics::Addons::URDFReadFromFile("models/simple_humanoid.urdf", model, true);
std::cout << "RBDL Benchmark" << std::endl;
std::cout << " model: " << "models/simple_humanoid.urdf" << std::endl;
std::cout << " nq: " << model->q_size << std::endl;
std::cout << " nv: " << model->qdot_size << std::endl;
RigidBodyDynamics::Math::VectorNd q =
RigidBodyDynamics::Math::VectorNd::Random(model->q_size);
RigidBodyDynamics::Math::VectorNd::Zero(model->q_size);
RigidBodyDynamics::Math::VectorNd qdot =
RigidBodyDynamics::Math::VectorNd::Random(model->qdot_size);
RigidBodyDynamics::Math::VectorNd::Zero(model->qdot_size);
RigidBodyDynamics::Math::VectorNd tau =
RigidBodyDynamics::Math::VectorNd::Random(model->qdot_size);
RigidBodyDynamics::Math::VectorNd::Zero(model->qdot_size);
RigidBodyDynamics::Math::VectorNd qddot =
RigidBodyDynamics::Math::VectorNd::Random(model->qdot_size);
RigidBodyDynamics::Math::VectorNd::Zero(model->qdot_size);
for (auto _: state)
{
state.PauseTiming();
q = RigidBodyDynamics::Math::VectorNd::Random(model->q_size);
qdot = RigidBodyDynamics::Math::VectorNd::Random(model->qdot_size);
tau = RigidBodyDynamics::Math::VectorNd::Random(model->qdot_size);
qddot = RigidBodyDynamics::Math::VectorNd::Random(model->qdot_size);
state.ResumeTiming();
RigidBodyDynamics::ForwardDynamics(*model, q, qdot, tau, qddot);
//std::cout << "qddot after ABA: " << qddot.transpose() << std::endl;
}
}
BENCHMARK(BM_RBDL_RNEA);
BENCHMARK(BM_RBDL_ABA);
BENCHMARK_MAIN();
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