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

Fix issue in GJK where the tolerance for x_norm was set too low.

Browse Source 
PiperOrigin-RevId: 880735671
Change-Id: I324e37e8677d3c7feeb4b8bcf065a2a0122326b6
This commit is contained in:
Kyle Bayes
2026-03-09 02:14:15 -07:00
committed by Copybara-Service
parent 1370a54007
commit 330c36c99b
2 changed files with 78 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_collision_gjk.c
+9 -4
View File
@@ -180,21 +180,26 @@ static void gjk(mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) {
mjtNum x_k[3]; // the kth approximation point in Minkowski difference
mjtNum lambda[4] = {1, 0, 0, 0}; // barycentric coordinates for x_k
mjtNum cutoff2 = status->dist_cutoff * status->dist_cutoff;
mjtNum tol2 = status->tolerance * status->tolerance;
// if both geoms are discrete, finite convergence is guaranteed; set tolerance to 0
mjtNum epsilon = discreteGeoms(obj1, obj2) ? 0 : 0.5 * status->tolerance * status->tolerance;
mjtNum epsilon = discreteGeoms(obj1, obj2) ? 0 : 0.5 * tol2;
// tolerance on squared norm of x_k
mjtNum min_norm2 = discreteGeoms(obj1, obj2) ? mjMINVAL2 : tol2;
mjtNum x_norm;
// set initial guess
sub3(x_k, x1_k, x2_k);
for (; k < kmax; k++) {
// compute the kth support point
x_norm = dot3(x_k, x_k);
if (x_norm < mjMINVAL2) {
// in tolerance for geoms to be in contact
if ((x_norm = dot3(x_k, x_k)) < min_norm2) {
break;
}
x_norm = mju_sqrt(x_norm);
// compute the kth support point
gjkSupport(simplex + n, obj1, obj2, x_k, x_norm);
mjtNum *s_k = simplex[n].vert;
test/engine/engine_collision_gjk_test.cc
+69 -2
View File
@@ -42,7 +42,7 @@ using ::testing::DoubleNear;
constexpr mjtNum kTolerance = 1e-6;
constexpr int kMaxIterations = 1000;
constexpr char kEllipoid[] = R"(
constexpr char kEllipsoidXml[] = R"(
<mujoco model="Ellipsoid Test">
<compiler angle="radian"/>
<size nkey="1"/>
@@ -1760,7 +1760,7 @@ TEST_F(MjGjkTest, MeshEdge2) {
TEST_F(MjGjkTest, EllipsoidEllipsoidPenetrating) {
char error[1024];
mjModel* model = LoadModelFromString(kEllipoid, error, sizeof(error));
mjModel* model = LoadModelFromString(kEllipsoidXml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error;
mjData* data = mj_makeData(model);
@@ -1806,6 +1806,73 @@ TEST_F(MjGjkTest, EllipsoidEllipsoid) {
mj_deleteModel(model);
}
TEST_F(MjGjkTest, EllipsoidEllipsoidSlowConvergence) {
// This example takes gjkIntersect 30 iterations to converge, thus failing to
// detect collision if opt.ccd_iterations is set to 35. With gjkIntersect
// disabled, GJK succeeds in 24 iterations.
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<geom name="geom1" type="ellipsoid" size=".05 .07 .15"/>
<geom name="geom2" type="ellipsoid" size=".05 .07 .15"/>
</worldbody>
</mujoco>)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error;
mjData* data = mj_makeData(model);
mj_forward(model, data);
mjtNum* xmat = data->geom_xmat;
mjtNum* xpos = data->geom_xpos;
xmat[0] = 0.98799245191271645172;
xmat[1] = -0.13055117805648397411;
xmat[2] = -0.08262750674904573156;
xmat[3] = -0.03910798838001148386;
xmat[4] = 0.30606703783583483203;
xmat[5] = -0.95120635699897815307;
xmat[6] = 0.14947066671547387662;
xmat[7] = 0.94301609650019369013;
xmat[8] = 0.29728632920665870598;
xpos[0] = 0.44826351361072502844;
xpos[1] = -0.10441423315171689812;
xpos[2] = 0.21216466278453649519;
xmat = data->geom_xmat + 9;
xpos = data->geom_xpos + 3;
xmat[0] = 0.53301656081215953442;
xmat[1] = -0.59335516633486651283;
xmat[2] = -0.60317741377119038493;
xmat[3] = -0.39469414877746378245;
xmat[4] = 0.45620167535223110633;
xmat[5] = -0.79755661888461082398;
xmat[6] = 0.74840488695679441289;
xmat[7] = 0.66318148194106107951;
xmat[8] = 0.00896923573374096283;
xpos[0] = 0.39694137187624300989;
xpos[1] = 0.00961542646741688108;
xpos[2] = 0.29832742817753182818;
int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1");
int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2");
mjCCDStatus status;
std::vector<mjtNum> dir, pos;
mjtNum dist;
Penetration(status, dist, dir, pos, model, data, geom1, geom2);
EXPECT_LT(dist, 0.0);
EXPECT_NEAR(dist, 0.0, kTolerance);
mj_deleteData(data);
mj_deleteModel(model);
}
TEST_F(MjGjkTest, BoxBox) {
static constexpr char xml[] = R"(
<mujoco>