Move bending stiffness from plugin to the compiler.

PiperOrigin-RevId: 758677644
Change-Id: I7d5c5140ba6948002bfa4ae9d6b4bf0f6de0e39b

src/user/user_init.c

@@ -238,6 +238,7 @@ void mjs_defaultFlex(mjsFlex* flex) {
   flex->rgba[0] = flex->rgba[1] = flex->rgba[2] = 0.5f;
   flex->rgba[3] = 1.0f;
   flex->thickness = -1;
+  flex->elastic2d = 1;
 }
 
 

src/user/user_mesh.cc

@@ -3075,6 +3075,56 @@ static void CreateFlapStencil(std::vector<StencilFlap>& flaps,
   }
 }
 
+// cotangent between two edges
+double inline cot(double* x, int v0, int v1, int v2) {
+  double normal[3];
+  double edge1[3] = {x[3*v1]-x[3*v0], x[3*v1+1]-x[3*v0+1], x[3*v1+2]-x[3*v0+2]};
+  double edge2[3] = {x[3*v2]-x[3*v0], x[3*v2+1]-x[3*v0+1], x[3*v2+2]-x[3*v0+2]};
+
+  mjuu_crossvec(normal, edge1, edge2);
+  return mjuu_dot3(edge1, edge2) / sqrt(mjuu_dot3(normal, normal));
+}
+
+// area of a triangle
+double inline ComputeVolume(const double* x, const int v[Stencil2D::kNumVerts]) {
+  double normal[3];
+  double edge1[3] = {x[3*v[1]]-x[3*v[0]], x[3*v[1]+1]-x[3*v[0]+1], x[3*v[1]+2]-x[3*v[0]+2]};
+  double edge2[3] = {x[3*v[2]]-x[3*v[0]], x[3*v[2]+1]-x[3*v[0]+1], x[3*v[2]+2]-x[3*v[0]+2]};
+
+  mjuu_crossvec(normal, edge1, edge2);
+  return sqrt(mjuu_dot3(normal, normal)) / 2;
+}
+
+// compute bending stiffness for a single edge
+template <typename T>
+void inline ComputeBending(double* bending, double* pos, const int v[4], double mu,
+                           double thickness) {
+  int vadj[3] = {v[1], v[0], v[3]};
+
+  if (v[3]== -1) {
+    // skip boundary edges
+    return;
+  }
+
+  // cotangent operator from Wardetzky at al., "Discrete Quadratic Curvature
+  // Energies", https://cims.nyu.edu/gcl/papers/wardetzky2007dqb.pdf
+
+  mjtNum a01 = cot(pos, v[0], v[1], v[2]);
+  mjtNum a02 = cot(pos, v[0], v[3], v[1]);
+  mjtNum a03 = cot(pos, v[1], v[2], v[0]);
+  mjtNum a04 = cot(pos, v[1], v[0], v[3]);
+  mjtNum c[4] = {a03 + a04, a01 + a02, -(a01 + a03), -(a02 + a04)};
+  mjtNum volume = ComputeVolume(pos, v) +
+                  ComputeVolume(pos, vadj);
+
+  for (int v1 = 0; v1 < T::kNumVerts; v1++) {
+    for (int v2 = 0; v2 < T::kNumVerts; v2++) {
+      bending[4 * v1 + v2] +=
+          1.5 * c[v1] * c[v2] / volume * mu * pow(thickness, 3) / 12;
+    }
+  }
+}
+
 //----------------------------- linear elasticity --------------------------------------------------
 
 // Gauss Legendre quadrature points in 1 dimension on the interval [a, b]
