Add support for excluded contacts in adhesion actuators.

- Update example model.
- Update video.

PiperOrigin-RevId: 468464196
Change-Id: I603c5a89c2d5a736de127440e6d542beffc12583

doc/XMLreference.rst

@@ -3409,7 +3409,8 @@ helps clarify the role of bodies and geoms in MuJoCo.
    assumption of uniform density. The internal default of 1000 is the density of water in SI units. This attribute is
    used only when the mass attribute above is unspecified.
 :at:`shellinertia` :at-val:`[false, true], "false"`
-   If true, the geom's inertia is computed assuming that all the mass is concentrated on the boundary. In this case :at:`density` is interpreted as surface density rather than volumetric density.
+   If true, the geom's inertia is computed assuming that all the mass is concentrated on the boundary. In this case
+   :at:`density` is interpreted as surface density rather than volumetric density.
 :at:`solmix`: :at-val:`real, "1"`
    This attribute specifies the weight used for averaging of contact parameters, and interacts with the priority
    attribute. See :ref:`CContact`.
@@ -4616,18 +4617,22 @@ This element has nine custom attributes in addition to the common attributes:
 :el-prefix:`actuator/` **adhesion** (*)
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
-..  youtube:: HdBue4MUZys
+..  youtube:: BcHZ5BFeTmU
     :align: right
-    :height: 200px
+    :height: 150px
 
-This element defines an active adhesion actuator which injects force at contacts in the normal direction. On the right
+This element defines an active adhesion actuator which injects forces at contacts in the normal direction, see
-is a video demonstrating this actuator type. The model shown in the video can be found `here
+illustration video. The model shown in the video can be found `here
-<https://github.com/deepmind/mujoco/tree/main/model/adhesion>`_. The transmission target is a :el:`body`, and adhesive
+<https://github.com/deepmind/mujoco/tree/main/model/adhesion>`_ and includes inline annotations. The transmission target
-forces are injected into all contacts involving geoms which belong to this body. The force is divided equally between
+is a :el:`body`, and adhesive forces are injected into all contacts involving geoms which belong to this body. The force
-multiple active contacts. Because it requires contact, it cannot apply a force at a distance, and is more like the active
+is divided equally between multiple contacts. When the :at:`gap` attribute is not used, this actuator requires active
-adhesion on the feet of geckos and insects rather than an industrial vacuum gripper. Adhesion actuators' length is
+contacts and cannot apply a force at a distance, more like the active adhesion on the feet of geckos and insects rather
-always 0. :at:`ctrlrange` is required and must also be nonnegative (no repulsive forces are allowed). The underlying
+than an industrial vacuum gripper. In order to enable "suction at a distance", "inflate" the body's geoms by
-:el:`general` attributes are set as follows:
+:at:`margin` and add a corresponding :at:`gap` which activates contacts only after :at:`gap` penetration distance. This
+will create a layer around the geom where contacts are detected but are inactive, and can be used for
+applying the adhesive force. In the video above, such inactive contacts are blue, while active contacts are orange.
+An adhesion actuator's length is always 0. :at:`ctrlrange` is required and must also be nonnegative (no repulsive forces
+are allowed). The underlying :el:`general` attributes are set as follows:
 
 =========== ======= =========== ========
 Attribute   Setting Attribute   Setting

doc/changelog.rst

@@ -7,15 +7,20 @@ Upcoming version (not yet released)
 
 General
 ^^^^^^^
+
+..  youtube:: BcHZ5BFeTmU
+    :align: right
+    :height: 150px
+
 - Added :ref:`adhesion actuators<adhesion>`.
-- Added an `active adhesion example model <https://github.com/deepmind/mujoco/tree/main/model/adhesion>`_.
+- Added an `active adhesion example <https://github.com/deepmind/mujoco/tree/main/model/adhesion>`_ and video:
 - Added :ref:`mj_jacSubtreeCom` for computing the translational Jacobian of the center-of-mass of a subtree.
 - Added moment of inertia computation for concave meshes. This is currently activated by setting the compiler flag
   :at:`exactmeshinertia` to ``true`` (defaults to ``false``). This default may change in the future.
