diff --git a/src/user/user_mesh.cc b/src/user/user_mesh.cc index c4012201..a1e6b3d2 100644 --- a/src/user/user_mesh.cc +++ b/src/user/user_mesh.cc @@ -4363,7 +4363,8 @@ void mjCFlex::Compile(const mjVFS* vfs) { } // compute unrotated node positions for stiffness computation - std::vector nodexpos_local = ComputeUnrotatedNodePositions(nodexpos); + double R0[9] = {1, 0, 0, 0, 1, 0, 0, 0, 1}; // identity by default + std::vector nodexpos_local = ComputeUnrotatedNodePositions(nodexpos, R0); // reorder tetrahedra so right-handed face orientation is outside // faces are (0,1,2); (0,2,3); (0,3,1); (1,3,2) @@ -4623,16 +4624,54 @@ void mjCFlex::Compile(const mjVFS* vfs) { // compute bounding box coordinates vert0_.assign(3*nvert, 0); - const mjtNum* bvh = tree.Bvh().data(); - size[0] = bvh[3] - radius; - size[1] = bvh[4] - radius; - size[2] = bvh[5] - radius; - for (int j=0; j < nvert; j++) { - for (int k=0; k < 3; k++) { - if (size[k] > mjMINVAL) { - vert0_[3*j+k] = (vertxpos[3*j+k] - bvh[k]) / (2*size[k]) + 0.5; - } else { - vert0_[3*j+k] = 0.5; + + if (interpolated && nnode > 0) { + // for interpolated flex, compute vert0_ in the unrotated local frame + // to make parametric coordinates rotation-invariant + std::vector vertxpos_local(3*nvert); + for (int j = 0; j < nvert; j++) { + mjuu_mulvecmat(vertxpos_local.data()+3*j, vertxpos.data()+3*j, R0); + } + + // compute local-frame bounding box from unrotated node positions + double lo[3] = {1e30, 1e30, 1e30}; + double hi[3] = {-1e30, -1e30, -1e30}; + for (int i = 0; i < nnode; i++) { + for (int k = 0; k < 3; k++) { + lo[k] = std::min(lo[k], nodexpos_local[3*i+k]); + hi[k] = std::max(hi[k], nodexpos_local[3*i+k]); + } + } + + // set size from local bounding box + for (int k = 0; k < 3; k++) { + size[k] = (hi[k] - lo[k]) / 2; + } + + // normalize vertex positions within local bounding box + for (int j = 0; j < nvert; j++) { + for (int k = 0; k < 3; k++) { + double extent = hi[k] - lo[k]; + if (extent > mjMINVAL) { + vert0_[3*j+k] = (vertxpos_local[3*j+k] - lo[k]) / extent; + } else { + vert0_[3*j+k] = 0.5; + } + } + } + } else { + // non-interpolated: use BVH bounding box (original behavior) + const mjtNum* bvh = tree.Bvh().data(); + size[0] = bvh[3] - radius; + size[1] = bvh[4] - radius; + size[2] = bvh[5] - radius; + for (int j=0; j < nvert; j++) { + for (int k=0; k < 3; k++) { + if (size[k] > mjMINVAL) { + vert0_[3*j+k] = (vertxpos[3*j+k] - bvh[k]) / (2*size[k]) + 0.5; + } else { + vert0_[3*j+k] = 0.5; + } } } } @@ -4655,7 +4694,7 @@ void mjCFlex::Compile(const mjVFS* vfs) { // be computed from axis-aligned positions to preserve the diagonal Jacobian // assumption in ComputeLinearStiffness. std::vector mjCFlex::ComputeUnrotatedNodePositions( - const std::vector& nodexpos) const { + const std::vector& nodexpos, double* R0_out) const { std::vector nodexpos_local(3*nnode); if (interpolated && nnode > 0) { int ny_global = spec.cellcount[1] * spec.order + 1; @@ -4707,6 +4746,22 @@ std::vector mjCFlex::ComputeUnrotatedNodePositions( double lk = mjuu_normvec(R0+6, 3); (void)li; (void)lj; (void)lk; + // assert R0 is orthonormal (rows are the normalized edge vectors) + for (int a = 0; a < 3; a++) { + for (int b = a; b < 3; b++) { + double dot = mjuu_dot3(R0 + 3*a, R0 + 3*b); + double expected = (a == b) ? 1.0 : 0.0; + if (std::abs(dot - expected) > 1e-8) { + throw mjCError(this, "flex grid rotation R0 is not orthonormal"); + } + } + } + + // output R0 if requested + if (R0_out) { + mjuu_copyvec(R0_out, R0, 9); + } + // 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; diff --git a/src/user/user_objects.h b/src/user/user_objects.h index bb3bbed5..c8e63938 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -1055,7 +1055,9 @@ class mjCFlex: public mjCFlex_, private mjsFlex { std::vector node0_; // node Cartesian positions // compute unrotated node positions for stiffness computation - std::vector ComputeUnrotatedNodePositions(const std::vector& nodexpos) const; + // optionally outputs the grid rotation matrix R0 (stored as rows) + std::vector ComputeUnrotatedNodePositions( + const std::vector& nodexpos, double* R0_out = nullptr) const; // stiffness caching std::string ComputeStiffnessCacheKey() const; diff --git a/test/user/user_flex_test.cc b/test/user/user_flex_test.cc index c4d10e10..47bae7c8 100644 --- a/test/user/user_flex_test.cc +++ b/test/user/user_flex_test.cc @@ -392,9 +392,9 @@ TEST_F(UserFlexTest, TrilinearInterpolation) { EXPECT_EQ(m1->nflexvert, m2->nflexvert); for (int i = 0; i < 3*m1->nflexvert; ++i) { - EXPECT_EQ(m1->flex_vert[i], d2->flexvert_xpos[i]); - EXPECT_EQ(m1->flex_vert0[i], m2->flex_vert0[i]); - EXPECT_EQ(d1->flexvert_xpos[i], d2->flexvert_xpos[i]); + EXPECT_NEAR(m1->flex_vert[i], d2->flexvert_xpos[i], 1e-7); + EXPECT_NEAR(m1->flex_vert0[i], m2->flex_vert0[i], 1e-7); + EXPECT_NEAR(d1->flexvert_xpos[i], d2->flexvert_xpos[i], 1e-7); } EXPECT_EQ(m1->nM, m2->nM); @@ -1350,6 +1350,55 @@ TEST_F(UserFlexTest, Dof2d) { mj_deleteData(d_full); } +TEST_F(UserFlexTest, Vert0RotationInvariant) { + // unrotated trilinear grid + static constexpr char xml_unrotated[] = R"( + + + + + + + + + + )"; + + // same grid rotated 45 degrees around Z via parent body quaternion + static constexpr char xml_rotated[] = R"( + + + + + + + + + + )"; + + std::array error; + mjModel* m1 = LoadModelFromString(xml_unrotated, error.data(), error.size()); + ASSERT_THAT(m1, NotNull()) << error.data(); + + mjModel* m2 = LoadModelFromString(xml_rotated, error.data(), error.size()); + ASSERT_THAT(m2, NotNull()) << error.data(); + + // same number of vertices + ASSERT_EQ(m1->nflexvert, m2->nflexvert); + + // vert0 must be identical regardless of rotation + for (int i = 0; i < 3 * m1->nflexvert; ++i) { + EXPECT_NEAR(m1->flex_vert0[i], m2->flex_vert0[i], 1e-10) + << "vert0 mismatch at index " << i; + } + + mj_deleteModel(m1); + mj_deleteModel(m2); +} + } // namespace } // namespace mujoco diff --git a/test/xml/xml_native_writer_test.cc b/test/xml/xml_native_writer_test.cc index baa613fa..9a70372f 100644 --- a/test/xml/xml_native_writer_test.cc +++ b/test/xml/xml_native_writer_test.cc @@ -1404,7 +1404,10 @@ std::vector GetWriteReadTestModels() { absl::StrContains(xml, "hfield_xml") || absl::StrContains(xml, "fromto_convex") || absl::StrContains(xml, "cube_skin") || - absl::StrContains(xml, "cube_3x3x3")) { + absl::StrContains(xml, "cube_3x3x3") || + // exclude files that fail since we do not save pinned flex nodes + absl::StrContains(xml, "gripper_trilinear") || + absl::StrContains(xml, "strain")) { continue; } models.push_back(xml);