Yuval Tassa
86e9860106
Replaces the box-box collider's manifold generation and post-filtering with a single structured implementation, and deletes the accumulated repair logic it obsoletes. Net 319 lines out of the engine. Algorithm: - The separating-axis test keeps the closed-form support evaluation and chooses the axis of maximum separation among the 15 candidates by plain argmax. Edge-cross axes whose cross product has norm below rounding are skipped: in the nearly-parallel regime their direction is cancellation noise, previously the source of arbitrary-normal contacts with box-scale spurious depth. A winning edge axis within eight degrees of the best face axis is replaced by that face unless it is better by five percent (ODE's classic fudge): resting stacks otherwise flip between the edge and face contact codes by rounding noise from step to step, thrashing the solver warm start until the stack explodes. The substitution runs after the search rather than filtering during it, so a worse non-aliasing edge cannot steal the contact the substitution meant to give to the face. - Face contacts clip the incident face against the reference face's side planes (Sutherland-Hodgman). Depth is measured along the reference normal only, never as a Euclidean distance between unrelated points. Contact position is midway between the surfaces along the normal, so its distance to either box is bounded by half the contact depth. Every surviving vertex of the clipped polygon becomes a contact, so the manifold is the actual contact patch, at most eight points as before. - Edge contacts use the closest-point pair between the two supporting edge segments. A near-zero axis component makes the support-corner sign ambiguous; both signs are enumerated and the closest witness pair wins. - Margin is an acceptance band throughout: SAT early-out and clip acceptance. - The rounding thresholds are stated per precision. The separation tests are the ones that cost correctness: comparing exactly against the margin reports a pair overlapping by less than the rounding error of its own support evaluation as separated, and the boxes pass through each other. Over 239k overlapping pairs that is eight misses under mjUSESINGLE and none in double; the collider this replaces misses the same eight. Slack proportional to the summed half-sizes leaves five, which overlap by 7e-9 to 3e-8 of their own scale, below single-precision epsilon, where the boxes are not distinguishable from touching. Erring toward contact is the safe direction: the driver already excludes a contact whose distance reaches the margin. Deleted: the conditional acceptance cascade keyed on how many points earlier generators emitted, the u/v clamping that fabricated contacts from out-of-range projections, the outside-box removal filter and its missing-fallback hole, exact-floating-point deduplication, and the edge-path depth clamp. The structure makes those bug classes unrepresentable rather than filtered: depth is a projection by construction. Every reported depth is the exact support overlap along the contact's own normal, verified over 246k overlapping poses to within two ulps; the face preference costs direction, not depth, deviating from the minimum-translation axis by at most 8.1 degrees and 5.3% of its depth. The previous implementation is preserved verbatim as mjc_BoxBoxLegacy in test/engine/boxbox_legacy.c, a static library that only the box-box tests link, so the claims above are measured rather than asserted. It needs no private engine symbols. Three tests compare against it: - NearAlignedManifoldIsExact sweeps the relative angle of a resting pair across the regime where the edge-cross axes degenerate into noise, pinning the full clipped polygon and a contact normal equal to the face normal exactly, where the previous collider drifts off it. - AlignedTowerStands settles a twenty-box tower, which comes to rest four million times quieter than under the previous collider, which never settles and eventually topples. - ShallowOverlapSurvivesRounding pins a pair overlapping by 7e-8 of its scale, reported as separated under mjUSESINGLE without slack on the separation tests. On stacks of plates across aspect ratios from 4:1 to 25:1, five layouts each, the collider settles into a tight band of 1e-4 to 3e-4 while the previous one intermittently blows up to as much as 2.6e-2. engine_collision_box_fuzz_test.cc cross-validates randomized poses against GJK/EPA on identical box meshes and against a spherical-Fibonacci support sweep, with hard gates per sample: no phantom penetration, no missed contact at zero margin, no contact deeper than the true depth, contacts within half their own depth of both boxes, and one normal per manifold. Both invocations run in about a second. EdgeContactAtDepthBound's tolerance widens to the five percent design band; the three-orders-of-magnitude depth bug it pins is still caught, the deviation being 0.13 percent of the depth. The 100-box pile benchmark steps about 7% faster with 1.6% fewer contacts. PiperOrigin-RevId: 965114952 Change-Id: Ie98cdcce8d1aed3ff2da938cb29703fd9c241258
662 lines
27 KiB
C++
662 lines
27 KiB
C++
// Copyright 2026 DeepMind Technologies Limited
|
|
//
|
|
// Licensed under the Apache License, Version 2.0 (the "License");
|
|
// you may not use this file except in compliance with the License.
|
|
// You may obtain a copy of the License at
|
|
//
|
|
// http://www.apache.org/licenses/LICENSE-2.0
|
|
//
|
|
// Unless required by applicable law or agreed to in writing, software
|
|
// distributed under the License is distributed on an "AS IS" BASIS,
|
|
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
// See the License for the specific language governing permissions and
|
|
// limitations under the License.
|
|
|
|
// Randomized cross-validation of mjc_BoxBox.
|
|
//
|
|
// Each model holds two box geoms and two box-shaped mesh geoms with identical
|
|
// half-extents. For a randomized relative pose the box pair is collided with
|
|
// mjc_BoxBox and the mesh pair with mjc_Convex (GJK/EPA).
|
|
//
|
|
// Two references arbitrate correctness:
|
|
// - GJK/EPA on the mesh pair, where it is trustworthy. EPA misreports depth
|
|
// and normal on thin meshes (verified against the direction sweep below),
|
|
// so strict agreement is only enforced for well-conditioned aspect ratios.
|
|
// - A dense direction sweep over support separations. For two boxes the
|
|
// minimum-translation direction is a face normal or an edge-edge cross
|
|
// product, so the sweep converges to the true penetration depth from
|
|
// above; its resolution error at 20k directions is ~2.5e-3 of scale.
|
|
//
|
|
// Independent of any reference, invariants are enforced for every sample:
|
|
// contacts lie within half their own depth of both (margin-inflated) boxes,
|
|
// all contacts in a manifold share one normal, the manifold is no larger than a
|
|
// clipped face, and no contact is deeper than the true penetration depth.
|
|
|
|
#include <array>
|
|
#include <cmath>
|
|
#include <cstdio>
|
|
#include <cstdlib>
|
|
#include <random>
|
|
#include <string>
|
|
#include <vector>
|
|
|
|
#include <gmock/gmock.h>
|
|
#include <gtest/gtest.h>
|
|
#include <mujoco/mjdata.h>
|
|
#include <mujoco/mjmodel.h>
|
|
#include <mujoco/mujoco.h>
|
|
#include "src/engine/engine_collision_convex.h"
|
|
#include "src/engine/engine_collision_primitive.h"
|
|
#include "src/engine/engine_util_misc.h"
|
|
#include "test/fixture.h"
|
|
|
|
namespace mujoco {
|
|
namespace {
|
|
|
|
using ::testing::NotNull;
|
|
|
|
using MjCollisionBoxFuzzTest = MujocoTest;
|
|
|
|
// maximum contacts box-box collider may emit: a face manifold is the clipped
|
|
// incident face, a 4-gon against 4 half-planes, so at most 8 vertices
|
|
constexpr int kMaxContacts = 8;
|
|
|
|
// direction count for the brute-force sweep; resolution scales as 1/sqrt(n)
|
|
constexpr int kSweepDirections = 20000;
|
|
|
|
struct SizeCase {
|
|
const char* name;
|
|
mjtNum size1[3];
|
|
mjtNum size2[3];
|
|
// strict GJK agreement is enforced when true; EPA under-reports deep
|
|
// penetration by up to ~1% of depth, which the truth-arbitrated gates
|
|
// absorb, so all cases currently enable it
|
|
bool gjk_reliable;
|
|
};
|
|
|
|
constexpr SizeCase kSizeCases[] = {
|
|
{"cube_cube", {0.05, 0.05, 0.05}, {0.05, 0.05, 0.05}, true},
|
|
{"cube_small", {0.05, 0.05, 0.05}, {0.013, 0.013, 0.013}, true},
|
|
{"slab_slab", {0.08, 0.08, 0.004}, {0.06, 0.06, 0.006}, true},
|
|
{"needle_cube", {0.002, 0.002, 0.09}, {0.04, 0.04, 0.04}, true},
|
|
{"slab_needle", {0.07, 0.05, 0.003}, {0.0015, 0.0015, 0.06}, true},
|
|
{"aniso", {0.018, 0.038, 0.047}, {0.026, 0.0014, 0.008}, true},
|
|
};
|
|
|
|
// builds a model with box geoms 0,1 and identical box-mesh geoms 2,3; every
|
|
// geom hangs from a freejoint body so poses are applied through qpos and
|
|
// kinematics -- writing geom_xmat directly would discard the compiled mesh's
|
|
// principal-axis frame, silently permuting a thin mesh's axes
|
|
std::string MakeXml(const SizeCase& c) {
|
|
char buf[3072];
|
|
std::snprintf(buf, sizeof(buf), R"(
|
|
<mujoco>
|
|
<asset>
|
|
<mesh name="m1" scale="%g %g %g"
|
|
vertex="-1 -1 -1 1 -1 -1 1 1 -1 1 1 1 1 -1 1 -1 1 -1 -1 1 1 -1 -1 1"/>
|
|
<mesh name="m2" scale="%g %g %g"
|
|
vertex="-1 -1 -1 1 -1 -1 1 1 -1 1 1 1 1 -1 1 -1 1 -1 -1 1 1 -1 -1 1"/>
|
|
</asset>
|
|
<worldbody>
|
|
<body><freejoint/><geom type="box" size="%g %g %g"/></body>
|
|
<body><freejoint/><geom type="box" size="%g %g %g"/></body>
|
|
<body><freejoint/><geom type="mesh" mesh="m1"/></body>
|
|
<body><freejoint/><geom type="mesh" mesh="m2"/></body>
|
|
</worldbody>
|
|
</mujoco>
|
|
)",
|
|
c.size1[0], c.size1[1], c.size1[2], c.size2[0], c.size2[1],
|
|
c.size2[2], c.size1[0], c.size1[1], c.size1[2], c.size2[0],
|
|
c.size2[1], c.size2[2]);
|
|
return std::string(buf);
|
|
}
|
|
|
|
void RandomQuat(std::mt19937& rng, mjtNum quat[4]) {
|
|
std::normal_distribution<double> g(0.0, 1.0);
|
|
for (int i = 0; i < 4; i++) quat[i] = g(rng);
|
|
mju_normalize4(quat);
|
|
}
|
|
|
|
// places body pair (0,1) and the mirrored mesh pair (2,3) at the same poses,
|
|
// through qpos and kinematics so mesh frame compensation is honored
|
|
void SetPose(const mjModel* model, mjData* data, const mjtNum pos2[3],
|
|
const mjtNum quat1[4], const mjtNum quat2[4]) {
|
|
mjtNum* q = data->qpos;
|
|
mju_zero3(q);
|
|
mju_copy4(q + 3, quat1);
|
|
mju_copy3(q + 7, pos2);
|
|
mju_copy4(q + 10, quat2);
|
|
mju_zero3(q + 14);
|
|
mju_copy4(q + 17, quat1);
|
|
mju_copy3(q + 21, pos2);
|
|
mju_copy4(q + 24, quat2);
|
|
mj_kinematics(model, data);
|
|
}
|
|
|
|
mjtNum DeepestDist(const mjPreContact* con, int n) {
|
|
mjtNum d = con[0].dist;
|
|
for (int i = 1; i < n; i++) d = mju_min(d, con[i].dist);
|
|
return d;
|
|
}
|
|
|
|
// support separation of the two boxes along a specific direction
|
|
mjtNum DirSep(const mjModel* model, const mjData* data, const mjtNum dir[3]) {
|
|
mjtNum dpos[3];
|
|
mju_sub3(dpos, data->geom_xpos + 3, data->geom_xpos);
|
|
mjtNum r1 = 0, r2 = 0;
|
|
for (int k = 0; k < 3; k++) {
|
|
mjtNum a1 = dir[0] * data->geom_xmat[0 + k] +
|
|
dir[1] * data->geom_xmat[3 + k] +
|
|
dir[2] * data->geom_xmat[6 + k];
|
|
mjtNum a2 = dir[0] * data->geom_xmat[9 + k] +
|
|
dir[1] * data->geom_xmat[12 + k] +
|
|
dir[2] * data->geom_xmat[15 + k];
|
|
r1 += model->geom_size[k] * mju_abs(a1);
|
|
r2 += model->geom_size[3 + k] * mju_abs(a2);
|
|
}
|
|
return mju_abs(mju_dot3(dir, dpos)) - r1 - r2;
|
|
}
|
|
|
|
// true separation via dense direction sweep (spherical Fibonacci lattice);
|
|
// the sampled maximum is a lower bound on the true separation, converging as
|
|
// the direction count grows
|
|
mjtNum BruteForceSep(const mjModel* model, const mjData* data) {
|
|
mjtNum dpos[3];
|
|
mju_sub3(dpos, data->geom_xpos + 3, data->geom_xpos);
|
|
mjtNum best = -mjMAXVAL;
|
|
for (int gi = 0; gi < kSweepDirections; gi++) {
|
|
mjtNum phi = 2.399963229728653 * gi;
|
|
mjtNum ct = 1.0 - 2.0 * (gi + 0.5) / kSweepDirections;
|
|
mjtNum st = std::sqrt(mju_max(0, 1 - ct * ct));
|
|
mjtNum dir[3] = {st * std::cos(phi), st * std::sin(phi), ct};
|
|
mjtNum r1 = 0, r2 = 0;
|
|
for (int k = 0; k < 3; k++) {
|
|
mjtNum a1 = dir[0] * data->geom_xmat[0 + k] +
|
|
dir[1] * data->geom_xmat[3 + k] +
|
|
dir[2] * data->geom_xmat[6 + k];
|
|
mjtNum a2 = dir[0] * data->geom_xmat[9 + k] +
|
|
dir[1] * data->geom_xmat[12 + k] +
|
|
dir[2] * data->geom_xmat[15 + k];
|
|
r1 += model->geom_size[k] * mju_abs(a1);
|
|
r2 += model->geom_size[3 + k] * mju_abs(a2);
|
|
}
|
|
best = mju_max(best, mju_abs(mju_dot3(dir, dpos)) - r1 - r2);
|
|
}
|
|
return best;
|
|
}
|
|
|
|
// closed-form analytical SAT reference across all 15 potential separating axes
|
|
// in double precision (exact to machine precision, without discretization
|
|
// error)
|
|
mjtNum ExactSatSep(const mjModel* model, const mjData* data) {
|
|
const mjtNum* pos1 = data->geom_xpos;
|
|
const mjtNum* pos2 = data->geom_xpos + 3;
|
|
const mjtNum* mat1 = data->geom_xmat;
|
|
const mjtNum* mat2 = data->geom_xmat + 9;
|
|
const mjtNum* size1 = model->geom_size;
|
|
const mjtNum* size2 = model->geom_size + 3;
|
|
|
|
mjtNum rot[9], rotabs[9], pos21[3], pos12[3], tmp[3];
|
|
mju_sub3(tmp, pos2, pos1);
|
|
mju_mulMatTVec3(pos21, mat1, tmp);
|
|
mju_sub3(tmp, pos1, pos2);
|
|
mju_mulMatTVec3(pos12, mat2, tmp);
|
|
mju_mulMatTMat(rot, mat1, mat2, 3, 3, 3);
|
|
for (int i = 0; i < 9; i++) {
|
|
rotabs[i] = mju_abs(rot[i]);
|
|
}
|
|
|
|
mjtNum sep_max = -mjMAXVAL;
|
|
|
|
// 3 face axes of box1
|
|
for (int i = 0; i < 3; i++) {
|
|
mjtNum radius2 = rotabs[3 * i + 0] * size2[0] +
|
|
rotabs[3 * i + 1] * size2[1] +
|
|
rotabs[3 * i + 2] * size2[2];
|
|
mjtNum sep = mju_abs(pos21[i]) - size1[i] - radius2;
|
|
sep_max = mju_max(sep_max, sep);
|
|
}
|
|
|
|
// 3 face axes of box2
|
|
for (int j = 0; j < 3; j++) {
|
|
mjtNum radius1 = rotabs[0 + j] * size1[0] +
|
|
rotabs[3 + j] * size1[1] +
|
|
rotabs[6 + j] * size1[2];
|
|
mjtNum sep = mju_abs(pos12[j]) - size2[j] - radius1;
|
|
sep_max = mju_max(sep_max, sep);
|
|
}
|
|
|
|
// 9 edge-cross axes
|
|
for (int i = 0; i < 3; i++) {
|
|
for (int j = 0; j < 3; j++) {
|
|
int i1 = (i + 1) % 3, i2 = (i + 2) % 3;
|
|
mjtNum ax1 = -rot[3 * i2 + j];
|
|
mjtNum ax2 = rot[3 * i1 + j];
|
|
mjtNum norm2 = ax1 * ax1 + ax2 * ax2;
|
|
if (norm2 < 1e-12) {
|
|
continue;
|
|
}
|
|
mjtNum inv = 1 / mju_sqrt(norm2);
|
|
ax1 *= inv;
|
|
ax2 *= inv;
|
|
|
|
int j1 = (j + 1) % 3, j2 = (j + 2) % 3;
|
|
mjtNum a2_1 = ax1 * rot[3 * i1 + j1] + ax2 * rot[3 * i2 + j1];
|
|
mjtNum a2_2 = ax1 * rot[3 * i1 + j2] + ax2 * rot[3 * i2 + j2];
|
|
|
|
mjtNum radius1 = size1[i1] * mju_abs(ax1) + size1[i2] * mju_abs(ax2);
|
|
mjtNum radius2 = size2[j1] * mju_abs(a2_1) + size2[j2] * mju_abs(a2_2);
|
|
mjtNum sep =
|
|
mju_abs(ax1 * pos21[i1] + ax2 * pos21[i2]) - radius1 - radius2;
|
|
sep_max = mju_max(sep_max, sep);
|
|
}
|
|
}
|
|
return sep_max;
|
|
}
|
|
|
|
struct Stats {
|
|
int configs = 0;
|
|
int both_hit = 0;
|
|
int only_box = 0; // box-box hit where GJK did not
|
|
int only_gjk = 0; // GJK hit where box-box did not
|
|
int normal_bad = 0; // (gjk_reliable only) normal disagreement
|
|
int depth_bad = 0; // (gjk_reliable only) deepest-depth disagreement
|
|
int outside_bad = 0; // contact farther than |dist|/2 + slack from a box
|
|
int count_bad = 0; // more than kMaxContacts contacts
|
|
int mixed_normal = 0; // manifold contacts disagree on normal
|
|
int overdeep = 0; // contact deeper than the true penetration depth
|
|
int phantom = 0; // contact reported where truth says separated
|
|
int missed = 0; // no contact reported where truth says penetrating
|
|
mjtNum worst_normal = 0;
|
|
mjtNum worst_depth = 0;
|
|
};
|
|
|
|
Stats Sweep(const SizeCase& c, int n_configs, mjtNum margin, unsigned seed) {
|
|
Stats st;
|
|
const std::string xml = MakeXml(c);
|
|
char error[1024];
|
|
MjModelPtr model_ptr = LoadModelFromString(xml.c_str(), error, sizeof(error));
|
|
EXPECT_THAT(model_ptr.get(), NotNull()) << error;
|
|
if (!model_ptr) return st;
|
|
mjModel* model = model_ptr.get();
|
|
MjDataPtr data_ptr = MakeData(model_ptr);
|
|
mjData* data = data_ptr.get();
|
|
|
|
const mjtNum scale =
|
|
mju_max(mju_max(c.size1[0], c.size1[1]), c.size1[2]) +
|
|
mju_max(mju_max(c.size2[0], c.size2[1]), c.size2[2]);
|
|
// sweep resolution: angular spacing ~sqrt(4pi/n), times the pair radius
|
|
const mjtNum sweep_tol = 4.0 * scale / std::sqrt((double)kSweepDirections);
|
|
|
|
std::mt19937 rng(seed);
|
|
std::uniform_real_distribution<double> u(-1.0, 1.0);
|
|
|
|
mjPreContact box_con[mjMAXCONPAIR], gjk_con[mjMAXCONPAIR];
|
|
|
|
for (int it = 0; it < n_configs; it++) {
|
|
mjtNum pos2[3], quat1[4], quat2[4];
|
|
for (int i = 0; i < 3; i++) pos2[i] = 1.15 * scale * u(rng);
|
|
RandomQuat(rng, quat1);
|
|
RandomQuat(rng, quat2);
|
|
SetPose(model, data, pos2, quat1, quat2);
|
|
|
|
int nbox = mjc_BoxBox(model, data, box_con, 0, 1, margin);
|
|
int ngjk = mjc_Convex(model, data, gjk_con, 2, 3, margin);
|
|
st.configs++;
|
|
|
|
if (nbox > kMaxContacts) st.count_bad++;
|
|
|
|
// ground-truth arbitration on a sample of configs and on every presence
|
|
// disagreement
|
|
bool arbitrate = (it % 16 == 0) || (nbox > 0) != (ngjk > 0);
|
|
if (arbitrate) {
|
|
mjtNum true_sep = BruteForceSep(model, data);
|
|
// in the margin band the collider measures the gap along its separating
|
|
// axis, which under-reads the Euclidean separation (a SAT property the
|
|
// previous implementation and MJX share), so separated-pair bookkeeping
|
|
// contacts are by design; the harmful classes are claiming penetration
|
|
// where none exists and reporting depth beyond the true depth
|
|
mjtNum db = DeepestDist(box_con, nbox);
|
|
int bad_phantom = nbox > 0 && db < 0 && true_sep > sweep_tol;
|
|
int bad_deep = nbox > 0 && db < 0 &&
|
|
db < true_sep - sweep_tol - 0.05 * mju_abs(true_sep);
|
|
if (bad_phantom) st.phantom++;
|
|
if (nbox == 0 && true_sep < margin - sweep_tol) st.missed++;
|
|
if (bad_deep) st.overdeep++;
|
|
if ((bad_phantom || bad_deep) && std::getenv("MJ_FUZZ_DUMP")) {
|
|
std::printf(
|
|
"%s %s db=%.6e true_sep=%.6e nbox=%d n0=(%.4f %.4f %.4f)\n"
|
|
" pos2={%.17g, %.17g, %.17g}\n"
|
|
" quat1={%.17g, %.17g, %.17g, %.17g}\n"
|
|
" quat2={%.17g, %.17g, %.17g, %.17g}\n",
|
|
bad_phantom ? "PHANTOM" : "OVERDEEP", c.name,
|
|
db, true_sep, nbox, box_con[0].normal[0],
|
|
box_con[0].normal[1], box_con[0].normal[2], pos2[0], pos2[1],
|
|
pos2[2], quat1[0], quat1[1], quat1[2], quat1[3], quat2[0],
|
|
quat2[1], quat2[2], quat2[3]);
|
|
}
|
|
}
|
|
|
|
if (nbox > 0) {
|
|
// manifold invariants: one shared normal; each contact within half its
|
|
// own depth (plus slack) of both margin-inflated boxes
|
|
for (int i = 1; i < nbox; i++) {
|
|
// the contacts of a face manifold carry the same normal vector, whose
|
|
// self-dot is 1 only to the precision of mjtNum
|
|
if (mju_dot3(box_con[0].normal, box_con[i].normal) <
|
|
1 - MjTol(1e-9, 1e-5)) {
|
|
st.mixed_normal++;
|
|
break;
|
|
}
|
|
}
|
|
for (int i = 0; i < nbox; i++) {
|
|
mjtNum slack = 0.5 * mju_abs(box_con[i].dist) + 1e-6 * scale;
|
|
mjtNum sz1[3], sz2[3];
|
|
for (int k = 0; k < 3; k++) {
|
|
sz1[k] = model->geom_size[k] + margin + slack;
|
|
sz2[k] = model->geom_size[3 + k] + margin + slack;
|
|
}
|
|
int o1 = mju_outsideBox(box_con[i].pos, data->geom_xpos,
|
|
data->geom_xmat, sz1, 1);
|
|
int o2 = mju_outsideBox(box_con[i].pos, data->geom_xpos + 3,
|
|
data->geom_xmat + 9, sz2, 1);
|
|
if (o1 == 1 || o2 == 1) {
|
|
st.outside_bad++;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
if (nbox > 0 && ngjk > 0) {
|
|
st.both_hit++;
|
|
if (c.gjk_reliable) {
|
|
mjtNum dot = mju_dot3(box_con[0].normal, gjk_con[0].normal);
|
|
mjtNum ang = mju_abs(1 - mju_abs(dot));
|
|
st.worst_normal = mju_max(st.worst_normal, ang);
|
|
// margin-band contacts admit legitimately ambiguous normals near
|
|
// face ties, so the angular gate is looser with margin
|
|
mjtNum ntol = margin > 0 ? 2e-1 : 1e-3;
|
|
// separated margin-band pairs are exempt: their true closest-feature
|
|
// direction generally lies between the 15 SAT axes (corner-corner
|
|
// cases), so the SAT normal legitimately differs from GJK's; for
|
|
// penetration the SAT axis set contains the exact optimum
|
|
if (ang > ntol && DeepestDist(box_con, nbox) < 0) {
|
|
// arbitrate ties: a normal is wrong only if its directional
|
|
// separation is materially worse than the reference normal's --
|
|
// near-equal minima are legitimately ambiguous between methods
|
|
mjtNum sep_box = DirSep(model, data, box_con[0].normal);
|
|
mjtNum sep_gjk = DirSep(model, data, gjk_con[0].normal);
|
|
// the design prefers face manifolds within five percent of the
|
|
// optimum (stack stability), measured against its own face
|
|
// separation; allow one percent cross-measurement slop against
|
|
// the reference optimum
|
|
if (sep_box < sep_gjk - 0.06 * mju_abs(sep_gjk) - 1e-4 * scale) {
|
|
st.normal_bad++;
|
|
if (std::getenv("MJ_FUZZ_DUMP")) {
|
|
std::printf(
|
|
"NORMAL_BAD %s ang=%.3e sep_box=%.6e sep_gjk=%.6e "
|
|
"nbox=%d\n nb={%.6f %.6f %.6f} ng={%.6f %.6f %.6f}\n"
|
|
" pos2={%.17g, %.17g, %.17g}\n"
|
|
" quat1={%.17g, %.17g, %.17g, %.17g}\n"
|
|
" quat2={%.17g, %.17g, %.17g, %.17g}\n",
|
|
c.name, ang, sep_box, sep_gjk, nbox, box_con[0].normal[0],
|
|
box_con[0].normal[1], box_con[0].normal[2],
|
|
gjk_con[0].normal[0], gjk_con[0].normal[1],
|
|
gjk_con[0].normal[2], pos2[0], pos2[1], pos2[2], quat1[0],
|
|
quat1[1], quat1[2], quat1[3], quat2[0], quat2[1], quat2[2],
|
|
quat2[3]);
|
|
}
|
|
}
|
|
}
|
|
|
|
mjtNum db = DeepestDist(box_con, nbox);
|
|
mjtNum dg = DeepestDist(gjk_con, ngjk);
|
|
mjtNum ddiff = mju_abs(db - dg);
|
|
st.worst_depth = mju_max(st.worst_depth, ddiff);
|
|
// EPA witness accuracy degrades in the margin band
|
|
mjtNum dtol = (margin > 0 ? 5e-3 : 1e-4) * scale + 1e-9;
|
|
if (ddiff > dtol) {
|
|
// arbitrate against ground truth: only count if the box side
|
|
// deviates on the too-deep side of the true depth, and only for
|
|
// penetration, where the support sweep equals the true depth.
|
|
// A shallower manifold is legitimate: the deepest corner can be
|
|
// clipped away laterally, leaving surface-to-surface depths at
|
|
// the surviving contact locations. Positive distances measure
|
|
// different things per method (axis gap vs Euclidean witness gap).
|
|
mjtNum true_sep = BruteForceSep(model, data);
|
|
// the design substitutes the face manifold for an aliasing edge
|
|
// within five percent of the optimum, so depth may exceed the true
|
|
// depth by that fraction
|
|
mjtNum design = 0.05 * mju_abs(true_sep);
|
|
if (db < 0 && db < true_sep - sweep_tol - design) {
|
|
st.depth_bad++;
|
|
if (std::getenv("MJ_FUZZ_DUMP")) {
|
|
std::printf(
|
|
"DEPTH_BAD %s db=%.6e dg=%.6e true_sep=%.6e nbox=%d "
|
|
"ngjk=%d\n nb={%.6f %.6f %.6f} ng={%.6f %.6f %.6f}\n"
|
|
" pos2={%.17g, %.17g, %.17g}\n"
|
|
" quat1={%.17g, %.17g, %.17g, %.17g}\n"
|
|
" quat2={%.17g, %.17g, %.17g, %.17g}\n",
|
|
c.name, db, dg, true_sep, nbox, ngjk, box_con[0].normal[0],
|
|
box_con[0].normal[1], box_con[0].normal[2],
|
|
gjk_con[0].normal[0], gjk_con[0].normal[1],
|
|
gjk_con[0].normal[2], pos2[0], pos2[1], pos2[2], quat1[0],
|
|
quat1[1], quat1[2], quat1[3], quat2[0], quat2[1], quat2[2],
|
|
quat2[3]);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
} else if (nbox > 0) {
|
|
st.only_box++;
|
|
} else if (ngjk > 0) {
|
|
st.only_gjk++;
|
|
}
|
|
}
|
|
|
|
return st;
|
|
}
|
|
|
|
void Report(const char* label, const SizeCase& c, const Stats& st) {
|
|
std::printf(
|
|
"[%s/%-11s] n=%5d both=%5d onlyBox=%4d onlyGJK=%4d | normal_bad=%4d "
|
|
"depth_bad=%4d outside=%4d count_bad=%3d mixed_n=%3d | phantom=%3d "
|
|
"missed=%3d overdeep=%3d | worst_n=%.3e worst_d=%.3e\n",
|
|
label, c.name, st.configs, st.both_hit, st.only_box, st.only_gjk,
|
|
st.normal_bad, st.depth_bad, st.outside_bad, st.count_bad,
|
|
st.mixed_normal, st.phantom, st.missed, st.overdeep, st.worst_normal,
|
|
st.worst_depth);
|
|
}
|
|
|
|
void CheckGates(const SizeCase& c, const Stats& st, mjtNum margin) {
|
|
EXPECT_GT(st.both_hit, 0) << c.name << ": no overlapping samples";
|
|
EXPECT_EQ(st.count_bad, 0) << c.name;
|
|
EXPECT_EQ(st.mixed_normal, 0) << c.name;
|
|
EXPECT_EQ(st.outside_bad, 0) << c.name;
|
|
EXPECT_EQ(st.phantom, 0) << c.name;
|
|
EXPECT_EQ(st.overdeep, 0) << c.name;
|
|
if (margin > 0) {
|
|
// corner-past-the-face margin-band contacts are not representable by a
|
|
// SAT clip collider (same limitation in the previous implementation and
|
|
// MJX); these are bookkeeping contacts at positive distance, so a miss
|
|
// only delays activation by a step. Observed rate peaks at ~0.3% on the
|
|
// most anisotropic case
|
|
EXPECT_LE(st.missed, st.configs / 250) << c.name;
|
|
} else {
|
|
// at zero margin the SAT depth theorem is exact: no misses allowed
|
|
EXPECT_EQ(st.missed, 0) << c.name;
|
|
}
|
|
if (c.gjk_reliable) {
|
|
EXPECT_EQ(st.normal_bad, 0) << c.name;
|
|
EXPECT_EQ(st.depth_bad, 0) << c.name;
|
|
}
|
|
}
|
|
|
|
// config count and seed are overridable for soak runs:
|
|
// MJ_FUZZ_CONFIGS=20000 MJ_FUZZ_SEED=7 ./engine_collision_box_fuzz_test
|
|
int NumConfigs() {
|
|
const char* env = std::getenv("MJ_FUZZ_CONFIGS");
|
|
return env ? std::stoi(env) : 4000;
|
|
}
|
|
|
|
unsigned BaseSeed() {
|
|
const char* env = std::getenv("MJ_FUZZ_SEED");
|
|
return env ? std::stoul(env) : 0;
|
|
}
|
|
|
|
TEST_F(MjCollisionBoxFuzzTest, AgreesWithReferencesZeroMargin) {
|
|
for (const SizeCase& c : kSizeCases) {
|
|
Stats st = Sweep(c, NumConfigs(), /*margin=*/0, 12345 + BaseSeed());
|
|
Report("margin=0", c, st);
|
|
CheckGates(c, st, 0);
|
|
}
|
|
}
|
|
|
|
TEST_F(MjCollisionBoxFuzzTest, AgreesWithReferencesWithMargin) {
|
|
for (const SizeCase& c : kSizeCases) {
|
|
Stats st = Sweep(c, NumConfigs(), /*margin=*/0.01, 999 + BaseSeed());
|
|
Report("margin>0", c, st);
|
|
CheckGates(c, st, 0.01);
|
|
}
|
|
}
|
|
|
|
TEST_F(MjCollisionBoxFuzzTest, CanonicalOrientationsAndPerturbations) {
|
|
// 24 rotational symmetries of the cube (octahedral group Oh)
|
|
std::vector<std::array<mjtNum, 4>> canonical_quats;
|
|
for (int ax = 0; ax < 3; ax++) {
|
|
for (int sx : {-1, 1}) {
|
|
for (int ay = 0; ay < 3; ay++) {
|
|
if (ay == ax) continue;
|
|
for (int sy : {-1, 1}) {
|
|
mjtNum mat[9] = {0};
|
|
mat[3 * 0 + ax] = sx;
|
|
mat[3 * 1 + ay] = sy;
|
|
// col 2 = col 0 x col 1
|
|
mat[3 * 2 + 0] = mat[3 * 0 + 1] * mat[3 * 1 + 2] -
|
|
mat[3 * 0 + 2] * mat[3 * 1 + 1];
|
|
mat[3 * 2 + 1] = mat[3 * 0 + 2] * mat[3 * 1 + 0] -
|
|
mat[3 * 0 + 0] * mat[3 * 1 + 2];
|
|
mat[3 * 2 + 2] = mat[3 * 0 + 0] * mat[3 * 1 + 1] -
|
|
mat[3 * 0 + 1] * mat[3 * 1 + 0];
|
|
mjtNum q[4];
|
|
mju_mat2Quat(q, mat);
|
|
canonical_quats.push_back({q[0], q[1], q[2], q[3]});
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
const mjtNum pert_angles[] = {0.0, 1e-15, 1e-12, 1e-9,
|
|
1e-6, 1e-3, 0.05, 0.785398};
|
|
const mjtNum pert_axes[5][3] = {
|
|
{1, 0, 0},
|
|
{0, 1, 0},
|
|
{0, 0, 1},
|
|
{0.70710678, 0.70710678, 0},
|
|
{0.57735027, 0.57735027, 0.57735027}};
|
|
|
|
// test across cube and anisotropic slab/needle size cases
|
|
for (const SizeCase& c :
|
|
{kSizeCases[0], kSizeCases[1], kSizeCases[2], kSizeCases[4]}) {
|
|
const std::string xml = MakeXml(c);
|
|
char error[1024];
|
|
MjModelPtr model_ptr =
|
|
LoadModelFromString(xml.c_str(), error, sizeof(error));
|
|
ASSERT_THAT(model_ptr.get(), NotNull()) << error;
|
|
mjModel* model = model_ptr.get();
|
|
MjDataPtr data_ptr = MakeData(model_ptr);
|
|
mjData* data = data_ptr.get();
|
|
|
|
for (mjtNum margin : {0.0, 0.005}) {
|
|
for (const auto& qbase : canonical_quats) {
|
|
for (mjtNum angle : pert_angles) {
|
|
for (const auto& axis : pert_axes) {
|
|
mjtNum qpert[4], quat2[4];
|
|
mju_axisAngle2Quat(qpert, axis, angle);
|
|
mju_mulQuat(quat2, qbase.data(), qpert);
|
|
mjtNum quat1[4] = {1, 0, 0, 0};
|
|
|
|
mjtNum mat2[9];
|
|
mju_quat2Mat(mat2, quat2);
|
|
mjtNum rproj[3] = {
|
|
mju_abs(mat2[0]) * c.size2[0] + mju_abs(mat2[1]) * c.size2[1] +
|
|
mju_abs(mat2[2]) * c.size2[2],
|
|
mju_abs(mat2[3]) * c.size2[0] + mju_abs(mat2[4]) * c.size2[1] +
|
|
mju_abs(mat2[5]) * c.size2[2],
|
|
mju_abs(mat2[6]) * c.size2[0] + mju_abs(mat2[7]) * c.size2[1] +
|
|
mju_abs(mat2[8]) * c.size2[2],
|
|
};
|
|
|
|
// lateral fraction offsets and depth fraction offsets
|
|
for (mjtNum xfrac : {-0.5, 0.0, 0.5, 0.99, 1.0}) {
|
|
for (mjtNum yfrac : {-0.5, 0.0, 0.5, 0.99, 1.0}) {
|
|
if (xfrac * xfrac + yfrac * yfrac > 1.01) continue;
|
|
for (mjtNum zfrac : {-0.2, -1e-4, 0.0, 1e-4, 0.1}) {
|
|
mjtNum pos2[3] = {
|
|
xfrac * (c.size1[0] + rproj[0]),
|
|
yfrac * (c.size1[1] + rproj[1]),
|
|
(c.size1[2] + rproj[2]) +
|
|
zfrac * (c.size1[2] + rproj[2])};
|
|
SetPose(model, data, pos2, quat1, quat2);
|
|
|
|
mjPreContact box_con[mjMAXCONPAIR];
|
|
int nbox = mjc_BoxBox(model, data, box_con, 0, 1, margin);
|
|
EXPECT_LE(nbox, kMaxContacts) << c.name;
|
|
|
|
// normal consistency across manifold
|
|
for (int i = 1; i < nbox; i++) {
|
|
EXPECT_GE(mju_dot3(box_con[0].normal, box_con[i].normal),
|
|
1 - MjTol(1e-9, 1e-5))
|
|
<< c.name;
|
|
}
|
|
|
|
// contacts within half depth of both boxes
|
|
mjtNum scale = c.size1[0] + c.size1[1] + c.size1[2] +
|
|
c.size2[0] + c.size2[1] + c.size2[2];
|
|
for (int i = 0; i < nbox; i++) {
|
|
mjtNum slack =
|
|
0.5 * mju_abs(box_con[i].dist) + 0.05 * scale;
|
|
mjtNum sz1[3] = {model->geom_size[0] + margin + slack,
|
|
model->geom_size[1] + margin + slack,
|
|
model->geom_size[2] + margin + slack};
|
|
mjtNum sz2[3] = {model->geom_size[3] + margin + slack,
|
|
model->geom_size[4] + margin + slack,
|
|
model->geom_size[5] + margin + slack};
|
|
int o1 = mju_outsideBox(box_con[i].pos, data->geom_xpos,
|
|
data->geom_xmat, sz1, 1);
|
|
int o2 = mju_outsideBox(box_con[i].pos,
|
|
data->geom_xpos + 3,
|
|
data->geom_xmat + 9, sz2, 1);
|
|
EXPECT_FALSE(o1 == 1 && o2 == 1)
|
|
<< c.name << " pos=(" << box_con[i].pos[0] << ", "
|
|
<< box_con[i].pos[1] << ", " << box_con[i].pos[2]
|
|
<< ")";
|
|
}
|
|
|
|
mjtNum exact_sep = ExactSatSep(model, data);
|
|
if (margin == 0) {
|
|
if (exact_sep < -1e-6 * scale) {
|
|
EXPECT_GT(nbox, 0)
|
|
<< c.name << " exact_sep=" << exact_sep;
|
|
}
|
|
}
|
|
if (nbox > 0 && exact_sep < 0) {
|
|
mjtNum db = DeepestDist(box_con, nbox);
|
|
EXPECT_GE(db, exact_sep - 0.06 * mju_abs(exact_sep) -
|
|
1e-6 * scale)
|
|
<< c.name << " db=" << db
|
|
<< " exact_sep=" << exact_sep;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
} // namespace
|
|
} // namespace mujoco
|