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