[benchmark] Added benchmark of atomic computations with geometry objects such...

[benchmark] Added benchmark of atomic computations with geometry objects such as updating global positions, determine if two objects are in collision, or compute the distance between two objects

CMakeLists.txt

@@ -67,7 +67,7 @@ IF(HPP_FCL_FOUND AND URDFDOM_FOUND)
   ADD_REQUIRED_DEPENDENCY("assimp")
 ENDIF(HPP_FCL_FOUND AND URDFDOM_FOUND)
 
-SET(BOOST_COMPONENTS filesystem unit_test_framework)
+SET(BOOST_COMPONENTS filesystem unit_test_framework system)
 SEARCH_FOR_BOOST()
 # Path to boost headers
 INCLUDE_DIRECTORIES(${Boost_INCLUDE_DIRS})

benchmark/CMakeLists.txt

 # ----------------------------------------------------
 # --- BENCHMARK --------------------------------------
 # ----------------------------------------------------
+MACRO(ADD_TEST_CFLAGS target flag)
+  SET_PROPERTY(TARGET ${target} APPEND_STRING PROPERTY COMPILE_FLAGS " ${flag}")
+ENDMACRO(ADD_TEST_CFLAGS)
 
 ADD_EXECUTABLE(timings timings.cpp)
 PKG_CONFIG_USE_DEPENDENCY(timings eigen3)
-PKG_CONFIG_USE_DEPENDENCY(timings urdfdom)
-TARGET_LINK_LIBRARIES(timings ${PROJECT_NAME})
+IF(URDFDOM_FOUND)
+  PKG_CONFIG_USE_DEPENDENCY(timings urdfdom)
+  PKG_CONFIG_USE_DEPENDENCY(timings assimp)
+ENDIF(URDFDOM_FOUND)
+IF(HPP_FCL_FOUND)
+	PKG_CONFIG_USE_DEPENDENCY(timings hpp-fcl)
+	ADD_TEST_CFLAGS(timings "-DWITH_HPP_FCL")
+ENDIF(HPP_FCL_FOUND)
+TARGET_LINK_LIBRARIES (timings ${Boost_LIBRARIES} ${PROJECT_NAME})
 
 SET_TARGET_PROPERTIES (timings PROPERTIES COMPILE_DEFINITIONS PINOCCHIO_SOURCE_DIR="${${PROJECT_NAME}_SOURCE_DIR}")

benchmark/timings.cpp

@@ -14,6 +14,11 @@
 #include "pinocchio/multibody/parser/urdf.hpp"
 #include "pinocchio/multibody/parser/sample-models.hpp"
 
+#ifdef WITH_HPP_FCL
+#include "pinocchio/multibody/geometry.hpp"
+#include "pinocchio/multibody/parser/urdf-with-geometry.hpp"
+#endif
+
 #include <iostream>
 
 #include "pinocchio/tools/timer.hpp"
@@ -137,6 +142,48 @@ int main(int argc, const char ** argv)
   }
   std::cout << "Geometry = \t"; timer.toc(std::cout,NBT);
 
+#ifdef WITH_HPP_FCL
+  std::string romeo_filename = PINOCCHIO_SOURCE_DIR"/models/romeo.urdf";
+  std::string romeo_meshDir  = PINOCCHIO_SOURCE_DIR"/models/";
+  std::pair < Model, GeometryModel > romeo = se3::urdf::buildModelAndGeom(romeo_filename, romeo_meshDir, se3::JointModelFreeFlyer());
+  se3::Model romeo_model = romeo.first;
+  se3::GeometryModel romeo_model_geom = romeo.second;
+  Data romeo_data(romeo_model);
+  GeometryData romeo_data_geom(romeo_data, romeo_model_geom);
+
+
+  timer.tic();
+  SMOOTH(NBT)
+  {
+    geometry(romeo_model,romeo_data,qs[_smooth]);
+  }
+  double geom_time = timer.toc(StackTicToc::US)/NBT;
+
+  timer.tic();
+  SMOOTH(NBT)
+  {
+    se3::geometry(romeo_model, romeo_data, qs[_smooth]);
+    updateCollisionGeometry(romeo_model,romeo_data,romeo_model_geom,romeo_data_geom,qs[_smooth]);
+  }
+  std::cout << "Update Collision Geometry = \t";
+  std::cout << timer.toc(StackTicToc::US)/NBT - geom_time
+                       << StackTicToc::unitName(StackTicToc::US) << std::endl;
+
+  timer.tic();
+  SMOOTH(NBT)
+  {
+    romeo_data_geom.collide(1,10);
+  }
+  std::cout << "Collision Test between two geometry objects = \t"; timer.toc(std::cout,NBT);
+
+  timer.tic();
+  SMOOTH(NBT)
+  {
+    romeo_data_geom.computeDistance(1, 10);
+  }
+  std::cout << "Compute Distance between two geometry objects = \t"; timer.toc(std::cout,NBT);
+#endif
+
   timer.tic();
   SMOOTH(NBT)
   {