From fe8384b6c5b8fccb5e96563d8989aa6eb14feb73 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Thu, 25 Sep 2025 19:01:34 -0700 Subject: [PATCH] Visualize joint and tendon limits by re-coloring. PiperOrigin-RevId: 811590674 Change-Id: I5b65f2294a4bcda3c321fb51ebf29501bd0d416d --- doc/XMLreference.rst | 10 +++++--- doc/changelog.rst | 6 +++++ src/engine/engine_vis_visualize.c | 41 +++++++++++++++++++++++++++++-- test/engine/testdata/catenary.xml | 2 +- 4 files changed, 53 insertions(+), 6 deletions(-) diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index 2dc24856..de940c4d 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -4886,7 +4886,9 @@ length X, as in the clip on the right of `this example model :at:`rgba`: :at-val:`real(4), "0.5 0.5 0.5 1"` Color and transparency of the tendon. When this value is different from the internal default, it overrides the - corresponding material properties. + corresponding material properties. If a :at:`material` is unspecified and :at:`rgba` has the default value, limited + tendons whose length exceeds the limit are recolored using the value of the :ref:`constraint impedance` + :math:`d` to mix the default color and :ref:`rgba/constraint`. .. _tendon-spatial-springlength: @@ -8292,7 +8294,9 @@ disables the rendering of the corresponding object. .. _visual-rgba-joint: :at:`joint`: :at-val:`real(4), "0.2 0.6 0.8 1"` - Color of the arrows used to render joint axes. + Color of the arrows used to render joint axes. If a joint is limited and the joint value exceeds the limit, the + value of the :ref:`constraint impedance` :math:`d` is used to mix this color and + :ref:`rgba/constraint`. .. _visual-rgba-actuator: @@ -8368,7 +8372,7 @@ disables the rendering of the corresponding object. .. _visual-rgba-constraint: :at:`constraint`: :at-val:`real(4), "0.9 0 0 1"` - Color of the capsules corresponding to spatial constraint violations. + Color corresponding to spatial constraint violations -- equality constraints, joint limits, and tendon limits. .. _visual-rgba-slidercrank: diff --git a/doc/changelog.rst b/doc/changelog.rst index 25a5b8a2..8e367991 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -19,6 +19,12 @@ General **Migration:** Replace ``meshdir`` and ``texturedir`` with ``compiler.meshdir`` and ``compiler.texturedir``. +- Joint decorators and spatial tendons which have limits defined and whose current value (angle or length) exceeds the + limit, are recolored by using the :ref:`constraint impedance` :math:`d` to mix the existing color with + :ref:`visual/rgba/constraint`. For spatial tendons, this visualization aid is active only if + no :ref:`material` is set and :ref:`rgba` is default. + + Version 3.3.6 (September 15, 2025) ---------------------------------- diff --git a/src/engine/engine_vis_visualize.c b/src/engine/engine_vis_visualize.c index f8002619..ad5a23da 100644 --- a/src/engine/engine_vis_visualize.c +++ b/src/engine/engine_vis_visualize.c @@ -1462,7 +1462,24 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, mjERROR("unknown joint type %d", m->jnt_type[i]); } - f2f(thisgeom->rgba, m->vis.rgba.joint, 4); + // loop over limit constraints, get impedance if this joint is limited + mjtNum imp = 0; + int efc_start = d->ne + d->nf; + int efc_end = efc_start + d->nl; + for (int k=efc_start; k < efc_end; k++) { + if (d->efc_type[k] == mjCNSTR_LIMIT_JOINT && d->efc_id[k] == i) { + imp = d->efc_KBIP[4*k + 2]; + } + } + + // use impedance to mix joint and constraint colors + float rgba[4]; + rgba[0] = (1-imp) * m->vis.rgba.joint[0] + imp * m->vis.rgba.constraint[0]; + rgba[1] = (1-imp) * m->vis.rgba.joint[1] + imp * m->vis.rgba.constraint[1]; + rgba[2] = (1-imp) * m->vis.rgba.joint[2] + imp * m->vis.rgba.constraint[2]; + rgba[3] = 1; + + f2f(thisgeom->rgba, rgba, 4); // vopt->label if (vopt->label == mjLABEL_JOINT) { @@ -2290,8 +2307,28 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, mjv_connector(thisgeom, mjGEOM_CAPSULE, sz[0], d->wrap_xpos+3*j, d->wrap_xpos+3*j+3); // set material properties - float* rgba = m->tendon_rgba+4*i; int tendon_matid = m->tendon_matid[i]; + float rgba[4]; + f2f(rgba, m->tendon_rgba+4*i, 4); + + // if tendon has no material and the color is the default gray, re-color it using limit impedance + if (tendon_matid == -1 && rgba[0] == 0.5 && rgba[1] == 0.5 && rgba[2] == 0.5 && rgba[3] == 1) { + // loop over limit constraints, get impedance if this tendon is limited + mjtNum imp = 0; + int efc_start = d->ne + d->nf; + int efc_end = efc_start + d->nl; + for (int k=efc_start; k < efc_end; k++) { + if (d->efc_type[k] == mjCNSTR_LIMIT_TENDON && d->efc_id[k] == i) { + imp = d->efc_KBIP[4*k + 2]; + } + } + + // use impedance to mix tendon and constraint colors + rgba[0] = (1-imp) * rgba[0] + imp * m->vis.rgba.constraint[0]; + rgba[1] = (1-imp) * rgba[1] + imp * m->vis.rgba.constraint[1]; + rgba[2] = (1-imp) * rgba[2] + imp * m->vis.rgba.constraint[2]; + } + setMaterial(m, thisgeom, tendon_matid, rgba, vopt->flags); // override if visualizing islands diff --git a/test/engine/testdata/catenary.xml b/test/engine/testdata/catenary.xml index 3793848f..13d0b34d 100644 --- a/test/engine/testdata/catenary.xml +++ b/test/engine/testdata/catenary.xml @@ -22,7 +22,7 @@ - +