diff --git a/src/engine/engine_core_constraint.c b/src/engine/engine_core_constraint.c index 7849836e..0f8f4a00 100644 --- a/src/engine/engine_core_constraint.c +++ b/src/engine/engine_core_constraint.c @@ -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)); diff --git a/src/engine/engine_core_smooth.c b/src/engine/engine_core_smooth.c index 8f3412f1..6b052147 100644 --- a/src/engine/engine_core_smooth.c +++ b/src/engine/engine_core_smooth.c @@ -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]); } diff --git a/src/engine/engine_core_util.c b/src/engine/engine_core_util.c index bf2b6994..52c5d46f 100644 --- a/src/engine/engine_core_util.c +++ b/src/engine/engine_core_util.c @@ -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); } } } diff --git a/src/engine/engine_core_util.h b/src/engine/engine_core_util.h index 949aa257..36ec5fd0 100644 --- a/src/engine/engine_core_util.h +++ b/src/engine/engine_core_util.h @@ -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]); diff --git a/src/engine/engine_derivative.c b/src/engine/engine_derivative.c index f5db7108..875fb70c 100644 --- a/src/engine/engine_derivative.c +++ b/src/engine/engine_derivative.c @@ -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; inflex; 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); } diff --git a/src/engine/engine_passive.c b/src/engine/engine_passive.c index bf59900b..8c76a573 100644 --- a/src/engine/engine_passive.c +++ b/src/engine/engine_passive.c @@ -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); } } diff --git a/src/engine/engine_vis_visualize.c b/src/engine/engine_vis_visualize.c index c0072dbb..327f0108 100644 --- a/src/engine/engine_vis_visualize.c +++ b/src/engine/engine_vis_visualize.c @@ -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; diff --git a/src/user/user_flexcomp.cc b/src/user/user_flexcomp.cc index d4c567d3..6bd14bb0 100644 --- a/src/user/user_flexcomp.cc +++ b/src/user/user_flexcomp.cc @@ -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 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 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; diff --git a/src/user/user_flexcomp.h b/src/user/user_flexcomp.h index 8624be7b..07977a39 100644 --- a/src/user/user_flexcomp.h +++ b/src/user/user_flexcomp.h @@ -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_ diff --git a/src/user/user_mesh.cc b/src/user/user_mesh.cc index 3e448fb5..ae54b634 100644 --- a/src/user/user_mesh.cc +++ b/src/user/user_mesh.cc @@ -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 cell_pos(3 * npc); int local = 0; diff --git a/src/user/user_objects.h b/src/user/user_objects.h index 1f80949d..5c81e40c 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -984,6 +984,7 @@ class mjCFlex_ : public mjCBase { std::vector stiffness; // elasticity stiffness matrix std::vector bending; // bending stiffness matrix bool has_strain_eq = false; // true if strain constraints reference this flex + std::vector cell_empty; // true if cell contains no mesh geometry // variable-size data std::vector vertbody_; // vertex body names diff --git a/test/user/user_flex_test.cc b/test/user/user_flex_test.cc index 5482568d..52e0bb30 100644 --- a/test/user/user_flex_test.cc +++ b/test/user/user_flex_test.cc @@ -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"( + + + + + + + + + )"; + std::array 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 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"( + + + + + + + + + + + + + )"; + std::array 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"( + + + + + + + + )"; + std::array 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"( + + + + + + + + )"; + std::array 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 +