@@ -3570,11 +3620,18 @@ void mjCFlex::Compile(const mjVFS* vfs) {
   // set size
   nedge = (int)edge.size();
 
+  // create flap stencil
+  if (dim == 2) {
+    CreateFlapStencil(flaps, elem_, edgeidx_);
+  }
+
   // compute elasticity
   if (young > 0) {
     if (poisson < 0 || poisson >= 0.5) {
       throw mjCError(this, "Poisson ratio must be in [0, 0.5)");
     }
+
+    // linear elasticity
     stiffness.assign(21*nelem, 0);
     if (interpolated) {
       int min_size = ceil(nodexpos.size()*nodexpos.size() / 21);
@@ -3583,11 +3640,13 @@ void mjCFlex::Compile(const mjVFS* vfs) {
       }
       ComputeLinearStiffness(stiffness, nodexpos.data(), young, poisson);
     }
+
+    // geometrically nonlinear elasticity
     for (unsigned int t = 0; t < nelem; t++) {
       if (interpolated) {
         continue;
       }
-      if (dim == 2) {
+      if (dim == 2 && elastic2d >= 2 && thickness > 0) {
         ComputeStiffness<Stencil2D>(stiffness, vertxpos,
                                     elem_.data() + (dim + 1) * t, t, young,
                                     poisson, thickness);
@@ -3597,6 +3656,16 @@ void mjCFlex::Compile(const mjVFS* vfs) {
                                     poisson);
       }
     }
+
+    // bending stiffness (2D only)
+    if (dim == 2 && (elastic2d == 1 || elastic2d == 3) && thickness > 0) {
+      bending.assign(nedge*16, 0);
+
+      for (unsigned int e = 0; e < nedge; e++) {
+        ComputeBending<StencilFlap>(bending.data() + 16 * e, vertxpos.data(), flaps[e].vertices,
+                                    young / (2 * (1 + poisson)), thickness);
+      }
+    }
   }
 
   // add plugins
@@ -3613,11 +3682,6 @@ void mjCFlex::Compile(const mjVFS* vfs) {
     }
   }
 
-  // create flap stencil
-  if (dim == 2) {
-    CreateFlapStencil(flaps, elem_, edgeidx_);
-  }
-
   // create shell fragments and element-vertex collision pairs
   CreateShellPair();
 

src/user/user_model.cc

@@ -3043,6 +3043,11 @@ void mjCModel::CopyObjects(mjModel* m) {
     } else {
       mjuu_zerovec(m->flex_stiffness + 21 * elem_adr, 21 * pfl->nelem);
     }
+    if (!pfl->bending.empty()) {
+      mjuu_copyvec(m->flex_bending + 16 * edge_adr, pfl->bending.data(), pfl->bending.size());
+    } else {
+      mjuu_zerovec(m->flex_bending + 16 * edge_adr, 16 * pfl->nedge);
+    }
     m->flex_damping[i] = (mjtNum)pfl->damping;
 
     // set fields: mesh-like

src/user/user_objects.h

@@ -857,6 +857,7 @@ class mjCFlex_ : public mjCBase {
   std::vector<double> elemaabb_;          // element bounding volume
   std::vector<int> edgeidx_;              // element edge ids
   std::vector<double> stiffness;          // elasticity stiffness matrix
+  std::vector<double> bending;            // bending stiffness matrix
 
   // variable-size data
   std::vector<std::string> vertbody_;     // vertex body names

src/xml/xml_native_reader.cc

@@ -311,7 +311,7 @@ const char* MJCF[nMJCF][mjXATTRNUM] = {
             "flatskin", "pos", "quat", "axisangle", "xyaxes", "zaxis", "euler", "origin"},
         {"<"},
             {"edge", "?", "5", "equality", "solref", "solimp", "stiffness", "damping"},
-            {"elasticity", "?", "4", "young", "poisson", "damping", "thickness"},
+            {"elasticity", "?", "5", "young", "poisson", "damping", "thickness", "elastic2d"},
             {"contact", "?", "14", "contype", "conaffinity", "condim", "priority",
                 "friction", "solmix", "solref", "solimp", "margin", "gap",
                 "internal", "selfcollide", "activelayers", "vertcollide"},
@@ -332,7 +332,7 @@ const char* MJCF[nMJCF][mjXATTRNUM] = {
                 "friction", "solmix", "solref", "solimp", "margin", "gap",
                 "internal", "selfcollide", "activelayers", "vertcollide"},
             {"edge", "?", "2", "stiffness", "damping"},
-            {"elasticity", "?", "4", "young", "poisson", "damping", "thickness"},
+            {"elasticity", "?", "5", "young", "poisson", "damping", "thickness", "elastic2d"},
         {">"},
         {"skin", "*", "9", "name", "file", "material", "rgba", "inflate",
             "vertex", "texcoord", "face", "group"},
@@ -1401,6 +1401,7 @@ void mjXReader::OneFlex(XMLElement* elem, mjsFlex* flex) {
     ReadAttr(elasticity, "poisson", 1, &flex->poisson, text);
     ReadAttr(elasticity, "thickness", 1, &flex->thickness, text);
     ReadAttr(elasticity, "damping", 1, &flex->damping, text);
+    ReadAttr(elasticity, "elastic2d", 1, &flex->elastic2d, text);
   }
 
   // write error info
@@ -2664,10 +2665,11 @@ void mjXReader::OneFlexcomp(XMLElement* elem, mjsBody* body, const mjVFS* vfs) {
     ReadAttr(elasticity, "poisson", 1, &dflex.poisson, text);
     ReadAttr(elasticity, "damping", 1, &dflex.damping, text);
     ReadAttr(elasticity, "thickness", 1, &dflex.thickness, text);
+    ReadAttr(elasticity, "elastic2d", 1, &dflex.elastic2d, text);
   }
 
   // check errors
-  if (elasticity && fcomp.equality) {
+  if (dflex.elastic2d >= 2 && fcomp.equality) {
     throw mjXError(elem, "elasticity and edge constraints cannot both be present");
   }
 

src/xml/xml_native_writer.cc

@@ -194,6 +194,7 @@ void mjXWriter::OneFlex(XMLElement* elem, const mjCFlex* flex) {
   WriteAttr(elastic, "poisson", 1, &flex->poisson, &defflex.poisson);
   WriteAttr(elastic, "thickness", 1, &flex->thickness, &defflex.thickness);
   WriteAttr(elastic, "damping", 1, &flex->damping, &defflex.damping);
+  WriteAttr(elastic, "elastic2d", 1, &flex->elastic2d, &defflex.elastic2d);
 
   // edge subelement
   XMLElement* edge = InsertEnd(elem, "edge");