add rbdl bench

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()

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

src/CMakeLists.txt

+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)

src/rbdl-bench.cpp

+#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;
+}