[Geometry] collisionRequest -> collisionRequests and distanceRequest -> distanceRequests

bindings/python/multibody/geometry-data.hpp

@@ -66,19 +66,18 @@ namespace pinocchio
                       "Vector of active CollisionPairs")
         
 #ifdef PINOCCHIO_WITH_HPP_FCL
-        .def_readonly("distanceRequest",
-                      &GeometryData::distanceRequest,
+        .def_readonly("distanceRequests",
+                      &GeometryData::distanceRequests,
                       "Defines which information should be computed by FCL for distance computations")
         .def_readonly("distanceResults",
                       &GeometryData::distanceResults,
                       "Vector of distance results.")
-        .def_readonly("collisionRequest",
-                      &GeometryData::collisionRequest,
+        .def_readonly("collisionRequests",
+                      &GeometryData::collisionRequests,
                       "Defines which information should be computed by FCL for collision computations.\n\n"
                       "Note: it is possible to define a security_margin and a break_distance for a collision request.\n"
                       "Most likely, for robotics application, these thresholds will be different for each collision pairs\n"
-                      "(e.g. the two hands can have a large security margin while the two hips cannot.\n"
-                      "It may be a good idea to have as many collision requests as collision pairs.)")
+                      "(e.g. the two hands can have a large security margin while the two hips cannot.)")
         .def_readonly("collisionResults",
                       &GeometryData::collisionResults,
                       "Vector of collision results.")
@@ -100,6 +99,20 @@ namespace pinocchio
              bp::args("self","pair_id"),
              "Deactivate the collsion pair pair_id in geomModel.collisionPairs if it exists.")
         ;
+
+#ifdef PINOCCHIO_WITH_HPP_FCL  
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
+        cl
+        .def_readonly("distanceRequest",
+                      &GeometryData::distanceRequest,
+                      "Deprecated. Use distanceRequests attribute instead.")
+        .def_readonly("collisionRequest",
+                      &GeometryData::collisionRequest,
+                      "Deprecated. Use collisionRequests attribute instead.")
+        ;
+#pragma GCC diagnostic pop
+#endif // PINOCCHIO_WITH_HPP_FCL
       }
              
       /* --- Expose --------------------------------------------------------- */

src/algorithm/geometry.hxx

@@ -72,7 +72,7 @@ namespace pinocchio
 
     fcl::collide (geom_model.geometryObjects[pair.first ].geometry.get(), oM1,
                   geom_model.geometryObjects[pair.second].geometry.get(), oM2,
-                  geom_data.collisionRequest,
+                  geom_data.collisionRequests[pairId],
                   collisionResult);
 
     return collisionResult.isCollision();
@@ -138,7 +138,7 @@ namespace pinocchio
                      oM2 (toFclTransform3f(geom_data.oMg[pair.second]));
     fcl::distance ( geom_model.geometryObjects[pair.first ].geometry.get(), oM1,
                     geom_model.geometryObjects[pair.second].geometry.get(), oM2,
-                    geom_data.distanceRequest,
+                    geom_data.distanceRequests[pairId],
                     geom_data.distanceResults[pairId]);
 
     return geom_data.distanceResults[pairId];

src/multibody/geometry.hpp

@@ -200,7 +200,13 @@ namespace pinocchio
     ///
     /// \brief Defines what information should be computed by distance computation.
     ///
-    fcl::DistanceRequest distanceRequest;
+    /// \deprecated use \ref distanceRequests instead
+    fcl::DistanceRequest distanceRequest PINOCCHIO_DEPRECATED;
+
+    ///
+    /// \brief Defines what information should be computed by distance computation.
+    /// There is one request per pair of geometries.
+    std::vector<fcl::DistanceRequest> distanceRequests;
 
     ///
     /// \brief Vector gathering the result of the distance computation for all the collision pairs.
@@ -210,7 +216,13 @@ namespace pinocchio
     ///
     /// \brief Defines what information should be computed by collision test.
     ///
-    fcl::CollisionRequest collisionRequest;
+    /// \deprecated use \ref collisionRequests instead
+    fcl::CollisionRequest collisionRequest PINOCCHIO_DEPRECATED;
+
+    ///
+    /// \brief Defines what information should be computed by collision test.
+    /// There is one request per pair of geometries.
+    std::vector<fcl::CollisionRequest> collisionRequests;
 
     ///
     /// \brief Vector gathering the result of the collision computation for all the collision pairs.

src/multibody/geometry.hxx

@@ -19,8 +19,10 @@ namespace pinocchio
   , activeCollisionPairs(geom_model.collisionPairs.size(), true)
 #ifdef PINOCCHIO_WITH_HPP_FCL
   , distanceRequest(true)
+  , distanceRequests(geom_model.collisionPairs.size(), hpp::fcl::DistanceRequest(true))
   , distanceResults(geom_model.collisionPairs.size())
   , collisionRequest(::hpp::fcl::NO_REQUEST,1)
+  , collisionRequests(geom_model.collisionPairs.size(), hpp::fcl::CollisionRequest(::hpp::fcl::NO_REQUEST,1))
   , collisionResults(geom_model.collisionPairs.size())
   , radius()
   , collisionPairIndex(0)
@@ -34,6 +36,16 @@ namespace pinocchio
     {
       collisionObjects.push_back(fcl::CollisionObject(geom_object.geometry));
     }
+    BOOST_FOREACH(hpp::fcl::CollisionRequest & creq, collisionRequests)
+    {
+      creq.enable_cached_gjk_guess = true;
+    }
+#if HPP_FCL_VERSION_AT_LEAST(1, 4, 5)
+    BOOST_FOREACH(hpp::fcl::DistanceRequest & dreq, distanceRequests)
+    {
+      dreq.enable_cached_gjk_guess = true;
+    }
+#endif
 #endif
     fillInnerOuterObjectMaps(geom_model);
   }
@@ -44,8 +56,10 @@ namespace pinocchio
 #ifdef PINOCCHIO_WITH_HPP_FCL
   , collisionObjects (other.collisionObjects)
   , distanceRequest (other.distanceRequest)
+  , distanceRequests (other.distanceRequests)
   , distanceResults (other.distanceResults)
   , collisionRequest (other.collisionRequest)
+  , collisionRequests (other.collisionRequests)
   , collisionResults (other.collisionResults)
   , radius (other.radius)
   , collisionPairIndex (other.collisionPairIndex)