From d360ea1ce6138707c4cb03e570f3f6a4516f4f7e Mon Sep 17 00:00:00 2001 From: Kyle Bayes Date: Fri, 23 Aug 2024 07:43:43 -0700 Subject: [PATCH] Minor GJK + EPA improvements: 1. Correct stopping criteria for GJK. 2. Store witness points from EPA in mjCCDObj, 3. Get dir directly from EPA instead of computing dir from witness points. PiperOrigin-RevId: 666791852 Change-Id: I6a168f635b57c17692d31da6f6076b48c32ff2e3 --- src/engine/engine_collision_gjk.c | 40 +++++++++++++++--------- test/engine/engine_collision_gjk_test.cc | 7 +++-- 2 files changed, 29 insertions(+), 18 deletions(-) diff --git a/src/engine/engine_collision_gjk.c b/src/engine/engine_collision_gjk.c index daab5889..ee7dfdcc 100644 --- a/src/engine/engine_collision_gjk.c +++ b/src/engine/engine_collision_gjk.c @@ -97,8 +97,8 @@ static void attachFace(Polytope* pt, int v1, int v2, int v3); // returns the penetration depth (negative distance) of the convex objects // witness points are stored in x1 and x2 -static mjtNum epa(const mjCCDConfig* config, Polytope* pt, - mjCCDObj* obj1, mjCCDObj* obj2, mjtNum x1[3], mjtNum x2[3]); +static mjtNum epa(const mjCCDConfig* config, Polytope* pt, mjCCDObj* obj1, mjCCDObj* obj2, + mjtNum dir[3]); // internal data structure for the returning simplex from GJK typedef struct { @@ -118,6 +118,7 @@ static mjtNum _gjk(const mjCCDConfig* config, mjCCDObj* obj1, mjCCDObj* obj2, mjtNum* x1_k = obj1->x0; mjtNum* x2_k = obj2->x0; mju_sub3(x_k, x1_k, x2_k); + mjtNum epsilon = config->tolerance * config->tolerance; int N = config->max_iterations; for (size_t k = 0; k < N; k++) { @@ -130,10 +131,10 @@ static mjtNum _gjk(const mjCCDConfig* config, mjCCDObj* obj1, mjCCDObj* obj2, mju_sub3(s_k, s1, s2); // the stopping criteria relies on the Frank-Wolfe duality gap given by - // f(x_k) - f(x_min) <= < grad f(x_k), (x_k - s_k) > + // |f(x_k) - f(x_min)|^2 <= < grad f(x_k), (x_k - s_k) > mjtNum diff[3]; mju_sub3(diff, x_k, s_k); - if (2*mju_dot3(x_k, diff) < config->tolerance) { + if (2*mju_dot3(x_k, diff) < epsilon) { break; } @@ -173,6 +174,11 @@ static mjtNum _gjk(const mjCCDConfig* config, mjCCDObj* obj1, mjCCDObj* obj2, // simplex in Minkowski difference mju_copy3(simplex + 3*n++, simplex + 3*i); } + + // we have a tetrahedron containing the origin so return early + if (n == 4) { + break; + } } if (ret1 && ret2) { ret1->nverts = n; @@ -888,8 +894,8 @@ static void epa_witness(const Polytope* pt, int index, mjtNum x1[3], mjtNum x2[3 } // returns the penetration depth (negative distance) of the convex objects -static mjtNum epa(const mjCCDConfig* config, Polytope* pt, - mjCCDObj* obj1, mjCCDObj* obj2, mjtNum x1[3], mjtNum x2[3]) { +static mjtNum epa(const mjCCDConfig* config, Polytope* pt, mjCCDObj* obj1, mjCCDObj* obj2, + mjtNum dir[3]) { mjtNum dist = mjMAXVAL; int index; Horizon h; @@ -941,7 +947,9 @@ static mjtNum epa(const mjCCDConfig* config, Polytope* pt, h.n = 0; // clear horizon } mju_free(h.edges); - epa_witness(pt, index, x1, x2); + Face face = pt->faces[index]; + mju_copy3(dir, face.v); + epa_witness(pt, index, obj1->x0, obj2->x0); return dist; } @@ -949,7 +957,7 @@ static mjtNum epa(const mjCCDConfig* config, Polytope* pt, // runs both GJK and EPA (if needed) static mjtNum _gjk_epa(const mjCCDConfig* config, mjCCDObj* obj1, mjCCDObj* obj2, Polytope* pt, - mjtNum x1[3], mjtNum x2[3]) { + mjtNum dir[3]) { Simplex simplex1, simplex2; mjtNum dist = _gjk(config, obj1, obj2, &simplex1, &simplex2); @@ -965,7 +973,7 @@ static mjtNum _gjk_epa(const mjCCDConfig* config, mjCCDObj* obj1, mjCCDObj* obj2 // simplex not on boundary (objects are penetrating) if (ret) { - dist = epa(config, pt, obj1, obj2, x1, x2); + dist = epa(config, pt, obj1, obj2, dir); return -dist; } return 0; @@ -981,21 +989,23 @@ int mj_gjkPenetration(const void *obj1, const void *obj2, const ccd_t *ccd, Polytope pt; initPolytope(&pt); mjCCDConfig config; + mjCCDObj* o1 = (mjCCDObj*) obj1; + mjtNum* x1 = o1->x0; mjCCDObj* o2 = (mjCCDObj*) obj2; - o1->center(o1->x0, o1); - o2->center(o2->x0, o2); + mjtNum* x2 = o2->x0; + + o1->center(x1, o1); + o2->center(x2, o2); config.max_iterations = ccd->max_iterations; config.tolerance = ccd->mpr_tolerance; - mjtNum x1[3], x2[3]; - mjtNum dist = _gjk_epa(&config, o1, o2, &pt, x1, x2); + mjtNum d[3]; + mjtNum dist = _gjk_epa(&config, o1, o2, &pt, d); if (dist < 0) { if (depth) *depth = -dist; if (dir) { - mjtNum d[3]; - mju_sub3(d, x1, x2); mju_normalize3(d); mju_copy3(dir->v, d); } diff --git a/test/engine/engine_collision_gjk_test.cc b/test/engine/engine_collision_gjk_test.cc index da1d1602..d41972a8 100644 --- a/test/engine/engine_collision_gjk_test.cc +++ b/test/engine/engine_collision_gjk_test.cc @@ -82,10 +82,11 @@ mjtNum run_gjkPenetration(mjModel* m, mjData* d, int g1, int g2, ccd_real_t depth; ccd_vec3_t ccd_dir, ccd_pos; - mj_gjkPenetration(&obj1, &obj2, &ccd, &depth, &ccd_dir, &ccd_pos); + int ret = mj_gjkPenetration(&obj1, &obj2, &ccd, &depth, &ccd_dir, &ccd_pos); + if (ret) return 0; // objects not colliding if (dir) mju_copy3(dir, ccd_dir.v); if (pos) mju_copy3(pos, ccd_pos.v); - return depth; + return -depth; } using MjGjkTest = MujocoTest; @@ -170,7 +171,7 @@ TEST_F(MjGjkTest, BoxBoxIntersect) { mjtNum dir[3], pos[3]; mjtNum dist = run_gjkPenetration(model, data, geom1, geom2, dir, pos); - EXPECT_NEAR(dist, 1, kTolerance); + EXPECT_NEAR(dist, -1, kTolerance); EXPECT_NEAR(dir[0], 1, kTolerance); EXPECT_NEAR(dir[1], 0, kTolerance); EXPECT_NEAR(dir[2], 0, kTolerance);