From 955392615857f500a300095fb3012a21af53c458 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Tue, 4 Aug 2026 08:03:25 -0700 Subject: [PATCH] Save flex node coordinates in the new nodecoord attribute. Interpolated flexes with pinned nodes could not be reloaded after saving: pinned nodes share their parent body, and their positions within it lived only in mjsFlex.node, which had no MJCF attribute. On reload the pinned nodes collapsed onto the parent body origin, degenerating the trilinear interpolation grid ("flex grid rotation R0 is not orthonormal"). This made model/flex/strain.xml and gripper_trilinear.xml fail to round-trip. Add flex/nodecoord, real(3*nnode), the node analog of flex/vertex: local node coordinates within the corresponding body frames. The reader picks it up from the regenerated schema tables; the writer emits it with the precision-aware WriteVector, since VectorToString ignores the XML precision setting and truncating node coordinates to 6 digits while body positions carry 17 fails the R0 orthonormality check at full precision. Add a WritesPinnedFlexNodes round-trip regression test, and remove the two write-read sweep exclusions documenting this bug. The removed substring filter "strain" was also matching core_constraint, silently excluding that entire testdata directory from the sweep; its ~40 models are now covered and pass. PiperOrigin-RevId: 959025281 Change-Id: I2fed28c01491c5a8431e813102a423d12b659911 --- doc/XMLreference.rst | 7 +++++++ doc/XMLschema.rst | 3 +++ doc/changelog.rst | 4 ++++ src/user/user_objects.h | 1 + src/xml/generated/mjcf.xsd | 5 +++++ src/xml/generated/mjcf_read_table.inc | 1 + src/xml/generated/mjcf_table.inc | 3 ++- src/xml/mjcf.schema | 1 + src/xml/xml_native_writer.cc | 3 +++ test/xml/xml_native_writer_test.cc | 29 +++++++++++++++++++++++++++ test/xml/xml_write_read_test.cc | 3 --- 11 files changed, 56 insertions(+), 4 deletions(-) diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index e4450406..2de70ddf 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -4389,6 +4389,13 @@ cases, the user will specify a :el:`flexcomp` which will then automatically cons An array of MuJoCo body names (separated by white space) to which each node belongs. The number of body names should equal the number of nodes (nnode). See the flexcomp :ref:`dof` attribute for more details. +.. _deformable-flex-nodecoord: + +:at:`nodecoord`: :at-val:`real(3*nnode), optional` + The local coordinates of the nodes within the corresponding body frames. If this attribute is omitted, all + coordinates are (0,0,0) or in other words, the nodes coincide with the centers of the body frames. Nonzero + coordinates are required when several nodes share a body, for example nodes pinned to a parent body. + .. _deformable-flex-cellcount: :at:`cellcount`: :at-val:`int(3), optional` diff --git a/doc/XMLschema.rst b/doc/XMLschema.rst index e8fd477d..85f3ffd4 100644 --- a/doc/XMLschema.rst +++ b/doc/XMLschema.rst @@ -1694,6 +1694,9 @@ .. grid-item:: :ref:`node` + .. grid-item:: + :ref:`nodecoord` + .. grid-item:: :ref:`cellcount` diff --git a/doc/changelog.rst b/doc/changelog.rst index f1c10b04..25396fc1 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -61,6 +61,10 @@ Models Bug fixes ^^^^^^^^^ +- Fixed a bug where models with pinned interpolated flex nodes (e.g. a :ref:`flexcomp` with + :at:`dof` "trilinear" and pinned vertices) could not be reloaded after saving: node coordinates within their body + frames were not saved, degenerating the interpolation grid. They are now saved in the new flex + :ref:`nodecoord` attribute. - Fixed a bug in the box-box collider where near-degenerate face clipping could generate contacts with spuriously large penetration depth between nearly touching thin boxes with positive margin, causing resting stacks to explode. - Fixed a bug in the box-box collider where penetrations deeper than a box's smallest half-size could produce no diff --git a/src/user/user_objects.h b/src/user/user_objects.h index 341cc980..b257000a 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -1043,6 +1043,7 @@ class mjCFlex: public mjCFlex_, private mjsFlex { const std::vector& get_texcoord() const { return texcoord_; } const std::vector& get_elemtexcoord() const { return elemtexcoord_; } const std::vector& get_nodebody() const { return nodebody_; } + const std::vector& get_node() const { return node_; } bool HasTexcoord() const; // texcoord not null void DelTexcoord(); // delete texcoord diff --git a/src/xml/generated/mjcf.xsd b/src/xml/generated/mjcf.xsd index 948be07c..03470869 100644 --- a/src/xml/generated/mjcf.xsd +++ b/src/xml/generated/mjcf.xsd @@ -2389,6 +2389,11 @@ space-separated body names + + + node positions within body frames + + seeded before reading diff --git a/src/xml/generated/mjcf_read_table.inc b/src/xml/generated/mjcf_read_table.inc index 5f8cb2bf..cae7c313 100644 --- a/src/xml/generated/mjcf_read_table.inc +++ b/src/xml/generated/mjcf_read_table.inc @@ -480,6 +480,7 @@ inline constexpr mjXAttr kFlexAttrs[] = { {"texcoord", mjXAttr::kFloatVec, 1, true, false, false, false, (int)offsetof(mjsFlex, texcoord)}, {"elemtexcoord", mjXAttr::kIntVec, 1, true, false, false, false, (int)offsetof(mjsFlex, elemtexcoord)}, {"node", mjXAttr::kStringVec, 1, true, false, false, false, (int)offsetof(mjsFlex, nodebody)}, + {"nodecoord", mjXAttr::kDoubleVec, 1, true, false, false, false, (int)offsetof(mjsFlex, node)}, }; inline constexpr int kFlexAttrsN = sizeof(kFlexAttrs) / sizeof(kFlexAttrs[0]); diff --git a/src/xml/generated/mjcf_table.inc b/src/xml/generated/mjcf_table.inc index ebc97710..7ce452dd 100644 --- a/src/xml/generated/mjcf_table.inc +++ b/src/xml/generated/mjcf_table.inc @@ -247,7 +247,8 @@ std::vector MJCF[] = { {"deformable", "*"}, {"<"}, {"flex", "*", "name", "group", "dim", "radius", "material", "rgba", "flatskin", "body", - "vertex", "element", "texcoord", "elemtexcoord", "node", "cellcount", "dof"}, + "vertex", "element", "texcoord", "elemtexcoord", "node", "nodecoord", "cellcount", + "dof"}, {"<"}, {"contact", "?", "contype", "conaffinity", "condim", "priority", "friction", "solmix", "solref", "solimp", "margin", "gap", "internal", "selfcollide", "activelayers", diff --git a/src/xml/mjcf.schema b/src/xml/mjcf.schema index 2c6a8bf7..dc25eaaf 100644 --- a/src/xml/mjcf.schema +++ b/src/xml/mjcf.schema @@ -1299,6 +1299,7 @@ element flex : mjsFlex { texcoord : float[] elemtexcoord : int[] node : string (field=nodebody) # space-separated body names + nodecoord : double[] (field=node) # node positions within body frames cellcount : int[3] = {1, 1, 1} (reading=custom) # seeded before reading dof : enum (reading=custom) # lowers to interpolation order child flexcomp_contact ? diff --git a/src/xml/xml_native_writer.cc b/src/xml/xml_native_writer.cc index 96c395fa..d987a090 100644 --- a/src/xml/xml_native_writer.cc +++ b/src/xml/xml_native_writer.cc @@ -175,6 +175,9 @@ void mjXWriter::OneFlex(XMLElement* elem, const mjCFlex* flex) { text = VectorToString(flex->get_nodebody()); WriteAttrTxt(elem, "node", text); } + if (!flex->get_node().empty()) { + WriteVector(elem, "nodecoord", flex->get_node()); + } // contact subelement XMLElement* cont = InsertEnd(elem, "contact"); diff --git a/test/xml/xml_native_writer_test.cc b/test/xml/xml_native_writer_test.cc index f7e2c57e..716266d8 100644 --- a/test/xml/xml_native_writer_test.cc +++ b/test/xml/xml_native_writer_test.cc @@ -936,6 +936,35 @@ TEST_F(XMLWriterTest, WritesSkin) { EXPECT_THAT(mtemp->nskin, 1); } +TEST_F(XMLWriterTest, WritesPinnedFlexNodes) { + static constexpr char xml[] = R"( + + + + + + + + + + + + )"; + + MjModelPtr model = LoadModelFromString(xml); + ASSERT_THAT(model.get(), NotNull()); + + // pinned nodes have no body of their own: their coordinates in the parent + // body frame must be saved or the interpolation grid degenerates on reload + std::string saved_xml = SaveAndReadXml(model.get()); + EXPECT_THAT(saved_xml, HasSubstr("nodecoord")); + + char error[1024]; + MjModelPtr mtemp = LoadModelFromString(saved_xml, error, sizeof(error)); + ASSERT_THAT(mtemp.get(), NotNull()) << error; + EXPECT_EQ(SaveAndReadXml(mtemp.get()), saved_xml); +} + TEST_F(XMLWriterTest, WritesHfield) { static constexpr char xml[] = R"( diff --git a/test/xml/xml_write_read_test.cc b/test/xml/xml_write_read_test.cc index e9776ed9..11afb3f9 100644 --- a/test/xml/xml_write_read_test.cc +++ b/test/xml/xml_write_read_test.cc @@ -67,9 +67,6 @@ std::vector GetWriteReadTestModels() { absl::StrContains(xml, "cube_3x3x3") || // flex_stiffness: stretch amplifies geometry XML rounds on save absl::StrContains(xml, "flex/bag") || - // exclude files that fail since we do not save pinned flex nodes - absl::StrContains(xml, "gripper_trilinear") || - absl::StrContains(xml, "strain") || // exclude conflict tests (known option conflict warnings/errors) absl::StrContains(xml, "xml/testdata/parent_")) { continue;