Fix interpolated flex stiffness and quadratic quadrature

Use the standard 2*mu symmetric-strain term for solid and membrane elements, and correct the three-point Gauss abscissae. Add analytical stretch, shear, and quadratic strain-energy checks for both interpolation orders and nonzero Poisson ratio.

PiperOrigin-RevId: 994297589
Change-Id: I729122f2a5863542fc2961f0508a29044afd304b

Author: yuvaltassa

doc/changelog.rst

@@ -5,6 +5,12 @@ Changelog
 Upcoming Version (not yet released)
 -----------------------------------
 
+Engine
+^^^^^^
+- Corrected the shear contribution to solid and membrane stiffness for interpolated flexes, and the three-point Gauss
+  quadrature used by quadratic flexes. Material parameters now produce the standard isotropic linear-elastic stiffness;
+  existing models using these flexes can change behavior.
+
 Compiler
 ^^^^^^^^
 .. admonition:: Breaking API changes

src/user/user_mesh.cc

@@ -3659,8 +3659,8 @@ void quadratureGaussLegendre(
     weights[1] = dpdx;
   } else {
     points[0]  = p0;
-    points[1]  = -dpdx / sqrt(3. / 5.) + p0;
-    points[2]  = dpdx / sqrt(3. / 5.) + p0;
+    points[1]  = -dpdx * sqrt(3. / 5.) + p0;
+    points[2]  = dpdx * sqrt(3. / 5.) + p0;
     weights[0] = 8. / 9. * dpdx;
     weights[1] = 5. / 9. * dpdx;
     weights[2] = 5. / 9. * dpdx;
@@ -3791,7 +3791,7 @@ void inline ComputeLinearStiffness(
         }
 
         // tensor contraction of the gradients of elastic strains
-        // (d(F+F')/dx : d(F+F')/dx)
+        // lambda * div(u) * div(v) + 2*mu * sym(grad(u)) : sym(grad(v))
         for (int i = 0; i < n; i++) {
           for (int j = 0; j < n; j++) {
             Matrix du;
@@ -3807,7 +3807,7 @@ void inline ComputeLinearStiffness(
                 dv[l][1]                           = invJ[1] * F[j][1];
                 dv[l][2]                           = invJ[2] * F[j][2];
                 K[ndof * (3 * i + k) + 3 * j + l] -= la * trace(du) * trace(dv) * dvol;
-                K[ndof * (3 * i + k) + 3 * j + l] -= mu * trace(inner(sym(du), sym(dv))) * dvol;
+                K[ndof * (3 * i + k) + 3 * j + l] -= 2 * mu * trace(inner(sym(du), sym(dv))) * dvol;
                 mjuu_zerovec(du[k].data(), 3);
                 mjuu_zerovec(dv[l].data(), 3);
               }
@@ -3901,8 +3901,7 @@ void inline ComputeLinearStiffness2D(std::vector<double>& K,
               dv[l][axis1] = invJ1 * F[j][1];
 
               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;
+              K[ndof * (3 * i + k) + 3 * j + l] -= 2 * mu * trace(inner(sym(du), sym(dv))) * dvol;
               mjuu_zerovec(du[k].data(), 3);
               mjuu_zerovec(dv[l].data(), 3);
             }

test/engine/engine_forward_test.cc

@@ -3493,7 +3493,7 @@ TEST_F(ForwardTest, FlexParentCoupling) {
 
   // implicit and explicit flex damping legitimately differ at
   // O(h*damping*K/M) in this comparison
-  EXPECT_LT(max_diff, MjTol(5e-5, 1.5e-2))
+  EXPECT_LT(max_diff, MjTol(1e-4, 1.5e-2))
       << "Implicit integrator should match Euler at small timestep";
 }
 

test/user/user_mesh_test.cc

@@ -797,6 +797,73 @@ TEST_F(MjCMeshTest, VolumeSmallAllowedShell) {
   EXPECT_LE(mju_abs(model->geom_size[2]), 1);
 }
 
+// Check the compiled stiffness against continuum strain energies on a unit
+// cube. Affine fields test the Lamé coefficients; quadratic fields also test
+// quadrature.
+TEST_F(MjCMeshTest, InterpolatedFlexStrainEnergy) {
+  for (bool membrane : {false, true}) {
+    for (bool quadratic : {false, true}) {
+      for (double poisson : {0.0, 0.25}) {
+        SCOPED_TRACE(absl::StrFormat("membrane=%d quadratic=%d poisson=%g",
+                                     membrane, quadratic, poisson));
+        std::string xml =
+            absl::StrFormat(R"(
+          <mujoco>
+            <worldbody>
+              <flexcomp name="f" type="grid" count="3 3 3" spacing=".5 .5 .5" dim="3" dof="%s">
+                <elasticity young="2" poisson="%g" thickness=".1" %s/>
+                <contact selfcollide="none"/>
+              </flexcomp>
+            </worldbody>
+          </mujoco>)",
+                            quadratic ? "quadratic" : "trilinear", poisson,
+                            membrane ? "elastic2d=\"stretch\"" : "");
+        char error[1024];
+        MjModelPtr model =
+            LoadModelFromString(xml.c_str(), error, sizeof(error));
+        ASSERT_THAT(model.get(), testing::NotNull()) << error;
+        int order = quadratic ? 2 : 1;
+        int nbasis = order + 1;
+        int nnode = membrane ? nbasis * nbasis : nbasis * nbasis * nbasis;
+        int ndof = 3 * nnode;
+        double mu = 1 / (1 + poisson);
+        double lambda = membrane
+                            ? 2 * poisson / (1 - poisson * poisson)
+                            : 2 * poisson / ((1 + poisson) * (1 - 2 * poisson));
+        for (int field = 0; field < (quadratic ? 3 : 2); ++field) {
+          SCOPED_TRACE(absl::StrFormat("field=%d", field));
+          // u_x=x, u_y=x, or u_x=x^2/2. For membranes use each face's in-plane
+          // axes.
+          for (int face = 0; face < (membrane ? 6 : 1); ++face) {
+            int axis0 = membrane ? (face / 2 + 1) % 3 : 0;
+            int axis1 = membrane ? (face / 2 + 2) % 3 : 1;
+            std::vector<double> u(ndof, 0);
+            for (int i = 0; i < nnode; ++i) {
+              int ix = i / (membrane ? nbasis : nbasis * nbasis);
+              double x = static_cast<double>(ix) / order - 0.5;
+              u[3 * i + (field == 1 ? axis1 : axis0)] =
+                  field == 2 ? x * x / 2 : x;
+            }
+            const mjtNum* stiffness = model->flex_stiffness +
+                                      model->flex_stiffnessadr[0] +
+                                      face * ndof * ndof;
+            double energy = 0;
+            for (int i = 0; i < ndof; ++i) {
+              for (int j = 0; j < ndof; ++j) {
+                energy -= 0.5 * u[i] * stiffness[i * ndof + j] * u[j];
+              }
+            }
+            double expected = 0.5 * (field == 1 ? mu : lambda + 2 * mu);
+            if (field == 2) expected /= 12;  // integral of x^2 on [-1/2, 1/2]
+            if (membrane) expected *= 0.1;   // thickness
+            EXPECT_NEAR(energy, expected, MjTol(1e-12, 2e-6));
+          }
+        }
+      }
+    }
+  }
+}
+
 TEST_F(MjCMeshTest, Flex2DElasticityRequiresPositiveThickness) {
   static constexpr char xml[] = R"(
   <mujoco>