From 0e8cca93e185c0ca7dfdd15b52db591c591a9c14 Mon Sep 17 00:00:00 2001 From: Kyle Bayes Date: Wed, 13 Nov 2024 10:27:54 -0800 Subject: [PATCH] Refactor nativeccd interface. - Update mj_ccd interface for multi-contacts, - Support GJK cutoff distance, and - Remove mjc_fixNormal from nativeccd (causes bug with cylindar box collisions). PiperOrigin-RevId: 696185362 Change-Id: I4828b7ee1bde078268220172a4937cd553f6187e --- src/engine/engine_collision_convex.c | 79 ++++++++++++++---------- src/engine/engine_collision_convex.h | 2 +- src/engine/engine_collision_gjk.c | 77 +++++++++++++++-------- src/engine/engine_collision_gjk.h | 34 +++++----- src/engine/engine_support.c | 10 +-- test/engine/engine_collision_gjk_test.cc | 69 +++++++++++++-------- 6 files changed, 162 insertions(+), 109 deletions(-) diff --git a/src/engine/engine_collision_convex.c b/src/engine/engine_collision_convex.c index 30e6d07f..1fac0a77 100644 --- a/src/engine/engine_collision_convex.c +++ b/src/engine/engine_collision_convex.c @@ -30,7 +30,7 @@ #include "engine/engine_util_misc.h" #include "engine/engine_util_spatial.h" -// call LibCCD or GJK to recover penetration info +// call libccd or nativeccd to recover penetration info static int mjc_penetration(const mjModel* m, mjCCDObj* obj1, mjCCDObj* obj2, const ccd_t* ccd, ccd_real_t* depth, ccd_vec3_t* dir, ccd_vec3_t* pos) { if (mjENABLED(mjENBL_NATIVECCD)) { @@ -40,8 +40,8 @@ static int mjc_penetration(const mjModel* m, mjCCDObj* obj1, mjCCDObj* obj2, // set config config.max_iterations = ccd->max_iterations, config.tolerance = ccd->mpr_tolerance, - config.contacts = 1; - config.distances = 0; // no geom distances needed + config.max_contacts = 1; + config.dist_cutoff = 0; // no geom distances needed mjtNum dist = mjc_ccd(&config, &status, obj1, obj2); if (dist < 0) { @@ -68,6 +68,7 @@ static int mjc_penetration(const mjModel* m, mjCCDObj* obj1, mjCCDObj* obj2, } + // ccd center function void mjccd_center(const void *obj, ccd_vec3_t *center) { mjc_center(center->v, (const mjCCDObj*) obj); @@ -744,12 +745,46 @@ static void mjc_initCCD(ccd_t* ccd, const mjModel* m) { // find single convex-convex collision -static int mjc_CCDIteration(mjCCDObj* obj1, mjCCDObj* obj2, const ccd_t* ccd, - const mjModel* m, const mjData* d, +static int mjc_CCDIteration(const mjModel* m, const mjData* d, mjCCDObj* obj1, mjCCDObj* obj2, mjContact* con, mjtNum margin) { + if (mjENABLED(mjENBL_NATIVECCD)) { + mjCCDConfig config; + mjCCDStatus status; + + // set config + config.max_iterations = m->opt.ccd_iterations; + config.tolerance = m->opt.ccd_tolerance; + config.max_contacts = 1; + config.dist_cutoff = 0; // no geom distances needed + + mjtNum dist = mjc_ccd(&config, &status, obj1, obj2); + if (dist < 0) { + con->dist = margin + dist; + mju_sub3(con->frame, status.x1, status.x2); + mju_normalize3(con->frame); + con->pos[0] = 0.5 * (status.x1[0] + status.x2[0]); + con->pos[1] = 0.5 * (status.x1[1] + status.x2[1]); + con->pos[2] = 0.5 * (status.x1[2] + status.x2[2]); + mju_zero3(con->frame+3); + return 1; + } + return 0; + } + + // init libccd structure + ccd_t ccd; + mjc_initCCD(&ccd, m); + ccd.first_dir = ccdFirstDirDefault; + ccd.center1 = mjccd_center; + ccd.center2 = mjccd_center; + ccd.support1 = mjccd_support; + ccd.support2 = mjccd_support; + ccd_vec3_t dir, pos; ccd_real_t depth; - if (mjc_penetration(m, obj1, obj2, ccd, &depth, &dir, &pos) == 0) { + + // call MPR from libccd + if (ccdMPRPenetration(obj1, obj2, &ccd, &depth, &dir, &pos) == 0) { // contact is found but normal is undefined if (ccdVec3Eq(&dir, ccd_vec3_origin)) { return 0; @@ -768,11 +803,7 @@ static int mjc_CCDIteration(mjCCDObj* obj1, mjCCDObj* obj2, const ccd_t* ccd, return 1; } - - // no contact found - else { - return 0; - } + return 0; } @@ -819,17 +850,8 @@ int mjc_Convex(const mjModel* m, const mjData* d, mjc_initCCDObj(&obj1, m, d, g1, margin); mjc_initCCDObj(&obj2, m, d, g2, margin); - // init libccd structure - ccd_t ccd; - mjc_initCCD(&ccd, m); - ccd.first_dir = ccdFirstDirDefault; - ccd.center1 = mjccd_center; - ccd.center2 = mjccd_center; - ccd.support1 = mjccd_support; - ccd.support2 = mjccd_support; - // find initial contact - int ncon = mjc_CCDIteration(&obj1, &obj2, &ccd, m, d, con, margin); + int ncon = mjc_CCDIteration(m, d, &obj1, &obj2, con, margin); // look for additional contacts if (ncon && mjENABLED(mjENBL_MULTICCD) // TODO(tassa) leave as bitflag or make geom attribute (?) @@ -878,7 +900,7 @@ int mjc_Convex(const mjModel* m, const mjData* d, mju_rotateFrame(con[0].pos, invrot, d->geom_xmat+9*g2, d->geom_xpos+3*g2); // search for new contact - int new_contact = mjc_CCDIteration(&obj1, &obj2, &ccd, m, d, con+ncon, margin); + int new_contact = mjc_CCDIteration(m, d, &obj1, &obj2, con+ncon, margin); // check new contact if (new_contact && mjc_isDistinctContact(con, ncon + 1, tolerance)) { @@ -1525,19 +1547,8 @@ int mjc_ConvexElem(const mjModel* m, const mjData* d, mjContact* con, mjc_setCCDObjFlex(&obj1, f1, e1, v1); mjc_setCCDObjFlex(&obj2, f2, e2, -1); - // init libccd structure - ccd_t ccd; - mjc_initCCD(&ccd, m); - ccd.first_dir = ccdFirstDirDefault; - ccd.center1 = mjccd_center; - ccd.center2 = mjccd_center; - ccd.support1 = mjccd_support; - ccd.support2 = mjccd_support; - // find contacts - int ncon = mjc_CCDIteration(&obj1, &obj2, &ccd, m, d, con, margin); - - return ncon; + return mjc_CCDIteration(m, d, &obj1, &obj2, con, margin); } diff --git a/src/engine/engine_collision_convex.h b/src/engine/engine_collision_convex.h index 1f2e347f..c402c9c6 100644 --- a/src/engine/engine_collision_convex.h +++ b/src/engine/engine_collision_convex.h @@ -85,7 +85,7 @@ int mjc_Convex (const mjModel* m, const mjData* d, int mjc_ConvexElem (const mjModel* m, const mjData* d, mjContact* con, int g1, int f1, int e1, int v1, int f2, int e2, mjtNum margin); -// heighfield-elem collision function using ccd +// heightfield-elem collision function using ccd int mjc_HFieldElem (const mjModel* m, const mjData* d, mjContact* con, int g, int f, int e, mjtNum margin); diff --git a/src/engine/engine_collision_gjk.c b/src/engine/engine_collision_gjk.c index f513dc19..59b4380f 100644 --- a/src/engine/engine_collision_gjk.c +++ b/src/engine/engine_collision_gjk.c @@ -77,8 +77,9 @@ static int newVertex(Polytope* pt, const mjtNum v1[3], const mjtNum v2[3]); // attaches a face to the polytope with the given vertex indices; returns non-zero on error static void attachFace(Polytope* pt, int v1, int v2, int v3, int adj1, int adj2, int adj3); -// returns 1 if objects are in contact, 0 otherwise; status must have initial tetrahedrons -static int gjkIntersect(mjCCDStatus* status, int start, mjCCDObj* obj1, mjCCDObj* obj2); +// returns 1 if objects are in contact; 0 if not; -1 if inconclusive +// status must have initial tetrahedrons +static int gjkIntersect(mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2); // returns the penetration depth of two convex objects; witness points are in status->{x1, x2} static mjtNum epa(mjCCDStatus* status, Polytope* pt, mjCCDObj* obj1, mjCCDObj* obj2); @@ -145,17 +146,18 @@ static int discreteGeoms(mjCCDObj* obj1, mjCCDObj* obj2) { // GJK algorithm static mjtNum gjk(mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) { - int get_dist = status->has_distances; // need to recover geom distances if not in contact - mjtNum *simplex1 = status->simplex1; // simplex for obj1 - mjtNum *simplex2 = status->simplex2; // simplex for obj2 - mjtNum *simplex = status->simplex; // simplex in Minkowski difference - int n = 0; // number of vertices in the simplex - int k = 0; // current iteration - int kmax = status->max_iterations; // max number of iterations - mjtNum* x1_k = status->x1; // the kth approximation point for obj1 - mjtNum* x2_k = status->x2; // the kth approximation point for obj2 - mjtNum x_k[3]; // the kth approximation point in Minkowski difference - mjtNum lambda[4]; // barycentric coordinates for x_k + int get_dist = status->dist_cutoff > 0; // need to recover geom distances if not in contact + mjtNum *simplex1 = status->simplex1; // simplex for obj1 + mjtNum *simplex2 = status->simplex2; // simplex for obj2 + mjtNum *simplex = status->simplex; // simplex in Minkowski difference + int n = 0; // number of vertices in the simplex + int k = 0; // current iteration + int kmax = status->max_iterations; // max number of iterations + mjtNum* x1_k = status->x1; // the kth approximation point for obj1 + mjtNum* x2_k = status->x2; // the kth approximation point for obj2 + mjtNum x_k[3]; // the kth approximation point in Minkowski difference + mjtNum lambda[4]; // barycentric coordinates for x_k + mjtNum cutoff2 = status->dist_cutoff * status->dist_cutoff; // if both geoms are discrete, finite convergence is guaranteed; set tolerance to 0 mjtNum epsilon = discreteGeoms(obj1, obj2) ? 0 : status->tolerance * status->tolerance; @@ -182,14 +184,29 @@ static mjtNum gjk(mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) { // if the hyperplane separates the Minkowski difference and origin, the objects don't collide // if geom distance isn't requested, return early - if (!get_dist && dot3(x_k, s_k) > 0) { - return mjMAXVAL; + if (!get_dist) { + if (dot3(x_k, s_k) > 0) { + status->gjk_iterations = k; + status->nsimplex = 0; + status->nx = 0; + return mjMAXVAL; + } + } else if (status->dist_cutoff < mjMAXVAL) { + mjtNum vs = mju_dot3(x_k, s_k), vv = mju_dot3(x_k, x_k); + if (mju_dot3(x_k, s_k) > 0 && (vs*vs / vv) >= cutoff2) { + status->gjk_iterations = k; + status->nsimplex = 0; + status->nx = 0; + return mjMAXVAL; + } } // tetrahedron is generated and only need contact info; fallback to gjkIntersect to // determine contact if (!get_dist && n == 3) { - return gjkIntersect(status, k, obj1, obj2) ? 0 : mjMAXVAL; + status->gjk_iterations = k; + status->nx = 0; + return gjkIntersect(status, obj1, obj2) > 0 ? 0 : mjMAXVAL; } // run the distance subalgorithm to compute the barycentric coordinates @@ -228,10 +245,11 @@ static mjtNum gjk(mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) { lincomb(x1_k, lambda, simplex1, n); lincomb(x2_k, lambda, simplex2, n); + status->nx = 1; status->gjk_iterations = k; status->nsimplex = n; - status->gjk_dist = mju_norm3(x_k); - return status->gjk_dist; + status->dist = mju_norm3(x_k); + return status->dist; } @@ -329,16 +347,16 @@ static inline mjtNum signedDistance(mjtNum normal[3], const mjtNum v1[3], const -// returns 0 if objects are in contact, mjMAXVAL otherwise; status must have initial tetrahedrons -static int gjkIntersect(mjCCDStatus* status, int start, mjCCDObj* obj1, mjCCDObj* obj2) { +// returns 1 if objects are in contact; 0 if not; -1 if inconclusive +static int gjkIntersect(mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) { mjtNum simplex1[12], simplex2[12], simplex[12]; memcpy(simplex1, status->simplex1, sizeof(mjtNum) * 12); memcpy(simplex2, status->simplex2, sizeof(mjtNum) * 12); memcpy(simplex, status->simplex, sizeof(mjtNum) * 12); int s[4] = {0, 3, 6, 9}; - int kmax = status->max_iterations; - for (int k = start; k < kmax; k++) { + int k = status->gjk_iterations, kmax = status->max_iterations; + for (; k < kmax; k++) { // compute the signed distance to each face in the simplex along with normals mjtNum dist[4], normals[12]; dist[0] = signedDistance(&normals[0], simplex + s[2], simplex + s[1], simplex + s[3]); @@ -359,6 +377,7 @@ static int gjkIntersect(mjCCDStatus* status, int start, mjCCDObj* obj1, mjCCDObj copy3(status->simplex1 + 3*n, simplex1 + s[n]); copy3(status->simplex2 + 3*n, simplex2 + s[n]); } + status->gjk_iterations = k; return 1; } @@ -368,6 +387,7 @@ static int gjkIntersect(mjCCDStatus* status, int start, mjCCDObj* obj1, mjCCDObj // found origin outside the Minkowski difference (return no collision) if (dot3(&normals[3*index], simplex + s[index]) < 0) { + status->gjk_iterations = k; return 0; } @@ -378,7 +398,8 @@ static int gjkIntersect(mjCCDStatus* status, int start, mjCCDObj* obj1, mjCCDObj s[i] = s[j]; s[j] = swap; } - return 0; // never found origin + status->gjk_iterations = k; + return -1; // never found origin } @@ -993,7 +1014,7 @@ static int polytope3(Polytope* pt, const mjCCDStatus* status, mjCCDObj* obj1, mj // TODO(kylebayes): It's possible for GJK to return a 2-simplex with the origin not contained in // it but within tolerance from it. In that case the hexahedron could possibly be constructed // that doesn't contain the origin, but nonetheless there is penetration depth. - if (status->gjk_dist > 10*mjMINVAL && !testTetra(v1, v2, v3, v4) && !testTetra(v1, v2, v3, v5)) { + if (status->dist > 10*mjMINVAL && !testTetra(v1, v2, v3, v4) && !testTetra(v1, v2, v3, v5)) { return 7; } @@ -1296,6 +1317,7 @@ static mjtNum epa(mjCCDStatus* status, Polytope* pt, mjCCDObj* obj1, mjCCDObj* o mj_freeStack(d); epaWitness(pt, face, status->x1, status->x2); status->epa_iterations = k; + status->nx = 1; return dist; } @@ -1306,16 +1328,17 @@ mjtNum mjc_ccd(const mjCCDConfig* config, mjCCDStatus* status, mjCCDObj* obj1, m // set up obj1->center(status->x1, obj1); obj2->center(status->x2, obj2); + status->gjk_iterations = 0; status->epa_iterations = -1; status->tolerance = config->tolerance; status->max_iterations = config->max_iterations; - status->has_contacts = config->contacts; - status->has_distances = config->distances; + status->max_contacts = config->max_contacts; + status->dist_cutoff = config->dist_cutoff; mjtNum dist = gjk(status, obj1, obj2); // penetration recovery for contacts not needed - if (!config->contacts) { + if (!config->max_contacts) { return dist; } diff --git a/src/engine/engine_collision_gjk.h b/src/engine/engine_collision_gjk.h index 80417b01..f04ca77a 100644 --- a/src/engine/engine_collision_gjk.h +++ b/src/engine/engine_collision_gjk.h @@ -16,7 +16,9 @@ #define MUJOCO_SRC_ENGINE_ENGINE_COLLISION_GJK_H_ #include +#include #include + #include "engine/engine_collision_convex.h" #ifdef __cplusplus @@ -27,30 +29,32 @@ extern "C" { struct _mjCCDConfig { int max_iterations; // the maximum number of iterations for GJK and EPA mjtNum tolerance; // tolerance used by GJK and EPA - int contacts; // set to true to recover contact (pendetration) info - int distances; // set to true to recover distance info + int max_contacts; // set to max number of contact points to recover + mjtNum dist_cutoff; // set to max geom distance to recover }; typedef struct _mjCCDConfig mjCCDConfig; // data produced from running GJK and EPA struct _mjCCDStatus { - mjtNum x1[3]; // witness point for geom 1 - mjtNum x2[3]; // witness point for geom 2 + // geom distance information + mjtNum dist; // distance between geoms + mjtNum x1[3 * mjMAXCONPAIR]; // witness points for geom 1 + mjtNum x2[3 * mjMAXCONPAIR]; // witness points for geom 2 + int nx; // number of witness points // configurations used - int max_iterations; // the maximum number of iterations for GJK and EPA - mjtNum tolerance; // tolerance used by GJK and EPA - int has_contacts; // set to true if attempted to recover contact (pendetration) info - int has_distances; // set to true if attempted to recover distance info + int max_iterations; // the maximum number of iterations for GJK and EPA + mjtNum tolerance; // tolerance used by GJK and EPA + int max_contacts; // set to max number of contact points to recover + mjtNum dist_cutoff; // set to max geom distance to recover // statistics for debugging purposes - mjtNum gjk_dist; // the distance returned by GJK - int gjk_iterations; // number of iterations that GJK ran - int epa_iterations; // number of iterations that EPA ran (negative if EPA did not run) - mjtNum simplex1[12]; // the simplex that GJK returned for obj1 - mjtNum simplex2[12]; // the simplex that GJK returned for obj2 - mjtNum simplex[12]; // the simplex that GJK returned for the Minkowski difference - int nsimplex; // size of simplex 1 & 2 + int gjk_iterations; // number of iterations that GJK ran + int epa_iterations; // number of iterations that EPA ran (negative if EPA did not run) + mjtNum simplex1[12]; // the simplex that GJK returned for obj1 + mjtNum simplex2[12]; // the simplex that GJK returned for obj2 + mjtNum simplex[12]; // the simplex that GJK returned for the Minkowski difference + int nsimplex; // size of simplex 1 & 2 }; typedef struct _mjCCDStatus mjCCDStatus; diff --git a/src/engine/engine_support.c b/src/engine/engine_support.c index 5b90a705..630431fd 100644 --- a/src/engine/engine_support.c +++ b/src/engine/engine_support.c @@ -1409,15 +1409,15 @@ void mj_objectAcceleration(const mjModel* m, const mjData* d, // returns the smallest distance between two geoms (using nativeccd) static mjtNum mj_geomDistanceCCD(const mjModel* m, const mjData* d, int g1, int g2, - mjtNum fromto[6]) { + mjtNum distmax, mjtNum fromto[6]) { mjCCDConfig config; mjCCDStatus status; // set config config.max_iterations = m->opt.ccd_iterations; config.tolerance = m->opt.ccd_tolerance; - config.contacts = 1; // want contacts - config.distances = 1; // want geom distances + config.max_contacts = 1; // want contacts + config.dist_cutoff = distmax; // want geom distances mjCCDObj obj1, obj2; mjc_initCCDObj(&obj1, m, d, g1, 0); @@ -1425,7 +1425,7 @@ static mjtNum mj_geomDistanceCCD(const mjModel* m, const mjData* d, int g1, int mjtNum dist = mjc_ccd(&config, &status, &obj1, &obj2); - if (fromto) { + if (fromto && status.nx > 0) { mju_copy3(fromto, status.x1); mju_copy3(fromto+3, status.x2); } @@ -1459,7 +1459,7 @@ mjtNum mj_geomDistance(const mjModel* m, const mjData* d, int geom1, int geom2, // use nativecdd if flag is enabled if (mjENABLED(mjENBL_NATIVECCD)) { if (func == mjc_Convex || func == mjc_BoxBox) { - return mj_geomDistanceCCD(m, d, g1, g2, fromto); + return mj_geomDistanceCCD(m, d, g1, g2, distmax, fromto); } } diff --git a/test/engine/engine_collision_gjk_test.cc b/test/engine/engine_collision_gjk_test.cc index a8ff146f..73137be1 100644 --- a/test/engine/engine_collision_gjk_test.cc +++ b/test/engine/engine_collision_gjk_test.cc @@ -56,23 +56,25 @@ constexpr char kEllipoid[] = R"( )"; mjtNum GeomDist(mjModel* m, mjData* d, int g1, int g2, mjtNum x1[3], - mjtNum x2[3]) { + mjtNum x2[3], mjtNum cutoff = mjMAXVAL) { mjCCDConfig config; mjCCDStatus status; // set config config.max_iterations = kMaxIterations, config.tolerance = kTolerance, - config.contacts = 0; // no geom contacts needed - config.distances = 1; + config.max_contacts = 0; // no geom contacts needed + config.dist_cutoff = cutoff; mjCCDObj obj1, obj2; mjc_initCCDObj(&obj1, m, d, g1, 0); mjc_initCCDObj(&obj2, m, d, g2, 0); mjtNum dist = mjc_ccd(&config, &status, &obj1, &obj2); - if (x1 != nullptr) mju_copy3(x1, status.x1); - if (x2 != nullptr) mju_copy3(x2, status.x2); + if (status.nx > 0) { + if (x1 != nullptr) mju_copy3(x1, status.x1); + if (x2 != nullptr) mju_copy3(x2, status.x2); + } return dist; } @@ -85,8 +87,8 @@ int PenetrationWrapper(mjCCDObj* obj1, mjCCDObj* obj2, const ccd_t* ccd, // set config config.max_iterations = ccd->max_iterations, config.tolerance = ccd->mpr_tolerance, - config.contacts = 1; - config.distances = 0; // no geom distances needed + config.max_contacts = 1; + config.dist_cutoff = 0; // no geom distances needed mjtNum dist = mjc_ccd(&config, &status, obj1, obj2); if (dist < 0) { @@ -141,16 +143,10 @@ using MjGjkTest = MujocoTest; TEST_F(MjGjkTest, SphereSphereDist) { static constexpr char xml[] = R"( - - - - - - - - - - + + + + )"; std::array error; @@ -172,19 +168,38 @@ TEST_F(MjGjkTest, SphereSphereDist) { mj_deleteModel(model); } +TEST_F(MjGjkTest, SphereSphereDistCutoff) { + static constexpr char xml[] = R"( + + + + + + )"; + + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data(); + + mjData* data = mj_makeData(model); + mj_forward(model, data); + + int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1"); + int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2"); + mjtNum dist = GeomDist(model, data, geom1, geom2, nullptr, nullptr, .999999); + + EXPECT_EQ(dist, mjMAXVAL); + mj_deleteData(data); + mj_deleteModel(model); +} + TEST_F(MjGjkTest, SphereSphereNoDist) { static constexpr char xml[] = R"( - - - - - - - - - - + + + + )"; std::array error;