Add XML compiler/saveinertial flag. Fixes #2405

PiperOrigin-RevId: 743607654
Change-Id: I1f2e61be89b79c1995e079f36aa285096f9fb441

Author: yuvaltassa

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 <body-inerital>` clauses for all bodies.
 
 .. _compiler-lengthrange:
 

doc/XMLschema.rst

@@ -54,7 +54,7 @@
 |                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
 |                                    |    |    | :ref:`discardvisual<compiler-discardvisual>`                    | :ref:`usethread<compiler-usethread>`                            | :ref:`fusestatic<compiler-fusestatic>`                          | :ref:`inertiafromgeom<compiler-inertiafromgeom>`                | |
 |                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
-|                                    |    |    | :ref:`inertiagrouprange<compiler-inertiagrouprange>`            | :ref:`assetdir<compiler-assetdir>`                              | :ref:`alignfree<compiler-alignfree>`                            |                                                                 | |
+|                                    |    |    | :ref:`inertiagrouprange<compiler-inertiagrouprange>`            | :ref:`saveinertial<compiler-saveinertial>`                      | :ref:`assetdir<compiler-assetdir>`                              | :ref:`alignfree<compiler-alignfree>`                            | |
 |                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
 +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
 | |_| compiler |br| |_| |L|          |    | .. table::                                                                                                                                                                                                                                                                   |

doc/changelog.rst

@@ -15,7 +15,9 @@ Upcoming version (not yet released)
 
 General
 ^^^^^^^
-- Add :ref:`orientation<body-composite-quat>` parameter to :ref:`composite<body-composite>`. Moreover, allow the
+- Added the :ref:`compiler/saveinertial<compiler-saveinertial>` flag, writing explicit inertial clauses for all
+  bodies when saving to XML.
+- Added :ref:`orientation<body-composite-quat>` attribute to :ref:`composite<body-composite>`. Moreover, allow the
   composite to be the direct child of a frame.
 
 Bug fixes

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;

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;

python/mujoco/introspect/structs.py

@@ -9137,6 +9137,11 @@
                  ),
                  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'),

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

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");

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);

test/xml/xml_native_writer_test.cc

@@ -22,7 +22,7 @@
 #include <array>
 #include <clocale>
 #include <cstdio>
-#include <filesystem>
+#include <filesystem>  // NOLINT(build/c++17)
 #include <string>
 #include <vector>
 
@@ -32,7 +32,6 @@
 #include <mujoco/mjmodel.h>
 #include <mujoco/mjtnum.h>
 #include <mujoco/mujoco.h>
-#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"(
+  <mujoco>
+    <compiler saveinertial="true"/>
+    <worldbody>
+      <body>
+        <geom type="box" size=".05 .05 .05"/>
+      </body>
+    </worldbody>
+  </mujoco>
+  )";
+  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"(
   <mujoco>
@@ -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);