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

Set tolerance in GJK to zero for colliding discrete geoms.

Browse Source 
PiperOrigin-RevId: 672326091
Change-Id: Iddec335272461c9aee13c1960640a453ff59f841
This commit is contained in:
Kyle Bayes
2024-09-08 13:48:55 -07:00
committed by Copybara-Service
parent 56bb3ca5de
commit 61721f8d3c
3 changed files with 72 additions and 32 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_collision_gjk.c
+29 -1
View File
@@ -97,6 +97,22 @@ static mjtNum epa(mjCCDStatus* status, Polytope* pt, mjCCDObj* obj1, mjCCDObj* o
// returns true if both geoms are discrete shapes (i.e. meshes or boxes)
static int discreteGeoms(mjCCDObj* obj1, mjCCDObj* obj2) {
// non-zero margin makes geoms smooth
if (obj1->margin != 0 || obj2->margin != 0) return 0;
// negative geom indices correspond to flex objects, return
if (obj1->geom < 0 || obj2->geom < 0) return 0;
int g1 = obj1->model->geom_type[obj1->geom];
int g2 = obj2->model->geom_type[obj2->geom];
return (g1 == mjGEOM_MESH || g1 == mjGEOM_BOX) &&
(g2 == mjGEOM_MESH || g2 == mjGEOM_BOX);
}
// GJK algorithm
static mjtNum gjk(mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) {
mjtNum simplex[12]; // our current simplex with max 4 vertices due to only 3 dimensions
@@ -110,6 +126,11 @@ static mjtNum gjk(mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) {
mju_sub3(x_k, x1_k, x2_k);
mjtNum epsilon = status->tolerance * status->tolerance;
// if both geoms are discrete, finite convergence is guaranteed; set tolerance to 0
if (discreteGeoms(obj1, obj2)) {
epsilon = 0;
}
int k = 0, N = status->max_iterations;
for (; k < N; k++) {
mjtNum s1[3], s2[3]; // the support points in obj1 and obj2
@@ -146,8 +167,9 @@ static mjtNum gjk(mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) {
// run the distance subalgorithm to compute the barycentric coordinates
// of the closest point to the origin in the simplex
mjtNum tmp[3];
signedVolume(lambda, simplex, ++n);
lincomb(x_k, lambda, simplex, 4);
lincomb(tmp, lambda, simplex, 4);
// compute the approximate witness points
lincomb(x1_k, lambda, simplex1, 4);
@@ -165,6 +187,12 @@ static mjtNum gjk(mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) {
mju_copy3(simplex + 3*n++, simplex + 3*i);
}
// x_k has converged to minimum
if (mju_equal3(tmp, x_k)) {
break;
}
mju_copy3(x_k, tmp);
// we have a tetrahedron containing the origin so return early
if (n == 4) {
break;