Add adhesion actuators.

- Adhesion actuators using contact normals as force transmission mechanism.
- Related video: https://youtu.be/HdBue4MUZys

Closes #229

PiperOrigin-RevId: 464389367
Change-Id: I9f69b3cd152d957e8f65870d208788463c036a6d

src/engine/engine_core_smooth.c

@@ -785,6 +785,48 @@ void mj_transmission(const mjModel* m, mjData* d) {
       mju_addTo(moment+i*nv, jac, nv);                            // add the two
       break;
 
+    case mjTRN_BODY:                  // body (adhesive contacts)
+      // cannot compute meaningful length, set to 0
+      length[i] = 0;
+
+      // moment is average of all contact normal Jacobians
+      {
+        // find and count all relevant contacts, mark them in efc_force
+        int counter = 0;
+        mjtNum* efc_force = mj_stackAlloc(d, d->nefc);
+        mju_zero(efc_force, d->nefc);
+        for (int j=0; j<d->ncon; j++) {
+          const mjContact* con = d->contact+j;
+          if (m->geom_bodyid[con->geom1]==id || m->geom_bodyid[con->geom2]==id) {
+            if (!con->exclude) {
+              counter++;
+
+              // condim 1 or elliptic cones: normal is in the first row
+              if (con->dim == 1 || m->opt.cone==mjCONE_ELLIPTIC) {
+                efc_force[con->efc_address] = 1;
+              }
+
+              // pyramidal cones: average all pyramid directions
+              else {
+                int npyramid = con->dim-1;  // number of frictional directions
+                for (int k=0; k<2*npyramid; k++) {
+                  efc_force[con->efc_address+k] = 0.5/npyramid;
+                }
+              }
+            } else if (con->exclude == 1) {
+              //  TODO(b/240848298): compute Jacobians for excluded contact (in gap)
+            }
+          }
+        }
+
+        // moment is average over contact normal Jacobians, make negative for adhesion
+        if (counter) {
+          mj_mulJacTVec(m, d, moment+i*nv, efc_force);
+          mju_scl(moment+i*nv, moment+i*nv, -1.0/counter, nv);
+        }
+      }
+      break;
+
     default:
       mju_error_i("Unknown transmission type %d", m->actuator_trntype[i]);  // SHOULD NOT OCCUR
     }

src/engine/engine_forward.c

@@ -97,7 +97,6 @@ void mj_fwdPosition(const mjModel* m, mjData* d) {
   mj_comPos(m, d);
   mj_camlight(m, d);
   mj_tendon(m, d);
-  mj_transmission(m, d);
   TM_END(mjTIMER_POS_KINEMATICS);
 
   TM_RESTART;
@@ -111,6 +110,7 @@ void mj_fwdPosition(const mjModel* m, mjData* d) {
 
   TM_RESTART;
   mj_makeConstraint(m, d);
+  mj_transmission(m, d);
   TM_END(mjTIMER_POS_MAKE);
 
   TM_RESTART;

src/engine/engine_vis_visualize.c

@@ -847,8 +847,8 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
 
           // site actuators
           if (m->actuator_trntype[i]==mjTRN_SITE) {
-            // set size of the geometry
-            mju_scl3(sz, m->site_size+3*j, 1.1);
+            // inflate sizes by 5%
+            mju_scl3(sz, m->site_size+3*j, 1.05);
 
             // make geom
             mjv_initGeom(thisgeom,
@@ -895,6 +895,34 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
           FINISH
         }
 
+        // body actuators
+        else if (m->actuator_trntype[i]==mjTRN_BODY) {
+          // iterate over body's geoms
+          int geomnum = m->body_geomnum[j];
+          int geomadr = m->body_geomadr[j];
+          for (int k=geomadr; k<geomadr+geomnum; k++) {
+            int geomtype = m->geom_type[k];
+            // add inflated geom if it is a regular primitive
+            if (geomtype != mjGEOM_PLANE && geomtype != mjGEOM_HFIELD && geomtype != mjGEOM_MESH) {
+              START
+              // inflate sizes by 5%
+              mju_scl3(sz, m->geom_size+3*k, 1.05);
+
+              // make geom
+              mjv_initGeom(thisgeom,
+                           m->geom_type[k], sz,
+                           d->geom_xpos + 3*k,
+                           d->geom_xmat + 9*k,
+                           thisgeom->rgba);
+
+              // set interpolated color
+              f2f(thisgeom->rgba, rgba, 4);
+
+              FINISH
+            }
+          }
+        }
+
         // spatial tendon actuators
         else if (m->actuator_trntype[i]==mjTRN_TENDON && d->ten_wrapnum[j]) {
           for (int k=d->ten_wrapadr[j]; k<d->ten_wrapadr[j]+d->ten_wrapnum[j]-1; k++) {