Replace margin/gap max with sum.

PiperOrigin-RevId: 868842521
Change-Id: Ia4a4ca5c6821c5a7b3ba7b0af7f71d981c2866c1

doc/changelog.rst

@@ -77,6 +77,9 @@ General
 
    - Removed ``getdir`` from the ``mjpResourceProvider`` struct. All Resource Providers now use the same shared
      implementation.
+   - When combining the ``margin`` and ``gap`` :ref:`parameters<CContact>` of two geoms to obtain the parameters
+     of a contact, the respective values are now **summed** rather than than taking the maximum. This allows geom
+     margins to be a proper "inflation" of the geom.
 
 - Camera frustum visualization is now triggered by setting :ref:`resolution<body-camera-resolution>` to values larger
   than 1. Relatedly, frustum visualization also works for :ref:`orthographic<body-camera-projection>` cameras.

doc/modeling.rst

@@ -490,7 +490,7 @@ are as follows:
    an individual geom. This is why MuJoCo does not allow anisotropic friction in the individual geom specifications, but
    only in the explicit contact pair specifications.
 **margin**, **gap**
-   The maximum of the two geom margins (or gaps respectively) is used. The geom priority is ignored here, because the
+   The sum of the two geom margins (or gaps respectively) is used. The geom priority is ignored here, because the
    margin and gap are distance properties and a one-sided specification makes little sense.
 
 .. _solmixing:

mjx/mujoco/mjx/_src/collision_driver.py

@@ -283,8 +283,8 @@ def _contact_groups(m: Model, d: Data) -> Dict[FunctionKey, Contact]:
       ))
     if geom1.size > 0 and geom2.size > 0:
       # other contacts get their params from geom fields
-      margin = jp.maximum(m.geom_margin[geom1], m.geom_margin[geom2])
+      margin = m.geom_margin[geom1] + m.geom_margin[geom2]
-      gap = jp.maximum(m.geom_gap[geom1], m.geom_gap[geom2])
+      gap = m.geom_gap[geom1] + m.geom_gap[geom2]
       solmix1, solmix2 = m.geom_solmix[geom1], m.geom_solmix[geom2]
       mix = solmix1 / (solmix1 + solmix2)
       mix = jp.where((solmix1 < eps) & (solmix2 < eps), 0.5, mix)

mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_convex.py

