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

Modify mjc_FlexSDF to support mjPreContact directly.

Browse Source 
PiperOrigin-RevId: 933068825
Change-Id: I328d64bb05270067ce146908b0df3ca83f10ed9b
This commit is contained in:
Kyle Bayes
2026-06-16 07:03:45 -07:00
committed by Copybara-Service
parent 55d7ff49ec
commit b469144a81
4 changed files with 79 additions and 69 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_collision_driver.c
+68 -30
View File
@@ -351,14 +351,23 @@ int mj_isElemActive(const mjModel* m, int f, int e) {
//----------------------------- collision detection entry point ------------------------------------
// binary search between two bodyflex trees
void mj_collideTree(const mjModel* m, mjData* d, int bf1, int bf2,
int merged, int startadr, int pairadr);
static void mj_collideTree(const mjModel* m, mjData* d, int bf1, int bf2,
int merged, int startadr, int pairadr);
// compute contacts for a batch of collision pairs contained in a buffer of
// stride 3 ints (g1, g2, ipair)
// if buffer is NULL, results are read from arena starting at parena
void mj_narrowphase(const mjModel* m, mjData* d, const int* buffer, int npair, size_t parena);
static void mj_narrowphase(const mjModel* m, mjData* d, const int* buffer, int npair,
size_t parena);
// test a plane geom and a flex for collision, add to contact list
static void mj_collidePlaneFlex(const mjModel* m, mjData* d, int g, int f);
// test an SDF geom and a flex for collision, add to contact list
static void mj_collideSdfFlex(const mjModel* m, mjData* d, int g, int f);
// test for internal flex collisions, add to contact list
static void mj_collideFlexInternal(const mjModel* m, mjData* d, int f);
// compare contact pairs by their geom/elem/vert IDs
static inline int contactcompare(const mjContact* c1, const mjContact* c2, void* context) {
@@ -937,8 +946,8 @@ int mj_collideOBB(const mjtNum aabb1[6], const mjtNum aabb2[6],
// binary search between two bodyflex trees
void mj_collideTree(const mjModel* m, mjData* d, int bf1, int bf2,
int merged, int startadr, int pairadr) {
static void mj_collideTree(const mjModel* m, mjData* d, int bf1, int bf2,
int merged, int startadr, int pairadr) {
int nbody = m->nbody, nbvhstatic = m->nbvhstatic;
mjtBool isbody1 = (bf1 < nbody);
mjtBool isbody2 = (bf2 < nbody);
@@ -1865,7 +1874,8 @@ static void collisionTask(const mjModel* m, mjData* d, void* arg, int thread_id,
// compute contacts for a batch of collision pairs contained in a buffer of
// stride 3 ints (g1, g2, ipair)
// if buffer is NULL, results are read from arena starting at parena
void mj_narrowphase(const mjModel* m, mjData* d, const int* buffer, int npair, size_t parena) {
static void mj_narrowphase(const mjModel* m, mjData* d, const int* buffer, int npair,
size_t parena) {
int nthread = mju_numThread(d);
int ccd_size = mjc_ccdSize(m->opt.ccd_iterations);
mjtNum margin, gap;
@@ -1992,7 +2002,7 @@ void mj_narrowphase(const mjModel* m, mjData* d, const int* buffer, int npair, s
// test a plane geom and a flex for collision, add to contact list
void mj_collidePlaneFlex(const mjModel* m, mjData* d, int g, int f) {
static void mj_collidePlaneFlex(const mjModel* m, mjData* d, int g, int f) {
mjContact con;
mjtNum radius = m->flex_radius[f];
mjtNum* pos = d->geom_xpos + 3*g;
@@ -2049,7 +2059,7 @@ void mj_collidePlaneFlex(const mjModel* m, mjData* d, int g, int f) {
// test an SDF geom and a flex for collision, add to contact list
void mj_collideSdfFlex(const mjModel* m, mjData* d, int g, int f) {
static void mj_collideSdfFlex(const mjModel* m, mjData* d, int g, int f) {
// only support dim==2 (triangular elements)
if (m->flex_dim[f] != 2) {
return;
@@ -2065,29 +2075,37 @@ void mj_collideSdfFlex(const mjModel* m, mjData* d, int g, int f) {
// allocate temporary contact array on stack (zero-initialized)
mj_markStack(d);
mjContact* con = mjSTACKALLOC(d, mjMAXCONPAIR, mjContact);
memset(con, 0, mjMAXCONPAIR * sizeof(mjContact));
mjPreContact* precon = mjSTACKALLOC(d, mjMAXCONPAIR, mjPreContact);
int* elem = mjSTACKALLOC(d, mjMAXCONPAIR, int);
// call batched flex-SDF collision
int num = mjc_FlexSDF(m, d, con, g, f, margin + gap);
int num = mjc_FlexSDF(m, d, precon, elem, g, f, margin + gap);
// add contacts to mjData
mjContact con;
memset(&con, 0, sizeof(mjContact));
for (int i = 0; i < num; i++) {
// copy properties from mjPreContact to mjContact
con.dist = precon[i].dist;
mju_copy3(con.pos, precon[i].pos);
mju_copy3(con.frame, precon[i].normal);
mju_copy3(con.frame + 3, precon[i].tangent);
// set contact ids
con[i].geom[0] = g;
con[i].geom[1] = -1;
con[i].flex[0] = -1;
con[i].flex[1] = f;
con[i].elem[0] = -1;
// con[i].elem[1] already set by mjc_FlexSDF with actual element index
con[i].vert[0] = -1;
con[i].vert[1] = -1;
con.geom[0] = g;
con.geom[1] = -1;
con.flex[0] = -1;
con.flex[1] = f;
con.elem[0] = -1;
con.elem[1] = elem[i];
con.vert[0] = -1;
con.vert[1] = -1;
// set remaining contact parameters
mj_setContact(m, con + i, condim, margin, solref, solreffriction, solimp, friction);
mj_setContact(m, &con, condim, margin, solref, solreffriction, solimp, friction);
// add to mjData, abort if too many contacts
if (mj_addContact(m, d, con + i)) {
if (mj_addContact(m, d, &con)) {
mj_freeStack(d);
return;
}
@@ -2098,7 +2116,7 @@ void mj_collideSdfFlex(const mjModel* m, mjData* d, int g, int f) {
// test single triangle plane : vertex
static int planeVertex(mjContact* con, const mjtNum* pos, mjtNum rad,
static int planeVertex(mjPreContact* con, const mjtNum* pos, mjtNum rad,
int t0, int t1, int t2, int v) {
// make t0 the origin
mjtNum e1[3], e2[3], ev[3];
@@ -2119,17 +2137,16 @@ static int planeVertex(mjContact* con, const mjtNum* pos, mjtNum rad,
// construct contact
con->dist = -dst-2*rad;
mju_scl3(con->frame, nrm, -1);
mju_zero3(con->frame+3);
mju_scl3(con->normal, nrm, -1);
mju_zero3(con->tangent);
mju_addScl3(con->pos, pos+3*v, nrm, -0.5*dst);
con->vert[1] = v;
return 1;
}
// test for internal flex collisions, add to contact list
// ignore margin to avoid permament self-collision
void mj_collideFlexInternal(const mjModel* m, mjData* d, int f) {
static void mj_collideFlexInternal(const mjModel* m, mjData* d, int f) {
int flex_evpairnum = m->flex_evpairnum[f];
// predefined element-vertex
@@ -2161,29 +2178,50 @@ void mj_collideFlexInternal(const mjModel* m, mjData* d, int f) {
// process all elements
const mjtNum* vertxpos = d->flexvert_xpos + 3*m->flex_vertadr[f];
for (int e=0; e < flex_elemnum; e++) {
mjPreContact precon;
const int* edata = m->flex_elem + m->flex_elemdataadr[f] + e*4;
con.elem[0] = e;
// face (0,1,2)
if (planeVertex(&con, vertxpos, radius, edata[0], edata[1], edata[2], edata[3])) {
if (planeVertex(&precon, vertxpos, radius, edata[0], edata[1], edata[2], edata[3])) {
con.vert[1] = edata[3];
con.dist = precon.dist;
mju_copy3(con.pos, precon.pos);
mju_copy3(con.frame, precon.normal);
mju_copy3(con.frame + 3, precon.tangent);
mj_setContact(m, &con, condim, 0, solref, solreffriction, solimp, friction);
if (mj_addContact(m, d, &con)) return;
}
// face (0,2,3)
if (planeVertex(&con, vertxpos, radius, edata[0], edata[2], edata[3], edata[1])) {
if (planeVertex(&precon, vertxpos, radius, edata[0], edata[2], edata[3], edata[1])) {
con.vert[1] = edata[1];
con.dist = precon.dist;
mju_copy3(con.pos, precon.pos);
mju_copy3(con.frame, precon.normal);
mju_copy3(con.frame + 3, precon.tangent);
mj_setContact(m, &con, condim, 0, solref, solreffriction, solimp, friction);
if (mj_addContact(m, d, &con)) return;
}
// face (0,3,1)
if (planeVertex(&con, vertxpos, radius, edata[0], edata[3], edata[1], edata[2])) {
if (planeVertex(&precon, vertxpos, radius, edata[0], edata[3], edata[1], edata[2])) {
con.vert[1] = edata[2];
con.dist = precon.dist;
mju_copy3(con.pos, precon.pos);
mju_copy3(con.frame, precon.normal);
mju_copy3(con.frame + 3, precon.tangent);
mj_setContact(m, &con, condim, 0, solref, solreffriction, solimp, friction);
if (mj_addContact(m, d, &con)) return;
}
// face (1,3,2)
if (planeVertex(&con, vertxpos, radius, edata[1], edata[3], edata[2], edata[0])) {
if (planeVertex(&precon, vertxpos, radius, edata[1], edata[3], edata[2], edata[0])) {
con.vert[1] = edata[0];
con.dist = precon.dist;
mju_copy3(con.pos, precon.pos);
mju_copy3(con.frame, precon.normal);
mju_copy3(con.frame + 3, precon.tangent);
mj_setContact(m, &con, condim, 0, solref, solreffriction, solimp, friction);
if (mj_addContact(m, d, &con)) return;
}
src/engine/engine_collision_driver.h
-9
View File
@@ -47,15 +47,6 @@ MJAPI int mj_isElemActive(const mjModel* m, int f, int e);
// broad phase collision detection; return list of bodyflex pairs
int mj_broadphase(const mjModel* m, mjData* d, int* bfpair, int maxpair);
// test a plane geom and a flex for collision, add to contact list
void mj_collidePlaneFlex(const mjModel* m, mjData* d, int g, int f);
// test an SDF geom and a flex for collision, add to contact list
void mj_collideSdfFlex(const mjModel* m, mjData* d, int g, int f);
// test for internal flex collisions, add to contact list
void mj_collideFlexInternal(const mjModel* m, mjData* d, int f);
// test active element self-collisions with SAP
void mj_collideFlexSAP(const mjModel* m, mjData* d, int f);
src/engine/engine_collision_sdf.c
+10 -29
View File
@@ -577,29 +577,10 @@ static int addPreContact(mjtNum* points, mjPreContact* con, const mjtNum x[3],
mju_addTo3(con->pos, pos2);
mju_addTo3(con->pos, vec);
return cnt+1;
return cnt + 1;
}
// adds candidate point to result
// flipNormal: 0 = normal points INTO SDF (for mesh-SDF where SDF is g2)
// 1 = normal points OUT of SDF (for flex-SDF where SDF is g1)
static int addContact(mjtNum* points, mjContact* con, const mjtNum x[3],
const mjtNum pos2[3], const mjtNum quat2[4], mjtNum dist,
int cnt, const mjModel* m, const mjSDF* s, const mjData* d,
int flipNormal) {
mjPreContact precon;
int ncon = addPreContact(points, &precon, x, pos2, quat2, dist, cnt, m, s, d,
flipNormal);
if (ncon > cnt) {
con[cnt].dist = precon.dist;
mju_copy3(con[cnt].pos, precon.pos);
mju_copy3(con[cnt].frame, precon.normal);
mju_zero3(con[cnt].frame + 3);
}
return ncon;
}
// finds minimum of Frank-Wolfe objective
static mjtNum stepFrankWolfe(mjtNum x[3], const mjtNum* corners, int ncorners,
const mjModel* m, const mjSDF* sdf, const mjData* d) {
@@ -1264,7 +1245,7 @@ static int flexElemCallback(int elem_idx, int node, void* ctx) {
}
int mjc_FlexSDF(const mjModel* m, const mjData* d, mjContact* con,
int mjc_FlexSDF(const mjModel* m, const mjData* d, mjPreContact* con, int* elem,
int g, int f, mjtNum margin) {
// g = SDF geom, f = flex
int dim = m->flex_dim[f];
@@ -1336,11 +1317,11 @@ int mjc_FlexSDF(const mjModel* m, const mjData* d, mjContact* con,
int elem_cnt = 0;
for (int i = 0; i < elem_ncandidate && cnt < mjMAXCONPAIR; i++) {
int old_elem_cnt = elem_cnt;
elem_cnt = addContact(elem_points, con + cnt, elem_candidate + 3*i, sdf_pos, sdf_quat,
elem_cnt = addPreContact(elem_points, con + cnt, elem_candidate + 3*i, sdf_pos, sdf_quat,
elem_dist[i], elem_cnt, m, &sdf, d, 1);
// set element ID for successfully added contact
if (elem_cnt > old_elem_cnt) {
con[cnt].elem[1] = e;
elem[cnt] = e;
cnt++;
}
}
@@ -1380,11 +1361,11 @@ int mjc_FlexSDF(const mjModel* m, const mjData* d, mjContact* con,
if (ncandidate <= mjMAXCONPAIR) {
for (int i = 0; i < ncandidate; i++) {
int old_cnt = cnt;
cnt = addContact(points, con, candidate + 3*i, sdf_pos, sdf_quat,
dist[i], cnt, m, &sdf, d, 1);
cnt = addPreContact(points, con + cnt, candidate + 3*i, sdf_pos, sdf_quat, dist[i], cnt, m,
&sdf, d, 1);
// set element ID for successfully added contact
if (cnt > old_cnt) {
con[old_cnt].elem[1] = elem_id[i];
elem[old_cnt] = elem_id[i];
}
}
return cnt;
@@ -1398,11 +1379,11 @@ int mjc_FlexSDF(const mjModel* m, const mjData* d, mjContact* con,
for (int i = 0; i < nselected; i++) {
int idx = selected_indices[i];
int old_cnt = cnt;
cnt = addContact(points, con, candidate + 3*idx, sdf_pos, sdf_quat,
dist[idx], cnt, m, &sdf, d, 1);
cnt = addPreContact(points, con + cnt, candidate + 3*idx, sdf_pos, sdf_quat, dist[idx], cnt, m,
&sdf, d, 1);
// set element ID for successfully added contact
if (cnt > old_cnt) {
con[old_cnt].elem[1] = elem_id[idx];
elem[old_cnt] = elem_id[idx];
}
}
src/engine/engine_collision_sdf.h
+1 -1
View File
@@ -45,7 +45,7 @@ int mjc_MeshSDF(const mjModel* m, mjData* d, mjPreContact* con, int g1, int g2,
int mjc_SDF(const mjModel* m, mjData* d, mjPreContact* con, int g1, int g2, mjtNum margin);
// collision between entire flex and SDF geom (batched processing)
int mjc_FlexSDF(const mjModel* m, const mjData* d, mjContact* con,
int mjc_FlexSDF(const mjModel* m, const mjData* d, mjPreContact* con, int* elem,
int g, int f, mjtNum margin);
#ifdef __cplusplus