diff --git a/src/engine/engine_collision_gjk.c b/src/engine/engine_collision_gjk.c index 20d13299..e35b2405 100644 --- a/src/engine/engine_collision_gjk.c +++ b/src/engine/engine_collision_gjk.c @@ -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; diff --git a/test/engine/engine_collision_gjk_test.cc b/test/engine/engine_collision_gjk_test.cc index cfcba486..09e2c58e 100644 --- a/test/engine/engine_collision_gjk_test.cc +++ b/test/engine/engine_collision_gjk_test.cc @@ -42,7 +42,7 @@ using ::testing::DoubleNear; constexpr mjtNum kTolerance = 1e-6; constexpr int kMaxIterations = 1000; -constexpr char kEllipoid[] = R"( +constexpr char kEllipsoidXml[] = R"( @@ -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"( + + + + + + )"; + + 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 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"(