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 ------------------------------------ //----------------------------- collision detection entry point ------------------------------------
// binary search between two bodyflex trees // binary search between two bodyflex trees
void mj_collideTree(const mjModel* m, mjData* d, int bf1, int bf2, static void mj_collideTree(const mjModel* m, mjData* d, int bf1, int bf2,
int merged, int startadr, int pairadr); int merged, int startadr, int pairadr);
// compute contacts for a batch of collision pairs contained in a buffer of // compute contacts for a batch of collision pairs contained in a buffer of
// stride 3 ints (g1, g2, ipair) // stride 3 ints (g1, g2, ipair)
// if buffer is NULL, results are read from arena starting at parena // 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 // compare contact pairs by their geom/elem/vert IDs
static inline int contactcompare(const mjContact* c1, const mjContact* c2, void* context) { 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 // binary search between two bodyflex trees
void mj_collideTree(const mjModel* m, mjData* d, int bf1, int bf2, static void mj_collideTree(const mjModel* m, mjData* d, int bf1, int bf2,
int merged, int startadr, int pairadr) { int merged, int startadr, int pairadr) {
int nbody = m->nbody, nbvhstatic = m->nbvhstatic; int nbody = m->nbody, nbvhstatic = m->nbvhstatic;
mjtBool isbody1 = (bf1 < nbody); mjtBool isbody1 = (bf1 < nbody);
mjtBool isbody2 = (bf2 < 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 // compute contacts for a batch of collision pairs contained in a buffer of
// stride 3 ints (g1, g2, ipair) // stride 3 ints (g1, g2, ipair)
// if buffer is NULL, results are read from arena starting at parena // 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 nthread = mju_numThread(d);
int ccd_size = mjc_ccdSize(m->opt.ccd_iterations); int ccd_size = mjc_ccdSize(m->opt.ccd_iterations);
mjtNum margin, gap; 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 // 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; mjContact con;
mjtNum radius = m->flex_radius[f]; mjtNum radius = m->flex_radius[f];
mjtNum* pos = d->geom_xpos + 3*g; 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 // 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) // only support dim==2 (triangular elements)
if (m->flex_dim[f] != 2) { if (m->flex_dim[f] != 2) {
return; 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) // allocate temporary contact array on stack (zero-initialized)
mj_markStack(d); mj_markStack(d);
mjContact* con = mjSTACKALLOC(d, mjMAXCONPAIR, mjContact); mjPreContact* precon = mjSTACKALLOC(d, mjMAXCONPAIR, mjPreContact);
memset(con, 0, mjMAXCONPAIR * sizeof(mjContact)); int* elem = mjSTACKALLOC(d, mjMAXCONPAIR, int);
// call batched flex-SDF collision // 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 // add contacts to mjData
mjContact con;
memset(&con, 0, sizeof(mjContact));
for (int i = 0; i < num; i++) { 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 // set contact ids
con[i].geom[0] = g; con.geom[0] = g;
con[i].geom[1] = -1; con.geom[1] = -1;
con[i].flex[0] = -1; con.flex[0] = -1;
con[i].flex[1] = f; con.flex[1] = f;
con[i].elem[0] = -1; con.elem[0] = -1;
// con[i].elem[1] already set by mjc_FlexSDF with actual element index con.elem[1] = elem[i];
con[i].vert[0] = -1; con.vert[0] = -1;
con[i].vert[1] = -1; con.vert[1] = -1;
// set remaining contact parameters // 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 // add to mjData, abort if too many contacts
if (mj_addContact(m, d, con + i)) { if (mj_addContact(m, d, &con)) {
mj_freeStack(d); mj_freeStack(d);
return; return;
} }
@@ -2098,7 +2116,7 @@ void mj_collideSdfFlex(const mjModel* m, mjData* d, int g, int f) {
// test single triangle plane : vertex // 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) { int t0, int t1, int t2, int v) {
// make t0 the origin // make t0 the origin
mjtNum e1[3], e2[3], ev[3]; mjtNum e1[3], e2[3], ev[3];
@@ -2119,17 +2137,16 @@ static int planeVertex(mjContact* con, const mjtNum* pos, mjtNum rad,
// construct contact // construct contact
con->dist = -dst-2*rad; con->dist = -dst-2*rad;
mju_scl3(con->frame, nrm, -1); mju_scl3(con->normal, nrm, -1);
mju_zero3(con->frame+3); mju_zero3(con->tangent);
mju_addScl3(con->pos, pos+3*v, nrm, -0.5*dst); mju_addScl3(con->pos, pos+3*v, nrm, -0.5*dst);
con->vert[1] = v;
return 1; return 1;
} }
// test for internal flex collisions, add to contact list // test for internal flex collisions, add to contact list
// ignore margin to avoid permament self-collision // 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]; int flex_evpairnum = m->flex_evpairnum[f];
// predefined element-vertex // predefined element-vertex
@@ -2161,29 +2178,50 @@ void mj_collideFlexInternal(const mjModel* m, mjData* d, int f) {
// process all elements // process all elements
const mjtNum* vertxpos = d->flexvert_xpos + 3*m->flex_vertadr[f]; const mjtNum* vertxpos = d->flexvert_xpos + 3*m->flex_vertadr[f];
for (int e=0; e < flex_elemnum; e++) { for (int e=0; e < flex_elemnum; e++) {
mjPreContact precon;
const int* edata = m->flex_elem + m->flex_elemdataadr[f] + e*4; const int* edata = m->flex_elem + m->flex_elemdataadr[f] + e*4;
con.elem[0] = e; con.elem[0] = e;
// face (0,1,2) // 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); mj_setContact(m, &con, condim, 0, solref, solreffriction, solimp, friction);
if (mj_addContact(m, d, &con)) return; if (mj_addContact(m, d, &con)) return;
} }
// face (0,2,3) // 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); mj_setContact(m, &con, condim, 0, solref, solreffriction, solimp, friction);
if (mj_addContact(m, d, &con)) return; if (mj_addContact(m, d, &con)) return;
} }
// face (0,3,1) // 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); mj_setContact(m, &con, condim, 0, solref, solreffriction, solimp, friction);
if (mj_addContact(m, d, &con)) return; if (mj_addContact(m, d, &con)) return;
} }
// face (1,3,2) // 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); mj_setContact(m, &con, condim, 0, solref, solreffriction, solimp, friction);
if (mj_addContact(m, d, &con)) return; 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 // broad phase collision detection; return list of bodyflex pairs
int mj_broadphase(const mjModel* m, mjData* d, int* bfpair, int maxpair); 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 // test active element self-collisions with SAP
void mj_collideFlexSAP(const mjModel* m, mjData* d, int f); 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, pos2);
mju_addTo3(con->pos, vec); 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 // finds minimum of Frank-Wolfe objective
static mjtNum stepFrankWolfe(mjtNum x[3], const mjtNum* corners, int ncorners, static mjtNum stepFrankWolfe(mjtNum x[3], const mjtNum* corners, int ncorners,
const mjModel* m, const mjSDF* sdf, const mjData* d) { 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) { int g, int f, mjtNum margin) {
// g = SDF geom, f = flex // g = SDF geom, f = flex
int dim = m->flex_dim[f]; 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; int elem_cnt = 0;
for (int i = 0; i < elem_ncandidate && cnt < mjMAXCONPAIR; i++) { for (int i = 0; i < elem_ncandidate && cnt < mjMAXCONPAIR; i++) {
int old_elem_cnt = elem_cnt; 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); elem_dist[i], elem_cnt, m, &sdf, d, 1);
// set element ID for successfully added contact // set element ID for successfully added contact
if (elem_cnt > old_elem_cnt) { if (elem_cnt > old_elem_cnt) {
con[cnt].elem[1] = e; elem[cnt] = e;
cnt++; cnt++;
} }
} }
@@ -1380,11 +1361,11 @@ int mjc_FlexSDF(const mjModel* m, const mjData* d, mjContact* con,
if (ncandidate <= mjMAXCONPAIR) { if (ncandidate <= mjMAXCONPAIR) {
for (int i = 0; i < ncandidate; i++) { for (int i = 0; i < ncandidate; i++) {
int old_cnt = cnt; int old_cnt = cnt;
cnt = addContact(points, con, candidate + 3*i, sdf_pos, sdf_quat, cnt = addPreContact(points, con + cnt, candidate + 3*i, sdf_pos, sdf_quat, dist[i], cnt, m,
dist[i], cnt, m, &sdf, d, 1); &sdf, d, 1);
// set element ID for successfully added contact // set element ID for successfully added contact
if (cnt > old_cnt) { if (cnt > old_cnt) {
con[old_cnt].elem[1] = elem_id[i]; elem[old_cnt] = elem_id[i];
} }
} }
return cnt; return cnt;
@@ -1398,11 +1379,11 @@ int mjc_FlexSDF(const mjModel* m, const mjData* d, mjContact* con,
for (int i = 0; i < nselected; i++) { for (int i = 0; i < nselected; i++) {
int idx = selected_indices[i]; int idx = selected_indices[i];
int old_cnt = cnt; int old_cnt = cnt;
cnt = addContact(points, con, candidate + 3*idx, sdf_pos, sdf_quat, cnt = addPreContact(points, con + cnt, candidate + 3*idx, sdf_pos, sdf_quat, dist[idx], cnt, m,
dist[idx], cnt, m, &sdf, d, 1); &sdf, d, 1);
// set element ID for successfully added contact // set element ID for successfully added contact
if (cnt > old_cnt) { 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); 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) // 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); int g, int f, mjtNum margin);
#ifdef __cplusplus #ifdef __cplusplus