Add equivalent inertia ellipsoid visualization.
PiperOrigin-RevId: 519154067 Change-Id: I35f1993c1493df9be752b7163385004649498070
This commit is contained in:
Kevin Zakka
committed by
Copybara-Service
Copybara-Service
parent
2a471d1e11
commit
3f14d97f6b
@@ -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:
|
||||
|
||||
|
||||
@@ -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 |
|
||||
|
||||
@@ -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
|
||||
^^^^^^^^^
|
||||
|
||||
|
||||
Binary file not shown.
|
After Width: | Height: | Size: 5.9 MiB |
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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");
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -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
|
||||
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) {
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
|
||||
+10
@@ -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>
|
||||
@@ -1768,6 +1768,7 @@ public unsafe struct global {
|
||||
public int offwidth;
|
||||
public int offheight;
|
||||
public int treedepth;
|
||||
public int ellipsoidinertia;
|
||||
}
|
||||
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
|
||||
Reference in New Issue
Block a user