Add tolerance to upper > lower bound check on first iteration of EPA.

PiperOrigin-RevId: 933615381 Change-Id: I175d3b97fa65f71a8f1cc34da5f6b9a23ab856d2
2026-06-17 03:18:29 -07:00
parent f6fb4af091
commit 386b45fb75
2 changed files with 46 additions and 1 deletions
@@ -1394,7 +1394,7 @@ static Face* epa(mjCCDStatus* status, Polytope* pt, mjCCDObj* obj1, mjCCDObj* ob
    }
    if (upper - lower < tolerance) {
      // terminate without contact when upper < lower on first iteration
-      if (k == 0 && upper < lower) {
+      if (k == 0 && upper < lower - 1e-10) {
        face = NULL;
      }
      break;