Fix flexcomp strain constraints with rotated grids.

The reference node positions and the positions used for computing stiffness eigenvectors were previously stored in world frame. However, the runtime expects these quantities in the unrotated local frame. This caused non-zero constraint residuals and simulation instability when the grid was rotated — either by the parent body's initial orientation, or by the flexcomp's own frame attributes. Rather than tracking each rotation source individually, this change extracts the total grid rotation directly from the cell geometry. All node positions are then un-rotated before computing the stiffness matrix. PiperOrigin-RevId: 903232388 Change-Id: If877af89025ce1e61a76b38c29403d593d892749
2026-04-21 08:01:32 -07:00
parent da01bd37a2
commit ba149aa043
3 changed files with 220 additions and 6 deletions
@@ -15,8 +15,6 @@
 // Tests for engine/engine_core_constraint.c.

 #include <array>
-#include <cstddef>
-#include <cstring>
 #include <string>
 #include <vector>

@@ -914,5 +912,143 @@ TEST_F(CoreConstraintTest, JdotvFwdInvIdentity) {
  }
 }

+// --------------------------- strain constraint rotated parent ----------------
+
+struct StrainConstraintTestCase {
+  std::string test_name;
+  std::string body_pos;
+  std::string body_quat;
+  std::string flex_spacing;
+  std::string flex_xyaxes;
+};
+
+class StrainConstraintRotatedTest : public CoreConstraintTest,
+                                    public ::testing::WithParamInterface<
+                                        StrainConstraintTestCase> {
+};
+
+TEST_P(StrainConstraintRotatedTest, ResidualIsZero) {
+  auto param = GetParam();
+  std::string xml = R"(
+  <mujoco>
+  <option integrator="implicitfast" jacobian="dense" gravity="0 0 0"/>
+  <worldbody>
+    <body name="parent" )";
+
+  if (!param.body_pos.empty()) {
+    xml += "pos=\"" + param.body_pos + "\" ";
+  }
+  if (!param.body_quat.empty()) {
+    xml += "quat=\"" + param.body_quat + "\" ";
+  }
+  xml += R"(>
+      <joint type="free"/>
+      <geom type="box" size=".01 .01 .01" mass=".1"/>
+      <flexcomp name="test" type="box" )";
+
+  if (!param.flex_spacing.empty()) {
+    xml += "spacing=\"" + param.flex_spacing + "\" ";
+  }
+  if (!param.flex_xyaxes.empty()) {
+    xml += "xyaxes=\"" + param.flex_xyaxes + "\" ";
+  }
+
+  xml += R"(radius="0.001"
+                pos="0 0 0" dof="trilinear" mass="1" dim="3">
+        <contact selfcollide="none"/>
+        <edge equality="strain"/>
+      </flexcomp>
+    </body>
+  </worldbody>
+  </mujoco>
+  )";
+
+  std::array<char, 1024> error;
+  mjModel* m = LoadModelFromString(xml.c_str(), error.data(), error.size());
+  ASSERT_THAT(m, NotNull()) << error.data();
+  mjData* d = mj_makeData(m);
+
+  mj_forward(m, d);
+
+  // Check we have strain constraints
+  EXPECT_GT(d->ne, 0) << "Expected strain constraints";
+
+  // The critical check: constraint residuals must be ~0 at the initial
+  // (undeformed) configuration, even though the body is rotated.
+  mjtNum max_pos = 0;
+  for (int i = 0; i < d->ne; i++) {
+    max_pos = mju_max(max_pos, mju_abs(d->efc_pos[i]));
+  }
+  EXPECT_LT(max_pos, 1e-6)
+      << "Strain constraint residual should be ~0"
+      << " (max_pos=" << max_pos << ")";
+
+  // Verify stability
+  for (int i = 0; i < 200; i++) {
+    mj_step(m, d);
+    ASSERT_FALSE(mju_isBad(d->qpos[0]))
+        << "Simulation unstable at step " << i;
+    for (int j = 0; j < m->nv; j++) {
+      ASSERT_LT(mju_abs(d->qvel[j]), 1000.0)
+          << "Velocity exploded at step " << i
+          << ", qvel[" << j << "]=" << d->qvel[j];
+    }
+  }
+
+  mj_deleteData(d);
+  mj_deleteModel(m);
+}
+
+INSTANTIATE_TEST_SUITE_P(
+  StrainConstraintRotatedTests, StrainConstraintRotatedTest,
+  testing::ValuesIn<StrainConstraintTestCase>({
+      // Test strain constraint with a rotated parent body.
+      // The flexcomp is placed inside a parent body that has a non-identity
+      // initial rotation. This reproduces the "grocery scene" bug where the
+      // stiffness matrix eigenvectors and reference positions were computed
+      // in world frame instead of the unrotated local frame, causing
+      // spurious constraint forces.
+      {
+          "RotatedParent",
+          "1 2 3",
+          "0.707107 0 0.707107 0",
+          ".1 .1 .1",
+          ""
+      },
+      // Same test with an anisotropic box (different spacing per axis) and
+      // arbitrary rotation (combined 45-deg Y + 30-deg X).
+      {
+          "RotatedParentAnisotropic",
+          "0.5 -1 2",
+          "0.8924 0.2392 0.3696 -0.0990",
+          ".15 .08 .05",
+          ""
+      },
+      // Test strain constraint with flexcomp-level xyaxes rotation.
+      // This is the "grocery scene" pattern where the flexcomp grid itself is
+      // rotated via xyaxes="0 1 0 0 0 1" (X->Y, Y->Z).
+      {
+          "FlexcompXyaxes",
+          "",
+          "",
+          ".1 .02 .1",
+          "0 1 0 0 0 1"
+      },
+      // Test combining parent body rotation with flexcomp xyaxes rotation.
+      // The total rotation is the composition of both.
+      {
+          "RotatedParentPlusXyaxes",
+          "1 2 3",
+          "0.707107 0 0.707107 0",
+          ".15 .08 .05",
+          "0 1 0 0 0 1"
+      }
+  }),
+  [](const testing::TestParamInfo<
+      StrainConstraintRotatedTest::ParamType>& info) {
+    return info.param.test_name;
+  }
+);
+
 }  // namespace
 }  // namespace mujoco