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

Prune face linear map in EPA.

Browse Source 
PiperOrigin-RevId: 705138358
Change-Id: I835ce7094d066b19ee39af48e0b83b51f7a0954e
This commit is contained in:
Kyle Bayes
2024-12-11 09:43:27 -08:00
committed by Copybara-Service
parent 2691887500
commit ec230fa4b6
2 changed files with 168 additions and 77 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_collision_gjk.c
+119 -77
View File
@@ -14,6 +14,7 @@
#include "engine/engine_collision_gjk.h"
#include <float.h>
#include <stddef.h>
#include <stdlib.h>
#include <string.h>
@@ -51,10 +52,10 @@ static void lincomb(mjtNum res[3], const mjtNum* coef, const mjtNum* v, int n);
// one face in a polytope
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]; // the projection of the origin on the face (can be used as face normal)
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
int index; // index in map
int index; // index in map; -1: not in map, -2: deleted from polytope
} Face;
// polytope used in the Expanding Polytope Algorithm (EPA)
@@ -290,10 +291,15 @@ static inline void support(mjtNum s1[3], mjtNum s2[3], mjCCDObj* obj1, mjCCDObj*
// compute the support points in obj1 and obj2 for the kth approximation point
static void gjkSupport(mjtNum s1[3], mjtNum s2[3], mjCCDObj* obj1, mjCCDObj* obj2,
const mjtNum x_k[3]) {
mjtNum dir[3], dir_neg[3];
copy3(dir_neg, x_k);
mju_normalize3(dir_neg); // mjc_support assumes a normalized direction
scl3(dir, dir_neg, -1);
mjtNum dir[3] = {-1, 0, 0}, dir_neg[3] = {1, 0, 0};
// mjc_support requires a normalized direction
mjtNum norm = dot3(x_k, x_k);
if (norm > mjMINVAL*mjMINVAL) {
norm = 1/mju_sqrt(norm);
scl3(dir_neg, x_k, norm);
scl3(dir, dir_neg, -1);
}
// compute S_{A-B}(dir) = S_A(dir) - S_B(-dir)
support(s1, s2, obj1, obj2, dir, dir_neg);
@@ -304,20 +310,14 @@ static void gjkSupport(mjtNum s1[3], mjtNum s2[3], mjCCDObj* obj1, mjCCDObj* obj
// compute the support point in the Minkowski difference for EPA
static void epaSupport(mjtNum s1[3], mjtNum s2[3], mjCCDObj* obj1, mjCCDObj* obj2,
const mjtNum d[3], mjtNum dnorm) {
mjtNum dir[3], dir_neg[3];
mjtNum dir[3] = {1, 0, 0}, dir_neg[3] = {-1, 0, 0};
// mjc_support assumes a normalized direction
if (dnorm < mjMINVAL) {
dir[0] = 1, dir_neg[0] = -1;
dir[1] = 0, dir_neg[1] = 0;
dir[2] = 0, dir_neg[2] = 0;
} else {
if (dnorm > mjMINVAL) {
dir[0] = d[0] / dnorm;
dir[1] = d[1] / dnorm;
dir[2] = d[2] / dnorm;
dir_neg[0] = -dir[0];
dir_neg[1] = -dir[1];
dir_neg[2] = -dir[2];
scl3(dir_neg, dir, -1);
}
// compute S_{A-B}(dir) = S_A(dir) - S_B(-dir)
@@ -343,12 +343,10 @@ static inline mjtNum signedDistance(mjtNum normal[3], const mjtNum v1[3], const
sub3(diff1, v3, v1);
sub3(diff2, v2, v1);
cross3(normal, diff1, diff2);
mjtNum norm = mju_norm3(normal);
if (norm > mjMINVAL && norm < mjMAXVAL) {
mjtNum invnorm = 1/norm;
normal[0] *= invnorm;
normal[1] *= invnorm;
normal[2] *= invnorm;
mjtNum norm = dot3(normal, normal);
if (norm > mjMINVAL*mjMINVAL && norm < mjMAXVAL*mjMAXVAL) {
norm = 1/mju_sqrt(norm);
scl3(normal, normal, norm);
return dot3(normal, v1);
}
return mjMAXVAL; // cannot recover normal (ignore face)
@@ -453,8 +451,8 @@ static inline void lincomb3(mjtNum res[3], const mjtNum coef[3], const mjtNum v1
// res = origin projected onto plane defined by v1, v2, v3
static inline void projectOriginPlane(mjtNum res[3], const mjtNum v1[3], const mjtNum v2[3],
const mjtNum v3[3]) {
static int projectOriginPlane(mjtNum res[3], const mjtNum v1[3], const mjtNum v2[3],
const mjtNum v3[3]) {
mjtNum diff21[3], diff31[3], diff32[3], n[3], nv, nn;
sub3(diff21, v2, v1);
sub3(diff31, v3, v1);
@@ -464,18 +462,20 @@ static inline void projectOriginPlane(mjtNum res[3], const mjtNum v1[3], const m
cross3(n, diff32, diff21);
nv = dot3(n, v2);
nn = dot3(n, n);
if (nn == 0) return 1;
if (nv != 0 && nn > mjMINVAL) {
scl3(res, n, nv / nn);
return;
return 0;
}
// n = (v2 - v1) x (v3 - v1)
cross3(n, diff21, diff31);
nv = dot3(n, v1);
nn = dot3(n, n);
if (nn == 0) return 1;
if (nv != 0 && nn > mjMINVAL) {
scl3(res, n, nv / nn);
return;
return 0;
}
// n = (v1 - v3) x (v2 - v3)
@@ -483,6 +483,7 @@ static inline void projectOriginPlane(mjtNum res[3], const mjtNum v1[3], const m
nv = dot3(n, v3);
nn = dot3(n, n);
scl3(res, n, nv / nn);
return 0;
}
@@ -619,7 +620,6 @@ static void S3D(mjtNum lambda[4], const mjtNum s1[3], const mjtNum s2[3], const
lambda[1] = lambda_2d[1];
lambda[2] = lambda_2d[2];
lambda[3] = 0;
dmin = d;
}
}
}
@@ -629,7 +629,11 @@ static void S3D(mjtNum lambda[4], const mjtNum s1[3], const mjtNum s2[3], const
static void S2D(mjtNum lambda[3], const mjtNum s1[3], const mjtNum s2[3], const mjtNum s3[3]) {
// project origin onto affine hull of the simplex
mjtNum p_o[3];
projectOriginPlane(p_o, s1, s2, s3);
if (projectOriginPlane(p_o, s1, s2, s3)) {
S1D(lambda, s1, s2);
lambda[2] = 0;
return;
}
// Below are the minors M_i4 of the matrix M given by
// [[ s1_x, s2_x, s3_x, s4_x ],
@@ -750,7 +754,6 @@ static void S2D(mjtNum lambda[3], const mjtNum s1[3], const mjtNum s2[3], const
lambda[0] = lambda_1d[0];
lambda[1] = lambda_1d[1];
lambda[2] = 0;
dmin = d;
}
}
}
@@ -897,7 +900,6 @@ static int polytope2(Polytope* pt, const mjCCDStatus* status, mjCCDObj* obj1, mj
int v4i = newVertex(pt, v4a, v4b);
int v5i = newVertex(pt, v5a, v5b);
// build hexahedron
attachFace(pt, v1i, v3i, v4i, 1, 3, 2);
attachFace(pt, v1i, v5i, v3i, 2, 4, 0);
@@ -909,10 +911,13 @@ static int polytope2(Polytope* pt, const mjCCDStatus* status, mjCCDObj* obj1, mj
// if the origin is on the affine hull of any of the faces then the origin is not in the
// hexahedron or the hexahedron is degenerate
for (int i = 0; i < 6; i++) {
pt->map[i] = pt->faces + i;
pt->faces[i].index = i;
if (pt->faces[i].dist < mjMINVAL) {
return 3;
}
}
pt->nmap = 6;
// valid hexahedron for EPA
return 0;
@@ -1053,10 +1058,13 @@ static int polytope3(Polytope* pt, const mjCCDStatus* status, mjCCDObj* obj1, mj
// if the origin is on the affine hull of any of the faces then the origin is not in the
// hexahedron or the hexahedron is degenerate
for (int i = 0; i < 6; i++) {
pt->map[i] = pt->faces + i;
pt->faces[i].index = i;
if (pt->faces[i].dist < mjMINVAL) {
return 8;
}
}
pt->nmap = 6;
return 0;
}
@@ -1078,7 +1086,6 @@ static inline void replaceSimplex3(Polytope* pt, mjCCDStatus* status, int v1, in
copy3(status->simplex + 6, pt->verts + v3);
pt->nfaces = 0;
pt->nmap = 0;
pt->nverts = 0;
}
@@ -1108,6 +1115,12 @@ static int polytope4(Polytope* pt, mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj
replaceSimplex3(pt, status, v4, v3, v2);
return polytope3(pt, status, obj1, obj2);
}
for (int i = 0; i < 4; i++) {
pt->map[i] = pt->faces + i;
pt->faces[i].index = i;
}
pt->nmap = 4;
return 0;
}
@@ -1125,16 +1138,12 @@ static int newVertex(Polytope* pt, const mjtNum v1[3], const mjtNum v2[3]) {
// delete face from map (return non-zero on error)
static int deleteFace(Polytope* pt, Face* face) {
// SHOULD NOT OCCUR
if (pt->nmap < 2) {
pt->nmap = 0;
return 1;
static void deleteFace(Polytope* pt, Face* face) {
if (face->index >= 0) {
pt->map[face->index] = pt->map[--pt->nmap];
pt->map[face->index]->index = face->index;
}
face->dist = -1;
pt->map[face->index] = pt->map[--pt->nmap];
pt->map[face->index]->index = face->index;
return 0;
face->index = -2; // mark face as deleted from map and polytope
}
@@ -1147,7 +1156,8 @@ static inline int maxFaces(Polytope* pt) {
// attach a face to the polytope with the given vertex indices; return distance to origin
static inline mjtNum attachFace(Polytope* pt, int v1, int v2, int v3, int adj1, int adj2, int adj3) {
static inline mjtNum attachFace(Polytope* pt, int v1, int v2, int v3,
int adj1, int adj2, int adj3) {
Face* face = &pt->faces[pt->nfaces++];
face->verts[0] = v1;
face->verts[1] = v2;
@@ -1159,13 +1169,11 @@ static inline mjtNum attachFace(Polytope* pt, int v1, int v2, int v3, int adj1,
face->adj[2] = adj3;
// compute witness point v
projectOriginPlane(face->v, pt->verts + v1, pt->verts + v2, pt->verts + v3);
face->dist = mju_norm3(face->v);
int ret = projectOriginPlane(face->v, pt->verts + v3, pt->verts + v2, pt->verts + v1);
if (ret) return 0;
face->dist = mju_sqrt(dot3(face->v, face->v));
face->index = -1;
// store face in map
int i = pt->nmap++;
face->index = i;
pt->map[i] = face;
return face->dist;
}
@@ -1205,13 +1213,13 @@ static int horizonRec(Horizon* h, Face* face, int e) {
// v is visible from w so it is deleted and adjacent faces are checked
if (dot3(face->v, h->w) >= dist2) {
if (deleteFace(h->pt, face)) return 1; // escape recursion on error
deleteFace(h->pt, face);
// recursively search the adjacent faces on the next two edges
for (int k = 1; k < 3; k++) {
int i = (e + k) % 3;
Face* adjFace = &h->pt->faces[face->adj[i]];
if (adjFace->dist > 0) {
if (adjFace->index > -2) {
int adjEdge = getEdge(adjFace, face->verts[(i + 1) % 3]);
if (!horizonRec(h, adjFace, adjEdge)) {
addEdge(h, face->adj[i], adjEdge);
@@ -1227,7 +1235,7 @@ static int horizonRec(Horizon* h, Face* face, int e) {
// create horizon given the face as starting point
static void horizon(Horizon* h, Face* face) {
if (deleteFace(h->pt, face)) return;
deleteFace(h->pt, face);
// first edge
Face* adjFace = &h->pt->faces[face->adj[0]];
@@ -1239,14 +1247,14 @@ static void horizon(Horizon* h, Face* face) {
// second edge
adjFace = &h->pt->faces[face->adj[1]];
adjEdge = getEdge(adjFace, face->verts[2]);
if (adjFace->dist > 0 && !horizonRec(h, adjFace, adjEdge)) {
if (adjFace->index > -2 && !horizonRec(h, adjFace, adjEdge)) {
addEdge(h, face->adj[1], adjEdge);
}
// third edge
adjFace = &h->pt->faces[face->adj[2]];
adjEdge = getEdge(adjFace, face->verts[0]);
if (adjFace->dist > 0 && !horizonRec(h, adjFace, adjEdge)) {
if (adjFace->index > -2 && !horizonRec(h, adjFace, adjEdge)) {
addEdge(h, face->adj[2], adjEdge);
}
}
@@ -1283,10 +1291,10 @@ static void epaWitness(const Polytope* pt, const Face* face, mjtNum x1[3], mjtNu
// return the penetration depth of two convex objects; witness points are in status->{x1, x2}
static mjtNum epa(mjCCDStatus* status, Polytope* pt, mjCCDObj* obj1, mjCCDObj* obj2) {
mjtNum dist, tolerance = status->tolerance;
mjtNum tolerance = status->tolerance, lower, upper = FLT_MAX;
int k, kmax = status->max_iterations;
mjData* d = (mjData*) obj1->data;
Face* face; // face closest to origin
Face* face, *pface; // face closest to origin
// initialize horizon
Horizon h;
@@ -1297,42 +1305,41 @@ static mjtNum epa(mjCCDStatus* status, Polytope* pt, mjCCDObj* obj1, mjCCDObj* o
h.pt = pt;
for (k = 0; k < kmax; k++) {
// find the face closest to the origin
if (!pt->nmap) {
mju_warning("EPA: empty polytope");
mj_freeStack(d);
return 0; // assume 0 depth
}
pface = face;
dist = mjMAXVAL;
// find the face closest to the origin (lower bound for penetration depth)
lower = FLT_MAX;
for (int i = 0; i < pt->nmap; i++) {
if (pt->map[i]->dist < dist) {
if (pt->map[i]->dist < lower) {
face = pt->map[i];
dist = face->dist;
lower = face->dist;
}
}
// check if dist is 0
if (dist <= 0) {
// face not valid, return previous face
if (lower > upper) {
face = pface;
break;
}
// check if lower bound is 0
if (lower <= 0) {
mju_warning("EPA: origin lies on affine hull of face");
break;
}
// compute support point w from the closest face's normal
mjtNum w1[3], w2[3], w[3];
epaSupport(w1, w2, obj1, obj2, face->v, dist);
epaSupport(w1, w2, obj1, obj2, face->v, lower);
sub3(w, w1, w2);
mjtNum next_dist = dot3(face->v, w) / dist;
if (next_dist - dist < tolerance) {
mjtNum upper_k = dot3(face->v, w) / lower; // upper bound for kth iteration
if (upper_k < upper) upper = upper_k;
if (upper - lower < tolerance) {
break;
}
h.w = w;
horizon(&h, face);
if (!pt->nmap) {
h.nedges = 0;
// next iteration will clean up and error out
continue;
}
// insert w as new vertex and attach faces along the horizon
int wi = newVertex(pt, w1, w2), nfaces = pt->nfaces, nedges = h.nedges;
@@ -1349,11 +1356,26 @@ static mjtNum epa(mjCCDStatus* status, Polytope* pt, mjCCDObj* obj1, mjCCDObj* o
int v1 = horFace->verts[horEdge],
v2 = horFace->verts[(horEdge + 1) % 3];
horFace->adj[horEdge] = nfaces;
attachFace(pt, wi, v2, v1, nfaces + nedges - 1, horIndex, nfaces + 1);
mjtNum dist = attachFace(pt, wi, v2, v1, nfaces + nedges - 1, horIndex, nfaces + 1);
// unrecoverable numerical issue
if (dist == 0) {
mj_freeStack(d);
status->epa_iterations = k;
status->nx = 0;
return 0;
}
// store face in map
if (dist >= lower && dist <= upper) {
int i = pt->nmap++;
pt->map[i] = &pt->faces[pt->nfaces - 1];
pt->map[i]->index = i;
}
// attach remaining faces
for (int i = 1; i < nedges; i++) {
int cur = nfaces + i; // index of attached face
int cur = nfaces + i; // index of attached face
int next = nfaces + (i + 1) % nedges; // index of next face
horIndex = h.indices[i], horEdge = h.edges[i];
@@ -1361,16 +1383,36 @@ static mjtNum epa(mjCCDStatus* status, Polytope* pt, mjCCDObj* obj1, mjCCDObj* o
v1 = horFace->verts[horEdge];
v2 = horFace->verts[(horEdge + 1) % 3];
horFace->adj[horEdge] = cur;
attachFace(pt, wi, v2, v1, cur - 1, horIndex, next);
dist = attachFace(pt, wi, v2, v1, cur - 1, horIndex, next);
// unrecoverable numerical issue
if (dist == 0) {
mj_freeStack(d);
status->epa_iterations = k;
status->nx = 0;
return 0;
}
// store face in map
if (dist >= lower && dist <= upper) {
int idx = pt->nmap++;
pt->map[idx] = &pt->faces[pt->nfaces - 1];
pt->map[idx]->index = idx;
}
}
h.nedges = 0; // clear horizon
// no face candidates left
if (!pt->nmap) {
break;
}
}
mj_freeStack(d);
epaWitness(pt, face, status->x1, status->x2);
status->epa_iterations = k;
status->nx = 1;
return dist;
return face->dist;
}
test/engine/engine_collision_gjk_test.cc
+49
View File
@@ -286,6 +286,55 @@ TEST_F(MjGjkTest, BoxBoxDepth) {
mj_deleteModel(model);
}
TEST_F(MjGjkTest, BoxBoxDepth2) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<geom name="geom1" type="box" pos="0 0 0" size="5 5 .1"/>
<geom name="geom2" type="box" pos="0 0 0" size="1 1 1"/>
</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* xmat = data->geom_xmat + 9;
mjtNum* xpos = data->geom_xpos + 3;
xpos[0] = -0.000171208577507291721461757383;
xpos[1] = -0.000171208577507290908310128019;
xpos[2] = 1.067119586248553853025100579544;
xmat[0] = 0.999999966039443077825410455262;
xmat[1] = -0.000000033960556969622165789148;
xmat[2] = -0.000260616790777324182967061850;
xmat[3] = -0.000000033960556972087627235699;
xmat[4] = 0.999999966039443077825410455262;
xmat[5] = -0.000260616790777321797722282382;
xmat[6] = 0.000260616790777324182967061850;
xmat[7] = 0.000260616790777321797722282382;
xmat[8] = 0.999999932078886044628518448008;
int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1");
int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2");
mjtNum dir[3], pos[3];
mjtNum dist = Penetration(model, data, geom1, geom2, dir, pos);
if (dist < 0) {
EXPECT_NEAR(dist, -0.033401579411886845, kTolerance);
EXPECT_NEAR(dir[0], 0, kTolerance);
EXPECT_NEAR(dir[1], 0, kTolerance);
EXPECT_NEAR(dir[2], 1, kTolerance);
}
mj_deleteData(data);
mj_deleteModel(model);
}
TEST_F(MjGjkTest, SmallBoxMesh) {
static constexpr char xml[] = R"(
<mujoco>