From 8655446f25a483e056eb689a3ae3c57c4f9801f2 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Sat, 1 Aug 2026 07:47:39 -0700 Subject: [PATCH] Fix spurious deep contacts in box-box collision. In the edge-edge path of the box-box collider, when line clipping yields no points, the corner generators accept points whose projection parameters are out of range and clamp them into the valid range. The depth of such a point is the Euclidean distance between two unrelated points, mixing lateral offset into penetration depth, and on the penetrating side it is admitted with no margin check. For thin boxes meeting edge-to-face within margin, this produced a contact with penetration three orders of magnitude larger than the boxes' true separation, exploding resting stacks. No contact can penetrate deeper than the support-interval overlap along the separating axis, which the SAT stage has already computed. Enforce this bound on all points emitted by the edge-edge path. The bound carries margin plus relative and size-scaled slack covering rounding error: the depth of the deepest legitimate point is algebraically equal to the bound, so an exact comparison would drop real contacts. The slack is precision-dependent: in mjUSESINGLE builds the two computations of the same overlap disagree by tens of ulps, and slack calibrated for double precision rejects real single-precision contacts. Differential fuzzing against the nativeccd oracle over 200k random near-contact thin-box configurations, in both precisions: impossibly deep contacts drop from 1018 to 4 (worst excess from 2.5x the bounding diameter to 0.001x), with no legitimate shallow-penetration contact lost. PiperOrigin-RevId: 957628830 Change-Id: Iaa1f10742f91c55bf831296cba0936eb50ea09b0 --- doc/changelog.rst | 6 + src/engine/engine_collision_box.c | 36 ++++- test/engine/engine_collision_box_test.cc | 167 +++++++++++++++++++++++ 3 files changed, 203 insertions(+), 6 deletions(-) diff --git a/doc/changelog.rst b/doc/changelog.rst index beaf5997..763576b9 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -47,6 +47,12 @@ Models Unlike the poncho models, which are bending-only, this model exercises the 2D :ref:`stretch` elasticity of a flex. +Bug fixes +^^^^^^^^^ + +- Fixed a bug in the box-box collider where near-degenerate face clipping could generate contacts with spuriously + large penetration depth between nearly touching thin boxes with positive margin, causing resting stacks to explode. + Version 3.11.0 (July 27, 2026) ------------------------------ diff --git a/src/engine/engine_collision_box.c b/src/engine/engine_collision_box.c index f1a96934..fc421ed4 100644 --- a/src/engine/engine_collision_box.c +++ b/src/engine/engine_collision_box.c @@ -19,6 +19,17 @@ #include "engine/engine_util_blas.h" #include "engine/engine_util_misc.h" +// rounding slack for the edge-edge depth bound: relative, and absolute times the sum of +// half-sizes; wide enough to cover rounding between two computations of the same overlap, +// orders of magnitude below the box-scale depths of spurious clipping artifacts +#ifdef mjUSESINGLE + #define mjDEPTHSLACKREL 1e-4f + #define mjDEPTHSLACKABS 1e-5f +#else + #define mjDEPTHSLACKREL 1e-6 + #define mjDEPTHSLACKABS 1e-12 +#endif + // hard-clamp vector to range [-limit(i), +limit(i)] static void mju_clampVec(mjtNum* vec, const mjtNum* limit, int n) { for (int i = 0; i < n; i++) { @@ -616,6 +627,7 @@ int _boxbox(const mjModel* M, const mjData* D, mjPreContact* con, int g1, int g2 depth[mjMAXCONPAIR], pts[6][3], ppts2[4][2], pu[4][3], axi[3][3]; mjtNum linesu[4][6], lines[4][6], clnorm[3], rnorm[3]; mjtNum penetration, c1, c2, c3, a, b, c, d, lx, ly, hz, l, x, y, u, v, llx, lly, innorm, margin2; + mjtNum maxdepth; int i0, i1, i2; mjtNum f0, f1, f2; @@ -1328,19 +1340,31 @@ edgeedge: mji_zero3(con[0].tangent); - for (i = 0; i < n; i++) { - con[i].dist = depth[i]; + // no contact can be deeper than the support overlap along the separating axis: clipping + // against a grazing face can synthesize spurious points with arbitrarily large depth; + // the slack covers rounding error so the deepest legitimate point is never rejected + maxdepth = mju_max(0, penetration); + maxdepth += margin + mjDEPTHSLACKREL * maxdepth + + mjDEPTHSLACKABS * (size1[0] + size1[1] + size1[2] + + size2[0] + size2[1] + size2[2]); + + for (i = 0, m = 0; i < n; i++) { + if (depth[i] < -maxdepth) + continue; + + con[m].dist = depth[i]; points[i][2] += hz; mji_mulMatVec3(tmp2, r, points[i]); - mji_add3(con[i].pos, tmp2, pos1); + mji_add3(con[m].pos, tmp2, pos1); - mji_copy3(con[i].normal, con[0].normal); - mji_zero3(con[i].tangent); + mji_copy3(con[m].normal, con[0].normal); + mji_zero3(con[m].tangent); + m++; } - return n; + return m; #undef rotaxis #undef rotmatx diff --git a/test/engine/engine_collision_box_test.cc b/test/engine/engine_collision_box_test.cc index e84e1296..cf3afbc0 100644 --- a/test/engine/engine_collision_box_test.cc +++ b/test/engine/engine_collision_box_test.cc @@ -290,5 +290,172 @@ TEST_F(MjCollisionBoxTest, BoxBoxContactDistance) { } } +TEST_F(MjCollisionBoxTest, ThinBoxNoSpuriousDeepContact) { + constexpr char xml[] = R"( + + + + + + + + + + + + + + + + )"; + char error[1024]; + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + MjDataPtr data = MakeData(model); + + // thin boxes meeting edge-to-face, separated by ~15um, within the margin + mj_resetDataKeyframe(model.get(), data.get(), 0); + mj_forward(model.get(), data.get()); + mjtNum gap = mj_geomDistance(model.get(), data.get(), 0, 1, 0.1, nullptr); + EXPECT_GT(gap, 0); + + // separated boxes: all margin-admitted contacts must have positive distance + ASSERT_GT(data->ncon, 0); + for (int i = 0; i < data->ncon; i++) { + EXPECT_GT(data->contact[i].dist, 0); + } + + // same, calling the collider directly (margin is the sum of geom margins) + mjPreContact precon[mjMAXCONPAIR]; + int num = mjc_BoxBox(model.get(), data.get(), precon, 0, 1, 2e-5); + for (int i = 0; i < num; i++) { + EXPECT_GT(precon[i].dist, 0); + } + + // push box 2 into box 1 along the normal: the deep contact is still reported + mju_addToScl3(data->qpos + 7, data->contact[0].frame, -1e-4); + mj_forward(model.get(), data.get()); + gap = mj_geomDistance(model.get(), data.get(), 0, 1, 0.1, nullptr); + EXPECT_LT(gap, 0); + ASSERT_GT(data->ncon, 0); + mjtNum deepest = data->contact[0].dist; + for (int i = 1; i < data->ncon; i++) { + deepest = mju_min(deepest, data->contact[i].dist); + } + EXPECT_THAT(deepest, MjNear(gap, 1e-8, 1e-6)); +} + +TEST_F(MjCollisionBoxTest, ThinBoxShallowPenetration) { + // thin boxes with zero margin, penetrating by ~100um: the deepest contact reaches the + // separating-axis bound up to rounding, and must not be dropped by the depth filter; + // the pose is written directly into mjData since the exact bits matter + constexpr char xml[] = R"( + + + + + + + )"; + char error[1024]; + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + MjDataPtr data = MakeData(model); + mj_kinematics(model.get(), data.get()); + + const mjtNum size1[3] = {0.00044359468518251132, 0.0019427705390657074, + 0.00056716790015765711}; + const mjtNum size2[3] = {0.00062288175555326323, 0.0010906336314477707, + 0.00030803275652456415}; + const mjtNum pos2[3] = {-0.0011513383735175778, -0.0013813387665010076, + -0.00043818094448460645}; + const mjtNum quat1[4] = {0.28720146798163904, -0.083561810000907483, + -0.51853459717827233, 0.80103346511099693}; + const mjtNum quat2[4] = {0.51588977358544519, 0.63445106644895233, + 0.50962161111201376, -0.26761053656263345}; + mju_copy3(model->geom_size, size1); + mju_copy3(model->geom_size + 3, size2); + mju_zero3(data->geom_xpos); + mju_copy3(data->geom_xpos + 3, pos2); + mju_quat2Mat(data->geom_xmat, quat1); + mju_quat2Mat(data->geom_xmat + 9, quat2); + + mjtNum gap = mj_geomDistance(model.get(), data.get(), 0, 1, 0.1, nullptr); + EXPECT_LT(gap, 0); + + mjPreContact precon[mjMAXCONPAIR]; + int num = mjc_BoxBox(model.get(), data.get(), precon, 0, 1, 0); + ASSERT_GT(num, 0); + mjtNum deepest = precon[0].dist; + for (int i = 1; i < num; i++) { + deepest = mju_min(deepest, precon[i].dist); + } + EXPECT_THAT(deepest, MjNear(gap, 1e-8, 1e-6)); +} + +TEST_F(MjCollisionBoxTest, EdgeContactAtDepthBound) { + // edge-edge contacts whose depth equals the separating-axis bound up to rounding: the + // depth filter's slack must cover single-precision rounding or the whole manifold is + // rejected; the pose is written directly into mjData since the exact bits matter + constexpr char xml[] = R"( + + + + + + + )"; + char error[1024]; + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + MjDataPtr data = MakeData(model); + mj_kinematics(model.get(), data.get()); + + struct Config { + mjtNum size1[3], size2[3], pos2[3], quat1[4], quat2[4]; + }; + const Config configs[2] = { + {{0.018352361395955086, 0.038452208042144775, 0.047704100608825684}, + {0.026817722246050835, 0.0014398059574887156, 0.0082265362143516541}, + {-0.026325162500143051, 0.030753342434763908, 0.02338058315217495}, + {-0.90091776847839355, -0.42279955744743347, 0.097014322876930237, + -0.013262901455163956}, + {0.23919239640235901, 0.056073460727930069, 0.91227829456329346, + 0.32770577073097229}}, + {{0.044790275394916534, 0.0057221869938075542, 0.0049537895247340202}, + {0.026496950536966324, 0.019804427400231361, 0.0057344711385667324}, + {0.0094044031575322151, 0.020275400951504707, -0.017358051612973213}, + {0.040385473519563675, 0.75189536809921265, -0.55430221557617188, + 0.35464280843734741}, + {-0.6716417670249939, 0.085770353674888611, 0.69683432579040527, + -0.23656430840492249}}}; + + for (const Config& config : configs) { + mju_copy3(model->geom_size, config.size1); + mju_copy3(model->geom_size + 3, config.size2); + mju_zero3(data->geom_xpos); + mju_copy3(data->geom_xpos + 3, config.pos2); + mju_quat2Mat(data->geom_xmat, config.quat1); + mju_quat2Mat(data->geom_xmat + 9, config.quat2); + + mjtNum gap = mj_geomDistance(model.get(), data.get(), 0, 1, 0.1, nullptr); + EXPECT_LT(gap, 0); + + mjPreContact precon[mjMAXCONPAIR]; + int num = mjc_BoxBox(model.get(), data.get(), precon, 0, 1, 0); + ASSERT_GT(num, 0); + mjtNum deepest = precon[0].dist; + for (int i = 1; i < num; i++) { + deepest = mju_min(deepest, precon[i].dist); + } + EXPECT_THAT(deepest, MjNear(gap, 1e-8, 1e-6)); + } +} + } // namespace } // namespace mujoco