Merge pull request #183 from jmirabel/devel

Add operator== for requests + bind StdVec of requests.

include/hpp/fcl/collision_data.h

@@ -157,6 +157,14 @@ struct HPP_FCL_DLLAPI QueryRequest
   {}
 
   void updateGuess(const QueryResult& result);
+
+  /// @brief whether two QueryRequest are the same or not
+  inline bool operator ==(const QueryRequest& other) const
+  {
+    return enable_cached_gjk_guess == other.enable_cached_gjk_guess
+      && cached_gjk_guess == other.cached_gjk_guess
+      && cached_support_func_guess == other.cached_support_func_guess;
+  }
 };
 
 /// @brief base class for all query results
@@ -234,6 +242,17 @@ struct HPP_FCL_DLLAPI CollisionRequest : QueryRequest
     }
 
   bool isSatisfied(const CollisionResult& result) const;
+
+  /// @brief whether two CollisionRequest are the same or not
+  inline bool operator ==(const CollisionRequest& other) const
+  {
+    return QueryRequest::operator==(other)
+      && num_max_contacts == other.num_max_contacts
+      && enable_contact == other.enable_contact
+      && enable_distance_lower_bound == other.enable_distance_lower_bound
+      && security_margin == other.security_margin
+      && break_distance == other.break_distance;
+  }
 };
 
 /// @brief collision result
@@ -354,6 +373,15 @@ struct HPP_FCL_DLLAPI DistanceRequest : QueryRequest
   }
 
   bool isSatisfied(const DistanceResult& result) const;
+
+  /// @brief whether two DistanceRequest are the same or not
+  inline bool operator ==(const DistanceRequest& other) const
+  {
+    return QueryRequest::operator==(other)
+      && enable_nearest_points == other.enable_nearest_points
+      && rel_err == other.rel_err
+      && abs_err == other.abs_err;
+  }
 };
 
 /// @brief distance result

include/hpp/fcl/narrowphase/narrowphase.h

