add nao & poppy models, rework submodules, try kdl

sorry for the big commit -_-

.gitmodules

-[submodule "libs/kdl"]
-	path = libs/kdl
-	url = https://github.com/orocos/orocos_kinematics_dynamics.git
-[submodule "libs/pinocchio"]
+[submodule "pinocchio"]
 	path = libs/pinocchio
-	url = https://github.com/stack-of-tasks/pinocchio.git
+	url = ../../stack-of-tasks/pinocchio.git
+	shallow = true
+	fetchRecurseSubmodules = true
 [submodule "benchmark"]
-	path = benchmark
-	url = https://github.com/google/benchmark.git
-[submodule "googletest"]
-	path = googletest
-	url = https://github.com/google/googletest.git
+	path = libs/benchmark
+	url = ../../google/benchmark.git
+	shallow = true
+	fetchRecurseSubmodules = false
+[submodule "nao"]
+	path = libs/nao_description
+	url = ../../iaslab-unipd/nao_description.git
+	shallow = true
+	fetchRecurseSubmodules = false
+[submodule "poppy"]
+	path = libs/poppy-humanoid
+	url = ../../poppy-project/poppy-humanoid.git
+	shallow = true
+	fetchRecurseSubmodules = false

CMakeLists.txt

@@ -15,9 +15,11 @@ ADD_REQUIRED_DEPENDENCY("eigen3")
 ADD_REQUIRED_DEPENDENCY("pinocchio")
 ADD_REQUIRED_DEPENDENCY("rbdl")
 ADD_REQUIRED_DEPENDENCY("orocos-kdl")
+ADD_REQUIRED_DEPENDENCY("kdl_parser")
 ADD_REQUIRED_DEPENDENCY("urdfdom")
 ADD_REQUIRED_DEPENDENCY("benchmark")
 
+ADD_SUBDIRECTORY("libs")
 ADD_SUBDIRECTORY("src")
 
 SETUP_PROJECT_FINALIZE()

README.md

 # Benchmarking Pinocchio
 
-## Setup your environment
+## Install Pinocchio & KDL binaries
 
-You need both eigen2 (for KDL) and eigen3 (for RBDL & Pinocchio):
 ```
-sudo apt install -qqy libeigen{2,3}-dev
+sudo apt install -qqy robotpkg-pinocchio liborocos-kdl-dev libkdl-parser-dev
 ```
 
-You have to choose the `PREFIX` in which you want to install Pinocchio, RBDL & KDL:
-
+## Configure your prefix
 
 ```
 export PREFIX=$PWD/prefix  # with bash / zsh
@@ -18,53 +16,13 @@ set -x PREFIX $PWD/prefix  # with fish
 set -x LD_LIBRARY_PATH $PREFIX/lib:$LD_LIBRARY_PATH
 ```
 
-## Pinocchio
-
-### Downloading
-
-If you did not clone this repository with the `--recursive` options, you will have to get the submodules with:
-```
-git submodule update --init --recursive
-```
-
-### Installing
-
-```
-mkdir -p build/pinocchio
-pushd build/pinocchio
-cmake -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$PREFIX -DCMAKE_INSTALL_LIBDIR=lib -DBUILD_PYTHON_INTERFACE=OFF -DBUILD_UNIT_TESTS=OFF ../../libs/pinocchio
-make -j8 install
-popd
-```
-
-## KDL
-
-### Installing
-
-```
-mkdir -p build/kdl
-pushd build/kdl
-cmake -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$PREFIX -DCMAKE_INSTALL_LIBDIR=lib ../../libs/kdl/orocos_kdl
-make -j8 install
-popd
-```
-
-## RBDL
-
-### Downloading
-
-RBDL uses mercurial, and git-hg can't clone it, so we can't provide a git submodule, and you have to download it:
+## Install RBDL
 
 ```
 wget https://bitbucket.org/rbdl/rbdl/get/default.zip
 unzip default.zip
 rm default.zip
 mv rbdl-rbdl-* libs/rbdl
-```
-
-### Installing
-
-```
 mkdir -p build/rbdl
 pushd build/rbdl
 cmake -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$PREFIX -DCMAKE_INSTALL_LIBDIR=lib -DRBDL_BUILD_ADDON_URDFREADER=ON ../../libs/rbdl
@@ -72,21 +30,17 @@ make -j8 install
 popd
 ```
 
