diff --git a/mjx/mujoco/mjx/_src/collision_driver_test.py b/mjx/mujoco/mjx/_src/collision_driver_test.py index 068856d7..4b083223 100644 --- a/mjx/mujoco/mjx/_src/collision_driver_test.py +++ b/mjx/mujoco/mjx/_src/collision_driver_test.py @@ -56,6 +56,7 @@ def _collide( d = mujoco.MjData(m) dx = mjx.put_data(m, d) + m.opt.enableflags |= mujoco.mjtEnableBit.mjENBL_NATIVECCD mujoco.mj_step(m, d) collision_jit_fn = jax.jit(mjx.collision) kinematics_jit_fn = jax.jit(mjx.kinematics) @@ -241,7 +242,7 @@ class EllipsoidCollisionTest(parameterized.TestCase): self.assertLess(dx.contact.dist[0], 0) for field in dataclasses.fields(Contact): _assert_attr_eq( - dx.contact, d.contact, field.name, 'ellipsoid-ellipsoid', 1e-5 + dx.contact, d.contact, field.name, 'ellipsoid-ellipsoid', 1e-2 ) _ELLIPSOID_SPHERE = """ @@ -265,7 +266,7 @@ class EllipsoidCollisionTest(parameterized.TestCase): self.assertLess(dx.contact.dist[0], 0) for field in dataclasses.fields(Contact): _assert_attr_eq( - dx.contact, d.contact, field.name, 'ellipsoid-sphere', 1e-3 + dx.contact, d.contact, field.name, 'ellipsoid-sphere', 1e-4 ) _ELLIPSOID_CAPSULE = """ @@ -288,7 +289,7 @@ class EllipsoidCollisionTest(parameterized.TestCase): self.assertLess(dx.contact.dist[0], 0) for field in dataclasses.fields(Contact): _assert_attr_eq( - dx.contact, d.contact, field.name, 'ellipsoid-capsule', 1e-3 + dx.contact, d.contact, field.name, 'ellipsoid-capsule', 1e-5 ) _ELLIPSOID_CYLINDER = """ diff --git a/src/engine/engine_collision_convex.c b/src/engine/engine_collision_convex.c index 35211837..712841a8 100644 --- a/src/engine/engine_collision_convex.c +++ b/src/engine/engine_collision_convex.c @@ -142,6 +142,16 @@ static inline void localToGlobal(mjtNum res[3], const mjtNum mat[9], const mjtNu +// point support function +void mjc_pointSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) { + const mjtNum* pos = obj->data->geom_xpos + 3*obj->geom; + res[0] = pos[0]; + res[1] = pos[1]; + res[2] = pos[2]; +} + + + // sphere support function static void mjc_sphereSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) { const mjModel* m = obj->model; @@ -158,6 +168,31 @@ static void mjc_sphereSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) +// line support function (capsule) +void mjc_lineSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) { + const mjModel* m = obj->model; + const mjData* d = obj->data; + + // capsule data + int i = 3*obj->geom; + const mjtNum* mat = d->geom_xmat + 3*i; + const mjtNum* pos = d->geom_xpos + i; + mjtNum length = m->geom_size[i+1]; + + // rotate dir to geom local frame + mjtNum local_dir[3], tmp[3]; + mulMatTVec3(local_dir, mat, dir); + + tmp[0] = 0; + tmp[1] = 0; + tmp[2] = (local_dir[2] >= 0 ? length : -length); + + // transform result to global frame + localToGlobal(res, mat, tmp, pos); +} + + + // capsule support function static void mjc_capsuleSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) { const mjModel* m = obj->model; @@ -180,7 +215,7 @@ static void mjc_capsuleSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3] tmp[2] = local_dir[2] * radius; // add cylinder contribution - tmp[2] += mju_sign(local_dir[2]) * length; + tmp[2] += (local_dir[2] >= 0 ? length : -length); // transform result to global frame localToGlobal(res, mat, tmp, pos); diff --git a/src/engine/engine_collision_convex.h b/src/engine/engine_collision_convex.h index c402c9c6..407a856c 100644 --- a/src/engine/engine_collision_convex.h +++ b/src/engine/engine_collision_convex.h @@ -73,6 +73,12 @@ MJAPI void mjccd_center(const void *obj, ccd_vec3_t *center); // libccd support function MJAPI void mjccd_support(const void *obj, const ccd_vec3_t *dir, ccd_vec3_t *vec); +// support function for point +void mjc_pointSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]); + +// support function for line (capsule) +void mjc_lineSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]); + // pairwise geom collision functions using ccd int mjc_PlaneConvex (const mjModel* m, const mjData* d, mjContact* con, int g1, int g2, mjtNum margin); diff --git a/src/engine/engine_collision_gjk.c b/src/engine/engine_collision_gjk.c index 493a6ed5..1be326de 100644 --- a/src/engine/engine_collision_gjk.c +++ b/src/engine/engine_collision_gjk.c @@ -156,7 +156,7 @@ static mjtNum gjk(mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) { 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 lambda[4] = {1, 0, 0, 0}; // 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 @@ -179,6 +179,7 @@ static mjtNum gjk(mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) { mjtNum diff[3]; sub3(diff, x_k, s_k); if (2*dot3(x_k, diff) < epsilon) { + if (!k) n = 1; break; } @@ -1374,9 +1375,30 @@ static mjtNum epa(mjCCDStatus* status, Polytope* pt, mjCCDObj* obj1, mjCCDObj* o +// inflate a contact by margin +static inline void inflate(mjCCDStatus* status, mjtNum margin1, mjtNum margin2) { + mjtNum n[3]; + sub3(n, status->x2, status->x1); + mju_normalize3(n); + if (margin1) { + status->x1[0] += margin1 * n[0]; + status->x1[1] += margin1 * n[1]; + status->x1[2] += margin1 * n[2]; + } + if (margin2) { + status->x2[0] -= margin2 * n[0]; + status->x2[1] -= margin2 * n[1]; + status->x2[2] -= margin2 * n[2]; + } + status->dist -= (margin1 + margin2); +} + + + // general convex collision detection mjtNum mjc_ccd(const mjCCDConfig* config, mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) { // set up + mjtNum dist; obj1->center(status->x1, obj1); obj2->center(status->x2, obj2); status->gjk_iterations = 0; @@ -1386,7 +1408,66 @@ mjtNum mjc_ccd(const mjCCDConfig* config, mjCCDStatus* status, mjCCDObj* obj1, m status->max_contacts = config->max_contacts; status->dist_cutoff = config->dist_cutoff; - mjtNum dist = gjk(status, obj1, obj2); + // special handling for sphere and capsule (shrink to point and line respectively) + if (obj1->geom_type == mjGEOM_SPHERE || obj2->geom_type == mjGEOM_SPHERE || + obj1->geom_type == mjGEOM_CAPSULE || obj2->geom_type == mjGEOM_CAPSULE) { + void (*support1)(mjtNum*, struct _mjCCDObj*, const mjtNum*) = obj1->support; + void (*support2)(mjtNum*, struct _mjCCDObj*, const mjtNum*) = obj2->support; + mjtNum margin1 = 0, margin2 = 0; + + if (obj1->geom_type == mjGEOM_SPHERE) { + const mjModel* m = obj1->model; + margin1 = m->geom_size[3*obj1->geom]; + support1 = obj1->support; + obj1->support = mjc_pointSupport; + } else if (obj1->geom_type == mjGEOM_CAPSULE) { + const mjModel* m = obj1->model; + margin1 = m->geom_size[3*obj1->geom]; + support1 = obj1->support; + obj1->support = mjc_lineSupport; + } + + if (obj2->geom_type == mjGEOM_SPHERE) { + const mjModel* m = obj2->model; + margin2 = m->geom_size[3*obj2->geom]; + support2 = obj2->support; + obj2->support = mjc_pointSupport; + } else if (obj2->geom_type == mjGEOM_CAPSULE) { + const mjModel* m = obj2->model; + margin2 = m->geom_size[3*obj2->geom]; + support2 = obj2->support; + obj2->support = mjc_lineSupport; + } + + status->dist_cutoff += margin1 + margin2; + dist = gjk(status, obj1, obj2); + status->dist_cutoff = config->dist_cutoff; + + // shallow penetration, inflate contact + if (dist > 0) { + inflate(status, margin1, margin2); + if (status->dist > status->dist_cutoff) { + status->dist = mjMAXVAL; + } + return status->dist; + } + + // contact not needed + if (!config->max_contacts) { + status->nx = 0; + status->dist = 0; + return 0; + } + + // deep penetration, reset everything and run GJK again + status->gjk_iterations = 0; + obj1->support = support1; + obj2->support = support2; + obj1->center(status->x1, obj1); + obj2->center(status->x2, obj2); + } + + dist = gjk(status, obj1, obj2); // penetration recovery for contacts not needed if (!config->max_contacts) { diff --git a/test/engine/engine_collision_gjk_test.cc b/test/engine/engine_collision_gjk_test.cc index aea0e260..3093d82a 100644 --- a/test/engine/engine_collision_gjk_test.cc +++ b/test/engine/engine_collision_gjk_test.cc @@ -245,8 +245,8 @@ TEST_F(MjGjkTest, SphereSphereIntersect) { // direction EXPECT_NEAR(dir[0], 1, kTolerance); - EXPECT_NEAR(dir[1], 0, 0.001); - EXPECT_NEAR(dir[2], 0, 0.001); + EXPECT_NEAR(dir[1], 0, kTolerance); + EXPECT_NEAR(dir[2], 0, kTolerance); // position EXPECT_NEAR(pos[0], 1, kTolerance);