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

Check if geoms are separated in GJK after an early exit. Fix #3383.

Browse Source 
PiperOrigin-RevId: 960316577
Change-Id: I328322e0b1bd14089defb26e9dc35a264642b4d6
This commit is contained in:
Kyle Bayes
2026-08-06 08:26:17 -07:00
committed by Copybara-Service
parent 39e4458806
commit 35342f403f
3 changed files with 81 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_collision_gjk.c
+24 -7
View File
@@ -203,6 +203,7 @@ static void gjk(mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) {
mjtNum lambda[4]; // barycentric coordinates for x_k
mjtNum cutoff2 = status->dist_cutoff * status->dist_cutoff;
mjtNum tol2 = status->tolerance * status->tolerance;
status->separated = 0;
// if both geoms are discrete, finite convergence is guaranteed; set tolerance to 0
mjtNum epsilon = discreteGeoms(obj1, obj2) ? 0 : 0.5 * tol2;
@@ -236,6 +237,7 @@ static void gjk(mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) {
// if geom distance isn't requested, return early
if (!get_dist) {
if (dot3(x_k, s_k) > 0) {
status->separated = 1;
status->gjk_iterations = k;
status->nsimplex = 0;
status->nx = 0;
@@ -245,6 +247,7 @@ static void gjk(mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) {
} else if (status->dist_cutoff < mjMAX_LIMIT) {
mjtNum vs = dot3(x_k, s_k);
if (vs > 0 && (vs * vs) >= cutoff2 * (x_norm * x_norm)) {
status->separated = 1;
status->gjk_iterations = k;
status->nsimplex = 0;
status->nx = 0;
@@ -260,6 +263,7 @@ static void gjk(mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) {
int ret = gjkIntersect(status, obj1, obj2);
if (ret != -1) {
status->nx = 0;
status->separated = ret == 0;
status->dist = ret > 0 ? 0 : mjMAX_LIMIT;
return;
}
@@ -285,20 +289,20 @@ static void gjk(mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) {
status->nsimplex = 0;
status->nx = 0;
status->dist = mjMAX_LIMIT;
status->separated = 1;
return;
}
// we have a tetrahedron containing the origin so return early
if (n == 4) {
x_norm = 0;
break;
}
// get the next iteration of x_k, save previous x_norm
lincomb(x_k, lambda, n, simplex[0].vert, simplex[1].vert,
simplex[2].vert, simplex[3].vert);
x_norm_prev = x_norm;
x_norm = norm3(x_k);
// we should have a tetrahedron containing the origin so return early
if (n == 4) {
break;
}
}
// compute the approximate witness points
@@ -309,6 +313,18 @@ static void gjk(mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) {
simplex[3].vert2);
}
// GJK exited early; do a final separation check
Vertex tmp;
gjkSupport(&tmp, obj1, obj2, x_k, x_norm);
if (dot3(x_k, tmp.vert) > 0) {
status->separated = 1;
}
// tetrahedron containing the origin
if (n == 4 && status->separated == 0) {
x_norm = 0;
}
status->nx = 1;
status->gjk_iterations = k;
status->nsimplex = n;
@@ -2371,7 +2387,8 @@ mjtNum mjc_ccd(const mjCCDConfig* config, mjCCDStatus* status, mjCCDObj* obj1, m
return status->dist;
}
if (status->dist <= config->tolerance && status->nsimplex > 1 && config->buffer) {
if (status->dist <= config->tolerance && status->nsimplex > 1
&& config->buffer && !status->separated) {
status->dist = 0; // assume touching
Polytope pt;
pt.nfaces = pt.nmap = pt.nverts = pt.horizon.nedges = 0;
src/engine/engine_collision_gjk.h
+2
View File
@@ -19,6 +19,7 @@
#include <stddef.h>
#include <mujoco/mjexport.h>
#include <mujoco/mjmodel.h>
#include <mujoco/mjtype.h>
#include "engine/engine_collision_convex.h"
@@ -82,6 +83,7 @@ typedef struct {
typedef struct {
// geom distance information
mjtNum dist; // distance between geoms
int separated; // set to true if geoms are verified to be separated
mjtNum x1[3 * mjMAXCONPAIR]; // witness points for geom 1
mjtNum x2[3 * mjMAXCONPAIR]; // witness points for geom 2
int nx; // number of witness points
test/engine/engine_collision_gjk_test.cc
+55 -8
View File
@@ -604,7 +604,6 @@ TEST_F(MjGjkTest, BoxBoxTouching) {
ASSERT_EQ(ncons, 0);
EXPECT_EQ(status.epa_status, -1);
}
TEST_F(MjGjkTest, BoxBoxMultiCCD) {
@@ -1100,7 +1099,6 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD10) {
int ncons = Penetration(status, dist, dir, pos, model, data, g1, g2, 0, 8);
ASSERT_EQ(ncons, 4);
}
TEST_F(MjGjkTest, BoxBoxMultiCCD11) {
@@ -1220,7 +1218,6 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD12) {
int ncons = Penetration(status, dist, dir, pos, model, data, g1, g2, 0, 8);
ASSERT_EQ(ncons, 4);
}
TEST_F(MjGjkTest, BoxBoxMultiCCD13) {
@@ -1959,11 +1956,8 @@ TEST_F(MjGjkTest, BoxBoxLarge) {
int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
int g2 = mj_name2id(model.get(), mjOBJ_GEOM, "geom2");
mjtNum* xmat = data->geom_xmat;
mjtNum* xpos = data->geom_xpos;
xpos = data->geom_xpos + 3;
xmat = data->geom_xmat + 9;
mjtNum* xpos = data->geom_xpos + 3;
mjtNum* xmat = data->geom_xmat + 9;
xpos[0] = -0.000000000043537;
xpos[1] = -0.000000000012973;
@@ -2131,5 +2125,58 @@ TEST_F(MjGjkTest, BoxEdgeFlipped) {
EXPECT_NEAR(status.x2[2], 0.700000, kTolerance);
}
TEST_F(MjGjkTest, ThinBoxGrazing) {
// thin boxes separated by +7.6933e-7, below the ccd_tolerance of 1e-6
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<geom name="geom1" type="box" size="0.015108062742470629 0.00020768330936692047 0.00033708479398415232"/>
<geom name="geom2" type="box" size="0.0022889234034685867 0.00039019141130537644 0.00066943745945985227"/>
</worldbody>
</mujoco>)";
MjModelPtr model = LoadModelFromString(xml);
MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get());
mjtNum* xpos = data->geom_xpos;
mjtNum* xmat = data->geom_xmat;
xmat[0] = 0.41747699392674587;
xmat[1] = 0.7445704580736479;
xmat[2] = -0.5208913442416622;
xmat[3] = 0.735898261710082;
xmat[4] = -0.6133129289446175;
xmat[5] = -0.28688150794614425;
xmat[6] = -0.5330728917830726;
xmat[7] = -0.263556605216739;
xmat[8] = -0.8039721437292835;
xpos = data->geom_xpos + 3;
xmat = data->geom_xmat + 9;
xpos[0] = -0.0045224108719272319;
xpos[1] = -0.0099558526994404459;
xpos[2] = 0.00749543399075938;
xmat[0] = -0.11330328891322261;
xmat[1] = 0.6014637637569688;
xmat[2] = -0.7908246996703808;
xmat[3] = 0.9557435215667675;
xmat[4] = 0.2834865022971088;
xmat[5] = 0.07867479900517776;
xmat[6] = 0.2715081687621951;
xmat[7] = -0.7469114699230777;
xmat[8] = -0.6069650487406557;
int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
int g2 = mj_name2id(model.get(), mjOBJ_GEOM, "geom2");
mjtNum dist = mj_geomDistance(model.get(), data.get(), g1, g2, 1, nullptr);
// below ccd_tolerance, but still positive distance
EXPECT_GE(dist, 0);
EXPECT_LE(dist, model->opt.ccd_tolerance);
}
} // namespace
} // namespace mujoco