-## Google Benchmark
-
-### Installing
+## Install Google Benchmark
 
 ```
 mkdir -p build/google-benchmark
 pushd build/google-benchmark
-cmake -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$PREFIX -DCMAKE_INSTALL_LIBDIR=lib -DBENCHMARK_ENABLE_GTEST_TESTS=OFF ../../benchmark
+cmake -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$PREFIX -DCMAKE_INSTALL_LIBDIR=lib -DBENCHMARK_ENABLE_GTEST_TESTS=OFF ../../libs/benchmark
 make -j8 install
 popd
 ```
 
-## Benchmarks
-
-### Installing
+## Install
 
 ```
 mkdir -p build/pinocchio-benchmarks
@@ -99,12 +53,10 @@ popd
 ### Running
 
 ```
-./prefix/bin/rbdl-bench models/simple_humanoid.urdf
-./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
+$PREFIX/bin/rbdl-test
+$PREFIX/bin/pinocchio-test
+$PREFIX/bin/pinocchio-benchmark
+$PREFIX/bin/rbdl-benchmark
 sudo cpupower frequency-set --governor powersave
 ```

benchmark @ 1f35fa4a

-Subproject commit 1f35fa4aa71bffb5e5672f7ca876561d6adef4fd

libs/CMakeLists.txt

+INSTALL(FILES
+  "pinocchio/models/simple_humanoid.urdf"
+  "pinocchio/models/romeo/romeo_description/urdf/romeo.urdf"
+  "nao_description/urdf/nao_robot_v4.urdf"
+  "poppy-humanoid/hardware/URDF/robots/Poppy_Humanoid.URDF"
+  DESTINATION "share/pinocchio-benchmarks/models"
+  )

benchmark @ 63e183b3

+Subproject commit 63e183b38945a0d67671f94bd3f74d7ffb6f5a82

kdl @ ce11d1c8

-Subproject commit ce11d1c8158e57f255a85700373c0d0b105cad82

nao_description @ 9e1d7b0e

+Subproject commit 9e1d7b0e1ed83a8f16dc9daf2fef0bb9a792a73d

poppy-humanoid @ 5b6c54e5

+Subproject commit 5b6c54e55cc8218757581041d6118a28c89515ed

models

-libs/pinocchio/models
\ No newline at end of file

run.sh

+#!/bin/bash
+
+set -e
+#set -x
+
+#rm -rf build prefix
+
+#sudo apt install -qqy robotpkg-pinocchio liborocos-kdl-dev libkdl-parser-dev
+
+export PREFIX=${1:-$PWD/prefix}  # with bash / zsh
+export LD_LIBRARY_PATH=$PREFIX/lib:$LD_LIBRARY_PATH
+export CXX="ccache clang++-3.8"
+export CC="ccache clang-3.8"
+
+if [[ ! -d libs/rbdl ]]
+then
+    wget https://bitbucket.org/rbdl/rbdl/get/default.zip
+    unzip default.zip
+    rm default.zip
+    mv rbdl-rbdl-* libs/rbdl
+fi
+
+mkdir -p build/rbdl
+pushd build/rbdl
+cmake -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$PREFIX -DCMAKE_INSTALL_LIBDIR=lib \
+    -DRBDL_BUILD_ADDON_URDFREADER=ON \
+    ../../libs/rbdl
+make -j8 install
+popd
+
+mkdir -p build/google-benchmark
+pushd build/google-benchmark
+cmake -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$PREFIX -DCMAKE_INSTALL_LIBDIR=lib \
+    -DBENCHMARK_ENABLE_GTEST_TESTS=OFF -DBENCHMARK_ENABLE_LTO=true \
+    -DLLVMAR_EXECUTABLE="/usr/bin/llvm-ar-3.8" \
+    -DLLVMNM_EXECUTABLE="/usr/bin/llvm-nm-3.8" \
+    -DLLVMRANLIB_EXECUTABLE="/usr/bin/llvm-ranlib-3.8" \
+    ../../libs/benchmark
+make -j8 install
+popd
+
+mkdir -p build/pinocchio-benchmarks
+pushd build/pinocchio-benchmarks
+cmake -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$PREFIX -DCMAKE_INSTALL_LIBDIR=lib \
+    -DCMAKE_PREFIX_PATH=$PREFIX \
+    ../..
+make -j8 install
+popd
+
+$PREFIX/bin/rbdl-test
+$PREFIX/bin/pinocchio-test
+$PREFIX/bin/kdl-test
+echo "don't forget to run 'sudo cpupower frequency-set --governor performance'"
+$PREFIX/bin/pinocchio-benchmark
+$PREFIX/bin/rbdl-benchmark
+echo "don't forget to run 'sudo cpupower frequency-set --governor powersave'"

src/CMakeLists.txt

-# RBDL
-ADD_EXECUTABLE(rbdl-bench rbdl-bench)
-PKG_CONFIG_USE_DEPENDENCY(rbdl-bench rbdl)
-TARGET_LINK_LIBRARIES(rbdl-bench rbdl_urdfreader)
-INSTALL(TARGETS rbdl-bench RUNTIME DESTINATION bin)
+CONFIG_FILES(models.h)
+INCLUDE_DIRECTORIES("${CMAKE_CURRENT_BINARY_DIR}")
 
-# Pinocchio
-ADD_EXECUTABLE(pinocchio-bench pinocchio-bench)
-PKG_CONFIG_USE_DEPENDENCY(pinocchio-bench pinocchio)
-INSTALL(TARGETS pinocchio-bench RUNTIME DESTINATION bin)
+# RBDL test
+ADD_EXECUTABLE(rbdl-test rbdl-test)
+PKG_CONFIG_USE_DEPENDENCY(rbdl-test rbdl)
+TARGET_LINK_LIBRARIES(rbdl-test rbdl_urdfreader)
+INSTALL(TARGETS rbdl-test RUNTIME DESTINATION bin)
 
-# RBDL & benchmark
+# Pinocchio test
+ADD_EXECUTABLE(pinocchio-test pinocchio-test)
+PKG_CONFIG_USE_DEPENDENCY(pinocchio-test pinocchio)
+INSTALL(TARGETS pinocchio-test RUNTIME DESTINATION bin)
+
+# KDL test
+ADD_EXECUTABLE(kdl-test kdl-test)
+PKG_CONFIG_USE_DEPENDENCY(kdl-test kdl_parser)
+INSTALL(TARGETS kdl-test 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
+# Pinocchio benchmark
 ADD_EXECUTABLE(pinocchio-benchmark pinocchio-benchmark)
 PKG_CONFIG_USE_DEPENDENCY(pinocchio-benchmark pinocchio)
 PKG_CONFIG_USE_DEPENDENCY(pinocchio-benchmark benchmark)

src/kdl-test.cpp

+#include <iostream>
+#include <fstream>
+
+#include <kdl_parser/kdl_parser.hpp>
+
+#include "models.h"
+
+void kdl_test(std::string model_file)
+{
+  KDL::Tree tree;
+  kdl_parser::treeFromFile(pinocchio_benchmarks::path + model_file, tree);
+
+  std::cout << "KDL Test" << std::endl;
+  std::cout << "  model: " << model_file << std::endl;
+  std::cout << "  nq: " << tree.getNrOfJoints() << std::endl;
+}
+
+int main()
+{
+  for (auto& model : pinocchio_benchmarks::models)
+  {
+    kdl_test(model);
+  }
+  return 0;
+}

src/models.h.in

+#ifndef __pinocchio_benchmarks_models__
+#define __pinocchio_benchmarks_models__
+
+#include <boost/array.hpp>
+
+namespace pinocchio_benchmarks
+{
+    static const std::string path = "@CMAKE_INSTALL_PREFIX@/share/pinocchio-benchmarks/models/";
+
+    static const boost::array<std::string, 4> models =
+    {
+        "simple_humanoid.urdf",
+        "romeo.urdf",
+        "nao_robot_v4.urdf",
+        "Poppy_Humanoid.URDF"
+    };
+}
+
+#endif

src/pinocchio-benchmark.cpp

@@ -8,10 +8,14 @@
 #include <pinocchio/algorithm/rnea.hpp>
 #include <pinocchio/algorithm/aba.hpp>
 
+#include "models.h"
+
 static void BM_Pinocchio_RNEA(benchmark::State& state)
 {
   se3::Model model;
-  se3::urdf::buildModel("models/simple_humanoid.urdf", se3::JointModelFreeFlyer(), model);
+  se3::urdf::buildModel(pinocchio_benchmarks::path +
+      pinocchio_benchmarks::models[state.range(0)],
+      se3::JointModelFreeFlyer(), model);
   se3::Data data(model);
 
   Eigen::VectorXd q = Eigen::VectorXd::Zero(model.nq);
@@ -31,11 +35,15 @@ static void BM_Pinocchio_RNEA(benchmark::State& state)
   }
 }
 
-
 static void BM_Pinocchio_ABA(benchmark::State& state)
 {
+  std::string model_file = pinocchio_benchmarks::path +
+    pinocchio_benchmarks::models[state.range(0)];
+
   se3::Model model;
-  se3::urdf::buildModel("models/simple_humanoid.urdf", se3::JointModelFreeFlyer(), model);
+  se3::urdf::buildModel(pinocchio_benchmarks::path +
+      pinocchio_benchmarks::models[state.range(0)],
+      se3::JointModelFreeFlyer(), model);
   se3::Data data(model);
 
   Eigen::VectorXd q = Eigen::VectorXd::Zero(model.nq);
@@ -55,7 +63,7 @@ static void BM_Pinocchio_ABA(benchmark::State& state)
   }
 }
 
-BENCHMARK(BM_Pinocchio_RNEA);
-BENCHMARK(BM_Pinocchio_ABA);
+BENCHMARK(BM_Pinocchio_RNEA)->Arg(0)->Arg(1)->Arg(2)->Arg(3);
+BENCHMARK(BM_Pinocchio_ABA)->Arg(0)->Arg(1)->Arg(2)->Arg(3);
 
 BENCHMARK_MAIN();

src/pinocchio-bench.cpp → src/pinocchio-test.cpp

@@ -6,43 +6,39 @@
 #include <pinocchio/algorithm/rnea.hpp>
 #include <pinocchio/algorithm/aba.hpp>
 
-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;
-  }
+#include "models.h"
 
+void pinocchio_test(std::string model_file)
+{
   se3::Model model;
+  se3::urdf::buildModel(pinocchio_benchmarks::path + model_file,
+      se3::JointModelFreeFlyer(), model);
 
-  // Load an urdf file provided by the user
-  se3::urdf::buildModel(argv[1], se3::JointModelFreeFlyer(), model);
-  std::cout << "Pinocchio Benchmark" << std::endl;
-  std::cout << "  model: " << argv[1] << std::endl;
+  std::cout << "Pinocchio Test" << std::endl;
+  std::cout << "  model: " << model_file << std::endl;
   std::cout << "  nq: " << model.nq << std::endl;
   std::cout << "  nv: " << model.nv << std::endl;
 
   se3::Data data(model);
 
-  Eigen::VectorXd q = Eigen::VectorXd::Random(model.nq);
-  Eigen::VectorXd qdot = Eigen::VectorXd::Random(model.nv);
-  Eigen::VectorXd tau = Eigen::VectorXd::Random(model.nv);
-  Eigen::VectorXd qddot = Eigen::VectorXd::Random(model.nv);
+  Eigen::VectorXd q = Eigen::VectorXd::Zero(model.nq);
+  Eigen::VectorXd qdot = Eigen::VectorXd::Zero(model.nv);
+  Eigen::VectorXd tau = Eigen::VectorXd::Zero(model.nv);
+  Eigen::VectorXd qddot = Eigen::VectorXd::Zero(model.nv);
+  q[0] = 1;
 
   se3::aba(model, data, q, qdot, tau);
   std::cout << "qddot after ABA: " << data.ddq.transpose() << std::endl;
 
   se3::rnea(model, data, q, qdot, qddot);
   std::cout << "tau aftern RNEA: " << data.tau.transpose() << std::endl;
+}
 
+int main()
+{
+  for (auto& model : pinocchio_benchmarks::models)
+  {
+    pinocchio_test(model);
+  }
   return 0;
 }

src/rbdl-benchmark.cpp

@@ -6,10 +6,13 @@
 #include <rbdl/rbdl.h>
 #include <rbdl/addons/urdfreader/urdfreader.h>
 
+#include "models.h"
+
 static void BM_RBDL_RNEA(benchmark::State& state)
 {
   RigidBodyDynamics::Model* model = new RigidBodyDynamics::Model();
-  RigidBodyDynamics::Addons::URDFReadFromFile("models/simple_humanoid.urdf", model, true);
+  RigidBodyDynamics::Addons::URDFReadFromFile((pinocchio_benchmarks::path +
+      pinocchio_benchmarks::models[state.range(0)]).c_str(), model, true);
 
   RigidBodyDynamics::Math::VectorNd q =
     RigidBodyDynamics::Math::VectorNd::Zero(model->q_size);
@@ -30,12 +33,15 @@ static void BM_RBDL_RNEA(benchmark::State& state)
 
     RigidBodyDynamics::InverseDynamics(*model, q, qdot, tau, qddot);
   }
+
+  delete model;
 }
 
 static void BM_RBDL_ABA(benchmark::State& state)
 {
   RigidBodyDynamics::Model* model = new RigidBodyDynamics::Model();
-  RigidBodyDynamics::Addons::URDFReadFromFile("models/simple_humanoid.urdf", model, true);
+  RigidBodyDynamics::Addons::URDFReadFromFile((pinocchio_benchmarks::path +
+      pinocchio_benchmarks::models[state.range(0)]).c_str(), model, true);
 
   RigidBodyDynamics::Math::VectorNd q =
     RigidBodyDynamics::Math::VectorNd::Zero(model->q_size);
@@ -56,10 +62,12 @@ static void BM_RBDL_ABA(benchmark::State& state)
 
     RigidBodyDynamics::ForwardDynamics(*model, q, qdot, tau, qddot);
   }
+
+  delete model;
 }
 
 
-BENCHMARK(BM_RBDL_RNEA);
-BENCHMARK(BM_RBDL_ABA);
+BENCHMARK(BM_RBDL_RNEA)->Arg(0)->Arg(1)->Arg(2)->Arg(3);
+BENCHMARK(BM_RBDL_ABA)->Arg(0)->Arg(1)->Arg(2)->Arg(3);
 
 BENCHMARK_MAIN();

src/rbdl-bench.cpp → src/rbdl-test.cpp

@@ -4,45 +4,45 @@
 #include <rbdl/rbdl.h>
 #include <rbdl/addons/urdfreader/urdfreader.h>
 
-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;
-  }
+#include "models.h"
 
+void rbdl_test(std::string model_file)
+{
   RigidBodyDynamics::Model* model = new RigidBodyDynamics::Model();
+  RigidBodyDynamics::Addons::URDFReadFromFile((pinocchio_benchmarks::path +
+      model_file).c_str(), model, true);
 
-  // Load an urdf file provided by the user
-  RigidBodyDynamics::Addons::URDFReadFromFile(argv[1], model, true);
-  std::cout << "RBDL Benchmark" << std::endl;
-  std::cout << "  model: " << argv[1] << std::endl;
+  std::cout << "RBDL Test" << std::endl;
+  std::cout << "  model: " << model_file << 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::Zero(model->q_size);
   RigidBodyDynamics::Math::VectorNd qdot =
-    RigidBodyDynamics::Math::VectorNd::Random(model->qdot_size);
+    RigidBodyDynamics::Math::VectorNd::Zero(model->qdot_size);
   RigidBodyDynamics::Math::VectorNd tau =
-    RigidBodyDynamics::Math::VectorNd::Random(model->qdot_size);
+    RigidBodyDynamics::Math::VectorNd::Zero(model->qdot_size);
   RigidBodyDynamics::Math::VectorNd qddot =
-    RigidBodyDynamics::Math::VectorNd::Random(model->qdot_size);
+    RigidBodyDynamics::Math::VectorNd::Zero(model->qdot_size);
+  q[0] = 1;
 
   RigidBodyDynamics::ForwardDynamics(*model, q, qdot, tau, qddot);
   std::cout << "qddot after ABA: " << qddot.transpose() << std::endl;
 
+  qddot = RigidBodyDynamics::Math::VectorNd::Zero(model->qdot_size);
+
   RigidBodyDynamics::InverseDynamics(*model, q, qdot, qddot, tau);
   std::cout << "tau aftern RNEA: " << tau.transpose() << std::endl;
 
   delete model;
+}
+
+int main()
+{
+  for (auto& model : pinocchio_benchmarks::models)
+  {
+    rbdl_test(model);
+  }
   return 0;
 }