add rbdl-benchmark with google benchmarks & fix pinocchio-benchmark

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
 ```

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)

src/rbdl-benchmark.cpp

+#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();