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

Optimize flex by pinning nodes in empty cells.

Browse Source 
This change introduces an optimization for flexcomp objects defined by a mesh. It identifies grid cells that do not contain any mesh vertices and marks them as empty. Nodes that are exclusively part of empty cells are pinned, preventing them from moving. Stiffness computations are skipped for empty cells, reducing computational cost. The total mass is now distributed only among the non-pinned nodes.

PiperOrigin-RevId: 902565735
Change-Id: Id0a9a685536d5e18a3e42124a25ab08ff3a918f2
This commit is contained in:
Alessio Quaglino
2026-04-20 04:40:34 -07:00
committed by Copybara-Service
parent fa7b36d111
commit 508e581ba9
12 changed files with 455 additions and 90 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_passive.c
+22 -8
View File
@@ -273,6 +273,15 @@ static void mj_springdamper(const mjModel* m, mjData* d) {
for (int ci = 0; ci < cx; ci++) {
for (int cj = 0; cj < cy; cj++) {
for (int ck = 0; ck < cz; ck++) {
// get cell stiffness matrix
mjtNum* k_cell = k + cell_idx * 3*npc * 3*npc;
// skip empty cells (zero stiffness)
if (k_cell[0] == 0) {
cell_idx++;
continue;
}
// gather cell-local node data
mjtNum quat[4];
mju_flexGatherCellState(order, cy, cz, ci, cj, ck, xpos_g, vel_g, xpos0,
@@ -289,9 +298,6 @@ static void mj_springdamper(const mjModel* m, mjData* d) {
mji_addScl3(displ_c+3*n, xpos_c+3*n, xpos0_c+3*n, -1);
}
// get cell stiffness matrix
mjtNum* k_cell = k + cell_idx * 3*npc * 3*npc;
// compute force in corotational frame
if (enbl_spring) {
mju_mulMatVec(frc_c, k_cell, displ_c, 3*npc, 3*npc);
@@ -332,12 +338,20 @@ static void mj_springdamper(const mjModel* m, mjData* d) {
// apply accumulated forces to bodies
for (int i = 0; i < nodenum; i++) {
mju_scl3(dmp_g+3*i, dmp_g+3*i, m->flex_damping[f]);
if (m->flex_centered[f]) {
if (enbl_spring) mji_addTo3(d->qfrc_spring + m->body_dofadr[bodyid[i]], frc_g+3*i);
if (enbl_damper) mji_addTo3(d->qfrc_damper + m->body_dofadr[bodyid[i]], dmp_g+3*i);
int bid = bodyid[i];
int nidx = i + m->flex_nodeadr[f];
// fast path: node at body origin (not pinned), direct DOF write
if (m->body_dofnum[bid] > 0 &&
(m->flex_centered[f] ||
(m->flex_node[3*nidx+0] == 0 &&
m->flex_node[3*nidx+1] == 0 &&
m->flex_node[3*nidx+2] == 0))) {
if (enbl_spring) mji_addTo3(d->qfrc_spring + m->body_dofadr[bid], frc_g+3*i);
if (enbl_damper) mji_addTo3(d->qfrc_damper + m->body_dofadr[bid], dmp_g+3*i);
} else {
if (enbl_spring) mj_applyFT(m, d, frc_g+3*i, 0, xpos_g+3*i, bodyid[i], d->qfrc_spring);
if (enbl_damper) mj_applyFT(m, d, dmp_g+3*i, 0, xpos_g+3*i, bodyid[i], d->qfrc_damper);
if (enbl_spring) mj_applyFT(m, d, frc_g+3*i, 0, xpos_g+3*i, bid, d->qfrc_spring);
if (enbl_damper) mj_applyFT(m, d, dmp_g+3*i, 0, xpos_g+3*i, bid, d->qfrc_damper);
}
}