diff --git a/CMakeLists.txt b/CMakeLists.txt
index ddb31c9f..23cbbe25 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -8,12 +8,12 @@ set(API_VERSION ${PROJECT_VERSION})
 
 list(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake)
 
-option(ROBOT_MODEL_PINOCCHIO "Build the Pinocchio-based robot model" OFF)
-option(ROBOT_MODEL_KDL "Build the KDL-based robot model" OFF)
-option(ROBOT_MODEL_HYRODYN "Build the HyRoDyn-based robot model" OFF)
-option(SOLVER_PROXQP "Build the ProxQP-based solver" OFF)
-option(SOLVER_EIQUADPROG "Build the Eiquadprog-based solver" OFF)
-option(SOLVER_QPSWIFT "Build the QPSwift-based solver" OFF)
+option(ROBOT_MODEL_RBDL "Also build the RBDL-based robot model, by default only pinocchio is built" OFF)
+option(ROBOT_MODEL_KDL "Also build the KDL-based robot model, by default only pinocchio is built" OFF)
+option(ROBOT_MODEL_HYRODYN "Also build the HyRoDyn-based robot model, by default only pinocchio is built" OFF)
+option(SOLVER_PROXQP "Build the ProxQP-based solver, by default only hls and qpoases are built" OFF)
+option(SOLVER_EIQUADPROG "Build the Eiquadprog-based solver, by default only hls and qpoases are built" OFF)
+option(SOLVER_QPSWIFT "Build the QPSwift-based solver, by default only hls and qpoases are built" OFF)
 
 add_subdirectory(src)
 add_subdirectory(tutorials)
diff --git a/manifest.xml b/manifest.xml
index d28446ba..9dffefef 100644
--- a/manifest.xml
+++ b/manifest.xml
@@ -30,11 +30,11 @@
 
   <depend package="base/types" />
   <depend package="control/qpOASES" />
-  <depend package="control/rbdl" />
+  <depend package="control/pinocchio" />
   
   <depend package="control/kdl_parser" optional="1"/>
   <depend package="control/hyrodyn" optional="1"/>
-  <depend package="control/pinocchio" optional="1"/>  
+  <depend package="control/rbdl" optional="1"/>  
   <depend package="control/proxqp" optional="1"/>  
   <depend package="control/qpSWIFT" optional="1"/>
   <depend package="control/eiquadprog" optional="1"/>
diff --git a/scripts/full_install.sh b/scripts/full_install.sh
index 739a7529..98917026 100644
--- a/scripts/full_install.sh
+++ b/scripts/full_install.sh
@@ -91,7 +91,7 @@ make -j8 && sudo make install && cd ../..
 
 # WBC
 mkdir wbc/build && cd wbc/build
-cmake .. -DROBOT_MODEL_PINOCCHIO=ON -DROBOT_MODEL_KDL=ON -DSOLVER_PROXQP=ON -DSOLVER_EIQUADPROG=ON -DSOLVER_QPSWIFT=ON -DCMAKE_BUILD_TYPE=RELEASE
+cmake .. -DROBOT_MODEL_RBDL=ON -DROBOT_MODEL_KDL=ON -DSOLVER_PROXQP=ON -DSOLVER_EIQUADPROG=ON -DSOLVER_QPSWIFT=ON -DCMAKE_BUILD_TYPE=RELEASE
 make -j8 && sudo make install && cd ..
 
 sudo ldconfig
diff --git a/scripts/install.sh b/scripts/install.sh
index df715251..940e7c25 100644
--- a/scripts/install.sh
+++ b/scripts/install.sh
@@ -23,12 +23,11 @@ sudo apt-get -y install liburdfdom-headers-dev liburdfdom-dev
 # Clone WBC repo here to have the patches
 git clone https://github.com/ARC-OPT/wbc.git
 
-# RBDL
-git clone --branch v3.2.1 --recurse-submodules https://github.com/rbdl/rbdl.git
-cd rbdl
-git apply ../wbc/patches/rbdl.patch --ignore-whitespace
+# Pinocchio
+git clone --branch v2.6.8 --recurse-submodules https://github.com/stack-of-tasks/pinocchio.git
+cd pinocchio
 mkdir build && cd build
-cmake .. -DRBDL_BUILD_ADDON_URDFREADER=ON
+cmake .. -DBUILD_PYTHON_INTERFACE=OFF -DBUILD_UNIT_TESTS=OFF -DCMAKE_BUILD_TYPE=RELEASE
 make -j8 && sudo make install && cd ../..
 
 # If not done yet, setup a ssh key pair using the command `ssh-keygen` and add the 
diff --git a/scripts/run_tests.sh b/scripts/run_tests.sh
index 387baabd..9f025709 100644
--- a/scripts/run_tests.sh
+++ b/scripts/run_tests.sh
@@ -15,9 +15,9 @@ cd ../..
 
 # Robot Models
 echo "Testing robot models ..."
-echo "Testing RobotModelRBDL ..."
-cd robot_models/rbdl/test
-./test_robot_model_rbdl
+echo "Testing RobotModelPinocchio ..."
+cd robot_models/pinocchio/test
+./test_robot_model_pinocchio
 cd ../..
 if [ -d "kdl" ]; then
   echo "Testing RobotModelKDL ..."
