Add vertex equality option.

PiperOrigin-RevId: 860299454
Change-Id: I7befd8e5f4c8bac6d74684a96a9cc3cee9ae1e29

src/user/user_flexcomp.cc

@@ -81,7 +81,7 @@ mjCFlexcomp::mjCFlexcomp(void) {
   mjuu_setvec(scale, 1, 1, 1);
   mass = 1;
   inertiabox = 0.005;
-  equality = false;
+  equality = 0;
   mjuu_setvec(pos, 0, 0, 0);
   mjuu_setvec(quat, 1, 0, 0, 0);
   rigid = false;
@@ -602,7 +602,7 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz) {
   if (equality) {
     mjsEquality* pe = mjs_addEquality(&model->spec, &def.spec);
     mjs_setDefault(pe->element, &model->Default()->spec);
-    pe->type = mjEQ_FLEX;
+    pe->type = equality == 1 ? mjEQ_FLEX : mjEQ_FLEXVERT;
     pe->active = true;
     mjs_setString(pe->name1, name.c_str());
   }

src/user/user_flexcomp.h

@@ -81,7 +81,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
-  bool equality;                  // create edge equality constraint
+  int equality;                   // create equality constraint, 0:none, 1:edge, 2:vert
   std::string file;               // mesh/gmsh file name
   mjtDof doftype;                 // dof type, all vertices or trilinear interpolation
 

src/user/user_model.cc

@@ -3403,11 +3403,16 @@ void mjCModel::CopyObjects(mjModel* m) {
     // find equality constraint referencing this flex
     m->flex_edgeequality[i] = 0;
     for (int k=0; k < (int)equalities_.size(); k++) {
-      if (equalities_[k]->type == mjEQ_FLEX && equalities_[k]->name1_ == pfl->name) {
-        m->flex_edgeequality[i] = 1;
-        break;
+      if (equalities_[k]->name1_ == pfl->name) {
+        if (equalities_[k]->type == mjEQ_FLEX) {
+          m->flex_edgeequality[i] = 1;
+          break;
+        }
+        if (equalities_[k]->type == mjEQ_FLEXVERT) {
+          m->flex_edgeequality[i] = 2;
+          break;
+        }
       }
-      // TODO: support flex_edgeequality = 2
     }
 
     // copy bvh data (flex aabb computed dynamically in mjData)

src/user/user_objects.cc

@@ -5995,7 +5995,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) {
+  } else if (type == mjEQ_FLEX || type == mjEQ_FLEXVERT) {
     object_type = mjOBJ_FLEX;
   } else {
     throw mjCError(this, "invalid type in equality constraint");
@@ -6052,7 +6052,7 @@ void mjCEquality::Compile(void) {
   ResolveReferences(model);
 
   // make sure flex is not rigid
-  if (type == mjEQ_FLEX && model->Flexes()[obj1id]->rigid) {
+  if ((type == mjEQ_FLEX || type == mjEQ_FLEXVERT) && model->Flexes()[obj1id]->rigid) {
     throw mjCError(this, "rigid flex '%s' in equality constraint %d", name1_.c_str(), id);
   }
 }