diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index 758e3335..e9507035 100644 --- a/doc/XMLreference.rst +++ b/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: diff --git a/doc/XMLschema.rst b/doc/XMLschema.rst index 2660f18b..a5edf0f0 100644 --- a/doc/XMLschema.rst +++ b/doc/XMLschema.rst @@ -104,6 +104,8 @@ | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | | | | | :ref:`offwidth` | :ref:`offheight` | :ref:`realtime` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +| | | | :ref:`ellipsoidinertia` | | | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ | |_| visual |br| |_| |L| | | .. table:: | | :ref:`quality | ? | :class: mjcf-attributes | diff --git a/doc/changelog.rst b/doc/changelog.rst index 8c2207f5..ddca9b23 100644 --- a/doc/changelog.rst +++ b/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` to visualize equivalent inertias with ellipsoids instead of the default boxes. + Bug fixes ^^^^^^^^^ diff --git a/doc/images/changelog/ellipsoidinertia.gif b/doc/images/changelog/ellipsoidinertia.gif new file mode 100644 index 00000000..239e4fc7 Binary files /dev/null and b/doc/images/changelog/ellipsoidinertia.gif differ diff --git a/doc/includes/references.h b/doc/includes/references.h index 3ac0cb44..c4c97b62 100644 --- a/doc/includes/references.h +++ b/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 diff --git a/include/mujoco/mjmodel.h b/include/mujoco/mjmodel.h index 0ffd26ef..fbe18093 100644 --- a/include/mujoco/mjmodel.h +++ b/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 diff --git a/python/mujoco/structs.cc b/python/mujoco/structs.cc index 907d01e8..a76a026b 100644 --- a/python/mujoco/structs.cc +++ b/python/mujoco/structs.cc @@ -1308,6 +1308,7 @@ PYBIND11_MODULE(_structs, m) { X(offwidth); X(offheight); X(treedepth); + X(ellipsoidinertia); #undef X py::class_ mjVisualQuality(mjVisual, "Quality"); diff --git a/src/engine/engine_io.c b/src/engine/engine_io.c index 9ed19509..62ba39dd 100644 --- a/src/engine/engine_io.c +++ b/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; diff --git a/src/engine/engine_vis_visualize.c b/src/engine/engine_vis_visualize.c index 92a4517f..e0ae56a0 100644 --- a/src/engine/engine_vis_visualize.c +++ b/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; inbody; 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 - sz[0] = mju_sqrt((m->body_inertia[3*i+1] + m->body_inertia[3*i+2] - - m->body_inertia[3*i+0]) *6/m->body_mass[i]) /2; - sz[1] = mju_sqrt((m->body_inertia[3*i+0] + m->body_inertia[3*i+2] - - m->body_inertia[3*i+1]) *6/m->body_mass[i]) /2; - 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; + mjtNum Ixx = m->body_inertia[3*i+0]; + mjtNum Iyy = m->body_inertia[3*i+1]; + mjtNum Izz = m->body_inertia[3*i+2]; + mjtNum mass = m->body_mass[i]; + mjtNum scale_inertia = ellipsoid ? mju_sqrt(5) : mju_sqrt(3); + + 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) { diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index 44319855..f2ea6c06 100644 --- a/src/xml/xml_native_reader.cc +++ b/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", - "offheight", "realtime"}, + {"global", "?", "10", "fovy", "ipd", "azimuth", "elevation", "linewidth", "glow", "offwidth", + "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}, - {"true", mjSHELL_MESH}, - }; - + {"false", mjVOLUME_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 diff --git a/src/xml/xml_native_writer.cc b/src/xml/xml_native_writer.cc index f32bd9d6..56ee77f5 100644 --- a/src/xml/xml_native_writer.cc +++ b/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); } diff --git a/test/engine/testdata/ellipsoidinertia.xml b/test/engine/testdata/ellipsoidinertia.xml new file mode 100644 index 00000000..8623add3 --- /dev/null +++ b/test/engine/testdata/ellipsoidinertia.xml @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index 574661fc..97730ff1 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/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)]