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;
src/engine/engine_util_blas.c
+39 -30
View File
@@ -38,6 +38,15 @@ void mju_zero3(mjtNum res[3]) {
// vec1 == vec2
int mju_equal3(const mjtNum vec1[3], const mjtNum vec2[3]) {
return mju_abs(vec1[0] - vec2[0]) < mjMINVAL &&
mju_abs(vec1[1] - vec2[1]) < mjMINVAL &&
mju_abs(vec1[2] - vec2[2]) < mjMINVAL;
}
// res = vec
void mju_copy3(mjtNum res[3], const mjtNum data[3]) {
res[0] = data[0];
@@ -195,46 +204,46 @@ void mju_rotVecMatT(mjtNum res[3], const mjtNum vec[3], const mjtNum mat[9]) {
// multiply 3x3 matrices,
void mju_mulMatMat3(mjtNum res[9], const mjtNum a[9], const mjtNum b[9]) {
res[0] = a[0]*b[0] + a[1]*b[3] + a[2]*b[6];
res[1] = a[0]*b[1] + a[1]*b[4] + a[2]*b[7];
res[2] = a[0]*b[2] + a[1]*b[5] + a[2]*b[8];
res[3] = a[3]*b[0] + a[4]*b[3] + a[5]*b[6];
res[4] = a[3]*b[1] + a[4]*b[4] + a[5]*b[7];
res[5] = a[3]*b[2] + a[4]*b[5] + a[5]*b[8];
res[6] = a[6]*b[0] + a[7]*b[3] + a[8]*b[6];
res[7] = a[6]*b[1] + a[7]*b[4] + a[8]*b[7];
res[8] = a[6]*b[2] + a[7]*b[5] + a[8]*b[8];
void mju_mulMatMat3(mjtNum res[9], const mjtNum mat1[9], const mjtNum mat2[9]) {
res[0] = mat1[0]*mat2[0] + mat1[1]*mat2[3] + mat1[2]*mat2[6];
res[1] = mat1[0]*mat2[1] + mat1[1]*mat2[4] + mat1[2]*mat2[7];
res[2] = mat1[0]*mat2[2] + mat1[1]*mat2[5] + mat1[2]*mat2[8];
res[3] = mat1[3]*mat2[0] + mat1[4]*mat2[3] + mat1[5]*mat2[6];
res[4] = mat1[3]*mat2[1] + mat1[4]*mat2[4] + mat1[5]*mat2[7];
res[5] = mat1[3]*mat2[2] + mat1[4]*mat2[5] + mat1[5]*mat2[8];
res[6] = mat1[6]*mat2[0] + mat1[7]*mat2[3] + mat1[8]*mat2[6];
res[7] = mat1[6]*mat2[1] + mat1[7]*mat2[4] + mat1[8]*mat2[7];
res[8] = mat1[6]*mat2[2] + mat1[7]*mat2[5] + mat1[8]*mat2[8];
}
// multiply 3x3 matrices, first argument transposed
void mju_mulMatTMat3(mjtNum res[9], const mjtNum a[9], const mjtNum b[9]) {
res[0] = a[0]*b[0] + a[3]*b[3] + a[6]*b[6];
res[1] = a[0]*b[1] + a[3]*b[4] + a[6]*b[7];
res[2] = a[0]*b[2] + a[3]*b[5] + a[6]*b[8];
res[3] = a[1]*b[0] + a[4]*b[3] + a[7]*b[6];
res[4] = a[1]*b[1] + a[4]*b[4] + a[7]*b[7];
res[5] = a[1]*b[2] + a[4]*b[5] + a[7]*b[8];
res[6] = a[2]*b[0] + a[5]*b[3] + a[8]*b[6];
res[7] = a[2]*b[1] + a[5]*b[4] + a[8]*b[7];
res[8] = a[2]*b[2] + a[5]*b[5] + a[8]*b[8];
void mju_mulMatTMat3(mjtNum res[9], const mjtNum mat1[9], const mjtNum mat2[9]) {
res[0] = mat1[0]*mat2[0] + mat1[3]*mat2[3] + mat1[6]*mat2[6];
res[1] = mat1[0]*mat2[1] + mat1[3]*mat2[4] + mat1[6]*mat2[7];
res[2] = mat1[0]*mat2[2] + mat1[3]*mat2[5] + mat1[6]*mat2[8];
res[3] = mat1[1]*mat2[0] + mat1[4]*mat2[3] + mat1[7]*mat2[6];
res[4] = mat1[1]*mat2[1] + mat1[4]*mat2[4] + mat1[7]*mat2[7];
res[5] = mat1[1]*mat2[2] + mat1[4]*mat2[5] + mat1[7]*mat2[8];
res[6] = mat1[2]*mat2[0] + mat1[5]*mat2[3] + mat1[8]*mat2[6];
res[7] = mat1[2]*mat2[1] + mat1[5]*mat2[4] + mat1[8]*mat2[7];
res[8] = mat1[2]*mat2[2] + mat1[5]*mat2[5] + mat1[8]*mat2[8];
}
// multiply 3x3 matrices, second argument transposed
void mju_mulMatMatT3(mjtNum res[9], const mjtNum a[9], const mjtNum b[9]) {
res[0] = a[0]*b[0] + a[1]*b[1] + a[2]*b[2];
res[1] = a[0]*b[3] + a[1]*b[4] + a[2]*b[5];
res[2] = a[0]*b[6] + a[1]*b[7] + a[2]*b[8];
res[3] = a[3]*b[0] + a[4]*b[1] + a[5]*b[2];
res[4] = a[3]*b[3] + a[4]*b[4] + a[5]*b[5];
res[5] = a[3]*b[6] + a[4]*b[7] + a[5]*b[8];
res[6] = a[6]*b[0] + a[7]*b[1] + a[8]*b[2];
res[7] = a[6]*b[3] + a[7]*b[4] + a[8]*b[5];
res[8] = a[6]*b[6] + a[7]*b[7] + a[8]*b[8];
void mju_mulMatMatT3(mjtNum res[9], const mjtNum mat1[9], const mjtNum mat2[9]) {
res[0] = mat1[0]*mat2[0] + mat1[1]*mat2[1] + mat1[2]*mat2[2];
res[1] = mat1[0]*mat2[3] + mat1[1]*mat2[4] + mat1[2]*mat2[5];
res[2] = mat1[0]*mat2[6] + mat1[1]*mat2[7] + mat1[2]*mat2[8];
res[3] = mat1[3]*mat2[0] + mat1[4]*mat2[1] + mat1[5]*mat2[2];
res[4] = mat1[3]*mat2[3] + mat1[4]*mat2[4] + mat1[5]*mat2[5];
res[5] = mat1[3]*mat2[6] + mat1[4]*mat2[7] + mat1[5]*mat2[8];
res[6] = mat1[6]*mat2[0] + mat1[7]*mat2[1] + mat1[8]*mat2[2];
res[7] = mat1[6]*mat2[3] + mat1[7]*mat2[4] + mat1[8]*mat2[5];
res[8] = mat1[6]*mat2[6] + mat1[7]*mat2[7] + mat1[8]*mat2[8];
}
src/engine/engine_util_blas.h
+4 -1
View File
@@ -67,6 +67,9 @@ extern "C" {
// res = 0
MJAPI void mju_zero3(mjtNum res[3]);
// vec1 == vec2
MJAPI int mju_equal3(const mjtNum vec1[3], const mjtNum vec2[3]);
// res = vec
MJAPI void mju_copy3(mjtNum res[3], const mjtNum data[3]);
@@ -119,7 +122,7 @@ MJAPI void mju_rotVecMatT(mjtNum res[3], const mjtNum vec[3], const mjtNum mat[9
MJAPI void mju_mulMatMat3(mjtNum res[9], const mjtNum mat1[9], const mjtNum mat2[9]);
// multiply 3x3 matrices, first argument transposed
MJAPI void mju_mulMatTMat3(mjtNum res[9], const mjtNum a[9], const mjtNum b[9]);
MJAPI void mju_mulMatTMat3(mjtNum res[9], const mjtNum mat1[9], const mjtNum mat2[9]);
// multiply 3x3 matrices, second argument transposed
MJAPI void mju_mulMatMatT3(mjtNum res[9], const mjtNum mat1[9], const mjtNum mat2[9]);