From a63f92356a89b02f1ce611d32bac0c6102acb9d8 Mon Sep 17 00:00:00 2001
From: Guilhem Saurel <guilhem.saurel@laas.fr>
Date: Tue, 24 Jul 2018 10:33:28 +0200
Subject: [PATCH] add rbdl-benchmark with google benchmarks & fix
pinocchio-benchmark
---
README.md | 4 ++++
src/CMakeLists.txt | 12 ++++++++++--
src/rbdl-benchmark.cpp | 43 ++++++++++++++++++++++++++++++++++++++++++
3 files changed, 57 insertions(+), 2 deletions(-)
create mode 100644 src/rbdl-benchmark.cpp
diff --git a/README.md b/README.md
index 96a0d6b..df6b6e6 100644
--- a/README.md
+++ b/README.md
@@ -103,4 +103,8 @@ popd
./prefix/bin/rbdl-bench models/romeo/romeo_description/urdf/romeo.urdf
./prefix/bin/pinocchio-bench models/simple_humanoid.urdf
./prefix/bin/pinocchio-bench models/romeo/romeo_description/urdf/romeo.urdf
+sudo cpupower frequency-set --governor performance
+./prefix/bin/pinocchio-benchmark
+./prefix/bin/rbdl-benchmark
+sudo cpupower frequency-set --governor powersave
```
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index fc15aca..fc84ba5 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -9,8 +9,16 @@ ADD_EXECUTABLE(pinocchio-bench pinocchio-bench)
PKG_CONFIG_USE_DEPENDENCY(pinocchio-bench pinocchio)
INSTALL(TARGETS pinocchio-bench RUNTIME DESTINATION bin)
+# RBDL & benchmark
+ADD_EXECUTABLE(rbdl-benchmark rbdl-benchmark)
+PKG_CONFIG_USE_DEPENDENCY(rbdl-benchmark rbdl)
+PKG_CONFIG_USE_DEPENDENCY(rbdl-benchmark benchmark)
+TARGET_LINK_LIBRARIES(rbdl-benchmark rbdl_urdfreader pthread)
+INSTALL(TARGETS rbdl-benchmark RUNTIME DESTINATION bin)
+
# Pinocchio & benchmark
ADD_EXECUTABLE(pinocchio-benchmark pinocchio-benchmark)
-PKG_CONFIG_USE_DEPENDENCY(pinocchio-bench pinocchio)
-PKG_CONFIG_USE_DEPENDENCY(pinocchio-bench benchmark)
+PKG_CONFIG_USE_DEPENDENCY(pinocchio-benchmark pinocchio)
+PKG_CONFIG_USE_DEPENDENCY(pinocchio-benchmark benchmark)
+TARGET_LINK_LIBRARIES(pinocchio-benchmark pthread)
INSTALL(TARGETS pinocchio-benchmark RUNTIME DESTINATION bin)
diff --git a/src/rbdl-benchmark.cpp b/src/rbdl-benchmark.cpp
new file mode 100644
index 0000000..2984fe9
--- /dev/null
+++ b/src/rbdl-benchmark.cpp
@@ -0,0 +1,43 @@
+#include <iostream>
+#include <fstream>
+
+#include <benchmark/benchmark.h>
+
+#include <rbdl/rbdl.h>
+#include <rbdl/addons/urdfreader/urdfreader.h>
+
+static void BM_RBDL_RNEA(benchmark::State& state)
+{
+ RigidBodyDynamics::Model* model = new RigidBodyDynamics::Model();
+
+ // Load an urdf file provided by the user
+ 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 qdot =
+ RigidBodyDynamics::Math::VectorNd::Random(model->qdot_size);
+ RigidBodyDynamics::Math::VectorNd tau =
+ RigidBodyDynamics::Math::VectorNd::Random(model->qdot_size);
+ RigidBodyDynamics::Math::VectorNd qddot =
+ RigidBodyDynamics::Math::VectorNd::Random(model->qdot_size);
+
+ for (auto _: state)
+ {
+ 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);
+
+ RigidBodyDynamics::ForwardDynamics(*model, q, qdot, tau, qddot);
+ //std::cout << "qddot after ABA: " << qddot.transpose() << std::endl;
+ }
+}
+
+BENCHMARK(BM_RBDL_RNEA);
+
+BENCHMARK_MAIN();
--
GitLab