Add geom adhesion: contacts that pull, via translated friction cones.

https://youtu.be/GioWwB36XHI The new geom attribute adhesion (units of force, signed; pair-level override) translates the contact friction cone along its normal so that the force origin lies strictly inside it. Consequences: each contact can pull with up to the given force before breaking, and the tangential friction budget becomes mu*(f_N + adhesion) -- the Mohr-Coulomb yield condition with cohesion c = mu*adhesion -- so lightly-squeezed grasps retain a guaranteed friction floor. A translated cone factors exactly into {constant attractive force} + {original cone}, so no solver kernels change. The implementation is this factorization: a constant attraction along contact normals accumulated into the new mjData.qfrc_adhesion (summed into qfrc_passive), plus a bias of adhesive contact rows' reference acceleration (aref += R*adhesion), which makes resting penetration exactly independent of adhesion. Contacts of adhesive pairs remain active throughout the gap zone, producing rows with positive violation whose reference acceleration pulls: a tether that resists pull-off smoothly, captures objects released within the band into steady contact, and detaches at the specified force. Adhesion values of the two geoms combine by sum; explicit pairs override. mj_contactForce reports the net interface force (cone force minus the adhesive pull), whose normal component can now be negative. Negative adhesion is allowed and produces a repulsive offset (air hockey). PiperOrigin-RevId: 950858148 Change-Id: I879c08eba7ae501e5c0f8c2f807167344da4c2bc
2026-07-20 08:35:17 -07:00
parent b942c9f922
commit a264d0bc8b
28 changed files with 784 additions and 50 deletions
@@ -1724,10 +1724,10 @@ int mj_broadphase(const mjModel* m, mjData* d, int* bfpair, int maxpair) {

 //----------------------------- narrow-phase collision detection -----------------------------------

-// compute contact condim, solref, solimp, friction
+// compute contact condim, solref, solimp, friction, adhesion
 static void mj_contactParam(const mjModel* m, int* condim,
                            mjtNum* solref, mjtNum* solimp, mjtNum* friction,
-                            int g1, int g2, int f1, int f2) {
+                            mjtNum* adhesion, int g1, int g2, int f1, int f2) {
  mjtNum fri[3];

  // get parameters from geom1 or flex1
@@ -1737,6 +1737,7 @@ static void mj_contactParam(const mjModel* m, int* condim,
  const mjtNum* solref1 =   (f1 < 0) ? m->geom_solref+g1*mjNREF : m->flex_solref+f1*mjNREF;
  const mjtNum* solimp1 =   (f1 < 0) ? m->geom_solimp+g1*mjNIMP : m->flex_solimp+f1*mjNIMP;
  const mjtNum* friction1 = (f1 < 0) ? m->geom_friction+g1*3    : m->flex_friction+f1*3;
+  mjtNum adhesion1 =        (f1 < 0) ? m->geom_adhesion[g1]     : 0;

  // get parameters from geom2 or flex2
  int priority2 =           (f2 < 0) ? m->geom_priority[g2]     : m->flex_priority[f2];
@@ -1745,6 +1746,10 @@ static void mj_contactParam(const mjModel* m, int* condim,
  const mjtNum* solref2 =   (f2 < 0) ? m->geom_solref+g2*mjNREF : m->flex_solref+f2*mjNREF;
  const mjtNum* solimp2 =   (f2 < 0) ? m->geom_solimp+g2*mjNIMP : m->flex_solimp+f2*mjNIMP;
  const mjtNum* friction2 = (f2 < 0) ? m->geom_friction+g2*3    : m->flex_friction+f2*3;
+  mjtNum adhesion2 =        (f2 < 0) ? m->geom_adhesion[g2]     : 0;
+
+  // adhesion: each surface contributes its own attraction
+  *adhesion = adhesion1 + adhesion2;

  // different priority: copy from item with higher priority
  if (priority1 > priority2) {
@@ -1752,12 +1757,14 @@ static void mj_contactParam(const mjModel* m, int* condim,
    mju_copy(solref, solref1, mjNREF);
    mju_copy(solimp, solimp1, mjNIMP);
    mju_copy(fri, friction1, 3);
+    *adhesion = adhesion1;
  }
  else if (priority1 < priority2) {
    *condim = condim2;
    mju_copy(solref, solref2, mjNREF);
    mju_copy(solimp, solimp2, mjNIMP);
    mju_copy(fri, friction2, 3);
+    *adhesion = adhesion2;
  }

  // same priority
@@ -1820,17 +1827,27 @@ static void mj_contactParam(const mjModel* m, int* condim,
 static void mj_setContact(const mjModel* m, mjContact* con,
                          int condim, mjtNum includemargin,
                          const mjtNum* solref, const mjtNum* solreffriction,
-                          const mjtNum* solimp, const mjtNum* friction) {
+                          const mjtNum* solimp, const mjtNum* friction,
+                          mjtNum adhesion) {
  // set parameters
  con->dim = condim;
  con->includemargin = includemargin;
+  con->adhesion = adhesion;
  mj_assignRef(m, con->solref, solref);
  mj_assignRef(m, con->solreffriction, solreffriction);
  mj_assignImp(m, con->solimp, solimp);
  mj_assignFriction(m, con->friction, friction);

-  // exclude in gap
-  con->exclude = (con->dist >= includemargin);
+  // exclude in gap; adhesive contacts remain active in the gap, producing
+  // rows with positive violation whose reference acceleration pulls (tether)
+  con->exclude = (con->dist >= includemargin) && !adhesion;
+
+  // in-gap adhesive contacts have no surface contact and therefore no
+  // friction: reduce to normal-only rows (with tangential rows present, fast
+  // slip would recruit normal force and brake tangential motion at a distance)
+  if (adhesion && con->dist >= includemargin) {
+    con->dim = 1;
+  }

  // complete frame
  mju_makeFrame(con->frame);
@@ -2010,7 +2027,7 @@ static void mj_narrowphase(const mjModel* m, mjData* d, const mjcPair* buffer, i
      continue;

    int condim;
-    mjtNum friction[5], solref[mjNREF], solimp[mjNIMP];
+    mjtNum friction[5], solref[mjNREF], solimp[mjNIMP], adhesion;
    mjtNum solreffriction[mjNREF] = {0};
    int g1 = pairbuffer[i].geom_geom.g1;
    int g2 = pairbuffer[i].geom_geom.g2;
@@ -2021,11 +2038,12 @@ static void mj_narrowphase(const mjModel* m, mjData* d, const mjcPair* buffer, i
      mju_copy(solref, m->pair_solref+mjNREF*ipair, mjNREF);
      mju_copy(solimp, m->pair_solimp+mjNIMP*ipair, mjNIMP);
      mju_copy(friction, m->pair_friction+5*ipair, 5);
+      adhesion = m->pair_adhesion[ipair];
      if (m->pair_solreffriction[mjNREF*ipair] || m->pair_solreffriction[mjNREF*ipair + 1]) {
        mju_copy(solreffriction, m->pair_solreffriction+mjNREF*ipair, mjNREF);
      }
    } else {
-      mj_contactParam(m, &condim, solref, solimp, friction, g1, g2, -1, -1);
+      mj_contactParam(m, &condim, solref, solimp, friction, &adhesion, g1, g2, -1, -1);
    }

    mjPreContact* bc = arg.conbuffer + pairbuffer[i].conpos;
@@ -2044,7 +2062,7 @@ static void mj_narrowphase(const mjModel* m, mjData* d, const mjcPair* buffer, i
      c->elem[1] = -1;
      c->vert[0] = -1;
      c->vert[1] = -1;
-      mj_setContact(m, c, condim, margin, solref, solreffriction, solimp, friction);
+      mj_setContact(m, c, condim, margin, solref, solreffriction, solimp, friction, adhesion);
    }
    conpos += ncon;
  }
@@ -2065,9 +2083,9 @@ static void mj_collidePlaneFlex(const mjModel* m, mjData* d, int g, int f) {
  int condim;
  int flex_vertnum = m->flex_vertnum[f];
  mjtNum gap = m->geom_gap[g] + m->flex_gap[f];
-  mjtNum solref[mjNREF], solimp[mjNIMP], friction[5];
+  mjtNum solref[mjNREF], solimp[mjNIMP], friction[5], adhesion;
  mjtNum solreffriction[mjNREF] = {0};
-  mj_contactParam(m, &condim, solref, solimp, friction, g, -1, -1, f);
+  mj_contactParam(m, &condim, solref, solimp, friction, &adhesion, g, -1, -1, f);

  // collide all flex vertices with plane
  for (int i=0; i < flex_vertnum; i++) {
@@ -2099,7 +2117,7 @@ static void mj_collidePlaneFlex(const mjModel* m, mjData* d, int g, int f) {
    con.vert[1] = i;

    // set remaining contact parameters
-    mj_setContact(m, &con, condim, margin, solref, solreffriction, solimp, friction);
+    mj_setContact(m, &con, condim, margin, solref, solreffriction, solimp, friction, adhesion);

    // add to mjData, abort if too many contacts
    if (mj_addContact(m, d, &con)) {
@@ -2120,9 +2138,9 @@ static void mj_collideSdfFlex(const mjModel* m, mjData* d, int g, int f) {
  mjtNum margin = mj_assignMargin(m, m->geom_margin[g] + m->flex_margin[f]);
  mjtNum gap = m->geom_gap[g] + m->flex_gap[f];
  int condim;
-  mjtNum solref[mjNREF], solimp[mjNIMP], friction[5];
+  mjtNum solref[mjNREF], solimp[mjNIMP], friction[5], adhesion;
  mjtNum solreffriction[mjNREF] = {0};
-  mj_contactParam(m, &condim, solref, solimp, friction, g, -1, -1, f);
+  mj_contactParam(m, &condim, solref, solimp, friction, &adhesion, g, -1, -1, f);

  // allocate temporary contact array on stack (zero-initialized)
  mj_markStack(d);
@@ -2153,7 +2171,7 @@ static void mj_collideSdfFlex(const mjModel* m, mjData* d, int g, int f) {
    con.vert[1] = -1;

    // set remaining contact parameters
-    mj_setContact(m, &con, condim, margin, solref, solreffriction, solimp, friction);
+    mj_setContact(m, &con, condim, margin, solref, solreffriction, solimp, friction, adhesion);

    // add to mjData, abort if too many contacts
    if (mj_addContact(m, d, &con)) {
@@ -2221,9 +2239,9 @@ static void mj_collideFlexInternal(const mjModel* m, mjData* d, int f) {
  int condim;
  int flex_elemnum = m->flex_elemnum[f];
  mjtNum radius = m->flex_radius[f];
-  mjtNum solref[mjNREF], solimp[mjNIMP], friction[5];
+  mjtNum solref[mjNREF], solimp[mjNIMP], friction[5], adhesion;
  mjtNum solreffriction[mjNREF] = {0};
-  mj_contactParam(m, &condim, solref, solimp, friction, -1, -1, f, f);
+  mj_contactParam(m, &condim, solref, solimp, friction, &adhesion, -1, -1, f, f);
  condim = 1;

  // process all elements
@@ -2240,7 +2258,7 @@ static void mj_collideFlexInternal(const mjModel* m, mjData* d, int f) {
      mju_copy3(con.pos, precon.pos);
      mju_copy3(con.frame, precon.normal);
      mju_copy3(con.frame + 3, precon.tangent);
-      mj_setContact(m, &con, condim, 0, solref, solreffriction, solimp, friction);
+      mj_setContact(m, &con, condim, 0, solref, solreffriction, solimp, friction, adhesion);
      if (mj_addContact(m, d, &con)) return;
    }

@@ -2251,7 +2269,7 @@ static void mj_collideFlexInternal(const mjModel* m, mjData* d, int f) {
      mju_copy3(con.pos, precon.pos);
      mju_copy3(con.frame, precon.normal);
      mju_copy3(con.frame + 3, precon.tangent);
-      mj_setContact(m, &con, condim, 0, solref, solreffriction, solimp, friction);
+      mj_setContact(m, &con, condim, 0, solref, solreffriction, solimp, friction, adhesion);
      if (mj_addContact(m, d, &con)) return;
    }

@@ -2262,7 +2280,7 @@ static void mj_collideFlexInternal(const mjModel* m, mjData* d, int f) {
      mju_copy3(con.pos, precon.pos);
      mju_copy3(con.frame, precon.normal);
      mju_copy3(con.frame + 3, precon.tangent);
-      mj_setContact(m, &con, condim, 0, solref, solreffriction, solimp, friction);
+      mj_setContact(m, &con, condim, 0, solref, solreffriction, solimp, friction, adhesion);
      if (mj_addContact(m, d, &con)) return;
    }

@@ -2273,7 +2291,7 @@ static void mj_collideFlexInternal(const mjModel* m, mjData* d, int f) {
      mju_copy3(con.pos, precon.pos);
      mju_copy3(con.frame, precon.normal);
      mju_copy3(con.frame + 3, precon.tangent);
-      mj_setContact(m, &con, condim, 0, solref, solreffriction, solimp, friction);
+      mj_setContact(m, &con, condim, 0, solref, solreffriction, solimp, friction, adhesion);
      if (mj_addContact(m, d, &con)) return;
    }
  }
@@ -2439,9 +2457,9 @@ void mj_collideGeomElem(const mjModel* m, mjData* d, int g, int f, int e) {

  // get contact parameters
  int condim;
-  mjtNum friction[5], solref[mjNREF], solimp[mjNIMP];
+  mjtNum friction[5], solref[mjNREF], solimp[mjNIMP], adhesion;
  mjtNum solreffriction[mjNREF] = {0};
-  mj_contactParam(m, &condim, solref, solimp, friction, g, -1, -1, f);
+  mj_contactParam(m, &condim, solref, solimp, friction, &adhesion, g, -1, -1, f);

  // allocate mjContact[num] on the arena
  mjContact* con =
@@ -2475,7 +2493,7 @@ void mj_collideGeomElem(const mjModel* m, mjData* d, int g, int f, int e) {
    }

    // set remaining contact parameters
-    mj_setContact(m, con + i, condim, margin, solref, solreffriction, solimp, friction);
+    mj_setContact(m, con + i, condim, margin, solref, solreffriction, solimp, friction, adhesion);
  }

  // add to ncon
@@ -2548,9 +2566,9 @@ void mj_collideElems(const mjModel* m, mjData* d, int f1, int e1, int f2, int e2

  // get contact parameters
  int condim;
-  mjtNum friction[5], solref[mjNREF], solimp[mjNIMP];
+  mjtNum friction[5], solref[mjNREF], solimp[mjNIMP], adhesion;
  mjtNum solreffriction[mjNREF] = {0};
-  mj_contactParam(m, &condim, solref, solimp, friction, -1, -1, f1, f2);
+  mj_contactParam(m, &condim, solref, solimp, friction, &adhesion, -1, -1, f1, f2);

  mjContact* con = (mjContact*) mj_arenaAllocByte(d, sizeof(mjContact) * num, _Alignof(mjContact));
  if (!con) {
@@ -2577,7 +2595,7 @@ void mj_collideElems(const mjModel* m, mjData* d, int f1, int e1, int f2, int e2
    mju_copy3(con[i].frame + 3, precon[i].tangent);

    // set remaining contact parameters
-    mj_setContact(m, con + i, condim, margin, solref, solreffriction, solimp, friction);
+    mj_setContact(m, con + i, condim, margin, solref, solreffriction, solimp, friction, adhesion);
  }

  // add to ncon
@@ -2638,9 +2656,9 @@ void mj_collideElemVert(const mjModel* m, mjData* d, int f, int e, int v) {

  // get contact parameters
  int condim;
-  mjtNum friction[5], solref[mjNREF], solimp[mjNIMP];
+  mjtNum friction[5], solref[mjNREF], solimp[mjNIMP], adhesion;
  mjtNum solreffriction[mjNREF] = {0};
-  mj_contactParam(m, &condim, solref, solimp, friction, -1, -1, f, f);
+  mj_contactParam(m, &condim, solref, solimp, friction, &adhesion, -1, -1, f, f);


  // allocate mjContact[num] on the arena
@@ -2671,7 +2689,7 @@ void mj_collideElemVert(const mjModel* m, mjData* d, int f, int e, int v) {
    mju_copy3(con[i].frame + 3, precon[i].tangent);

    // set remaining contact parameters
-    mj_setContact(m, con + i, condim, 0, solref, solreffriction, solimp, friction);
+    mj_setContact(m, con + i, condim, 0, solref, solreffriction, solimp, friction, adhesion);
  }

  // add to ncon
@@ -3199,6 +3199,38 @@ static void mj_addSurfaceVel(const mjModel* m, mjData* d) {



+// bias aref of adhesive contact rows: makes the compression branch of the net
+// force-penetration curve independent of adhesion (exact translated-cone semantics)
+static void mj_adhesionRef(const mjModel* m, mjData* d) {
+  // no adhesion on any geom or pair: quick return
+  if (!m->flg_adhesion) {
+    return;
+  }
+
+  int ispyramid = mj_isPyramidal(m), ncon = d->ncon;
+
+  for (int i=0; i < ncon; i++) {
+    const mjContact* con = d->contact + i;
+    if (!con->adhesion || con->efc_address < 0) {
+      continue;
+    }
+
+    int adr = con->efc_address, dim = con->dim;
+    if (dim == 1 || !ispyramid) {
+      // normal row
+      d->efc_aref[adr] += d->efc_R[adr] * con->adhesion;
+    } else {
+      // pyramid rows: the normal decomposes equally over the 2*(dim-1) edges
+      mjtNum edge = con->adhesion / (2*(dim-1));
+      for (int j=0; j < 2*(dim-1); j++) {
+        d->efc_aref[adr+j] += d->efc_R[adr+j] * edge;
+      }
+    }
+  }
+}
+
+
+
 // compute efc_vel, efc_aref
 void mj_referenceConstraint(const mjModel* m, mjData* d) {
  int nefc = d->nefc;
@@ -3216,6 +3248,9 @@ void mj_referenceConstraint(const mjModel* m, mjData* d) {
                     -KBIP[4*i]*KBIP[4*i+2]*(d->efc_pos[i]-d->efc_margin[i]);
  }

+  // bias adhesive contact rows
+  mj_adhesionRef(m, d);
+
  // subtract Jdot*v correction for connect/weld equality constraints
  if (d->ne > 0) {
    mj_Jdotv(m, d, d->efc_aref);
@@ -1077,6 +1077,9 @@ void mj_contactForce(const mjModel* m, const mjData* d, int id, mjtNum result[6]
    } else {
      mju_copy(result, d->efc_force + con->efc_address, con->dim);
    }
+
+    // report the net interface force: the solver's cone force minus the adhesive pull
+    result[0] -= con->adhesion;
  }
 }

@@ -962,6 +962,71 @@ int mj_contactPassive(const mjModel* m, mjData* d) {
 }


+// adhesion forces: constant attraction along the normals of adhesive contacts
+int mj_adhesion(const mjModel* m, mjData* d) {
+  int ncon = d->ncon, issparse = mj_isSparse(m);
+  int NV, nv = m->nv, *chain = NULL;
+  mjtNum *jac, *jacdif, *jac1p, *jac2p, *qfrc;
+  mjContact* con;
+  int has_adhesion = 0;
+
+  if (!m->flg_adhesion || mjDISABLED(mjDSBL_CONTACT) || ncon == 0 || nv == 0) {
+    return 0;
+  }
+
+  // early return if no adhesive contact
+  for (int i=0; i < ncon; i++) {
+    if (d->contact[i].adhesion && d->contact[i].exclude <= 1) {
+      has_adhesion = 1;
+      break;
+    }
+  }
+  if (!has_adhesion) {
+    return 0;
+  }
+
+  // allocate Jacobian; contacts are normal-only (dim 1): no rotational buffers
+  mj_markStack(d);
+  jac    = mjSTACKALLOC(d, nv, mjtNum);
+  jacdif = mjSTACKALLOC(d, 3*nv, mjtNum);
+  jac1p  = mjSTACKALLOC(d, 3*nv, mjtNum);
+  jac2p  = mjSTACKALLOC(d, 3*nv, mjtNum);
+  qfrc   = mjSTACKALLOC(d, nv, mjtNum);
+  if (issparse) {
+    chain = mjSTACKALLOC(d, nv, int);
+  }
+
+  // pull adhesive contacts together along the contact normal
+  for (int i=0; i < ncon; i++) {
+    con = d->contact + i;
+    if (!con->adhesion || con->exclude > 1) {
+      continue;
+    }
+
+    // normal Jacobian
+    NV = mj_contactJacobian(m, d, con, 1, jac, jacdif, jacdif, NULL,
+                            jac1p, jac2p, NULL, NULL, chain);
+    if (NV == 0) {
+      continue;
+    }
+    mju_mulMatMat(jac, con->frame, jacdif, 1, 3, NV);
+
+    // accumulate qfrc_adhesion += jacN' * (-adhesion)
+    if (!issparse) {
+      mju_addToScl(d->qfrc_adhesion, jac, -con->adhesion, nv);
+    } else {
+      mju_scl(qfrc, jac, -con->adhesion, NV);
+      for (int j=0; j < NV; j++) {
+        d->qfrc_adhesion[chain[j]] += qfrc[j];
+      }
+    }
+  }
+
+  mj_freeStack(d);
+  return 1;
+}
+
+
 // all passive forces
 void mj_passive(const mjModel* m, mjData* d) {
  int sleep_filter = mjENABLED(mjENBL_SLEEP) && d->nv_awake < m->nv;
@@ -974,12 +1039,14 @@ void mj_passive(const mjModel* m, mjData* d) {
    mju_zeroInd(d->qfrc_damper,   nv, dof_awake_ind);
    mju_zeroInd(d->qfrc_gravcomp, nv, dof_awake_ind);
    mju_zeroInd(d->qfrc_fluid,    nv, dof_awake_ind);
+    mju_zeroInd(d->qfrc_adhesion, nv, dof_awake_ind);
    mju_zeroInd(d->qfrc_passive,  nv, dof_awake_ind);
  } else {
    mju_zero(d->qfrc_spring,   nv);
    mju_zero(d->qfrc_damper,   nv);
    mju_zero(d->qfrc_gravcomp, nv);
    mju_zero(d->qfrc_fluid,    nv);
+    mju_zero(d->qfrc_adhesion, nv);
    mju_zero(d->qfrc_passive,  nv);
  }

@@ -1000,6 +1067,9 @@ void mj_passive(const mjModel* m, mjData* d) {
  // contact forces
  mj_contactPassive(m, d);

+  // adhesion forces
+  int has_adhesion = mj_adhesion(m, d);
+
  // add passive forces into qfrc_passive
  if (sleep_filter) {
    mju_addInd(d->qfrc_passive, d->qfrc_spring, d->qfrc_damper, dof_awake_ind, nv);
@@ -1015,6 +1085,14 @@ void mj_passive(const mjModel* m, mjData* d) {
    }
  }

+  if (has_adhesion) {
+    if (sleep_filter) {
+      mju_addToInd(d->qfrc_passive, d->qfrc_adhesion, dof_awake_ind, nv);
+    } else {
+      mju_addTo(d->qfrc_passive, d->qfrc_adhesion, nv);
+    }
+  }
+
  if (has_gravcomp) {
    int ndof = sleep_filter ? d->nv_awake : nv;
    for (int v=0; v < ndof; v++) {
@@ -28,6 +28,9 @@ extern "C" {
 // all passive forces
 MJAPI void mj_passive(const mjModel* m, mjData* d);

+// adhesion forces: constant attraction along the normals of adhesive contacts
+int mj_adhesion(const mjModel* m, mjData* d);
+



@@ -223,6 +223,21 @@ static void setFixed(mjModel* m, mjData* d) {
  }
  m->flg_surfacevel = flg_surfacevel;

+  // compute flg_adhesion: whether any geom or pair has nonzero adhesion
+  mjtBool flg_adhesion = 0;
+  for (int i=0; i < m->ngeom; i++) {
+    if (m->geom_adhesion[i]) {
+      flg_adhesion = 1;
+      break;
+    }
+  }
+  for (int i=0; i < m->npair && !flg_adhesion; i++) {
+    if (m->pair_adhesion[i]) {
+      flg_adhesion = 1;
+    }
+  }
+  m->flg_adhesion = flg_adhesion;
+
  // set jnt_actuatorid and tendon_actuatorid
  mju_fillInt(m->jnt_actuatorid, -1, m->njnt);
  mju_fillInt(m->tendon_actuatorid, -1, m->ntendon);