@@ -25,10 +25,10 @@ if [ -d "kdl" ]; then
   ./test_robot_model_kdl
   cd ../..
 fi
-if [ -d "pinocchio" ]; then
-  echo "Testing RobotModelPinocchio ..."
-  cd pinocchio/test
-  ./test_robot_model_pinocchio
+if [ -d "rbdl" ]; then
+  echo "Testing RobotModelRBDL ..."
+  cd rbdl/test
+  ./test_robot_model_rbdl
   cd ../..
 fi
 if [ -d "hyrodyn" ]; then
diff --git a/src/core/RobotModelConfig.hpp b/src/core/RobotModelConfig.hpp
index 72ddb74d..352049a1 100644
--- a/src/core/RobotModelConfig.hpp
+++ b/src/core/RobotModelConfig.hpp
@@ -33,7 +33,7 @@ struct RobotModelConfig{
 public:
     RobotModelConfig(){
         floating_base = false;
-        type = "rbdl";
+        type = "pinocchio";
     }
     RobotModelConfig(const std::string& file_or_string,
                      const bool floating_base = false,
@@ -41,7 +41,7 @@ struct RobotModelConfig{
                      const std::string& submechanism_file = "") :
         file_or_string(file_or_string),
         submechanism_file(submechanism_file),
-        type("rbdl"),
+        type("pinocchio"),
         floating_base(floating_base),
         contact_points(contact_points){
 
@@ -59,7 +59,7 @@ struct RobotModelConfig{
     std::string file_or_string;
     /** Only Hyrodyn models: Absolute path to submechanism file, which describes the kinematic structure including parallel mechanisms.*/
     std::string submechanism_file;
-    /** Model type. Must be the exact name of one of the registered robot model plugins. See src/robot_models for all available plugins. Default is rbdl*/
+    /** Model type. Must be the exact name of one of the registered robot model plugins. See src/robot_models for all available plugins. Default is pinocchio*/
     std::string type;
     /** Optional: Attach a virtual 6 DoF floating base to the model: Naming scheme of the joints is currently fixed:
       * floating_base_trans_x, floating_base_trans_y, floating_base_trans_z,
diff --git a/src/core/test/test_core.cpp b/src/core/test/test_core.cpp
index 88bfdc9a..e00bed32 100644
--- a/src/core/test/test_core.cpp
+++ b/src/core/test/test_core.cpp
@@ -78,15 +78,15 @@ BOOST_AUTO_TEST_CASE(task_config){
 }
 
 BOOST_AUTO_TEST_CASE(robot_model_factory){
-    BOOST_CHECK_NO_THROW(PluginLoader::loadPlugin("libwbc-robot_models-rbdl.so"));
+    BOOST_CHECK_NO_THROW(PluginLoader::loadPlugin("libwbc-robot_models-pinocchio.so"));
     PluginLoader::PluginMap *plugin_map = PluginLoader::getPluginMap();
-    BOOST_CHECK(plugin_map->count("libwbc-robot_models-rbdl.so") == 1);
+    BOOST_CHECK(plugin_map->count("libwbc-robot_models-pinocchio.so") == 1);
     RobotModelFactory::RobotModelMap *robot_model_map = RobotModelFactory::getRobotModelMap();
-    BOOST_CHECK(robot_model_map->count("rbdl") == 1);
+    BOOST_CHECK(robot_model_map->count("pinocchio") == 1);
     RobotModel* model;
-    BOOST_CHECK_NO_THROW(model = RobotModelFactory::createInstance("rbdl"));
+    BOOST_CHECK_NO_THROW(model = RobotModelFactory::createInstance("pinocchio"));
     BOOST_CHECK(model != 0);
-    BOOST_CHECK_NO_THROW(PluginLoader::unloadPlugin("libwbc-robot_models-rbdl.so"));
+    BOOST_CHECK_NO_THROW(PluginLoader::unloadPlugin("libwbc-robot_models-pinocchio.so"));
 }
 
 BOOST_AUTO_TEST_CASE(qp_solver_factory){
diff --git a/src/robot_models/CMakeLists.txt b/src/robot_models/CMakeLists.txt
index e1ffbdd5..132cd746 100644
--- a/src/robot_models/CMakeLists.txt
+++ b/src/robot_models/CMakeLists.txt
@@ -1,6 +1,6 @@
-add_subdirectory(rbdl)
-if(ROBOT_MODEL_PINOCCHIO)
-    add_subdirectory(pinocchio)
+add_subdirectory(pinocchio)
+if(ROBOT_MODEL_RBDL)
+    add_subdirectory(rbdl)
 endif()
 if(ROBOT_MODEL_HYRODYN)
     add_subdirectory(hyrodyn)
diff --git a/src/robot_models/test/CMakeLists.txt b/src/robot_models/test/CMakeLists.txt
index 95ffdc29..2661fca1 100644
--- a/src/robot_models/test/CMakeLists.txt
+++ b/src/robot_models/test/CMakeLists.txt
@@ -1,3 +1,4 @@
+find_package(Boost COMPONENTS system filesystem unit_test_framework serialization REQUIRED)
 add_executable(test_model_consistency test_model_consistency.cpp test_robot_model.cpp)
 target_link_libraries(test_model_consistency
                      wbc-robot_models-hyrodyn
diff --git a/src/robot_models/test/test_model_consistency.cpp b/src/robot_models/test/test_model_consistency.cpp
index 99fb68ab..32c9ed50 100644
--- a/src/robot_models/test/test_model_consistency.cpp
+++ b/src/robot_models/test/test_model_consistency.cpp
@@ -296,12 +296,12 @@ void compareRobotModels(RobotModelConfig cfg, string tip_frame, bool verbose = f
 
 BOOST_AUTO_TEST_CASE(fixed_base){
 
-    vector<string> urdf_files = {"../../../models/rh5/urdf/rh5_single_leg.urdf",
-                                 "../../../models/kuka/urdf/kuka_iiwa.urdf",
-                                 "../../../models/rh5v2/urdf/rh5v2.urdf"};
-    vector<string> sub_mec_files = {"../../../models/rh5/hyrodyn/rh5_single_leg.yml",
-                                   "../../../models/kuka/hyrodyn/kuka_iiwa.yml",
-                                   "../../../models/rh5v2/hyrodyn/rh5v2.yml"};
+    vector<string> urdf_files = {"../../../../models/rh5/urdf/rh5_single_leg.urdf",
+                                 "../../../../models/kuka/urdf/kuka_iiwa.urdf",
+                                 "../../../../models/rh5v2/urdf/rh5v2.urdf"};
+    vector<string> sub_mec_files = {"../../../../models/rh5/hyrodyn/rh5_single_leg.yml",
+                                   "../../../../models/kuka/hyrodyn/kuka_iiwa.yml",
+                                   "../../../../models/rh5v2/hyrodyn/rh5v2.yml"};
     vector<string> tip_frames = {"LLAnkle_FT", "kuka_lbr_l_tcp", "ALWristFT_Link"};
     bool verbose = false;
 
@@ -318,10 +318,11 @@ BOOST_AUTO_TEST_CASE(fixed_base){
     }
 }
 
+/*
 BOOST_AUTO_TEST_CASE(floating_base){
 
-    string urdf_file = "../../../models/kuka/urdf/kuka_iiwa.urdf";
-    string sub_mec_file = "../../../models/kuka/hyrodyn/kuka_iiwa_floating_base.yml";
+    string urdf_file = "../../../../models/kuka/urdf/kuka_iiwa.urdf";
+    string sub_mec_file = "../../../../models/kuka/hyrodyn/kuka_iiwa_floating_base.yml";
     string tip_frame = "kuka_lbr_l_tcp";
     bool verbose = false;
 
@@ -331,3 +332,4 @@ BOOST_AUTO_TEST_CASE(floating_base){
 
     compareRobotModels(cfg, tip_frame, verbose);
 }
+*/
diff --git a/src/scenes/acceleration/test/CMakeLists.txt b/src/scenes/acceleration/test/CMakeLists.txt
index 86e93639..5ed48b06 100644
--- a/src/scenes/acceleration/test/CMakeLists.txt
+++ b/src/scenes/acceleration/test/CMakeLists.txt
@@ -2,6 +2,6 @@ find_package(Boost COMPONENTS system filesystem unit_test_framework serializatio
 add_executable(test_acceleration_scene test_acceleration_scene.cpp)
 target_link_libraries(test_acceleration_scene
                       wbc-scenes-acceleration
-                      wbc-robot_models-rbdl
+                      wbc-robot_models-pinocchio
                       wbc-solvers-qpoases
                       Boost::unit_test_framework)
diff --git a/src/scenes/acceleration/test/test_acceleration_scene.cpp b/src/scenes/acceleration/test/test_acceleration_scene.cpp
index e2990dbe..98b64ff8 100644
--- a/src/scenes/acceleration/test/test_acceleration_scene.cpp
+++ b/src/scenes/acceleration/test/test_acceleration_scene.cpp
@@ -1,7 +1,7 @@
 #define BOOST_TEST_DYN_LINK
 #define BOOST_TEST_MAIN
 #include <boost/test/unit_test.hpp>
-#include "robot_models/rbdl/RobotModelRBDL.hpp"
+#include "robot_models/pinocchio/RobotModelPinocchio.hpp"
 #include "scenes/acceleration/AccelerationScene.hpp"
 #include "solvers/qpoases/QPOasesSolver.hpp"
 
@@ -15,7 +15,7 @@ BOOST_AUTO_TEST_CASE(simple_test){
      */
 
     // Configure Robot model
-    shared_ptr<RobotModelRBDL> robot_model = make_shared<RobotModelRBDL>();
+    shared_ptr<RobotModelPinocchio> robot_model = make_shared<RobotModelPinocchio>();
     RobotModelConfig config;
     config.file_or_string = "../../../../../models/kuka/urdf/kuka_iiwa.urdf";
     BOOST_CHECK_EQUAL(robot_model->configure(config), true);
diff --git a/src/scenes/acceleration_reduced_tsid/test/CMakeLists.txt b/src/scenes/acceleration_reduced_tsid/test/CMakeLists.txt
index 16b33a42..13b0404d 100644
--- a/src/scenes/acceleration_reduced_tsid/test/CMakeLists.txt
+++ b/src/scenes/acceleration_reduced_tsid/test/CMakeLists.txt
@@ -2,6 +2,6 @@ find_package(Boost COMPONENTS system filesystem unit_test_framework serializatio
 add_executable(test_acceleration_scene_reduced_tsid test_acceleration_scene_reduced_tsid.cpp)
 target_link_libraries(test_acceleration_scene_reduced_tsid
                       wbc-scenes-acceleration_reduced_tsid
-                      wbc-robot_models-rbdl
+                      wbc-robot_models-pinocchio
                       wbc-solvers-qpoases
                       Boost::unit_test_framework)
diff --git a/src/scenes/acceleration_reduced_tsid/test/test_acceleration_scene_reduced_tsid.cpp b/src/scenes/acceleration_reduced_tsid/test/test_acceleration_scene_reduced_tsid.cpp
index e3cb9621..a89ca7ec 100644
--- a/src/scenes/acceleration_reduced_tsid/test/test_acceleration_scene_reduced_tsid.cpp
+++ b/src/scenes/acceleration_reduced_tsid/test/test_acceleration_scene_reduced_tsid.cpp
@@ -1,7 +1,7 @@
 #define BOOST_TEST_DYN_LINK
 #define BOOST_TEST_MAIN
 #include <boost/test/unit_test.hpp>
-#include "robot_models/rbdl/RobotModelRBDL.hpp"
+#include "robot_models/pinocchio/RobotModelPinocchio.hpp"
 #include "scenes/acceleration_reduced_tsid/AccelerationSceneReducedTSID.hpp"
 #include "solvers/qpoases/QPOasesSolver.hpp"
 
@@ -15,7 +15,7 @@ BOOST_AUTO_TEST_CASE(simple_test){
      */
 
     // Configure Robot model
-    shared_ptr<RobotModelRBDL> robot_model = make_shared<RobotModelRBDL>();
+    shared_ptr<RobotModelPinocchio> robot_model = make_shared<RobotModelPinocchio>();
     RobotModelConfig config;
     config.file_or_string = "../../../../../models/rh5/urdf/rh5_legs.urdf";
     config.floating_base = true;
diff --git a/src/scenes/acceleration_tsid/test/CMakeLists.txt b/src/scenes/acceleration_tsid/test/CMakeLists.txt
index 541d5820..1cd2c9c8 100644
--- a/src/scenes/acceleration_tsid/test/CMakeLists.txt
+++ b/src/scenes/acceleration_tsid/test/CMakeLists.txt
@@ -2,6 +2,6 @@ find_package(Boost COMPONENTS system filesystem unit_test_framework serializatio
 add_executable(test_acceleration_scene_tsid test_acceleration_scene_tsid.cpp)
 target_link_libraries(test_acceleration_scene_tsid
                       wbc-scenes-acceleration_tsid
-                      wbc-robot_models-rbdl
+                      wbc-robot_models-pinocchio
                       wbc-solvers-qpoases
                       Boost::unit_test_framework)
diff --git a/src/scenes/acceleration_tsid/test/test_acceleration_scene_tsid.cpp b/src/scenes/acceleration_tsid/test/test_acceleration_scene_tsid.cpp
index 40bda2d6..51338f9a 100644
--- a/src/scenes/acceleration_tsid/test/test_acceleration_scene_tsid.cpp
+++ b/src/scenes/acceleration_tsid/test/test_acceleration_scene_tsid.cpp
@@ -1,7 +1,7 @@
 #define BOOST_TEST_DYN_LINK
 #define BOOST_TEST_MAIN
 #include <boost/test/unit_test.hpp>
-#include "robot_models/rbdl/RobotModelRBDL.hpp"
+#include "robot_models/pinocchio/RobotModelPinocchio.hpp"
 #include "scenes/acceleration_tsid/AccelerationSceneTSID.hpp"
 #include "solvers/qpoases/QPOasesSolver.hpp"
 
@@ -15,7 +15,7 @@ BOOST_AUTO_TEST_CASE(simple_test){
      */
 
     // Configure Robot model
-    shared_ptr<RobotModelRBDL> robot_model = make_shared<RobotModelRBDL>();
+    shared_ptr<RobotModelPinocchio> robot_model = make_shared<RobotModelPinocchio>();
     RobotModelConfig config;
     config.file_or_string = "../../../../../models/rh5/urdf/rh5_legs.urdf";
     config.floating_base = true;
diff --git a/src/scenes/velocity/test/CMakeLists.txt b/src/scenes/velocity/test/CMakeLists.txt
index d500eba2..4173c66d 100644
--- a/src/scenes/velocity/test/CMakeLists.txt
+++ b/src/scenes/velocity/test/CMakeLists.txt
@@ -2,7 +2,7 @@ find_package(Boost COMPONENTS system filesystem unit_test_framework serializatio
 add_executable(test_velocity_scene test_velocity_scene.cpp)
 target_link_libraries(test_velocity_scene
                       wbc-scenes-velocity
-                      wbc-robot_models-rbdl
+                      wbc-robot_models-pinocchio
                       wbc-solvers-hls
                       Boost::unit_test_framework)
 
diff --git a/src/scenes/velocity/test/test_velocity_scene.cpp b/src/scenes/velocity/test/test_velocity_scene.cpp
index d015ab5f..c5a27ac5 100644
--- a/src/scenes/velocity/test/test_velocity_scene.cpp
+++ b/src/scenes/velocity/test/test_velocity_scene.cpp
@@ -1,7 +1,7 @@
 #define BOOST_TEST_DYN_LINK
 #define BOOST_TEST_MAIN
 #include <boost/test/unit_test.hpp>
-#include "robot_models/rbdl/RobotModelRBDL.hpp"
+#include "robot_models/pinocchio/RobotModelPinocchio.hpp"
 #include "scenes/velocity/VelocityScene.hpp"
 #include "solvers/hls/HierarchicalLSSolver.hpp"
 
@@ -16,7 +16,7 @@ BOOST_AUTO_TEST_CASE(configuration_test){
 
     TaskConfig cart_task("cart_pos_ctrl_left", 0, "kuka_lbr_l_link_0", "kuka_lbr_l_tcp", "kuka_lbr_l_link_0");
 
-    shared_ptr<RobotModelRBDL> robot_model = make_shared<RobotModelRBDL>();
+    shared_ptr<RobotModelPinocchio> robot_model = make_shared<RobotModelPinocchio>();
     RobotModelConfig config;
     config.file_or_string = "../../../../../models/kuka/urdf/kuka_iiwa.urdf";
     BOOST_CHECK_EQUAL(robot_model->configure(config), true);
@@ -47,7 +47,7 @@ BOOST_AUTO_TEST_CASE(simple_test){
      */
 
     // Configure Robot model
-    shared_ptr<RobotModelRBDL> robot_model = make_shared<RobotModelRBDL>();
+    shared_ptr<RobotModelPinocchio> robot_model = make_shared<RobotModelPinocchio>();
     RobotModelConfig config;
     config.file_or_string = "../../../../../models/kuka/urdf/kuka_iiwa.urdf";
     BOOST_CHECK(robot_model->configure(config));
diff --git a/src/scenes/velocity_qp/test/CMakeLists.txt b/src/scenes/velocity_qp/test/CMakeLists.txt
index f5a89921..ddd621c7 100644
--- a/src/scenes/velocity_qp/test/CMakeLists.txt
+++ b/src/scenes/velocity_qp/test/CMakeLists.txt
@@ -2,7 +2,7 @@ find_package(Boost COMPONENTS system filesystem unit_test_framework serializatio
 add_executable(test_velocity_scene_quadratic_cost test_velocity_scene_quadratic_cost.cpp)
 target_link_libraries(test_velocity_scene_quadratic_cost
                       wbc-scenes-velocity_qp
-                      wbc-robot_models-rbdl
+                      wbc-robot_models-pinocchio
                       wbc-solvers-qpoases
                       Boost::unit_test_framework)
 
diff --git a/src/scenes/velocity_qp/test/test_velocity_scene_quadratic_cost.cpp b/src/scenes/velocity_qp/test/test_velocity_scene_quadratic_cost.cpp
index da3a23d7..9bc9f2c2 100644
--- a/src/scenes/velocity_qp/test/test_velocity_scene_quadratic_cost.cpp
+++ b/src/scenes/velocity_qp/test/test_velocity_scene_quadratic_cost.cpp
@@ -1,7 +1,7 @@
 #define BOOST_TEST_DYN_LINK
 #define BOOST_TEST_MAIN
 #include <boost/test/unit_test.hpp>
-#include "robot_models/rbdl/RobotModelRBDL.hpp"
+#include "robot_models/pinocchio/RobotModelPinocchio.hpp"
 #include "scenes/velocity_qp/VelocitySceneQP.hpp"
 #include "solvers/qpoases/QPOasesSolver.hpp"
 
@@ -15,7 +15,7 @@ BOOST_AUTO_TEST_CASE(simple_test){
      */
 
     // Configure Robot model
-    shared_ptr<RobotModelRBDL> robot_model = make_shared<RobotModelRBDL>();
+    shared_ptr<RobotModelPinocchio> robot_model = make_shared<RobotModelPinocchio>();
     RobotModelConfig config;
     config.file_or_string = "../../../../../models/rh5/urdf/rh5_legs.urdf";
     config.floating_base = true;
diff --git a/tutorials/kuka_iiwa/CMakeLists.txt b/tutorials/kuka_iiwa/CMakeLists.txt
index d486fd77..50208ea3 100644
--- a/tutorials/kuka_iiwa/CMakeLists.txt
+++ b/tutorials/kuka_iiwa/CMakeLists.txt
@@ -2,26 +2,26 @@ add_executable(cart_pos_ctrl_hls cart_pos_ctrl_hls.cpp)
 target_link_libraries(cart_pos_ctrl_hls
                       wbc-solvers-hls
                       wbc-scenes-velocity
-                      wbc-robot_models-rbdl
+                      wbc-robot_models-pinocchio
                       wbc-controllers)
 
 add_executable(cart_pos_ctrl_qpoases cart_pos_ctrl_qpoases.cpp)
 target_link_libraries(cart_pos_ctrl_qpoases
                       wbc-solvers-qpoases
                       wbc-scenes-velocity_qp
-                      wbc-robot_models-rbdl
+                      wbc-robot_models-pinocchio
                       wbc-controllers)
 
 add_executable(cart_pos_ctrl_hls_weights cart_pos_ctrl_hls_weights.cpp)
 target_link_libraries(cart_pos_ctrl_hls_weights
                       wbc-solvers-hls
                       wbc-scenes-velocity
-                      wbc-robot_models-rbdl
+                      wbc-robot_models-pinocchio
                       wbc-controllers)
 
 add_executable(cart_pos_ctrl_hls_hierarchies cart_pos_ctrl_hls_hierarchies.cpp)
 target_link_libraries(cart_pos_ctrl_hls_hierarchies
                       wbc-solvers-hls
                       wbc-scenes-velocity
-                      wbc-robot_models-rbdl
+                      wbc-robot_models-pinocchio
                       wbc-controllers)
diff --git a/tutorials/kuka_iiwa/cart_pos_ctrl_hls.cpp b/tutorials/kuka_iiwa/cart_pos_ctrl_hls.cpp
index df0e4967..1ce93825 100644
--- a/tutorials/kuka_iiwa/cart_pos_ctrl_hls.cpp
+++ b/tutorials/kuka_iiwa/cart_pos_ctrl_hls.cpp
@@ -1,5 +1,5 @@
 #include <core/RobotModelConfig.hpp>
-#include <robot_models/rbdl/RobotModelRBDL.hpp>
+#include <robot_models/pinocchio/RobotModelPinocchio.hpp>
 #include <scenes/velocity/VelocityScene.hpp>
 #include <solvers/hls/HierarchicalLSSolver.hpp>
 #include <controllers/CartesianPosPDController.hpp>
@@ -37,7 +37,7 @@ using namespace wbc;
 int main(){
 
     // Create a robot model. Each robot model is derived from a common RobotModel class, as it will be passed to the WBC scene later on.
-    RobotModelPtr robot_model = make_shared<RobotModelRBDL>();
+    RobotModelPtr robot_model = make_shared<RobotModelPinocchio>();
 
     // Configure the robot model by passing the RobotModelConfig. The simplest configuration can by obtained by only setting
     // the path to the URDF file. In doing so, WBC will assume:
diff --git a/tutorials/kuka_iiwa/cart_pos_ctrl_hls_hierarchies.cpp b/tutorials/kuka_iiwa/cart_pos_ctrl_hls_hierarchies.cpp
index e86005ed..cbb31c17 100644
--- a/tutorials/kuka_iiwa/cart_pos_ctrl_hls_hierarchies.cpp
+++ b/tutorials/kuka_iiwa/cart_pos_ctrl_hls_hierarchies.cpp
@@ -1,4 +1,4 @@
-#include <robot_models/rbdl/RobotModelRBDL.hpp>
+#include <robot_models/pinocchio/RobotModelPinocchio.hpp>
 #include <core/RobotModelConfig.hpp>
 #include <scenes/velocity/VelocityScene.hpp>
 #include <solvers/hls/HierarchicalLSSolver.hpp>
@@ -15,7 +15,7 @@ using namespace wbc;
  */
 int main(int argc, char** argv){
 
-    RobotModelPtr robot_model = make_shared<RobotModelRBDL>();
+    RobotModelPtr robot_model = make_shared<RobotModelPinocchio>();
 
     // Configure the robot model by passing the RobotModelConfig. The simplest configuration can by obtained by only setting
     // the path to the URDF file. In doing so, WBC will assume:
diff --git a/tutorials/kuka_iiwa/cart_pos_ctrl_hls_weights.cpp b/tutorials/kuka_iiwa/cart_pos_ctrl_hls_weights.cpp
index a11f1fc6..366a2026 100644
--- a/tutorials/kuka_iiwa/cart_pos_ctrl_hls_weights.cpp
+++ b/tutorials/kuka_iiwa/cart_pos_ctrl_hls_weights.cpp
@@ -1,4 +1,4 @@
-#include <robot_models/rbdl/RobotModelRBDL.hpp>
+#include <robot_models/pinocchio/RobotModelPinocchio.hpp>
 #include <core/RobotModelConfig.hpp>
 #include <scenes/velocity/VelocityScene.hpp>
 #include <solvers/hls/HierarchicalLSSolver.hpp>
@@ -14,7 +14,7 @@ using namespace wbc;
  */
 int main(){
 
-    RobotModelPtr robot_model = make_shared<RobotModelRBDL>();
+    RobotModelPtr robot_model = make_shared<RobotModelPinocchio>();
 
     // Configure the robot model by passing the RobotModelConfig. The simplest configuration can by obtained by only setting
     // the path to the URDF file. In doing so, WBC will assume:
diff --git a/tutorials/kuka_iiwa/cart_pos_ctrl_qpoases.cpp b/tutorials/kuka_iiwa/cart_pos_ctrl_qpoases.cpp
index 813b03f9..55f02319 100644
--- a/tutorials/kuka_iiwa/cart_pos_ctrl_qpoases.cpp
+++ b/tutorials/kuka_iiwa/cart_pos_ctrl_qpoases.cpp
@@ -1,4 +1,4 @@
-#include <robot_models/rbdl/RobotModelRBDL.hpp>
+#include <robot_models/pinocchio/RobotModelPinocchio.hpp>
 #include <core/RobotModelConfig.hpp>
 #include <scenes/velocity_qp/VelocitySceneQP.hpp>
 #include <solvers/qpoases/QPOasesSolver.hpp>
@@ -39,7 +39,7 @@ int main(){
     double dt = 0.01;
 
     // Create a robot model. Each robot model is derived from a common RobotModel class, as it will be passed to the WBC scene later on.
-    RobotModelPtr robot_model = make_shared<RobotModelRBDL>();
+    RobotModelPtr robot_model = make_shared<RobotModelPinocchio>();
 
     // Configure the robot model by passing the RobotModelConfig. The simplest configuration can by obtained by only setting
     // the path to the URDF file. In doing so, WBC will assume:
@@ -110,6 +110,7 @@ int main(){
     while((setpoint.pose.position - feedback.pose.position).norm() > 1e-4){
 
         // Update the robot model. WBC will only work if at least one joint state with valid timestamp has been passed to the robot model
+        auto s = std::chrono::high_resolution_clock::now();
         robot_model->update(joint_state);
 
         // Update controller. The feedback is the pose of the tip link described in ref_frame link
@@ -125,7 +126,6 @@ int main(){
 
         // Solve the QP. The output is the joint velocity that achieves the task space velocity demanded by the controller, i.e.,
         // this joint velocity will drive the end effector to the reference x_r
-        auto s = std::chrono::high_resolution_clock::now();
         solver_output = scene.solve(hqp);
         auto e = std::chrono::high_resolution_clock::now();
         double solve_time = std::chrono::duration_cast<std::chrono::microseconds>(e-s).count();
diff --git a/tutorials/rh5/CMakeLists.txt b/tutorials/rh5/CMakeLists.txt
index 0a8cc810..209e5aa0 100644
--- a/tutorials/rh5/CMakeLists.txt
+++ b/tutorials/rh5/CMakeLists.txt
@@ -3,14 +3,14 @@ target_link_libraries(rh5_single_leg
                       wbc-solvers-qpoases
                       wbc-controllers
                       wbc-scenes-velocity_qp
-                      wbc-robot_models-rbdl)
+                      wbc-robot_models-pinocchio)
 
 add_executable(rh5_legs_floating_base rh5_legs_floating_base.cpp)
 target_link_libraries(rh5_legs_floating_base
                       wbc-solvers-qpoases
                       wbc-scenes-velocity_qp
                       wbc-controllers
-                      wbc-robot_models-rbdl)
+                      wbc-robot_models-pinocchio)
 
 if(ROBOT_MODEL_HYRODYN)
    add_executable(rh5_single_leg_hybrid rh5_single_leg_hybrid.cpp)
diff --git a/tutorials/rh5/rh5_legs_floating_base.cpp b/tutorials/rh5/rh5_legs_floating_base.cpp
index b0bf0a50..74d33368 100644
--- a/tutorials/rh5/rh5_legs_floating_base.cpp
+++ b/tutorials/rh5/rh5_legs_floating_base.cpp
@@ -1,10 +1,11 @@
 #include <solvers/qpoases/QPOasesSolver.hpp>
-#include <robot_models/rbdl/RobotModelRBDL.hpp>
+#include <robot_models/pinocchio/RobotModelPinocchio.hpp>
 #include <core/RobotModelConfig.hpp>
 #include <scenes/velocity_qp/VelocitySceneQP.hpp>
 #include <controllers/CartesianPosPDController.hpp>
 #include <tools/JointIntegrator.hpp>
 #include <unistd.h>
+#include <chrono>
 
 using namespace wbc;
 using namespace std;
@@ -44,7 +45,7 @@ int main(){
     double dt = 0.001;
 
     // Create robot model
-    RobotModelPtr robot_model = std::make_shared<RobotModelRBDL>();
+    RobotModelPtr robot_model = std::make_shared<RobotModelPinocchio>();
 
     // Configure a serial robot model with floating base and two contact points: {"LLAnkle_FT", "LRAnkle_FT"}.
     // Note that the joint names have to contain {"floating_base_trans_x", "floating_base_trans_y", "floating_base_trans_z",
@@ -126,6 +127,9 @@ int main(){
     base::commands::Joints solver_output;
     while((setpoint.pose.position - feedback.pose.position).norm() > 1e-4){
 
+
+        auto s = std::chrono::high_resolution_clock::now();
+
         // Update the robot model. WBC will only work if at least one joint state with valid timestamp has been passed to the robot model.
         // Note that you have to pass the floating base state as well now!
         robot_model->update(joint_state, floating_base_state);
@@ -151,6 +155,10 @@ int main(){
             joint_state[i].speed = solver_output[i].speed;
         }
 
+
+        auto e = std::chrono::high_resolution_clock::now();
+        double solve_time = std::chrono::duration_cast<std::chrono::microseconds>(e-s).count();
+
         // Update floating base pose, use an over-simplistic estimation
         base::Pose t1 = robot_model->rigidBodyState("world", "RH5_Root_Link").pose;
         base::Pose t2 = robot_model->rigidBodyState("world", "LLAnkle_FT").pose;
@@ -166,6 +174,7 @@ int main(){
         cout<<"Solver output: "; cout<<endl;
         cout<<"Joint Names:   "; for(uint i = 0; i < nj; i++) cout<<solver_output.names[i]<<" "; cout<<endl;
         cout<<"Velocity:      "; for(uint i = 0; i < nj; i++) cout<<solver_output[i].speed<<" "; cout<<endl;
+        cout<<"solve time:    " << solve_time << " (mu s)" << endl;
         cout<<"---------------------------------------------------------------------------------------------"<<endl<<endl;
 
         usleep(loop_time * 1e6);
diff --git a/tutorials/rh5/rh5_single_leg.cpp b/tutorials/rh5/rh5_single_leg.cpp
index 92159f6c..163ea21e 100644
--- a/tutorials/rh5/rh5_single_leg.cpp
+++ b/tutorials/rh5/rh5_single_leg.cpp
@@ -1,5 +1,5 @@
 #include <solvers/qpoases/QPOasesSolver.hpp>
-#include <robot_models/rbdl/RobotModelRBDL.hpp>
+#include <robot_models/pinocchio/RobotModelPinocchio.hpp>
 #include <core/RobotModelConfig.hpp>
 #include <scenes/velocity_qp/VelocitySceneQP.hpp>
 #include <controllers/CartesianPosPDController.hpp>
@@ -39,7 +39,7 @@ int main(){
     double dt = 0.001;
 
     // Create Hyrodyn based robot model.  In this case, we use the Hyrodyn-based model, which allows handling of parallel mechanisms.
-    RobotModelPtr robot_model = make_shared<RobotModelRBDL>();
+    RobotModelPtr robot_model = make_shared<RobotModelPinocchio>();
 
     // Configure the model. We pass a serial model description to RobotModelHyrodyn, which means that the solution will be identical
     // to the one obtained by  RobotModelPinocchio. RobotModelHyrodyn assumes that you specify correctly all joints (actuated and unactuated).
diff --git a/tutorials/rh5/rh5_single_leg_hybrid.cpp b/tutorials/rh5/rh5_single_leg_hybrid.cpp
index dab5514c..90abfdf0 100644
--- a/tutorials/rh5/rh5_single_leg_hybrid.cpp
+++ b/tutorials/rh5/rh5_single_leg_hybrid.cpp
@@ -23,7 +23,7 @@ int main(){
     double dt = 0.001;
 
     // Create Hyrodyn based robot model.  In this case, we use the Hyrodyn-based model, which allows handling of parallel mechanisms.
-    RobotModelPtr robot_model = make_shared<RobotModelHyrodyn>();
+    RobotModelPtr robot_model = make_shared<RobotModelPinocchio>();
 
     // Configure the full robot model, containing all linear actuators and parallel mechanisms.
     RobotModelConfig config;
diff --git a/tutorials/rh5v2/CMakeLists.txt b/tutorials/rh5v2/CMakeLists.txt
index 710d386a..0b736d1d 100644
--- a/tutorials/rh5v2/CMakeLists.txt
+++ b/tutorials/rh5v2/CMakeLists.txt
@@ -3,7 +3,7 @@ target_link_libraries(rh5v2
                       wbc-solvers-qpoases
                       wbc-scenes-acceleration_tsid
                       wbc-controllers
-                      wbc-robot_models-rbdl)
+                      wbc-robot_models-pinocchio)
 
 if(ROBOT_MODEL_HYRODYN)
    add_executable(rh5v2_hybrid rh5v2_hybrid.cpp)
diff --git a/tutorials/rh5v2/rh5v2.cpp b/tutorials/rh5v2/rh5v2.cpp
index 7b3fa008..169228da 100644
--- a/tutorials/rh5v2/rh5v2.cpp
+++ b/tutorials/rh5v2/rh5v2.cpp
@@ -1,4 +1,4 @@
-#include <robot_models/rbdl/RobotModelRBDL.hpp>
+#include <robot_models/pinocchio/RobotModelPinocchio.hpp>
 #include <solvers/qpoases/QPOasesSolver.hpp>
 #include <scenes/acceleration_tsid/AccelerationSceneTSID.hpp>
 #include <tools/JointIntegrator.hpp>
@@ -45,7 +45,7 @@ int main()
     double dt = 0.01;
 
     // Create robot model, use hyrodyn-based model in this case
-    RobotModelPtr robot_model = make_shared<RobotModelRBDL>();
+    RobotModelPtr robot_model = make_shared<RobotModelPinocchio>();
 
     // Configure robot model. We configure a serial model without parallel mechanisms.
     // Blacklist the finger joints, as they are not relevant and may slow down the solver