From 9c6a4f76eb84bedd3f32d765001c40d1764cfd72 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Wed, 29 Apr 2026 10:16:38 -0700 Subject: [PATCH] Add 2D membrane elasticity for interpolated flex shell mode When elastic2d="stretch" is set on an interpolated flexcomp, treat the bounding box boundary as membrane elements rather than volumetric cells. This computes plane-stress stiffness over the boundary faces and updates the runtime force/derivative kernels accordingly. Interior vertex tracking (moving vertices that follow the deforming shell) is not yet implemented so all mesh vertices need to be on the bounding box surface or the background grid should have no interior nodes (i.e. cellcount should be 1 on at least one axis). PiperOrigin-RevId: 907654080 Change-Id: I51b90e2f6a1d1b036f9604e42de20e377dc5d3f9 --- doc/XMLreference.rst | 3 +- doc/changelog.rst | 2 + model/flex/hollow_vs_solid.xml | 102 ++++++ src/engine/engine_core_constraint.c | 262 +++++++++----- src/engine/engine_derivative.c | 222 ++++++------ src/engine/engine_passive.c | 137 ++++---- src/engine/engine_util_misc.c | 127 +++++++ src/engine/engine_util_misc.h | 13 + src/user/user_flexcomp.cc | 52 ++- src/user/user_mesh.cc | 259 +++++++++++--- src/user/user_model.cc | 60 +++- test/engine/engine_core_constraint_test.cc | 40 +++ test/engine/engine_passive_test.cc | 36 ++ test/engine/engine_util_misc_test.cc | 385 +++++++++++++++++++++ test/user/user_mesh_test.cc | 27 +- 15 files changed, 1378 insertions(+), 349 deletions(-) create mode 100644 model/flex/hollow_vs_solid.xml diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index 1bc3c846..55c220b7 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -4328,7 +4328,8 @@ stress-strain relationship. See also :ref:`deformable ` objects and :at:`elastic2d`: :at-val:`[none, bend, stretch, both], "none"` Elastic contribution to passive forces of 2D flexes. "none": none, "bend": bending only, "stretch": stretching only, - "both": bending and stretching. Not yet supported by :ref:`dof` **trilinear** and **quadratic**. + "both": bending and stretching. Bending is not yet supported by :ref:`dof` **trilinear** and + **quadratic**. .. _flex-contact: diff --git a/doc/changelog.rst b/doc/changelog.rst index bb1c7a20..1f5e1fd1 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -6,6 +6,8 @@ Upcoming version (not yet released) ----------------------------------- - Added island support for the :ref:`PGS solver`. +- Added support for :ref:`elastic2d` for trilinear and quadratic flex + :ref:`dofs`. Python ^^^^^^ diff --git a/model/flex/hollow_vs_solid.xml b/model/flex/hollow_vs_solid.xml new file mode 100644 index 00000000..709fc114 --- /dev/null +++ b/model/flex/hollow_vs_solid.xml @@ -0,0 +1,102 @@ + + + + + + diff --git a/src/engine/engine_core_constraint.c b/src/engine/engine_core_constraint.c index 39996c33..b0285a33 100644 --- a/src/engine/engine_core_constraint.c +++ b/src/engine/engine_core_constraint.c @@ -706,11 +706,12 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) { break; case mjEQ_FLEXSTRAIN: { - // each constraint represents a single cell; cell index in eq_data + // each constraint represents a single element (3D cell or 2D face) int f = id[0]; int nodenum = m->flex_nodenum[f]; - int order = m->flex_interp[f]; - order = order < 0 ? -order : order; + int interp = m->flex_interp[f]; + int order = interp < 0 ? -interp : interp; + int shell_mode = (interp < 0); // skip if not interpolated (order == 0 or no nodes) if (!order || !nodenum) { @@ -722,60 +723,93 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) { mjERROR("flex strain constraints only support order 1 and 2, got %d", order); } - int npc = (order+1)*(order+1)*(order+1); + 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 nstart = m->flex_nodeadr[f]; int* bodyid = m->flex_nodebodyid + nstart; - // read cell index from eq_data - int ci = (int)data[0]; - int cj = (int)data[1]; - int ck = (int)data[2]; + // nodes per element and element index + int npe; + int elem_idx; + if (shell_mode) { + npe = (order+1) * (order+1); + elem_idx = (int)data[0]; // face element index + } else { + npe = (order+1) * (order+1) * (order+1); + int ci = (int)data[0]; + int cj = (int)data[1]; + int ck = (int)data[2]; + elem_idx = ci * cy * cz + cj * cz + ck; + } mj_markStack(d); - // get cell node indices + // get element node indices int gindices[125]; // max npc = 125 for quadratic - mju_flexGatherCellState(order, cy, cz, ci, cj, ck, - NULL, NULL, NULL, NULL, NULL, NULL, gindices, NULL); + if (shell_mode) { + mju_flexGatherFaceState(order, cx, cy, cz, elem_idx, + NULL, NULL, NULL, NULL, NULL, NULL, gindices, NULL); + } else { + int ci = (int)data[0], cj = (int)data[1], ck = (int)data[2]; + mju_flexGatherCellState(order, cy, cz, ci, cj, ck, + NULL, NULL, NULL, NULL, NULL, NULL, gindices, NULL); + } - // compute positions only for cell nodes (npc << nodenum) - mjtNum* xpos_c = mjSTACKALLOC(d, 3*npc, mjtNum); - mjtNum* refpos_c = mjSTACKALLOC(d, 3*npc, mjtNum); - for (int n = 0; n < npc; n++) { + // compute positions only for element nodes (npe << nodenum) + mjtNum* xpos_e = mjSTACKALLOC(d, 3*npe, mjtNum); + mjtNum* refpos_e = mjSTACKALLOC(d, 3*npe, mjtNum); + for (int n = 0; n < npe; n++) { int gn = gindices[n]; 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]); + mju_copy3(xpos_e + 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)); - mju_addTo3(xpos_c + 3*n, d->xpos + 3*bodyid[gn]); + mju_mulMatVec3(xpos_e + 3*n, d->xmat + 9*bodyid[gn], m->flex_node + 3*(gn + nstart)); + mju_addTo3(xpos_e + 3*n, d->xpos + 3*bodyid[gn]); } - mju_copy3(refpos_c + 3*n, m->flex_node0 + 3*(gn + nstart)); + mju_copy3(refpos_e + 3*n, m->flex_node0 + 3*(gn + nstart)); } - // compute corotational quaternion from cell-local positions - mjtNum cell_quat[4] = {1, 0, 0, 0}; - { + // compute corotational quaternion + mjtNum elem_quat[4] = {1, 0, 0, 0}; + if (shell_mode) { + // determine face normal axis from elem_idx + int face_sizes[6] = {cy*cz, cy*cz, cx*cz, cx*cz, cx*cy, cx*cy}; + int face_normals[6] = {0, 0, 1, 1, 2, 2}; + int cumul = 0, normal_axis = 0; + for (int ff = 0; ff < 6; ff++) { + if (elem_idx < cumul + face_sizes[ff]) { + normal_axis = face_normals[ff]; + break; + } + cumul += face_sizes[ff]; + } + int na0 = (normal_axis + 1) % 3; + int na1 = (normal_axis + 2) % 3; + + // compute corotational rotation from 2D deformation gradient at face center + mjtNum p[2] = {.5, .5}; + mju_flexInterpRotation2D(order, xpos_e, npe, na0, na1, normal_axis, p, elem_quat); + } else { mjtNum center[3] = {0.5, 0.5, 0.5}; mjtNum mat[9]; - mju_defGradient(mat, center, xpos_c, order); - mju_mat2Rot(cell_quat, mat); - mju_negQuat(cell_quat, cell_quat); + mju_defGradient(mat, center, xpos_e, order); + mju_mat2Rot(elem_quat, mat); + mju_negQuat(elem_quat, elem_quat); } - // build per-cell sparse chain and node Jacobians - int* cell_chain = mjSTACKALLOC(d, nv, int); - int cell_nnz = 0; - mjtNum* cell_node_jac = cell_pos_and_jac(m, d, f, npc, gindices, nv, xpos_c, cell_chain, - &cell_nnz); + // build per-element sparse chain and node Jacobians + int* elem_chain = mjSTACKALLOC(d, nv, int); + int elem_nnz = 0; + mjtNum* elem_node_jac = cell_pos_and_jac(m, d, f, npe, gindices, nv, xpos_e, elem_chain, + &elem_nnz); - mjtNum* strain_jac = mjSTACKALLOC(d, cell_nnz, mjtNum); - mjtNum* dSdx_local = mjSTACKALLOC(d, 3*npc, mjtNum); + mjtNum* strain_jac = mjSTACKALLOC(d, elem_nnz, mjtNum); + mjtNum* dSdx_local = mjSTACKALLOC(d, 3*npe, mjtNum); // for dense mode: allocate and zero a dense Jacobian buffer once mjtNum* dense_jac = NULL; @@ -785,58 +819,55 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) { } // read eigenmode data from flex_stiffness - int ndof_cell = 3 * npc; - int cell_idx = ci * m->flex_cellnum[3*f+1] * m->flex_cellnum[3*f+2] - + cj * m->flex_cellnum[3*f+2] + ck; - const mjtNum* k_cell = m->flex_stiffness + m->flex_stiffnessadr[f] - + cell_idx * ndof_cell * ndof_cell; - int neig = (int)k_cell[0]; + int ndof_elem = 3 * npe; + const mjtNum* k_elem = m->flex_stiffness + m->flex_stiffnessadr[f] + + elem_idx * ndof_elem * ndof_elem; + int neig = (int)k_elem[0]; // compute displacement in corotational frame - mjtNum* displ_c = mjSTACKALLOC(d, ndof_cell, mjtNum); - for (int n = 0; n < npc; n++) { - // rotate xpos_c to corotational frame + mjtNum* displ_e = mjSTACKALLOC(d, ndof_elem, mjtNum); + for (int n = 0; n < npe; n++) { + // rotate xpos_e to corotational frame mjtNum xrot[3]; - mju_rotVecQuat(xrot, xpos_c + 3*n, cell_quat); - displ_c[3*n + 0] = xrot[0] - refpos_c[3*n + 0]; - displ_c[3*n + 1] = xrot[1] - refpos_c[3*n + 1]; - displ_c[3*n + 2] = xrot[2] - refpos_c[3*n + 2]; + mju_rotVecQuat(xrot, xpos_e + 3*n, elem_quat); + displ_e[3*n + 0] = xrot[0] - refpos_e[3*n + 0]; + displ_e[3*n + 1] = xrot[1] - refpos_e[3*n + 1]; + displ_e[3*n + 2] = xrot[2] - refpos_e[3*n + 2]; } // compute inverse quaternion for rotating eigenvectors to world frame - mjtNum cell_quat_inv[4]; - mju_negQuat(cell_quat_inv, cell_quat); + mjtNum elem_quat_inv[4]; + mju_negQuat(elem_quat_inv, elem_quat); // loop over eigenmodes for (int eig = 0; eig < neig; eig++) { - const mjtNum* eigvec = k_cell + 1 + eig * ndof_cell; + const mjtNum* eigvec = k_elem + 1 + eig * ndof_elem; // constraint residual: dot product of scaled eigenvector with displacement mjtNum residual = 0; - for (int j = 0; j < ndof_cell; j++) { - residual += eigvec[j] * displ_c[j]; + for (int j = 0; j < ndof_elem; j++) { + residual += eigvec[j] * displ_e[j]; } cpos[0] = residual; // rotate eigenvector to world frame for Jacobian - // dSdx_local[3*n+c] = Σ_d R_inv[c][d] * eigvec[3*n+d] - for (int n = 0; n < npc; n++) { - mju_rotVecQuat(dSdx_local + 3*n, eigvec + 3*n, cell_quat_inv); + for (int n = 0; n < npe; n++) { + mju_rotVecQuat(dSdx_local + 3*n, eigvec + 3*n, elem_quat_inv); } - // contract with cell_node_jac to get sparse Jacobian - cell_strain_jacobian(npc, cell_nnz, dSdx_local, cell_node_jac, strain_jac); + // contract with elem_node_jac to get sparse Jacobian + cell_strain_jacobian(npe, elem_nnz, dSdx_local, elem_node_jac, strain_jac); if (issparse) { mj_addConstraint(m, d, strain_jac, cpos, 0, 0, 1, mjCNSTR_EQUALITY, i, - cell_nnz, cell_chain); + elem_nnz, elem_chain); } else { - for (int k = 0; k < cell_nnz; k++) { - dense_jac[cell_chain[k]] = strain_jac[k]; + for (int k = 0; k < elem_nnz; k++) { + dense_jac[elem_chain[k]] = strain_jac[k]; } mj_addConstraint(m, d, dense_jac, cpos, 0, 0, 1, mjCNSTR_EQUALITY, i, 0, NULL); - for (int k = 0; k < cell_nnz; k++) { - dense_jac[cell_chain[k]] = 0; + for (int k = 0; k < elem_nnz; k++) { + dense_jac[elem_chain[k]] = 0; } } } @@ -1674,36 +1705,56 @@ void mj_diagApprox(const mjModel* m, mjData* d) { break; case mjEQ_FLEXSTRAIN: { - // strain constraints: per-cell, use avg inv weight of cell's npc nodes + // strain constraints: use avg inv weight of element's nodes int flex_id = m->eq_obj1id[id]; int nstart = m->flex_nodeadr[flex_id]; - int order = m->flex_interp[flex_id]; - order = order < 0 ? -order : order; - int npc = (order+1)*(order+1)*(order+1); + int interp = m->flex_interp[flex_id]; + int order = interp < 0 ? -interp : interp; + int is_shell = (interp < 0); - // per-cell constraint count - int nquad = order + 1; - int ngauss = nquad * nquad * nquad; - int nconstraint = (order == 1) ? (2 + 3 * ngauss) : (6 * ngauss); - - // get cell index from eq_data - int eq_id = d->efc_id[i]; - int ci_cell = (int)m->eq_data[mjNEQDATA*eq_id + 0]; - int cj_cell = (int)m->eq_data[mjNEQDATA*eq_id + 1]; - int ck_cell = (int)m->eq_data[mjNEQDATA*eq_id + 2]; + int cx = m->flex_cellnum[3*flex_id+0]; int cy = m->flex_cellnum[3*flex_id+1]; int cz = m->flex_cellnum[3*flex_id+2]; + // nodes per element + int npe; + int elem_idx; + if (is_shell) { + npe = (order+1) * (order+1); + elem_idx = (int)m->eq_data[mjNEQDATA*id + 0]; + } else { + npe = (order+1) * (order+1) * (order+1); + int ci_cell = (int)m->eq_data[mjNEQDATA*id + 0]; + int cj_cell = (int)m->eq_data[mjNEQDATA*id + 1]; + int ck_cell = (int)m->eq_data[mjNEQDATA*id + 2]; + elem_idx = ci_cell * cy * cz + cj_cell * cz + ck_cell; + } + + // read neig from flex_stiffness + int ndof_elem = 3 * npe; + const mjtNum* k_elem = m->flex_stiffness + m->flex_stiffnessadr[flex_id] + + elem_idx * ndof_elem * ndof_elem; + int nconstraint = (int)k_elem[0]; + + // get element node indices int gindices[125]; - mju_flexGatherCellState(order, cy, cz, ci_cell, cj_cell, ck_cell, - NULL, NULL, NULL, NULL, NULL, NULL, gindices, NULL); + if (is_shell) { + mju_flexGatherFaceState(order, cx, cy, cz, elem_idx, + NULL, NULL, NULL, NULL, NULL, NULL, gindices, NULL); + } else { + int ci_cell = (int)m->eq_data[mjNEQDATA*id + 0]; + int cj_cell = (int)m->eq_data[mjNEQDATA*id + 1]; + int ck_cell = (int)m->eq_data[mjNEQDATA*id + 2]; + mju_flexGatherCellState(order, cy, cz, ci_cell, cj_cell, ck_cell, + NULL, NULL, NULL, NULL, NULL, NULL, gindices, NULL); + } mjtNum avg_invweight = 0; - for (int n = 0; n < npc; n++) { + for (int n = 0; n < npe; n++) { int bodyid = m->flex_nodebodyid[nstart + gindices[n]]; avg_invweight += m->body_invweight0[2*bodyid]; } - avg_invweight /= npc; + avg_invweight /= npe; for (int c = 0; c < nconstraint; c++) { dA[i++] = avg_invweight; } @@ -2296,37 +2347,56 @@ static int mj_ne(const mjModel* m, mjData* d, int* nnz) { break; case mjEQ_FLEXSTRAIN: { - // per-cell strain constraints: each equality is one cell + // per-element strain constraints: each equality is one cell or face int f = id[0]; - int order = m->flex_interp[f]; - order = order < 0 ? -order : order; + int interp = m->flex_interp[f]; + int order = interp < 0 ? -interp : interp; + int is_shell = (interp < 0); if (!order || !m->flex_nodenum[f]) { break; } - int npc = (order+1)*(order+1)*(order+1); - // read eigenmode count from flex_stiffness - int ndof_cell = 3 * npc; - int ci_cell = (int)m->eq_data[mjNEQDATA*i + 0]; - int cj_cell = (int)m->eq_data[mjNEQDATA*i + 1]; - int ck_cell = (int)m->eq_data[mjNEQDATA*i + 2]; + 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 cell_idx = ci_cell * cy * cz + cj_cell * cz + ck_cell; - const mjtNum* k_cell = m->flex_stiffness + m->flex_stiffnessadr[f] - + cell_idx * ndof_cell * ndof_cell; - size = (int)k_cell[0]; // neig stored as first element + + int npe; + int elem_idx; + if (is_shell) { + npe = (order+1) * (order+1); + elem_idx = (int)m->eq_data[mjNEQDATA*i + 0]; + } else { + npe = (order+1) * (order+1) * (order+1); + int ci_cell = (int)m->eq_data[mjNEQDATA*i + 0]; + int cj_cell = (int)m->eq_data[mjNEQDATA*i + 1]; + int ck_cell = (int)m->eq_data[mjNEQDATA*i + 2]; + elem_idx = ci_cell * cy * cz + cj_cell * cz + ck_cell; + } + + // read eigenmode count from flex_stiffness + int ndof_elem = 3 * npe; + const mjtNum* k_elem = m->flex_stiffness + m->flex_stiffnessadr[f] + + elem_idx * ndof_elem * ndof_elem; + size = (int)k_elem[0]; // neig stored as first element if (nnz) { - // get the npc node body IDs for this cell + // get element node body IDs int gindices[125]; - mju_flexGatherCellState(order, cy, cz, ci_cell, cj_cell, ck_cell, - NULL, NULL, NULL, NULL, NULL, NULL, gindices, NULL); + if (is_shell) { + mju_flexGatherFaceState(order, cx, cy, cz, elem_idx, + NULL, NULL, NULL, NULL, NULL, NULL, gindices, NULL); + } else { + int ci_cell = (int)m->eq_data[mjNEQDATA*i + 0]; + int cj_cell = (int)m->eq_data[mjNEQDATA*i + 1]; + int ck_cell = (int)m->eq_data[mjNEQDATA*i + 2]; + mju_flexGatherCellState(order, cy, cz, ci_cell, cj_cell, ck_cell, + NULL, NULL, NULL, NULL, NULL, NULL, gindices, NULL); + } int nstart = m->flex_nodeadr[f]; - for (int n = 0; n < npc; n++) { + for (int n = 0; n < npe; n++) { cell_bodies[n] = m->flex_nodebodyid[nstart + gindices[n]]; } - NV = mj_jacSumCount(m, d, chain, npc, cell_bodies); // npc nodes only + NV = mj_jacSumCount(m, d, chain, npe, cell_bodies); NV = size * NV; } break; diff --git a/src/engine/engine_derivative.c b/src/engine/engine_derivative.c index 60c80073..3728a239 100644 --- a/src/engine/engine_derivative.c +++ b/src/engine/engine_derivative.c @@ -882,23 +882,29 @@ static void mjd_flexInterp_kernel(const mjModel* m, mjData* d, mjtFlexOp op, // compute upper bounds across all interpolated flexes int max_nodenum = 0; - int max_npc = 0; + int max_npe = 0; // max nodes per element (3D cell or 2D face) 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 shell_mode = order < 0; order = order < 0 ? -order : order; - int npc = (order+1)*(order+1)*(order+1); - if (npc > max_npc) max_npc = npc; + int npe; + if (shell_mode) { + npe = (order+1)*(order+1); + } else { + npe = (order+1)*(order+1)*(order+1); + } + if (npe > max_npe) max_npe = npe; if (m->flex_nodenum[f] > max_nodenum) max_nodenum = m->flex_nodenum[f]; } // nothing to do - if (max_npc == 0) { + if (max_npe == 0) { return; } - int max_dim_c = 3 * max_npc; + int max_dim_c = 3 * max_npe; // single unconditional markStack mj_markStack(d); @@ -915,8 +921,8 @@ static void mjd_flexInterp_kernel(const mjModel* m, mjData* d, mjtFlexOp op, // 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); + // per-element arrays (upper bound) + mjtNum* xpos_c = mjSTACKALLOC(d, 3*max_npe, mjtNum); mjtNum* K_rot_cell = mjSTACKALLOC(d, max_dim_c*max_dim_c, mjtNum); // sparse Jacobian for one cell (upper bound) @@ -967,131 +973,141 @@ static void mjd_flexInterp_kernel(const mjModel* m, mjData* d, mjtFlexOp op, } int order = m->flex_interp[f]; + int shell_mode = order < 0; order = order < 0 ? -order : order; - int npc = (order+1)*(order+1)*(order+1); 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* bodyid = m->flex_nodebodyid + m->flex_nodeadr[f]; - int dim_c = 3 * npc; + // determine element type: 2D boundary quads (shell) or 3D cells (volume) + int npe; + int nelem_fe; + if (shell_mode) { + npe = (order+1)*(order+1); + nelem_fe = 2*(cy*cz + cx*cz + cx*cy); + } else { + npe = (order+1)*(order+1)*(order+1); + nelem_fe = cx * cy * cz; + } + int dim_e = 3 * npe; // gather raw node positions (unrotated) mju_flexGatherState(m, d, f, xpos, NULL); - // loop over cells - int cell_idx = 0; - 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; + // loop over finite elements + for (int fe = 0; fe < nelem_fe; fe++) { + // get element stiffness + mjtNum* k_elem = K + fe * 3*npe * 3*npe; - // 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; - } + // skip empty elements: stiffness buffer is zero-initialized at compile time + // (user_model.cc), and non-empty elements have strictly positive diagonal + if (k_elem[0] == 0) { + continue; + } - // gather cell-local node positions - int gindices[125]; // max npc = 125 for quadratic - mjtNum quat[4]; - mju_flexGatherCellState(order, cy, cz, ci, cj, ck, xpos, NULL, NULL, - xpos_c, NULL, NULL, gindices, quat); + // gather element-local node positions + int gindices[125]; // max npe = 125 for quadratic 3D + mjtNum quat[4]; + if (shell_mode) { + mju_flexGatherFaceState(order, cx, cy, cz, fe, xpos, NULL, NULL, + xpos_c, NULL, NULL, gindices, quat); + } else { + int ci = fe / (cy * cz); + int cj = (fe / cz) % cy; + int ck = fe % cz; + mju_flexGatherCellState(order, cy, cz, ci, cj, ck, xpos, NULL, NULL, + xpos_c, NULL, NULL, gindices, quat); + } - // R = R_global2local, RT = R_local2global - mjtNum R[9], RT[9]; - mju_quat2Mat(R, quat); - mju_transpose(RT, R, 3, 3); + // R = R_global2local, RT = R_local2global + mjtNum R[9], RT[9]; + mju_quat2Mat(R, quat); + mju_transpose(RT, R, 3, 3); - // 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++) { - for (int b = 0; b < npc; b++) { - mjtNum blk[9], tmp[9]; + // compute K_rot = RT * K_elem * R (block-wise) + mju_zero(K_rot_cell, dim_e*dim_e); + for (int a = 0; a < npe; a++) { + for (int b = 0; b < npe; b++) { + mjtNum blk[9], tmp[9]; - // get K_cell(a,b) 3x3 block - int adr_cell = (3*a)*(3*npc) + 3*b; - for (int r = 0; r < 3; r++) { - for (int c = 0; c < 3; c++) { - blk[3*r+c] = k_cell[adr_cell + r*(3*npc) + c]; - } - } - - // tmp = K * R - mju_mulMatMat3(tmp, blk, R); - // blk = RT * tmp = RT * K * R - mju_mulMatMat3(blk, RT, tmp); - - // store in K_rot_cell at (a, b) - int adr_out = (3*a)*dim_c + 3*b; - for (int r = 0; r < 3; r++) { - for (int c = 0; c < 3; c++) { - K_rot_cell[adr_out + r*dim_c + c] = scale * blk[3*r+c]; - } - } + // get K_elem(a,b) 3x3 block + int adr_cell = (3*a)*(3*npe) + 3*b; + for (int r = 0; r < 3; r++) { + for (int c = 0; c < 3; c++) { + blk[3*r+c] = k_elem[adr_cell + r*(3*npe) + c]; } } - // construct sparse Jacobian for this cell's nodes - int current_adr = 0; - for (int n = 0; n < npc; n++) { - int bid = bodyid[gindices[n]]; - int chain_nnz = mj_bodyChain(m, bid, chain_colind); - mj_jacSparse(m, d, blk_jac, NULL, xpos+3*gindices[n], bid, - chain_nnz, chain_colind, /*flg_skipcommon=*/0); + // tmp = K * R + mju_mulMatMat3(tmp, blk, R); + // blk = RT * tmp = RT * K * R + mju_mulMatMat3(blk, RT, tmp); - for (int r = 0; r < 3; r++) { - int row_idx = 3*n + r; - J_rownnz[row_idx] = chain_nnz; - J_rowadr[row_idx] = current_adr; - - for (int idx = 0; idx < chain_nnz; idx++) { - J_colind[current_adr] = chain_colind[idx]; - J_val[current_adr] = blk_jac[r*chain_nnz + idx]; - current_adr++; - } + // store in K_rot_cell at (a, b) + int adr_out = (3*a)*dim_e + 3*b; + for (int r = 0; r < 3; r++) { + for (int c = 0; c < 3; c++) { + K_rot_cell[adr_out + r*dim_e + c] = scale * blk[3*r+c]; } } + } + } - // apply operation with cell's K_rot and J - if (op == mjFLEXOP_VEC) { - addJTBJ_mulSparse(m, d, res, vec, J_rownnz, J_rowadr, J_colind, - J_val, K_rot_cell, dim_c); - } else if (op == mjFLEXOP_ADDH) { - // H -= J_cell^T * K_rot_cell * J_cell (banded format) - mju_zero(J_reduced, dim_c*ndof); + // construct sparse Jacobian for this element's nodes + int current_adr = 0; + for (int n = 0; n < npe; n++) { + int bid = bodyid[gindices[n]]; + int chain_nnz = mj_bodyChain(m, bid, chain_colind); + mj_jacSparse(m, d, blk_jac, NULL, xpos+3*gindices[n], bid, + chain_nnz, chain_colind, /*flg_skipcommon=*/0); - for (int i = 0; i < dim_c; i++) { - int nnz = J_rownnz[i]; - int adr = J_rowadr[i]; - for (int idx = 0; idx < nnz; idx++) { - int global_col = J_colind[adr + idx]; - int local_idx = global2local[global_col]; - if (local_idx >= 0) { - J_reduced[i*ndof + local_idx] = J_val[adr + idx]; - } - } - } + for (int r = 0; r < 3; r++) { + int row_idx = 3*n + r; + J_rownnz[row_idx] = chain_nnz; + J_rowadr[row_idx] = current_adr; - // KJ = K_rot_cell * J_reduced (dim_c x ndof) - mju_mulMatMat(KJ, K_rot_cell, J_reduced, dim_c, dim_c, ndof); + for (int idx = 0; idx < chain_nnz; idx++) { + J_colind[current_adr] = chain_colind[idx]; + J_val[current_adr] = blk_jac[r*chain_nnz + idx]; + current_adr++; + } + } + } - // H[i,j] -= J_reduced[k,i] * KJ[k,j], store lower triangle in banded format - for (int i = 0; i < ndof; i++) { - for (int j = mjMAX(0, i-nband+1); j <= i; j++) { - mjtNum val = 0; - for (int dim_idx = 0; dim_idx < dim_c; dim_idx++) { - val += J_reduced[dim_idx*ndof + i] * KJ[dim_idx*ndof + j]; - } - res[i*nband + nband-1-(i-j)] -= val; - } + // apply operation with element's K_rot and J + if (op == mjFLEXOP_VEC) { + addJTBJ_mulSparse(m, d, res, vec, J_rownnz, J_rowadr, J_colind, + J_val, K_rot_cell, dim_e); + } else if (op == mjFLEXOP_ADDH) { + // H -= J_elem^T * K_rot * J_elem (banded format) + mju_zero(J_reduced, dim_e*ndof); + + for (int i = 0; i < dim_e; i++) { + int nnz = J_rownnz[i]; + int adr = J_rowadr[i]; + for (int idx = 0; idx < nnz; idx++) { + int global_col = J_colind[adr + idx]; + int local_idx = global2local[global_col]; + if (local_idx >= 0) { + J_reduced[i*ndof + local_idx] = J_val[adr + idx]; } } + } - cell_idx++; + // KJ = K_rot * J_reduced (dim_e x ndof) + mju_mulMatMat(KJ, K_rot_cell, J_reduced, dim_e, dim_e, ndof); + + // H[i,j] -= J_reduced[k,i] * KJ[k,j], store lower triangle in banded format + for (int i = 0; i < ndof; i++) { + for (int j = mjMAX(0, i-nband+1); j <= i; j++) { + mjtNum val = 0; + for (int dim_idx = 0; dim_idx < dim_e; dim_idx++) { + val += J_reduced[dim_idx*ndof + i] * KJ[dim_idx*ndof + j]; + } + res[i*nband + nband-1-(i-j)] -= val; + } } } } diff --git a/src/engine/engine_passive.c b/src/engine/engine_passive.c index 295ec4a7..f8d2ba42 100644 --- a/src/engine/engine_passive.c +++ b/src/engine/engine_passive.c @@ -236,13 +236,23 @@ static void mj_springdamper(const mjModel* m, mjData* d) { if (m->flex_interp[f]) { int order = m->flex_interp[f]; + int shell_mode = order < 0; order = order < 0 ? -order : order; - int npc = (order+1)*(order+1)*(order+1); // nodes per cell 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 ny_g = cy * order + 1; - int nz_g = cz * order + 1; + + // determine element type: 2D boundary quads (shell) or 3D cells (volume) + int npe; // nodes per element + int nelem_fe; // total finite elements + + if (shell_mode) { + npe = (order+1)*(order+1); + nelem_fe = 2*(cy*cz + cx*cz + cx*cy); + } else { + npe = (order+1)*(order+1)*(order+1); + nelem_fe = cx * cy * cz; + } mj_markStack(d); @@ -261,77 +271,70 @@ static void mj_springdamper(const mjModel* m, mjData* d) { mju_zero(frc_g, 3*nodenum); mju_zero(dmp_g, 3*nodenum); - // per-cell arrays - mjtNum* xpos_c = mjSTACKALLOC(d, 3*npc, mjtNum); - mjtNum* vel_c = mjSTACKALLOC(d, 3*npc, mjtNum); - mjtNum* xpos0_c = mjSTACKALLOC(d, 3*npc, mjtNum); - mjtNum* displ_c = mjSTACKALLOC(d, 3*npc, mjtNum); - mjtNum* frc_c = mjSTACKALLOC(d, 3*npc, mjtNum); - mjtNum* dmp_c = mjSTACKALLOC(d, 3*npc, mjtNum); + // per-element arrays (sized for npe) + mjtNum* xpos_e = mjSTACKALLOC(d, 3*npe, mjtNum); + mjtNum* vel_e = mjSTACKALLOC(d, 3*npe, mjtNum); + mjtNum* xpos0_e = mjSTACKALLOC(d, 3*npe, mjtNum); + mjtNum* displ_e = mjSTACKALLOC(d, 3*npe, mjtNum); + mjtNum* frc_e = mjSTACKALLOC(d, 3*npe, mjtNum); + mjtNum* dmp_e = mjSTACKALLOC(d, 3*npe, mjtNum); + int* gindices = mjSTACKALLOC(d, npe, int); - // loop over cells - int cell_idx = 0; - 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; + // loop over finite elements + for (int fe = 0; fe < nelem_fe; fe++) { + // get element stiffness matrix + mjtNum* k_elem = k + fe * 3*npe * 3*npe; - // skip empty cells (zero stiffness) - if (k_cell[0] == 0) { - cell_idx++; - continue; - } + // skip empty elements (zero stiffness) + if (k_elem[0] == 0) { + continue; + } - // gather cell-local node data - mjtNum quat[4]; - mju_flexGatherCellState(order, cy, cz, ci, cj, ck, xpos_g, vel_g, xpos0, - xpos_c, vel_c, xpos0_c, NULL, quat); + // gather element-local node data and compute corotational rotation + mjtNum quat[4]; + if (shell_mode) { + mju_flexGatherFaceState(order, cx, cy, cz, fe, xpos_g, vel_g, xpos0, + xpos_e, vel_e, xpos0_e, gindices, quat); + } else { + int ci = fe / (cy * cz); + int cj = (fe / cz) % cy; + int ck = fe % cz; + mju_flexGatherCellState(order, cy, cz, ci, cj, ck, xpos_g, vel_g, + xpos0, xpos_e, vel_e, xpos0_e, gindices, + quat); + } - // rotate to corotational frame - for (int n = 0; n < npc; n++) { - mju_rotVecQuat(xpos_c+3*n, xpos_c+3*n, quat); - mju_rotVecQuat(vel_c+3*n, vel_c+3*n, quat); - } + // rotate to corotational frame + for (int n = 0; n < npe; n++) { + mju_rotVecQuat(xpos_e+3*n, xpos_e+3*n, quat); + mju_rotVecQuat(vel_e+3*n, vel_e+3*n, quat); + } - // compute displacement - for (int n = 0; n < npc; n++) { - mji_addScl3(displ_c+3*n, xpos_c+3*n, xpos0_c+3*n, -1); - } + // compute displacement + for (int n = 0; n < npe; n++) { + mji_addScl3(displ_e+3*n, xpos_e+3*n, xpos0_e+3*n, -1); + } - // compute force in corotational frame - if (enbl_spring) { - mju_mulMatVec(frc_c, k_cell, displ_c, 3*npc, 3*npc); - } - if (enbl_damper) { - mju_mulMatVec(dmp_c, k_cell, vel_c, 3*npc, 3*npc); - } + // compute force in corotational frame + if (enbl_spring) { + mju_mulMatVec(frc_e, k_elem, displ_e, 3*npe, 3*npe); + } + if (enbl_damper) { + mju_mulMatVec(dmp_e, k_elem, vel_e, 3*npe, 3*npe); + } - // rotate back to global frame and scatter - mju_negQuat(quat, quat); - int local = 0; - for (int li = 0; li <= order; li++) { - for (int lj = 0; lj <= order; lj++) { - for (int lk = 0; lk <= order; lk++) { - int gi = ci*order + li; - int gj = cj*order + lj; - int gk = ck*order + lk; - int gidx = gi*ny_g*nz_g + gj*nz_g + gk; - mjtNum qfrc[3], qdmp[3]; - mji_rotVecQuat(qfrc, frc_c+3*local, quat); - mji_rotVecQuat(qdmp, dmp_c+3*local, quat); - if (enbl_spring) { - mji_addTo3(frc_g + 3*gidx, qfrc); - } - if (enbl_damper) { - mji_addTo3(dmp_g + 3*gidx, qdmp); - } - local++; - } - } - } - - cell_idx++; + // rotate back to global frame and scatter using node indices + mju_negQuat(quat, quat); + for (int n = 0; n < npe; n++) { + mjtNum qfrc[3], qdmp[3]; + mji_rotVecQuat(qfrc, frc_e+3*n, quat); + mji_rotVecQuat(qdmp, dmp_e+3*n, quat); + int gidx = gindices[n]; + if (enbl_spring) { + mji_addTo3(frc_g + 3*gidx, qfrc); + } + if (enbl_damper) { + mji_addTo3(dmp_g + 3*gidx, qdmp); } } } diff --git a/src/engine/engine_util_misc.c b/src/engine/engine_util_misc.c index 6ec01f72..ef9916a0 100644 --- a/src/engine/engine_util_misc.c +++ b/src/engine/engine_util_misc.c @@ -726,6 +726,133 @@ void mju_flexGatherCellState(int order, int cy, int cz, int ci, int cj, int ck, } +// compute corotational rotation from 2D deformation gradient on a flat face +void mju_flexInterpRotation2D(int order, const mjtNum* xpos_f, int npe, + int axis0, int axis1, int normal_axis, + const mjtNum local[2], mjtNum* quat) { + // compute 3x2 deformation gradient F at parametric point local + mjtNum t1[3] = {0, 0, 0}; // tangent along axis0 + mjtNum t2[3] = {0, 0, 0}; // tangent along axis1 + int idx = 0; + for (int l0 = 0; l0 <= order; l0++) { + for (int l1 = 0; l1 <= order; l1++) { + mjtNum grad0 = dphi(local[0], l0, order) * phi(local[1], l1, order); + mjtNum grad1 = phi(local[0], l0, order) * dphi(local[1], l1, order); + for (int d = 0; d < 3; d++) { + t1[d] += xpos_f[3*idx + d] * grad0; + t2[d] += xpos_f[3*idx + d] * grad1; + } + idx++; + } + } + + // normal = t1 x t2 + mjtNum normal[3]; + mju_cross(normal, t1, t2); + + // build 3x3 matrix with columns assigned to canonical axes (row-major) + // axis0 → t1, axis1 → t2, normal_axis → normal + // this ensures identity rotation for axis-aligned grids + mjtNum mat[9] = {0}; + mjtNum* vecs[3]; + vecs[axis0] = t1; + vecs[axis1] = t2; + vecs[normal_axis] = normal; + + for (int col = 0; col < 3; col++) { + mat[0*3 + col] = vecs[col][0]; + mat[1*3 + col] = vecs[col][1]; + mat[2*3 + col] = vecs[col][2]; + } + + // extract rotation via polar decomposition + quat[0] = 1; + quat[1] = 0; + quat[2] = 0; + quat[3] = 0; + mju_mat2Rot(quat, mat); + mju_negQuat(quat, quat); +} + + +// gather face-element-local quantities and optionally compute rotation (shell mode) +// +// face element enumeration for a grid with cell counts (cx, cy, cz): +// face 0: x=0 cy*cz quads (normal=0) +// face 1: x=max cy*cz quads (normal=0) +// face 2: y=0 cx*cz quads (normal=1) +// face 3: y=max cx*cz quads (normal=1) +// face 4: z=0 cx*cy quads (normal=2) +// face 5: z=max cx*cy quads (normal=2) +void mju_flexGatherFaceState(int order, int cx, int cy, int cz, + int face_elem_idx, + const mjtNum* xpos_g, const mjtNum* vel_g, + const mjtNum* xpos0_g, + mjtNum* xpos_f, mjtNum* vel_f, mjtNum* xpos0_f, + int* nodeindices, mjtNum* quat) { + int ny_g = cy * order + 1; + int nz_g = cz * order + 1; + int npe = (order + 1) * (order + 1); + + // face sizes and properties + int face_sizes[6] = {cy*cz, cy*cz, cx*cz, cx*cz, cx*cy, cx*cy}; + int face_normal[6] = {0, 0, 1, 1, 2, 2}; + int face_count1[6] = {cz, cz, cx, cx, cy, cy}; + int face_fixed_vals[6]; + face_fixed_vals[0] = 0; + face_fixed_vals[1] = cx * order; + face_fixed_vals[2] = 0; + face_fixed_vals[3] = cy * order; + face_fixed_vals[4] = 0; + face_fixed_vals[5] = cz * order; + + // determine which face and quad within face + int face_id = 0; + int within_face = face_elem_idx; + int cumul = 0; + for (int f = 0; f < 6; f++) { + if (face_elem_idx < cumul + face_sizes[f]) { + face_id = f; + within_face = face_elem_idx - cumul; + break; + } + cumul += face_sizes[f]; + } + + int normal_axis = face_normal[face_id]; + int na0 = (normal_axis + 1) % 3; // slow in-plane axis + int na1 = (normal_axis + 2) % 3; // fast in-plane axis + int c1 = face_count1[face_id]; + int g_fixed = face_fixed_vals[face_id]; + int q0 = within_face / c1; + int q1 = within_face % c1; + + // gather nodes + int local = 0; + for (int l0 = 0; l0 <= order; l0++) { + for (int l1 = 0; l1 <= order; l1++) { + int g[3]; + g[normal_axis] = g_fixed; + g[na0] = q0 * order + l0; + g[na1] = q1 * order + l1; + int gidx = g[0] * ny_g * nz_g + g[1] * nz_g + g[2]; + + if (xpos_f && xpos_g) mju_copy3(xpos_f + 3*local, xpos_g + 3*gidx); + if (vel_f && vel_g) mju_copy3(vel_f + 3*local, vel_g + 3*gidx); + if (xpos0_f && xpos0_g) mju_copy3(xpos0_f + 3*local, xpos0_g + 3*gidx); + if (nodeindices) nodeindices[local] = gidx; + + local++; + } + } + + if (quat && xpos_f) { + mjtNum p[2] = {.5, .5}; + mju_flexInterpRotation2D(order, xpos_f, npe, na0, na1, normal_axis, p, quat); + } +} + + //------------------------------ actuator models --------------------------------------------------- // normalized muscle length-gain curve diff --git a/src/engine/engine_util_misc.h b/src/engine/engine_util_misc.h index b5574670..374f54c2 100644 --- a/src/engine/engine_util_misc.h +++ b/src/engine/engine_util_misc.h @@ -103,6 +103,19 @@ MJAPI void mju_flexGatherCellState(int order, int cy, int cz, int ci, int cj, in const mjtNum* xpos0_g, mjtNum* xpos_c, mjtNum* vel_c, mjtNum* xpos0_c, int* nodeindices, mjtNum* quat); +// gather face-element-local quantities and optionally compute rotation (shell mode) +MJAPI void mju_flexGatherFaceState(int order, int cx, int cy, int cz, + int face_elem_idx, + const mjtNum* xpos_g, const mjtNum* vel_g, + const mjtNum* xpos0_g, + mjtNum* xpos_f, mjtNum* vel_f, mjtNum* xpos0_f, + int* nodeindices, mjtNum* quat); + +// compute corotational rotation from 2D deformation gradient on a flat face +MJAPI void mju_flexInterpRotation2D(int order, const mjtNum* xpos_f, int npe, + int axis0, int axis1, int normal_axis, + const mjtNum local[2], mjtNum* quat); + // ----------------------------- Base64 ------------------------------------------------------------ diff --git a/src/user/user_flexcomp.cc b/src/user/user_flexcomp.cc index 081781ac..85b900ae 100644 --- a/src/user/user_flexcomp.cc +++ b/src/user/user_flexcomp.cc @@ -884,27 +884,47 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz, const mjVFS* vf pe->active = true; mjs_setString(pe->name1, name.c_str()); } else if (equality == 3) { - // create one strain constraint per cell, storing cell index in eq_data + // create one strain constraint per finite element, storing element index flex->has_strain_eq = true; int cell_cx = flex->spec.cellcount[0]; int cell_cy = flex->spec.cellcount[1]; int cell_cz = flex->spec.cellcount[2]; - 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; + bool shell = (doftype == mjFCOMPDOF_TRILINEAR || + doftype == mjFCOMPDOF_QUADRATIC) && + flex->spec.elastic2d; + + if (shell) { + // shell mode: one constraint per boundary face element + int nelem_fe = 2*(cell_cy*cell_cz + cell_cx*cell_cz + cell_cx*cell_cy); + for (int fe = 0; fe < nelem_fe; fe++) { + mjsEquality* pe = mjs_addEquality(&model->spec, &def.spec); + mjs_setDefault(pe->element, &model->Default()->spec); + pe->type = mjEQ_FLEXSTRAIN; + pe->active = true; + mjs_setString(pe->name1, name.c_str()); + pe->data[0] = fe; + pe->data[1] = -1; // sentinel: shell mode + pe->data[2] = -1; + } + } else { + // volume mode: one constraint per 3D cell + 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; + pe->active = true; + mjs_setString(pe->name1, name.c_str()); + pe->data[0] = ci; + pe->data[1] = cj; + pe->data[2] = ck; } - mjsEquality* pe = mjs_addEquality(&model->spec, &def.spec); - mjs_setDefault(pe->element, &model->Default()->spec); - pe->type = mjEQ_FLEXSTRAIN; - pe->active = true; - mjs_setString(pe->name1, name.c_str()); - pe->data[0] = ci; - pe->data[1] = cj; - pe->data[2] = ck; } } } diff --git a/src/user/user_mesh.cc b/src/user/user_mesh.cc index 3389a71e..c4012201 100644 --- a/src/user/user_mesh.cc +++ b/src/user/user_mesh.cc @@ -3807,6 +3807,98 @@ void inline ComputeLinearStiffness(std::vector& K, } +// compute the linear stiffness matrix for a flat 2D quad face element (membrane) +// K: output stiffness matrix, size 3*npe x 3*npe, npe = (order+1)^2 +// pos: node positions (3*npe doubles), ordered row-major in 2D parametric domain +// E, nu: Young's modulus and Poisson's ratio +// order: interpolation order (1 or 2) +// thickness: shell thickness +// normal_axis: axis perpendicular to the face (0=x, 1=y, 2=z) +void inline ComputeLinearStiffness2D(std::vector& K, + const double* pos, + double E, double nu, int order, + double thickness, int normal_axis) { + int nbasis = order + 1; + int npe = nbasis * nbasis; // nodes per face element + int ndof = 3 * npe; + + // in-plane axes + int axis0 = (normal_axis + 1) % 3; // slow-varying + int axis1 = (normal_axis + 2) % 3; // fast-varying + + // compute quadrature points + std::vector points(nbasis); + std::vector weight(nbasis); + quadratureGaussLegendre(points.data(), weight.data(), nbasis, 0, 1); + + // compute element transformation (diagonal Jacobian on flat face) + double d0 = (pos + 3*(npe-1))[axis0] - pos[axis0]; // extent along axis0 + double d1 = (pos + 3*(npe-1))[axis1] - pos[axis1]; // extent along axis1 + if (d0 == 0 || d1 == 0) { + throw mjCError(nullptr, "degenerate 2D element with zero extent"); + } + double detJ = d0 * d1; + double invJ0 = 1.0 / d0; + double invJ1 = 1.0 / d1; + + // plane-stress Lamé parameter: lambda* = E*nu/(1 - nu^2) + double la = E * nu / (1.0 - nu * nu); + double mu = E / (2.0 * (1.0 + nu)); + + // basis function gradients (2-component) + std::vector> F(npe); + + // loop over quadrature points (2D) + for (int ps = 0; ps < nbasis; ps++) { + for (int pt = 0; pt < nbasis; pt++) { + double s = points[ps]; + double t = points[pt]; + double dvol = weight[ps] * weight[pt] * detJ * thickness; + int dof = 0; + + // cartesian product of 2D basis functions + for (int b0 = 0; b0 < nbasis; b0++) { + for (int b1 = 0; b1 < nbasis; b1++) { + F[dof][0] = dphi(s, b0, order) * phi(t, b1, order); + F[dof][1] = phi(s, b0, order) * dphi(t, b1, order); + dof++; + } + } + + if (dof != npe) { + throw mjCError(nullptr, "incorrect number of 2D basis functions"); + } + + // tensor contraction (same structure as 3D but with zero normal column) + for (int i = 0; i < npe; i++) { + for (int j = 0; j < npe; j++) { + Matrix du; + Matrix dv; + du.fill({0, 0, 0}); + dv.fill({0, 0, 0}); + for (int k = 0; k < 3; k++) { + for (int l = 0; l < 3; l++) { + // du[k] has non-zero entries only at in-plane axes + du[k][axis0] = invJ0 * F[i][0]; + du[k][axis1] = invJ1 * F[i][1]; + // du[k][normal_axis] = 0 (already zero) + dv[l][axis0] = invJ0 * F[j][0]; + dv[l][axis1] = invJ1 * F[j][1]; + // dv[l][normal_axis] = 0 (already zero) + K[ndof*(3*i+k) + 3*j+l] -= la * trace(du) * trace(dv) * dvol; + // mu (not 2*mu): same convention as 3D ComputeLinearStiffness + K[ndof*(3*i+k) + 3*j+l] -= mu * trace(inner(sym(du), sym(dv))) * dvol; + mjuu_zerovec(du[k].data(), 3); + mjuu_zerovec(dv[l].data(), 3); + } + } + } + } + } + } +} + + // Eigendecompose cell stiffness matrix and store scaled eigenvectors. // K_cell is n×n stored (negative convention: K_stored = -K_physical). // Output layout in `out`: @@ -3964,7 +4056,8 @@ void mjCFlex::ResolveReferences(const mjCModel* m) { mjCBody* pbody = static_cast(m->FindObject(mjOBJ_BODY, vertbody)); if (pbody) { vertbodyid.push_back(pbody->id); - if (pbody->joints.size() != 3 && dim == 2 && (elastic2d == 1 || elastic2d == 3)) { + if (pbody->joints.size() != 3 && dim == 2 && + (elastic2d == 1 || elastic2d == 3) && !interpolated) { // TODO(quaglino): add support for pins throw mjCError(this, "pins are not supported for bending"); } @@ -4105,8 +4198,8 @@ void mjCFlex::Compile(const mjVFS* vfs) { if (thickness <= 0) { throw mjCError(this, "2d elasticity requires positive thickness"); } - if (interpolated) { - throw mjCError(this, "interpolated flex does not yet support 2d elasticity"); + if (interpolated && elastic2d != 2) { + mju_warning("bending passive force is not implemented for interpolated flex"); } if (dim != 2 && !interpolated) { throw mjCError(this, "2d elasticity requires 2d flex"); @@ -4141,6 +4234,11 @@ 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); @@ -4354,7 +4452,7 @@ void mjCFlex::Compile(const mjVFS* vfs) { } // bending stiffness (2D only) - if (dim == 2 && (elastic2d == 1 || elastic2d == 3)) { + if (dim == 2 && (elastic2d == 1 || elastic2d == 3) && !interpolated) { bending.assign(nedge*17, 0); for (unsigned int e = 0; e < nedge; e++) { @@ -4392,57 +4490,130 @@ void mjCFlex::Compile(const mjVFS* vfs) { double K_young = has_strain_eq ? 1e1 : young; double K_poisson = has_strain_eq ? 0.3 : poisson; - int npc = pow(spec.order + 1, 3); // nodes per cell - int ndof_cell = 3 * npc; int cx = spec.cellcount[0], cy = spec.cellcount[1], cz = spec.cellcount[2]; - int ncells = cx * cy * cz; int ny_global = cy * spec.order + 1; int nz_global = cz * spec.order + 1; - // total stiffness = ncells * ndof_cell^2 - stiffness.resize(ncells * ndof_cell * ndof_cell, 0); + // determine element type: 2D boundary quads (shell) or 3D cells (volume) + bool shell_mode = elastic2d != 0; + int npe; // nodes per element + int nelem_fe; // total finite elements - // compute stiffness per cell - for (int ci = 0; ci < cx; ci++) { - for (int cj = 0; cj < cy; cj++) { - for (int ck = 0; ck < cz; ck++) { - int cell_idx = ci * cy * cz + cj * cz + ck; + if (shell_mode) { + npe = pow(spec.order + 1, 2); // (order+1)^2 for 2D quads + nelem_fe = 2*(cy*cz + cx*cz + cx*cy); + } else { + npe = pow(spec.order + 1, 3); // (order+1)^3 for 3D cells + nelem_fe = cx * cy * cz; + } + int ndof_elem = 3 * npe; - // skip stiffness computation for empty cells (no mesh content) - if (!cell_empty.empty() && cell_empty[cell_idx]) { - continue; + // total stiffness = nelem_fe * ndof_elem^2 + stiffness.resize(nelem_fe * ndof_elem * ndof_elem, 0); + + // face layout for shell mode: + // face 0: x=0 (cy*cz quads, normal=0, in-plane=(1,2)) + // face 1: x=max (cy*cz quads, normal=0, in-plane=(1,2)) + // face 2: y=0 (cx*cz quads, normal=1, in-plane=(0,2)) + // face 3: y=max (cx*cz quads, normal=1, in-plane=(0,2)) + // face 4: z=0 (cx*cy quads, normal=2, in-plane=(0,1)) + // face 5: z=max (cx*cy quads, normal=2, in-plane=(0,1)) + // face_sizes = {cy*cz, cy*cz, cx*cz, cx*cz, cx*cy, cx*cy} + int face_sizes[6] = {cy*cz, cy*cz, cx*cz, cx*cz, cx*cy, cx*cy}; + int face_normal[6] = {0, 0, 1, 1, 2, 2}; + // cell counts along each in-plane axis for each face + int face_count1[6] = {cz, cz, cx, cx, cy, cy}; // fast axis count + // fixed axis value (in grid node units, 0 or max) + int face_fixed[6] = {0, cx*spec.order, 0, cy*spec.order, 0, cz*spec.order}; + + // compute stiffness per element + for (int fe = 0; fe < nelem_fe; fe++) { + // gather element node positions + std::vector elem_pos(3 * npe); + int normal_axis = -1; + + if (shell_mode) { + // determine which face and quad within face + int face_id = 0, within_face = fe; + int cumul = 0; + for (int f = 0; f < 6; f++) { + if (fe < cumul + face_sizes[f]) { + face_id = f; + within_face = fe - cumul; + break; } + cumul += face_sizes[f]; + } - // gather cell's local node positions - std::vector cell_pos(3 * npc); - int local = 0; - for (int li = 0; li <= spec.order; li++) { - for (int lj = 0; lj <= spec.order; lj++) { - for (int lk = 0; lk <= spec.order; lk++) { - int gi = ci * spec.order + li; - int gj = cj * spec.order + lj; - int gk = ck * spec.order + lk; - int global = gi * ny_global * nz_global + gj * nz_global + gk; - mjuu_copyvec(cell_pos.data() + 3*local, nodexpos_local.data() + 3*global, 3); - local++; - } - } - } + normal_axis = face_normal[face_id]; + int na0 = (normal_axis + 1) % 3; // slow in-plane axis + int na1 = (normal_axis + 2) % 3; // fast in-plane axis + int c1 = face_count1[face_id]; // cell count along fast axis + int g_fixed = face_fixed[face_id]; // grid index along normal axis + int q0 = within_face / c1; // quad index along slow in-plane axis + int q1 = within_face % c1; // quad index along fast in-plane axis - // compute per-cell stiffness - std::vector K_cell(ndof_cell * ndof_cell, 0); - ComputeLinearStiffness(K_cell, cell_pos.data(), K_young, K_poisson, spec.order); - double* out = stiffness.data() + cell_idx * ndof_cell * ndof_cell; - - if (has_strain_eq) { - // eigendecompose: store [neig, sqrt(λ)*v_1, sqrt(λ)*v_2, ...] - std::fill(out, out + ndof_cell * ndof_cell, 0.0); - EigendecomposeStiffness(K_cell.data(), out, ndof_cell); - } else { - // store raw K for passive forces - std::copy(K_cell.begin(), K_cell.end(), out); + // gather 2D face element nodes + int local = 0; + for (int l0 = 0; l0 <= spec.order; l0++) { + for (int l1 = 0; l1 <= spec.order; l1++) { + // build global node index from 3 axis values + int g[3]; + g[normal_axis] = g_fixed; + g[na0] = q0 * spec.order + l0; + g[na1] = q1 * spec.order + l1; + int global = g[0] * ny_global * nz_global + g[1] * nz_global + g[2]; + mjuu_copyvec(elem_pos.data() + 3*local, + nodexpos_local.data() + 3*global, 3); + local++; } } + } else { + // 3D cell: convert flat index to (ci, cj, ck) + int ci = fe / (cy * cz); + int cj = (fe / cz) % cy; + int ck = fe % cz; + + // skip stiffness computation for empty cells (no mesh content) + if (!cell_empty.empty() && cell_empty[fe]) { + continue; + } + + // gather cell's local node positions + int local = 0; + for (int li = 0; li <= spec.order; li++) { + for (int lj = 0; lj <= spec.order; lj++) { + for (int lk = 0; lk <= spec.order; lk++) { + int gi = ci * spec.order + li; + int gj = cj * spec.order + lj; + int gk = ck * spec.order + lk; + int global = gi * ny_global * nz_global + gj * nz_global + gk; + mjuu_copyvec(elem_pos.data() + 3*local, + nodexpos_local.data() + 3*global, 3); + local++; + } + } + } + } + + // compute per-element stiffness + std::vector K_elem(ndof_elem * ndof_elem, 0); + if (shell_mode) { + ComputeLinearStiffness2D(K_elem, elem_pos.data(), K_young, K_poisson, + spec.order, thickness, normal_axis); + } else { + ComputeLinearStiffness(K_elem, elem_pos.data(), K_young, K_poisson, + spec.order); + } + double* out = stiffness.data() + fe * ndof_elem * ndof_elem; + + if (has_strain_eq) { + // eigendecompose: store [neig, sqrt(λ)*v_1, sqrt(λ)*v_2, ...] + std::fill(out, out + ndof_elem * ndof_elem, 0.0); + EigendecomposeStiffness(K_elem.data(), out, ndof_elem); + } else { + // store raw K for passive forces + std::copy(K_elem.begin(), K_elem.end(), out); } } } diff --git a/src/user/user_model.cc b/src/user/user_model.cc index 19672803..6c33cdec 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -2191,12 +2191,20 @@ void mjCModel::SetSizes() { nflexevpair += (int)flexes_[i]->evpair.size()/2; nflextexcoord += (flexes_[i]->HasTexcoord() ? flexes_[i]->get_texcoord().size()/2 : 0); if (flexes_[i]->spec.order != 0) { - int npc = (int)pow(flexes_[i]->spec.order + 1, 3); - int ndof_cell = 3 * npc; - int ncells = flexes_[i]->spec.cellcount[0] * - flexes_[i]->spec.cellcount[1] * - flexes_[i]->spec.cellcount[2]; - extra_stiffness_size += ncells * ndof_cell * ndof_cell; + int cx = flexes_[i]->spec.cellcount[0]; + int cy = flexes_[i]->spec.cellcount[1]; + int cz = flexes_[i]->spec.cellcount[2]; + bool shell = (flexes_[i]->elastic2d != 0); + int npe, nelem; + if (shell) { + npe = (int)pow(flexes_[i]->spec.order + 1, 2); + nelem = 2*(cy*cz + cx*cz + cx*cy); + } else { + npe = (int)pow(flexes_[i]->spec.order + 1, 3); + nelem = cx * cy * cz; + } + int ndof_elem = 3 * npe; + extra_stiffness_size += nelem * ndof_elem * ndof_elem; } if (flexes_[i]->interpolated || flexes_[i]->rigid) { continue; @@ -3476,10 +3484,20 @@ void mjCModel::CopyObjects(mjModel* m) { m->flex_stiffnessadr[i] = 21 * elem_adr; } else { m->flex_stiffnessadr[i] = current_extra_stiffness_adr; - int npc = (int)pow(pfl->spec.order + 1, 3); - int ndof_cell = 3 * npc; - int ncells = pfl->spec.cellcount[0] * pfl->spec.cellcount[1] * pfl->spec.cellcount[2]; - current_extra_stiffness_adr += ncells * ndof_cell * ndof_cell; + int pcx = pfl->spec.cellcount[0]; + int pcy = pfl->spec.cellcount[1]; + int pcz = pfl->spec.cellcount[2]; + bool shell = (pfl->elastic2d != 0); + int npe, nelem; + if (shell) { + npe = (int)pow(pfl->spec.order + 1, 2); + nelem = 2*(pcy*pcz + pcx*pcz + pcx*pcy); + } else { + npe = (int)pow(pfl->spec.order + 1, 3); + nelem = pcx * pcy * pcz; + } + int ndof_elem = 3 * npe; + current_extra_stiffness_adr += nelem * ndof_elem * ndof_elem; } if (!pfl->stiffness.empty()) { @@ -3490,10 +3508,20 @@ void mjCModel::CopyObjects(mjModel* m) { if (pfl->spec.order == 0) { stiff_size = 21 * pfl->nelem; } else { - int npc = (int)pow(pfl->spec.order + 1, 3); - int ndof_cell = 3 * npc; - int ncells = pfl->spec.cellcount[0] * pfl->spec.cellcount[1] * pfl->spec.cellcount[2]; - stiff_size = ncells * ndof_cell * ndof_cell; + int scx = pfl->spec.cellcount[0]; + int scy = pfl->spec.cellcount[1]; + int scz = pfl->spec.cellcount[2]; + bool shell = (pfl->elastic2d != 0); + int npe, sncells; + if (shell) { + npe = (int)pow(pfl->spec.order + 1, 2); + sncells = 2*(scy*scz + scx*scz + scx*scy); + } else { + npe = (int)pow(pfl->spec.order + 1, 3); + sncells = scx * scy * scz; + } + int ndof_elem = 3 * npe; + stiff_size = sncells * ndof_elem * ndof_elem; } mjuu_zerovec(m->flex_stiffness + m->flex_stiffnessadr[i], stiff_size); } @@ -3629,8 +3657,8 @@ void mjCModel::CopyObjects(mjModel* m) { memcpy(m->flex_nodebodyid + node_adr, pfl->nodebodyid.data(), pfl->nnode*sizeof(int)); } - // set interpolation type, only two types for now - m->flex_interp[i] = pfl->spec.order; + // set interpolation type: positive = volumetric, negative = shell mode + m->flex_interp[i] = pfl->spec.elastic2d ? -pfl->spec.order : pfl->spec.order; // set cell count for multi-cell finite cell method m->flex_cellnum[3*i+0] = pfl->spec.cellcount[0]; diff --git a/test/engine/engine_core_constraint_test.cc b/test/engine/engine_core_constraint_test.cc index 07204df8..69b0a173 100644 --- a/test/engine/engine_core_constraint_test.cc +++ b/test/engine/engine_core_constraint_test.cc @@ -682,6 +682,46 @@ TEST_F(CoreConstraintTest, StrainConstraintQuadratic) { mj_deleteModel(m); } +TEST_F(CoreConstraintTest, ShellModeBendZeroForceAtRest) { + static constexpr char xml[] = R"( + + + )"; + + char error[1024] = {0}; + mjModel* m = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(m, testing::NotNull()) << error; + mjData* d = mj_makeData(m); + + mj_forward(m, d); + + // Check number of equalities + EXPECT_EQ(m->neq, 6); + + // Check total number of scalar equality constraints + EXPECT_EQ(d->ne, 48); // 6 faces * 8 modes per face + + // all constraint residuals should be zero at rest + for (int i = 0; i < d->ne; i++) { + EXPECT_NEAR(d->efc_pos[i], 0, 1e-10) + << "nonzero constraint residual at " << i; + } + + mj_deleteData(d); + mj_deleteModel(m); +} + // Test quadratic passive forces (no constraints) for stability TEST_F(CoreConstraintTest, QuadraticPassiveForceStability) { static constexpr char xml[] = R"( diff --git a/test/engine/engine_passive_test.cc b/test/engine/engine_passive_test.cc index dc4f4bcf..f87dc0a5 100644 --- a/test/engine/engine_passive_test.cc +++ b/test/engine/engine_passive_test.cc @@ -847,5 +847,41 @@ TEST_F(PassiveTest, PolynomialDampingTendon) { mj_deleteModel(m); } +// shell-mode (elastic2d=stretch) flexcomp must have zero passive spring forces +// at rest (initial configuration); any nonzero force indicates a rotation +// mismatch between compile-time reference positions and runtime corotation. +TEST_F(ElasticityTest, ShellModeZeroForceAtRest) { + static constexpr char xml[] = R"( + + + )"; + + char error[1024] = {0}; + mjModel* m = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(m, testing::NotNull()) << error; + mjData* d = mj_makeData(m); + + mj_forward(m, d); + + // all spring forces should be zero at rest + for (int i = 0; i < m->nv; i++) { + EXPECT_NEAR(d->qfrc_spring[i], 0, 1e-10) + << "nonzero spring force at DOF " << i; + } + + mj_deleteData(d); + mj_deleteModel(m); +} + } // namespace } // namespace mujoco diff --git a/test/engine/engine_util_misc_test.cc b/test/engine/engine_util_misc_test.cc index d1eb8314..053a1e09 100644 --- a/test/engine/engine_util_misc_test.cc +++ b/test/engine/engine_util_misc_test.cc @@ -1165,5 +1165,390 @@ TEST_F(HistoryTest, CubicInterpolation) { EXPECT_NEAR(res[1], 1.0 - expected_0_8, MjTol(1e-9, 1e-9)); } +// -------------------------------- Face State --------------------------------- + +using FaceStateTest = MujocoTest; + +// verify mju_flexGatherFaceState returns correct node indices for all 6 faces +// of a 1x1x1 trilinear grid (2x2x2 = 8 nodes, 4 nodes per face) +TEST_F(FaceStateTest, NodeIndicesSingleCell) { + int order = 1; + int cx = 1, cy = 1, cz = 1; + int ny_g = cy * order + 1; // 2 + int nz_g = cz * order + 1; // 2 + + // nelem_fe = 2*(1*1 + 1*1 + 1*1) = 6 face elements + // face 0: x=0, face 1: x=max, face 2: y=0, face 3: y=max, + // face 4: z=0, face 5: z=max + + // create dummy positions for 8 nodes + std::vector xpos(3 * 8, 0); + for (int i = 0; i < 8; i++) { + xpos[3*i + 0] = (i / 4) * 1.0; + xpos[3*i + 1] = ((i / 2) % 2) * 1.0; + xpos[3*i + 2] = (i % 2) * 1.0; + } + + // helper: compute expected global node index from (gx, gy, gz) + auto gidx = [&](int gx, int gy, int gz) { + return gx * ny_g * nz_g + gy * nz_g + gz; + }; + + // face 0: x=0 (fixed g[0]=0, varying g[1], g[2]) + // normal_axis=0, na0=1, na1=2 + { + int indices[4]; + mju_flexGatherFaceState(order, cx, cy, cz, 0, xpos.data(), NULL, NULL, + NULL, NULL, NULL, indices, NULL); + EXPECT_EQ(indices[0], gidx(0, 0, 0)); + EXPECT_EQ(indices[1], gidx(0, 0, 1)); + EXPECT_EQ(indices[2], gidx(0, 1, 0)); + EXPECT_EQ(indices[3], gidx(0, 1, 1)); + } + + // face 1: x=max (fixed g[0]=1, varying g[1], g[2]) + { + int indices[4]; + mju_flexGatherFaceState(order, cx, cy, cz, 1, xpos.data(), NULL, NULL, + NULL, NULL, NULL, indices, NULL); + EXPECT_EQ(indices[0], gidx(1, 0, 0)); + EXPECT_EQ(indices[1], gidx(1, 0, 1)); + EXPECT_EQ(indices[2], gidx(1, 1, 0)); + EXPECT_EQ(indices[3], gidx(1, 1, 1)); + } + + // face 2: y=0 (fixed g[1]=0) + // normal_axis=1, na0=2(z slow), na1=0(x fast) + // loop order: l0→z, l1→x + { + int indices[4]; + mju_flexGatherFaceState(order, cx, cy, cz, 2, xpos.data(), NULL, NULL, + NULL, NULL, NULL, indices, NULL); + EXPECT_EQ(indices[0], gidx(0, 0, 0)); // l0=0(z=0), l1=0(x=0) + EXPECT_EQ(indices[1], gidx(1, 0, 0)); // l0=0(z=0), l1=1(x=1) + EXPECT_EQ(indices[2], gidx(0, 0, 1)); // l0=1(z=1), l1=0(x=0) + EXPECT_EQ(indices[3], gidx(1, 0, 1)); // l0=1(z=1), l1=1(x=1) + } + + // face 3: y=max (fixed g[1]=1) + // normal_axis=1, na0=2(z slow), na1=0(x fast) + { + int indices[4]; + mju_flexGatherFaceState(order, cx, cy, cz, 3, xpos.data(), NULL, NULL, + NULL, NULL, NULL, indices, NULL); + EXPECT_EQ(indices[0], gidx(0, 1, 0)); // l0=0(z=0), l1=0(x=0) + EXPECT_EQ(indices[1], gidx(1, 1, 0)); // l0=0(z=0), l1=1(x=1) + EXPECT_EQ(indices[2], gidx(0, 1, 1)); // l0=1(z=1), l1=0(x=0) + EXPECT_EQ(indices[3], gidx(1, 1, 1)); // l0=1(z=1), l1=1(x=1) + } + + // face 4: z=0 (fixed g[2]=0, varying g[0], g[1]) + // normal_axis=2, na0=0, na1=1 + { + int indices[4]; + mju_flexGatherFaceState(order, cx, cy, cz, 4, xpos.data(), NULL, NULL, + NULL, NULL, NULL, indices, NULL); + EXPECT_EQ(indices[0], gidx(0, 0, 0)); + EXPECT_EQ(indices[1], gidx(0, 1, 0)); + EXPECT_EQ(indices[2], gidx(1, 0, 0)); + EXPECT_EQ(indices[3], gidx(1, 1, 0)); + } + + // face 5: z=max (fixed g[2]=1) + { + int indices[4]; + mju_flexGatherFaceState(order, cx, cy, cz, 5, xpos.data(), NULL, NULL, + NULL, NULL, NULL, indices, NULL); + EXPECT_EQ(indices[0], gidx(0, 0, 1)); + EXPECT_EQ(indices[1], gidx(0, 1, 1)); + EXPECT_EQ(indices[2], gidx(1, 0, 1)); + EXPECT_EQ(indices[3], gidx(1, 1, 1)); + } +} + +// verify node indices for a multi-cell grid (2x2x2 cells → 3x3x3 = 27 nodes) +TEST_F(FaceStateTest, NodeIndicesMultiCell) { + int order = 1; + int cx = 2, cy = 2, cz = 2; + int ny_g = 3, nz_g = 3; // (2*1+1) = 3 + + // nelem_fe = 2*(2*2 + 2*2 + 2*2) = 24 face elements + // face 0: x=0, cy*cz = 4 quads (indices 0-3) + // face 1: x=max, 4 quads (indices 4-7) + // face 2: y=0, cx*cz = 4 quads (indices 8-11) + // face 3: y=max, 4 quads (indices 12-15) + // face 4: z=0, cx*cy = 4 quads (indices 16-19) + // face 5: z=max, 4 quads (indices 20-23) + + std::vector xpos(3 * 27, 0); + for (int i = 0; i < 27; i++) { + int gi = i / 9; + int gj = (i / 3) % 3; + int gk = i % 3; + xpos[3*i + 0] = gi * 0.1; + xpos[3*i + 1] = gj * 0.1; + xpos[3*i + 2] = gk * 0.1; + } + + auto gidx = [&](int gx, int gy, int gz) { + return gx * ny_g * nz_g + gy * nz_g + gz; + }; + + // face 0 (x=0), quad 0: (q0=0, q1=0) within cy*cz face + // c1 = face_count1[0] = cz = 2, so quad (0,0) → within_face = 0 + // na0=1, na1=2: g[0]=0, g[1]=0..1, g[2]=0..1 + { + int indices[4]; + mju_flexGatherFaceState(order, cx, cy, cz, 0, xpos.data(), NULL, NULL, + NULL, NULL, NULL, indices, NULL); + EXPECT_EQ(indices[0], gidx(0, 0, 0)); + EXPECT_EQ(indices[1], gidx(0, 0, 1)); + EXPECT_EQ(indices[2], gidx(0, 1, 0)); + EXPECT_EQ(indices[3], gidx(0, 1, 1)); + } + + // face 0 (x=0), quad 3: (q0=1, q1=1) → within_face = 1*2+1 = 3 + { + int indices[4]; + mju_flexGatherFaceState(order, cx, cy, cz, 3, xpos.data(), NULL, NULL, + NULL, NULL, NULL, indices, NULL); + EXPECT_EQ(indices[0], gidx(0, 1, 1)); + EXPECT_EQ(indices[1], gidx(0, 1, 2)); + EXPECT_EQ(indices[2], gidx(0, 2, 1)); + EXPECT_EQ(indices[3], gidx(0, 2, 2)); + } + + // face 1 (x=max), quad 0: fe_idx = 4 (after face 0's 4 quads) + // g[0] = cx*order = 2 + { + int indices[4]; + mju_flexGatherFaceState(order, cx, cy, cz, 4, xpos.data(), NULL, NULL, + NULL, NULL, NULL, indices, NULL); + EXPECT_EQ(indices[0], gidx(2, 0, 0)); + EXPECT_EQ(indices[1], gidx(2, 0, 1)); + EXPECT_EQ(indices[2], gidx(2, 1, 0)); + EXPECT_EQ(indices[3], gidx(2, 1, 1)); + } +} + +// verify node indices for a non-cubic grid (cx != cz) +TEST_F(FaceStateTest, NodeIndicesNonCubicGrid) { + int order = 1; + int cx = 2, cy = 1, cz = 3; + int ny_g = cy * order + 1; // 2 + int nz_g = cz * order + 1; // 4 + + // create dummy positions for (2*1+1)*(1*1+1)*(3*1+1) = 3*2*4 = 24 nodes + std::vector xpos(3 * 24, 0); + for (int i = 0; i < 24; i++) { + int gi = i / 8; + int gj = (i / 4) % 2; + int gk = i % 4; + xpos[3*i + 0] = gi * 0.1; + xpos[3*i + 1] = gj * 0.1; + xpos[3*i + 2] = gk * 0.1; + } + + auto gidx = [&](int gx, int gy, int gz) { + return gx * ny_g * nz_g + gy * nz_g + gz; + }; + + // face 2 (y=0): normal_axis=1, na0=2(z slow), na1=0(x fast) + // counts: na0 -> cz = 3, na1 -> cx = 2 + // total quads on face 2 = 6 + // we test within_face = 2 (third quad) + // correct: c1 = cx = 2. q0 = 2/2 = 1, q1 = 2%2 = 0 + // + // face element index calculation: + // face 0: cy*cz = 1*3 = 3 quads (indices 0-2) + // face 1: cy*cz = 1*3 = 3 quads (indices 3-5) + // face 2: cx*cz = 2*3 = 6 quads. Quad 2 is index 2 within this face. + // Total flat index = 3 + 3 + 2 = 8 + { + int indices[4]; + mju_flexGatherFaceState(order, cx, cy, cz, 8, xpos.data(), NULL, NULL, + NULL, NULL, NULL, indices, NULL); + EXPECT_EQ(indices[0], gidx(0, 0, 1)); + EXPECT_EQ(indices[1], gidx(1, 0, 1)); + EXPECT_EQ(indices[2], gidx(0, 0, 2)); + EXPECT_EQ(indices[3], gidx(1, 0, 2)); + } +} + +// verify data gathering: positions, velocities, and reference positions +TEST_F(FaceStateTest, DataGathering) { + int order = 1; + int cx = 1, cy = 1, cz = 1; + int npe = 4; + int nnodes = 8; + + // create positions and velocities for 8 nodes + std::vector xpos(3 * nnodes); + std::vector vel(3 * nnodes); + std::vector xpos0(3 * nnodes); + for (int i = 0; i < nnodes; i++) { + for (int d = 0; d < 3; d++) { + xpos[3*i + d] = 10 * i + d; + vel[3*i + d] = 100 * i + d; + xpos0[3*i + d] = 1000 * i + d; + } + } + + // gather face 4 (z=0): nodes at (0,0,0), (0,1,0), (1,0,0), (1,1,0) + // = global indices 0, 2, 4, 6 + std::vector xpos_f(3 * npe); + std::vector vel_f(3 * npe); + std::vector xpos0_f(3 * npe); + int indices[4]; + + mju_flexGatherFaceState(order, cx, cy, cz, 4, xpos.data(), vel.data(), + xpos0.data(), xpos_f.data(), vel_f.data(), + xpos0_f.data(), indices, NULL); + + for (int n = 0; n < npe; n++) { + int gi = indices[n]; + for (int d = 0; d < 3; d++) { + EXPECT_EQ(xpos_f[3*n + d], xpos[3*gi + d]); + EXPECT_EQ(vel_f[3*n + d], vel[3*gi + d]); + EXPECT_EQ(xpos0_f[3*n + d], xpos0[3*gi + d]); + } + } +} + +// verify that flexInterpRotation2D produces identity for axis-aligned faces +// (tested via mju_flexGatherFaceState with quat output) +TEST_F(FaceStateTest, IdentityRotationAxisAligned) { + int order = 1; + int cx = 1, cy = 1, cz = 1; + int npe = 4; + + // create an axis-aligned unit cube: 8 nodes at {0,1}^3 + std::vector xpos(3 * 8); + int idx = 0; + for (int i = 0; i <= 1; i++) { + for (int j = 0; j <= 1; j++) { + for (int k = 0; k <= 1; k++) { + xpos[3*idx + 0] = i; + xpos[3*idx + 1] = j; + xpos[3*idx + 2] = k; + idx++; + } + } + } + + std::vector xpos_f(3 * npe); + mjtNum quat[4]; + + // test all 6 faces: each should give identity rotation (quat = [1,0,0,0]) + int nelem_fe = 6; + for (int fe = 0; fe < nelem_fe; fe++) { + mju_flexGatherFaceState(order, cx, cy, cz, fe, xpos.data(), NULL, NULL, + xpos_f.data(), NULL, NULL, NULL, quat); + EXPECT_NEAR(mju_abs(quat[0]), 1.0, 1e-10) << "face " << fe; + EXPECT_NEAR(quat[1], 0.0, 1e-10) << "face " << fe; + EXPECT_NEAR(quat[2], 0.0, 1e-10) << "face " << fe; + EXPECT_NEAR(quat[3], 0.0, 1e-10) << "face " << fe; + } +} + +// verify that flexInterpRotation2D extracts the correct rotation for a +// globally rotated cube (90° around z-axis) +TEST_F(FaceStateTest, RotatedCubeRotation) { + int order = 1; + int cx = 1, cy = 1, cz = 1; + int npe = 4; + + // create an axis-aligned unit cube, then rotate 90° around z + // rotation: (x,y,z) → (-y, x, z) + std::vector xpos(3 * 8); + int idx = 0; + for (int i = 0; i <= 1; i++) { + for (int j = 0; j <= 1; j++) { + for (int k = 0; k <= 1; k++) { + mjtNum orig[3] = {(mjtNum)i, (mjtNum)j, (mjtNum)k}; + mjtNum axis[3] = {0, 0, 1}; + mjtNum rot_quat[4]; + mju_axisAngle2Quat(rot_quat, axis, mjPI / 2); + mju_rotVecQuat(xpos.data() + 3*idx, orig, rot_quat); + idx++; + } + } + } + + std::vector xpos_f(3 * npe); + mjtNum quat[4]; + + // expected rotation: global→local is inverse of the 90° z rotation + // 90° around z: quat = [cos(45°), 0, 0, sin(45°)] + // inverse (global→local): [cos(45°), 0, 0, -sin(45°)] + mjtNum sq2 = mju_sqrt(0.5); + + // test face 4 (z=0): normal_axis=2, in-plane axes are (0,1) + // tangent vectors should reflect the 90° z rotation + mju_flexGatherFaceState(order, cx, cy, cz, 4, xpos.data(), NULL, NULL, + xpos_f.data(), NULL, NULL, NULL, quat); + + EXPECT_NEAR(quat[0], sq2, 1e-5); + EXPECT_NEAR(quat[1], 0.0, 1e-5); + EXPECT_NEAR(quat[2], 0.0, 1e-5); + EXPECT_NEAR(quat[3], -sq2, 1e-5); + + // test face 5 (z=max): should give same rotation + mju_flexGatherFaceState(order, cx, cy, cz, 5, xpos.data(), NULL, NULL, + xpos_f.data(), NULL, NULL, NULL, quat); + + EXPECT_NEAR(quat[0], sq2, 1e-5); + EXPECT_NEAR(quat[1], 0.0, 1e-5); + EXPECT_NEAR(quat[2], 0.0, 1e-5); + EXPECT_NEAR(quat[3], -sq2, 1e-5); +} + +// verify that flexInterpRotation2D matches the 3D cell rotation for +// the same globally-rotated cube +TEST_F(FaceStateTest, RotationConsistencyWith3D) { + int order = 1; + int cx = 1, cy = 1, cz = 1; + + // create 90° z-rotated unit cube + std::vector xpos(3 * 8); + int idx = 0; + for (int i = 0; i <= 1; i++) { + for (int j = 0; j <= 1; j++) { + for (int k = 0; k <= 1; k++) { + mjtNum orig[3] = {(mjtNum)i, (mjtNum)j, (mjtNum)k}; + mjtNum axis[3] = {0, 0, 1}; + mjtNum rot_quat[4]; + mju_axisAngle2Quat(rot_quat, axis, mjPI / 6); + mju_rotVecQuat(xpos.data() + 3*idx, orig, rot_quat); + idx++; + } + } + } + + // get 3D cell rotation + int npc = 8; + std::vector xpos_c(3 * npc); + mjtNum quat_3d[4]; + mju_flexGatherCellState(order, cy, cz, 0, 0, 0, xpos.data(), NULL, NULL, + xpos_c.data(), NULL, NULL, NULL, quat_3d); + + // get 2D face rotation for each face and verify it matches the 3D rotation + int npe = 4; + std::vector xpos_f(3 * npe); + + int nelem_fe = 6; + for (int fe = 0; fe < nelem_fe; fe++) { + mjtNum quat_2d[4]; + mju_flexGatherFaceState(order, cx, cy, cz, fe, xpos.data(), NULL, NULL, + xpos_f.data(), NULL, NULL, NULL, quat_2d); + + // quaternions may differ by sign; compare unsigned + mjtNum dot = quat_3d[0]*quat_2d[0] + quat_3d[1]*quat_2d[1] + + quat_3d[2]*quat_2d[2] + quat_3d[3]*quat_2d[3]; + EXPECT_NEAR(mju_abs(dot), 1.0, 1e-5) + << "face " << fe << ": 2D rotation differs from 3D cell rotation"; + } +} + } // namespace } // namespace mujoco diff --git a/test/user/user_mesh_test.cc b/test/user/user_mesh_test.cc index 59833aef..67d06da7 100644 --- a/test/user/user_mesh_test.cc +++ b/test/user/user_mesh_test.cc @@ -836,11 +836,11 @@ TEST_F(MjCMeshTest, Flex2DElasticityRequiresPositiveThickness) { HasSubstr("2d elasticity requires positive thickness")); } -TEST_F(MjCMeshTest, InterpolatedFlexDoesNotSupport2DElasticity) { +TEST_F(MjCMeshTest, InterpolatedFlexSupportsBendElasticityWithWarning) { static constexpr char xml[] = R"( - + @@ -849,10 +849,25 @@ TEST_F(MjCMeshTest, InterpolatedFlexDoesNotSupport2DElasticity) { )"; std::array error; mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - EXPECT_THAT(model, testing::IsNull()); - EXPECT_THAT( - error.data(), - HasSubstr("interpolated flex does not yet support 2d elasticity")); + EXPECT_THAT(model, testing::NotNull()) << error.data(); + mj_deleteModel(model); +} + +TEST_F(MjCMeshTest, InterpolatedFlexSupportsBothElasticityWithWarning) { + static constexpr char xml[] = R"( + + + + + + + + + )"; + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(model, testing::NotNull()) << error.data(); + mj_deleteModel(model); } TEST_F(MjCMeshTest, Flex2DElasticityRequires2DFlex) {