Fix bugs in nativeccd with collisions with margin.

PiperOrigin-RevId: 730903745
Change-Id: I45ddb674f32cb1f28ff37c587ea953ec595241b3

src/engine/engine_collision_convex.c

@@ -889,6 +889,28 @@ static void mju_rotateFrame(const mjtNum origin[3], const mjtNum rot[9],
 
 
 
+// return true if multiccd can run in a single pass
+static int singlePass(const mjCCDObj* obj1, const mjCCDObj* obj2) {
+  const mjModel* m = obj1->model;
+
+  // single pass not supported for margins
+  if (obj1->margin > 0 || obj2->margin > 0) {
+    return 0;
+  }
+
+  // supported geoms for single pass
+  int type1 = m->geom_type[obj1->geom];
+  int type2 = m->geom_type[obj2->geom];
+  if (type1 == mjGEOM_BOX || type1 == mjGEOM_MESH) {
+    if (type2 == mjGEOM_BOX || type2 == mjGEOM_MESH) {
+      return 1;
+    }
+  }
+  return 0;
+}
+
+
+
 // multi-point convex-convex collision, using libccd
 int mjc_Convex(const mjModel* m, const mjData* d,
                mjContact* con, int g1, int g2, mjtNum margin) {
@@ -897,7 +919,8 @@ int mjc_Convex(const mjModel* m, const mjData* d,
   mjc_initCCDObj(&obj1, m, d, g1, margin);
   mjc_initCCDObj(&obj2, m, d, g2, margin);
   int max_contacts = 1;
-  if (mjENABLED(mjENBL_MULTICCD)) {
+
+  if (mjENABLED(mjENBL_MULTICCD) && singlePass(&obj1, &obj2)) {
     max_contacts = 4;
   }
 
@@ -905,9 +928,7 @@ int mjc_Convex(const mjModel* m, const mjData* d,
   int ncon = mjc_CCDIteration(m, d, &obj1, &obj2, con, max_contacts, margin);
 
   // nativeccd supports multi Box-Box collision directly
-  if (mjENABLED(mjENBL_NATIVECCD) &&
-    (m->geom_type[g1] == mjGEOM_BOX || m->geom_type[g1] == mjGEOM_MESH) &&
-    (m->geom_type[g2] == mjGEOM_BOX || m->geom_type[g2] == mjGEOM_MESH)) {
+  if (mjENABLED(mjENBL_NATIVECCD) && singlePass(&obj1, &obj2)) {
     return ncon;
   }
 

src/engine/engine_collision_gjk.c

@@ -2222,39 +2222,46 @@ mjtNum mjc_ccd(const mjCCDConfig* config, mjCCDStatus* status, mjCCDObj* obj1, m
       obj1->geom_type == mjGEOM_CAPSULE || obj2->geom_type == mjGEOM_CAPSULE) {
     void (*support1)(mjtNum*, struct _mjCCDObj*, const mjtNum*) = obj1->support;
     void (*support2)(mjtNum*, struct _mjCCDObj*, const mjtNum*) = obj2->support;
-    mjtNum margin1 = 0, margin2 = 0;
+    mjtNum full_margin1 = 0, full_margin2 = 0;
+    mjtNum margin1 = obj1->margin, margin2 = obj2->margin;
 
     if (obj1->geom_type == mjGEOM_SPHERE) {
       const mjModel* m = obj1->model;
-      margin1 = m->geom_size[3*obj1->geom];
-      support1 = obj1->support;
+      full_margin1 = m->geom_size[3*obj1->geom] + 0.5*margin1;
       obj1->support = mjc_pointSupport;
+      obj1->margin = 0;
     } else if (obj1->geom_type == mjGEOM_CAPSULE) {
       const mjModel* m = obj1->model;
-      margin1 = m->geom_size[3*obj1->geom];
-      support1 = obj1->support;
+      full_margin1 = m->geom_size[3*obj1->geom] + 0.5*margin1;
       obj1->support = mjc_lineSupport;
+      obj1->margin = 0;
     }
 
     if (obj2->geom_type == mjGEOM_SPHERE) {
       const mjModel* m = obj2->model;
-      margin2 = m->geom_size[3*obj2->geom];
-      support2 = obj2->support;
+      full_margin2 = m->geom_size[3*obj2->geom] + 0.5*margin2;
       obj2->support = mjc_pointSupport;
+      obj2->margin = 0;
     } else if (obj2->geom_type == mjGEOM_CAPSULE) {
       const mjModel* m = obj2->model;
-      margin2 = m->geom_size[3*obj2->geom];
-      support2 = obj2->support;
+      full_margin2 = m->geom_size[3*obj2->geom] + 0.5*margin2;
       obj2->support = mjc_lineSupport;
+      obj2->margin = 0;
     }
 
-    status->dist_cutoff += margin1 + margin2;
+    status->dist_cutoff += full_margin1 + full_margin2;
     gjk(status, obj1, obj2);
     status->dist_cutoff = config->dist_cutoff;
 
+    // restore original margin and support
+    obj1->margin = margin1;
+    obj2->margin = margin2;
+    obj1->support = support1;
+    obj2->support = support2;
+
     // shallow penetration, inflate contact
-    if (status->dist > 0) {
-      inflate(status, margin1, margin2);
+    if (status->dist > status->tolerance) {
+      inflate(status, full_margin1, full_margin2);
       if (status->dist > status->dist_cutoff) {
         status->dist = mjMAXVAL;
       }
@@ -2268,14 +2275,11 @@ mjtNum mjc_ccd(const mjCCDConfig* config, mjCCDStatus* status, mjCCDObj* obj1, m
       return 0;
     }
 
-    // deep penetration, reset everything and run GJK again
+    // deep penetration, reset initial conditions and rerun GJK + EPA
     status->gjk_iterations = 0;
-    obj1->support = support1;
-    obj2->support = support2;
     obj1->center(status->x1, obj1);
     obj2->center(status->x2, obj2);
   }
-
   gjk(status, obj1, obj2);
 
   // penetration recovery for contacts not needed

test/engine/engine_collision_gjk_test.cc

@@ -1568,5 +1568,38 @@ TEST_F(MjGjkTest, CapsuleCapsule) {
   mj_deleteModel(model);
 }
 
+TEST_F(MjGjkTest, CylinderBoxMargin) {
+  static constexpr char xml[] = R"(
+  <mujoco>
+    <statistic meansize="0.15"/>
+    <option>
+      <flag gravity="disable"/>
+    </option>
+
+    <worldbody>
+      <body pos="0 0 .265">
+        <freejoint/>
+        <geom type="box" size=".05 .05 .05" margin="0.1" gap="0.1"/>
+      </body>
+      <body mocap="true">
+        <geom name="geom2" type="cylinder" size=".2 .2"/>
+      </body>
+    </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);
+
+  EXPECT_EQ(data->ncon, 1);
+  EXPECT_LT(data->contact[0].efc_address, 0);
+
+  mj_deleteData(data);
+  mj_deleteModel(model);
+}
+
 }  // namespace
 }  // namespace mujoco