Add equivalent inertia ellipsoid visualization.

PiperOrigin-RevId: 519154067
Change-Id: I35f1993c1493df9be752b7163385004649498070

doc/XMLreference.rst

@@ -898,6 +898,12 @@ is effectively a miscellaneous subsection.
 :at:`offheight`: :at-val:`int, "480"`
    This attribute specifies the height in pixels of the OpenGL off-screen rendering buffer.
 
+.. _visual-global-ellipsoidinertia:
+
+:at:`ellipsoidinertia`: :at-val:`[false, true], "false"`
+   This attribute specifies how the equivalent inertia is visualized. "false":
+   use box, "true": use ellipsoid.
+
 
 .. _visual-quality:
 

doc/XMLschema.rst

@@ -104,6 +104,8 @@
 |                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
 |                                    |    |    | :ref:`offwidth<visual-global-offwidth>`                         | :ref:`offheight<visual-global-offheight>`                       | :ref:`realtime<visual-global-realtime>`                         | |
 |                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+|                                    |    |    | :ref:`ellipsoidinertia<visual-global-ellipsoidinertia>`         |                                                                 |                                                                 | |
+|                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
 +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
 | |_| visual |br| |_| |L|            |    | .. table::                                                                                                                                                                                                 |
 | :ref:`quality                      | ?  |    :class: mjcf-attributes                                                                                                                                                                                 |

doc/changelog.rst

@@ -5,6 +5,15 @@ Changelog
 Upcoming version (not yet released)
 -----------------------------------
 
+General
+^^^^^^^
+
+.. image:: images/changelog/ellipsoidinertia.gif
+   :align: right
+   :width: 320px
+
+- Added :ref:`ellipsoidinertia<visual-global-ellipsoidinertia>` to visualize equivalent inertias with ellipsoids instead of the default boxes.
+
 Bug fixes
 ^^^^^^^^^
 

doc/includes/references.h

@@ -654,6 +654,7 @@ struct mjVisual_ {                // visualization options
     int offwidth;                 // width of offscreen buffer
     int offheight;                // height of offscreen buffer
     int treedepth;                // depth of the bounding volume hierarchy
+    int ellipsoidinertia;         // geom for inertia visualization (0: box, 1: ellipsoid)
   } global;
 
   struct {                        // rendering quality

include/mujoco/mjmodel.h

@@ -432,6 +432,7 @@ struct mjVisual_ {                // visualization options
     int offwidth;                 // width of offscreen buffer
     int offheight;                // height of offscreen buffer
     int treedepth;                // depth of the bounding volume hierarchy
+    int ellipsoidinertia;         // geom for inertia visualization (0: box, 1: ellipsoid)
   } global;
 
   struct {                        // rendering quality

python/mujoco/structs.cc

@@ -1308,6 +1308,7 @@ PYBIND11_MODULE(_structs, m) {
   X(offwidth);
   X(offheight);
   X(treedepth);
+  X(ellipsoidinertia);
 #undef X
 
   py::class_<raw::MjVisualQuality> mjVisualQuality(mjVisual, "Quality");

src/engine/engine_io.c

@@ -145,6 +145,7 @@ void mj_defaultVisual(mjVisual* vis) {
   vis->global.offheight           = 480;
   vis->global.realtime            = 1.0;
   vis->global.treedepth           = 1;
+  vis->global.ellipsoidinertia    = 0;
 
   // rendering quality
   vis->quality.shadowsize         = 4096;

src/engine/engine_vis_visualize.c

@@ -588,35 +588,41 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
   // inertia
   objtype = mjOBJ_BODY;
   if (vopt->flags[mjVIS_INERTIA]) {
+    int ellipsoid = m->vis.global.ellipsoidinertia == 1;
     for (int i=1; i<m->nbody; i++) {
       // skip if mass too small or if this body is static and static bodies are masked
       if (m->body_mass[i]>mjMINVAL && (bodycategory(m, i) & catmask)) {
         START
 
-        // compute sizes of equivalent box
+        mjtNum Ixx = m->body_inertia[3*i+0];
-        sz[0] = mju_sqrt((m->body_inertia[3*i+1] + m->body_inertia[3*i+2] -
+        mjtNum Iyy = m->body_inertia[3*i+1];
-                          m->body_inertia[3*i+0]) *6/m->body_mass[i]) /2;
+        mjtNum Izz = m->body_inertia[3*i+2];
-        sz[1] = mju_sqrt((m->body_inertia[3*i+0] + m->body_inertia[3*i+2] -
+        mjtNum mass = m->body_mass[i];
-                          m->body_inertia[3*i+1]) *6/m->body_mass[i]) /2;
+        mjtNum scale_inertia = ellipsoid ? mju_sqrt(5) : mju_sqrt(3);
-        sz[2] = mju_sqrt((m->body_inertia[3*i+0] + m->body_inertia[3*i+1] -
+
-                          m->body_inertia[3*i+2]) *6/m->body_mass[i]) /2;
+        sz[0] = mju_sqrt((Iyy + Izz - Ixx) / (2 * mass)) * scale_inertia;
+        sz[1] = mju_sqrt((Ixx + Izz - Iyy) / (2 * mass)) * scale_inertia;
+        sz[2] = mju_sqrt((Ixx + Iyy - Izz) / (2 * mass)) * scale_inertia;
 
         // scale with mass if enabled
         if (vopt->flags[mjVIS_SCLINERTIA]) {
           // density = mass / volume
-          mjtNum density = m->body_mass[i] / mju_max(mjMINVAL, 8*sz[0]*sz[1]*sz[2]);
+          mjtNum scale_volume = ellipsoid ? 4.0/3.0*mjPI : 8.0;
+          mjtNum volume = scale_volume * sz[0]*sz[1]*sz[2];
+          mjtNum density = mass / mju_max(mjMINVAL, volume);
 
           // scale = root3(density)
           mjtNum scl = mju_pow(density*0.001, 1.0/3.0);
 
-          // scale sizes, so that box with density of 1000 has same mass
+          // scale sizes, so that box/ellipsoid with density of 1000 has same mass
           sz[0] *= scl;
           sz[1] *= scl;
           sz[2] *= scl;
         }
 
         // construct geom
-        mjv_initGeom(thisgeom, mjGEOM_BOX, sz, d->xipos+3*i, d->ximat+9*i, m->vis.rgba.inertia);
+        mjtGeom type = ellipsoid ? mjGEOM_ELLIPSOID : mjGEOM_BOX;
+        mjv_initGeom(thisgeom, type, sz, d->xipos+3*i, d->ximat+9*i, m->vis.rgba.inertia);
 
         // glow
         if (pert->select==i) {

src/xml/xml_native_reader.cc

@@ -113,8 +113,8 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = {
 
     {"visual", "*", "0"},
     {"<"},
-        {"global", "?", "9", "fovy", "ipd", "azimuth", "elevation", "linewidth", "glow", "offwidth",
+        {"global", "?", "10", "fovy", "ipd", "azimuth", "elevation", "linewidth", "glow", "offwidth",
-            "offheight", "realtime"},
+            "offheight", "realtime", "ellipsoidinertia"},
         {"quality", "?", "5", "shadowsize", "offsamples", "numslices", "numstacks",
             "numquads"},
         {"headlight", "?", "4", "ambient", "diffuse", "specular", "active"},
@@ -695,10 +695,9 @@ const mjMap tkind_map[2] = {
 
 // mesh type
 const mjMap  meshtype_map[2] = {
-    {"false", mjVOLUME_MESH},
+  {"false", mjVOLUME_MESH},
-    {"true",  mjSHELL_MESH},
+  {"true",  mjSHELL_MESH},
-  };
+};
-
 
 
 //---------------------------------- class mjXReader implementation --------------------------------
@@ -2368,10 +2367,15 @@ void mjXReader::Visual(XMLElement* section) {
       ReadAttr(elem,    "glow",      1, &vis->global.glow,      text);
       ReadAttrInt(elem, "offwidth",     &vis->global.offwidth);
       ReadAttrInt(elem, "offheight",    &vis->global.offheight);
-      if (ReadAttr(elem, "realtime", 1, &vis->global.realtime, text))
+      if (ReadAttr(elem, "realtime", 1, &vis->global.realtime, text)) {
         if (vis->global.realtime<=0) {
           throw mjXError(elem, "realtime must be greater than 0");
         }
+      }
+      int ellipsoidinertia;
+      if (MapValue(elem, "ellipsoidinertia", &ellipsoidinertia, bool_map, 2)) {
+        vis->global.ellipsoidinertia = (ellipsoidinertia==1);
+      }
     }
 
     // quality sub-element

src/xml/xml_native_writer.cc

@@ -887,6 +887,7 @@ void mjXWriter::Visual(XMLElement* root) {
   WriteAttr(elem,    "realtime",  1,   &vis->global.realtime,   &visdef.global.realtime);
   WriteAttrInt(elem, "offwidth",       vis->global.offwidth,    visdef.global.offwidth);
   WriteAttrInt(elem, "offheight",      vis->global.offheight,   visdef.global.offheight);
+  WriteAttrKey(elem, "ellipsoidinertia", bool_map, 2, vis->global.ellipsoidinertia, visdef.global.ellipsoidinertia);
   if (!elem->FirstAttribute()) {
     section->DeleteChild(elem);
   }

test/engine/testdata/ellipsoidinertia.xml

@@ -0,0 +1,10 @@
+<mujoco>
+  <visual>
+    <global ellipsoidinertia="true"/>
+  </visual>
+  <worldbody>
+    <body pos="0 0 .3">
+      <geom type="capsule" size=".01" fromto="0 0 0 .2 0 0"/>
+    </body>
+  </worldbody>
+</mujoco>

unity/Runtime/Bindings/MjBindings.cs

@@ -1768,6 +1768,7 @@ public unsafe struct global {
   public int offwidth;
   public int offheight;
   public int treedepth;
+  public int ellipsoidinertia;
 }
 
 [StructLayout(LayoutKind.Sequential)]