-- Added parameter :at:`shellinertia` in :at:`geom` for treating a mesh as a boundary mesh (shell) for inertia
+- Added parameter :at:`shellinertia` to :at:`geom`, for locating the inferred inertia on the boundary (shell).
-  computations. This is currently supported only for meshes.
+  Currently only meshes are supported.
-- Raise error if the orientation of mesh faces is not consistent, which causes the inertia computations to be
+- For meshes from which volumetric inertia is inferred, raise error if the orientation of mesh faces is not consistent.
-  inaccurate. If this occurs, open the mesh in MeshLab or Blender and recalculate the faces.
+  If this occurs, fix the mesh in e.g., MeshLab or Blender.
 - Added ``azimuth`` and ``elevation`` attributes to :ref:`visual/global<global>`, defining the initial orientation of
   the free camera at model load time.
 - Added ``mjv_defaultFreeCamera`` which sets the default free camera, respecting the above attributes.

model/adhesion/active_adhesion.xml

@@ -1,4 +1,8 @@
 <mujoco model="Active adhesion example">
+  <!--
+    Adding some fluid viscosity to prevent the hanging sphere from jiggling too much.
+  -->
+  <option viscosity="1"/>
 
   <size nconmax="500" njmax="1000"/>
 
@@ -8,12 +12,7 @@
 
   <default>
     <joint damping=".3" axis="0 1 0"/>
-    <!--
+    <geom type="box" friction=".5"/>
-      By adding 3mm of margin yet making solimp barely apply any force until 3mm penetration, we get
-      3mm of "action at a distance". This is important so that small changes in distance don't lead
-      to loss of adhesion
-    -->
-    <geom type="box" friction=".5" margin=".003" solimp="0 .99 .003 .9 6"/>
     <default class="wall">
       <geom rgba=".5 .5 .5 .4"/>
     </default>
@@ -22,7 +21,12 @@
       <tendon rgba=".5 .5 .8 1"/>
     </default>
     <default class="active_adhesion">
-      <geom rgba=".8 .5 .5 1"/>
+      <!--
+        Geoms in the active_adhesion class are "inflated" with a margin of 1cm, but contacts are
+        activated only at a depth gap of 1cm, leading to a layer of inactive contacts at a 1cm
+        distance from the surface. However the contacts can be used by the adhesion actuator.
+      -->
+      <geom rgba=".8 .5 .5 1" margin=".01" gap=".01"/>
     </default>
     <default class="object">
       <geom rgba=".5 .8 .5 1" density="100"/>
@@ -77,18 +81,15 @@
     <site name="hook_left" pos=".08 0 .3"/>
     <site name="hook_right" pos=".12 0 .3"/>
     <body name="sphere" pos=".1 0 .2" childclass="active_adhesion">
-      <!--
+      <freejoint/>
-        Note we are adding damping to a free joint. This is an easy yet unphysical way to prevent
-        the sphere from jiggling too much on the winch tendons. An alternative would be to add fluid
-        density or viscosity.
-      -->
-      <joint type="free" damping=".1"/>
       <!--
         The composite balls in the crate have only 3 linear DoFs with condim=1, effectively
         frictionless point particles. In order to make them stick to the sphere we give the sphere
         priority 2, to force condim=3.
+
+        Also note the sphere has a margin+gap of 3cm as opposed to the 1cm of the arm box.
       -->
-      <geom type="sphere" size=".03" priority="2"/>
+      <geom type="sphere" size=".03" priority="2" margin=".03" gap=".03"/>
       <site name="pin_left" pos="-.025 0 .025"/>
       <site name="pin_right" pos=".025 0 .025"/>
     </body>
@@ -127,9 +128,9 @@
   <actuator>
     <position name="conveyor" joint="conveyor" ctrlrange="-.2 .2" ctrllimited="true" kp="400"/>
     <position name="arm" joint="arm2" ctrlrange="-.8 1" ctrllimited="true" kp="10"/>
-    <adhesion name="adhere_arm" body="4boxes" ctrlrange="0 1" gain="20"/>
+    <adhesion name="adhere_arm" body="4boxes" ctrlrange="0 1" gain="5"/>
     <position name="winch" joint="winch" ctrlrange="-.7 .5" ctrllimited="true" kp="10"/>