@@ -156,7 +156,7 @@ namespace fcl
           if (gjk.hasPenetrationInformation(shape)) {
             gjk.getClosestPoints (shape, w0, w1);
             distance = gjk.distance;
-            normal = tf1.getRotation() * (w1 - w0).normalized();
+            normal = tf1.getRotation() * (w0 - w1).normalized();
             p1 = p2 = tf1.transform((w0 + w1) / 2);
           } else {
             details::EPA epa(epa_max_face_num, epa_max_vertex_num, epa_max_iterations, epa_tolerance);
@@ -168,7 +168,7 @@ namespace fcl
             {
               epa.getClosestPoints (shape, w0, w1);
               distance = -epa.depth;
-              normal = -epa.normal;
+              normal = tf1.getRotation() * epa.normal;
               p1 = p2 = tf1.transform(w0 - epa.normal*(epa.depth *0.5));
               assert (distance <= 1e-6);
             } else {
@@ -261,7 +261,7 @@ namespace fcl
             // Return contact points in case of collision
             //p1 = tf1.transform (p1);
             //p2 = tf1.transform (p2);
-            normal = (tf1.getRotation() * (p2 - p1)).normalized();
+            normal = (tf1.getRotation() * (p1 - p2)).normalized();
             p1 = tf1.transform(p1);
             p2 = tf1.transform(p2);
           } else {

python/CMakeLists.txt

@@ -141,6 +141,7 @@ INSTALL(TARGETS ${LIBRARY_NAME}
 # --- INSTALL SCRIPTS
 SET(PYTHON_FILES
   __init__.py
+  viewer.py
   )
 
 FOREACH(python ${PYTHON_FILES})

python/collision.cc

@@ -102,6 +102,13 @@ void exposeCollisionAPI ()
       ;
   }
 
+  if(!eigenpy::register_symbolic_link_to_registered_type< std::vector<CollisionRequest> >())
+  {
+    class_< std::vector<CollisionRequest> >("StdVec_CollisionRequest")
+      .def(vector_indexing_suite< std::vector<CollisionRequest> >())
+      ;
+  }
+
   if(!eigenpy::register_symbolic_link_to_registered_type<Contact>())
   {
     class_ <Contact> ("Contact",

python/distance.cc

@@ -89,6 +89,13 @@ void exposeDistanceAPI ()
       ;
   }
 
+  if(!eigenpy::register_symbolic_link_to_registered_type< std::vector<DistanceRequest> >())
+  {
+    class_< std::vector<DistanceRequest> >("StdVec_DistanceRequest")
+      .def(vector_indexing_suite< std::vector<DistanceRequest> >())
+      ;
+  }
+
   if(!eigenpy::register_symbolic_link_to_registered_type<DistanceResult>())
   {
     class_ <DistanceResult, bases<QueryResult> > ("DistanceResult",

python/hppfcl/viewer.py

+# Software License Agreement (BSD License)
+#
+#  Copyright (c) 2019 CNRS
+#  Author: Joseph Mirabel
+
+import hppfcl, numpy as np
+from gepetto import Color
+
+def applyConfiguration(gui, name, tf):
+    gui.applyConfiguration(name, tf.getTranslation().tolist() + tf.getQuatRotation().coeffs().tolist())
+
+def displayShape(gui, name, geom, color = (.9, .9, .9, 1.)):
+    if isinstance(geom, hppfcl.Capsule):
+        return gui.addCapsule(name, geom.radius, 2. * geom.halfLength, color)
+    elif isinstance(geom, hppfcl.Cylinder):
+        return gui.addCylinder(name, geom.radius, 2. * geom.halfLength, color)
+    elif isinstance(geom, hppfcl.Box):
+        w, h, d = (2. * geom.halfSide).tolist()
+        return gui.addBox(name, w, h, d, color)
+    elif isinstance(geom, hppfcl.Sphere):
+        return gui.addSphere(name, geom.radius, color)
+    elif isinstance(geom, hppfcl.Cone):
+        return gui.addCone(name, geom.radius, 2. * geom.halfLength, color)
+    elif isinstance(geom, hppfcl.Convex):
+        pts = [ geom.points(geom.polygons(f)[i]).tolist() for f in range(geom.num_polygons) for i in range(3) ]
+        gui.addCurve(name, pts, color)
+        gui.setCurveMode(name, "TRIANGLES")
+        gui.setLightingMode(name, "ON")
+        gui.setBoolProperty(name, "BackfaceDrawing", True)
+        return True
+    elif isinstance(geom, hppfcl.ConvexBase):
+        pts = [ geom.points(i).tolist() for i in range(geom.num_points) ]
+        gui.addCurve(name, pts, color)
+        gui.setCurveMode(name, "POINTS")
+        gui.setLightingMode(name, "OFF")
+        return True
+    else:
+        msg = "Unsupported geometry type for %s (%s)" % (geometry_object.name, type(geom) )
+        warnings.warn(msg, category=UserWarning, stacklevel=2)
+        return False
+
+def displayDistanceResult(gui, group_name, res, closest_points = True, normal = True):
+    gui.createGroup(group_name)
+    r = 0.01
+    if closest_points:
+        p = [ group_name+"/p1", group_name+"/p2" ]
+        gui.addSphere(p[0], r, Color.red)
+        gui.addSphere(p[1], r, Color.blue)
+        qid = [0,0,0,1]
+        gui.applyConfigurations(p, [ res.getNearestPoint1().tolist() + qid, res.getNearestPoint2().tolist() + qid, ])
+    if normal:
+        n = group_name+"/normal"
+        gui.addArrow(n, r, 0.1, Color.green)
+        gui.applyConfiguration(n,
+                res.getNearestPoint1().tolist() + hppfcl.Quaternion.FromTwoVectors(np.array([1,0,0]), res.normal).coeffs().tolist())
+    gui.refresh()
+
+def displayCollisionResult(gui, group_name, res, color=Color.green):
+    if res.isCollision():
+        if gui.nodeExists(group_name):
+            gui.setVisibility(group_name, "ON")
+        else:
+            gui.createGroup(group_name)
+        for i in range(res.numContacts()):
+            contact = res.getContact(i)
+            n = group_name+"/contact"+str(i)
+            depth = contact.penetration_depth
+            if gui.nodeExists(n):
+                gui.setFloatProperty(n, 'Size', depth)
+                gui.setFloatProperty(n, 'Radius', 0.1*depth)
+                gui.setColor(n, color)
+            else:
+                gui.addArrow(n, depth*0.1, depth, color)
+            N = contact.normal
+            P = contact.pos
+            gui.applyConfiguration(n, (P-depth*N/2).tolist() +
+                    hppfcl.Quaternion.FromTwoVectors(np.array([1,0,0]), N).coeffs().tolist())
+        gui.refresh()
+    elif gui.nodeExists(group_name):
+        gui.setVisibility(group_name, "OFF")

src/narrowphase/gjk.cpp

@@ -575,7 +575,10 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess,
     alpha = std::max(alpha, omega);
     FCL_REAL diff (rl - alpha);
     if (iterations == 0) diff = std::abs(diff);
-    if(diff - tolerance * rl <= 0)
+    // TODO here, we can stop at iteration 0 if this condition is met.
+    // We stopping at iteration 0, the closest point will not be valid.
+    // if(diff - tolerance * rl <= 0)
+    if(iterations > 0 && diff - tolerance * rl <= 0)
     {
       if (iterations > 0)
         removeVertex(simplices[current]);

test/gjk.cpp

@@ -368,7 +368,7 @@ void test_gjk_triangle_capsule (Vec3f T, bool expect_collision,
 
     // Check that guess works as expected
     Vec3f guess = gjk.getGuessFromSimplex();
-    details::GJK gjk2 (0, 1e-6);
+    details::GJK gjk2 (3, 1e-6);
     details::GJK::Status status2 = gjk2.evaluate(shape, guess);
     BOOST_CHECK_EQUAL(status2, details::GJK::Valid);
   }