diff --git a/CMakeLists.txt b/CMakeLists.txt
index 3c3c794d2bc1af4db2393fe620a7315bae8aeb45..fed5577c003340546999d0ef5b3e75e76d61513b 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 c893cb16982a88557e11844a3a9eaa4f3302707b..434fab8ee9b3b27cd04b92e6f234ccd301dccb0f 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 0000000000000000000000000000000000000000..180867a8a9f8eeec7093f2f412dfb3ff7b4f1521
--- /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 0000000000000000000000000000000000000000..2c17526bae50aece0d83ef7209b5e5eb871e8469
--- /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;
+}