From 6af0d4c823a693f9bb0f18da9a568c11f652dfc7 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Wed, 11 Feb 2026 14:21:37 -0800 Subject: [PATCH] Replace margin/gap max with sum. PiperOrigin-RevId: 868842521 Change-Id: Ia4a4ca5c6821c5a7b3ba7b0af7f71d981c2866c1 --- doc/changelog.rst | 3 ++ doc/modeling.rst | 2 +- mjx/mujoco/mjx/_src/collision_driver.py | 4 +-- .../mujoco_warp/_src/collision_convex.py | 2 +- .../mujoco_warp/_src/collision_driver.py | 10 +++--- .../mujoco_warp/_src/collision_primitive.py | 4 +-- src/engine/engine_collision_driver.c | 27 +++++++-------- test/engine/engine_collision_driver_test.cc | 33 +++++++++++++++++++ test/engine/engine_solver_test.cc | 6 ++-- 9 files changed, 61 insertions(+), 30 deletions(-) diff --git a/doc/changelog.rst b/doc/changelog.rst index d084b356..8dcb554c 100644 --- a/doc/changelog.rst +++ b/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` 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` to values larger than 1. Relatedly, frustum visualization also works for :ref:`orthographic` cameras. diff --git a/doc/modeling.rst b/doc/modeling.rst index 5016fc29..957baa11 100644 --- a/doc/modeling.rst +++ b/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: diff --git a/mjx/mujoco/mjx/_src/collision_driver.py b/mjx/mujoco/mjx/_src/collision_driver.py index 0f15524b..3371a674 100644 --- a/mjx/mujoco/mjx/_src/collision_driver.py +++ b/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]) - gap = jp.maximum(m.geom_gap[geom1], m.geom_gap[geom2]) + margin = m.geom_margin[geom1] + m.geom_margin[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) diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_convex.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_convex.py index cd6dd6e6..51208102 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_convex.py +++ b/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): diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_driver.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_driver.py index 97fc1305..da51c562 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_driver.py +++ b/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() diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_primitive.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_primitive.py index 54f05709..fd606ad6 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_primitive.py +++ b/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]) - gap = wp.max(geom_gap[gap_id, g1], geom_gap[gap_id, g2]) + margin = geom_margin[margin_id, g1] + geom_margin[margin_id, g2] + gap = geom_gap[gap_id, g1] + geom_gap[gap_id, g2] friction = vec5( wp.max(MJ_MINMU, friction[0]), diff --git a/src/engine/engine_collision_driver.c b/src/engine/engine_collision_driver.c index cce1f8ea..ac12d0e4 100644 --- a/src/engine/engine_collision_driver.c +++ b/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, maxmargin); + mjtNum margin = mj_assignMargin(m, m->geom_margin[nodeid1] + m->geom_margin[nodeid2]); 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, maxmargin); + mjtNum margin = mj_assignMargin(m, m->body_margin[bf1] + m->body_margin[bf2]); 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, maxmargin); + mjtNum margin = mj_assignMargin(m, m->geom_margin[nodeid1] + m->flex_margin[f2]); 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, maxmargin); + mjtNum margin = mj_assignMargin(m, m->body_margin[bf1] + m->flex_margin[f2]); 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, maxmargin); + mjtNum margin = mj_assignMargin(m, m->flex_margin[f1] + m->flex_margin[f2]); 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 = mju_max(gap1, gap2); + // gap: add + *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; diff --git a/test/engine/engine_collision_driver_test.cc b/test/engine/engine_collision_driver_test.cc index 750c39fb..0db2556d 100644 --- a/test/engine/engine_collision_driver_test.cc +++ b/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"( + + + + + + + + + + + + + )"; + 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 diff --git a/test/engine/engine_solver_test.cc b/test/engine/engine_solver_test.cc index 41bc4ff0..fc722308 100644 --- a/test/engine/engine_solver_test.cc +++ b/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, - // i.e. in that case the test fails with rtol smaller than {5e-3, 5e-4, 5e-5}. + // 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 {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];