diff --git a/src/engine/engine_collision_gjk.c b/src/engine/engine_collision_gjk.c index b4fa8d98..4ac22a44 100644 --- a/src/engine/engine_collision_gjk.c +++ b/src/engine/engine_collision_gjk.c @@ -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; diff --git a/src/engine/engine_collision_gjk.h b/src/engine/engine_collision_gjk.h index df5a121c..336c165a 100644 --- a/src/engine/engine_collision_gjk.h +++ b/src/engine/engine_collision_gjk.h @@ -19,6 +19,7 @@ #include #include +#include #include #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 diff --git a/test/engine/engine_collision_gjk_test.cc b/test/engine/engine_collision_gjk_test.cc index 54c804ba..23d915b2 100644 --- a/test/engine/engine_collision_gjk_test.cc +++ b/test/engine/engine_collision_gjk_test.cc @@ -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"( + + + + + + )"; + + 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