Deformable solid plugin for particles.

Using coordinate-free formula equivalent to linear finite elements.

PiperOrigin-RevId: 491317615
Change-Id: I096b9a56075bbb630f64bfffac8a8d66457e5e2d

test/plugin/elasticity/elasticity_test.cc

@@ -24,6 +24,7 @@
 #include <gtest/gtest.h>
 #include <mujoco/mujoco.h>
 #include "test/fixture.h"
+#include "plugin/elasticity/solid.h"
 
 namespace mujoco {
 namespace {
@@ -50,6 +51,62 @@ class PluginTest : public MujocoTest {
   }
 };
 
+// -------------------------------- solid -----------------------------------
+TEST_F(PluginTest, ElasticEnergy) {
+  static constexpr char cantilever_xml[] = R"(
+  <mujoco>
+  <extension>
+    <required plugin="mujoco.elasticity.solid"/>
+  </extension>
+
+  <worldbody>
+    <composite type="particle" count="8 8 8" spacing="1">
+      <geom size=".025" group="4"/>
+      <plugin plugin="mujoco.elasticity.solid">
+        <config key="nx" value="8"/>
+        <config key="ny" value="8"/>
+        <config key="nz" value="8"/>
+        <config key="poisson" value="0"/>
+        <config key="young" value="2"/>
+      </plugin>
+    </composite>
+  </worldbody>
+  </mujoco>
+  )";
+
+  char error[1024] = {0};
+  mjModel* m = LoadModelFromString(cantilever_xml, error, sizeof(error));
+  ASSERT_THAT(m, testing::NotNull()) << error;
+  mjData* d = mj_makeData(m);
+
+  EXPECT_THAT(mjp_pluginCount(), 2);
+  auto* solid = reinterpret_cast<plugin::elasticity::Solid*>(d->plugin_data[0]);
+
+  // check that if the entire geometry is rescaled by a factor "scale", then
+  // trace(strain^2) = 3*scale^2
+
+  for (mjtNum scale = 1; scale < 4; scale++) {
+    for (int t = 0; t < solid->nt; t++) {
+      mjtNum energy = 0;
+      mjtNum volume = 1./6.;
+      for (int e1 = 0; e1 < 6; e1++) {
+        for (int e2 = 0; e2 < 6; e2++) {
+          int idx1 = solid->tetrahedra[t].edges[e1];
+          int idx2 = solid->tetrahedra[t].edges[e2];
+          mjtNum elongation1 = scale*solid->reference[idx1];
+          mjtNum elongation2 = scale*solid->reference[idx2];
+          energy += solid->metric[36*t+6*e2+e1] * elongation1 * elongation2;
+        }
+      }
+      EXPECT_NEAR(
+        energy/volume, 3*scale*scale, std::numeric_limits<float>::epsilon());
+    }
+  }
+
+  mj_deleteData(d);
+  mj_deleteModel(m);
+}
+
 // -------------------------------- cable -----------------------------------
 
 TEST_F(PluginTest, CantileverIntoCircle) {