Compute moment of inertia for concave and boundary meshes. Resolve #338.

- Mesh inertias can now be computed exactly for well-formed (no holes) non-convex meshes.
- To activate this feature, set `<compiler exactmeshinertia="true">` (defaults to `false`). This default may change in the future.
- Added `<geom shellinertia="true/false">` (defaults to `false`). When true, geom inertia is computed assuming all the mass is concentrated on the surface, and `density` is interpreted as surface density (mass/area). Currently only mesh geoms are supported.

PiperOrigin-RevId: 464368395
Change-Id: I17afd99b9b221c9d24ae951f6e63d5c61ff89820

src/xml/xml_base.h

@@ -68,6 +68,7 @@ extern const mjMap gain_map[];
 extern const mjMap bias_map[];
 extern const mjMap stage_map[];
 extern const mjMap datatype_map[];
+extern const mjMap meshtype_map[];
 
 
 //---------------------------------- Base XML class ------------------------------------------------

src/xml/xml_native_reader.cc

@@ -46,10 +46,10 @@ static const int nMJCF = 163;
 static const char* MJCF[nMJCF][mjXATTRNUM] = {
 {"mujoco", "!", "1", "model"},
 {"<"},
-    {"compiler", "*", "17", "boundmass", "boundinertia", "settotalmass", "balanceinertia",
+    {"compiler", "*", "18", "boundmass", "boundinertia", "settotalmass", "balanceinertia",
         "strippath", "coordinate", "angle", "fitaabb", "eulerseq",
         "meshdir", "texturedir", "discardvisual", "convexhull", "usethread",
-        "fusestatic", "inertiafromgeom", "inertiagrouprange"},
+        "fusestatic", "inertiafromgeom", "inertiagrouprange", "exactmeshinertia"},
     {"<"},
         {"lengthrange", "?", "10", "mode", "useexisting", "uselimit",
             "accel", "maxforce", "timeconst", "timestep",
@@ -103,9 +103,9 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = {
             "limited", "solreflimit", "solimplimit",
             "solreffriction", "solimpfriction", "stiffness", "range", "margin",
             "ref", "springref", "armature", "damping", "frictionloss", "user"},
-        {"geom", "?", "30", "type", "pos", "quat", "contype", "conaffinity", "condim",
+        {"geom", "?", "31", "type", "pos", "quat", "contype", "conaffinity", "condim",
             "group", "priority", "size", "material", "friction", "mass", "density",
-            "solmix", "solref", "solimp",
+            "shellinertia", "solmix", "solref", "solimp",
             "margin", "gap", "fromto", "axisangle", "xyaxes", "zaxis", "euler",
             "hfield", "mesh", "fitscale", "rgba", "fluidshape", "fluidcoef", "user"},
         {"site", "?", "13", "type", "group", "pos", "quat", "material",
@@ -160,7 +160,7 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = {
     {"asset", "*", "0"},
     {"<"},
         {"texture", "*", "21", "name", "type", "file", "gridsize", "gridlayout",
-            "fileright", "fileleft", "fileup", "filedown","filefront", "fileback",
+            "fileright", "fileleft", "fileup", "filedown", "filefront", "fileback",
             "builtin", "rgb1", "rgb2", "mark", "markrgb", "random", "width", "height",
             "hflip", "vflip"},
         {"hfield", "*", "5", "name", "file", "nrow", "ncol", "size"},
@@ -186,9 +186,9 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = {
             "stiffness", "range", "margin", "ref", "springref", "armature", "damping",
             "frictionloss", "user"},
         {"freejoint", "*", "2", "name", "group"},
-        {"geom", "*", "32", "name", "class", "type", "contype", "conaffinity", "condim",
+        {"geom", "*", "33", "name", "class", "type", "contype", "conaffinity", "condim",
             "group", "priority", "size", "material", "friction", "mass", "density",
-            "solmix", "solref", "solimp",
+            "shellinertia", "solmix", "solref", "solimp",
             "margin", "gap", "fromto", "pos", "quat", "axisangle", "xyaxes", "zaxis", "euler",
             "hfield", "mesh", "fitscale", "rgba", "fluidshape", "fluidcoef", "user"},
         {"site", "*", "15", "name", "class", "type", "group", "pos", "quat",
@@ -608,6 +608,13 @@ const mjMap tkind_map[2] = {
 };
 
 
+// mesh type
+const mjMap  meshtype_map[2] = {
+    {"false", mjVOLUME_MESH},
+    {"true",  mjSHELL_MESH},
+  };
+
+
 
 //---------------------------------- class mjXReader implementation --------------------------------
 
@@ -793,6 +800,9 @@ void mjXReader::Compiler(XMLElement* section, mjCModel* mod) {
   }
   MapValue(section, "inertiafromgeom", &mod->inertiafromgeom, TFAuto_map, 3);
   ReadAttr(section, "inertiagrouprange", 2, mod->inertiagrouprange, text);
+  if (MapValue(section, "exactmeshinertia", &n, bool_map, 2)){
+    mod->exactmeshinertia = (n==1);
+  }
 
   // lengthrange subelement
   XMLElement* elem = FindSubElem(section, "lengthrange");
@@ -1149,6 +1159,11 @@ void mjXReader::OneGeom(XMLElement* elem, mjCGeom* pgeom) {
   ReadAttr(elem, "quat", 4, pgeom->quat, text);
   ReadAlternative(elem, pgeom->alt);
 
+  // compute inertia using either solid or shell geometry
+  if (MapValue(elem, "shellinertia", &n, meshtype_map, 2)) {
+    pgeom->typeinertia = (mjtMeshType)n;
+  }
+
   GetXMLPos(elem, pgeom);
 }
 

src/xml/xml_native_writer.cc

@@ -253,13 +253,14 @@ void mjXWriter::OneGeom(XMLElement* elem, mjCGeom* pgeom, mjCDef* def) {
       mjCMesh* pmesh = model->meshes[pgeom->meshid];
 
       // write pos/quat if there is a difference
-      if (!SameVector(pgeom->locpos, pmesh->pos, 3) ||
-          !SameVector(pgeom->locquat, pmesh->quat, 4)) {
+      if (!SameVector(pgeom->locpos, pmesh->GetPosPtr(pgeom->typeinertia), 3) ||
+          !SameVector(pgeom->locquat, pmesh->GetQuatPtr(pgeom->typeinertia), 4)) {
         // recover geom pos/quat before mesh frame transformation
         double p[3], q[4];
         mjuu_copyvec(p, pgeom->locpos, 3);
         mjuu_copyvec(q, pgeom->locquat, 4);
-        mjuu_frameaccuminv(p, q, pmesh->pos, pmesh->quat);
+        mjuu_frameaccuminv(p, q, pmesh->GetPosPtr(pgeom->typeinertia),
+                           pmesh->GetQuatPtr(pgeom->typeinertia));
 
         // write
         WriteAttr(elem, "pos", 3, p, unitq+1);
@@ -292,6 +293,7 @@ void mjXWriter::OneGeom(XMLElement* elem, mjCGeom* pgeom, mjCDef* def) {
   WriteAttr(elem, "gap", 1, &pgeom->gap, &def->geom.gap);
   WriteAttrKey(elem, "fluidshape", fluid_map, 2, pgeom->fluid_switch, def->geom.fluid_switch);
   WriteAttr(elem, "fluidcoef", 5, pgeom->fluid_coefs, def->geom.fluid_coefs);
+  WriteAttrKey(elem, "shellinertia", meshtype_map, 2, pgeom->typeinertia, def->geom.typeinertia);
   if (mjuu_defined(pgeom->_mass)) {
     double mass = pgeom->GetVolume() * def->geom.density;
     WriteAttr(elem, "mass", 1, &pgeom->mass, &mass);
@@ -657,6 +659,7 @@ void mjXWriter::Compiler(XMLElement* root) {
   if (!model->usethread) {
     WriteAttrTxt(section, "usethread", "false");
   }
+  WriteAttrTxt(section, "exactmeshinertia", FindValue(bool_map, 2, model->exactmeshinertia));
 }
 
 

src/xml/xml_util.h

@@ -46,7 +46,7 @@ class mjXError {
 
 
 // max number of attribute fields in schema (plus 3)
-#define mjXATTRNUM 35
+#define mjXATTRNUM 36
 
 
 // Custom XML file validation