diff --git a/model/flex/bunny_shell.xml b/model/flex/bunny_shell.xml new file mode 100644 index 00000000..31a614af --- /dev/null +++ b/model/flex/bunny_shell.xml @@ -0,0 +1,43 @@ + + + + + + diff --git a/src/engine/engine_core_constraint.c b/src/engine/engine_core_constraint.c index 52c7feae..aadbbc75 100644 --- a/src/engine/engine_core_constraint.c +++ b/src/engine/engine_core_constraint.c @@ -277,10 +277,18 @@ static int mj_vertBodyWeight(const mjModel* m, const mjData* d, int f, int* v, mju_addToScl3(coord, m->flex_vert0 + 3*v[i], mju_abs(vweight[i])); } - int order = m->flex_interp[f]; - order = order < 0 ? -order : order; + int interp = m->flex_interp[f]; + int order = interp < 0 ? -interp : interp; int npc = (order+1)*(order+1)*(order+1); // number of nodes per cell + // grid dimensions for shell mode + int nx = 0, ny = 0, nz = 0; + if (interp < 0) { + nx = m->flex_cellnum[3*f+0] * order + 1; + ny = m->flex_cellnum[3*f+1] * order + 1; + nz = m->flex_cellnum[3*f+2] * order + 1; + } + // cell lookup: get local coords and node indices mjtNum local[3]; int nodeindices[27]; // max npc for quadratic: 3^3 = 27 @@ -290,14 +298,46 @@ static int mj_vertBodyWeight(const mjModel* m, const mjData* d, int f, int* v, int nstart = m->flex_nodeadr[f]; int nb = 0; + if (!m->flex_nodebodyid) { + return 0; + } + if (npc > 27) { for (int j = 0; j < npc; j++) { mjtNum w = mju_evalBasis(local, j, order); if (w < 1e-5) { continue; } - if (bweight) bweight[nb] = sign * w; - body[nb++] = m->flex_nodebodyid[nstart + nodeindices[j]]; + + int idx = nodeindices[j]; + + // shell mode: map interior nodes to boundary + if (interp < 0) { + int k_idx = idx % nz; + int rest = idx / nz; + int j_idx = rest % ny; + int i_idx = rest / ny; + + if (i_idx > 0 && i_idx < nx-1 && j_idx > 0 && j_idx < ny-1 && k_idx > 0 && k_idx < nz-1) { + mju_shellTFIWeights(nx, ny, nz, i_idx, j_idx, k_idx, sign * w, &nb, body, bweight, m->flex_nodebodyid, nstart); + continue; + } + } + + // add node, check for duplicates (especially needed when combining with TFI) + int b = m->flex_nodebodyid[nstart + idx]; + int found = 0; + for (int k = 0; k < nb; k++) { + if (body[k] == b) { + if (bweight) bweight[k] += sign * w; + found = 1; + break; + } + } + if (!found) { + if (bweight) bweight[nb] = sign * w; + body[nb++] = b; + } } } else { mjtNum basis[27]; @@ -308,11 +348,40 @@ static int mj_vertBodyWeight(const mjModel* m, const mjData* d, int f, int* v, if (w < 1e-5) { continue; } - if (bweight) bweight[nb] = sign * w; - body[nb++] = m->flex_nodebodyid[nstart + nodeindices[j]]; + + int idx = nodeindices[j]; + + // shell mode: map interior nodes to boundary + if (interp < 0) { + int k_idx = idx % nz; + int rest = idx / nz; + int j_idx = rest % ny; + int i_idx = rest / ny; + + if (i_idx > 0 && i_idx < nx-1 && j_idx > 0 && j_idx < ny-1 && k_idx > 0 && k_idx < nz-1) { + mju_shellTFIWeights(nx, ny, nz, i_idx, j_idx, k_idx, sign * w, &nb, body, bweight, m->flex_nodebodyid, nstart); + continue; + } + } + + // add node, check for duplicates (especially needed when combining with TFI) + int b = m->flex_nodebodyid[nstart + idx]; + int found = 0; + for (int k = 0; k < nb; k++) { + if (body[k] == b) { + if (bweight) bweight[k] += sign * w; + found = 1; + break; + } + } + if (!found) { + if (bweight) bweight[nb] = sign * w; + body[nb++] = b; + } } } + return nb; } diff --git a/src/engine/engine_core_constraint.h b/src/engine/engine_core_constraint.h index 484a30ec..da585555 100644 --- a/src/engine/engine_core_constraint.h +++ b/src/engine/engine_core_constraint.h @@ -66,10 +66,10 @@ void mj_instantiateEquality(const mjModel* m, mjData* d); 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); +MJAPI 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 ---------------------------------------- diff --git a/src/engine/engine_core_smooth.c b/src/engine/engine_core_smooth.c index 164a6527..d0baa655 100644 --- a/src/engine/engine_core_smooth.c +++ b/src/engine/engine_core_smooth.c @@ -607,6 +607,11 @@ void mj_flex(const mjModel* m, mjData* d) { mjERROR("flex_interp_order mismatch"); } + // shell mode: reconstruct interior node positions from boundary via TFI + if (interp < 0) { + mju_shellTrackInterior(nodexpos, nx_g, ny_g, nz_g); + } + for (int i=vstart; i < vend; i++) { mju_zero3(d->flexvert_xpos+3*i); diff --git a/src/engine/engine_core_util.c b/src/engine/engine_core_util.c index 52c5d46f..7cedb477 100644 --- a/src/engine/engine_core_util.c +++ b/src/engine/engine_core_util.c @@ -1020,6 +1020,23 @@ void mju_flexGatherState(const mjModel* m, const mjData* d, int f, mjtNum* xpos, mju_addTo3(vel + 3*i, cross); } } + + // shell mode: reconstruct interior node positions and velocities via TFI + int interp = m->flex_interp[f]; + if (interp < 0) { + int order = -interp; + int cx = m->flex_cellnum[3*f+0]; + int cy = m->flex_cellnum[3*f+1]; + int cz = m->flex_cellnum[3*f+2]; + int nx_g = cx * order + 1; + int ny_g = cy * order + 1; + int nz_g = cz * order + 1; + + mju_shellTrackInterior(xpos, nx_g, ny_g, nz_g); + if (vel) { + mju_shellTrackInterior(vel, nx_g, ny_g, nz_g); + } + } } diff --git a/src/engine/engine_util_misc.c b/src/engine/engine_util_misc.c index 65b0d6c2..4a4e1f9a 100644 --- a/src/engine/engine_util_misc.c +++ b/src/engine/engine_util_misc.c @@ -898,6 +898,151 @@ void mju_flexFaceNormal2D(mjtNum normal[3], mjtNum t1[3], mjtNum t2[3], } +// helper: get nodexpos value for node (i,j,k) in an nx*ny*nz grid +static inline const mjtNum* nodeAt(const mjtNum* nodexpos, int ny, int nz, int i, int j, int k) { + return nodexpos + 3*(i*ny*nz + j*nz + k); +} + +// reconstruct interior node positions from boundary nodes via Transfinite Interpolation +void mju_shellTrackInterior(mjtNum* nodexpos, int nx, int ny, int nz) { + // need at least 3 nodes in each direction to have interior nodes + if (nx < 3 || ny < 3 || nz < 3) { + return; + } + + for (int i = 1; i < nx-1; i++) { + for (int j = 1; j < ny-1; j++) { + for (int k = 1; k < nz-1; k++) { + // parametric coordinates in [0, 1] + mjtNum s = (mjtNum)i / (nx-1); + mjtNum t = (mjtNum)j / (ny-1); + mjtNum u = (mjtNum)k / (nz-1); + + mjtNum result[3] = {0, 0, 0}; + + // --- face contributions (bilinear interpolation on each face pair) --- + // x-faces: i=0 and i=nx-1 + for (int d = 0; d < 3; d++) { + result[d] += (1-s) * nodeAt(nodexpos, ny, nz, 0, j, k)[d] + + s * nodeAt(nodexpos, ny, nz, nx-1, j, k)[d]; + } + // y-faces: j=0 and j=ny-1 + for (int d = 0; d < 3; d++) { + result[d] += (1-t) * nodeAt(nodexpos, ny, nz, i, 0, k)[d] + + t * nodeAt(nodexpos, ny, nz, i, ny-1, k)[d]; + } + // z-faces: k=0 and k=nz-1 + for (int d = 0; d < 3; d++) { + result[d] += (1-u) * nodeAt(nodexpos, ny, nz, i, j, 0)[d] + + u * nodeAt(nodexpos, ny, nz, i, j, nz-1)[d]; + } + + // --- edge corrections (subtract 12 edges, each linearly interpolated) --- + // edges along x (4 edges: (j,k) at corners of y-z face) + for (int d = 0; d < 3; d++) { + result[d] -= (1-t)*(1-u) * nodeAt(nodexpos, ny, nz, i, 0, 0)[d]; + result[d] -= (1-t)* u * nodeAt(nodexpos, ny, nz, i, 0, nz-1)[d]; + result[d] -= t *(1-u) * nodeAt(nodexpos, ny, nz, i, ny-1, 0)[d]; + result[d] -= t * u * nodeAt(nodexpos, ny, nz, i, ny-1, nz-1)[d]; + } + // edges along y (4 edges: (i,k) at corners of x-z face) + for (int d = 0; d < 3; d++) { + result[d] -= (1-s)*(1-u) * nodeAt(nodexpos, ny, nz, 0, j, 0)[d]; + result[d] -= (1-s)* u * nodeAt(nodexpos, ny, nz, 0, j, nz-1)[d]; + result[d] -= s *(1-u) * nodeAt(nodexpos, ny, nz, nx-1, j, 0)[d]; + result[d] -= s * u * nodeAt(nodexpos, ny, nz, nx-1, j, nz-1)[d]; + } + // edges along z (4 edges: (i,j) at corners of x-y face) + for (int d = 0; d < 3; d++) { + result[d] -= (1-s)*(1-t) * nodeAt(nodexpos, ny, nz, 0, 0, k)[d]; + result[d] -= (1-s)* t * nodeAt(nodexpos, ny, nz, 0, ny-1, k)[d]; + result[d] -= s *(1-t) * nodeAt(nodexpos, ny, nz, nx-1, 0, k)[d]; + result[d] -= s * t * nodeAt(nodexpos, ny, nz, nx-1, ny-1, k)[d]; + } + + // --- corner corrections (add 8 corners back) --- + for (int d = 0; d < 3; d++) { + result[d] += (1-s)*(1-t)*(1-u) * nodeAt(nodexpos, ny, nz, 0, 0, 0)[d]; + result[d] += (1-s)*(1-t)* u * nodeAt(nodexpos, ny, nz, 0, 0, nz-1)[d]; + result[d] += (1-s)* t *(1-u) * nodeAt(nodexpos, ny, nz, 0, ny-1, 0)[d]; + result[d] += (1-s)* t * u * nodeAt(nodexpos, ny, nz, 0, ny-1, nz-1)[d]; + result[d] += s *(1-t)*(1-u) * nodeAt(nodexpos, ny, nz, nx-1, 0, 0)[d]; + result[d] += s *(1-t)* u * nodeAt(nodexpos, ny, nz, nx-1, 0, nz-1)[d]; + result[d] += s * t *(1-u) * nodeAt(nodexpos, ny, nz, nx-1, ny-1, 0)[d]; + result[d] += s * t * u * nodeAt(nodexpos, ny, nz, nx-1, ny-1, nz-1)[d]; + } + + // write result to interior node + mju_copy3(nodexpos + 3*(i*ny*nz + j*nz + k), result); + } + } + } +} + + +// helper to accumulate weights in a sparse list +static void addWeight(int* nb, int* body, mjtNum* bweight, int b, mjtNum w) { + for (int i = 0; i < *nb; i++) { + if (body[i] == b) { + if (bweight) { + bweight[i] += w; + } + return; + } + } + body[*nb] = b; + if (bweight) { + bweight[*nb] = w; + } + (*nb)++; +} + +// compute TFI weights for an interior node (i,j,k) and distribute to boundary nodes +void mju_shellTFIWeights(int nx, int ny, int nz, int i, int j, int k, + mjtNum w, int* nb, int* body, mjtNum* bweight, + const int* nodebodyid, int nstart) { + mjtNum s = (mjtNum)i / (nx-1); + mjtNum t = (mjtNum)j / (ny-1); + mjtNum u = (mjtNum)k / (nz-1); + + // face contributions + addWeight(nb, body, bweight, nodebodyid[nstart + 0*ny*nz + j*nz + k], w * (1-s)); + addWeight(nb, body, bweight, nodebodyid[nstart + (nx-1)*ny*nz + j*nz + k], w * s); + + addWeight(nb, body, bweight, nodebodyid[nstart + i*ny*nz + 0*nz + k], w * (1-t)); + addWeight(nb, body, bweight, nodebodyid[nstart + i*ny*nz + (ny-1)*nz + k], w * t); + + addWeight(nb, body, bweight, nodebodyid[nstart + i*ny*nz + j*nz + 0], w * (1-u)); + addWeight(nb, body, bweight, nodebodyid[nstart + i*ny*nz + j*nz + (nz-1)], w * u); + + // edge corrections + addWeight(nb, body, bweight, nodebodyid[nstart + i*ny*nz + 0*nz + 0], -w * (1-t)*(1-u)); + addWeight(nb, body, bweight, nodebodyid[nstart + i*ny*nz + 0*nz + (nz-1)], -w * (1-t)*u); + addWeight(nb, body, bweight, nodebodyid[nstart + i*ny*nz + (ny-1)*nz + 0], -w * t*(1-u)); + addWeight(nb, body, bweight, nodebodyid[nstart + i*ny*nz + (ny-1)*nz + (nz-1)], -w * t*u); + + addWeight(nb, body, bweight, nodebodyid[nstart + 0*ny*nz + j*nz + 0], -w * (1-s)*(1-u)); + addWeight(nb, body, bweight, nodebodyid[nstart + 0*ny*nz + j*nz + (nz-1)], -w * (1-s)*u); + addWeight(nb, body, bweight, nodebodyid[nstart + (nx-1)*ny*nz + j*nz + 0], -w * s*(1-u)); + addWeight(nb, body, bweight, nodebodyid[nstart + (nx-1)*ny*nz + j*nz + (nz-1)], -w * s*u); + + addWeight(nb, body, bweight, nodebodyid[nstart + 0*ny*nz + 0*nz + k], -w * (1-s)*(1-t)); + addWeight(nb, body, bweight, nodebodyid[nstart + 0*ny*nz + (ny-1)*nz + k], -w * (1-s)*t); + addWeight(nb, body, bweight, nodebodyid[nstart + (nx-1)*ny*nz + 0*nz + k], -w * s*(1-t)); + addWeight(nb, body, bweight, nodebodyid[nstart + (nx-1)*ny*nz + (ny-1)*nz + k], -w * s*t); + + // corner corrections + addWeight(nb, body, bweight, nodebodyid[nstart + 0*ny*nz + 0*nz + 0], w * (1-s)*(1-t)*(1-u)); + addWeight(nb, body, bweight, nodebodyid[nstart + 0*ny*nz + 0*nz + (nz-1)], w * (1-s)*(1-t)*u); + addWeight(nb, body, bweight, nodebodyid[nstart + 0*ny*nz + (ny-1)*nz + 0], w * (1-s)*t*(1-u)); + addWeight(nb, body, bweight, nodebodyid[nstart + 0*ny*nz + (ny-1)*nz + (nz-1)], w * (1-s)*t*u); + addWeight(nb, body, bweight, nodebodyid[nstart + (nx-1)*ny*nz + 0*nz + 0], w * s*(1-t)*(1-u)); + addWeight(nb, body, bweight, nodebodyid[nstart + (nx-1)*ny*nz + 0*nz + (nz-1)], w * s*(1-t)*u); + addWeight(nb, body, bweight, nodebodyid[nstart + (nx-1)*ny*nz + (ny-1)*nz + 0], w * s*t*(1-u)); + addWeight(nb, body, bweight, nodebodyid[nstart + (nx-1)*ny*nz + (ny-1)*nz + (nz-1)], w * s*t*u); +} + + //------------------------------ actuator models --------------------------------------------------- // normalized muscle length-gain curve diff --git a/src/engine/engine_util_misc.h b/src/engine/engine_util_misc.h index 42aa3704..8617b922 100644 --- a/src/engine/engine_util_misc.h +++ b/src/engine/engine_util_misc.h @@ -147,6 +147,14 @@ static inline mjtNum mju_flexDphi(mjtNum s, int i, int order) { default: return 0; } } +// reconstruct interior node positions from boundary nodes via Transfinite Interpolation +MJAPI void mju_shellTrackInterior(mjtNum* nodexpos, int nx, int ny, int nz); + +// compute TFI weights for an interior node (i,j,k) and distribute to boundary nodes +MJAPI void mju_shellTFIWeights(int nx, int ny, int nz, int i, int j, int k, + mjtNum w, int* nb, int* body, mjtNum* bweight, + const int* nodebodyid, int nstart); + // ----------------------------- Base64 ------------------------------------------------------------ diff --git a/src/engine/engine_vis_interact.c b/src/engine/engine_vis_interact.c index 60533e27..300db93e 100644 --- a/src/engine/engine_vis_interact.c +++ b/src/engine/engine_vis_interact.c @@ -873,14 +873,21 @@ int mjv_select(const mjModel* m, const mjData* d, const mjvOption* vopt, mju_cellLookup(coord, m->flex_cellnum+3*i, order, loc, nodeindices); // find node with largest weight in this cell + // in shell mode, skip interior nodes (pinned to worldbody) int nodeid = -1; int nstart = m->flex_nodeadr[i]; mjtNum w = 0; + int shell_mode = m->flex_interp[i] < 0; for (int j = 0; j < npc; j++) { mjtNum ww = mju_evalBasis(loc, j, order); + int nid = nodeindices[j]; + // skip interior nodes in shell mode (they map to worldbody) + if (shell_mode && m->body_dofnum[m->flex_nodebodyid[nstart + nid]] == 0) { + continue; + } if (ww > w) { w = ww; - nodeid = nodeindices[j]; + nodeid = nid; } } flexbodyid = m->flex_nodebodyid[nstart + nodeid]; diff --git a/src/engine/engine_vis_visualize.c b/src/engine/engine_vis_visualize.c index 28ccb448..2bad628a 100644 --- a/src/engine/engine_vis_visualize.c +++ b/src/engine/engine_vis_visualize.c @@ -1496,6 +1496,8 @@ static void addFlexBvhGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, int NY = cy * order + 1; int NZ = cz * order + 1; + int shell_mode = m->flex_interp[f] < 0; + for (int i=0; i < NX; i++) { for (int j=0; j < NY; j++) { for (int k=0; k < NZ; k++) { @@ -1506,36 +1508,58 @@ static void addFlexBvhGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, continue; } + // shell mode: skip interior nodes entirely + int is_boundary = (i == 0 || i == NX-1 || + j == 0 || j == NY-1 || + k == 0 || k == NZ-1); + if (shell_mode && !is_boundary) { + 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 && m->body_jntnum[bodyid[(i+1)*NY*NZ + j*NZ + k]] > 0) { - mjvGeom* thisgeom = acquireGeom(scn, i, mjCAT_DECOR, mjOBJ_UNKNOWN); - if (!thisgeom) { - return; - } - mjv_connector(thisgeom, mjGEOM_LINE, 3, xpos+offset, xpos+offset1); - releaseGeom(&thisgeom, scn); + // edge along i: draw if neighbor is also on boundary (shell) or has joints + if (i < NX-1 && m->body_jntnum[bodyid[(i+1)*NY*NZ + j*NZ + k]] > 0) { + int nb_boundary = ((i+1) == 0 || (i+1) == NX-1 || + j == 0 || j == NY-1 || + k == 0 || k == NZ-1); + if (!shell_mode || nb_boundary) { + mjvGeom* thisgeom = acquireGeom(scn, i, mjCAT_DECOR, mjOBJ_UNKNOWN); + if (!thisgeom) { + return; + } + mjv_connector(thisgeom, mjGEOM_LINE, 3, xpos+offset, xpos+offset1); + releaseGeom(&thisgeom, scn); + } } 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; + int nb_boundary = (i == 0 || i == NX-1 || + (j+1) == 0 || (j+1) == NY-1 || + k == 0 || k == NZ-1); + if (!shell_mode || nb_boundary) { + mjvGeom* thisgeom = acquireGeom(scn, i, mjCAT_DECOR, mjOBJ_UNKNOWN); + if (!thisgeom) { + return; + } + mjv_connector(thisgeom, mjGEOM_LINE, 3, xpos+offset, xpos+offset2); + releaseGeom(&thisgeom, scn); } - - mjv_connector(thisgeom, mjGEOM_LINE, 3, xpos+offset, xpos+offset2); - releaseGeom(&thisgeom, scn); } 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; + int nb_boundary = (i == 0 || i == NX-1 || + j == 0 || j == NY-1 || + (k+1) == 0 || (k+1) == NZ-1); + if (!shell_mode || nb_boundary) { + mjvGeom* thisgeom = acquireGeom(scn, i, mjCAT_DECOR, mjOBJ_UNKNOWN); + if (!thisgeom) { + return; + } + mjv_connector(thisgeom, mjGEOM_LINE, 3, xpos+offset, xpos+offset3); + releaseGeom(&thisgeom, scn); } - - mjv_connector(thisgeom, mjGEOM_LINE, 3, xpos+offset, xpos+offset3); - releaseGeom(&thisgeom, scn); } } } diff --git a/src/user/user_flexcomp.cc b/src/user/user_flexcomp.cc index f85aa413..4a62d6d3 100644 --- a/src/user/user_flexcomp.cc +++ b/src/user/user_flexcomp.cc @@ -644,8 +644,26 @@ 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); + // mark empty cells and pin nodes exclusively in empty cells (volume mode only) + if (!dflex->elastic2d) { + MarkEmptyCells(flex, point.data(), npnt, minmax, nx, ny, nz); + } + + // shell mode: pin all interior (non-boundary) nodes + if (dflex->elastic2d) { + for (int gi = 0; gi < nx; gi++) { + for (int gj = 0; gj < ny; gj++) { + for (int gk = 0; gk < nz; gk++) { + bool is_boundary = (gi == 0 || gi == nx-1 || + gj == 0 || gj == ny-1 || + gk == 0 || gk == nz-1); + if (!is_boundary) { + pinned[gi*ny*nz + gj*nz + gk] = true; + } + } + } + } + } // if MarkEmptyCells pinned any nodes, force centered=false // so that pf->node (local positions) is saved to the model diff --git a/src/user/user_mesh.cc b/src/user/user_mesh.cc index 17c4c9b6..b1a1e7c3 100644 --- a/src/user/user_mesh.cc +++ b/src/user/user_mesh.cc @@ -4698,11 +4698,7 @@ void mjCFlex::Compile(const mjVFS* vfs) { if (spec.cellcount[0] == 0 || spec.cellcount[1] == 0 || spec.cellcount[2] == 0) { throw mjCError(this, "cellcount cannot be 0 in any dimension when interpolation order > 0"); } - if (elastic2d && !(spec.cellcount[0] == 1 || spec.cellcount[1] == 1 || spec.cellcount[2] == 1)) { - throw mjCError(this, - "shell trilinear flex requires at least one dimension " - "with cell count equal to one (no interior nodes)"); - } + int expected_nodes = (spec.cellcount[0] * spec.order + 1) * (spec.cellcount[1] * spec.order + 1) * (spec.cellcount[2] * spec.order + 1); @@ -4944,9 +4940,9 @@ void mjCFlex::Compile(const mjVFS* vfs) { // create shell fragments and element-vertex collision pairs CreateShellPair(); - // recompute cell_empty from vertex/element geometry + // recompute cell_empty from vertex/element geometry (volume mode only) // (survives XML round-trips where flexcomp data is lost) - if (interpolated && cell_empty.empty()) { + if (interpolated && !elastic2d && cell_empty.empty()) { int cx = spec.cellcount[0], cy = spec.cellcount[1], cz = spec.cellcount[2]; if (cx * cy * cz > 1) { ComputeCellEmpty(vertxpos.data(), elem_.data(), nvert, nelem, dim); diff --git a/test/engine/engine_core_constraint_test.cc b/test/engine/engine_core_constraint_test.cc index 5cd69696..0541bd06 100644 --- a/test/engine/engine_core_constraint_test.cc +++ b/test/engine/engine_core_constraint_test.cc @@ -15,6 +15,7 @@ // Tests for engine/engine_core_constraint.c. #include +#include #include #include @@ -1091,5 +1092,103 @@ INSTANTIATE_TEST_SUITE_P( } ); +TEST_F(CoreConstraintTest, ShellModeContactJacobian) { + constexpr char xml[] = R"( + + + )"; + char error[1024]; + mjModel* model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model, testing::NotNull()) << error; + mjData* data = mj_makeData(model); + + mj_forward(model, data); + + // find central vertex index (13 for 3x3x3 grid) + int central_idx = 13; + + // verify it is interior + int nx = 3, ny = 3, nz = 3; + int k = central_idx / (nx * ny); + int rest = central_idx % (nx * ny); + int j = rest / nx; + int i = rest % nx; + ASSERT_TRUE(i > 0 && i < nx-1 && j > 0 && j < ny-1 && k > 0 && k < nz-1); + + // create manual contact with central vertex + mjContact con; + memset(&con, 0, sizeof(mjContact)); + con.flex[0] = -1; + con.flex[1] = -1; + con.vert[0] = -1; + con.vert[1] = -1; + con.geom[0] = model->ngeom - 1; // plane geom + con.geom[1] = -1; // must be -1 to trigger flex branch in mj_contactJacobian + con.flex[1] = 0; + con.vert[1] = central_idx; + con.dim = 1; + mju_copy3(con.pos, data->flexvert_xpos + 3*central_idx); + con.frame[0] = 0; con.frame[1] = 0; con.frame[2] = 1; // normal + + // buffer for Jacobian + std::vector jacdif(3*model->nv, 0.0); + + // call mj_contactJacobian + mj_contactJacobian(model, data, &con, 1, nullptr, jacdif.data(), + nullptr, nullptr, nullptr, + nullptr, nullptr, nullptr, + nullptr); + + // check that boundary nodes have non-zero entries, and central node has zero + + + bool boundary_has_dof = false; + bool interior_has_dof = false; + + for (int n = 0; n < model->flex_nodenum[0]; n++) { + int b = model->flex_nodebodyid[model->flex_nodeadr[0] + n]; + int dofadr = model->body_dofadr[b]; + int dofnum = model->body_dofnum[b]; + + bool has_jac = false; + if (dofadr >= 0) { + for (int d = 0; d < dofnum; d++) { + if (mju_abs(jacdif[dofadr + d]) > 1e-6) { + has_jac = true; + } + } + } + + int kn = n / (nx * ny); + int restn = n % (nx * ny); + int jn = restn / nx; + int in = restn % nx; + bool is_interior = (in > 0 && in < nx - 1 && jn > 0 && jn < ny - 1 && + kn > 0 && kn < nz - 1); + + if (is_interior) { + if (has_jac) interior_has_dof = true; + } else { + if (has_jac) boundary_has_dof = true; + } + } + + EXPECT_TRUE(boundary_has_dof) + << "Boundary nodes should receive contact force"; + EXPECT_FALSE(interior_has_dof) + << "Interior nodes should not receive contact force"; + + mj_deleteData(data); + mj_deleteModel(model); +} + } // namespace } // namespace mujoco diff --git a/test/engine/engine_core_util_test.cc b/test/engine/engine_core_util_test.cc index 9bd56f7e..d3fd2df7 100644 --- a/test/engine/engine_core_util_test.cc +++ b/test/engine/engine_core_util_test.cc @@ -88,6 +88,55 @@ TEST_F(FlexGatherStateTest, mju_flexGatherState_Grid) { mj_deleteModel(model); } +TEST_F(FlexGatherStateTest, mju_flexGatherState_ShellMode) { + static constexpr char xml[] = R"( + + + + + + + + + )"; + + char error[1024]; + mjModel* model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model, NotNull()) << error; + + ASSERT_EQ(model->nflex, 1); + int f = 0; + model->flex_interp[f] = -1; + + mjData* data = mj_makeData(model); + mj_forward(model, data); + + int nodenum = model->flex_nodenum[f]; + int nstart = model->flex_nodeadr[f]; + + // Move boundary nodes, keep interior node stuck (it is pinned) + mjtNum shift[3] = {0.1, 0.2, 0.3}; + for (int i = 0; i < nodenum; i++) { + if (i == 13) continue; // Skip center node + int b = model->flex_nodebodyid[nstart + i]; + data->xpos[3*b + 0] += shift[0]; + data->xpos[3*b + 1] += shift[1]; + data->xpos[3*b + 2] += shift[2]; + } + + std::vector xpos(3 * nodenum); + mju_flexGatherState(model, data, f, xpos.data(), NULL); + + // Verify that gathered xpos for center node (13) is the TFI reconstructed position + EXPECT_NEAR(xpos[3*13 + 0], shift[0], 1e-5); + EXPECT_NEAR(xpos[3*13 + 1], shift[1], 1e-5); + EXPECT_NEAR(xpos[3*13 + 2], shift[2], 1e-5); + + mj_deleteData(data); + mj_deleteModel(model); +} + using AngMomMatTest = MujocoTest; diff --git a/test/engine/engine_util_misc_test.cc b/test/engine/engine_util_misc_test.cc index 57f36d79..8e1d742b 100644 --- a/test/engine/engine_util_misc_test.cc +++ b/test/engine/engine_util_misc_test.cc @@ -1550,5 +1550,167 @@ TEST_F(FaceStateTest, RotationConsistencyWith3D) { } } +// ------------------------------ Shell TFI Interpolation ---------------------- + +using ShellTFITest = MujocoTest; + +// helper: set up a regular nx*ny*nz grid with positions at grid indices +static void MakeRegularGrid(mjtNum* nodexpos, int nx, int ny, int nz) { + for (int i = 0; i < nx; i++) { + for (int j = 0; j < ny; j++) { + for (int k = 0; k < nz; k++) { + int idx = i*ny*nz + j*nz + k; + nodexpos[3*idx+0] = (mjtNum)i; + nodexpos[3*idx+1] = (mjtNum)j; + nodexpos[3*idx+2] = (mjtNum)k; + } + } + } +} + +TEST_F(ShellTFITest, IdentityGrid) { + // 3x3x3 grid: 1 interior node at (1,1,1) + constexpr int nx = 3, ny = 3, nz = 3; + mjtNum nodexpos[3*nx*ny*nz]; + MakeRegularGrid(nodexpos, nx, ny, nz); + + // save expected interior position + mjtNum expected[3] = {1.0, 1.0, 1.0}; + + // run TFI + mju_shellTrackInterior(nodexpos, nx, ny, nz); + + // interior node at (1,1,1) should match + int idx = 1*ny*nz + 1*nz + 1; + EXPECT_NEAR(nodexpos[3*idx+0], expected[0], MjTol(1e-12, 1e-5)); + EXPECT_NEAR(nodexpos[3*idx+1], expected[1], MjTol(1e-12, 1e-5)); + EXPECT_NEAR(nodexpos[3*idx+2], expected[2], MjTol(1e-12, 1e-5)); +} + +TEST_F(ShellTFITest, UniformScaling) { + // 3x3x3: scale all boundary nodes by 2x, interior should follow + constexpr int nx = 3, ny = 3, nz = 3; + mjtNum nodexpos[3*nx*ny*nz]; + MakeRegularGrid(nodexpos, nx, ny, nz); + + // scale all nodes + for (int i = 0; i < 3*nx*ny*nz; i++) { + nodexpos[i] *= 2.0; + } + + // run TFI — interior should be reconstructed to 2*original + mju_shellTrackInterior(nodexpos, nx, ny, nz); + + int idx = 1*ny*nz + 1*nz + 1; + EXPECT_NEAR(nodexpos[3*idx+0], 2.0, MjTol(1e-12, 1e-5)); + EXPECT_NEAR(nodexpos[3*idx+1], 2.0, MjTol(1e-12, 1e-5)); + EXPECT_NEAR(nodexpos[3*idx+2], 2.0, MjTol(1e-12, 1e-5)); +} + +TEST_F(ShellTFITest, AffineDeformation) { + // 4x4x4 grid with 8 interior nodes. Apply affine transform to boundary, + // then verify TFI reproduces the same affine transform on interior nodes. + constexpr int nx = 4, ny = 4, nz = 4; + mjtNum nodexpos[3*nx*ny*nz]; + MakeRegularGrid(nodexpos, nx, ny, nz); + + // affine: F(x,y,z) = A*[x,y,z]^T + b + // A = [[2, 0.5, 0], [0.3, 1.5, 0], [0, 0, 1]], b = [10, 20, 30] + auto affine = [](mjtNum x, mjtNum y, mjtNum z, mjtNum out[3]) { + out[0] = 2.0*x + 0.5*y + 10.0; + out[1] = 0.3*x + 1.5*y + 20.0; + out[2] = z + 30.0; + }; + + // apply affine to all nodes + for (int i = 0; i < nx; i++) { + for (int j = 0; j < ny; j++) { + for (int k = 0; k < nz; k++) { + int idx = i*ny*nz + j*nz + k; + affine((mjtNum)i, (mjtNum)j, (mjtNum)k, nodexpos + 3*idx); + } + } + } + + // corrupt interior nodes to verify TFI actually reconstructs them + for (int i = 1; i < nx-1; i++) { + for (int j = 1; j < ny-1; j++) { + for (int k = 1; k < nz-1; k++) { + int idx = i*ny*nz + j*nz + k; + nodexpos[3*idx+0] = -999; + nodexpos[3*idx+1] = -999; + nodexpos[3*idx+2] = -999; + } + } + } + + // run TFI + mju_shellTrackInterior(nodexpos, nx, ny, nz); + + // check all interior nodes match affine + for (int i = 1; i < nx-1; i++) { + for (int j = 1; j < ny-1; j++) { + for (int k = 1; k < nz-1; k++) { + int idx = i*ny*nz + j*nz + k; + mjtNum expected[3]; + affine((mjtNum)i, (mjtNum)j, (mjtNum)k, expected); + EXPECT_NEAR(nodexpos[3*idx+0], expected[0], MjTol(1e-12, 1e-4)) + << "i=" << i << " j=" << j << " k=" << k; + EXPECT_NEAR(nodexpos[3*idx+1], expected[1], MjTol(1e-12, 1e-4)) + << "i=" << i << " j=" << j << " k=" << k; + EXPECT_NEAR(nodexpos[3*idx+2], expected[2], MjTol(1e-12, 1e-4)) + << "i=" << i << " j=" << j << " k=" << k; + } + } + } +} + +TEST_F(ShellTFITest, BoundaryUnmodified) { + // verify that boundary nodes are not modified by TFI + constexpr int nx = 4, ny = 4, nz = 4; + mjtNum nodexpos[3*nx*ny*nz]; + MakeRegularGrid(nodexpos, nx, ny, nz); + + // save boundary node values + mjtNum saved[3*nx*ny*nz]; + mju_copy(saved, nodexpos, 3*nx*ny*nz); + + mju_shellTrackInterior(nodexpos, nx, ny, nz); + + // check all boundary nodes unchanged + for (int i = 0; i < nx; i++) { + for (int j = 0; j < ny; j++) { + for (int k = 0; k < nz; k++) { + bool is_boundary = (i == 0 || i == nx-1 || + j == 0 || j == ny-1 || + k == 0 || k == nz-1); + if (is_boundary) { + int idx = i*ny*nz + j*nz + k; + EXPECT_EQ(nodexpos[3*idx+0], saved[3*idx+0]); + EXPECT_EQ(nodexpos[3*idx+1], saved[3*idx+1]); + EXPECT_EQ(nodexpos[3*idx+2], saved[3*idx+2]); + } + } + } + } +} + +TEST_F(ShellTFITest, NoInteriorSmallGrid) { + // 2x2x2 and 2x3x2: no interior nodes, TFI should be a no-op + constexpr int nx = 2, ny = 3, nz = 2; + mjtNum nodexpos[3*nx*ny*nz]; + MakeRegularGrid(nodexpos, nx, ny, nz); + + mjtNum saved[3*nx*ny*nz]; + mju_copy(saved, nodexpos, 3*nx*ny*nz); + + mju_shellTrackInterior(nodexpos, nx, ny, nz); + + // all nodes unchanged + for (int i = 0; i < 3*nx*ny*nz; i++) { + EXPECT_EQ(nodexpos[i], saved[i]); + } +} + } // namespace } // namespace mujoco