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) {