-    <adhesion name="adhere_winch" body="sphere" ctrlrange="0 1" gain="20"/>
+    <adhesion name="adhere_winch" body="sphere" ctrlrange="0 1" gain="5"/>
   </actuator>
 
   <sensor>

src/engine/engine_core_smooth.c

@@ -626,6 +626,12 @@ void mj_transmission(const mjModel* m, mjData* d) {
   jacS = mj_stackAlloc(d, 3*nv);
   mju_zero(moment, nu*nv);
 
+  // define variables required for body transmission, don't allocate
+  int issparse = mj_isSparse(m);
+  mjtNum* efc_force = NULL;  // used as marker for allocation requirement
+  mjtNum *moment_exclude, *jacdifp, *jac1p, *jac2p;
+  int *chain;
+
   // compute lengths and moments
   for (int i=0; i<nu; i++) {
     // extract info
@@ -791,37 +797,81 @@ void mj_transmission(const mjModel* m, mjData* d) {
 
       // moment is average of all contact normal Jacobians
       {
-        // find and count all relevant contacts, mark them in efc_force
+        // allocate stack variables for the first mjTRN_BODY
-        int counter = 0;
+        if (!efc_force) {
-        mjtNum* efc_force = mj_stackAlloc(d, d->nefc);
+          efc_force = mj_stackAlloc(d, d->nefc);
+          moment_exclude = mj_stackAlloc(d, nv);
+          jacdifp = mj_stackAlloc(d, 3*nv);
+          jac1p = mj_stackAlloc(d, 3*nv);
+          jac2p = mj_stackAlloc(d, 3*nv);
+          chain = issparse ? (int*)mj_stackAlloc(d, nv) : NULL;
+        }
+
+        // clear efc_force and moment_exclude
         mju_zero(efc_force, d->nefc);
+        mju_zero(moment_exclude, nv);
+
+        // count all relevant contacts, accumulate Jacobians
+        int counter = 0;
         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) {
+          int b1 = m->geom_bodyid[con->geom1];
-            if (!con->exclude) {
+          int b2 = m->geom_bodyid[con->geom2];
-              counter++;
 
-              // condim 1 or elliptic cones: normal is in the first row
+          // irrelevant contact, continue
-              if (con->dim == 1 || m->opt.cone==mjCONE_ELLIPTIC) {
+          if (b1 != id && b2 != id) {
-                efc_force[con->efc_address] = 1;
+            continue;
-              }
+          }
 
-              // pyramidal cones: average all pyramid directions
+          // mark contact normals in efc_force
-              else {
+          if (!con->exclude) {
-                int npyramid = con->dim-1;  // number of frictional directions
+            counter++;
-                for (int k=0; k<2*npyramid; k++) {
+
-                  efc_force[con->efc_address+k] = 0.5/npyramid;
+            // 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)
+          }
+
+          // excluded contact, get sparse or dense Jacobian, accumulate
+          else if (con->exclude == 1) {
+            counter++;
+
+            // get Jacobian difference
+            int NV = mj_jacDifPair(m, d, chain, b1, b2, con->pos, con->pos,
+                                   jac1p, jac2p, jacdifp, NULL, NULL, NULL);
+
+            // project Jacobian along the normal of the contact frame
+            mju_mulMatMat(jac, con->frame, jacdifp, 1, 3, NV);
+
+            // accumulate in moment_exclude
+            if (issparse) {
+              for (int k=0; k<NV; k++) {
+                moment_exclude[chain[k]] += jac[k];
+              }
+            } else {
+              mju_addTo(moment_exclude, jac, nv);
             }
           }
         }
 
         // moment is average over contact normal Jacobians, make negative for adhesion
         if (counter) {
+          // accumulate active contact Jacobians into moment
           mj_mulJacTVec(m, d, moment+i*nv, efc_force);
+
+          // add Jacobians from excluded contacts
+          mju_addTo(moment+i*nv, moment_exclude, nv);
+
+          // normalize by total contacts, flip sign
           mju_scl(moment+i*nv, moment+i*nv, -1.0/counter, nv);
         }
       }