Add contact labels showing which geom pairs are in contact.

PiperOrigin-RevId: 510422404
Change-Id: Ia73570098752e7c7175e4b5a5f2a0f8b08a6c006

doc/changelog.rst

@@ -20,6 +20,15 @@ Python bindings
   sizes that change between time steps depending on the number of active constraints, breaking strict correspondence
   between joints and `efc_` rows.
 
+.. image:: images/changelog/contactlabel.png
+   :align: right
+   :width: 400px
+
+Simulate
+^^^^^^^^
+
+- Added optional labels to contact visualization, indicating which two geoms are contacting (names if defined, ids
+  otherwise). This can be useful in cluttered scenes.
 
 Version 2.3.2 (February 7, 2023)
 --------------------------------

doc/includes/references.h

@@ -1565,6 +1565,7 @@ typedef enum mjtLabel_ {          // object labeling
   mjLABEL_SKIN,                   // skin labels
   mjLABEL_SELECTION,              // selected object
   mjLABEL_SELPNT,                 // coordinates of selection point
+  mjLABEL_CONTACTPOINT,           // contact information
   mjLABEL_CONTACTFORCE,           // magnitude of contact force
 
   mjNLABEL                        // number of label types

include/mujoco/mjvisualize.h

@@ -75,6 +75,7 @@ typedef enum mjtLabel_ {          // object labeling
   mjLABEL_SKIN,                   // skin labels
   mjLABEL_SELECTION,              // selected object
   mjLABEL_SELPNT,                 // coordinates of selection point
+  mjLABEL_CONTACTPOINT,           // contact information
   mjLABEL_CONTACTFORCE,           // magnitude of contact force
 
   mjNLABEL                        // number of label types

introspect/enums.py

@@ -471,8 +471,9 @@ ENUMS: Mapping[str, EnumDecl] = dict([
              ('mjLABEL_SKIN', 10),
              ('mjLABEL_SELECTION', 11),
              ('mjLABEL_SELPNT', 12),
-             ('mjLABEL_CONTACTFORCE', 13),
+             ('mjLABEL_CONTACTPOINT', 13),
-             ('mjNLABEL', 14),
+             ('mjLABEL_CONTACTFORCE', 14),
+             ('mjNLABEL', 15),
          ]),
      )),
     ('mjtFrame',

model/humanoid100/humanoid100.xml

@@ -74,7 +74,7 @@
 
   <worldbody>
     <light directional="true" diffuse=".8 .8 .8" pos="0 0 10" dir="0 0 -10"/>
-    <geom pos="0 0 0" type="plane" size="3 3 .5" material="MatPlane"/>
+    <geom name="floor" type="plane" size="3 3 .5" material="MatPlane"/>
     <geom class="border" fromto="-3 3 0 3 3 0"  />
     <geom class="border" fromto="-3 -3 0 3 -3 0"  />
     <geom class="border" fromto="3 3 0 3 -3 0"  />

simulate/simulate.cc

@@ -634,7 +634,7 @@ void makerendering(mj::Simulate* sim, int oldstate) {
       2,
       &(sim->vopt.label),
       "None\nBody\nJoint\nGeom\nSite\nCamera\nLight\nTendon\n"
-      "Actuator\nConstraint\nSkin\nSelection\nSel Pnt\nForce"
+      "Actuator\nConstraint\nSkin\nSelection\nSel Pnt\nContact\nForce"
     },
     {
       mjITEM_SELECT,

src/engine/engine_vis_init.c

@@ -46,6 +46,7 @@ const char* mjLABELSTRING[mjNLABEL] = {
   "Skin",
   "Selection",
   "SelPoint",
+  "Contact",
   "ContactForce"
 };
 

src/engine/engine_vis_visualize.c

@@ -129,6 +129,17 @@ static void addContactGeom(const mjModel* m, mjData* d, const mjtByte* flags,
         f2f(thisgeom->rgba, m->vis.rgba.contactgap, 4);
       }
 
+      // label contacting geom names or ids
+      if (vopt->label==mjLABEL_CONTACTPOINT) {
+        const char* name1 = mj_id2name(m, mjOBJ_GEOM, con->geom1);
+        char id1[10];
+        mjSNPRINTF(id1, "%d", con->geom1);
+        const char* name2 = mj_id2name(m, mjOBJ_GEOM, con->geom2);
+        char id2[10];
+        mjSNPRINTF(id2, "%d", con->geom2);
+        mjSNPRINTF(thisgeom->label, "%s | %s", name1 ? name1 : id1 , name2 ? name2 : id2);
+      }
+
       FINISH
     }
 

unity/Runtime/Bindings/MjBindings.cs

@@ -445,8 +445,9 @@ public enum mjtLabel : int{
   mjLABEL_SKIN = 10,
   mjLABEL_SELECTION = 11,
   mjLABEL_SELPNT = 12,
-  mjLABEL_CONTACTFORCE = 13,
+  mjLABEL_CONTACTPOINT = 13,
-  mjNLABEL = 14,
+  mjLABEL_CONTACTFORCE = 14,
+  mjNLABEL = 15,
 }
 public enum mjtFrame : int{
   mjFRAME_NONE = 0,