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
This commit is contained in:
committed by
Copybara-Service
parent
b942c9f922
commit
a264d0bc8b
@@ -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++) {
|
||||
|
||||
Reference in New Issue
Block a user