chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Use GJK to recover contact points for sphere and capsule geoms in nativeccd.

Browse Source 
PiperOrigin-RevId: 704727907
Change-Id: Iff641b6a32637c48e9892dbaa60826617c54ed06
This commit is contained in:
Kyle Bayes
2024-12-10 08:52:26 -08:00
committed by Copybara-Service
parent a7fed25fdd
commit a364308d5a
5 changed files with 131 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
mjx/mujoco/mjx/_src/collision_driver_test.py
+4 -3
View File
@@ -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 = """
src/engine/engine_collision_convex.c
+36 -1
View File
@@ -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);
src/engine/engine_collision_convex.h
+6
View File
@@ -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);
src/engine/engine_collision_gjk.c
+83 -2
View File
@@ -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) {
test/engine/engine_collision_gjk_test.cc
+2 -2
View File
@@ -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);