diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index 63bf1952..09e9b3f1 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -4550,9 +4550,24 @@ extensions specific to flexes. .. _flex-contact-passive: :at:`passive`: :at-val:`[true, false], "false"` - When enabled, the contact is not added to the contact solver but it is instead used to compute passive - (spring-damper) contact forces. All contacts, regardless of the specified condim, are frictionless (condim 1). This - is an experimental feature. + When enabled, contact of this flex with another flex, with itself, or with static geometry is not added to the + contact solver and is instead applied as a passive normal force. Contact with a body that can move is left on the + constraint solver. + + Friction is not modelled on this path: every passive contact is frictionless (condim 1) regardless of the + specified condim, and the force is purely normal. A flex therefore slides freely over static geometry, so a cloth + will not stay draped over a fixed shape and will not come to rest on a slope. Where friction matters more than + non-penetration, leave this option off. + + The force is a penalty on penetration depth whose stiffness is chosen as a natural frequency scaled by the + participating vertex mass, so a single value is appropriate across model scales; it is not user-specified. That + stiffness is integrated implicitly, its curvature being carried by the effective metric, and is therefore far + stiffer than an explicit force at the same timestep could be. It follows that the feature requires an integrator + whose constraint solve runs in that metric: :at:`implicit` or :at:`implicitfast` with the CG solver, pyramidal + friction cones and sleep disabled. A model requesting passive flex collisions otherwise is rejected with an error. + + Being a penalty force, it does not guarantee non-penetration: a thin flex moving fast enough to cross another + within one step will pass through it. This is an experimental feature. .. _deformable-skin: diff --git a/doc/changelog.rst b/doc/changelog.rst index 3b91f6db..3a75700a 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -46,6 +46,17 @@ Engine .. admonition:: Breaking API changes :class: attention + - Contacts of a flex with :ref:`passive` collisions are now integrated implicitly: + their stiffness is carried by the effective metric M + K rather than applied as an explicit spring, and so can + be far stiffer than the timestep would otherwise permit -- at a 2 ms timestep, roughly 50x what an explicit + force of the same step could hold. Being a penalty force it still does not guarantee non-penetration. A model + using passive collisions changes in three ways and should be re-checked: the feature now requires an integrator + whose constraint solve runs in that metric (:at:`implicit` or :at:`implicitfast` with the CG solver, pyramidal + cones and sleep disabled) and is rejected with an error otherwise; passive handling now covers contact of such a + flex with another flex, with itself, and with static geometry, while contact with a moving body returns to the + constraint solver and gains friction; and the stiffness is no longer a fixed 1e4 but a natural frequency scaled + by the participating vertex mass, which is considerably stiffer for typical models. + - Removed ``mjData.efm_L_rownnz``, ``mjData.efm_L_rowadr`` and ``mjData.efm_L_colind``. They described the sparsity of the effective-metric Cholesky factor, which no longer exists; ``mjData.efm_L`` now holds dense 3x3 blocks, 9 numbers per covered vertex. ``mjData.efm_active`` no longer takes the value 2: nothing selects a solve path on @@ -57,6 +68,10 @@ Engine Models ^^^^^^ +- Added `drape `__ example model: three + cloths draped over a sphere, demonstrating :ref:`passive` collisions. It replaces the + ``sphere_passive`` model, which has been removed. + - Added `bag `__ example model: a cloth bag, held open by pinning the ring of vertices around its mouth, catching the standard humanoid dropped in from above. Unlike the poncho models, which are bending-only, this model exercises the 2D diff --git a/model/flex/drape.xml b/model/flex/drape.xml new file mode 100644 index 00000000..79da523f --- /dev/null +++ b/model/flex/drape.xml @@ -0,0 +1,52 @@ + + + + + + + + + diff --git a/model/flex/sphere_passive.xml b/model/flex/sphere_passive.xml deleted file mode 100644 index 3ca4d0d2..00000000 --- a/model/flex/sphere_passive.xml +++ /dev/null @@ -1,36 +0,0 @@ - - - - - - diff --git a/src/engine/engine_core_constraint.c b/src/engine/engine_core_constraint.c index 5627acf0..38226254 100644 --- a/src/engine/engine_core_constraint.c +++ b/src/engine/engine_core_constraint.c @@ -2549,11 +2549,26 @@ static int mj_nc(const mjModel* m, mjData* d, int* nnz) { for (int i=0; i < ncon; i++) { mjContact* con = d->contact + i; - // skip if passive - if ((con->flex[0] > -1 && m->flex_passive[con->flex[0]]) || - (con->flex[1] > -1 && m->flex_passive[con->flex[1]])) { - con->efc_address = -1; - con->exclude = 4; + // Passive handling covers flex-flex contact, self-collision included, and contact between a + // flex and STATIC geometry. What those have in common is that every dof the contact touches is + // a flex vertex carried by the effective metric -- a static geom has none -- so the Hessian + // k*J^T*J is assembled in full rather than truncated. A flex against a MOVING body keeps the + // constraint path: its dofs would be dropped from the Hessian, and the passive force is a + // normal penalty with no friction cone, which is the wrong trade where a gripper closes on + // cloth. A contact is passive if either flex asks for it. + { + int f0 = con->flex[0], f1 = con->flex[1]; + int wants = (f0 > -1 && m->flex_passive[f0]) || (f1 > -1 && m->flex_passive[f1]); + int ok = (f0 > -1 && f1 > -1); // flex-flex, or a flex with itself + for (int s = 0; s < 2 && !ok; s++) { + if (con->flex[s] < 0 && con->geom[s] > -1) { + ok = (m->body_weldid[m->geom_bodyid[con->geom[s]]] == 0); // welded to the world + } + } + if (wants && ok) { + con->efc_address = -1; + con->exclude = 4; + } } // skip if excluded diff --git a/src/engine/engine_derivative.c b/src/engine/engine_derivative.c index 1f04b441..6a697342 100644 --- a/src/engine/engine_derivative.c +++ b/src/engine/engine_derivative.c @@ -17,6 +17,7 @@ #include #include #include // IWYU pragma: keep +#include "engine/engine_core_constraint.h" #include "engine/engine_core_smooth.h" #include "engine/engine_core_util.h" #include "engine/engine_crossplatform.h" @@ -1630,6 +1631,62 @@ mjtBool mjd_flexInterpAssemblable(const mjModel* m) { // does ANY flex contribute assemblable implicit stiffness? (cheap existence check for the // solver gate: stretch stiffness on a standard flex, or -- when Krot will be supplied -- an // operator-processed interp flex) +// does any flex hand its contacts to the passive path? Such a flex wants the metric for the +// contact stiffness alone, which is a different reason from carrying elasticity -- the two must +// stay distinguishable, because an empty CSR is a VALID state for an elastic model (the matrix-free +// operators carry bending and interp) but means "nothing at all" for a contact-only one. +static mjtBool flexPassiveContact_any(const mjModel* m) { + for (int f = 0; f < m->nflex; f++) { + if (!m->flex_interp[f] && !m->flex_rigid[f] && m->flex_dim[f] >= 2 && m->flex_passive[f]) { + return 1; + } + } + return 0; +} + +// res += scale * K_contact * vec, where K_contact = sum_c k_c * J_c^T J_c over passive flex +// contacts. The linearly-implicit scheme is (M + h^2 K) a = f - h K v; the shift is assembled +// per class (see mjd_effShift), so a class that contributes to K must contribute here too. +// Contributing only to K makes the contact stiffer WITHOUT the velocity correction that damps it, +// which is a ringing contact rather than a quiet one. +void mjd_flexContact_mul(const mjModel* m, mjData* d, mjtNum* res, const mjtNum* vec, + mjtNum scale) { + if (!d->ncon) { + return; + } + int nv = m->nv; + mj_markStack(d); + mjtNum* jacdif = mjSTACKALLOC(d, 3*nv, mjtNum); + mjtNum* jac1 = mjSTACKALLOC(d, 3*nv, mjtNum); + mjtNum* jac2 = mjSTACKALLOC(d, 3*nv, mjtNum); + mjtNum* jacn = mjSTACKALLOC(d, 3*nv, mjtNum); + int* chain = mjSTACKALLOC(d, nv, int); + for (int i = 0; i < d->ncon; i++) { + const mjContact* con = d->contact + i; + if (con->exclude != 4) { + continue; + } + mjtNum k = mjd_flexContactStiffness(m, d, con); + if (k <= 0) { + continue; + } + int NV = mj_contactJacobian(m, d, con, con->dim, jacdif, NULL, jac1, jac2, NULL, NULL, chain); + if (!NV) { + continue; + } + mju_mulMatMat(jacn, con->frame, jacdif, con->dim > 1 ? 3 : 1, 3, NV); + mjtNum Jv = 0; + for (int a = 0; a < NV; a++) { + Jv += jacn[a] * vec[chain[a]]; + } + mjtNum s = scale * k * Jv; + for (int a = 0; a < NV; a++) { + res[chain[a]] += s * jacn[a]; + } + } + mj_freeStack(d); +} + mjtBool mjd_flexStiff_any(const mjModel* m, int flg_interp) { for (int f = 0; f < m->nflex; f++) { if (flg_interp && flexInterp_processed(m, f)) { @@ -1645,6 +1702,14 @@ mjtBool mjd_flexStiff_any(const mjModel* m, int flg_interp) { // does this standard flex contribute implicit stiffness under the given term flags? +// A flex participates in the metric structure if it carries elasticity OR if its contacts are +// handled passively: the contact stiffness is a passive term in its own right and may be the only +// stiffness such a flex has, so the vertex slots (and hence the CSR rows, and hence the covered +// dofs) must exist for it either way. Gating participation on elasticity alone would let an +// unrelated modelling choice decide whether contact can be implicit. +static mjtBool flexMetric_participates(const mjModel* m, int f, int flg_bend, int flg_stretch, + int flg_contact); + static mjtBool flexStiff_active(const mjModel* m, int f, int flg_bend, int flg_stretch) { if (m->flex_interp[f] || m->flex_rigid[f] || m->flex_dim[f] < 2) { return 0; @@ -1655,6 +1720,91 @@ static mjtBool flexStiff_active(const mjModel* m, int f, int flg_bend, int flg_s return bend || stretch; } +// Passive flex contact stiffness, expressed as a natural frequency scaled by the participating +// mass: k = omega^2 * m_min. A frequency travels across models where an absolute stiffness does +// not, which is why the same number works from a millimetre cloth to a metre-scale bag. The min is +// over NONZERO masses: flex vertices pinned to a rigid attachment carry mass 0 (their inertia is in +// the rigid body) and a plain min would give k = 0. +#define mjFLEXCONTACT_OMEGA2 5e7 + +mjtNum mjd_flexContactStiffness(const mjModel* m, const mjData* d, const mjContact* con) { + mjtNum mmin = 0; + for (int side = 0; side < 2; side++) { + int f = con->flex[side]; + if (f < 0) { + continue; + } + int gv[8], ngv = 0; + if (con->vert[side] >= 0) { + gv[ngv++] = m->flex_vertadr[f] + con->vert[side]; + } else if (con->elem[side] >= 0) { + int nvrt = m->flex_dim[f] + 1; + const int* e = m->flex_elem + m->flex_elemdataadr[f] + nvrt*con->elem[side]; + for (int j = 0; j < nvrt && ngv < 8; j++) { + gv[ngv++] = m->flex_vertadr[f] + e[j]; + } + } + for (int j = 0; j < ngv; j++) { + int b = m->flex_vertbodyid[gv[j]]; + if (m->body_dofnum[b] != 3) { + continue; + } + int da = m->body_dofadr[b]; + mjtNum mv = d->M[m->M_rowadr[da] + m->M_rownnz[da] - 1]; // diagonal: the point mass + if (mv > 0 && (mmin == 0 || mv < mmin)) { + mmin = mv; + } + } + } + return mjFLEXCONTACT_OMEGA2 * mmin; // 0 if every participant is massless: no stiffness, no NaN +} + +// The flex vertex slots a passive contact couples: the vertex itself for a vertex side, the +// element's vertices for an element side. Duplicates dropped, and slots outside the metric skipped. +static int contactFlexSlots(const mjModel* m, const mjContact* con, const int* vslot, + int* out, int cap) { + int n = 0; + for (int side = 0; side < 2; side++) { + int f = con->flex[side]; + if (f < 0) { + continue; + } + int gv[8], ngv = 0; + if (con->vert[side] >= 0) { + gv[ngv++] = m->flex_vertadr[f] + con->vert[side]; + } else if (con->elem[side] >= 0) { + int nvrt = m->flex_dim[f] + 1; + const int* e = m->flex_elem + m->flex_elemdataadr[f] + nvrt*con->elem[side]; + for (int j = 0; j < nvrt && ngv < 8; j++) { + gv[ngv++] = m->flex_vertadr[f] + e[j]; + } + } + for (int j = 0; j < ngv; j++) { + int s = vslot[gv[j]]; + if (s < 0) { + continue; + } + int dup = 0; + for (int q = 0; q < n; q++) { + if (out[q] == s) { dup = 1; break; } + } + if (!dup && n < cap) { + out[n++] = s; + } + } + } + return n; +} + +static mjtBool flexMetric_participates(const mjModel* m, int f, int flg_bend, int flg_stretch, + int flg_contact) { + if (flexStiff_active(m, f, flg_bend, flg_stretch)) { + return 1; + } + return flg_contact && m->flex_passive[f] && !m->flex_rigid[f] && !m->flex_interp[f] && + m->flex_dim[f] >= 2; +} + // assemble the standard-flex implicit stiffness K = (s1 + s2*damping) * (K_bend + K_stretch) // into dof-level CSR (same terms mjd_flexBend_mul / mjd_flexStretch_mul apply matrix-free; the @@ -1669,7 +1819,7 @@ static mjtBool flexStiff_active(const mjModel* m, int f, int flg_bend, int flg_s // so one CSR replaces all three matrix-free operators uniformly. int mjd_flexStiff_assemble(const mjModel* m, mjData* d, int* rownnz, int* rowadr, int* colind, mjtNum* val, mjtNum s1, mjtNum s2, - int flg_bend, int flg_stretch, const mjtNum* Krot) { + int flg_bend, int flg_stretch, int flg_contact, const mjtNum* Krot) { int nv = m->nv; mj_markStack(d); @@ -1680,7 +1830,7 @@ int mjd_flexStiff_assemble(const mjModel* m, mjData* d, int* rownnz, int* rowadr vslot[i] = -1; } for (int f = 0; f < m->nflex; f++) { - if (!flexStiff_active(m, f, flg_bend, flg_stretch)) { + if (!flexMetric_participates(m, f, flg_bend, flg_stretch, flg_contact)) { continue; } for (int lv = 0; lv < m->flex_vertnum[f]; lv++) { @@ -1777,6 +1927,22 @@ int mjd_flexStiff_assemble(const mjModel* m, mjData* d, int* rownnz, int* rowadr } } + // passive contacts (counting): a contact makes its participating vertices mutual neighbours, so + // the CSR gains the blocks its Hessian k*J^T*J will occupy. This is the only source of structure + // for a flex whose contacts are passive but which carries no elasticity. + if (flg_contact) { + for (int i = 0; i < d->ncon; i++) { + const mjContact* con = d->contact + i; + if (con->exclude != 4) { + continue; + } + int cs[8], ncs = contactFlexSlots(m, con, vslot, cs, 8); + for (int a = 0; a < ncs; a++) { + ncand[cs[a]] += ncs; + } + } + } + // gather candidate neighbor lists (vertex slots, with duplicates) int* cadr = mjSTACKALLOC(d, nvert + 1, int); cadr[0] = 0; @@ -1842,6 +2008,22 @@ int mjd_flexStiff_assemble(const mjModel* m, mjData* d, int* rownnz, int* rowadr } } + // passive contacts (filling) + if (flg_contact) { + for (int i = 0; i < d->ncon; i++) { + const mjContact* con = d->contact + i; + if (con->exclude != 4) { + continue; + } + int cs[8], ncs = contactFlexSlots(m, con, vslot, cs, 8); + for (int a = 0; a < ncs; a++) { + for (int b = 0; b < ncs; b++) { + cand[cadr[cs[a]] + ncand[cs[a]]++] = cs[b]; + } + } + } + } + // per vertex: sort by neighbor dofadr, unique -> neighbor lists int* nadr = mjSTACKALLOC(d, nvert + 1, int); int* neigh = mjSTACKALLOC(d, cadr[nvert] > 0 ? cadr[nvert] : 1, int); @@ -2103,6 +2285,73 @@ int mjd_flexStiff_assemble(const mjModel* m, mjData* d, int* rownnz, int* rowadr }) } } + // passive contacts (values): k*J^T*J over the contact's flex vertices, where J is the normal- + // direction row of the contact Jacobian. Both participants are flex vertices carried by the + // metric (mj_makeConstraint hands this path no contact with a moving body), so the block is assembled + // in full -- no truncation, and the Hessian matches the force exactly. The force is applied by + // mj_contactPassive from the same stiffness helper; the two are one linearization. + if (flg_contact && d->ncon) { + int nv = m->nv; + int* dof2slot = mjSTACKALLOC(d, nv, int); + mjtNum* jacdif = mjSTACKALLOC(d, 3*nv, mjtNum); + mjtNum* jac1 = mjSTACKALLOC(d, 3*nv, mjtNum); + mjtNum* jac2 = mjSTACKALLOC(d, 3*nv, mjtNum); + mjtNum* jacn = mjSTACKALLOC(d, 3*nv, mjtNum); + int* chain = mjSTACKALLOC(d, nv, int); + mjtNum* w = mjSTACKALLOC(d, 3*(nvert > 0 ? nvert : 1), mjtNum); + for (int i = 0; i < nv; i++) { + dof2slot[i] = -1; + } + for (int s = 0; s < nvert; s++) { + for (int k = 0; k < 3; k++) { + dof2slot[vdof[s] + k] = s; + } + } + for (int i = 0; i < d->ncon; i++) { + const mjContact* con = d->contact + i; + if (con->exclude != 4) { + continue; + } + int cs[8], ncs = contactFlexSlots(m, con, vslot, cs, 8); + if (ncs < 1) { + continue; + } + mjtNum k = mjd_flexContactStiffness(m, d, con); + if (k <= 0) { + continue; + } + int NV = mj_contactJacobian(m, d, con, con->dim, jacdif, NULL, jac1, jac2, NULL, NULL, chain); + if (NV == 0) { + continue; + } + // rotate into the contact frame and keep the normal row + mju_mulMatMat(jacn, con->frame, jacdif, con->dim > 1 ? 3 : 1, 3, NV); + for (int a = 0; a < ncs; a++) { + mju_zero(w + 3*cs[a], 3); + } + for (int a = 0; a < NV; a++) { + int s = dof2slot[chain[a]]; + if (s >= 0) { + w[3*s + (chain[a] - vdof[s])] = jacn[a]; + } + } + for (int a = 0; a < ncs; a++) { + for (int b = 0; b < ncs; b++) { + int pos; + FLEXSTIFF_BLOCK(cs[a], cs[b], pos); + if (pos < 0) { + continue; + } + for (int r = 0; r < 3; r++) { + for (int c = 0; c < 3; c++) { + val[rowadr[vdof[cs[a]] + r] + 3*pos + c] += s1 * k * w[3*cs[a] + r] * w[3*cs[b] + c]; + } + } + } + } + } + } + #undef FLEXSTIFF_BLOCK #undef FLEXINTERP_WALK @@ -3166,6 +3415,7 @@ void mjd_effShift(const mjModel* m, mjData* d) { mjd_flexInterp_mul(m, d, d->efm_c, d->qvel, h, 0, d->flexelem_krot); mjd_flexBend_mul(m, d, d->efm_c, d->qvel, -h, 0); mjd_flexStretch_mul(m, d, d->efm_c, d->qvel, -h, 0); + mjd_flexContact_mul(m, d, d->efm_c, d->qvel, -h); } @@ -3197,16 +3447,17 @@ void mjd_effBuild(const mjModel* m, mjData* d, int active, int flg_factor) { const mjtNum* krot = mjd_flexInterpAssemblable(m) ? d->flexelem_krot : NULL; d->efm_K_rownnz = EFMALLOC(int, nv); d->efm_K_rowadr = EFMALLOC(int, nv); - if (mjd_flexStiff_any(m, krot != NULL)) { + if (mjd_flexStiff_any(m, krot != NULL) || flexPassiveContact_any(m)) { d->nefmK = mjd_flexStiff_assemble(m, d, d->efm_K_rownnz, d->efm_K_rowadr, - NULL, NULL, h*h, h, /*bend*/ 1, /*stretch*/ 1, krot); + NULL, NULL, h*h, h, /*bend*/ 1, /*stretch*/ 1, + /*contact*/ 1, krot); } if (d->nefmK) { d->efm_K_colind = EFMALLOC(int, d->nefmK); d->efm_K_val = EFMALLOC(mjtNum, d->nefmK); mjd_flexStiff_assemble(m, d, d->efm_K_rownnz, d->efm_K_rowadr, d->efm_K_colind, d->efm_K_val, h*h, h, - /*bend*/ 1, /*stretch*/ 1, krot); + /*bend*/ 1, /*stretch*/ 1, /*contact*/ 1, krot); // per-step factor of the flex block of (M + K): the stiffness is constant during the // step, so one factorization here turns every preconditioner application into a direct // solve (the stiff flex block stops being iterated on). Consumers that only multiply @@ -3219,6 +3470,7 @@ void mjd_effBuild(const mjModel* m, mjData* d, int active, int flg_factor) { mju_zeroInt(d->efm_K_rownnz, nv); mju_zeroInt(d->efm_K_rowadr, nv); } + d->efm_active = 1; // fill the shift with the current velocity (refreshed again in the velocity stage) diff --git a/src/engine/engine_derivative.h b/src/engine/engine_derivative.h index b9011fd0..84ea3a8d 100644 --- a/src/engine/engine_derivative.h +++ b/src/engine/engine_derivative.h @@ -77,9 +77,18 @@ MJAPI void mjd_flexStretch_mul(const mjModel* m, mjData* d, mjtNum* res, const m // dof-level CSR; phase 1 (colind==NULL) fills rownnz/rowadr and returns total nnz, phase 2 // fills colind/val. Interp flexes are assembled iff Krot (mjd_flexInterp_cacheKrot cache) is // non-NULL and the centered fast path applies (check mjd_flexInterpAssemblable first). +// Stiffness of a passive flex contact: omega^2 scaled by the smallest nonzero participating mass. +// The force (mj_contactPassive) and the Hessian (mjd_flexStiff_assemble) MUST use this same value, +// or the linearization the metric carries does not match the force being applied. +// res += scale * K_contact * vec (the shift counterpart of the contact stiffness in the metric) +MJAPI void mjd_flexContact_mul(const mjModel* m, mjData* d, mjtNum* res, const mjtNum* vec, + mjtNum scale); + +MJAPI mjtNum mjd_flexContactStiffness(const mjModel* m, const mjData* d, const mjContact* con); + MJAPI int mjd_flexStiff_assemble(const mjModel* m, mjData* d, int* rownnz, int* rowadr, int* colind, mjtNum* val, mjtNum s1, mjtNum s2, - int flg_bend, int flg_stretch, const mjtNum* Krot); + int flg_bend, int flg_stretch, int flg_contact, const mjtNum* Krot); // can all interp flexes be assembled to dof-level CSR? (centered fast path everywhere) MJAPI mjtBool mjd_flexInterpAssemblable(const mjModel* m); diff --git a/src/engine/engine_forward.c b/src/engine/engine_forward.c index 2d3bec9f..9cae1932 100644 --- a/src/engine/engine_forward.c +++ b/src/engine/engine_forward.c @@ -1591,6 +1591,19 @@ void mj_RungeKutta(const mjModel* m, mjData* d, int N) { // return 1 if any flex needs implicit stiffness treatment (interp or bending) +// a flex whose contacts are handled passively wants the effective metric in its own right: the +// contact stiffness is a passive term like any other, and it is the only stiffness such a flex may +// have. Gating it on the flex's ELASTICITY would let an unrelated modelling choice decide whether +// contact is implicit. +static mjtBool flex_has_passive_contact(const mjModel* m) { + for (int f=0; f < m->nflex; f++) { + if (!m->flex_rigid[f] && m->flex_passive[f]) { + return 1; + } + } + return 0; +} + static mjtBool flex_has_implicit_stiffness(const mjModel* m) { for (int f=0; f < m->nflex; f++) { if (m->flex_rigid[f]) { @@ -1634,7 +1647,7 @@ int mj_flexCG(const mjModel* m) { return m->opt.solver == mjSOL_CG && (m->opt.integrator == mjINT_IMPLICIT || m->opt.integrator == mjINT_IMPLICITFAST) && m->opt.cone != mjCONE_ELLIPTIC && !mjENABLED(mjENBL_SLEEP) && - flex_has_implicit_stiffness(m); + (flex_has_implicit_stiffness(m) || flex_has_passive_contact(m)); } @@ -1776,6 +1789,15 @@ void mj_implicit(const mjModel* m, mjData* d) { void mj_forwardSkip(const mjModel* m, mjData* d, int skipstage, int skipsensor) { TM_START; + // Passive flex contact is stiff by construction (see mjd_flexContactStiffness: omega^2 scaled by + // the participating mass), which is only stable because its Hessian is carried in the effective + // metric. Integrated explicitly it diverges immediately, so say so rather than let the model + // explode: the feature requires an integrator whose constraint solve runs in that metric. + if (flex_has_passive_contact(m) && !mj_flexCG(m)) { + mjERROR("passive flex contact requires the effective metric: use integrator=\"implicit\" or " + "\"implicitfast\" with solver=\"CG\", pyramidal cones and sleep disabled"); + } + // position-dependent if (skipstage < mjSTAGE_POS) { mj_fwdPosition(m, d); diff --git a/src/engine/engine_passive.c b/src/engine/engine_passive.c index dd1385ed..ba1f1eee 100644 --- a/src/engine/engine_passive.c +++ b/src/engine/engine_passive.c @@ -22,6 +22,7 @@ #include "engine/engine_callback.h" #include "engine/engine_core_constraint.h" #include "engine/engine_core_util.h" +#include "engine/engine_derivative.h" #include "engine/engine_crossplatform.h" #include "engine/engine_inline.h" #include "engine/engine_memory.h" @@ -36,8 +37,6 @@ //----------------------------- passive forces ----------------------------------------------------- -// stiffness for passive contacts -static const mjtNum kContactStiffness = 1e4; // local edge-based vertex indexing for 2D and 3D elements, 2D and 3D elements // have 3 and 6 edges, respectively so the missing indexes are set to 0 @@ -962,8 +961,10 @@ int mj_contactPassive(const mjModel* m, mjData* d) { // rotate Jacobian differences to contact frame mju_mulMatMat(jac, con->frame, jacdifp, dim > 1 ? 3 : 1, 3, NV); - // compute passive contact force (dim = 1) - mjtNum scl = -kContactStiffness*con->dist; + // compute passive contact force (dim = 1). The stiffness is mass-scaled and shared with the + // Hessian the effective metric carries for this contact; the pair is one linearization, so the + // two must not drift apart. + mjtNum scl = -mjd_flexContactStiffness(m, d, con)*con->dist; if (!issparse) { mju_addToScl(d->qfrc_spring, jac, scl, nv); } else { diff --git a/src/engine/engine_setconst.c b/src/engine/engine_setconst.c index 3d0034ae..8a8f9502 100644 --- a/src/engine/engine_setconst.c +++ b/src/engine/engine_setconst.c @@ -1416,10 +1416,11 @@ static void setEfm0Factor(mjModel* m, mjData* d) { int* K_rownnz = mjSTACKALLOC(d, nv, int); int* K_rowadr = mjSTACKALLOC(d, nv, int); int nK = mjd_flexStiff_assemble(m, d, K_rownnz, K_rowadr, NULL, NULL, h*h, h, - /*flg_bend=*/1, /*flg_stretch=*/0, NULL); + /*flg_bend=*/1, /*flg_stretch=*/0, /*flg_contact=*/0, + NULL); int* K_colind = mjSTACKALLOC(d, nK > 0 ? nK : 1, int); mjtNum* K_val = mjSTACKALLOC(d, nK > 0 ? nK : 1, mjtNum); - mjd_flexStiff_assemble(m, d, K_rownnz, K_rowadr, K_colind, K_val, h*h, h, 1, 0, NULL); + mjd_flexStiff_assemble(m, d, K_rownnz, K_rowadr, K_colind, K_val, h*h, h, 1, 0, 0, NULL); // inverse map: dof address -> compact factor row (monotone: slots follow dof order) int* dofrow = mjSTACKALLOC(d, nv, int); diff --git a/test/engine/engine_derivative_test.cc b/test/engine/engine_derivative_test.cc index 50cbc94d..f5847720 100644 --- a/test/engine/engine_derivative_test.cc +++ b/test/engine/engine_derivative_test.cc @@ -2153,13 +2153,13 @@ TEST_F(DerivativeTest, FlexStiffAssemble) { std::vector rownnz(nv), rowadr(nv); int nnz = mjd_flexStiff_assemble(model.get(), data.get(), rownnz.data(), rowadr.data(), NULL, NULL, s1, s2, - /*flg_bend=*/1, /*flg_stretch=*/1, NULL); + /*flg_bend=*/1, /*flg_stretch=*/1, /*flg_contact=*/0, NULL); ASSERT_GT(nnz, 0); std::vector colind(nnz); std::vector val(nnz); mjd_flexStiff_assemble(model.get(), data.get(), rownnz.data(), rowadr.data(), colind.data(), val.data(), s1, s2, /*flg_bend=*/1, - /*flg_stretch=*/1, NULL); + /*flg_stretch=*/1, /*flg_contact=*/0, NULL); // compare CSR apply vs operators on test vectors for (int trial = 0; trial < 3; trial++) { @@ -2220,13 +2220,13 @@ TEST_F(DerivativeTest, FlexStiffAssembleInterp) { mjtNum s1 = 4e-6, s2 = 2e-3; std::vector rownnz(nv), rowadr(nv); int nnz = mjd_flexStiff_assemble(model.get(), data.get(), rownnz.data(), rowadr.data(), - NULL, NULL, s1, s2, /*flg_bend=*/0, /*flg_stretch=*/0, + NULL, NULL, s1, s2, /*flg_bend=*/0, /*flg_stretch=*/0, /*flg_contact=*/0, krot.data()); ASSERT_GT(nnz, 0); std::vector colind(nnz); std::vector val(nnz); mjd_flexStiff_assemble(model.get(), data.get(), rownnz.data(), rowadr.data(), - colind.data(), val.data(), s1, s2, /*flg_bend=*/0, /*flg_stretch=*/0, + colind.data(), val.data(), s1, s2, /*flg_bend=*/0, /*flg_stretch=*/0, /*flg_contact=*/0, krot.data()); // compare CSR apply vs the operator called with negated scales (its convention) diff --git a/test/engine/engine_forward_test.cc b/test/engine/engine_forward_test.cc index a44e8e2b..cc31aaf1 100644 --- a/test/engine/engine_forward_test.cc +++ b/test/engine/engine_forward_test.cc @@ -3630,6 +3630,68 @@ TEST_F(ActuatorDampingTest, DampingVsKvGearScaling) { } // flex sheet dropping on a plane should not gain energy from implicit bending +// Passive flex contact is applied at a stiffness far beyond what an explicit force could hold at +// this timestep -- roughly 50x the 4*m/h^2 limit -- because its curvature is carried by the +// effective metric. Both the curvature and the shift -h*K*v are needed: with the curvature alone +// the contact is stiff but undamped and rings itself apart, so this settles at the drop's +// free-fall speed only when both are present. Self-collision is on, which the undamped form could +// not survive at all. +TEST_F(ImplicitIntegratorTest, PassiveFlexContactIsImplicit) { + static constexpr char xml[] = R"( + + + )"; + char error[1024]; + MjModelPtr m = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(m, NotNull()) << error; + MjDataPtr d = MakeData(m); + const mjModel* model = m.get(); + mjData* data = d.get(); + + // The upper sheet drops onto the lower one, which sags between its pinned corners, so the fall is + // a couple of decimetres and the physical peak speed is a little over 2 m/s. The bound only has + // to separate that from an energy-injecting contact, which is not a close call: with the metric + // carrying the contact curvature but not the matching shift, this same scene reaches 143 m/s. + mjtNum vmax = 0; + for (int i = 0; i < 1000; i++) { + mj_step(model, data); + for (int j = 0; j < model->nv; j++) { + vmax = mju_max(vmax, mju_abs(data->qvel[j])); + } + ASSERT_FALSE(data->warning[mjWARN_BADQACC].number) << "diverged at step " << i; + } + EXPECT_LT(vmax, 4.0) << "peak speed " << vmax; + + // and the upper sheet has not passed through the lower one. The lower sheet is pinned only at + // its corners and sags into a bowl with the upper sheet resting in the bottom of it, so neither + // an absolute height nor a comparison of means says anything; what must hold is that the upper + // sheet never gets below the lowest point of the lower one. + mjtNum lo[2] = {1e30, 1e30}; + for (int k = 0; k < 2; k++) { + int f = mj_name2id(model, mjOBJ_FLEX, k ? "upper" : "lower"); + for (int i = 0; i < model->flex_vertnum[f]; i++) { + lo[k] = mju_min(lo[k], data->flexvert_xpos[3*(model->flex_vertadr[f] + i) + 2]); + } + } + EXPECT_GT(lo[1], lo[0] - 0.01) << "upper sheet passed through: lowest z " << lo[1] + << " against the lower sheet's " << lo[0]; + +} + TEST_F(ImplicitIntegratorTest, FlexContactEnergy) { static constexpr char xml[] = R"(