From 713b5524bce13ff55318941f280657d2069def27 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Tue, 10 Mar 2026 04:25:19 -0700 Subject: [PATCH] Introduces a new "strain" equality mode (flex_edgeequality = 3) for flexcomps with trilinear interpolation. This mode enforces constraints on the three invariants of the Green-Lagrange strain tensor at 8 Gauss quadrature points within each flex element, resulting in 24 equality constraints per flexcomp. Also fixes a bug when attaching a flex with constraints. PiperOrigin-RevId: 881349765 Change-Id: I8e6df8239488bfdc87d7e5b473568975e38c737e --- doc/XMLreference.rst | 51 ++- doc/XMLschema.rst | 24 ++ doc/changelog.rst | 2 + doc/includes/references.h | 3 +- include/mujoco/mjmodel.h | 3 +- model/flex/strain.xml | 64 ++++ python/mujoco/introspect/enums.py | 3 +- python/mujoco/introspect/structs.py | 2 +- src/engine/engine_core_constraint.c | 416 +++++++++++++++++++-- src/engine/engine_core_smooth.c | 13 + src/engine/engine_io.c | 1 + src/user/user_flexcomp.cc | 9 +- src/user/user_flexcomp.h | 2 +- src/user/user_mesh.cc | 1 + src/user/user_model.cc | 4 + src/user/user_objects.cc | 5 +- src/xml/xml_native_reader.cc | 14 +- src/xml/xml_native_reader.h | 2 +- src/xml/xml_native_writer.cc | 1 + test/engine/engine_core_constraint_test.cc | 254 +++++++++++++ test/user/user_flex_test.cc | 69 +++- unity/Runtime/Bindings/MjBindings.cs | 3 +- wasm/codegen/generated/bindings.cc | 1 + 23 files changed, 899 insertions(+), 48 deletions(-) create mode 100644 model/flex/strain.xml diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index 156386b9..a771d3cd 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -3614,7 +3614,9 @@ saving the XML: for the entire flex, independent of the number of vertices. The positions of the vertices are updated using quadratic interpolation over the bounding box. While this option requires more degrees of freedom than trilinear flexes, it enables curved deformation modes, while the only modes achievable for trilinear flexes are - strech/compression and shear. + strech/compression and shear. To understand the difference between the two parametrizations, see `a trilinear cube + `__ and `a quadratic cube + `__. Note that a higher interpolation order generally requires a smaller time step for stability, although usually not as large as with the "full" option and a fine mesh. @@ -3820,10 +3822,12 @@ element is used to adjust the properties of all edges in the flex. .. _flexcomp-edge-equality: -:at:`equality`: :at-val:`[false, true, vert], "false"` - The type of equality constraint applied to this edge. If **false**, no equality constraint is applied. If **true**, - then edge constraints are enforced. If **vert**, an averaged constraint is used, see - :ref:`flexvert`. +:at:`equality`: :at-val:`[false, true, vert, strain], "false"` + The type of equality constraint applied to this edge. If :at-val:`false`, no equality constraint is applied. If + :at-val:`true`, then edge constraints are enforced. If :at-val:`vert`, an averaged constraint is used, see + :ref:`flexvert`. if :at-val:`strain`, then a constraint is added to enforce that the invariants of + the strain tensor do not change; this is only equality constraint type supported for trilinear and quadratic + :ref:`dofs` elements and :ref:`here`. .. _flexcomp-edge-solref: .. _flexcomp-edge-solimp: @@ -4254,7 +4258,8 @@ The elasticity model is a `Saint Venant-Kirchhoff `__ model discretized with piecewise linear finite elements, intended to simulate the compression or elongation of hyperelastic materials subjected to large displacements (finite rotations) and small strains, since it uses a nonlinear strain-displacement but a linear -stress-strain relationship.. See also :ref:`deformable ` objects. +stress-strain relationship. See also :ref:`deformable ` objects and `this model +`__. .. _flex-elasticity-young: @@ -4787,7 +4792,8 @@ This element constrains the length of one tendon to be a quartic polynomial of a This element constrains the lengths of all edges of a specified flex to their respective lengths in the initial model configuration. In this way the edges are used to maintain the shape of the deformable entity. Note that all other equality constraint types add a fixed number of scalar constraints, while this element adds as many scalar constraints -as there are edges in the specified flex. +as there are edges in the specified flex. See `this model +`__ for an example. .. _equality-flex-name: .. _equality-flex-class: @@ -4811,8 +4817,9 @@ as there are edges in the specified flex. This element constrains the trace and the derminant of the strain tensor to that of the identity matrix as in Chen, Kry, and Vouga, "Locking-free Simulation of Isometric Thin Plates", 2019. The strain tensor is computed per triangle and -averaged over all triangles adjacent to a vertex. This reduces the number of constraints from 2T to 2V, freeing -V degrees of freedom to avoid locking. It is only supported for dimension 2, i.e., cloth-like flexes. +averaged over all triangles adjacent to a vertex. This reduces the number of constraints from 2T to 2V, freeing V +degrees of freedom to avoid locking. It is only supported for dimension 2, i.e., cloth-like flexes. See `this model +`__ for an example. .. _equality-flexvert-name: .. _equality-flexvert-class: @@ -4829,6 +4836,32 @@ V degrees of freedom to avoid locking. It is only supported for dimension 2, i.e Name of the flex whose vertices are being constrained. +.. _equality-flexstrain: + +:el-prefix:`equality/` |-| **flexstrain** |*| +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +This element constrains the strain invariants of a trilinear or quadratic flex to their initial values. Specifically, it +enforces that the trace and determinant of the deformation gradient remain constant, preserving volume and preventing +excessive stretching. This constraint type is only supported for dimension 3 trilinear flexes (i.e., volumetric +deformable bodies using trilinear interpolation). See `this model +`__ for an example. + +.. _equality-flexstrain-name: +.. _equality-flexstrain-class: +.. _equality-flexstrain-active: +.. _equality-flexstrain-solref: +.. _equality-flexstrain-solimp: + +:at:`name`, :at:`class`, :at:`active`, :at:`solref`, :at:`solimp` + Same as in :ref:`connect ` element. + +.. _equality-flexstrain-flex: + +:at:`flex`: :at-val:`string, required` + Name of the flex whose strain invariants are being constrained. + + .. _equality-distance: :el-prefix:`equality/` |-| **distance** |*| diff --git a/doc/XMLschema.rst b/doc/XMLschema.rst index ee8b994c..3eb57cf0 100755 --- a/doc/XMLschema.rst +++ b/doc/XMLschema.rst @@ -2045,6 +2045,30 @@ :ref:`solimp` + .. dropdown:: :ref:`flexstrain` |*| + + .. grid:: 2 3 4 4 + :gutter: 0 + + .. grid-item:: + :ref:`name` + + .. grid-item:: + :ref:`class` + + .. grid-item:: + :ref:`flex` + + .. grid-item:: + :ref:`active` + + .. grid-item:: + :ref:`solref` + + .. grid-item:: + :ref:`solimp` + + .. dropdown:: :ref:`tendon` |*| diff --git a/doc/changelog.rst b/doc/changelog.rst index 5dcdcedd..81b32760 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -15,6 +15,8 @@ General ``ten_J_colind`` have been moved from :ref:`mjData` to :ref:`mjModel` and are no longer computed at run time by ``mj_tendon`` but at compile time. +- Added a new ``strain`` :ref:`equality constraint` type for trilinear and quadratic + :ref:`dofs`. - Flexes now support collisions with SDF geoms. - Improved memory requirements for ``ten_J`` and ``ten_J_colind`` by reducing the upper bound for the number of non-zeros ``nJten``. diff --git a/doc/includes/references.h b/doc/includes/references.h index 8622ba24..fd005344 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -609,6 +609,7 @@ typedef enum mjtEq_ { // type of equality constraint mjEQ_TENDON, // couple the lengths of two tendons with cubic mjEQ_FLEX, // fix all edge lengths of a flex mjEQ_FLEXVERT, // fix all vertex lengths of a flex + mjEQ_FLEXSTRAIN, // fix strain invariants of a trilinear flex mjEQ_DISTANCE // unsupported, will cause an error if used } mjtEq; typedef enum mjtWrap_ { // type of tendon wrap object @@ -1350,7 +1351,7 @@ struct mjModel_ { mjtNum* flex_damping; // Rayleigh's damping coefficient (nflex x 1) mjtNum* flex_edgestiffness; // edge stiffness (nflex x 1) mjtNum* flex_edgedamping; // edge damping (nflex x 1) - int* flex_edgeequality; // 0: none, 1: edges, 2: vertices (nflex x 1) + int* flex_edgeequality; // 0:none, 1:edges, 2:vertices, 3:strain (nflex x 1) mjtByte* flex_rigid; // are all vertices in the same body (nflex x 1) mjtByte* flexedge_rigid; // are both edge vertices in same body (nflexedge x 1) mjtByte* flex_centered; // are all vertex coordinates (0,0,0) (nflex x 1) diff --git a/include/mujoco/mjmodel.h b/include/mujoco/mjmodel.h index 9fc191db..449020a2 100644 --- a/include/mujoco/mjmodel.h +++ b/include/mujoco/mjmodel.h @@ -211,6 +211,7 @@ typedef enum mjtEq_ { // type of equality constraint mjEQ_TENDON, // couple the lengths of two tendons with cubic mjEQ_FLEX, // fix all edge lengths of a flex mjEQ_FLEXVERT, // fix all vertex lengths of a flex + mjEQ_FLEXSTRAIN, // fix strain invariants of a trilinear flex mjEQ_DISTANCE // unsupported, will cause an error if used } mjtEq; @@ -1011,7 +1012,7 @@ struct mjModel_ { mjtNum* flex_damping; // Rayleigh's damping coefficient (nflex x 1) mjtNum* flex_edgestiffness; // edge stiffness (nflex x 1) mjtNum* flex_edgedamping; // edge damping (nflex x 1) - int* flex_edgeequality; // 0: none, 1: edges, 2: vertices (nflex x 1) + int* flex_edgeequality; // 0:none, 1:edges, 2:vertices, 3:strain (nflex x 1) mjtByte* flex_rigid; // are all vertices in the same body (nflex x 1) mjtByte* flexedge_rigid; // are both edge vertices in same body (nflexedge x 1) mjtByte* flex_centered; // are all vertex coordinates (0,0,0) (nflex x 1) diff --git a/model/flex/strain.xml b/model/flex/strain.xml new file mode 100644 index 00000000..f3e8889b --- /dev/null +++ b/model/flex/strain.xml @@ -0,0 +1,64 @@ + + + + + + diff --git a/python/mujoco/introspect/enums.py b/python/mujoco/introspect/enums.py index 711b0ee2..d4dcdf34 100644 --- a/python/mujoco/introspect/enums.py +++ b/python/mujoco/introspect/enums.py @@ -222,7 +222,8 @@ ENUMS: Mapping[str, EnumDecl] = dict([ ('mjEQ_TENDON', 3), ('mjEQ_FLEX', 4), ('mjEQ_FLEXVERT', 5), - ('mjEQ_DISTANCE', 6), + ('mjEQ_FLEXSTRAIN', 6), + ('mjEQ_DISTANCE', 7), ]), )), ('mjtWrap', diff --git a/python/mujoco/introspect/structs.py b/python/mujoco/introspect/structs.py index 433b8167..2ebe7d6c 100644 --- a/python/mujoco/introspect/structs.py +++ b/python/mujoco/introspect/structs.py @@ -2980,7 +2980,7 @@ STRUCTS: Mapping[str, StructDecl] = dict([ type=PointerType( inner_type=ValueType(name='int'), ), - doc='0: none, 1: edges, 2: vertices', + doc='0:none, 1:edges, 2:vertices, 3:strain', array_extent=('nflex',), ), StructFieldDecl( diff --git a/src/engine/engine_core_constraint.c b/src/engine/engine_core_constraint.c index 1155af03..1d207f5b 100644 --- a/src/engine/engine_core_constraint.c +++ b/src/engine/engine_core_constraint.c @@ -47,6 +47,26 @@ //-------------------------- utility functions ----------------------------------------------------- +// basis functions for flex strain constraints +static void basis(int order, int i, mjtNum p, mjtNum* phi, mjtNum* dphi) { + if (order == 1) { + *phi = (i == 0 ? 1 - p : p); + *dphi = (i == 0 ? -1 : 1); + } else { + if (i == 0) { + *phi = 2 * p * p - 3 * p + 1; + *dphi = 4 * p - 3; + } else if (i == 1) { + *phi = 4 * (p - p * p); + *dphi = 4 * (1 - 2 * p); + } else { + *phi = 2 * p * p - p; + *dphi = 4 * p - 1; + } + } +} + + // allocate efc arrays on arena, return 1 on success, 0 on failure static int arenaAllocEfc(const mjModel* m, mjData* d) { #undef MJ_M @@ -612,36 +632,336 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) { size = 1; break; - case mjEQ_FLEX: - flex_edgeadr = m->flex_edgeadr[id[0]]; - flex_edgenum = m->flex_edgenum[id[0]]; - // add one constraint per non-rigid edge - for (int e=flex_edgeadr; e < flex_edgeadr+flex_edgenum; e++) { - // skip rigid - if (m->flexedge_rigid[e]) { - continue; - } + case mjEQ_FLEXSTRAIN: { + // strain constraint mode: add 24 constraints (3 invariants at 8 Gauss points) + int f = id[0]; + int nodenum = m->flex_nodenum[f]; + int* bodyid = m->flex_nodebodyid + m->flex_nodeadr[f]; + int order = m->flex_interp[f]; - // position error - cpos[0] = d->flexedge_length[e] - m->flexedge_length0[e]; + // skip if not interpolated (order == 0 or no nodes) + if (!order || !nodenum) { + break; + } - // add constraint: sparse or dense - if (issparse) { - mj_addConstraint(m, d, d->flexedge_J+m->flexedge_J_rowadr[e], cpos, 0, 0, - 1, mjCNSTR_EQUALITY, i, - m->flexedge_J_rownnz[e], - m->flexedge_J_colind+m->flexedge_J_rowadr[e]); + // allocate stack for node positions and Jacobians + mj_markStack(d); + mjtNum* xpos = mjSTACKALLOC(d, 3*nodenum, mjtNum); + mjtNum* node_jac = mjSTACKALLOC(d, 3*nodenum*nv, mjtNum); + int* chain_col = mjSTACKALLOC(d, nv, int); + mjtNum* strain_jac = mjSTACKALLOC(d, nv, mjtNum); + + // compute global node positions from body states + // Green-Lagrange strain E = ½(F'F - I) is rotationally invariant, + // so we don't need corotational decomposition - use global positions directly + int nstart = m->flex_nodeadr[f]; + for (int n = 0; n < nodenum; n++) { + if (m->flex_centered[f]) { + mju_copy3(xpos + 3*n, d->xpos + 3*bodyid[n]); } else { - mju_zero(jac[0], nv); // reuse first row of jac[0] - int rowadr = m->flexedge_J_rowadr[e]; - int rownnz = m->flexedge_J_rownnz[e]; - for (int k=0; kflexedge_J_colind[rowadr+k]] = d->flexedge_J[rowadr+k]; - } - mj_addConstraint(m, d, jac[0], cpos, 0, 0, 1, mjCNSTR_EQUALITY, i, 0, NULL); + mju_mulMatVec3(xpos + 3*n, d->xmat + 9*bodyid[n], m->flex_node + 3*(n + nstart)); + mju_addTo3(xpos + 3*n, d->xpos + 3*bodyid[n]); } } + + // compute node Jacobians in global frame (3*nodenum rows x nv cols) + mjtNum* blk_jac = mjSTACKALLOC(d, 3*nv, mjtNum); + mju_zero(node_jac, 3*nodenum*nv); + for (int n = 0; n < nodenum; n++) { + int chain_nnz = mj_bodyChain(m, bodyid[n], chain_col); + mju_zero(blk_jac, 3*nv); + mj_jacSparse(m, d, blk_jac, NULL, xpos + 3*n, bodyid[n], chain_nnz, chain_col); + + // expand sparse Jacobian to dense row format + for (int r = 0; r < 3; r++) { + for (int k = 0; k < chain_nnz; k++) { + node_jac[(3*n + r)*nv + chain_col[k]] = blk_jac[r*chain_nnz + k]; + } + } + } + + // compute combined chain for all nodes (for sparse mode) + int* combined_chain = mjSTACKALLOC(d, nv, int); + int combined_nnz = 0; + if (issparse) { + // mark which DOFs are used by any node body + int* dof_used = mjSTACKALLOC(d, nv, int); + mju_zeroInt(dof_used, nv); + for (int n = 0; n < nodenum; n++) { + int temp_chain[200]; // max DOFs per body + int temp_nnz = mj_bodyChain(m, bodyid[n], temp_chain); + for (int k = 0; k < temp_nnz; k++) { + dof_used[temp_chain[k]] = 1; + } + } + + // build combined chain from marked DOFs + for (int q = 0; q < nv; q++) { + if (dof_used[q]) { + combined_chain[combined_nnz++] = q; + } + } + } + + // Gauss-Legendre quadrature points in [0,1]^3 + // order=1: 2x2x2=8 points, order=2: 3x3x3=27 points + int nquad = order + 1; // quadrature order per axis + int ngauss = nquad * nquad * nquad; // total Gauss points + + // 1D Gauss-Legendre points and weights on [0,1] + mjtNum gp1d[3]; // max 3 points for order=2 + if (nquad == 2) { + gp1d[0] = 0.5 - 0.5/mju_sqrt(3.0); // ~ 0.211 + gp1d[1] = 0.5 + 0.5/mju_sqrt(3.0); // ~ 0.789 + } else { // nquad == 3 + gp1d[0] = 0.5 - 0.5*mju_sqrt(0.6); // ~ 0.113 + gp1d[1] = 0.5; // 0.5 + gp1d[2] = 0.5 + 0.5*mju_sqrt(0.6); // ~ 0.887 + } + + // build 3D Gauss points array (max 27 points) + mjtNum gauss[27][3]; + for (int gi = 0; gi < nquad; gi++) { + for (int gj = 0; gj < nquad; gj++) { + for (int gk = 0; gk < nquad; gk++) { + int idx = gi*nquad*nquad + gj*nquad + gk; + gauss[idx][0] = gp1d[gi]; + gauss[idx][1] = gp1d[gj]; + gauss[idx][2] = gp1d[gk]; + } + } + } + + // loop over Gauss points + // get reference positions from m->flex_node0 (Cartesian positions at qpos0) + mjtNum* refpos = mjSTACKALLOC(d, 3*nodenum, mjtNum); + for (int n = 0; n < nodenum; n++) { + mju_copy3(refpos + 3*n, m->flex_node0 + 3*(n + nstart)); + } + + for (int g = 0; g < ngauss; g++) { + mjtNum* p = gauss[g]; + + // compute current position gradient dxcur/dxi + mjtNum Fcur[9]; + mju_defGradient(Fcur, p, xpos, order); + + // compute reference position gradient dxref/dxi + mjtNum Fref[9]; + mju_defGradient(Fref, p, refpos, order); + + // compute true deformation gradient F = Fcur * inv(Fref) + // F maps from reference to current configuration + mjtNum Fref_inv[9]; + mju_transpose(Fref_inv, Fref, 3, 3); // start with transpose for cofactor + mjtNum det = Fref[0]*(Fref[4]*Fref[8] - Fref[5]*Fref[7]) - + Fref[1]*(Fref[3]*Fref[8] - Fref[5]*Fref[6]) + + Fref[2]*(Fref[3]*Fref[7] - Fref[4]*Fref[6]); + + // compute adjugate (cofactor transposed) + Fref_inv[0] = (Fref[4]*Fref[8] - Fref[5]*Fref[7]) / det; + Fref_inv[1] = -(Fref[1]*Fref[8] - Fref[2]*Fref[7]) / det; + Fref_inv[2] = (Fref[1]*Fref[5] - Fref[2]*Fref[4]) / det; + Fref_inv[3] = -(Fref[3]*Fref[8] - Fref[5]*Fref[6]) / det; + Fref_inv[4] = (Fref[0]*Fref[8] - Fref[2]*Fref[6]) / det; + Fref_inv[5] = -(Fref[0]*Fref[5] - Fref[2]*Fref[3]) / det; + Fref_inv[6] = (Fref[3]*Fref[7] - Fref[4]*Fref[6]) / det; + Fref_inv[7] = -(Fref[0]*Fref[7] - Fref[1]*Fref[6]) / det; + Fref_inv[8] = (Fref[0]*Fref[4] - Fref[1]*Fref[3]) / det; + + // F = Fcur * Fref_inv + mjtNum F[9]; + mju_mulMatMat(F, Fcur, Fref_inv, 3, 3, 3); + + // compute C = F'*F (right Cauchy-Green tensor) + mjtNum C[9]; + mju_mulMatTMat3(C, F, F); + + // compute Green-Lagrange strain E = 0.5*(C - I) + mjtNum E[9]; + for (int j = 0; j < 9; j++) { + E[j] = 0.5 * C[j]; + } + E[0] -= 0.5; + E[4] -= 0.5; + E[8] -= 0.5; + + // compute 3 invariants of E + // I1 = tr(E) = E[0] + E[4] + E[8] + mjtNum I1 = E[0] + E[4] + E[8]; + + // I2 = 0.5*(tr(E)^2 - tr(E^2)) + mjtNum trE2 = E[0]*E[0] + E[1]*E[3] + E[2]*E[6] + + E[3]*E[1] + E[4]*E[4] + E[5]*E[7] + + E[6]*E[2] + E[7]*E[5] + E[8]*E[8]; + mjtNum I2 = 0.5 * (I1*I1 - trE2); + + // I3 = det(E) + mjtNum I3 = E[0]*(E[4]*E[8] - E[5]*E[7]) - + E[1]*(E[3]*E[8] - E[5]*E[6]) + + E[2]*(E[3]*E[7] - E[4]*E[6]); + + // compute shape function gradients at this Gauss point + // grad[n][k] = d(basis_n)/d(p_k), for n in [0, nodenum), k in [0,3) + // indexing matches mju_defGradient: idx = i*(order+1)^2 + j*(order+1) + k + mjtNum grad[27][3]; // max 27 nodes for order=2 + int npoint = (order + 1) * (order + 1) * (order + 1); + for (int n = 0; n < npoint; n++) { + int stride = order + 1; + int ix = n / (stride * stride); + int iy = (n / stride) % stride; + int iz = n % stride; + + mjtNum phi_x, phi_y, phi_z, dphi_x, dphi_y, dphi_z; + basis(order, ix, p[0], &phi_x, &dphi_x); + basis(order, iy, p[1], &phi_y, &dphi_y); + basis(order, iz, p[2], &phi_z, &dphi_z); + + grad[n][0] = dphi_x * phi_y * phi_z; + grad[n][1] = phi_x * dphi_y * phi_z; + grad[n][2] = phi_x * phi_y * dphi_z; + } + + // compute Jacobian for each invariant + mjtNum invariants[3] = {I1, I2, I3}; + + for (int inv = 0; inv < 3; inv++) { + cpos[0] = invariants[inv]; + + // compute dI/dE (3x3 symmetric, stored as 9) + mjtNum dIdE[9]; + if (inv == 0) { + // dI1/dE = I (identity) + mju_zero(dIdE, 9); + dIdE[0] = dIdE[4] = dIdE[8] = 1.0; + } else if (inv == 1) { + // dI2/dE = tr(E)*I - E + mju_zero(dIdE, 9); + dIdE[0] = I1 - E[0]; + dIdE[4] = I1 - E[4]; + dIdE[8] = I1 - E[8]; + dIdE[1] = -E[1]; dIdE[3] = -E[3]; + dIdE[2] = -E[2]; dIdE[6] = -E[6]; + dIdE[5] = -E[5]; dIdE[7] = -E[7]; + } else { + // dI3/dE = cofactor(E) = det(E) * E^{-T} for invertible E + // For small strain, use adjugate directly + dIdE[0] = E[4]*E[8] - E[5]*E[7]; + dIdE[1] = -(E[1]*E[8] - E[2]*E[7]); + dIdE[2] = E[1]*E[5] - E[2]*E[4]; + dIdE[3] = -(E[3]*E[8] - E[5]*E[6]); + dIdE[4] = E[0]*E[8] - E[2]*E[6]; + dIdE[5] = -(E[0]*E[5] - E[2]*E[3]); + dIdE[6] = E[3]*E[7] - E[4]*E[6]; + dIdE[7] = -(E[0]*E[7] - E[1]*E[6]); + dIdE[8] = E[0]*E[4] - E[1]*E[3]; + } + + // dI/dx_n = sum over i,j: dI/dE_ij * dE_ij/dx_n + // where dE_ij/dx_n = 0.5 * d(F'F)_ij/dx_n + // d(F'F)_ij/dx_n = sum_k (dF_ki/dx_n * F_kj + F_ki * dF_kj/dx_n) + // + // With F = Fcur * Fref_inv: + // dF_ab/d(x_n)_c = sum_k (dFcur_ak/d(x_n)_c) * Fref_inv[kb] + // = sum_k delta_{ac} * grad[n][k] * Fref_inv[kb] + // So only row a=c contributes + + // compute dI/dx for all nodes (3*nodenum values) + mjtNum* dIdx = mjSTACKALLOC(d, 3*nodenum, mjtNum); + mju_zero(dIdx, 3*nodenum); + + for (int n = 0; n < nodenum; n++) { + for (int c = 0; c < 3; c++) { // component of x_n + mjtNum dI = 0; + for (int ij = 0; ij < 9; ij++) { + int ii = ij / 3; // row of E/C + int jj = ij % 3; // col of E/C + + // dF_ab/d(x_n)_c = sum_k grad[n][k] * Fref_inv[kb] (only for a=c) + // dC_ij/d(x_n)_c = dF_ci * F_cj + F_ci * dF_cj + // = (sum_k grad[n][k]*Fref_inv[ki]) * F_cj + // + F_ci * (sum_k grad[n][k]*Fref_inv[kj]) + + // compute dF_ci = sum_k grad[n][k] * Fref_inv[ki] + mjtNum dF_ci = 0; + for (int k = 0; k < 3; k++) { + dF_ci += grad[n][k] * Fref_inv[k*3 + ii]; + } + // compute dF_cj = sum_k grad[n][k] * Fref_inv[kj] + mjtNum dF_cj = 0; + for (int k = 0; k < 3; k++) { + dF_cj += grad[n][k] * Fref_inv[k*3 + jj]; + } + + mjtNum dC_ij = dF_ci * F[c*3 + jj] + F[c*3 + ii] * dF_cj; + + // dE_ij = 0.5 * dC_ij + dI += dIdE[ij] * 0.5 * dC_ij; + } + dIdx[3*n + c] = dI; + } + } + + // strain_jac[q] = sum_n sum_c dIdx[3*n+c] * node_jac[(3*n+c)*nv + q] + mju_zero(strain_jac, nv); + for (int n = 0; n < nodenum; n++) { + for (int c = 0; c < 3; c++) { + int row = 3*n + c; + for (int q = 0; q < nv; q++) { + strain_jac[q] += dIdx[row] * node_jac[row*nv + q]; + } + } + } + + // add constraint: sparse or dense + if (issparse) { + // convert dense strain_jac to sparse format + mjtNum* sparse_jac = mjSTACKALLOC(d, combined_nnz, mjtNum); + for (int k = 0; k < combined_nnz; k++) { + sparse_jac[k] = strain_jac[combined_chain[k]]; + } + mj_addConstraint(m, d, sparse_jac, cpos, 0, 0, 1, mjCNSTR_EQUALITY, i, + combined_nnz, combined_chain); + } else { + mj_addConstraint(m, d, strain_jac, cpos, 0, 0, 1, mjCNSTR_EQUALITY, i, 0, NULL); + } + } + } + + mj_freeStack(d); break; + } + + case mjEQ_FLEX: + // edge constraint mode: add one constraint per non-rigid edge + flex_edgeadr = m->flex_edgeadr[id[0]]; + flex_edgenum = m->flex_edgenum[id[0]]; + for (int e=flex_edgeadr; e < flex_edgeadr+flex_edgenum; e++) { + // skip rigid + if (m->flexedge_rigid[e]) { + continue; + } + + // position error + cpos[0] = d->flexedge_length[e] - m->flexedge_length0[e]; + + // add constraint: sparse or dense + if (issparse) { + mj_addConstraint(m, d, d->flexedge_J+m->flexedge_J_rowadr[e], cpos, 0, 0, + 1, mjCNSTR_EQUALITY, i, + m->flexedge_J_rownnz[e], + m->flexedge_J_colind+m->flexedge_J_rowadr[e]); + } else { + mju_zero(jac[0], nv); // reuse first row of jac[0] + int rowadr = m->flexedge_J_rowadr[e]; + int rownnz = m->flexedge_J_rownnz[e]; + for (int k=0; kflexedge_J_colind[rowadr+k]] = d->flexedge_J[rowadr+k]; + } + mj_addConstraint(m, d, jac[0], cpos, 0, 0, 1, mjCNSTR_EQUALITY, i, 0, NULL); + } + } + break; case mjEQ_FLEXVERT: // add two constraints per vertex @@ -1186,6 +1506,26 @@ void mj_diagApprox(const mjModel* m, mjData* d) { i--; break; + case mjEQ_FLEXSTRAIN: { + // strain constraints: 24 constraints, use average node inv weight + int flex_id = m->eq_obj1id[id]; + int nodenum = m->flex_nodenum[flex_id]; + int nstart = m->flex_nodeadr[flex_id]; + mjtNum avg_invweight = 0; + for (int n = 0; n < nodenum; n++) { + int bodyid = m->flex_nodebodyid[nstart + n]; + avg_invweight += m->body_invweight0[2*bodyid]; + } + avg_invweight /= nodenum; + for (int c = 0; c < 24; c++) { + dA[i++] = avg_invweight; + } + + // adjust constraint counter + i--; + break; + } + default: mjERROR("unknown constraint type %d", d->efc_type[i]); // SHOULD NOT OCCUR } @@ -1788,6 +2128,31 @@ static int mj_ne(const mjModel* m, mjData* d, int* nnz) { } break; + case mjEQ_FLEXSTRAIN: { + // strain constraints: 3 invariants × ngauss Gauss points + // skip if not interpolated (order == 0 or no nodes) + int order = m->flex_interp[id[0]]; + int nodenum = m->flex_nodenum[id[0]]; + if (!order || !nodenum) { + break; + } + int nquad = order + 1; // 2 for order=1, 3 for order=2 + int ngauss = nquad * nquad * nquad; // 8 or 27 + size = 3 * ngauss; // 24 or 81 + + if (nnz) { + // NV is sum of all node Jacobians + int nstart = m->flex_nodeadr[id[0]]; + for (int n = 0; n < nodenum; n++) { + int bodyid = m->flex_nodebodyid[nstart + n]; + NV += mj_bodyChain(m, bodyid, chain); + } + // each constraint row shares this NV + NV = size * NV; + } + break; + } + default: // might occur in case of the now-removed distance equality constraint mjERROR("unknown constraint type %d", m->eq_type[i]); // SHOULD NOT OCCUR @@ -1795,7 +2160,8 @@ static int mj_ne(const mjModel* m, mjData* d, int* nnz) { // accumulate counts; flex NV already accumulated ne += mj_addConstraintCount(m, size, NV); - if (m->eq_type[i] == mjEQ_FLEX || m->eq_type[i] == mjEQ_FLEXVERT) { + if (m->eq_type[i] == mjEQ_FLEX || m->eq_type[i] == mjEQ_FLEXVERT || + m->eq_type[i] == mjEQ_FLEXSTRAIN) { nnze += NV; } else { nnze += size*NV; diff --git a/src/engine/engine_core_smooth.c b/src/engine/engine_core_smooth.c index ab57ee8b..97c26f60 100644 --- a/src/engine/engine_core_smooth.c +++ b/src/engine/engine_core_smooth.c @@ -2573,6 +2573,19 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) { i += 2*m->flex_vertnum[k]; break; + case mjEQ_FLEXSTRAIN: { + // increment with 3 invariants × ngauss Gauss points + k = m->eq_obj1id[id]; + int order = m->flex_interp[k]; + int nodenum = m->flex_nodenum[k]; + if (order && nodenum) { + int nquad = order + 1; + int ngauss = nquad * nquad * nquad; + i += 3 * ngauss; + } + break; + } + default: mjERROR("unknown constraint type type %d", m->eq_type[id]); // SHOULD NOT OCCUR } diff --git a/src/engine/engine_io.c b/src/engine/engine_io.c index f0cf467f..8a8e59b0 100644 --- a/src/engine/engine_io.c +++ b/src/engine/engine_io.c @@ -1958,6 +1958,7 @@ const char* mj_validateReferences(const mjModel* m) { case mjEQ_FLEX: case mjEQ_FLEXVERT: + case mjEQ_FLEXSTRAIN: if (obj1id >= m->nflex || obj1id < 0) { return "Invalid model: eq_obj1id out of bounds."; } diff --git a/src/user/user_flexcomp.cc b/src/user/user_flexcomp.cc index b313a671..867f0707 100644 --- a/src/user/user_flexcomp.cc +++ b/src/user/user_flexcomp.cc @@ -644,7 +644,14 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz, const mjVFS* vf if (equality) { mjsEquality* pe = mjs_addEquality(&model->spec, &def.spec); mjs_setDefault(pe->element, &model->Default()->spec); - pe->type = equality == 1 ? mjEQ_FLEX : mjEQ_FLEXVERT; + // equality 1=edge(mjEQ_FLEX), 2=vert(mjEQ_FLEXVERT), 3=strain(mjEQ_FLEXSTRAIN) + if (equality == 1) { + pe->type = mjEQ_FLEX; + } else if (equality == 2) { + pe->type = mjEQ_FLEXVERT; + } else if (equality == 3) { + pe->type = mjEQ_FLEXSTRAIN; + } pe->active = true; mjs_setString(pe->name1, name.c_str()); } diff --git a/src/user/user_flexcomp.h b/src/user/user_flexcomp.h index 4b346cf4..09e13c41 100644 --- a/src/user/user_flexcomp.h +++ b/src/user/user_flexcomp.h @@ -83,7 +83,7 @@ class mjCFlexcomp { double origin[3]; // origin for generating a 3D mesh from a convex 2D mesh double mass; // total mass of auto-generated bodies double inertiabox; // size of inertia box for each body - int equality; // create equality constraint, 0:none, 1:edge, 2:vert + int equality; // create equality constraint, 0:none, 1:edge, 2:vert, 3:strain std::string file; // mesh/gmsh file name mjtDof doftype; // dof type, all vertices or trilinear interpolation diff --git a/src/user/user_mesh.cc b/src/user/user_mesh.cc index f3c9d19c..20244116 100644 --- a/src/user/user_mesh.cc +++ b/src/user/user_mesh.cc @@ -3876,6 +3876,7 @@ void mjCFlex::PointToLocal() { void mjCFlex::NameSpace(const mjCModel* m) { + mjCBase::NameSpace(m); for (auto& name : spec_vertbody_) { name = m->prefix + name + m->suffix; } diff --git a/src/user/user_model.cc b/src/user/user_model.cc index 7c3d37a6..7a429536 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -3533,6 +3533,10 @@ void mjCModel::CopyObjects(mjModel* m) { m->flex_edgeequality[i] = 2; break; } + if (equalities_[k]->type == mjEQ_FLEXSTRAIN) { + m->flex_edgeequality[i] = 3; + break; + } } } diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index 7e330e5c..718c9f70 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -6292,7 +6292,7 @@ void mjCEquality::ResolveReferences(const mjCModel* m) { object_type = mjOBJ_JOINT; } else if (type == mjEQ_TENDON) { object_type = mjOBJ_TENDON; - } else if (type == mjEQ_FLEX || type == mjEQ_FLEXVERT) { + } else if (type == mjEQ_FLEX || type == mjEQ_FLEXVERT || type == mjEQ_FLEXSTRAIN) { object_type = mjOBJ_FLEX; } else { throw mjCError(this, "invalid type in equality constraint"); @@ -6349,7 +6349,8 @@ void mjCEquality::Compile(void) { ResolveReferences(model); // make sure flex is not rigid - if ((type == mjEQ_FLEX || type == mjEQ_FLEXVERT) && model->Flexes()[obj1id]->rigid) { + if ((type == mjEQ_FLEX || type == mjEQ_FLEXVERT || type == mjEQ_FLEXSTRAIN) && + model->Flexes()[obj1id]->rigid) { throw mjCError(this, "rigid flex '%s' in equality constraint %d", name1_.c_str(), id); } } diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index ebbfb24f..8e66004d 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -366,6 +366,8 @@ std::vector MJCF[nMJCF] = { "active", "solref", "solimp"}, {"flexvert", "*", "name", "class", "flex", "active", "solref", "solimp"}, + {"flexstrain", "*", "name", "class", "flex", + "active", "solref", "solimp"}, {">"}, {"tendon", "*"}, @@ -669,7 +671,7 @@ const mjMap solver_map[solver_sz] = { // constraint type -const int equality_sz = 7; +const int equality_sz = 8; const mjMap equality_map[equality_sz] = { {"connect", mjEQ_CONNECT}, {"weld", mjEQ_WELD}, @@ -677,6 +679,7 @@ const mjMap equality_map[equality_sz] = { {"tendon", mjEQ_TENDON}, {"flex", mjEQ_FLEX}, {"flexvert", mjEQ_FLEXVERT}, + {"flexstrain", mjEQ_FLEXSTRAIN}, {"distance", mjEQ_DISTANCE} }; @@ -930,10 +933,11 @@ const mjMap elastic2d_map[5] = { // flex equality type -const mjMap flexeq_map[3] = { +const mjMap flexeq_map[4] = { {"false", 0}, {"true", 1}, {"vert", 2}, + {"strain", 3}, }; @@ -2207,6 +2211,7 @@ void mjXReader::OneEquality(XMLElement* elem, mjsEquality* equality) { case mjEQ_FLEX: case mjEQ_FLEXVERT: + case mjEQ_FLEXSTRAIN: ReadAttrTxt(elem, "flex", name1, true); break; @@ -2768,7 +2773,7 @@ void mjXReader::OneFlexcomp(XMLElement* elem, mjsBody* body, const mjVFS* vfs) { // edge XMLElement* edge = FirstChildElement(elem, "edge"); if (edge) { - MapValue(edge, "equality", &fcomp.equality, flexeq_map, 3); + MapValue(edge, "equality", &fcomp.equality, flexeq_map, 4); ReadAttr(edge, "solref", mjNREF, fcomp.def.spec.equality->solref, text, false, false); ReadAttr(edge, "solimp", mjNIMP, fcomp.def.spec.equality->solimp, text, false, false); ReadAttr(edge, "stiffness", 1, &dflex.edgestiffness, text); @@ -2789,6 +2794,9 @@ void mjXReader::OneFlexcomp(XMLElement* elem, mjsBody* body, const mjVFS* vfs) { if (dflex.elastic2d >= 2 && fcomp.equality) { throw mjXError(elem, "elasticity and edge constraints cannot both be present"); } + if (fcomp.equality == 3 && dflex.young > 0) { + throw mjXError(elem, "strain constraint and elasticity (young) cannot both be present"); + } // contact XMLElement* cont = FirstChildElement(elem, "contact"); diff --git a/src/xml/xml_native_reader.h b/src/xml/xml_native_reader.h index 2ad11e6d..4e0369ce 100644 --- a/src/xml/xml_native_reader.h +++ b/src/xml/xml_native_reader.h @@ -102,7 +102,7 @@ class mjXReader : public mjXBase { }; // MJCF schema -#define nMJCF 245 +#define nMJCF 246 extern std::vector MJCF[nMJCF]; #endif // MUJOCO_SRC_XML_XML_NATIVE_READER_H_ diff --git a/src/xml/xml_native_writer.cc b/src/xml/xml_native_writer.cc index 199f15aa..a681d8d3 100644 --- a/src/xml/xml_native_writer.cc +++ b/src/xml/xml_native_writer.cc @@ -711,6 +711,7 @@ void mjXWriter::OneEquality(XMLElement* elem, const mjCEquality* equality, mjCDe case mjEQ_FLEX: case mjEQ_FLEXVERT: + case mjEQ_FLEXSTRAIN: WriteAttrTxt(elem, "flex", mjs_getString(equality->name1)); break; diff --git a/test/engine/engine_core_constraint_test.cc b/test/engine/engine_core_constraint_test.cc index 3ac2944c..2d7b46fb 100644 --- a/test/engine/engine_core_constraint_test.cc +++ b/test/engine/engine_core_constraint_test.cc @@ -14,6 +14,7 @@ // Tests for engine/engine_core_constraint.c. +#include #include #include #include @@ -489,5 +490,258 @@ TEST_F(CoreConstraintTest, FlexvertEquality) { mj_deleteModel(model); } +// Test flex strain constraint with pinned nodes attached to freejoint parent +TEST_F(CoreConstraintTest, BoxShellPinnedParentWithFreejoint) { + static constexpr char xml[] = R"( + + + )"; + std::array error; + mjModel* m = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(m, NotNull()) << error.data(); + mjData* d = mj_makeData(m); + + mj_resetData(m, d); + mj_forward(m, d); + + // Check that we have constraints + EXPECT_GT(d->nefc, 0) << "No constraints generated"; + EXPECT_GT(d->ne, 0) << "Expected some strain constraints"; + + // Check qacc and forces at rest with gravity=0 + EXPECT_NEAR(d->qacc_smooth[6], 0, 1e-6) << "qacc_smooth should be 0 at rest"; + EXPECT_NEAR(d->qacc[6], 0, 1e-6) << "qacc should be 0 at rest"; + + // Check initial constraint values (efc_pos) + bool has_bad_constraint = false; + for (int i = 0; i < d->nefc; i++) { + if (d->efc_type[i] == mjCNSTR_EQUALITY) { + if (mju_abs(d->efc_pos[i]) > 1.0) { + has_bad_constraint = true; + } + } + } + EXPECT_FALSE(has_bad_constraint) + << "Some constraint values are too large at rest"; + + // Check Jacobian values - look for NaN or huge values + int nv = m->nv; + bool has_bad_jacobian = false; + for (int i = 0; i < d->nefc; i++) { + if (d->efc_type[i] == mjCNSTR_EQUALITY) { + for (int j = 0; j < nv; j++) { + mjtNum val = d->efc_J[i*nv + j]; + if (mju_isBad(val) || mju_abs(val) > 1e10) { + has_bad_jacobian = true; + } + } + } + } + EXPECT_FALSE(has_bad_jacobian) << "Jacobian contains NaN or huge values"; + + // Verify Jacobian with finite differences for first few constraints + mjtNum eps = 1e-6; + std::vector qpos0(m->nq); + mju_copy(qpos0.data(), d->qpos, m->nq); + + // Store original constraint values + std::vector efc_pos0(d->nefc); + mju_copy(efc_pos0.data(), d->efc_pos, d->nefc); + + int num_constraints_to_check = mju_min(3, d->ne); + bool has_jacobian_mismatch = false; + for (int j = 0; j < nv && j < 6; j++) { + mju_copy(d->qpos, qpos0.data(), m->nq); + mjtNum dqpos[100] = {0}; + dqpos[j] = eps; + mj_integratePos(m, d->qpos, dqpos, 1); + mj_forward(m, d); + + for (int i = 0; i < num_constraints_to_check; i++) { + mjtNum fd = (d->efc_pos[i] - efc_pos0[i]) / eps; + mjtNum analytic = d->efc_J[i*nv + j]; + // Use relative tolerance with absolute floor to handle near-zero values + mjtNum tol = mju_max(1e-8, 0.1 * (mju_abs(fd) + mju_abs(analytic))); + if (mju_abs(fd - analytic) > tol) { + has_jacobian_mismatch = true; + } + } + } + EXPECT_FALSE(has_jacobian_mismatch) + << "Jacobian FD mismatch at initial config"; + + // Test rotation invariance: rotate via freejoint quaternion + mju_copy(d->qpos, qpos0.data(), m->nq); + mjtNum angle = 0.785398; // 45 degrees + d->qpos[3] = mju_cos(angle/2); // w + d->qpos[4] = 0; + d->qpos[5] = 0; + d->qpos[6] = mju_sin(angle/2); // z + mj_forward(m, d); + + mjtNum max_strain_rotated = 0; + for (int i = 0; i < d->ne; i++) { + if (mju_abs(d->efc_pos[i]) > max_strain_rotated) { + max_strain_rotated = mju_abs(d->efc_pos[i]); + } + } + EXPECT_LT(max_strain_rotated, 1e-6) + << "Strain should remain ~0 after rigid rotation"; + + // Check Jacobian in rotated configuration via FD + std::vector qpos_rot(m->nq); + mju_copy(qpos_rot.data(), d->qpos, m->nq); + std::vector efc_pos_rot(d->nefc); + mju_copy(efc_pos_rot.data(), d->efc_pos, d->nefc); + + bool has_rotated_jacobian_mismatch = false; + for (int j = 0; j < nv; j++) { + mju_copy(d->qpos, qpos_rot.data(), m->nq); + mjtNum dqpos[100] = {0}; + dqpos[j] = eps; + mj_integratePos(m, d->qpos, dqpos, 1); + mj_forward(m, d); + + mjtNum fd = (d->efc_pos[0] - efc_pos_rot[0]) / eps; + mjtNum analytic = d->efc_J[0*nv + j]; + mjtNum tol = 0.1 * (mju_abs(fd) + mju_abs(analytic) + 1e-8); + if ((mju_abs(fd) > 1e-8 || mju_abs(analytic) > 1e-8) && + mju_abs(fd - analytic) > tol) { + has_rotated_jacobian_mismatch = true; + } + } + EXPECT_FALSE(has_rotated_jacobian_mismatch) + << "Jacobian FD mismatch in rotated config"; + + // Reset for simulation + mju_copy(d->qpos, qpos0.data(), m->nq); + mj_forward(m, d); + + // Run simulation only if checks pass + if (!has_bad_constraint && !has_bad_jacobian) { + for (int i = 0; i < 2000; i++) { + mj_step(m, d); + + ASSERT_FALSE(mju_isBad(d->qpos[0])) + << "Simulation became unstable at step " << i; + ASSERT_FALSE(mju_isBad(d->qvel[0])) + << "Velocity became unstable at step " << i; + + for (int j = 0; j < m->nv; j++) { + ASSERT_LT(mju_abs(d->qvel[j]), 1000.0) + << "Velocity exploded at step " << i << ", qvel[" << j + << "]=" << d->qvel[j]; + } + } + } + + mj_deleteData(d); + mj_deleteModel(m); +} + +// Test flex strain constraint WITHOUT pinned nodes (simpler case) +TEST_F(CoreConstraintTest, StrainConstraintNoPinning) { + static constexpr char xml[] = R"( + + + )"; + std::array error; + mjModel* m = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(m, NotNull()) << error.data(); + mjData* d = mj_makeData(m); + + mj_resetData(m, d); + mj_forward(m, d); + + // Check constraints + EXPECT_GT(d->ne, 0) << "Expected strain constraints"; + + // Check no contacts + EXPECT_EQ(d->ncon, 0); + + // Check that initial strain is ~0 + mjtNum max_pos = 0; + for (int i = 0; i < d->ne; i++) { + if (mju_abs(d->efc_pos[i]) > max_pos) { + max_pos = mju_abs(d->efc_pos[i]); + } + } + EXPECT_LT(max_pos, 1e-6) << "Initial strain should be ~0"; + + // Check Jacobian for NaN + int nv = m->nv; + bool has_bad_jacobian = false; + for (int i = 0; i < d->ne; i++) { + for (int j = 0; j < nv; j++) { + if (mju_isBad(d->efc_J[i*nv + j])) { + has_bad_jacobian = true; + } + } + } + EXPECT_FALSE(has_bad_jacobian) << "Jacobian has NaN"; + + // Test rigid rotation: rotate flex and check strain still ~0 + std::vector qpos0(m->nq); + mju_copy(qpos0.data(), d->qpos, m->nq); + // Rotate by 45 degrees around Z axis via quaternion + mjtNum angle = 0.785398; // 45 degrees + d->qpos[3] = mju_cos(angle/2); // w + d->qpos[4] = 0; // x + d->qpos[5] = 0; // y + d->qpos[6] = mju_sin(angle/2); // z + mj_forward(m, d); + + mjtNum max_strain_rotated = 0; + for (int i = 0; i < d->ne; i++) { + if (mju_abs(d->efc_pos[i]) > max_strain_rotated) { + max_strain_rotated = mju_abs(d->efc_pos[i]); + } + } + EXPECT_LT(max_strain_rotated, 1e-6) + << "Strain should remain ~0 after rigid rotation"; + + // Run simulation for a few steps to check stability + mju_copy(d->qpos, qpos0.data(), m->nq); + mj_forward(m, d); + + for (int i = 0; i < 100; i++) { + mj_step(m, d); + ASSERT_FALSE(mju_isBad(d->qpos[0])) << "Simulation unstable at step " << i; + } + + mj_deleteData(d); + mj_deleteModel(m); +} + } // namespace } // namespace mujoco diff --git a/test/user/user_flex_test.cc b/test/user/user_flex_test.cc index c37ac7dc..e0398de6 100644 --- a/test/user/user_flex_test.cc +++ b/test/user/user_flex_test.cc @@ -15,7 +15,7 @@ // Tests for user/user_model.cc. #include -#include +#include #include #include @@ -919,5 +919,72 @@ TEST_F(UserFlexTest, FlexcompMeshLoadsFromVFS) { mj_deleteVFS(&vfs); } +// Test that flex constraints are preserved when attaching a model +TEST_F(UserFlexTest, FlexAttachConstraintPreserved) { + // Child model with flex and strain constraint + static constexpr char flex_xml[] = R"( + + + + + + + + + + + )"; + + // Parent model that attaches the flex model + static constexpr char parent_xml[] = R"( + + + + + + + + + + + )"; + + // Set up VFS with both XML files + auto vfs = std::make_unique(); + mj_defaultVFS(vfs.get()); + mj_addBufferVFS(vfs.get(), "flex.xml", flex_xml, sizeof(flex_xml)); + + // First verify the standalone flex model has constraints + std::array error; + mjModel* m_standalone = + LoadModelFromString(flex_xml, error.data(), error.size(), vfs.get()); + ASSERT_THAT(m_standalone, NotNull()) << error.data(); + mjData* d_standalone = mj_makeData(m_standalone); + mj_forward(m_standalone, d_standalone); + int standalone_neq = m_standalone->neq; + EXPECT_GT(standalone_neq, 0) << "Standalone flex should have constraints"; + mj_deleteData(d_standalone); + mj_deleteModel(m_standalone); + + // Now load the parent model which attaches the flex + mjModel* m_attached = + LoadModelFromString(parent_xml, error.data(), error.size(), vfs.get()); + ASSERT_THAT(m_attached, NotNull()) << error.data(); + mjData* d_attached = mj_makeData(m_attached); + mj_forward(m_attached, d_attached); + + // THE BUG: flex constraints disappear when attached + EXPECT_GT(m_attached->neq, 0) + << "Attached flex should preserve strain constraints"; + EXPECT_EQ(m_attached->neq, standalone_neq) + << "Attached flex should have same number of constraints as standalone"; + + mj_deleteData(d_attached); + mj_deleteModel(m_attached); + mj_deleteVFS(vfs.get()); +} + } // namespace } // namespace mujoco diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index 0bd6e798..17e2eb36 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -288,7 +288,8 @@ public enum mjtEq : int{ mjEQ_TENDON = 3, mjEQ_FLEX = 4, mjEQ_FLEXVERT = 5, - mjEQ_DISTANCE = 6, + mjEQ_FLEXSTRAIN = 6, + mjEQ_DISTANCE = 7, } public enum mjtWrap : int{ mjWRAP_NONE = 0, diff --git a/wasm/codegen/generated/bindings.cc b/wasm/codegen/generated/bindings.cc index 3aa71a8c..5cb9a5bd 100644 --- a/wasm/codegen/generated/bindings.cc +++ b/wasm/codegen/generated/bindings.cc @@ -10868,6 +10868,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) { .value("mjEQ_TENDON", mjEQ_TENDON) .value("mjEQ_FLEX", mjEQ_FLEX) .value("mjEQ_FLEXVERT", mjEQ_FLEXVERT) + .value("mjEQ_FLEXSTRAIN", mjEQ_FLEXSTRAIN) .value("mjEQ_DISTANCE", mjEQ_DISTANCE); enum_("mjtEvent") .value("mjEVENT_NONE", mjEVENT_NONE)