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_core_constraint.c
+4 -1
View File
@@ -743,7 +743,10 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) {
mjtNum* refpos_c = mjSTACKALLOC(d, 3*npc, mjtNum);
for (int n = 0; n < npc; n++) {
int gn = gindices[n];
if (m->flex_centered[f]) {
if (m->flex_centered[f] ||
(m->flex_node[3*(gn + nstart)+0] == 0 &&
m->flex_node[3*(gn + nstart)+1] == 0 &&
m->flex_node[3*(gn + nstart)+2] == 0)) {
mju_copy3(xpos_c + 3*n, d->xpos + 3*bodyid[gn]);
} else {
mju_mulMatVec3(xpos_c + 3*n, d->xmat + 9*bodyid[gn], m->flex_node + 3*(gn + nstart));
src/engine/engine_core_smooth.c
+14 -16
View File
@@ -564,16 +564,13 @@ void mj_flex(const mjModel* m, mjData* d) {
// 0: vertices are the mesh vertices, 1: vertices are interpolated from nodal dofs
if (m->flex_interp[f] == 0) {
// centered: copy body position
if (m->flex_centered[f]) {
for (int i=vstart; i < vend; i++) {
for (int i=vstart; i < vend; i++) {
if (m->flex_centered[f] ||
(m->flex_vert[3*i+0] == 0 &&
m->flex_vert[3*i+1] == 0 &&
m->flex_vert[3*i+2] == 0)) {
mji_copy3(d->flexvert_xpos+3*i, d->xpos+3*m->flex_vertbodyid[i]);
}
}
// non-centered: map from local to global
else {
for (int i=vstart; i < vend; i++) {
} else {
mji_mulMatVec3(d->flexvert_xpos+3*i, d->xmat+9*m->flex_vertbodyid[i], m->flex_vert+3*i);
mji_addTo3(d->flexvert_xpos+3*i, d->xpos+3*m->flex_vertbodyid[i]);
}
@@ -585,13 +582,14 @@ void mj_flex(const mjModel* m, mjData* d) {
int nodenum = nend - nstart;
mj_markStack(d);
mjtNum* nodexpos = mjSTACKALLOC(d, 3*nodenum, mjtNum);
if (m->flex_centered[f]) {
for (int i=nstart; i < nend; i++) {
mji_copy3(nodexpos + 3*(i-nstart), d->xpos + 3*m->flex_nodebodyid[i]);
}
} else {
for (int i=nstart; i < nend; i++) {
int j = i - nstart;
for (int i=nstart; i < nend; i++) {
int j = i - nstart;
if (m->flex_centered[f] ||
(m->flex_node[3*i+0] == 0 &&
m->flex_node[3*i+1] == 0 &&
m->flex_node[3*i+2] == 0)) {
mji_copy3(nodexpos + 3*j, d->xpos + 3*m->flex_nodebodyid[i]);
} else {
mji_mulMatVec3(nodexpos + 3*j, d->xmat + 9*m->flex_nodebodyid[i], m->flex_node + 3*i);
mji_addTo3(nodexpos + 3*j, d->xpos + 3*m->flex_nodebodyid[i]);
}
src/engine/engine_core_util.c
+25 -17
View File
@@ -988,28 +988,36 @@ void mj_local2Global(mjData* d, mjtNum xpos[3], mjtNum xmat[9],
//-------------------------- miscellaneous utilities -----------------------------------------------
// gather global node positions and velocities
void mju_flexGatherState(const mjModel* m, mjData* d, int f, mjtNum* xpos, mjtNum* vel) {
void mju_flexGatherState(const mjModel* m, const mjData* d, int f, mjtNum* xpos, mjtNum* vel) {
int nodenum = m->flex_nodenum[f];
int nstart = m->flex_nodeadr[f];
int* bodyid = m->flex_nodebodyid + m->flex_nodeadr[f];
// compute positions
if (m->flex_centered[f]) {
for (int i=0; i < nodenum; i++) {
mju_copy3(xpos + 3*i, d->xpos + 3*bodyid[i]);
if (vel) {
mju_copy3(vel + 3*i, d->qvel + m->body_dofadr[bodyid[i]]);
}
// compute positions and velocities
for (int i=0; i < nodenum; i++) {
int bid = bodyid[i];
if (m->flex_centered[f] ||
(m->flex_node[3*(i+nstart)+0] == 0 &&
m->flex_node[3*(i+nstart)+1] == 0 &&
m->flex_node[3*(i+nstart)+2] == 0)) {
mju_copy3(xpos + 3*i, d->xpos + 3*bid);
} else {
mju_mulMatVec3(xpos + 3*i, d->xmat + 9*bid, m->flex_node + 3*(i+nstart));
mju_addTo3(xpos + 3*i, d->xpos + 3*bid);
}
} else {
mjtNum screw[6];
for (int i=0; i < nodenum; i++) {
mju_mulMatVec3(xpos + 3*i, d->xmat + 9*bodyid[i], m->flex_node + 3*(i+nstart));
mju_addTo3(xpos + 3*i, d->xpos + 3*bodyid[i]);
if (vel) {
mj_objectVelocity(m, d, mjOBJ_BODY, bodyid[i], screw, 0);
mju_copy3(vel + 3*i, screw + 3);
}
if (vel) {
mjtNum body_vel[6];
mj_objectVelocity(m, d, mjOBJ_BODY, bid, body_vel, 0); // returns [omega, v_CoM] in world frame
// linear velocity at CoM
mju_copy3(vel + 3*i, body_vel + 3);
// add omega x (xpos - xipos)
mjtNum r[3], cross[3];
mju_sub3(r, xpos + 3*i, d->xipos + 3*bid);
mju_cross(cross, body_vel, r);
mju_addTo3(vel + 3*i, cross);
}
}
}
src/engine/engine_core_util.h
+1 -1
View File
@@ -130,7 +130,7 @@ MJAPI void mj_local2Global(mjData* d, mjtNum xpos[3], mjtNum xmat[9],
//-------------------------- miscellaneous ---------------------------------------------------------
// gather global node positions and velocities
MJAPI void mju_flexGatherState(const mjModel* m, mjData* d, int f, mjtNum* xpos, mjtNum* vel);
MJAPI void mju_flexGatherState(const mjModel* m, const mjData* d, int f, mjtNum* xpos, mjtNum* vel);
// extract 6D force:torque for one contact, in contact frame
MJAPI void mj_contactForce(const mjModel* m, const mjData* d, int id, mjtNum result[6]);
src/engine/engine_derivative.c
+62 -36
View File
@@ -880,17 +880,62 @@ static void mjd_flexInterp_kernel(const mjModel* m, mjData* d, mjtFlexOp op,
const int* dof_indices, int ndof, int nband) {
int nv = m->nv;
// build global2local map for ADDH
int* global2local = NULL;
// compute upper bounds across all interpolated flexes
int max_nodenum = 0;
int max_npc = 0;
for (int f = 0; f < m->nflex; f++) {
if (!m->flex_interp[f]) continue;
if (m->flex_rigid[f]) continue;
int order = m->flex_interp[f];
int npc = (order+1)*(order+1)*(order+1);
if (npc > max_npc) max_npc = npc;
if (m->flex_nodenum[f] > max_nodenum) max_nodenum = m->flex_nodenum[f];
}
// nothing to do
if (max_npc == 0) {
return;
}
int max_dim_c = 3 * max_npc;
// single unconditional markStack
mj_markStack(d);
// global2local map for ADDH
int* global2local = mjSTACKALLOC(d, nv, int);
if (op == mjFLEXOP_ADDH) {
mj_markStack(d);
global2local = mjSTACKALLOC(d, nv, int);
mju_fillInt(global2local, -1, nv);
for (int i=0; i<ndof; i++) {
global2local[dof_indices[i]] = i;
}
}
// per-flex node positions (upper bound)
mjtNum* xpos = mjSTACKALLOC(d, 3*max_nodenum, mjtNum);
// per-cell arrays (upper bound)
mjtNum* xpos_c = mjSTACKALLOC(d, 3*max_npc, mjtNum);
mjtNum* K_rot_cell = mjSTACKALLOC(d, max_dim_c*max_dim_c, mjtNum);
// sparse Jacobian for one cell (upper bound)
int* J_rownnz = mjSTACKALLOC(d, max_dim_c, int);
int* J_rowadr = mjSTACKALLOC(d, max_dim_c, int);
mjtNum* J_val = mjSTACKALLOC(d, max_dim_c*nv, mjtNum);
int* J_colind = mjSTACKALLOC(d, max_dim_c*nv, int);
// temp allocations for chain
int* chain_colind = mjSTACKALLOC(d, nv, int);
mjtNum* blk_jac = mjSTACKALLOC(d, 3*nv, mjtNum);
// ADDH-specific allocations (upper bound)
mjtNum* J_reduced = NULL;
mjtNum* KJ = NULL;
if (op == mjFLEXOP_ADDH) {
J_reduced = mjSTACKALLOC(d, max_dim_c*ndof, mjtNum);
KJ = mjSTACKALLOC(d, max_dim_c*ndof, mjtNum);
}
// loop over flexes
for (int f=0; f < m->nflex; f++) {
// only process flex_interp
@@ -899,10 +944,10 @@ static void mjd_flexInterp_kernel(const mjModel* m, mjData* d, mjtFlexOp op,
}
// get stiffness and damping
mjtNum* k = m->flex_stiffness + m->flex_stiffnessadr[f];
mjtNum* K = m->flex_stiffness + m->flex_stiffnessadr[f];
// skip if rigid or no stiffness
if (m->flex_rigid[f] || k[0] == 0) {
if (m->flex_rigid[f] || K[0] == 0) {
continue;
}
@@ -926,27 +971,9 @@ static void mjd_flexInterp_kernel(const mjModel* m, mjData* d, mjtFlexOp op,
int cy = m->flex_cellnum[3*f+1];
int cz = m->flex_cellnum[3*f+2];
int nodenum = m->flex_nodenum[f];
int* bodyid = m->flex_nodebodyid + m->flex_nodeadr[f];
// standard stack allocation
mj_markStack(d);
mjtNum* xpos = mjSTACKALLOC(d, 3*nodenum, mjtNum);
// per-cell arrays
int dim_c = 3 * npc;
mjtNum* xpos_c = mjSTACKALLOC(d, 3*npc, mjtNum);
mjtNum* K_rot_cell = mjSTACKALLOC(d, dim_c*dim_c, mjtNum);
// sparse Jacobian for one cell
int* J_rownnz = mjSTACKALLOC(d, dim_c, int);
int* J_rowadr = mjSTACKALLOC(d, dim_c, int);
mjtNum* J_val = mjSTACKALLOC(d, dim_c*nv, mjtNum);
int* J_colind = mjSTACKALLOC(d, dim_c*nv, int);
// temp allocations for chain
int* chain_colind = mjSTACKALLOC(d, nv, int);
mjtNum* blk_jac = mjSTACKALLOC(d, 3*nv, mjtNum);
// gather raw node positions (unrotated)
mju_flexGatherState(m, d, f, xpos, NULL);
@@ -956,6 +983,16 @@ static void mjd_flexInterp_kernel(const mjModel* m, mjData* d, mjtFlexOp op,
for (int ci = 0; ci < cx; ci++) {
for (int cj = 0; cj < cy; cj++) {
for (int ck = 0; ck < cz; ck++) {
// get cell stiffness
mjtNum* k_cell = K + cell_idx * 3*npc * 3*npc;
// skip empty cells: stiffness buffer is zero-initialized at compile time
// (user_model.cc), and non-empty cells have strictly positive diagonal
if (k_cell[0] == 0) {
cell_idx++;
continue;
}
// gather cell-local node positions
int gindices[125]; // max npc = 125 for quadratic
mjtNum quat[4];
@@ -967,9 +1004,6 @@ static void mjd_flexInterp_kernel(const mjModel* m, mjData* d, mjtFlexOp op,
mju_quat2Mat(R, quat);
mju_transpose(RT, R, 3, 3);
// get cell stiffness
mjtNum* k_cell = k + cell_idx * 3*npc * 3*npc;
// compute K_rot_cell = RT * K_cell * R (block-wise)
mju_zero(K_rot_cell, dim_c*dim_c);
for (int a = 0; a < npc; a++) {
@@ -1025,9 +1059,7 @@ static void mjd_flexInterp_kernel(const mjModel* m, mjData* d, mjtFlexOp op,
addJTBJ_mulSparse(m, d, res, vec, J_rownnz, J_rowadr, J_colind,
J_val, K_rot_cell, dim_c);
} else if (op == mjFLEXOP_ADDH) {
mj_markStack(d);
// H -= J_cell^T * K_rot_cell * J_cell (banded format)
mjtNum* J_reduced = mjSTACKALLOC(d, dim_c*ndof, mjtNum);
mju_zero(J_reduced, dim_c*ndof);
for (int i = 0; i < dim_c; i++) {
@@ -1043,7 +1075,6 @@ static void mjd_flexInterp_kernel(const mjModel* m, mjData* d, mjtFlexOp op,
}
// KJ = K_rot_cell * J_reduced (dim_c x ndof)
mjtNum* KJ = mjSTACKALLOC(d, dim_c*ndof, mjtNum);
mju_mulMatMat(KJ, K_rot_cell, J_reduced, dim_c, dim_c, ndof);
// H[i,j] -= J_reduced[k,i] * KJ[k,j], store lower triangle in banded format
@@ -1056,20 +1087,15 @@ static void mjd_flexInterp_kernel(const mjModel* m, mjData* d, mjtFlexOp op,
res[i*nband + nband-1-(i-j)] -= val;
}
}
mj_freeStack(d);
}
cell_idx++;
}
}
}
mj_freeStack(d);
}
if (op == mjFLEXOP_ADDH) {
mj_freeStack(d); // free global2local
}
mj_freeStack(d);
}
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);
}
}
src/engine/engine_vis_visualize.c
+11 -4
View File
@@ -1459,11 +1459,18 @@ static void addFlexBvhGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
for (int i=0; i < NX; i++) {
for (int j=0; j < NY; j++) {
for (int k=0; k < NZ; k++) {
int offset = 3*(i*NY*NZ + j*NZ + k);
int n0 = i*NY*NZ + j*NZ + k;
// skip if this node is pinned (no joints on its body)
if (m->body_jntnum[bodyid[n0]] == 0) {
continue;
}
int offset = 3*n0;
int offset1 = 3*((i+1)*NY*NZ + j*NZ + k);
int offset2 = 3*(i*NY*NZ + (j+1)*NZ + k);
int offset3 = 3*(i*NY*NZ + j*NZ + (k+1));
if (i < NX-1) {
if (i < NX-1 && m->body_jntnum[bodyid[(i+1)*NY*NZ + j*NZ + k]] > 0) {
mjvGeom* thisgeom = acquireGeom(scn, i, mjCAT_DECOR, mjOBJ_UNKNOWN);
if (!thisgeom) {
return;
@@ -1472,7 +1479,7 @@ static void addFlexBvhGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
mjv_connector(thisgeom, mjGEOM_LINE, 3, xpos+offset, xpos+offset1);
releaseGeom(&thisgeom, scn);
}
if (j < NY-1) {
if (j < NY-1 && m->body_jntnum[bodyid[i*NY*NZ + (j+1)*NZ + k]] > 0) {
mjvGeom* thisgeom = acquireGeom(scn, i, mjCAT_DECOR, mjOBJ_UNKNOWN);
if (!thisgeom) {
return;
@@ -1481,7 +1488,7 @@ static void addFlexBvhGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
mjv_connector(thisgeom, mjGEOM_LINE, 3, xpos+offset, xpos+offset2);
releaseGeom(&thisgeom, scn);
}
if (k < NZ-1) {
if (k < NZ-1 && m->body_jntnum[bodyid[i*NY*NZ + j*NZ + (k+1)]] > 0) {
mjvGeom* thisgeom = acquireGeom(scn, i, mjCAT_DECOR, mjOBJ_UNKNOWN);
if (!thisgeom) {
return;
src/user/user_flexcomp.cc
+108 -7
View File
@@ -98,6 +98,71 @@ mjCFlexcomp::mjCFlexcomp(void) {
}
// identify empty cells and pin nodes exclusively in empty cells
void mjCFlexcomp::MarkEmptyCells(mjCFlex* flex, const double* points,
int npnt, const double minmax[6],
int nx, int ny, int nz) {
int cx = flex->spec.cellcount[0];
int cy = flex->spec.cellcount[1];
int cz = flex->spec.cellcount[2];
int ncells = cx * cy * cz;
int order = flex->spec.order;
// determine which cells contain mesh vertices
flex->cell_empty.assign(ncells, true);
for (int i = 0; i < npnt; i++) {
// compute parametric coordinates of mesh vertex in [0, 1]^3
// for flat meshes (zero extent along an axis), default to 0.5
double dx = minmax[3] - minmax[0];
double dy = minmax[4] - minmax[1];
double dz = minmax[5] - minmax[2];
double sx = dx > 0 ? (points[3*i+0] - minmax[0]) / dx : 0.5;
double sy = dy > 0 ? (points[3*i+1] - minmax[1]) / dy : 0.5;
double sz = dz > 0 ? (points[3*i+2] - minmax[2]) / dz : 0.5;
// find containing cell
int ci = std::min((int)(sx * cx), cx - 1);
int cj = std::min((int)(sy * cy), cy - 1);
int ck = std::min((int)(sz * cz), cz - 1);
ci = std::max(ci, 0);
cj = std::max(cj, 0);
ck = std::max(ck, 0);
flex->cell_empty[ci * cy * cz + cj * cz + ck] = false;
}
// pin nodes that belong exclusively to empty cells
for (int gi = 0; gi < nx; gi++) {
for (int gj = 0; gj < ny; gj++) {
for (int gk = 0; gk < nz; gk++) {
// find all cells that reference this node
bool all_empty = true;
int ci_min = std::max(0, gi == 0 ? 0 : (gi - 1) / order);
int ci_max = std::min(cx - 1, gi / order);
int cj_min = std::max(0, gj == 0 ? 0 : (gj - 1) / order);
int cj_max = std::min(cy - 1, gj / order);
int ck_min = std::max(0, gk == 0 ? 0 : (gk - 1) / order);
int ck_max = std::min(cz - 1, gk / order);
for (int ci = ci_min; ci <= ci_max && all_empty; ci++) {
for (int cj = cj_min; cj <= cj_max && all_empty; cj++) {
for (int ck = ck_min; ck <= ck_max && all_empty; ck++) {
if (!flex->cell_empty[ci * cy * cz + cj * cz + ck]) {
all_empty = false;
}
}
}
}
if (all_empty) {
int idx = gi * ny * nz + gj * nz + gk;
pinned[idx] = true;
}
}
}
}
}
// make flexcomp object
bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz, const mjVFS* vfs) {
@@ -588,15 +653,30 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz, const mjVFS* vf
int nz = flex->spec.cellcount[2] * flex->spec.order + 1;
int nnode = nx * ny * nz;
// mark empty cells and pin nodes exclusively in empty cells
MarkEmptyCells(flex, point.data(), npnt, minmax, nx, ny, nz);
// if MarkEmptyCells pinned any nodes, force centered=false
// so that pf->node (local positions) is saved to the model
if (centered) {
for (int i = 0; i < nnode; i++) {
if (pinned[i]) {
centered = false;
break;
}
}
}
std::vector<double> node(3 * nnode, 0);
int idx = 0;
// Simpson's rule weights for quadratic mass distribution
double massP2[3] = {1. / 6., 2. / 3., 1. / 6.};
// compute per-node mass for trilinear:
// mass / nnode (uniform), or use Simpson for quadratic
double node_mass_uniform = mass / nnode;
// collect created bodies for mass normalization
std::vector<mjsBody*> node_bodies;
for (int gi = 0; gi < nx; gi++) {
for (int gj = 0; gj < ny; gj++) {
@@ -629,7 +709,7 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz, const mjVFS* vf
// mass distribution
if (doftype == mjFCOMPDOF_TRILINEAR) {
pb->mass = node_mass_uniform;
pb->mass = 1.0;
} else {
// local index within the cell for mass computation
int li = gi % flex->spec.order;
@@ -639,14 +719,15 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz, const mjVFS* vf
int ncells_i = (gi > 0 && gi < nx-1 && li == 0) ? 2 : 1;
int ncells_j = (gj > 0 && gj < ny-1 && lj == 0) ? 2 : 1;
int ncells_k = (gk > 0 && gk < nz-1 && lk == 0) ? 2 : 1;
// use Simpson weights scaled by cell count
// use Simpson weights
double wi = massP2[li == 0 ? 0 : li];
double wj = massP2[lj == 0 ? 0 : lj];
double wk = massP2[lk == 0 ? 0 : lk];
pb->mass = mass * wi * wj * wk * ncells_i * ncells_j * ncells_k
/ (flex->spec.cellcount[0] * flex->spec.cellcount[1] * flex->spec.cellcount[2]);
pb->mass = wi * wj * wk * ncells_i * ncells_j * ncells_k;
}
node_bodies.push_back(pb);
pb->inertia[0] = pb->mass*(2.0*inertiabox*inertiabox)/3.0;
pb->inertia[1] = pb->mass*(2.0*inertiabox*inertiabox)/3.0;
pb->inertia[2] = pb->mass*(2.0*inertiabox*inertiabox)/3.0;
@@ -671,6 +752,21 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz, const mjVFS* vf
}
}
// normalize masses so total equals prescribed mass
double total_mass = 0;
for (mjsBody* pb : node_bodies) {
total_mass += pb->mass;
}
if (total_mass > 0) {
double scale = mass / total_mass;
for (mjsBody* pb : node_bodies) {
pb->mass *= scale;
pb->inertia[0] *= scale;
pb->inertia[1] *= scale;
pb->inertia[2] *= scale;
}
}
if (!centered) {
mjs_setDouble(pf->node, node.data(), node.size());
}
@@ -698,6 +794,11 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz, const mjVFS* vf
for (int ci = 0; ci < cell_cx; ci++) {
for (int cj = 0; cj < cell_cy; cj++) {
for (int ck = 0; ck < cell_cz; ck++) {
// skip empty cells
if (!flex->cell_empty.empty() &&
flex->cell_empty[ci * cell_cy * cell_cz + cj * cell_cz + ck]) {
continue;
}
mjsEquality* pe = mjs_addEquality(&model->spec, &def.spec);
mjs_setDefault(pe->element, &model->Default()->spec);
pe->type = mjEQ_FLEXSTRAIN;
src/user/user_flexcomp.h
+5
View File
@@ -116,6 +116,11 @@ class mjCFlexcomp {
std::string plugin_name;
std::string plugin_instance_name;
mjsPlugin plugin;
private:
// identify empty cells and pin nodes exclusively in empty cells
void MarkEmptyCells(mjCFlex* flex, const double* points, int npnt,
const double minmax[6], int nx, int ny, int nz);
};
#endif // MUJOCO_SRC_USER_USER_FLEXCOMP_H_
src/user/user_mesh.cc
+5
View File
@@ -4407,6 +4407,11 @@ void mjCFlex::Compile(const mjVFS* vfs) {
for (int ck = 0; ck < cz; ck++) {
int cell_idx = ci * cy * cz + cj * cz + ck;
// skip stiffness computation for empty cells (no mesh content)
if (!cell_empty.empty() && cell_empty[cell_idx]) {
continue;
}
// gather cell's local node positions
std::vector<double> cell_pos(3 * npc);
int local = 0;
src/user/user_objects.h
+1
View File
@@ -984,6 +984,7 @@ class mjCFlex_ : public mjCBase {
std::vector<double> stiffness; // elasticity stiffness matrix
std::vector<double> bending; // bending stiffness matrix
bool has_strain_eq = false; // true if strain constraints reference this flex
std::vector<bool> cell_empty; // true if cell contains no mesh geometry
// variable-size data
std::vector<std::string> vertbody_; // vertex body names
test/user/user_flex_test.cc
+197
View File
@@ -1031,5 +1031,202 @@ TEST_F(UserFlexTest, FlexNoConstraintsWarning) {
mj_deleteModel(m);
}
TEST_F(UserFlexTest, EmptyCellNodePinning) {
// A 2x2x2 grid with a box mesh that fills all cells.
// No nodes should be pinned.
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<flexcomp name="test" type="box" spacing=".1 .1 .1" dim="3"
dof="trilinear" mass="1" cellcount="2 2 2">
<contact selfcollide="none"/>
<elasticity young="1"/>
</flexcomp>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(m, NotNull()) << error.data();
// A 2x2x2 grid with trilinear order has (2+1)^3 = 27 node positions.
int nadr = m->flex_nodeadr[0];
int nnode = m->flex_nodenum[0];
EXPECT_EQ(nnode, 27);
// All cells are occupied by the box, so no node should be pinned.
int pinned = 0;
for (int n = nadr; n < nadr + nnode; n++) {
int bid = m->flex_nodebodyid[n];
if (m->body_jntnum[bid] == 0) {
pinned++;
}
}
EXPECT_EQ(pinned, 0);
// Verify simulation works
mjData* d = mj_makeData(m);
for (int i = 0; i < 10; i++) {
mj_step(m, d);
}
mj_deleteData(d);
mj_deleteModel(m);
}
TEST_F(UserFlexTest, EmptyCellNodePinningMesh) {
// Load bunny_multicell.xml which has a 3x3x3 grid.
// The bunny mesh only occupies some cells, so many nodes should be pinned.
const std::string xml_path =
GetModelPath("flex/bunny_multicell.xml");
std::array<char, 1024> error;
mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size());
ASSERT_THAT(m, NotNull()) << error.data();
// 3x3x3 grid, order=1: (3+1)^3 = 64 node positions
int nadr = m->flex_nodeadr[0];
int nnode = m->flex_nodenum[0];
EXPECT_EQ(nnode, 64);
// Count pinned nodes (no joints)
int pinned = 0;
int free_nodes = 0;
for (int n = nadr; n < nadr + nnode; n++) {
int bid = m->flex_nodebodyid[n];
if (m->body_jntnum[bid] == 0) {
pinned++;
} else {
free_nodes++;
}
}
// At least some nodes should be pinned since the bunny doesn't fill all cells
EXPECT_GT(pinned, 0) << "Expected some nodes to be pinned from empty cells";
EXPECT_GT(free_nodes, 0) << "Expected some nodes to remain free";
EXPECT_EQ(pinned + free_nodes, nnode);
// Verify the model can simulate
mjData* d = mj_makeData(m);
mj_forward(m, d);
for (int i = 0; i < 10; i++) {
mj_step(m, d);
}
mj_deleteData(d);
mj_deleteModel(m);
}
TEST_F(UserFlexTest, EmptyCellNodePinningQuadratic) {
// Regression test for ci_min calculation with order=2.
// A 2x1x1 quadratic grid has nodes at gi=0..4 (5 nodes per axis).
// We place mesh vertices only in cell 0 (x in [0, 0.5]), so cell 1 is empty.
//
// Node gi=3 belongs only to cell 1 (1*2 <= 3 <= 2*2).
// With the old formula (gi-order)/order = (3-2)/2 = 0, it would also check
// cell 0 (non-empty), incorrectly marking gi=3 as non-pinned.
// Single hex element at x=[0,0.3], well inside cell 0 of a 3x1x1 grid.
// Anchor vertex at x=1.0 extends the bounding box to [0,1]^3.
// The 3x1x1 quadratic grid splits at x=0.33, 0.67.
// Cell 0 has vertices, cells 1 and 2 are empty.
// Interior nodes for cells 1,2 should be pinned to the parent body.
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body name="parent">
<freejoint/>
<inertial mass="0.01" pos="0 0 0"
diaginertia="0.001 0.001 0.001"/>
<flexcomp name="test" type="direct" dim="3"
dof="quadratic" mass="1" cellcount="3 1 1"
point="0.0 0.0 0.0 0.3 0.0 0.0
0.0 1.0 0.0 0.3 1.0 0.0
0.0 0.0 1.0 0.3 0.0 1.0
0.0 1.0 1.0 0.3 1.0 1.0
1.0 0.5 0.5"
element="0 1 3 2 4 5 7 6">
<contact selfcollide="none"/>
<elasticity young="1"/>
</flexcomp>
</body>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(m, NotNull()) << error.data();
// 3x1x1 quadratic grid: (3*2+1) * (1*2+1) * (1*2+1) = 7*3*3 = 63 nodes
int nadr = m->flex_nodeadr[0];
int nnode = m->flex_nodenum[0];
EXPECT_EQ(nnode, 63);
// Count pinned nodes: pinned nodes are assigned to the parent body.
int parent_bid = mj_name2id(m, mjOBJ_BODY, "parent");
ASSERT_GT(parent_bid, 0);
int pinned = 0;
for (int n = nadr; n < nadr + nnode; n++) {
if (m->flex_nodebodyid[n] == parent_bid) {
pinned++;
}
}
// Cells 1 and 2 are empty, so nodes exclusively in those cells are pinned.
// Nodes at gi=3..6 (with any gj, gk) are only in cells 1 and/or 2.
// That's 4 * 3 * 3 = 36 nodes.
EXPECT_EQ(pinned, 36);
mj_deleteData(mj_makeData(m));
mj_deleteModel(m);
}
TEST_F(UserFlexTest, TotalMassTrilinear) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<flexcomp name="test" type="grid" count="2 2 2" spacing="1 1 1"
dim="3" dof="trilinear" mass="1.5">
<contact selfcollide="none" internal="false"/>
</flexcomp>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(m, NotNull()) << error.data();
double total_mass = 0;
for (int i = 1; i < m->nbody; ++i) {
total_mass += m->body_mass[i];
}
EXPECT_NEAR(total_mass, 1.5, 1e-5);
mj_deleteModel(m);
}
TEST_F(UserFlexTest, TotalMassQuadratic) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<flexcomp name="test" type="grid" count="3 2 2" spacing="1 1 1"
dim="3" dof="quadratic" mass="2.0">
<contact selfcollide="none" internal="false"/>
</flexcomp>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(m, NotNull()) << error.data();
double total_mass = 0;
for (int i = 1; i < m->nbody; ++i) {
total_mass += m->body_mass[i];
}
EXPECT_NEAR(total_mass, 2.0, 1e-5);
mj_deleteModel(m);
}
} // namespace
} // namespace mujoco