Fix issue in GJK where the tolerance for x_norm was set too low.

PiperOrigin-RevId: 880735671
Change-Id: I324e37e8677d3c7feeb4b8bcf065a2a0122326b6

src/engine/engine_collision_gjk.c

@@ -180,21 +180,26 @@ static void gjk(mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) {
   mjtNum x_k[3];                           // the kth approximation point in Minkowski difference
   mjtNum lambda[4] = {1, 0, 0, 0};         // barycentric coordinates for x_k
   mjtNum cutoff2 = status->dist_cutoff * status->dist_cutoff;
+  mjtNum tol2 = status->tolerance * status->tolerance;
 
   // if both geoms are discrete, finite convergence is guaranteed; set tolerance to 0
-  mjtNum epsilon = discreteGeoms(obj1, obj2) ? 0 : 0.5 * status->tolerance * status->tolerance;
+  mjtNum epsilon = discreteGeoms(obj1, obj2) ? 0 : 0.5 * tol2;
+
+  // tolerance on squared norm of x_k
+  mjtNum min_norm2 = discreteGeoms(obj1, obj2) ? mjMINVAL2 : tol2;
   mjtNum x_norm;
 
   // set initial guess
   sub3(x_k, x1_k, x2_k);
 
   for (; k < kmax; k++) {
-    // compute the kth support point
+    // in tolerance for geoms to be in contact
-    x_norm = dot3(x_k, x_k);
+    if ((x_norm = dot3(x_k, x_k)) < min_norm2) {
-    if (x_norm < mjMINVAL2) {
       break;
     }
     x_norm = mju_sqrt(x_norm);
+
+    // compute the kth support point
     gjkSupport(simplex + n, obj1, obj2, x_k, x_norm);
     mjtNum *s_k = simplex[n].vert;
 

test/engine/engine_collision_gjk_test.cc

@@ -42,7 +42,7 @@ using ::testing::DoubleNear;
 
 constexpr mjtNum kTolerance = 1e-6;
 constexpr int kMaxIterations = 1000;
-constexpr char kEllipoid[] = R"(
+constexpr char kEllipsoidXml[] = R"(
 <mujoco model="Ellipsoid Test">
   <compiler angle="radian"/>
   <size nkey="1"/>
@@ -1760,7 +1760,7 @@ TEST_F(MjGjkTest, MeshEdge2) {
 
 TEST_F(MjGjkTest, EllipsoidEllipsoidPenetrating) {
   char error[1024];
-  mjModel* model = LoadModelFromString(kEllipoid, error, sizeof(error));
+  mjModel* model = LoadModelFromString(kEllipsoidXml, error, sizeof(error));
   ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error;
 
   mjData* data = mj_makeData(model);
@@ -1806,6 +1806,73 @@ TEST_F(MjGjkTest, EllipsoidEllipsoid) {
   mj_deleteModel(model);
 }
 
+TEST_F(MjGjkTest, EllipsoidEllipsoidSlowConvergence) {
+  // This example takes gjkIntersect 30 iterations to converge, thus failing to
+  // detect collision if opt.ccd_iterations is set to 35. With gjkIntersect
+  // disabled, GJK succeeds in 24 iterations.
+  static constexpr char xml[] = R"(
+  <mujoco>
+    <worldbody>
+      <geom name="geom1" type="ellipsoid" size=".05 .07 .15"/>
+      <geom name="geom2" type="ellipsoid" size=".05 .07 .15"/>
+    </worldbody>
+  </mujoco>)";
+
+  char error[1024];
+  mjModel* model = LoadModelFromString(xml, error, sizeof(error));
+  ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error;
+
+  mjData* data = mj_makeData(model);
+  mj_forward(model, data);
+
+  mjtNum* xmat = data->geom_xmat;
+  mjtNum* xpos = data->geom_xpos;
+
+  xmat[0] = 0.98799245191271645172;
+  xmat[1] = -0.13055117805648397411;
+  xmat[2] = -0.08262750674904573156;
+  xmat[3] = -0.03910798838001148386;
+  xmat[4] = 0.30606703783583483203;
+  xmat[5] = -0.95120635699897815307;
+  xmat[6] = 0.14947066671547387662;
+  xmat[7] = 0.94301609650019369013;
+  xmat[8] = 0.29728632920665870598;
+
+  xpos[0] = 0.44826351361072502844;
+  xpos[1] = -0.10441423315171689812;
+  xpos[2] = 0.21216466278453649519;
+
+  xmat = data->geom_xmat + 9;
+  xpos = data->geom_xpos + 3;
+
+  xmat[0] = 0.53301656081215953442;
+  xmat[1] = -0.59335516633486651283;
+  xmat[2] = -0.60317741377119038493;
+  xmat[3] = -0.39469414877746378245;
+  xmat[4] = 0.45620167535223110633;
+  xmat[5] = -0.79755661888461082398;
+  xmat[6] = 0.74840488695679441289;
+  xmat[7] = 0.66318148194106107951;
+  xmat[8] = 0.00896923573374096283;
+
+  xpos[0] = 0.39694137187624300989;
+  xpos[1] = 0.00961542646741688108;
+  xpos[2] = 0.29832742817753182818;
+
+  int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1");
+  int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2");
+
+  mjCCDStatus status;
+  std::vector<mjtNum> dir, pos;
+  mjtNum dist;
+  Penetration(status, dist, dir, pos, model, data, geom1, geom2);
+
+  EXPECT_LT(dist, 0.0);
+  EXPECT_NEAR(dist, 0.0, kTolerance);
+  mj_deleteData(data);
+  mj_deleteModel(model);
+}
+
 TEST_F(MjGjkTest, BoxBox) {
   static constexpr char xml[] = R"(
   <mujoco>