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