From ba149aa043718f6b2018804e27e4f96674b25fc3 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Tue, 21 Apr 2026 08:01:32 -0700 Subject: [PATCH] Fix flexcomp strain constraints with rotated grids. MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 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 --- src/user/user_mesh.cc | 83 +++++++++++- src/user/user_objects.h | 3 +- test/engine/engine_core_constraint_test.cc | 140 ++++++++++++++++++++- 3 files changed, 220 insertions(+), 6 deletions(-) diff --git a/src/user/user_mesh.cc b/src/user/user_mesh.cc index a7b7dc04..212381c8 100644 --- a/src/user/user_mesh.cc +++ b/src/user/user_mesh.cc @@ -4251,6 +4251,9 @@ void mjCFlex::Compile(const mjVFS* vfs) { } } + // compute unrotated node positions for stiffness computation + std::vector nodexpos_local = ComputeUnrotatedNodePositions(nodexpos); + // reorder tetrahedra so right-handed face orientation is outside // faces are (0,1,2); (0,2,3); (0,3,1); (1,3,2) if (dim == 3) { @@ -4410,7 +4413,7 @@ void mjCFlex::Compile(const mjVFS* vfs) { int gj = cj * spec.order + lj; int gk = ck * spec.order + lk; int global = gi * ny_global * nz_global + gj * nz_global + gk; - mjuu_copyvec(cell_pos.data() + 3*local, nodexpos.data() + 3*global, 3); + mjuu_copyvec(cell_pos.data() + 3*local, nodexpos_local.data() + 3*global, 3); local++; } } @@ -4453,14 +4456,88 @@ void mjCFlex::Compile(const mjVFS* vfs) { } } - // store node cartesian positions + // store node positions in unrotated (body-local) frame + // this ensures the runtime displacement refpos - R^{-1}*x is zero at rest node0_.assign(3*nnode, 0); for (int i=0; i < nnode; i++) { - mjuu_copyvec(node0_.data()+3*i, nodexpos.data()+3*i, 3); + mjuu_copyvec(node0_.data()+3*i, nodexpos_local.data()+3*i, 3); } } +// compute unrotated node positions for stiffness computation and node0_ +// +// the runtime corotational code extracts rotation R from the deformation +// gradient and computes displacement as R^{-1}*x - refpos; at rest R = R0 +// (the total grid rotation), so refpos must equal R0^{-1}*nodexpos to get +// zero displacement at rest; additionally, the stiffness eigenvectors must +// be computed from axis-aligned positions to preserve the diagonal Jacobian +// assumption in ComputeLinearStiffness. +std::vector mjCFlex::ComputeUnrotatedNodePositions( + const std::vector& nodexpos) const { + std::vector nodexpos_local(3*nnode); + if (interpolated && nnode > 0) { + int ny_global = spec.cellcount[1] * spec.order + 1; + int nz_global = spec.cellcount[2] * spec.order + 1; + + // find first non-empty cell + int cx = spec.cellcount[0], cy = spec.cellcount[1], cz = spec.cellcount[2]; + int ref_ci = 0, ref_cj = 0, ref_ck = 0; + bool found = false; + for (int ci = 0; ci < cx && !found; ci++) { + for (int cj = 0; cj < cy && !found; cj++) { + for (int ck = 0; ck < cz && !found; ck++) { + int cell_idx = ci * cy * cz + cj * cz + ck; + if (cell_empty.empty() || !cell_empty[cell_idx]) { + ref_ci = ci; ref_cj = cj; ref_ck = ck; + found = true; + } + } + } + } + + // corner indices of the reference cell (order=1 corners at local 0,0,0 + // and at offsets along each parametric axis) + int g000 = (ref_ci * spec.order) * ny_global * nz_global + + (ref_cj * spec.order) * nz_global + + (ref_ck * spec.order); + int g100 = ((ref_ci * spec.order) + spec.order) * ny_global * nz_global + + (ref_cj * spec.order) * nz_global + + (ref_ck * spec.order); + int g010 = (ref_ci * spec.order) * ny_global * nz_global + + ((ref_cj * spec.order) + spec.order) * nz_global + + (ref_ck * spec.order); + int g001 = (ref_ci * spec.order) * ny_global * nz_global + + (ref_cj * spec.order) * nz_global + + ((ref_ck * spec.order) + spec.order); + + // edge vectors (columns of the deformation gradient F = R * S) + // we store them as rows in R0 to use mjuu_mulvecmat for applying R0^{-1} + double R0[9]; + for (int d = 0; d < 3; d++) { + R0[0+d] = nodexpos[3*g100 + d] - nodexpos[3*g000 + d]; + R0[3+d] = nodexpos[3*g010 + d] - nodexpos[3*g000 + d]; + R0[6+d] = nodexpos[3*g001 + d] - nodexpos[3*g000 + d]; + } + + // normalize to get rotation matrix columns (valid for regular grids) + double li = mjuu_normvec(R0+0, 3); + double lj = mjuu_normvec(R0+3, 3); + double lk = mjuu_normvec(R0+6, 3); + (void)li; (void)lj; (void)lk; + + // apply inverse rotation to each nodexpos to get local-frame positions + for (int i = 0; i < nnode; i++) { + const double* p = nodexpos.data() + 3*i; + double* q = nodexpos_local.data() + 3*i; + mjuu_mulvecmat(q, p, R0); + } + } else { + nodexpos_local = nodexpos; + } + return nodexpos_local; +} + // create flex BVH void mjCFlex::CreateBVH() { diff --git a/src/user/user_objects.h b/src/user/user_objects.h index 4f4da58d..bb3bbed5 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -1054,7 +1054,8 @@ class mjCFlex: public mjCFlex_, private mjsFlex { std::vector vert0_; // vertex positions in [0, 1]^d in the bounding box std::vector node0_; // node Cartesian positions - + // compute unrotated node positions for stiffness computation + std::vector ComputeUnrotatedNodePositions(const std::vector& nodexpos) const; // stiffness caching std::string ComputeStiffnessCacheKey() const; diff --git a/test/engine/engine_core_constraint_test.cc b/test/engine/engine_core_constraint_test.cc index 93e3b947..07204df8 100644 --- a/test/engine/engine_core_constraint_test.cc +++ b/test/engine/engine_core_constraint_test.cc @@ -15,8 +15,6 @@ // Tests for engine/engine_core_constraint.c. #include -#include -#include #include #include @@ -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"( + + + )"; + + std::array 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({ + // 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