Move tactile sensor to the engine.

PiperOrigin-RevId: 788863133 Change-Id: I3172ebb7641fa8146469da25cba7db794760e7b1
2025-07-30 05:26:39 -07:00
parent 89f4789085
commit 51babec9c5
28 changed files with 418 additions and 687 deletions
@@ -844,6 +844,86 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
    }
  }

+  // tactile sensor
+  category = mjCAT_DECOR;
+  objtype = mjOBJ_UNKNOWN;
+  if (vopt->flags[mjVIS_CONTACTPOINT]) {
+    mj_markStack(d);
+
+    for (int id = 0; id < m->nsensor; id++) {
+      if (m->sensor_type[id] == mjSENS_TACTILE) {
+        // get site id and frame
+        int mesh_id = m->sensor_objid[id];
+        int geom_id = m->sensor_refid[id];
+        mjtNum* geom_pos = d->geom_xpos + 3*geom_id;
+        mjtNum* geom_mat = d->geom_xmat + 9*geom_id;
+        mjtNum geom_quat[4];
+        mju_mat2Quat(geom_quat, geom_mat);
+
+        // get sensor data
+        mjtNum* sensordata = d->sensordata + m->sensor_adr[id];
+        int nchannel = m->sensor_dim[id] / m->mesh_vertnum[mesh_id];
+
+        // get maximum absolute normal force
+        mjtNum maxval = 0;
+        for (int j=0; j < m->mesh_vertnum[mesh_id]; j++) {
+          maxval = mju_max(maxval, mju_abs(sensordata[j]));
+        }
+
+        // if no normal force readings, quick return
+        if (!maxval || m->geom_rbound[geom_id] < mjMINVAL) {
+          continue;
+        }
+
+        // draw geoms
+        float* mesh_vert = m->mesh_vert + 3*m->mesh_vertadr[mesh_id];
+        int* face = m->mesh_face + 3*m->mesh_faceadr[mesh_id];
+        for (int i=0; i < m->mesh_facenum[mesh_id]; i++) {
+          if (scn->ngeom >= scn->maxgeom) {
+            mj_warning(d, mjWARN_VGEOMFULL, scn->maxgeom);
+            mj_freeStack(d);
+            return;
+          } else {
+            // triangle in global frame
+            mjtNum pos[3][3];
+            for (int j = 0; j < 3; j++) {
+              mjtNum v[3] = {mesh_vert[3 * face[3 * i + j] + 0],
+                             mesh_vert[3 * face[3 * i + j] + 1],
+                             mesh_vert[3 * face[3 * i + j] + 2]};
+              mju_mulMatVec3(pos[j], geom_mat, v);
+              mju_addTo3(pos[j], geom_pos);
+            }
+
+            // color
+            float rgba[4] = {0, 0, 0, 1.0};
+            mjtNum nval[3] = {0, 0, 0};
+            for (int r = 0; r < mjMIN(nchannel, 3); r++) {
+              for (int j = 0; j < 3; j++) {
+                mjtNum val = sensordata[r*m->mesh_vertnum[mesh_id] + face[3*i+j]];
+                rgba[r] += mju_abs(val) / maxval;
+                if (val) {
+                  nval[r] += 1;
+                }
+              }
+              if (nval[r]) {
+                rgba[r] /= nval[r];
+              }
+            }
+
+            // draw triangles, one per side
+            for (int j = 0; j < 2; j++) {
+              START
+              makeTriangle(thisgeom, pos[0], pos[j ? 1 : 2], pos[j ? 2 : 1], rgba);
+              thisgeom->objid = id;
+              FINISH
+            }
+          }
+        }
+      }
+    }
+    mj_freeStack(d);
+  }
+
  // inertia
  objtype = mjOBJ_BODY;
  if (vopt->flags[mjVIS_INERTIA]) {