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

Remove square roots when possible in EPA improving numerical precision.

Browse Source 
PiperOrigin-RevId: 745559836
Change-Id: I3f8b23406e93ccfc4bc3499bfbed829b35462a74
This commit is contained in:
Kyle Bayes
2025-04-09 06:17:58 -07:00
committed by Copybara-Service
parent 8fc616bf8f
commit 85b323c7fb
2 changed files with 153 additions and 59 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_collision_gjk.c
+89 -59
View File
@@ -24,6 +24,9 @@
#include "engine/engine_util_blas.h"
#include "engine/engine_util_errmem.h"
#define mjMINVAL2 (mjMINVAL * mjMINVAL)
#define mjMAXVAL2 (mjMAXVAL * mjMAXVAL)
// subdistance algorithm for GJK that computes the barycentric coordinates of the point in a
// simplex closest to the origin
// implementation adapted from Montanari et al, ToG 2017
@@ -50,7 +53,7 @@ typedef struct {
int verts[3]; // indices of the three vertices of the face in the polytope
int adj[3]; // adjacent faces, one for each edge: [v1,v2], [v2,v3], [v3,v1]
mjtNum v[3]; // projection of the origin on face, can be used as face normal
mjtNum dist; // norm of v; negative if deleted
mjtNum dist2; // squared norm of v; negative if deleted
int index; // index in map; -1: not in map, -2: deleted from polytope
} Face;
@@ -78,7 +81,7 @@ static int epaSupport(Polytope* pt, mjCCDObj* obj1, mjCCDObj* obj2,
// make copy of vertex in polytope and return its index
static int insertVertex(Polytope* pt, const Vertex* v);
// attach a face to the polytope with the given vertex indices; return distance to origin
// attach a face to the polytope with the given vertex indices; return squared distance to origin
static mjtNum attachFace(Polytope* pt, int v1, int v2, int v3, int adj1, int adj2, int adj3);
// return 1 if objects are in contact; 0 if not; -1 if inconclusive
@@ -318,7 +321,7 @@ static void gjkSupport(Vertex* v, mjCCDObj* obj1, mjCCDObj* obj2,
// mjc_support requires a normalized direction
mjtNum norm = dot3(x_k, x_k);
if (norm > mjMINVAL*mjMINVAL) {
if (norm > mjMINVAL2) {
norm = 1/mju_sqrt(norm);
scl3(dir_neg, x_k, norm);
scl3(dir, dir_neg, -1);
@@ -392,10 +395,9 @@ static inline mjtNum signedDistance(mjtNum normal[3], const Vertex* v1, const Ve
sub3(diff1, v3->vert, v1->vert);
sub3(diff2, v2->vert, v1->vert);
cross3(normal, diff1, diff2);
mjtNum norm = dot3(normal, normal);
if (norm > mjMINVAL*mjMINVAL && norm < mjMAXVAL*mjMAXVAL) {
norm = 1/mju_sqrt(norm);
scl3(normal, normal, norm);
mjtNum norm2 = dot3(normal, normal);
if (norm2 > mjMINVAL2 && norm2 < mjMAXVAL2) {
scl3(normal, normal, 1 / mju_sqrt(norm2));
return dot3(normal, v1->vert);
}
return mjMAX_LIMIT; // cannot recover normal (ignore face)
@@ -964,27 +966,27 @@ static int polytope2(Polytope* pt, mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj
mjtNum* v5 = pt->verts[v5i].vert;
// build hexahedron
if (attachFace(pt, v1i, v3i, v4i, 1, 3, 2) < mjMINVAL) {
if (attachFace(pt, v1i, v3i, v4i, 1, 3, 2) < mjMINVAL2) {
replaceSimplex3(pt, status, v1i, v3i, v4i);
return polytope3(pt, status, obj1, obj2);
}
if (attachFace(pt, v1i, v5i, v3i, 2, 4, 0) < mjMINVAL) {
if (attachFace(pt, v1i, v5i, v3i, 2, 4, 0) < mjMINVAL2) {
replaceSimplex3(pt, status, v1i, v5i, v3i);
return polytope3(pt, status, obj1, obj2);
}
if (attachFace(pt, v1i, v4i, v5i, 0, 5, 1) < mjMINVAL) {
if (attachFace(pt, v1i, v4i, v5i, 0, 5, 1) < mjMINVAL2) {
replaceSimplex3(pt, status, v1i, v4i, v5i);
return polytope3(pt, status, obj1, obj2);
}
if (attachFace(pt, v2i, v4i, v3i, 5, 0, 4) < mjMINVAL) {
if (attachFace(pt, v2i, v4i, v3i, 5, 0, 4) < mjMINVAL2) {
replaceSimplex3(pt, status, v2i, v4i, v3i);
return polytope3(pt, status, obj1, obj2);
}
if (attachFace(pt, v2i, v3i, v5i, 3, 1, 5) < mjMINVAL) {
if (attachFace(pt, v2i, v3i, v5i, 3, 1, 5) < mjMINVAL2) {
replaceSimplex3(pt, status, v2i, v3i, v5i);
return polytope3(pt, status, obj1, obj2);
}
if (attachFace(pt, v2i, v5i, v4i, 4, 2, 3) < mjMINVAL) {
if (attachFace(pt, v2i, v5i, v4i, 4, 2, 3) < mjMINVAL2) {
replaceSimplex3(pt, status, v2i, v5i, v4i);
return polytope3(pt, status, obj1, obj2);
}
@@ -1120,22 +1122,22 @@ static int polytope3(Polytope* pt, mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj
}
// create hexahedron for EPA
if (attachFace(pt, v4i, v1i, v2i, 1, 3, 2) < mjMINVAL) {
if (attachFace(pt, v4i, v1i, v2i, 1, 3, 2) < mjMINVAL2) {
return mjEPA_P3_ORIGIN_ON_FACE;
}
if (attachFace(pt, v4i, v3i, v1i, 2, 4, 0) < mjMINVAL) {
if (attachFace(pt, v4i, v3i, v1i, 2, 4, 0) < mjMINVAL2) {
return mjEPA_P3_ORIGIN_ON_FACE;
}
if (attachFace(pt, v4i, v2i, v3i, 0, 5, 1) < mjMINVAL) {
if (attachFace(pt, v4i, v2i, v3i, 0, 5, 1) < mjMINVAL2) {
return mjEPA_P3_ORIGIN_ON_FACE;
}
if (attachFace(pt, v5i, v2i, v1i, 5, 0, 4) < mjMINVAL) {
if (attachFace(pt, v5i, v2i, v1i, 5, 0, 4) < mjMINVAL2) {
return mjEPA_P3_ORIGIN_ON_FACE;
}
if (attachFace(pt, v5i, v1i, v3i, 3, 1, 5) < mjMINVAL) {
if (attachFace(pt, v5i, v1i, v3i, 3, 1, 5) < mjMINVAL2) {
return mjEPA_P3_ORIGIN_ON_FACE;
}
if (attachFace(pt, v5i, v3i, v2i, 4, 2, 3) < mjMINVAL) {
if (attachFace(pt, v5i, v3i, v2i, 4, 2, 3) < mjMINVAL2) {
return mjEPA_P3_ORIGIN_ON_FACE;
}
@@ -1160,19 +1162,19 @@ static int polytope4(Polytope* pt, mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj
int v4 = insertVertex(pt, status->simplex + 3);
// if the origin is on a face, replace the 3-simplex with a 2-simplex
if (attachFace(pt, v1, v2, v3, 1, 3, 2) < mjMINVAL) {
if (attachFace(pt, v1, v2, v3, 1, 3, 2) < mjMINVAL2) {
replaceSimplex3(pt, status, v1, v2, v3);
return polytope3(pt, status, obj1, obj2);
}
if (attachFace(pt, v1, v4, v2, 2, 3, 0) < mjMINVAL) {
if (attachFace(pt, v1, v4, v2, 2, 3, 0) < mjMINVAL2) {
replaceSimplex3(pt, status, v1, v4, v2);
return polytope3(pt, status, obj1, obj2);
}
if (attachFace(pt, v1, v3, v4, 0, 3, 1) < mjMINVAL) {
if (attachFace(pt, v1, v3, v4, 0, 3, 1) < mjMINVAL2) {
replaceSimplex3(pt, status, v1, v3, v4);
return polytope3(pt, status, obj1, obj2);
}
if (attachFace(pt, v4, v3, v2, 2, 0, 1) < mjMINVAL) {
if (attachFace(pt, v4, v3, v2, 2, 0, 1) < mjMINVAL2) {
replaceSimplex3(pt, status, v4, v3, v2);
return polytope3(pt, status, obj1, obj2);
}
@@ -1222,7 +1224,7 @@ static inline int maxFaces(Polytope* pt) {
// attach a face to the polytope with the given vertex indices; return distance to origin
// attach a face to the polytope with the given vertex indices; return squared distance to origin
static inline mjtNum attachFace(Polytope* pt, int v1, int v2, int v3,
int adj1, int adj2, int adj3) {
Face* face = &pt->faces[pt->nfaces++];
@@ -1240,10 +1242,10 @@ static inline mjtNum attachFace(Polytope* pt, int v1, int v2, int v3,
if (ret) {
return 0;
}
face->dist = norm3(face->v);
face->dist2 = dot3(face->v, face->v);
face->index = -1;
return face->dist;
return face->dist2;
}
@@ -1267,10 +1269,8 @@ static inline int getEdge(Face* face, int vertex) {
// recursive call to build horizon; return 1 if face is visible from w otherwise 0
static int horizonRec(Polytope* pt, Face* face, int e) {
mjtNum dist2 = face->dist * face->dist;
// v is visible from w so it is deleted and adjacent faces are checked
if (dot3(face->v, pt->horizon.w) > dist2) {
if (dot3(face->v, pt->horizon.w) - face->dist2 > mjMINVAL) {
deleteFace(pt, face);
// recursively search the adjacent faces on the next two edges
@@ -1350,7 +1350,7 @@ static void epaWitness(const Polytope* pt, const Face* face, mjtNum x1[3], mjtNu
// return a face of the expanded polytope that best approximates the pentration depth
// witness points are in status->{x1, x2}
static Face* epa(mjCCDStatus* status, Polytope* pt, mjCCDObj* obj1, mjCCDObj* obj2) {
mjtNum tolerance = status->tolerance, lower, upper = mjMAX_LIMIT;
mjtNum tolerance = status->tolerance, lower2, upper = mjMAX_LIMIT, upper2 = mjMAX_LIMIT;
int k, kmax = status->max_iterations;
Face* face = NULL, *pface = NULL; // face closest to origin
@@ -1358,31 +1358,35 @@ static Face* epa(mjCCDStatus* status, Polytope* pt, mjCCDObj* obj1, mjCCDObj* ob
pface = face;
// find the face closest to the origin (lower bound for penetration depth)
lower = mjMAX_LIMIT;
lower2 = mjMAX_LIMIT;
for (int i = 0; i < pt->nmap; i++) {
if (pt->map[i]->dist < lower) {
if (pt->map[i]->dist2 < lower2) {
face = pt->map[i];
lower = face->dist;
lower2 = face->dist2;
}
}
// face not valid, return previous face
if (lower > upper || !face) {
if (lower2 > upper2 || !face) {
face = pface;
break;
}
// check if lower bound is 0
if (lower <= 0) {
if (lower2 <= 0) {
mju_warning("EPA: origin lies on affine hull of face");
break;
}
// compute support point w from the closest face's normal
mjtNum lower = mju_sqrt(lower2);
int wi = epaSupport(pt, obj1, obj2, face->v, lower);
mjtNum* w = pt->verts[wi].vert;
mjtNum upper_k = dot3(face->v, w) / lower; // upper bound for kth iteration
if (upper_k < upper) upper = upper_k;
if (upper_k < upper) {
upper = upper_k;
upper2 = upper * upper;
}
if (upper - lower < tolerance) {
break;
}
@@ -1411,16 +1415,16 @@ static Face* epa(mjCCDStatus* status, Polytope* pt, mjCCDObj* obj1, mjCCDObj* ob
int v1 = horFace->verts[horEdge],
v2 = horFace->verts[(horEdge + 1) % 3];
horFace->adj[horEdge] = nfaces;
mjtNum dist = attachFace(pt, wi, v2, v1, nfaces + nedges - 1, horIndex, nfaces + 1);
mjtNum dist2 = attachFace(pt, wi, v2, v1, nfaces + nedges - 1, horIndex, nfaces + 1);
// unrecoverable numerical issue
if (dist == 0) {
if (dist2 == 0) {
face = NULL;
break;
}
// store face in map
if (dist >= lower && dist <= upper) {
if (dist2 >= lower2 && dist2 <= upper2) {
int i = pt->nmap++;
pt->map[i] = &pt->faces[pt->nfaces - 1];
pt->map[i]->index = i;
@@ -1436,16 +1440,16 @@ static Face* epa(mjCCDStatus* status, Polytope* pt, mjCCDObj* obj1, mjCCDObj* ob
v1 = horFace->verts[horEdge];
v2 = horFace->verts[(horEdge + 1) % 3];
horFace->adj[horEdge] = cur;
dist = attachFace(pt, wi, v2, v1, cur - 1, horIndex, next);
dist2 = attachFace(pt, wi, v2, v1, cur - 1, horIndex, next);
// unrecoverable numerical issue
if (dist == 0) {
if (dist2 == 0) {
face = NULL;
break;
}
// store face in map
if (dist >= lower && dist <= upper) {
if (dist2 >= lower2 && dist2 <= upper2) {
int idx = pt->nmap++;
pt->map[idx] = &pt->faces[pt->nfaces - 1];
pt->map[idx]->index = idx;
@@ -1463,7 +1467,7 @@ static Face* epa(mjCCDStatus* status, Polytope* pt, mjCCDObj* obj1, mjCCDObj* ob
if (face) {
epaWitness(pt, face, status->x1, status->x2);
status->nx = 1;
status->dist = -face->dist;
status->dist = -mju_sqrt(face->dist2);
} else {
status->nx = 0;
status->dist = 0;
@@ -1565,7 +1569,7 @@ static mjtNum planeNormal(mjtNum res[3], const mjtNum v1[3], const mjtNum v2[3],
// find what side of a plane a point p lies
static int halfspace(const mjtNum a[3], const mjtNum n[3], const mjtNum p[3]) {
mjtNum diff[3] = {p[0] - a[0], p[1] - a[1], p[2] - a[2]};
return dot3(diff, n) >= 0.0;
return dot3(diff, n) > -mjMINVAL;
}
@@ -1855,13 +1859,37 @@ static int meshEdgeNormals(mjtNum* res, mjtNum* endverts, int dim, mjCCDObj* obj
// try recovering box normal from collision normal
static int boxNormals2(mjtNum res[9], int resind[3], const mjtNum mat[9], const mjtNum n[3]) {
// list of box face normals
mjtNum normals[18] = {1, 0, 0, -1, 0, 0,
0, 1, 0, 0, -1, 0,
0, 0, 1, 0, 0, -1};
// get local coordinates of the normal
mjtNum local_n[3];
local_n[0] = mat[0]*n[0] + mat[3]*n[1] + mat[6]*n[2];
local_n[1] = mat[1]*n[0] + mat[4]*n[1] + mat[7]*n[2];
local_n[2] = mat[2]*n[0] + mat[5]*n[1] + mat[8]*n[2];
scl3(local_n, local_n, 1/mju_sqrt(dot3(local_n, local_n)));
// determine if there is a side close to the normal
for (int i = 0; i < 6; i++) {
if (dot3(local_n, normals + 3*i) > mjFACE_TOL) {
globalcoord(res, mat, NULL, normals[3*i], normals[3*i + 1], normals[3*i + 2]);
resind[0] = i;
return 1;
}
}
return 0;
}
// compute possible face normals of a box given up to 3 vertices
static int boxNormals(mjtNum res[9], int resind[3], int dim, mjCCDObj* obj,
int v1, int v2, int v3) {
// box data
int g = 3*obj->geom;
const mjtNum* mat = obj->data->geom_xmat + 3*g;
int v1, int v2, int v3, const mjtNum dir[3]) {
const mjtNum* mat = obj->data->geom_xmat + 9*obj->geom;
if (dim == 3) {
int c = 0;
int x = ((v1 & 1) && (v2 & 1) && (v3 & 1)) - (!(v1 & 1) && !(v2 & 1) && !(v3 & 1));
@@ -1873,7 +1901,7 @@ static int boxNormals(mjtNum res[9], int resind[3], int dim, mjCCDObj* obj,
if (y) resind[c++] = 2;
if (z) resind[c++] = 4;
if (sgn == -1) resind[0]++;
return c == 1 ? 1 : 0; // return 1 only if vertices make a valid face
return c == 1 ? 1 : boxNormals2(res, resind, mat, dir);
}
if (dim == 2) {
@@ -1893,8 +1921,7 @@ static int boxNormals(mjtNum res[9], int resind[3], int dim, mjCCDObj* obj,
globalcoord(res + 3, mat, NULL, 0, 0, z);
resind[c++] = (z > 0) ? 4 : 5;
}
// TODO(kylebayes): Should be able to recover multiple contacts here.
return c == 2 ? 2 : 0;
return c == 2 ? 2 : boxNormals2(res, resind, mat, dir);
}
if (dim == 1) {
@@ -2107,14 +2134,18 @@ static void multicontact(Polytope* pt, Face* face, mjCCDStatus* status,
mjtNum n1[3 * mjMAX_POLYVERT], n2[3 * mjMAX_POLYVERT]; // normals of possible face collisions
int idx1[mjMAX_POLYVERT], idx2[mjMAX_POLYVERT]; // indices of faces
mjtNum dir[3], dir_neg[3];
sub3(dir, status->x2, status->x1);
sub3(dir_neg, status->x1, status->x2);
// get all possible face normals for each geom
if (obj1->geom_type == mjGEOM_BOX) {
nnorms1 = boxNormals(n1, idx1, nface1, obj1, v11i, v12i, v13i);
nnorms1 = boxNormals(n1, idx1, nface1, obj1, v11i, v12i, v13i, dir_neg);
} else if (obj1->geom_type == mjGEOM_MESH) {
nnorms1 = meshNormals(n1, idx1, nface1, obj1, v11i, v12i, v13i);
}
if (obj2->geom_type == mjGEOM_BOX) {
nnorms2 = boxNormals(n2, idx2, nface2, obj2, v21i, v22i, v23i);
nnorms2 = boxNormals(n2, idx2, nface2, obj2, v21i, v22i, v23i, dir);
} else if (obj2->geom_type == mjGEOM_MESH) {
nnorms2 = meshNormals(n2, idx2, nface2, obj2, v21i, v22i, v23i);
}
@@ -2181,25 +2212,24 @@ static void multicontact(Polytope* pt, Face* face, mjCCDStatus* status,
// TODO(kylebayes): this approximates the contact direction, by scaling the face normal by the
// single contact direction's magnitude. This is effective, but polygonClip should compute
// this for each contact point.
mjtNum diff[3], approx_dir[3];
sub3(diff, status->x2, status->x1);
mjtNum approx_dir[3];
// face1 is an edge; clip face1 against face2
if (edgecon1) {
scl3(approx_dir, n2 + 3*j, norm3(diff));
scl3(approx_dir, n2 + 3*j, norm3(dir));
polygonClip(status, face2, nface2, face1, nface1, n2 + 3*j, approx_dir);
return;
}
// face2 is an edge; clip face2 against face1
if (edgecon2) {
scl3(approx_dir, n1 + 3*j, -norm3(diff));
scl3(approx_dir, n1 + 3*j, -norm3(dir));
polygonClip(status, face1, nface1, face2, nface2, n1 + 3*j, approx_dir);
return;
}
// face-face collision
scl3(approx_dir, n2 + 3*j, norm3(diff));
scl3(approx_dir, n2 + 3*j, norm3(dir));
polygonClip(status, face1, nface1, face2, nface2, n1 + 3*i, approx_dir);
}
test/engine/engine_collision_gjk_test.cc
+64
View File
@@ -1088,6 +1088,70 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD11) {
mj_deleteModel(model);
}
TEST_F(MjGjkTest, BoxBoxMultiCCD12) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<geom name="geom1" type="box" pos="0 0 0" size="0.025 0.025 0.025"/>
<geom name="geom2" type="box" pos="0 0 0" size="0.025 0.025 0.025"/>
</worldbody>
</mujoco>)";
std::array<char, 1000> 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);
mjtNum* xpos = data->geom_xpos;
mjtNum* xmat = data->geom_xmat;
xmat[0] = 1.0000000000000000000000000000000000000000;
xmat[1] = 0.0000000000000000000000000000000000000000;
xmat[2] = 0.0000000000000000000000000000000000000000;
xmat[3] = 0.0000000000000000000000000000000000000000;
xmat[4] = 1.0000000000000000000000000000000000000000;
xmat[5] = -0.0000000000000000032154383478277941584027;
xmat[6] = 0.0000000000000000000000000000000000000000;
xmat[7] = 0.0000000000000000032154383478277941584027;
xmat[8] = 1.0000000000000000000000000000000000000000;
xpos[0] = 0.0164299999999999862820843077315657865256;
xpos[1] = -0.0764300000000000256950016819246229715645;
xpos[2] = 0.1252706891962387103500731200256268493831;
xpos = data->geom_xpos + 3;
xmat = data->geom_xmat + 9;
xmat[0] = 1.0000000000000000000000000000000000000000;
xmat[1] = 0.0000000000000000000000000000000000000000;
xmat[2] = 0.0000000000000000000000000000000000000000;
xmat[3] = 0.0000000000000000000000000000000000000000;
xmat[4] = 1.0000000000000000000000000000000000000000;
xmat[5] = -0.0000000000000000018997602302052549055743;
xmat[6] = 0.0000000000000000000000000000000000000000;
xmat[7] = 0.0000000000000000018997602302052549055743;
xmat[8] = 1.0000000000000000000000000000000000000000;
xpos[0] = 0.0164299999999999862820843077315657865256;
xpos[1] = -0.0764300000000000256950016819246229715645;
xpos[2] = 0.1748374248948718623353215662064030766487;
int g1 = mj_name2id(model, mjOBJ_GEOM, "geom1");
int g2 = mj_name2id(model, mjOBJ_GEOM, "geom2");
mjCCDStatus status;
std::vector<mjtNum> dir, pos;
mjtNum dist;
int ncons = Penetration(status, dist, dir, pos, model, data, g1, g2, 0, 8);
EXPECT_EQ(ncons, 4);
mj_deleteData(data);
mj_deleteModel(model);
}
TEST_F(MjGjkTest, SmallBoxMesh) {
static constexpr char xml[] = R"(
<mujoco>