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

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());
   }

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
 

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;
   }

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;
+        }
       }
     }
 

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);
   }
 }