@@ -91,7 +91,7 @@ def _hfield_filter(
   r2 = geom_rbound[rbound_id, g2]
 
   # TODO(team): margin?
-  margin = wp.max(geom_margin[margin_id, g1], geom_margin[margin_id, g2])
+  margin = geom_margin[margin_id, g1] + geom_margin[margin_id, g2]
 
   # box-sphere test: horizontal plane
   for i in range(2):

mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_driver.py

@@ -101,18 +101,18 @@ def _plane_filter(
   if size1 == 0.0:
     # geom1 is a plane
     dist = wp.dot(xpos2 - xpos1, wp.vec3(xmat1[0, 2], xmat1[1, 2], xmat1[2, 2]))
-    return dist <= size2 + wp.max(margin1, margin2)
+    return dist <= size2 + margin1 + margin2
   elif size2 == 0.0:
     # geom2 is a plane
     dist = wp.dot(xpos1 - xpos2, wp.vec3(xmat2[0, 2], xmat2[1, 2], xmat2[2, 2]))
-    return dist <= size1 + wp.max(margin1, margin2)
+    return dist <= size1 + margin1 + margin2
 
   return True
 
 
 @wp.func
 def _sphere_filter(size1: float, size2: float, margin1: float, margin2: float, xpos1: wp.vec3, xpos2: wp.vec3) -> bool:
-  bound = size1 + size2 + wp.max(margin1, margin2)
+  bound = size1 + size2 + margin1 + margin2
   dif = xpos2 - xpos1
   dist_sq = wp.dot(dif, dif)
   return dist_sq <= bound * bound
@@ -141,7 +141,7 @@ def _aabb_filter(
   center1 = xmat1 @ center1 + xpos1
   center2 = xmat2 @ center2 + xpos2
 
-  margin = wp.max(margin1, margin2)
+  margin = margin1 + margin2
 
   max_x1 = -MJ_MAXVAL
   max_y1 = -MJ_MAXVAL
@@ -236,7 +236,7 @@ def _obb_filter(
   xmat2: wp.mat33,
 ) -> bool:
   """Oriented bounding boxes collision (see Gottschalk et al.), see mj_collideOBB."""
-  margin = wp.max(margin1, margin2)
+  margin = margin1 + margin2
 
   xcenter = mat23()
   normal = mat63()

mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_primitive.py

@@ -552,8 +552,8 @@ def contact_params(
     solreffriction = wp.vec2(0.0, 0.0)
     solimp = mix * geom_solimp[solimp_id, g1] + (1.0 - mix) * geom_solimp[solimp_id, g2]
     # geom priority is ignored
-    margin = wp.max(geom_margin[margin_id, g1], geom_margin[margin_id, g2])
+    margin = geom_margin[margin_id, g1] + geom_margin[margin_id, g2]
-    gap = wp.max(geom_gap[gap_id, g1], geom_gap[gap_id, g2])
+    gap = geom_gap[gap_id, g1] + geom_gap[gap_id, g2]
 
   friction = vec5(
     wp.max(MJ_MINMU, friction[0]),

src/engine/engine_collision_driver.c

@@ -689,8 +689,7 @@ void mj_collideTree(const mjModel* m, mjData* d, int bf1, int bf2,
     if (isbody1 && isbody2) {
       // both are leaves
       if (isleaf1 && isleaf2) {
-        mjtNum maxmargin = mju_max(m->geom_margin[nodeid1], m->geom_margin[nodeid2]);
+        mjtNum margin = mj_assignMargin(m, m->geom_margin[nodeid1] + m->geom_margin[nodeid2]);
-        mjtNum margin = mj_assignMargin(m, maxmargin);
 
         if (!mj_filterSphere(m, d, nodeid1, nodeid2, margin)) {
           if (mj_collideOBB(m->geom_aabb + 6*nodeid1, m->geom_aabb + 6*nodeid2,
@@ -708,8 +707,7 @@ void mj_collideTree(const mjModel* m, mjData* d, int bf1, int bf2,
       }
 
       // if no intersection at intermediate levels, stop
-      mjtNum maxmargin = mju_max(m->body_margin[bf1], m->body_margin[bf2]);
+      mjtNum margin = mj_assignMargin(m, m->body_margin[bf1] + m->body_margin[bf2]);
-      mjtNum margin = mj_assignMargin(m, maxmargin);
       if (!mj_collideOBB(bvh1 + 6*node1, bvh2 + 6*node2,
                          d->xipos + 3*bf1, d->ximat + 9*bf1,
                          d->xipos + 3*bf2, d->ximat + 9*bf2,
@@ -722,8 +720,7 @@ void mj_collideTree(const mjModel* m, mjData* d, int bf1, int bf2,
     else if (isbody1 && !isbody2) {
       // both are leaves
       if (isleaf1 && isleaf2) {
-        mjtNum maxmargin = mju_max(m->geom_margin[nodeid1], m->flex_margin[f2]);
+        mjtNum margin = mj_assignMargin(m, m->geom_margin[nodeid1] + m->flex_margin[f2]);
-        mjtNum margin = mj_assignMargin(m, maxmargin);
 
         if (!filterBitmask(m->geom_contype[nodeid1], m->geom_conaffinity[nodeid1],
                            m->flex_contype[f2], m->flex_conaffinity[f2]) &&
@@ -747,8 +744,7 @@ void mj_collideTree(const mjModel* m, mjData* d, int bf1, int bf2,
       }
 
       // if no intersection at intermediate levels, stop
-      mjtNum maxmargin = mju_max(m->body_margin[bf1], m->flex_margin[f2]);
+      mjtNum margin = mj_assignMargin(m, m->body_margin[bf1] + m->flex_margin[f2]);
-      mjtNum margin = mj_assignMargin(m, maxmargin);
       if (!mj_collideOBB(bvh1 + 6*node1, bvh2 + 6*node2,
                          d->xipos + 3*bf1, d->ximat + 9*bf1,
                          NULL, NULL,
@@ -776,8 +772,7 @@ void mj_collideTree(const mjModel* m, mjData* d, int bf1, int bf2,
       }
 
       // if no intersection at intermediate levels, stop
-      mjtNum maxmargin = mju_max(m->flex_margin[f1], m->flex_margin[f2]);
+      mjtNum margin = mj_assignMargin(m, m->flex_margin[f1] + m->flex_margin[f2]);
-      mjtNum margin = mj_assignMargin(m, maxmargin);
       if (filterBox(bvh1 + 6*node1, bvh2 + 6*node2, margin)) {
         continue;
       }
@@ -1330,8 +1325,8 @@ static void mj_contactParam(const mjModel* m, int* condim, mjtNum* gap,
   const mjtNum* solimp2 =   (f2 < 0) ? m->geom_solimp+g2*mjNIMP : m->flex_solimp+f2*mjNIMP;
   const mjtNum* friction2 = (f2 < 0) ? m->geom_friction+g2*3    : m->flex_friction+f2*3;
 
-  // gap: max
+  // gap: add
-  *gap = mju_max(gap1, gap2);
+  *gap = gap1 + gap2;
 
   // different priority: copy from item with higher priority
   if (priority1 > priority2) {
@@ -1513,7 +1508,7 @@ void mj_collideGeoms(const mjModel* m, mjData* d, int g1, int g2) {
 
   // set margin: dynamic or pair
   if (ipair < 0) {
-    margin = mj_assignMargin(m, mju_max(m->geom_margin[g1], m->geom_margin[g2]));
+    margin = mj_assignMargin(m, m->geom_margin[g1] + m->geom_margin[g2]);
   } else {
     margin = mj_assignMargin(m, m->pair_margin[ipair]);
   }
@@ -1668,7 +1663,7 @@ void mj_collidePlaneFlex(const mjModel* m, mjData* d, int g, int f) {
   mjtNum nrm[3] = {mat[2], mat[5], mat[8]};
 
   // prepare contact parameters (same for all vertices)
-  mjtNum margin = mj_assignMargin(m, mju_max(m->geom_margin[g], m->flex_margin[f]));
+  mjtNum margin = mj_assignMargin(m, m->geom_margin[g] + m->flex_margin[f]);
   int condim;
   int flex_vertnum = m->flex_vertnum[f];
   mjtNum gap, solref[mjNREF], solimp[mjNIMP], friction[5];
@@ -1869,7 +1864,7 @@ void mj_collideFlexSAP(const mjModel* m, mjData* d, int f) {
 
 // test a geom and an elem for collision, add to contact list
 void mj_collideGeomElem(const mjModel* m, mjData* d, int g, int f, int e) {
-  mjtNum margin = mj_assignMargin(m, mju_max(m->geom_margin[g], m->flex_margin[f]));
+  mjtNum margin = mj_assignMargin(m, m->geom_margin[g] + m->flex_margin[f]);
   int dim = m->flex_dim[f], type = m->geom_type[g];
   int num;
 
@@ -2005,7 +2000,7 @@ void mj_collideGeomElem(const mjModel* m, mjData* d, int g, int f, int e) {
 
 // test two elems for collision, add to contact list
 void mj_collideElems(const mjModel* m, mjData* d, int f1, int e1, int f2, int e2) {
-  mjtNum margin = mj_assignMargin(m, mju_max(m->flex_margin[f1], m->flex_margin[f2]));
+  mjtNum margin = mj_assignMargin(m, m->flex_margin[f1] + m->flex_margin[f2]);
   int dim1 = m->flex_dim[f1], dim2 = m->flex_dim[f2];
   int num;
 

test/engine/engine_collision_driver_test.cc

@@ -357,5 +357,38 @@ TEST_F(MjCollisionTest, PinchingSucceeds) {
   mj_deleteModel(m);
 }
 
+TEST_F(MjCollisionTest, MarginSumming) {
+  // Two spheres with size 0.1, placed 0.21 apart (distance of 0.01).
+  // With margin summing, margin1 + margin2 = 0.00999 + 0.00999 = 0.01998 > 0.01
+  // so a contact should be generated.
+  constexpr char xml[] = R"(
+  <mujoco>
+    <worldbody>
+      <body>
+        <geom name="sphere1" type="sphere" size=".1" margin="0.00999"/>
+        <joint type="slide" axis="1 0 0"/>
+      </body>
+      <body pos=".21 0 0">
+        <geom name="sphere2" type="sphere" size=".1" margin="0.00999"/>
+        <joint type="slide" axis="1 0 0"/>
+      </body>
+    </worldbody>
+  </mujoco>
+  )";
+  char error[1024];
+  mjModel* m = LoadModelFromString(xml, error, sizeof(error));
+  ASSERT_THAT(m, NotNull()) << error;
+  mjData* d = mj_makeData(m);
+  ASSERT_THAT(d, NotNull());
+
+  mj_fwdPosition(m, d);
+
+  // With margin summing, we expect 1 contact
+  EXPECT_EQ(d->ncon, 1);
+
+  mj_deleteData(d);
+  mj_deleteModel(m);
+}
+
 }  // namespace
 }  // namespace mujoco

test/engine/engine_solver_test.cc

@@ -57,15 +57,15 @@ TEST_F(SolverTest, IslandsEquivalent) {
   mjData* data_noisland = mj_makeData(model);
 
   // Below are 3 tolerances associated with 3 different iteration counts,
-  // they are only moderately tight, 10x higher than x86-64 failure on Linux,
+  // they are only moderately tight, 12x higher than x86-64 failure on Linux,
-  // i.e. in that case the test fails with rtol smaller than {5e-3, 5e-4, 5e-5}.
+  // i.e. in that case the test fails with rtol smaller than {6e-3, 6e-4, 6e-5}.
   // The point of this test is to show that CG convergence is actually not very
   // precise, simply changing whether islands are used changes the solution by
   // quite a lot, even at high iteration count and zero {ls_}tolerance.
   // Increasing the iteration count higher than 60 does not improve convergence.
   constexpr int kNumTol = 3;
   mjtNum maxiter[kNumTol] = {30,   40,   60};
-  mjtNum rtol[kNumTol] =    {5e-2, 5e-3, 5e-4};
+  mjtNum rtol[kNumTol] =    {6e-2, 6e-3, 6e-4};
 
   for (int i = 0; i < kNumTol; ++i) {
     model->opt.iterations = maxiter[i];