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

Fast flex: Interpolate contact points directly.

Browse Source 
For each contact point, we used to first compute the vertex weights on a triangle and then add a contact point per vertex using the flex interpolation (trilinear or quadratic), obtaining the total weight by multiplying the vertex weight and the basis function value at that vertex. After this change, each contact point is added directly by evaluating the basis function directly a the point.

PiperOrigin-RevId: 834218909
Change-Id: Ic0fdb03fc0ef5478798c293df99204fd33116fd1
This commit is contained in:
Alessio Quaglino
2025-11-19 03:39:15 -08:00
committed by Copybara-Service
parent a2fd5fdd4a
commit 3aa973894c
1 changed files with 95 additions and 68 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_core_constraint.c
+61 -34
View File
@@ -183,9 +183,16 @@ static int mj_elemBodyWeight(const mjModel* m, const mjData* d, int f, int e, in
// compute body weights for a given contact vertex, return #bodies // compute body weights for a given contact vertex, return #bodies
static int mj_vertBodyWeight(const mjModel* m, const mjData* d, int f, int v, static int mj_vertBodyWeight(const mjModel* m, const mjData* d, int f, int* v,
int* body, mjtNum* weight, mjtNum bw) { int* body, mjtNum* bweight, const mjtNum* vweight, int nw) {
mjtNum* coord = m->flex_vert0 + 3*v; if (nw == 0) {
return 0;
}
mjtNum coord[3] = {0, 0, 0};
for (int i = 0; i < nw; i++) {
mju_addToScl3(coord, m->flex_vert0 + 3*v[i], vweight[i]);
}
int nstart = m->flex_nodeadr[f]; int nstart = m->flex_nodeadr[f];
int nend = m->flex_nodeadr[f] + m->flex_nodenum[f]; int nend = m->flex_nodeadr[f] + m->flex_nodenum[f];
int nb = 0; int nb = 0;
@@ -195,7 +202,7 @@ static int mj_vertBodyWeight(const mjModel* m, const mjData* d, int f, int v,
if (w < 1e-5) { if (w < 1e-5) {
continue; continue;
} }
if (weight) weight[nb] = w * bw; if (bweight) bweight[nb] = w;
body[nb++] = m->flex_nodebodyid[i]; body[nb++] = m->flex_nodebodyid[i];
} }
@@ -899,10 +906,6 @@ int mj_contactJacobian(const mjModel* m, mjData* d, const mjContact* con, int di
int bid[729]; // 729 = 27*27 int bid[729]; // 729 = 27*27
mjtNum bweight[729]; mjtNum bweight[729];
for (int side=0; side < 2; side++) { for (int side=0; side < 2; side++) {
int nw = 0;
int vid[4];
mjtNum bw[4];
// geom // geom
if (con->geom[side] >= 0) { if (con->geom[side] >= 0) {
bid[nb] = m->geom_bodyid[con->geom[side]]; bid[nb] = m->geom_bodyid[con->geom[side]];
@@ -910,32 +913,39 @@ int mj_contactJacobian(const mjModel* m, mjData* d, const mjContact* con, int di
nb++; nb++;
} }
// flex vert // flex
else if (con->vert[side] >= 0) { else {
int nw = 0;
int vid[4];
mjtNum vweight[4];
// vert
if (con->vert[side] >= 0) {
vid[0] = m->flex_vertadr[con->flex[side]] + con->vert[side]; vid[0] = m->flex_vertadr[con->flex[side]] + con->vert[side];
bw[0] = side ? +1 : -1; vweight[0] = side ? +1 : -1;
nw = 1; nw = 1;
} }
// flex elem // elem
else { else {
nw = mj_elemBodyWeight(m, d, con->flex[side], con->elem[side], nw = mj_elemBodyWeight(m, d, con->flex[side], con->elem[side],
con->vert[1-side], con->pos, vid, bw); con->vert[1-side], con->pos, vid, vweight);
// negative sign for first side of contact // negative sign for first side of contact
if (side == 0) { if (side == 0) {
mju_scl(bw, bw, -1, nw); mju_scl(vweight, vweight, -1, nw);
} }
} }
// get body or node ids and weights // get body or node ids and weights
for (int k=0; k < nw; k++) {
if (m->flex_interp[con->flex[side]] == 0) { if (m->flex_interp[con->flex[side]] == 0) {
for (int k=0; k < nw; k++) {
bid[nb] = m->flex_vertbodyid[vid[k]]; bid[nb] = m->flex_vertbodyid[vid[k]];
bweight[nb] = bw[k]; bweight[nb] = vweight[k];
nb++; nb++;
}
} else { } else {
nb += mj_vertBodyWeight(m, d, con->flex[side], vid[k], bid+nb, bweight+nb, bw[k]); nb += mj_vertBodyWeight(m, d, con->flex[side], vid, bid+nb, bweight+nb, vweight, nw);
} }
} }
} }
@@ -1154,8 +1164,8 @@ void mj_diagApprox(const mjModel* m, mjData* d) {
tran = rot = 0; tran = rot = 0;
for (int side=0; side < 2; side++) { for (int side=0; side < 2; side++) {
// get bodies and weights // get bodies and weights
int nb = 0, bid[729], vid[4], nw = 0; int nb = 0, bid[729];
mjtNum bweight[729], bw[4]; mjtNum bweight[729];
// geom // geom
if (con->geom[side] >= 0) { if (con->geom[side] >= 0) {
@@ -1164,27 +1174,34 @@ void mj_diagApprox(const mjModel* m, mjData* d) {
nb = 1; nb = 1;
} }
// flex vert // flex
else if (con->vert[side] >= 0) { else {
int nw = 0;
int vid[4];
mjtNum vweight[4];
// vert
if (con->vert[side] >= 0) {
vid[0] = m->flex_vertadr[con->flex[side]] + con->vert[side]; vid[0] = m->flex_vertadr[con->flex[side]] + con->vert[side];
bw[0] = 1; vweight[0] = 1;
nw = 1; nw = 1;
} }
// flex elem // elem
else { else {
nw = mj_elemBodyWeight(m, d, con->flex[side], con->elem[side], nw = mj_elemBodyWeight(m, d, con->flex[side], con->elem[side],
con->vert[1-side], con->pos, vid, bw); con->vert[1-side], con->pos, vid, vweight);
} }
// get body or node ids and weights // convert verted ids and weights to body ids and weights
for (int k=0; k < nw; k++) {
if (m->flex_interp[con->flex[side]] == 0) { if (m->flex_interp[con->flex[side]] == 0) {
for (int k=0; k < nw; k++) {
bid[k] = m->flex_vertbodyid[vid[k]]; bid[k] = m->flex_vertbodyid[vid[k]];
bweight[k] = bw[k]; bweight[k] = vweight[k];
nb++; nb++;
}
} else { } else {
nb = mj_vertBodyWeight(m, d, con->flex[side], vid[k], bid, bweight, bw[k]); nb += mj_vertBodyWeight(m, d, con->flex[side], vid, bid, bweight, vweight, nw);
} }
} }
@@ -1884,17 +1901,21 @@ static int mj_nc(const mjModel* m, mjData* d, int* nnz) {
// get bodies // get bodies
int nb = 0, bid[729]; int nb = 0, bid[729];
for (int side=0; side < 2; side++) { for (int side=0; side < 2; side++) {
int nw = 0;
int vid[4];
// geom // geom
if (con->geom[side] >= 0) { if (con->geom[side] >= 0) {
bid[nb++] = m->geom_bodyid[con->geom[side]]; bid[nb++] = m->geom_bodyid[con->geom[side]];
} }
// flex
else {
int nw = 0;
int vid[4];
mjtNum vweight[4];
// flex vert // flex vert
else if (con->vert[side] >= 0) { if (con->vert[side] >= 0) {
vid[nw++] = m->flex_vertadr[con->flex[side]] + con->vert[side]; vid[nw++] = m->flex_vertadr[con->flex[side]] + con->vert[side];
vweight[0] = 1;
} }
// flex elem // flex elem
@@ -1905,15 +1926,21 @@ static int mj_nc(const mjModel* m, mjData* d, int* nnz) {
for (int k=0; k <= fdim; k++) { for (int k=0; k <= fdim; k++) {
vid[nw++] = m->flex_vertadr[f] + edata[k]; vid[nw++] = m->flex_vertadr[f] + edata[k];
} }
if (m->flex_interp[f]) {
nw = mj_elemBodyWeight(m, d, con->flex[side], con->elem[side],
con->vert[1-side], con->pos, vid, vweight);
}
} }
// get body or node ids and weights // get body or node ids and weights
for (int k=0; k < nw; k++) {
if (m->flex_interp[con->flex[side]] == 0) { if (m->flex_interp[con->flex[side]] == 0) {
for (int k=0; k < nw; k++) {
bid[nb] = m->flex_vertbodyid[vid[k]]; bid[nb] = m->flex_vertbodyid[vid[k]];
nb++; nb++;
}
} else { } else {
nb += mj_vertBodyWeight(m, d, con->flex[side], vid[k], bid + nb, NULL, 0); nb += mj_vertBodyWeight(m, d, con->flex[side], vid, bid+nb, NULL, vweight, nw);
} }
} }
} }