Fix GJK algorithm

If GJK::evaluate fails to converge but that the distance of the origin to the simplex converges to 0, set distance and return fail. In shapeDistance, return collision if GJK::evaluate failed.
humanoid-path-planner · Dec 26, 2018 · 9c98029 · 9c98029
1 parent 59f86be
commit 9c98029
Show file tree

Hide file tree

Showing 2 changed files with 30 additions and 15 deletions.
diff --git a/include/hpp/fcl/narrowphase/narrowphase.h b/include/hpp/fcl/narrowphase/narrowphase.h
@@ -169,6 +169,7 @@ namespace fcl
                          FCL_REAL& distance, Vec3f& p1, Vec3f& p2,
                          Vec3f& normal) const
     {
+      FCL_REAL eps (sqrt(std::numeric_limits<FCL_REAL>::epsilon()));
       bool compute_normal (true);
       Vec3f guess(1, 0, 0);
       if(enable_cached_guess) guess = cached_guess;
@@ -183,7 +184,23 @@ namespace fcl
       details::GJK::Status gjk_status = gjk.evaluate(shape, -guess);
       if(enable_cached_guess) cached_guess = gjk.getGuessFromSimplex();
 
-      if(gjk_status == details::GJK::Valid)
+      if(gjk_status == details::GJK::Failed)
+      {
+        // TODO: understand why GJK fails between cylinder and box
+        assert (distance * distance < sqrt (eps));
+        Vec3f w0 (Vec3f::Zero()), w1 (Vec3f::Zero());
+        for(size_t i = 0; i < gjk.getSimplex()->rank; ++i)
+        {
+          FCL_REAL p = gjk.getSimplex()->coefficient[i];
+          w0 += shape.support(gjk.getSimplex()->vertex[i]->d, 0) * p;
+          w1 += shape.support(-gjk.getSimplex()->vertex[i]->d, 1) * p;
+        }
+        distance = 0;
+        p1 = p2 = tf1.transform (.5* (w0 + w1));
+        normal = Vec3f (0,0,0);
+        return false;
+      }
+      else if(gjk_status == details::GJK::Valid)
         {
           Vec3f w0 (Vec3f::Zero()), w1 (Vec3f::Zero());
           for(size_t i = 0; i < gjk.getSimplex()->rank; ++i)
@@ -199,8 +216,9 @@ namespace fcl
           p2 = tf1.transform (w1);
           return true;
         }
-      else if (gjk_status == details::GJK::Inside)
+      else
         {
+          assert (gjk_status == details::GJK::Inside);
           if (compute_normal)
             {
               details::EPA epa(epa_max_face_num, epa_max_vertex_num,
@@ -214,8 +232,8 @@ namespace fcl
                       w0 += shape.support(epa.result.vertex[i]->d, 0) *
                         epa.result.coefficient[i];
                     }
-                  assert (epa.depth >= 0);
-                  distance = -epa.depth;
+                  assert (epa.depth >= -eps);
+                  distance = std::min (0., -epa.depth);
                   normal = tf2.getRotation() * epa.normal;
                   p1 = p2 = tf1.transform(w0 - epa.normal*(epa.depth *0.5));
                 }
@@ -236,11 +254,6 @@ namespace fcl
             }
           return false;
         }
-      else
-        {
-          // Failure
-          abort ();
-        }
     }
 
     /// @brief default setting for GJK algorithm

diff --git a/src/narrowphase/gjk.cpp b/src/narrowphase/gjk.cpp
@@ -74,9 +74,10 @@ Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir)
   case GEOM_BOX:
     {
       const Box* box = static_cast<const Box*>(shape);
-      return Vec3f((dir[0]>0)?(box->side[0]/2):(-box->side[0]/2),
-                   (dir[1]>0)?(box->side[1]/2):(-box->side[1]/2),
-                   (dir[2]>0)?(box->side[2]/2):(-box->side[2]/2));
+      Vec3f res((dir[0]>0)?(box->side[0]/2):(-box->side[0]/2),
+                (dir[1]>0)?(box->side[1]/2):(-box->side[1]/2),
+                (dir[2]>0)?(box->side[2]/2):(-box->side[2]/2));
+      return res;
     }
     break;
   case GEOM_SPHERE:
@@ -194,6 +195,7 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess)
   FCL_REAL alpha = 0;
   Vec3f lastw[4];
   size_t clastw = 0;
+  Project::ProjectResult project_res;
 
   free_v[0] = &store_v[0];
   free_v[1] = &store_v[1];
@@ -244,7 +246,6 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess)
       break;
     }
 
-    Project::ProjectResult project_res;
     switch(curr_simplex.rank)
     {
     case 2:
@@ -262,7 +263,6 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess)
                                                      curr_simplex.vertex[3]->w);
       break;
     }
-
     if(project_res.sqr_distance >= 0)
     {
       next_simplex.rank = 0;
@@ -299,7 +299,9 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess)
   {
   case Valid: distance = ray.norm(); break;
   case Inside: distance = 0; break;
-  default: break;
+  default:
+    distance = sqrt (project_res.sqr_distance);
+    break;
   }
   return status;
 }