chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions 2 Packages Projects Releases Wiki Activity

Fix spurious deep contacts in box-box collision.

Browse Source 
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
This commit is contained in:
Yuval Tassa
2026-08-01 07:47:39 -07:00
committed by Copybara-Service
parent b389b28b87
commit 8655446f25
3 changed files with 203 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/changelog.rst
+6
View File
@@ -47,6 +47,12 @@ Models
Unlike the poncho models, which are bending-only, this model exercises the 2D
:ref:`stretch<flex-elasticity-elastic2d>` 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)
------------------------------
src/engine/engine_collision_box.c
+30 -6
View File
@@ -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
test/engine/engine_collision_box_test.cc
+167
View File
@@ -290,5 +290,172 @@ TEST_F(MjCollisionBoxTest, BoxBoxContactDistance) {
}
}
TEST_F(MjCollisionBoxTest, ThinBoxNoSpuriousDeepContact) {
constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<freejoint/>
<geom type="box" size="0.047 0.032 0.00035" margin="1e-5" mass="2e-3"/>
</body>
<body>
<freejoint/>
<geom type="box" size="0.047 0.032 0.00035" margin="1e-5" mass="2e-3"/>
</body>
</worldbody>
<keyframe>
<key qpos="2.8218257713084481e-05 -0.013292121195706197 0.029256072037790268
0.84130787082244052 0.54054898504499482 0.0015299333749655413
0.0023495878161510389
-5.3521944872628108e-05 0.01427117523229726 0.029254416229696868
0.8413227886432264 -0.54053283716888822 0.00024615852936852315
-0.00039580009650069315"/>
</keyframe>
</mujoco>
)";
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"(
<mujoco>
<worldbody>
<geom type="box" size="0.1 0.1 0.1"/>
<geom type="box" size="0.1 0.1 0.1"/>
</worldbody>
</mujoco>
)";
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"(
<mujoco>
<worldbody>
<geom type="box" size="0.1 0.1 0.1"/>
<geom type="box" size="0.1 0.1 0.1"/>
</worldbody>
</mujoco>
)";
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