Refactor nativeccd interface.

- Update mj_ccd interface for multi-contacts,
- Support GJK cutoff distance, and
- Remove mjc_fixNormal from nativeccd (causes bug with cylindar box collisions).

PiperOrigin-RevId: 696185362
Change-Id: I4828b7ee1bde078268220172a4937cd553f6187e

src/engine/engine_collision_convex.c

@@ -30,7 +30,7 @@
 #include "engine/engine_util_misc.h"
 #include "engine/engine_util_spatial.h"
 
-// call LibCCD or GJK to recover penetration info
+// call libccd or nativeccd to recover penetration info
 static int mjc_penetration(const mjModel* m, mjCCDObj* obj1, mjCCDObj* obj2,
                            const ccd_t* ccd, ccd_real_t* depth, ccd_vec3_t* dir, ccd_vec3_t* pos) {
   if (mjENABLED(mjENBL_NATIVECCD)) {
@@ -40,8 +40,8 @@ static int mjc_penetration(const mjModel* m, mjCCDObj* obj1, mjCCDObj* obj2,
     // set config
     config.max_iterations = ccd->max_iterations,
     config.tolerance = ccd->mpr_tolerance,
-    config.contacts = 1;
-    config.distances = 0;  // no geom distances needed
+    config.max_contacts = 1;
+    config.dist_cutoff = 0;  // no geom distances needed
 
     mjtNum dist = mjc_ccd(&config, &status, obj1, obj2);
     if (dist < 0) {
@@ -68,6 +68,7 @@ static int mjc_penetration(const mjModel* m, mjCCDObj* obj1, mjCCDObj* obj2,
 }
 
 
+
 // ccd center function
 void mjccd_center(const void *obj, ccd_vec3_t *center) {
   mjc_center(center->v, (const mjCCDObj*) obj);
@@ -744,12 +745,46 @@ static void mjc_initCCD(ccd_t* ccd, const mjModel* m) {
 
 
 // find single convex-convex collision
-static int mjc_CCDIteration(mjCCDObj* obj1, mjCCDObj* obj2, const ccd_t* ccd,
-                            const mjModel* m, const mjData* d,
+static int mjc_CCDIteration(const mjModel* m, const mjData* d, mjCCDObj* obj1, mjCCDObj* obj2,
                             mjContact* con, mjtNum margin) {
+  if (mjENABLED(mjENBL_NATIVECCD)) {
+    mjCCDConfig config;
+    mjCCDStatus status;
+
+    // set config
+    config.max_iterations = m->opt.ccd_iterations;
+    config.tolerance = m->opt.ccd_tolerance;
+    config.max_contacts = 1;
+    config.dist_cutoff = 0;  // no geom distances needed
+
+    mjtNum dist = mjc_ccd(&config, &status, obj1, obj2);
+    if (dist < 0) {
+      con->dist = margin + dist;
+      mju_sub3(con->frame, status.x1, status.x2);
+      mju_normalize3(con->frame);
+      con->pos[0] = 0.5 * (status.x1[0] + status.x2[0]);
+      con->pos[1] = 0.5 * (status.x1[1] + status.x2[1]);
+      con->pos[2] = 0.5 * (status.x1[2] + status.x2[2]);
+      mju_zero3(con->frame+3);
+      return 1;
+    }
+    return 0;
+  }
+
+  // init libccd structure
+  ccd_t ccd;
+  mjc_initCCD(&ccd, m);
+  ccd.first_dir = ccdFirstDirDefault;
+  ccd.center1 = mjccd_center;
+  ccd.center2 = mjccd_center;
+  ccd.support1 = mjccd_support;
+  ccd.support2 = mjccd_support;
+
   ccd_vec3_t dir, pos;
   ccd_real_t depth;
-  if (mjc_penetration(m, obj1, obj2, ccd, &depth, &dir, &pos) == 0) {
+
+  // call MPR from libccd
+  if (ccdMPRPenetration(obj1, obj2, &ccd, &depth, &dir, &pos) == 0) {
     // contact is found but normal is undefined
     if (ccdVec3Eq(&dir, ccd_vec3_origin)) {
       return 0;
@@ -768,11 +803,7 @@ static int mjc_CCDIteration(mjCCDObj* obj1, mjCCDObj* obj2, const ccd_t* ccd,
 
     return 1;
   }
-
-  // no contact found
-  else {
-    return 0;
-  }
+  return 0;
 }
 
 
@@ -819,17 +850,8 @@ int mjc_Convex(const mjModel* m, const mjData* d,
   mjc_initCCDObj(&obj1, m, d, g1, margin);
   mjc_initCCDObj(&obj2, m, d, g2, margin);
 
-  // init libccd structure
-  ccd_t ccd;
-  mjc_initCCD(&ccd, m);
-  ccd.first_dir = ccdFirstDirDefault;
-  ccd.center1 = mjccd_center;
-  ccd.center2 = mjccd_center;
-  ccd.support1 = mjccd_support;
-  ccd.support2 = mjccd_support;
-
   // find initial contact
-  int ncon = mjc_CCDIteration(&obj1, &obj2, &ccd, m, d, con, margin);
+  int ncon = mjc_CCDIteration(m, d, &obj1, &obj2, con, margin);
 
   // look for additional contacts
   if (ncon && mjENABLED(mjENBL_MULTICCD)  // TODO(tassa) leave as bitflag or make geom attribute (?)
@@ -878,7 +900,7 @@ int mjc_Convex(const mjModel* m, const mjData* d,
         mju_rotateFrame(con[0].pos, invrot, d->geom_xmat+9*g2, d->geom_xpos+3*g2);
 
         // search for new contact
-        int new_contact = mjc_CCDIteration(&obj1, &obj2, &ccd, m, d, con+ncon, margin);
+        int new_contact = mjc_CCDIteration(m, d, &obj1, &obj2, con+ncon, margin);
 
         // check new contact
         if (new_contact && mjc_isDistinctContact(con, ncon + 1, tolerance)) {
@@ -1525,19 +1547,8 @@ int mjc_ConvexElem(const mjModel* m, const mjData* d, mjContact* con,
   mjc_setCCDObjFlex(&obj1, f1, e1, v1);
   mjc_setCCDObjFlex(&obj2, f2, e2, -1);
 
-  // init libccd structure
-  ccd_t ccd;
-  mjc_initCCD(&ccd, m);
-  ccd.first_dir = ccdFirstDirDefault;
-  ccd.center1 = mjccd_center;
-  ccd.center2 = mjccd_center;
-  ccd.support1 = mjccd_support;
-  ccd.support2 = mjccd_support;
-
   // find contacts
-  int ncon = mjc_CCDIteration(&obj1, &obj2, &ccd, m, d, con, margin);
-
-  return ncon;
+  return mjc_CCDIteration(m, d, &obj1, &obj2, con, margin);
 }
 
 

src/engine/engine_collision_convex.h

@@ -85,7 +85,7 @@ int mjc_Convex        (const mjModel* m, const mjData* d,
 int mjc_ConvexElem    (const mjModel* m, const mjData* d, mjContact* con,
                        int g1, int f1, int e1, int v1, int f2, int e2, mjtNum margin);
 
-// heighfield-elem collision function using ccd
+// heightfield-elem collision function using ccd
 int mjc_HFieldElem    (const mjModel* m, const mjData* d, mjContact* con,
                        int g, int f, int e, mjtNum margin);
 

src/engine/engine_collision_gjk.c

@@ -77,8 +77,9 @@ static int newVertex(Polytope* pt, const mjtNum v1[3], const mjtNum v2[3]);
 // attaches a face to the polytope with the given vertex indices; returns non-zero on error
 static void attachFace(Polytope* pt, int v1, int v2, int v3, int adj1, int adj2, int adj3);
 
-// returns 1 if objects are in contact, 0 otherwise; status must have initial tetrahedrons
-static int gjkIntersect(mjCCDStatus* status, int start, mjCCDObj* obj1, mjCCDObj* obj2);
+// returns 1 if objects are in contact; 0 if not; -1 if inconclusive
+// status must have initial tetrahedrons
+static int gjkIntersect(mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2);
 
 // returns the penetration depth of two convex objects; witness points are in status->{x1, x2}
 static mjtNum epa(mjCCDStatus* status, Polytope* pt, mjCCDObj* obj1, mjCCDObj* obj2);
@@ -145,17 +146,18 @@ static int discreteGeoms(mjCCDObj* obj1, mjCCDObj* obj2) {
 
 // GJK algorithm
 static mjtNum gjk(mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) {
-  int get_dist = status->has_distances;  // need to recover geom distances if not in contact
-  mjtNum *simplex1 = status->simplex1;   // simplex for obj1
-  mjtNum *simplex2 = status->simplex2;   // simplex for obj2
-  mjtNum *simplex  = status->simplex;    // simplex in Minkowski difference
-  int n = 0;                             // number of vertices in the simplex
-  int k = 0;                             // current iteration
-  int kmax = status->max_iterations;     // max number of iterations
-  mjtNum* x1_k = status->x1;             // the kth approximation point for obj1
-  mjtNum* x2_k = status->x2;             // the kth approximation point for obj2
-  mjtNum x_k[3];                         // the kth approximation point in Minkowski difference
-  mjtNum lambda[4];                      // barycentric coordinates for x_k
+  int get_dist = status->dist_cutoff > 0;  // need to recover geom distances if not in contact
+  mjtNum *simplex1 = status->simplex1;     // simplex for obj1
+  mjtNum *simplex2 = status->simplex2;     // simplex for obj2
+  mjtNum *simplex  = status->simplex;      // simplex in Minkowski difference
+  int n = 0;                               // number of vertices in the simplex
+  int k = 0;                               // current iteration
+  int kmax = status->max_iterations;       // max number of iterations
+  mjtNum* x1_k = status->x1;               // the kth approximation point for obj1
+  mjtNum* x2_k = status->x2;               // the kth approximation point for obj2
+  mjtNum x_k[3];                           // the kth approximation point in Minkowski difference
+  mjtNum lambda[4];                        // barycentric coordinates for x_k
+  mjtNum cutoff2 = status->dist_cutoff * status->dist_cutoff;
 
   // if both geoms are discrete, finite convergence is guaranteed; set tolerance to 0
   mjtNum epsilon = discreteGeoms(obj1, obj2) ? 0 : status->tolerance * status->tolerance;
@@ -182,14 +184,29 @@ static mjtNum gjk(mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) {
 
     // if the hyperplane separates the Minkowski difference and origin, the objects don't collide
     // if geom distance isn't requested, return early
-    if (!get_dist && dot3(x_k, s_k) > 0) {
-      return mjMAXVAL;
+    if (!get_dist) {
+      if (dot3(x_k, s_k) > 0) {
+        status->gjk_iterations = k;
+        status->nsimplex = 0;
+        status->nx = 0;
+        return mjMAXVAL;
+      }
+    } else if (status->dist_cutoff < mjMAXVAL) {
+      mjtNum vs = mju_dot3(x_k, s_k), vv = mju_dot3(x_k, x_k);
+      if (mju_dot3(x_k, s_k) > 0 && (vs*vs / vv) >= cutoff2) {
+        status->gjk_iterations = k;
+        status->nsimplex = 0;
+        status->nx = 0;
+        return mjMAXVAL;
+      }
     }
 
     // tetrahedron is generated and only need contact info; fallback to gjkIntersect to
     // determine contact
     if (!get_dist && n == 3) {
-      return gjkIntersect(status, k, obj1, obj2) ? 0 : mjMAXVAL;
+      status->gjk_iterations = k;
+      status->nx = 0;
+      return gjkIntersect(status, obj1, obj2) > 0 ? 0 : mjMAXVAL;
     }
 
     // run the distance subalgorithm to compute the barycentric coordinates
@@ -228,10 +245,11 @@ static mjtNum gjk(mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) {
   lincomb(x1_k, lambda, simplex1, n);
   lincomb(x2_k, lambda, simplex2, n);
 
+  status->nx = 1;
   status->gjk_iterations = k;
   status->nsimplex = n;
-  status->gjk_dist = mju_norm3(x_k);
-  return status->gjk_dist;
+  status->dist = mju_norm3(x_k);
+  return status->dist;
 }
 
 
@@ -329,16 +347,16 @@ static inline mjtNum signedDistance(mjtNum normal[3], const mjtNum v1[3], const
 
 
 
-// returns 0 if objects are in contact, mjMAXVAL otherwise; status must have initial tetrahedrons
-static int gjkIntersect(mjCCDStatus* status, int start, mjCCDObj* obj1, mjCCDObj* obj2) {
+// returns 1 if objects are in contact; 0 if not; -1 if inconclusive
+static int gjkIntersect(mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) {
   mjtNum simplex1[12], simplex2[12], simplex[12];
   memcpy(simplex1, status->simplex1, sizeof(mjtNum) * 12);
   memcpy(simplex2, status->simplex2, sizeof(mjtNum) * 12);
   memcpy(simplex, status->simplex, sizeof(mjtNum) * 12);
   int s[4] = {0, 3, 6, 9};
 
-  int kmax = status->max_iterations;
-  for (int k = start; k < kmax; k++) {
+  int k = status->gjk_iterations, kmax = status->max_iterations;
+  for (; k < kmax; k++) {
     // compute the signed distance to each face in the simplex along with normals
     mjtNum dist[4], normals[12];
     dist[0] = signedDistance(&normals[0], simplex + s[2], simplex + s[1], simplex + s[3]);
@@ -359,6 +377,7 @@ static int gjkIntersect(mjCCDStatus* status, int start, mjCCDObj* obj1, mjCCDObj
         copy3(status->simplex1 + 3*n, simplex1 + s[n]);
         copy3(status->simplex2 + 3*n, simplex2 + s[n]);
       }
+      status->gjk_iterations = k;
       return 1;
     }
 
@@ -368,6 +387,7 @@ static int gjkIntersect(mjCCDStatus* status, int start, mjCCDObj* obj1, mjCCDObj
 
     // found origin outside the Minkowski difference (return no collision)
     if (dot3(&normals[3*index], simplex + s[index]) < 0) {
+      status->gjk_iterations = k;
       return 0;
     }
 
@@ -378,7 +398,8 @@ static int gjkIntersect(mjCCDStatus* status, int start, mjCCDObj* obj1, mjCCDObj
     s[i] = s[j];
     s[j] = swap;
   }
-  return 0;  // never found origin
+  status->gjk_iterations = k;
+  return -1;  // never found origin
 }
 
 
@@ -993,7 +1014,7 @@ static int polytope3(Polytope* pt, const mjCCDStatus* status, mjCCDObj* obj1, mj
   // TODO(kylebayes): It's possible for GJK to return a 2-simplex with the origin not contained in
   // it but within tolerance from it. In that case the hexahedron could possibly be constructed
   // that doesn't contain the origin, but nonetheless there is penetration depth.
-  if (status->gjk_dist > 10*mjMINVAL && !testTetra(v1, v2, v3, v4) && !testTetra(v1, v2, v3, v5)) {
+  if (status->dist > 10*mjMINVAL && !testTetra(v1, v2, v3, v4) && !testTetra(v1, v2, v3, v5)) {
     return 7;
   }
 
@@ -1296,6 +1317,7 @@ static mjtNum epa(mjCCDStatus* status, Polytope* pt, mjCCDObj* obj1, mjCCDObj* o
   mj_freeStack(d);
   epaWitness(pt, face, status->x1, status->x2);
   status->epa_iterations = k;
+  status->nx = 1;
   return dist;
 }
 
@@ -1306,16 +1328,17 @@ mjtNum mjc_ccd(const mjCCDConfig* config, mjCCDStatus* status, mjCCDObj* obj1, m
   // set up
   obj1->center(status->x1, obj1);
   obj2->center(status->x2, obj2);
+  status->gjk_iterations = 0;
   status->epa_iterations = -1;
   status->tolerance = config->tolerance;
   status->max_iterations = config->max_iterations;
-  status->has_contacts = config->contacts;
-  status->has_distances = config->distances;
+  status->max_contacts = config->max_contacts;
+  status->dist_cutoff = config->dist_cutoff;
 
   mjtNum dist = gjk(status, obj1, obj2);
 
   // penetration recovery for contacts not needed
-  if (!config->contacts) {
+  if (!config->max_contacts) {
     return dist;
   }
 

src/engine/engine_collision_gjk.h

@@ -16,7 +16,9 @@
 #define MUJOCO_SRC_ENGINE_ENGINE_COLLISION_GJK_H_
 
 #include <mujoco/mjexport.h>
+#include <mujoco/mjmodel.h>
 #include <mujoco/mjtnum.h>
+
 #include "engine/engine_collision_convex.h"
 
 #ifdef __cplusplus
@@ -27,30 +29,32 @@ extern "C" {
 struct _mjCCDConfig {
   int max_iterations;   // the maximum number of iterations for GJK and EPA
   mjtNum tolerance;     // tolerance used by GJK and EPA
-  int contacts;         // set to true to recover contact (pendetration) info
-  int distances;        // set to true to recover distance info
+  int max_contacts;     // set to max number of contact points to recover
+  mjtNum dist_cutoff;   // set to max geom distance to recover
 };
 typedef struct _mjCCDConfig mjCCDConfig;
 
 // data produced from running GJK and EPA
 struct _mjCCDStatus {
-  mjtNum x1[3];              // witness point for geom 1
-  mjtNum x2[3];              // witness point for geom 2
+  // geom distance information
+  mjtNum dist;                  // distance between geoms
+  mjtNum x1[3 * mjMAXCONPAIR];  // witness points for geom 1
+  mjtNum x2[3 * mjMAXCONPAIR];  // witness points for geom 2
+  int nx;                       // number of witness points
 
   // configurations used
-  int max_iterations;         // the maximum number of iterations for GJK and EPA
-  mjtNum tolerance;           // tolerance used by GJK and EPA
-  int has_contacts;           // set to true if attempted to recover contact (pendetration) info
-  int has_distances;          // set to true if attempted to recover distance info
+  int max_iterations;           // the maximum number of iterations for GJK and EPA
+  mjtNum tolerance;             // tolerance used by GJK and EPA
+  int max_contacts;             // set to max number of contact points to recover
+  mjtNum dist_cutoff;           // set to max geom distance to recover
 
   // statistics for debugging purposes
-  mjtNum gjk_dist;             // the distance returned by GJK
-  int gjk_iterations;         // number of iterations that GJK ran
-  int epa_iterations;         // number of iterations that EPA ran (negative if EPA did not run)
-  mjtNum simplex1[12];        // the simplex that GJK returned for obj1
-  mjtNum simplex2[12];        // the simplex that GJK returned for obj2
-  mjtNum simplex[12];         // the simplex that GJK returned for the Minkowski difference
-  int nsimplex;               // size of simplex 1 & 2
+  int gjk_iterations;           // number of iterations that GJK ran
+  int epa_iterations;           // number of iterations that EPA ran (negative if EPA did not run)
+  mjtNum simplex1[12];          // the simplex that GJK returned for obj1
+  mjtNum simplex2[12];          // the simplex that GJK returned for obj2
+  mjtNum simplex[12];           // the simplex that GJK returned for the Minkowski difference
+  int nsimplex;                 // size of simplex 1 & 2
 };
 typedef struct _mjCCDStatus mjCCDStatus;
 

src/engine/engine_support.c

@@ -1409,15 +1409,15 @@ void mj_objectAcceleration(const mjModel* m, const mjData* d,
 
 // returns the smallest distance between two geoms (using nativeccd)
 static mjtNum mj_geomDistanceCCD(const mjModel* m, const mjData* d, int g1, int g2,
-                                 mjtNum fromto[6]) {
+                                 mjtNum distmax, mjtNum fromto[6]) {
   mjCCDConfig config;
   mjCCDStatus status;
 
   // set config
   config.max_iterations = m->opt.ccd_iterations;
   config.tolerance = m->opt.ccd_tolerance;
-  config.contacts = 1;   // want contacts
-  config.distances = 1;  // want geom distances
+  config.max_contacts = 1;        // want contacts
+  config.dist_cutoff = distmax;   // want geom distances
 
   mjCCDObj obj1, obj2;
   mjc_initCCDObj(&obj1, m, d, g1, 0);
@@ -1425,7 +1425,7 @@ static mjtNum mj_geomDistanceCCD(const mjModel* m, const mjData* d, int g1, int
 
   mjtNum dist = mjc_ccd(&config, &status, &obj1, &obj2);
 
-  if (fromto) {
+  if (fromto && status.nx > 0) {
     mju_copy3(fromto, status.x1);
     mju_copy3(fromto+3, status.x2);
   }
@@ -1459,7 +1459,7 @@ mjtNum mj_geomDistance(const mjModel* m, const mjData* d, int geom1, int geom2,
   // use nativecdd if flag is enabled
   if (mjENABLED(mjENBL_NATIVECCD)) {
     if (func == mjc_Convex || func == mjc_BoxBox) {
-      return mj_geomDistanceCCD(m, d, g1, g2, fromto);
+      return mj_geomDistanceCCD(m, d, g1, g2, distmax, fromto);
     }
   }
 

test/engine/engine_collision_gjk_test.cc

@@ -56,23 +56,25 @@ constexpr char kEllipoid[] = R"(
 </mujoco>)";
 
 mjtNum GeomDist(mjModel* m, mjData* d, int g1, int g2, mjtNum x1[3],
-                mjtNum x2[3]) {
+                mjtNum x2[3], mjtNum cutoff = mjMAXVAL) {
   mjCCDConfig config;
   mjCCDStatus status;
 
   // set config
   config.max_iterations = kMaxIterations,
   config.tolerance = kTolerance,
-  config.contacts = 0;   // no geom contacts needed
-  config.distances = 1;
+  config.max_contacts = 0;   // no geom contacts needed
+  config.dist_cutoff = cutoff;
 
   mjCCDObj obj1, obj2;
   mjc_initCCDObj(&obj1, m, d, g1, 0);
   mjc_initCCDObj(&obj2, m, d, g2, 0);
 
   mjtNum dist = mjc_ccd(&config, &status, &obj1, &obj2);
-  if (x1 != nullptr) mju_copy3(x1, status.x1);
-  if (x2 != nullptr) mju_copy3(x2, status.x2);
+  if (status.nx > 0) {
+    if (x1 != nullptr) mju_copy3(x1, status.x1);
+    if (x2 != nullptr) mju_copy3(x2, status.x2);
+  }
   return dist;
 }
 
@@ -85,8 +87,8 @@ int PenetrationWrapper(mjCCDObj* obj1, mjCCDObj* obj2, const ccd_t* ccd,
   // set config
   config.max_iterations = ccd->max_iterations,
   config.tolerance = ccd->mpr_tolerance,
-  config.contacts = 1;
-  config.distances = 0;  // no geom distances needed
+  config.max_contacts = 1;
+  config.dist_cutoff = 0;  // no geom distances needed
 
   mjtNum dist = mjc_ccd(&config, &status, obj1, obj2);
   if (dist < 0) {
@@ -141,16 +143,10 @@ using MjGjkTest = MujocoTest;
 TEST_F(MjGjkTest, SphereSphereDist) {
   static constexpr char xml[] = R"(
   <mujoco>
-  <worldbody>
-    <body pos="-1.5 0 0">
-      <freejoint/>
-      <geom name="geom1" type="sphere" size="1"/>
-    </body>
-    <body pos="1.5 0 0">
-      <freejoint/>
-      <geom name="geom2" type="sphere" size="1"/>
-    </body>
-  </worldbody>
+    <worldbody>
+      <geom name="geom1" type="sphere" pos="-1.5 0 0" size="1"/>
+      <geom name="geom2" type="sphere" pos="1.5 0 0" size="1"/>
+    </worldbody>
   </mujoco>)";
 
   std::array<char, 1000> error;
@@ -172,19 +168,38 @@ TEST_F(MjGjkTest, SphereSphereDist) {
   mj_deleteModel(model);
 }
 
+TEST_F(MjGjkTest, SphereSphereDistCutoff) {
+  static constexpr char xml[] = R"(
+  <mujoco>
+    <worldbody>
+      <geom name="geom1" type="sphere" pos="-1.5 0 0" size="1"/>
+      <geom name="geom2" type="sphere" pos="1.5 0 0" size="1"/>
+    </worldbody>
+  </mujoco>)";
+
+  std::array<char, 1000> error;
+  mjModel* model = LoadModelFromString(xml, error.data(), error.size());
+  ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
+
+  mjData* data = mj_makeData(model);
+  mj_forward(model, data);
+
+  int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1");
+  int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2");
+  mjtNum dist = GeomDist(model, data, geom1, geom2, nullptr, nullptr, .999999);
+
+  EXPECT_EQ(dist, mjMAXVAL);
+  mj_deleteData(data);
+  mj_deleteModel(model);
+}
+
 TEST_F(MjGjkTest, SphereSphereNoDist) {
   static constexpr char xml[] = R"(
   <mujoco>
-  <worldbody>
-    <body pos="-1.5 0 0">
-      <freejoint/>
-      <geom name="geom1" type="sphere" size="1"/>
-    </body>
-    <body pos="1.5 0 0">
-      <freejoint/>
-      <geom name="geom2" type="sphere" size="1"/>
-    </body>
-  </worldbody>
+    <worldbody>
+      <geom name="geom1" type="sphere" pos="-1.5 0 0" size="1"/>
+      <geom name="geom2" type="sphere" pos="1.5 0 0" size="1"/>
+    </worldbody>
   </mujoco>)";
 
   std::array<char, 1000> error;