From f96f3e1c22ac1e231165b2a1fda8b68ddc143c6a Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Thu, 3 Apr 2025 10:11:35 -0700 Subject: [PATCH] Add XML `compiler/saveinertial` flag. Fixes #2405 PiperOrigin-RevId: 743607654 Change-Id: I1f2e61be89b79c1995e079f36aa285096f9fb441 --- doc/XMLreference.rst | 4 ++++ doc/XMLschema.rst | 2 +- doc/changelog.rst | 4 +++- doc/includes/references.h | 1 + include/mujoco/mjspec.h | 1 + python/mujoco/introspect/structs.py | 5 +++++ src/user/user_init.c | 1 + src/xml/xml_native_reader.cc | 9 ++++++--- src/xml/xml_native_writer.cc | 3 ++- test/xml/xml_native_writer_test.cc | 26 ++++++++++++++++++++++---- unity/Runtime/Bindings/MjBindings.cs | 1 + 11 files changed, 47 insertions(+), 10 deletions(-) diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index 4e5b4393..16ab04d7 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -832,6 +832,10 @@ has any effect. The settings here are global and apply to the entire model. necessary to adjust this attribute and the geom-specific groups so as to exclude world geoms from the inertial computation. +.. _compiler-saveinertial: + +:at:`saveinertial`: :at-val:`[false, true], "false"` + If set to "true", the compiler will save explicit :ref:`inertial ` clauses for all bodies. .. _compiler-lengthrange: diff --git a/doc/XMLschema.rst b/doc/XMLschema.rst index 6cbc0121..5af5fca5 100644 --- a/doc/XMLschema.rst +++ b/doc/XMLschema.rst @@ -54,7 +54,7 @@ | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | | | | | :ref:`discardvisual` | :ref:`usethread` | :ref:`fusestatic` | :ref:`inertiafromgeom` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`inertiagrouprange` | :ref:`assetdir` | :ref:`alignfree` | | | +| | | | :ref:`inertiagrouprange` | :ref:`saveinertial` | :ref:`assetdir` | :ref:`alignfree` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ | |_| compiler |br| |_| |L| | | .. table:: | diff --git a/doc/changelog.rst b/doc/changelog.rst index 155dff63..3f727ebb 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -15,7 +15,9 @@ Upcoming version (not yet released) General ^^^^^^^ -- Add :ref:`orientation` parameter to :ref:`composite`. Moreover, allow the +- Added the :ref:`compiler/saveinertial` flag, writing explicit inertial clauses for all + bodies when saving to XML. +- Added :ref:`orientation` attribute to :ref:`composite`. Moreover, allow the composite to be the direct child of a frame. Bug fixes diff --git a/doc/includes/references.h b/doc/includes/references.h index 8d1cf9c8..069b895a 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -1736,6 +1736,7 @@ typedef struct mjsCompiler_ { // compiler options mjtByte fusestatic; // fuse static bodies with parent int inertiafromgeom; // use geom inertias (mjtInertiaFromGeom) int inertiagrouprange[2]; // range of geom groups used to compute inertia + mjtByte saveinertial; // save explicit inertial clause for all bodies to XML int alignfree; // align free joints with inertial frame mjLROpt LRopt; // options for lengthrange computation } mjsCompiler; diff --git a/include/mujoco/mjspec.h b/include/mujoco/mjspec.h index 3764257b..65eb55a4 100644 --- a/include/mujoco/mjspec.h +++ b/include/mujoco/mjspec.h @@ -138,6 +138,7 @@ typedef struct mjsCompiler_ { // compiler options mjtByte fusestatic; // fuse static bodies with parent int inertiafromgeom; // use geom inertias (mjtInertiaFromGeom) int inertiagrouprange[2]; // range of geom groups used to compute inertia + mjtByte saveinertial; // save explicit inertial clause for all bodies to XML int alignfree; // align free joints with inertial frame mjLROpt LRopt; // options for lengthrange computation } mjsCompiler; diff --git a/python/mujoco/introspect/structs.py b/python/mujoco/introspect/structs.py index 2cd2ff27..73007b79 100644 --- a/python/mujoco/introspect/structs.py +++ b/python/mujoco/introspect/structs.py @@ -9137,6 +9137,11 @@ STRUCTS: Mapping[str, StructDecl] = dict([ ), doc='range of geom groups used to compute inertia', ), + StructFieldDecl( + name='saveinertial', + type=ValueType(name='mjtByte'), + doc='save explicit inertial clause for all bodies to XML', + ), StructFieldDecl( name='alignfree', type=ValueType(name='int'), diff --git a/src/user/user_init.c b/src/user/user_init.c index 76ea52ad..183034ce 100644 --- a/src/user/user_init.c +++ b/src/user/user_init.c @@ -42,6 +42,7 @@ void mjs_defaultSpec(mjSpec* spec) { spec->compiler.usethread = 1; spec->compiler.inertiafromgeom = mjINERTIAFROMGEOM_AUTO; spec->compiler.inertiagrouprange[1] = mjNGROUP-1; + spec->compiler.saveinertial = 0; mj_defaultLROpt(&spec->compiler.LRopt); // engine data diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index 35b1837c..695ceeac 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -97,10 +97,10 @@ static void UpdateString(string& psuffix, int count, int i) { const char* MJCF[nMJCF][mjXATTRNUM] = { {"mujoco", "!", "1", "model"}, {"<"}, - {"compiler", "*", "19", "autolimits", "boundmass", "boundinertia", "settotalmass", + {"compiler", "*", "20", "autolimits", "boundmass", "boundinertia", "settotalmass", "balanceinertia", "strippath", "coordinate", "angle", "fitaabb", "eulerseq", - "meshdir", "texturedir", "discardvisual", "usethread", - "fusestatic", "inertiafromgeom", "inertiagrouprange", "assetdir", "alignfree"}, + "meshdir", "texturedir", "discardvisual", "usethread", "fusestatic", "inertiafromgeom", + "inertiagrouprange", "saveinertial", "assetdir", "alignfree"}, {"<"}, {"lengthrange", "?", "10", "mode", "useexisting", "uselimit", "accel", "maxforce", "timeconst", "timestep", @@ -1015,6 +1015,9 @@ void mjXReader::Compiler(XMLElement* section, mjSpec* spec) { if (MapValue(section, "alignfree", &n, bool_map, 2)) { spec->compiler.alignfree = (n == 1); } + if (MapValue(section, "saveinertial", &n, bool_map, 2)) { + spec->compiler.saveinertial = (n == 1); + } // lengthrange subelement XMLElement* elem = FindSubElem(section, "lengthrange"); diff --git a/src/xml/xml_native_writer.cc b/src/xml/xml_native_writer.cc index 77a7dc6a..639ae081 100644 --- a/src/xml/xml_native_writer.cc +++ b/src/xml/xml_native_writer.cc @@ -1632,7 +1632,8 @@ void mjXWriter::Body(XMLElement* elem, mjCBody* body, mjCFrame* frame, string_vi WriteVector(elem, "user", body->get_userdata()); // write inertial - if (body->explicitinertial && model->compiler.inertiafromgeom != mjINERTIAFROMGEOM_TRUE) { + if (model->compiler.saveinertial || + (body->explicitinertial && model->compiler.inertiafromgeom != mjINERTIAFROMGEOM_TRUE)) { XMLElement* inertial = InsertEnd(elem, "inertial"); WriteAttr(inertial, "pos", 3, body->ipos); WriteAttr(inertial, "quat", 4, body->iquat, unitq); diff --git a/test/xml/xml_native_writer_test.cc b/test/xml/xml_native_writer_test.cc index 4de0a7fd..9bb31819 100644 --- a/test/xml/xml_native_writer_test.cc +++ b/test/xml/xml_native_writer_test.cc @@ -22,7 +22,7 @@ #include #include #include -#include +#include // NOLINT(build/c++17) #include #include @@ -32,7 +32,6 @@ #include #include #include -#include "src/cc/array_safety.h" #include "src/xml/xml_numeric_format.h" #include "test/fixture.h" @@ -135,6 +134,23 @@ TEST_F(XMLWriterTest, SavesDisableSensor) { mj_deleteModel(model); } +TEST_F(XMLWriterTest, SavesInertial) { + static constexpr char xml[] = R"( + + + + + + + + + )"; + mjModel* model = LoadModelFromString(xml); + std::string saved_xml = SaveAndReadXml(model); + EXPECT_THAT(saved_xml, HasSubstr("mass=\"1\"")); + mj_deleteModel(model); +} + TEST_F(XMLWriterTest, EmptyUserSensor) { static constexpr char xml[] = R"( @@ -957,8 +973,10 @@ TEST_F(XMLWriterTest, WritesSkin) { ASSERT_THAT(model, NotNull()); EXPECT_THAT(model->nskin, 1); - mjModel* mtemp = LoadModelFromString(SaveAndReadXml(model)); - ASSERT_THAT(mtemp, NotNull()); + char error[1024]; + mjModel* mtemp = LoadModelFromString(SaveAndReadXml(model), + error, sizeof(error)); + ASSERT_THAT(mtemp, NotNull()) << error; EXPECT_THAT(mtemp->nskin, 1); mj_deleteModel(model); diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index d7e79806..3fb3f812 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -5765,6 +5765,7 @@ public unsafe struct mjsCompiler_ { public byte fusestatic; public int inertiafromgeom; public fixed int inertiagrouprange[2]; + public byte saveinertial; public int alignfree; public mjLROpt_ LRopt; }