Support margin with nativeccd multicontact.

PiperOrigin-RevId: 1000723014
Change-Id: I411e1e346b8e26e4857c321d08e7e8e77a69feff

Author: kbayes

doc/changelog.rst

@@ -9,6 +9,7 @@ Engine
 ^^^^^^
 - Trees marked :ref:`sleep="init"<body-sleep>` which cannot be put to sleep now cause a warning rather than an error,
   and start awake. See :ref:`Sleeping<siSleepSleeping>` for details.
+- Added single-shot :ref:`multicontact<coMultiCCD>` for geoms with positive contact :ref:`margins<body-geom-margin>`.
 
 Python bindings
 ^^^^^^^^^^^^^^^

doc/computation/index.rst

@@ -1860,14 +1860,13 @@ multi-run pipeline (legacy)
   Multiple contact points are found by rotating the two geoms by ±1e-3 radians around the tangential axes and
   re-running the collision routine. If a new contact is detected it is added, allowing for up to 4 additional contact
   points. This method is effective, but increases the cost of each collision call by a factor of 5.  This method is
-  used when the :ref:`nativeccd<option-flag-nativeccd>` flag is disabled, and for geom collisions involving capsules
-  or with :ref:`positive contact margins<body-geom-margin>`.
+  used when the :ref:`nativeccd<option-flag-nativeccd>` flag is disabled.
 
 single-shot pipeline
   The single-shot pipeline is used in conjunction with the native CCD pipeline, i.e., when the
   :ref:`nativeccd<option-flag-nativeccd>` flag is enabled. As this pipeline is one-shot and most of the geom analysis
-  is done at compilation time, there is very little performance overhead. Supported geoms are boxes, cylinders, and meshes
-  without :ref:`positive contact margins<body-geom-margin>`.
+  is done at compilation time, there is very little performance overhead. Supported geoms are capsules, cylinders,
+  boxes, and meshes.
 
 .. _coDistance:
 
@@ -1913,8 +1912,7 @@ Pair-wise colliders
 
 The table below provides information about the colliders used for different geom pairs. These values can be computed
 dynamically by the :ref:`mj_maxContact` function. Use the toggles to see the max number of contacts returned with the
-parameters :ref:`nativeccd<option-flag-nativeccd>`, :ref:`multiccd<option-flag-multiccd>`, and
-:ref:`margin<body-geom-margin>`.
+parameters :ref:`nativeccd<option-flag-nativeccd>` and :ref:`multiccd<option-flag-multiccd>`.
 
 .. raw:: html
 
@@ -1933,13 +1931,6 @@ parameters :ref:`nativeccd<option-flag-nativeccd>`, :ref:`multiccd<option-flag-m
        </label>
        <span>multiccd</span>
      </div>
-     <div class="pairwise-toggle-item">
-       <label class="pairwise-switch">
-         <input type="checkbox" id="margin-checkbox">
-         <span class="pairwise-slider"></span>
-       </label>
-       <span>with margin</span>
-     </div>
    </div>
 
 .. list-table::
@@ -2134,8 +2125,7 @@ parameters :ref:`nativeccd<option-flag-nativeccd>`, :ref:`multiccd<option-flag-m
        const table = document.querySelector('.table-pairwise');
        const toggles = [
          {id: 'nativeccd-checkbox', cls: 'nativeccd-enabled'},
-         {id: 'multiccd-checkbox', cls: 'multiccd-enabled'},
-         {id: 'margin-checkbox', cls: 'margin-enabled'}
+         {id: 'multiccd-checkbox', cls: 'multiccd-enabled'}
        ];
 
        toggles.forEach(toggle => {

doc/css/theme_overrides.css

@@ -124,24 +124,11 @@ body[data-theme="dark"] table.docutils:not(.mjcf-attributes) {
   display: none;
 }
 
-.multiccd-enabled.nativeccd-enabled:not(.margin-enabled) .multiccd-native {
+.multiccd-enabled.nativeccd-enabled .multiccd-native {
   display: inline;
 }
 
-.multiccd-enabled:not(.nativeccd-enabled) .multiccd-legacy,
-.multiccd-enabled.nativeccd-enabled.margin-enabled .multiccd-legacy {
-  display: inline;
-}
-
-.margin-show {
-  display: none;
-}
-
-.margin-enabled .margin-hide {
-  display: none;
-}
-
-.margin-enabled .margin-show {
+.multiccd-enabled:not(.nativeccd-enabled) .multiccd-legacy {
   display: inline;
 }
 

src/engine/engine_collision_convex.c

@@ -903,8 +903,8 @@ static void mju_rotateFrame(const mjtNum origin[3], const mjtNum rot[9],
 
 // return number of contacts supported by a single pass of narrowphase
 static int maxContacts(const mjModel* m, const mjCCDObj* obj1, const mjCCDObj* obj2) {
-  // single pass not supported for margins nor libccd
-  if (obj1->margin > 0 || obj2->margin > 0 || mjDISABLED(mjDSBL_NATIVECCD)) {
+  // single pass not supported for libccd
+  if (mjDISABLED(mjDSBL_NATIVECCD)) {
     return 1;
   }
 

src/engine/engine_collision_driver.c

@@ -64,8 +64,7 @@ mjfCollision mjCOLLISIONFUNC[mjNGEOMTYPES][mjNGEOMTYPES] = {
 
 
 // return the maximum number of contacts that can be generated between two geoms
-// if has_margin is -1, then the margin is pulled from the model, otherwise if has_margin > 0
-// indicates that the geoms have a positive margin
+// has_margin is ignored
 int mj_maxContact(const mjModel* m, int g1, int g2, int has_margin) {
   int type1 = m->geom_type[g1];
   int type2 = m->geom_type[g2];
@@ -129,34 +128,6 @@ int mj_maxContact(const mjModel* m, int g1, int g2, int has_margin) {
     return 5;
   }
 
-  // check margin from model
-  if (has_margin < 0) {
-    has_margin = 0;
-    if (mjENABLED(mjENBL_OVERRIDE)) {
-      has_margin = m->opt.o_margin > 0.0;
-    } else {
-      int npair = m->npair;
-      int ipair = -1;
-      for (int k=0; k < npair; k++) {
-        if ((m->pair_geom1[k] == g1 && m->pair_geom2[k] == g2) ||
-            (m->pair_geom1[k] == g2 && m->pair_geom2[k] == g1)) {
-            ipair = k;
-            break;
-          }
-        }
-
-      if (ipair > -1) {
-        has_margin = m->pair_margin[ipair] > 0.0;
-      } else {
-        has_margin = m->geom_margin[g1] > 0.0 || m->geom_margin[g2] > 0.0;
-      }
-    }
-  }
-
-  if (has_margin) {
-    return 5;
-  }
-
   // colliding geoms with flat faces
   if (hasface1 && hasface2) {
     return 4;

src/engine/engine_collision_driver.h

@@ -28,8 +28,7 @@ extern "C" {
 MJAPI extern mjfCollision mjCOLLISIONFUNC[mjNGEOMTYPES][mjNGEOMTYPES];
 
 // return the maximum number of contacts that can be generated between two geoms
-// if has_margin is -1, then the margin is pulled from the model, otherwise if has_margin > 0
-// indicates that the geoms have a positive margin
+// has_margin is ignored
 MJAPI int mj_maxContact(const mjModel* m, int g1, int g2, int has_margin);
 
 // collision detection entry point