Add implicit stiffness for flex_interp to mj_implicitSkip.

PiperOrigin-RevId: 867706885 Change-Id: Ic94c65b618a415609bffe3d69a86f9034f2d2400
2026-02-09 11:53:56 -08:00
parent c1b3b3063e
commit 0041fdcbb0
9 changed files with 910 additions and 70 deletions
@@ -15,6 +15,7 @@
 // Tests for engine/engine_forward.c.

 #include "src/engine/engine_forward.h"
+#include "src/engine/engine_derivative.h"

 #include <cmath>
 #include <cstdlib>
@@ -1617,5 +1618,143 @@ TEST_F(ForwardTest, ActuatorDelayLinearInterp) {
  mj_deleteModel(model);
 }

+TEST_F(ForwardTest, FlexTrilinearInstability) {
+  // model parameters matches user's trilinear.xml
+  constexpr char xml[] = R"(
+  <mujoco model="stability_test">
+      <option gravity="0 0 -9.81" iterations="100" solver="CG" tolerance="1e-10"
+              timestep="0.002" integrator="implicitfast">
+          <flag warmstart="disable" island="disable"/>
+      </option>
+      <worldbody>
+          <geom name="floor" size="0 0 .05" type="plane" condim="3"/>
+          <flexcomp name="bed" type="grid" count="17 17 3" spacing="0.05 0.05 0.05"
+                    pos="0 0 0.05" radius="0.0005" dim="3" mass="10" dof="trilinear">
+              <contact condim="3" solref="0.005 1" solimp=".99 .99 .001" selfcollide="none"/>
+              <elasticity young="865067.00" poisson="0.1" damping="1"/>
+          </flexcomp>
+          <body name="box" pos="0.05 0.05 0.5">
+              <freejoint/>
+              <geom name="box_geom" type="box" size="0.04 0.04 0.04" mass="0.5"
+                    solref="0.001 1" solimp="0.99 0.99 0.01"/>
+          </body>
+      </worldbody>
+  </mujoco>
+  )";
+  char error[1024];
+  mjModel* model = LoadModelFromString(xml, error, sizeof(error));
+  ASSERT_THAT(model, NotNull()) << error;
+
+  mjData* data = mj_makeData(model);
+
+  // flex stiffness sign checks
+  // verify correct sign of flex stiffness derivatives before simulation
+  int nv = model->nv;
+  mjtNum h = model->opt.timestep;
+
+  // create a test vector
+  std::vector<mjtNum> v(nv), Mv(nv), flex_Kv(nv);
+  for (int i = 0; i < nv; i++) v[i] = mju_Halton(i, 2) - 0.5;
+  mjtNum vnorm = mju_norm(v.data(), nv);
+  for (int i = 0; i < nv; i++) v[i] /= vnorm;
+
+  mj_forward(model, data);
+
+  // compute M*v and stiffness contributions
+  mj_mulM(model, data, Mv.data(), v.data());
+
+  // note: we use mjd_flexInterp_mulK here (unscaled by h^2) to check raw
+  // stiffness logic similar to what we expect in the solver now
+  mjtNum* v_copy = (mjtNum*)mju_malloc(nv * sizeof(mjtNum));
+  mju_copy(v_copy, v.data(), nv);
+  mju_zero(flex_Kv.data(), nv);
+
+  // using mulKD for legacy check consistency, but we know it applies h^2+h*d
+  // scaling; actually, let's stick to the high-level property checks from
+  // FlexStiffnessSign which used mulKD
+  mjd_flexInterp_mulKD(model, data, flex_Kv.data(), v.data(), h);
+
+  // compute v^T*M*v and v^T*scale*K*v
+  mjtNum vMv = mju_dot(v.data(), Mv.data(), nv);
+  // mulKD returns -scale*K*v, so -flex_Kv = +scale*K*v
+  mjtNum vKv = -mju_dot(v.data(), flex_Kv.data(), nv);
+
+  // assertions from FlexStiffnessSign
+  EXPECT_GT(vKv, 0) << "Stiffness contribution should be positive";
+  EXPECT_GT(vMv + vKv, vMv) << "Full Hessian should exceed M alone";
+
+  mju_free(v_copy);
+
+  // stability simulation
+  // run for steps to catch instability
+  for (int i = 0; i < 2000; ++i) {
+    mj_step(model, data);
+
+    for (int j = 0; j < model->nq; ++j) {
+      if (mju_abs(data->qpos[j]) > 1000.0) {
+        ADD_FAILURE() << "Instability detected at step " << i << " dof " << j
+                      << " val " << data->qpos[j];
+        return;  // Exit early
+      }
+    }
+  }
+
+  mj_deleteData(data);
+  mj_deleteModel(model);
+}
+// Verify that flex damping does not affect rigid body motion
+TEST_F(ForwardTest, FlexDampingRigidMotion) {
+  constexpr char xml[] = R"(
+  <mujoco>
+      <option gravity="0 0 0" timestep="0.01" integrator="implicitfast"/>
+      <worldbody>
+          <flexcomp name="flex" type="grid" count="3 3 3" spacing="0.1 0.1 0.1"
+                    pos="0 0 0" euler="45 45 45" radius="0.01" dim="3" mass="1" dof="trilinear">
+              <contact selfcollide="none"/>
+              <elasticity young="1e5" poisson="0.3" damping="10"/>
+          </flexcomp>
+      </worldbody>
+  </mujoco>
+  )";
+  char error[1024];
+  mjModel* model = LoadModelFromString(xml, error, sizeof(error));
+  ASSERT_THAT(model, NotNull()) << error;
+  mjData* data = mj_makeData(model);
+
+  // Set initial rigid rotation velocity about Z axis
+  // Center of mass is roughly at 0 0 0 because pos="0 0 0" and symmetric grid.
+  // v = w x r. Let w = (1, 1, 1).
+  mjtNum w[3] = {10.0, 10.0, 10.0};
+  for (int i = 0; i < model->nv / 3; ++i) {
+    int qpos_adr = model->jnt_qposadr[i];
+    int qvel_adr = model->jnt_dofadr[i];
+    mjtNum* pos = data->qpos + qpos_adr;
+    mjtNum* vel = data->qvel + qvel_adr;
+
+    mjtNum r[3] = {pos[0], pos[1], pos[2]};
+    mju_cross(vel, w, r);
+  }
+
+  mj_forward(model, data);
+  mjtNum initial_energy = data->energy[0] + data->energy[1];
+
+  // Run a few steps
+  for (int i = 0; i < 10; ++i) {
+    mj_step(model, data);
+  }
+
+  mj_forward(model, data);
+  mjtNum final_energy = data->energy[0] + data->energy[1];
+
+  // Expect energy conservation.
+  // With the bug, damping force acts on rigid rotation, dissipating energy.
+  EXPECT_NEAR(final_energy, initial_energy, 1e-6 * initial_energy)
+      << "Energy decayed significantly (" << initial_energy << " -> "
+      << final_energy << ")";
+
+  mj_deleteData(data);
+  mj_deleteModel(model);
+}
+
 }  // namespace
 }  // namespace mujoco