Merge pull request #954 from jmirabel/urdf_convex

[URDF][Experimental] Enable convex representation of meshes for collision detection

.travis.yml

@@ -4,6 +4,8 @@ git:
   depth: false
 compiler:
   - gcc
+cache:
+  ccache: true
 env:
   global:
     - secure: "SnIBG/xLIHX3CSvUbqqsX8xTVqIqQ7fFS6HWO6KZQVBsT6yugTwYHbyhNiU531JejYJ/I3ZrDhXfYH3qFZiYxnH1sifvwV+fnTtMXpPN7qPZwIymkjcmm6gJF51e0C7VOfUbvKFv0ngwj+ul21rgZSMuoEvxPK0WxtE3/ZSfn9c="

models/simple_humanoid.urdf

@@ -17,10 +17,16 @@
         <cylinder radius="1" length="1"/>
       </geometry>
     </collision>
+    <collision name="box">
+      <geometry>
+        <mesh filename="package://simple_humanoid_description/box.stl" />
+      </geometry>
+    </collision>
     <collision_checking>
       <!--- This tells to pinocchio to replace the cylinder called "test"
            by a capsule with the same radius and length -->
       <capsule name="test"/>
+      <convex name="box"/>
     </collision_checking>
   </link>
 

models/simple_humanoid_description/box.stl

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

src/parsers/urdf/geometry.hxx

@@ -56,8 +56,8 @@ namespace pinocchio
           } // BOOST_FOREACH
         }
         
-        bool replaceCylinderByCapsule (const std::string & linkName,
-                                       const std::string & geomName) const
+        bool isCapsule (const std::string & linkName,
+                        const std::string & geomName) const
         {
           LinkMap_t::const_iterator _link = links_.find(linkName);
           assert (_link != links_.end());
@@ -66,8 +66,36 @@ namespace pinocchio
             return false;
           BOOST_FOREACH(const ptree::value_type & cc, link.get_child("collision_checking")) {
             if (cc.first == "capsule") {
+#ifdef PINOCCHIO_URDFDOM_COLLISION_WITH_GROUP_NAME
+              std::cerr << "Warning: support for tag link/collision_checking/capsule"
+                " is not available for URDFDOM < 0.3.0" << std::endl;
+#else
               std::string name = cc.second.get<std::string>("<xmlattr>.name");
               if (geomName == name) return true;
+#endif
+            }
+          } // BOOST_FOREACH
+          
+          return false;
+        }
+
+        bool isMeshConvex (const std::string & linkName,
+                           const std::string & geomName) const
+        {
+          LinkMap_t::const_iterator _link = links_.find(linkName);
+          assert (_link != links_.end());
+          const ptree& link = _link->second;
+          if (link.count ("collision_checking") == 0)
+            return false;
+          BOOST_FOREACH(const ptree::value_type & cc, link.get_child("collision_checking")) {
+            if (cc.first == "convex") {
+#ifdef PINOCCHIO_URDFDOM_COLLISION_WITH_GROUP_NAME
+              std::cerr << "Warning: support for tag link/collision_checking/convex"
+                " is not available for URDFDOM < 0.3.0" << std::endl;
+#else
+              std::string name = cc.second.get<std::string>("<xmlattr>.name");
+              if (geomName == name) return true;
+#endif
             }
           } // BOOST_FOREACH
           
@@ -93,7 +121,13 @@ namespace pinocchio
         mesh->scale.z;
       }
 
-#ifdef PINOCCHIO_WITH_HPP_FCL      
+#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
+
       /**
        * @brief      Get a fcl::CollisionObject from an urdf geometry, searching
        *             for it in specified package directories
@@ -138,14 +172,24 @@ namespace pinocchio
           retrieveMeshScale(urdf_mesh, meshScale);
           
           // Create FCL mesh by parsing Collada file.
+#ifdef PINOCCHIO_HPP_FCL_SUPERIOR_TO_1_1_3
+          hpp::fcl::BVHModelPtr_t bvh = meshLoader->load (meshPath, scale);
+          bool convex = tree.isMeshConvex (linkName, geomName);
+          if (convex) {
+            bvh->buildConvexRepresentation (false);
+            geometry = bvh->convex;
+          } else
+            geometry = bvh;
+#else
           geometry = meshLoader->load (meshPath, scale);
+#endif
         }
 
         // Handle the case where collision geometry is a cylinder
         // Use FCL capsules for cylinders
         else if (urdf_geometry->type == ::urdf::Geometry::CYLINDER)
         {
-          bool capsule = tree.replaceCylinderByCapsule(linkName, geomName);
+          bool capsule = tree.isCapsule(linkName, geomName);
           meshScale << 1,1,1;
           const ::urdf::CylinderSharedPtr collisionGeometry = ::urdf::dynamic_pointer_cast< ::urdf::Cylinder> (urdf_geometry);
           
@@ -475,4 +519,8 @@ namespace pinocchio
   } // namespace urdf
 } // namespace pinocchio
             
+#ifdef PINOCCHIO_HPP_FCL_SUPERIOR_TO_1_1_3
+# undef PINOCCHIO_HPP_FCL_SUPERIOR_TO_1_1_3
+#endif
+
 #endif // ifndef __pinocchio_multibody_parsers_urdf_geometry_hxx__

unittest/urdf.cpp

@@ -9,6 +9,10 @@
 #include "pinocchio/multibody/model.hpp"
 #include "pinocchio/parsers/urdf.hpp"
 
+#ifdef PINOCCHIO_WITH_HPP_FCL
+#include <hpp/fcl/collision_object.h>
+#endif // PINOCCHIO_WITH_HPP_FCL
+
 #include <boost/test/unit_test.hpp>
 
 #include <urdf_parser/urdf_parser.h>
@@ -32,11 +36,39 @@ 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
+  // Check that cylinder is converted into capsule.
+#ifdef PINOCCHIO_URDFDOM_COLLISION_WITH_GROUP_NAME
+  BOOST_CHECK_EQUAL(geomModel.geometryObjects[0].fcl->getNodeType(), hpp::fcl::GEOM_CYLINDER);
+#else // PINOCCHIO_URDFDOM_COLLISION_WITH_GROUP_NAME
+  BOOST_CHECK_EQUAL(geomModel.geometryObjects[0].fcl->getNodeType(), hpp::fcl::GEOM_CAPSULE);
+#endif
+
+#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
+
+#if defined(PINOCCHIO_HPP_FCL_SUPERIOR_TO_1_1_3) && !defined(PINOCCHIO_URDFDOM_COLLISION_WITH_GROUP_NAME)
+  BOOST_CHECK_EQUAL(geomModel.geometryObjects[1].fcl->getNodeType(), hpp::fcl::GEOM_CONVEX);
+#undef PINOCCHIO_HPP_FCL_SUPERIOR_TO_1_1_3
+#else // PINOCCHIO_HPP_FCL_SUPERIOR_TO_1_1_3 && !PINOCCHIO_URDFDOM_COLLISION_WITH_GROUP_NAME
+  BOOST_CHECK_EQUAL(geomModel.geometryObjects[1].fcl->getObjectType(), hpp::fcl::OT_BVH);
+#endif // PINOCCHIO_HPP_FCL_SUPERIOR_TO_1_1_3 && !PINOCCHIO_URDFDOM_COLLISION_WITH_GROUP_NAME
+#endif // PINOCCHIO_WITH_HPP_FCL
   
   pinocchio::Model model_ff;
   pinocchio::urdf::buildModel(filename, pinocchio::JointModelFreeFlyer(), model_ff);