diff --git a/models/simple_humanoid.urdf b/models/simple_humanoid.urdf
index 4a1c6621a0..7f8a7175df 100644
--- a/models/simple_humanoid.urdf
+++ b/models/simple_humanoid.urdf
@@ -17,10 +17,16 @@
+
+
+
+
+
+
diff --git a/models/simple_humanoid_description/box.stl b/models/simple_humanoid_description/box.stl
new file mode 100644
index 0000000000..6e11e77274
--- /dev/null
+++ b/models/simple_humanoid_description/box.stl
@@ -0,0 +1,86 @@
+solid Exported from Blender-2.78 (sub 0)
+facet normal -1.000000 0.000000 0.000000
+outer loop
+vertex -0.500000 -0.500000 -0.500000
+vertex -0.500000 -0.500000 0.500000
+vertex -0.500000 0.500000 0.500000
+endloop
+endfacet
+facet normal -1.000000 0.000000 0.000000
+outer loop
+vertex -0.500000 0.500000 0.500000
+vertex -0.500000 0.500000 -0.500000
+vertex -0.500000 -0.500000 -0.500000
+endloop
+endfacet
+facet normal 0.000000 1.000000 0.000000
+outer loop
+vertex -0.500000 0.500000 -0.500000
+vertex -0.500000 0.500000 0.500000
+vertex 0.500000 0.500000 0.500000
+endloop
+endfacet
+facet normal 0.000000 1.000000 0.000000
+outer loop
+vertex 0.500000 0.500000 0.500000
+vertex 0.500000 0.500000 -0.500000
+vertex -0.500000 0.500000 -0.500000
+endloop
+endfacet
+facet normal 1.000000 0.000000 0.000000
+outer loop
+vertex 0.500000 0.500000 -0.500000
+vertex 0.500000 0.500000 0.500000
+vertex 0.500000 -0.500000 0.500000
+endloop
+endfacet
+facet normal 1.000000 0.000000 0.000000
+outer loop
+vertex 0.500000 -0.500000 0.500000
+vertex 0.500000 -0.500000 -0.500000
+vertex 0.500000 0.500000 -0.500000
+endloop
+endfacet
+facet normal 0.000000 -1.000000 0.000000
+outer loop
+vertex -0.500000 -0.500000 0.500000
+vertex -0.500000 -0.500000 -0.500000
+vertex 0.500000 -0.500000 -0.500000
+endloop
+endfacet
+facet normal 0.000000 -1.000000 0.000000
+outer loop
+vertex 0.500000 -0.500000 -0.500000
+vertex 0.500000 -0.500000 0.500000
+vertex -0.500000 -0.500000 0.500000
+endloop
+endfacet
+facet normal 0.000000 0.000000 -1.000000
+outer loop
+vertex 0.500000 -0.500000 -0.500000
+vertex -0.500000 -0.500000 -0.500000
+vertex -0.500000 0.500000 -0.500000
+endloop
+endfacet
+facet normal 0.000000 0.000000 -1.000000
+outer loop
+vertex -0.500000 0.500000 -0.500000
+vertex 0.500000 0.500000 -0.500000
+vertex 0.500000 -0.500000 -0.500000
+endloop
+endfacet
+facet normal 0.000000 0.000000 1.000000
+outer loop
+vertex 0.500000 0.500000 0.500000
+vertex -0.500000 0.500000 0.500000
+vertex -0.500000 -0.500000 0.500000
+endloop
+endfacet
+facet normal 0.000000 0.000000 1.000000
+outer loop
+vertex -0.500000 -0.500000 0.500000
+vertex 0.500000 -0.500000 0.500000
+vertex 0.500000 0.500000 0.500000
+endloop
+endfacet
+endsolid Exported from Blender-2.78 (sub 0)
diff --git a/unittest/urdf.cpp b/unittest/urdf.cpp
index 93cbd3d93a..808f56383d 100644
--- a/unittest/urdf.cpp
+++ b/unittest/urdf.cpp
@@ -9,6 +9,10 @@
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/parsers/urdf.hpp"
+#ifdef PINOCCHIO_WITH_HPP_FCL
+#include
+#endif // PINOCCHIO_WITH_HPP_FCL
+
#include
#include
@@ -32,11 +36,31 @@ BOOST_AUTO_TEST_CASE ( build_model )
BOOST_AUTO_TEST_CASE ( build_model_simple_humanoid )
{
const std::string filename = PINOCCHIO_MODEL_DIR + std::string("/simple_humanoid.urdf");
+ const std::string dir = PINOCCHIO_MODEL_DIR;
pinocchio::Model model;
pinocchio::urdf::buildModel(filename, model);
- BOOST_CHECK(model.nq == 29);
+ BOOST_CHECK_EQUAL(model.nq, 29);
+
+ // Check that parsing collision_checking works.
+ pinocchio::GeometryModel geomModel;
+ pinocchio::urdf::buildGeom(model, filename, pinocchio::COLLISION, geomModel, dir);
+ BOOST_CHECK_EQUAL(geomModel.ngeoms, 2);
+#ifdef PINOCCHIO_WITH_HPP_FCL
+# if ( HPP_FCL_MAJOR_VERSION>1 || ( HPP_FCL_MAJOR_VERSION==1 && \
+ ( HPP_FCL_MINOR_VERSION>1 || ( HPP_FCL_MINOR_VERSION==1 && \
+ HPP_FCL_PATCH_VERSION>3))))
+# define PINOCCHIO_HPP_FCL_SUPERIOR_TO_1_1_3
+#endif
+ BOOST_CHECK_EQUAL(geomModel.geometryObjects[0].fcl->getNodeType(), hpp::fcl::GEOM_CAPSULE);
+#ifdef PINOCCHIO_HPP_FCL_SUPERIOR_TO_1_1_3
+ BOOST_CHECK_EQUAL(geomModel.geometryObjects[1].fcl->getNodeType(), hpp::fcl::GEOM_CONVEX);
+#undef PINOCCHIO_HPP_FCL_SUPERIOR_TO_1_1_3
+#else
+ BOOST_CHECK_EQUAL(geomModel.geometryObjects[1].fcl->getObjectType(), hpp::fcl::OT_BVH);
+#endif // PINOCCHIO_HPP_FCL_SUPERIOR_TO_1_1_3
+#endif // PINOCCHIO_WITH_HPP_FCL
pinocchio::Model model_ff;
pinocchio::urdf::buildModel(filename, pinocchio::JointModelFreeFlyer(), model_ff);