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

Add passive collision mode for flexes.

Browse Source 
PiperOrigin-RevId: 801754907
Change-Id: I4147af2ee2596519d42cb9a03cf8d5488f0bc3e8
This commit is contained in:
Alessio Quaglino
2025-09-01 04:13:07 -07:00
committed by Copybara-Service
parent b9900db00e
commit 8acd83f317
17 changed files with 320 additions and 134 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_core_constraint.c
+140 -123
View File
@@ -852,6 +852,86 @@ void mj_instantiateLimit(const mjModel* m, mjData* d) {
// compute Jacobian for contact, return number of DOFs affected
int mj_contactJacobian(const mjModel* m, mjData* d, const mjContact* con, int dim,
mjtNum* jac, mjtNum* jacdif, mjtNum* jacdifp,
mjtNum* jacdifr, mjtNum* jac1p, mjtNum* jac2p,
mjtNum* jac1r, mjtNum* jac2r, int* chain) {
// special case: single body on each side
if ((con->geom[0] >= 0 || (con->vert[0] >= 0 && m->flex_interp[con->flex[0]] == 0)) &&
(con->geom[1] >= 0 || (con->vert[1] >= 0 && m->flex_interp[con->flex[1]] == 0))) {
// get bodies
int bid[2];
for (int side=0; side < 2; side++) {
bid[side] = (con->geom[side] >= 0) ?
m->geom_bodyid[con->geom[side]] :
m->flex_vertbodyid[m->flex_vertadr[con->flex[side]] + con->vert[side]];
}
// compute Jacobian differences
if (dim > 3) {
return mj_jacDifPair(m, d, chain, bid[0], bid[1], con->pos, con->pos,
jac1p, jac2p, jacdifp, jac1r, jac2r, jacdifr);
} else {
return mj_jacDifPair(m, d, chain, bid[0], bid[1], con->pos, con->pos,
jac1p, jac2p, jacdifp, NULL, NULL, NULL);
}
}
// general case: flex elements involved
else {
// get bodies and weights
int nb = 0;
int bid[64];
mjtNum bweight[64];
for (int side=0; side < 2; side++) {
int nw = 0;
int vid[4];
mjtNum bw[4];
// geom
if (con->geom[side] >= 0) {
bid[nb] = m->geom_bodyid[con->geom[side]];
bweight[nb] = side ? +1 : -1;
nb++;
}
// flex vert
else if (con->vert[side] >= 0) {
vid[0] = m->flex_vertadr[con->flex[side]] + con->vert[side];
bw[0] = side ? +1 : -1;
nw = 1;
}
// flex elem
else {
nw = mj_elemBodyWeight(m, d, con->flex[side], con->elem[side],
con->vert[1-side], con->pos, vid, bw);
// negative sign for first side of contact
if (side == 0) {
mju_scl(bw, bw, -1, nw);
}
}
// get body or node ids and weights
for (int k=0; k < nw; k++) {
if (m->flex_interp[con->flex[side]] == 0) {
bid[nb] = m->flex_vertbodyid[vid[k]];
bweight[nb] = bw[k];
nb++;
} else {
nb += mj_vertBodyWeight(m, d, con->flex[side], vid[k],
con->pos, bid+nb, bweight+nb, bw[k]);
}
}
}
// combine weighted Jacobians
return mj_jacSum(m, d, chain, nb, bid, bweight, con->pos, jacdif, dim > 3);
}
}
// frictionless and frictional contacts
void mj_instantiateContact(const mjModel* m, mjData* d) {
int ispyramid = mj_isPyramidal(m), issparse = mj_isSparse(m), ncon = d->ncon;
@@ -880,142 +960,72 @@ void mj_instantiateContact(const mjModel* m, mjData* d) {
// find contacts to be included
for (int i=0; i < ncon; i++) {
if (!d->contact[i].exclude) {
// get contact info, safe efc_address
con = d->contact + i;
dim = con->dim;
con->efc_address = d->nefc;
if (d->contact[i].exclude) {
continue;
}
// special case: single body on each side
if ((con->geom[0] >= 0 || (con->vert[0] >= 0 && m->flex_interp[con->flex[0]] == 0)) &&
(con->geom[1] >= 0 || (con->vert[1] >= 0 && m->flex_interp[con->flex[1]] == 0))) {
// get bodies
int bid[2];
for (int side=0; side < 2; side++) {
bid[side] = (con->geom[side] >= 0) ?
m->geom_bodyid[con->geom[side]] :
m->flex_vertbodyid[m->flex_vertadr[con->flex[side]] + con->vert[side]];
}
// get contact info, save efc_address
con = d->contact + i;
dim = con->dim;
con->efc_address = d->nefc;
NV = mj_contactJacobian(m, d, con, dim, jac, jacdif, jacdifp, jacdifr,
jac1p, jac2p, jac1r, jac2r, chain);
// compute Jacobian differences
if (dim > 3) {
NV = mj_jacDifPair(m, d, chain, bid[0], bid[1], con->pos, con->pos,
jac1p, jac2p, jacdifp, jac1r, jac2r, jacdifr);
} else {
NV = mj_jacDifPair(m, d, chain, bid[0], bid[1], con->pos, con->pos,
jac1p, jac2p, jacdifp, NULL, NULL, NULL);
}
}
// skip contact if no DOFs affected
if (NV == 0) {
con->efc_address = -1;
con->exclude = 3;
continue;
}
// general case: flex elements involved
else {
// get bodies and weights
int nb = 0;
int bid[64];
mjtNum bweight[64];
for (int side=0; side < 2; side++) {
int nw = 0;
int vid[4];
mjtNum bw[4];
// rotate Jacobian differences to contact frame
mju_mulMatMat(jac, con->frame, jacdifp, dim > 1 ? 3 : 1, 3, NV);
if (dim > 3) {
mju_mulMatMat(jac + 3*NV, con->frame, jacdifr, dim-3, 3, NV);
}
// geom
if (con->geom[side] >= 0) {
bid[nb] = m->geom_bodyid[con->geom[side]];
bweight[nb] = side ? +1 : -1;
nb++;
}
// make frictionless contact
if (dim == 1) {
// add constraint
mj_addConstraint(m, d, jac, &(con->dist), &(con->includemargin), 0,
1, mjCNSTR_CONTACT_FRICTIONLESS, i,
issparse ? NV : 0,
issparse ? chain : NULL);
}
// flex vert
else if (con->vert[side] >= 0) {
vid[0] = m->flex_vertadr[con->flex[side]] + con->vert[side];
bw[0] = side ? +1 : -1;
nw = 1;
}
// make pyramidal friction cone
else if (ispyramid) {
// pos = dist
cpos[0] = cpos[1] = con->dist;
cmargin[0] = cmargin[1] = con->includemargin;
// flex elem
else {
nw = mj_elemBodyWeight(m, d, con->flex[side], con->elem[side],
con->vert[1-side], con->pos, vid, bw);
// one pair per friction dimension
for (int k=1; k < con->dim; k++) {
// Jacobian for pair of opposing pyramid edges
mju_addScl(jacdifp, jac, jac + k*NV, con->friction[k-1], NV);
mju_addScl(jacdifp + NV, jac, jac + k*NV, -con->friction[k-1], NV);
// negative sign for first side of contact
if (side == 0) {
mju_scl(bw, bw, -1, nw);
}
}
// get body or node ids and weights
for (int k=0; k < nw; k++) {
if (m->flex_interp[con->flex[side]] == 0) {
bid[nb] = m->flex_vertbodyid[vid[k]];
bweight[nb] = bw[k];
nb++;
} else {
nb += mj_vertBodyWeight(m, d, con->flex[side], vid[k],
con->pos, bid+nb, bweight+nb, bw[k]);
}
}
}
// combine weighted Jacobians
NV = mj_jacSum(m, d, chain, nb, bid, bweight, con->pos, jacdif, dim > 3);
}
// skip contact if no DOFs affected
if (NV == 0) {
con->efc_address = -1;
con->exclude = 3;
continue;
}
// rotate Jacobian differences to contact frame
mju_mulMatMat(jac, con->frame, jacdifp, dim > 1 ? 3 : 1, 3, NV);
if (dim > 3) {
mju_mulMatMat(jac + 3*NV, con->frame, jacdifr, dim-3, 3, NV);
}
// make frictionless contact
if (dim == 1) {
// add constraint
mj_addConstraint(m, d, jac, &(con->dist), &(con->includemargin), 0,
1, mjCNSTR_CONTACT_FRICTIONLESS, i,
mj_addConstraint(m, d, jacdifp, cpos, cmargin, 0,
2, mjCNSTR_CONTACT_PYRAMIDAL, i,
issparse ? NV : 0,
issparse ? chain : NULL);
}
}
// make pyramidal friction cone
else if (ispyramid) {
// pos = dist
cpos[0] = cpos[1] = con->dist;
cmargin[0] = cmargin[1] = con->includemargin;
// make elliptic friction cone
else {
// normal pos = dist, all others 0
mju_zero(cpos, con->dim);
mju_zero(cmargin, con->dim);
cpos[0] = con->dist;
cmargin[0] = con->includemargin;
// one pair per friction dimension
for (int k=1; k < con->dim; k++) {
// Jacobian for pair of opposing pyramid edges
mju_addScl(jacdifp, jac, jac + k*NV, con->friction[k-1], NV);
mju_addScl(jacdifp + NV, jac, jac + k*NV, -con->friction[k-1], NV);
// add constraint
mj_addConstraint(m, d, jacdifp, cpos, cmargin, 0,
2, mjCNSTR_CONTACT_PYRAMIDAL, i,
issparse ? NV : 0,
issparse ? chain : NULL);
}
}
// make elliptic friction cone
else {
// normal pos = dist, all others 0
mju_zero(cpos, con->dim);
mju_zero(cmargin, con->dim);
cpos[0] = con->dist;
cmargin[0] = con->includemargin;
// add constraint
mj_addConstraint(m, d, jac, cpos, cmargin, 0,
con->dim, mjCNSTR_CONTACT_ELLIPTIC, i,
issparse ? NV : 0,
issparse ? chain : NULL);
}
// add constraint
mj_addConstraint(m, d, jac, cpos, cmargin, 0,
con->dim, mjCNSTR_CONTACT_ELLIPTIC, i,
issparse ? NV : 0,
issparse ? chain : NULL);
}
}
@@ -1814,6 +1824,13 @@ static int mj_nc(const mjModel* m, mjData* d, int* nnz) {
for (int i=0; i < ncon; i++) {
mjContact* con = d->contact + i;
// skip if passive
if ((con->flex[0] > -1 && m->flex_passive[con->flex[0]]) ||
(con->flex[1] > -1 && m->flex_passive[con->flex[1]])) {
con->efc_address = -1;
con->exclude = 4;
}
// skip if excluded
if (con->exclude) {
continue;
src/engine/engine_core_constraint.h
+7 -1
View File
@@ -65,9 +65,15 @@ void mj_instantiateFriction(const mjModel* m, mjData* d);
// joint and tendon limits
void mj_instantiateLimit(const mjModel* m, mjData* d);
// frictionelss and frictional contacts
// frictionless and frictional contacts
void mj_instantiateContact(const mjModel* m, mjData* d);
// compute Jacobian for contact, return number of DOFs affected
int mj_contactJacobian(const mjModel* m, mjData* d, const mjContact* con, int dim,
mjtNum* jac, mjtNum* jacdif, mjtNum* jacdifp,
mjtNum* jacdifr, mjtNum* jac1p, mjtNum* jac2p,
mjtNum* jac1r, mjtNum* jac2r, int* chain);
//------------------------ parameter computation/extraction ----------------------------------------
src/engine/engine_passive.c
+92 -1
View File
@@ -36,6 +36,9 @@
//----------------------------- passive forces -----------------------------------------------------
// stiffness for passive contacts
static const mjtNum kContactStiffness = 1e4;
// local edge-based vertex indexing for 2D and 3D elements, 2D and 3D elements
// have 3 and 6 edges, respectively so the missing indexes are set to 0
static const int edges[2][6][2] = {{{1, 2}, {2, 0}, {0, 1}, {0, 0}, {0, 0}, {0, 0}},
@@ -515,6 +518,89 @@ static int mj_fluid(const mjModel* m, mjData* d) {
// passive contact forces
int mj_contactPassive(const mjModel* m, mjData* d) {
int ncon = d->ncon, issparse = mj_isSparse(m);
int dim, NV, nv = m->nv, *chain = NULL;
mjtNum *jac, *jacdif, *jacdifp, *jacdifr, *jac1p, *jac2p, *jac1r, *jac2r, *qfrc;
mjContact* con;
int has_contact = 0;
if (mjDISABLED(mjDSBL_CONTACT) || ncon == 0 || nv == 0) {
return 0;
}
// early return if no contact to be included
for (int i=0; i < ncon; i++) {
if (d->contact[i].exclude != 4) {
continue;
}
has_contact = 1;
}
if (!has_contact) {
return 0;
}
// allocate Jacobian
mj_markStack(d);
jac = mjSTACKALLOC(d, 6*nv, mjtNum);
jacdif = mjSTACKALLOC(d, 6*nv, mjtNum);
jacdifp = jacdif;
jacdifr = jacdif + 3*nv;
jac1p = mjSTACKALLOC(d, 3*nv, mjtNum);
jac2p = mjSTACKALLOC(d, 3*nv, mjtNum);
jac1r = mjSTACKALLOC(d, 3*nv, mjtNum);
jac2r = mjSTACKALLOC(d, 3*nv, mjtNum);
qfrc = mjSTACKALLOC(d, nv, mjtNum);
if (issparse) {
chain = mjSTACKALLOC(d, nv, int);
}
// find contacts to be included
for (int i=0; i < ncon; i++) {
if (d->contact[i].exclude != 4) {
continue;
}
// get contact info, safe efc_address
con = d->contact + i;
dim = con->dim;
con->efc_address = -1;
NV = mj_contactJacobian(m, d, con, dim, jac, jacdif, jacdifp, jacdifr,
jac1p, jac2p, jac1r, jac2r, chain);
// skip contact if no DOFs affected
if (NV == 0) {
con->efc_address = -1;
con->exclude = 3;
continue;
}
// rotate Jacobian differences to contact frame
mju_mulMatMat(jac, con->frame, jacdifp, dim > 1 ? 3 : 1, 3, NV);
if (dim > 3) {
mju_mulMatMat(jac + 3*NV, con->frame, jacdifr, dim-3, 3, NV);
}
// compute passive contact force (dim = 1)
mjtNum scl = -kContactStiffness*con->dist;
if (!issparse) {
mju_addToScl(d->qfrc_spring, jac, scl, nv);
} else {
mju_scl(qfrc, jac, scl, NV);
for (int j=0; j < NV; j++) {
d->qfrc_spring[chain[j]] += qfrc[j];
}
}
}
mj_freeStack(d);
return has_contact;
}
// all passive forces
void mj_passive(const mjModel* m, mjData* d) {
int nv = m->nv;
@@ -540,9 +626,14 @@ void mj_passive(const mjModel* m, mjData* d) {
// fluid forces
int has_fluid = mj_fluid(m, d);
// contact forces
mj_contactPassive(m, d);
// add passive forces into qfrc_passive
mju_add(d->qfrc_passive, d->qfrc_spring, d->qfrc_damper, nv);
if (has_fluid) mju_addTo(d->qfrc_passive, d->qfrc_fluid, nv);
if (has_fluid) {
mju_addTo(d->qfrc_passive, d->qfrc_fluid, nv);
}
if (has_gravcomp) {
int njnt = m->njnt;
for (int i=0; i < njnt; i++) {