From 702f0651f6549af1c69ca4ee0942c0e066b7b7ef Mon Sep 17 00:00:00 2001
From: Guilhem Saurel <guilhem.saurel@laas.fr>
Date: Wed, 18 Jul 2018 17:47:44 +0200
Subject: [PATCH] add rbdl bench

---
 CMakeLists.txt     |  2 ++
 README.md          |  9 +++++++++
 src/CMakeLists.txt |  7 +++++++
 src/rbdl-bench.cpp | 44 ++++++++++++++++++++++++++++++++++++++++++++
 4 files changed, 62 insertions(+)
 create mode 100644 src/CMakeLists.txt
 create mode 100644 src/rbdl-bench.cpp

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 3c3c794..fed5577 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -16,4 +16,6 @@ ADD_REQUIRED_DEPENDENCY("pinocchio")
 ADD_REQUIRED_DEPENDENCY("rbdl")
 ADD_REQUIRED_DEPENDENCY("orocos-kdl")
 
+ADD_SUBDIRECTORY("src")
+
 SETUP_PROJECT_FINALIZE()
diff --git a/README.md b/README.md
index c893cb1..434fab8 100644
--- a/README.md
+++ b/README.md
@@ -12,8 +12,10 @@ You have to choose the `PREFIX` in which you want to install Pinocchio, RBDL & K
 
 ```
 export PREFIX=$PWD/prefix  # with bash / zsh
+export LD_LIBRARY_PATH=$PREFIX/lib:$LD_LIBRARY_PATH
 # OR
 set -x PREFIX $PWD/prefix  # with fish
+set -x LD_LIBRARY_PATH $PREFIX/lib:$LD_LIBRARY_PATH
 ```
 
 ## Pinocchio
@@ -81,3 +83,10 @@ cmake -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$PREFIX -DCMAKE_PREFIX_P
 make install
 popd
 ```
+
+### Running
+
+```
+./prefix/bin/rbdl-bench models/simple_humanoid.urdf
+./prefix/bin/rbdl-bench models/romeo/romeo_description/urdf/romeo.urdf
+```
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
new file mode 100644
index 0000000..180867a
--- /dev/null
+++ b/src/CMakeLists.txt
@@ -0,0 +1,7 @@
+LINK_DIRECTORIES(${RBDL_LIBRARY_DIRS})
+
+ADD_EXECUTABLE(rbdl-bench rbdl-bench)
+TARGET_INCLUDE_DIRECTORIES(rbdl-bench PUBLIC ${RBDL_INCLUDE_DIRS})
+TARGET_LINK_LIBRARIES(rbdl-bench rbdl_urdfreader ${RBDL_LIBRARIES} ${EIGEN3_LIBRARIES})
+
+INSTALL(TARGETS rbdl-bench RUNTIME DESTINATION bin)
diff --git a/src/rbdl-bench.cpp b/src/rbdl-bench.cpp
new file mode 100644
index 0000000..2c17526
--- /dev/null
+++ b/src/rbdl-bench.cpp
@@ -0,0 +1,44 @@
+#include <iostream>
+#include <fstream>
+
+#include <rbdl/rbdl.h>
+#include <rbdl/addons/urdfreader/urdfreader.h>
+
+using namespace RigidBodyDynamics;
+using namespace RigidBodyDynamics::Addons;
+using namespace RigidBodyDynamics::Math;
+
+int main (int argc, char** argv)
+{
+  if (argc != 2)
+  {
+    std::cerr << "You have to specify a path to an urdf file" << std::endl;
+    return 1;
+  }
+
+  std::ifstream path(argv[1]);
+  if (!path)
+  {
+    std::cerr << "This path is not a valid file: " << argv[1] << std::endl;
+    return 2;
+  }
+
+  Model* model = new Model();
+
+  // Load an urdf file provided by the user, with a floating base
+  URDFReadFromFile(argv[1], model, true);
+
+  VectorNd Q = VectorNd::Zero (model->dof_count);
+  VectorNd QDot = VectorNd::Zero (model->dof_count);
+  VectorNd Tau = VectorNd::Zero (model->dof_count);
+  VectorNd QDDot = VectorNd::Zero (model->dof_count);
+
+  ForwardDynamics (*model, Q, QDot, Tau, QDDot);
+  std::cout << "QDDot after ABA: " << QDDot.transpose() << std::endl;
+
+  InverseDynamics (*model, Q, QDot, QDDot, Tau);
+  std::cout << "Tau aftern RNEA: " << Tau.transpose() << std::endl;
+
+  delete model;
+  return 0;
+}
-- 
GitLab