chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Integrate passive flex contact implicitly

Browse Source 
Contact of a flex with `passive` collisions enabled was applied as an
explicit spring of fixed stiffness 1e4, which the timestep bounds: any
stiffness worth having oscillates faster than the step can resolve, so
the force was too soft to keep sheets apart and interpenetration was
routine.

Carry its curvature in the effective metric M + K instead, alongside the
flex's own stretch and bending stiffness. The contact block k*J^T*J is
appended to the per-vertex candidate list already assembled for the flex
stencils, so it costs additional entries in an existing matrix rather
than a new one, and the accompanying shift -h*K*v is what damps the
stiff modes. At a 2 ms timestep this holds roughly 50x the stiffness an
explicit force of the same step could.

With the timestep no longer setting the bound, the stiffness is chosen
as a natural frequency scaled by the participating vertex mass rather
than left at a fixed 1e4, so one value suits models of any scale.

Passive handling is scoped to contacts whose every dof is a flex vertex
carried by the metric: flex against flex, flex against itself, and flex
against static geometry, which contributes no dofs of its own. For those
the Hessian is assembled in full. Contact with a body that can move
would have that body's dofs dropped from it, and is left on the
constraint solver.

The feature now requires an integrator whose constraint solve runs in
that metric, and is rejected with an error otherwise.

Add model/flex/drape.xml as the example model, replacing sphere_passive,
whose contacts no longer demonstrated the feature.
This commit is contained in:
Alessio
2026-08-06 15:38:05 +01:00
parent 0accc5b3c7
commit 2a3554c8a3
12 changed files with 469 additions and 61 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/XMLreference.rst
+18 -3
View File
@@ -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:
doc/changelog.rst
+15
View File
@@ -46,6 +46,17 @@ Engine
.. admonition:: Breaking API changes
:class: attention
- Contacts of a flex with :ref:`passive<flexcomp-contact-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 <https://github.com/google-deepmind/mujoco/blob/main/model/flex/drape.xml>`__ example model: three
cloths draped over a sphere, demonstrating :ref:`passive<flex-contact-passive>` collisions. It replaces the
``sphere_passive`` model, which has been removed.
- Added `bag <https://github.com/google-deepmind/mujoco/blob/main/model/flex/bag.xml>`__ 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
model/flex/drape.xml
+52
View File
@@ -0,0 +1,52 @@
<!-- Copyright 2026 DeepMind Technologies Limited
Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at
http://www.apache.org/licenses/LICENSE-2.0
Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
-->
<mujoco model="Drape">
<include file="scene.xml"/>
<!-- Three cloths dropped over a sphere. Every contact here is handled by the passive path: cloth
against cloth, each cloth against itself, and cloth against the sphere, which is static and
so contributes no dofs of its own. Their stiffness is carried implicitly by the effective
metric, which is why the sheets rest on each other without interpenetrating. -->
<!-- The metric solve for qacc_smooth is iterative and its blocks do not see the vertex-to-vertex
coupling that contact introduces, so a contact-rich scene like this one needs a larger
iteration budget than the default to converge it. -->
<option timestep="0.002" solver="CG" tolerance="1e-6" iterations="400" integrator="implicitfast"/>
<size memory="50M"/>
<worldbody>
<geom name="ball" type="sphere" size=".3" pos="0 0 .3" rgba=".45 .45 .5 1"/>
<flexcomp type="grid" count="13 13 1" spacing=".055 .055 .055" pos="0 0 .68"
radius=".004" mass=".25" name="cloth1" dim="2" rgba=".85 .35 .25 1">
<contact selfcollide="auto" passive="true" solref="0.01 1" solimp=".95 .99 .0001"/>
<elasticity young="2e4" poisson=".2" thickness="1e-3" elastic2d="both" damping="1e-4"/>
</flexcomp>
<flexcomp type="grid" count="13 13 1" spacing=".055 .055 .055" pos=".06 -.04 .78"
radius=".004" mass=".25" name="cloth2" dim="2" rgba=".25 .55 .8 1">
<contact selfcollide="auto" passive="true" solref="0.01 1" solimp=".95 .99 .0001"/>
<elasticity young="2e4" poisson=".2" thickness="1e-3" elastic2d="both" damping="1e-4"/>
</flexcomp>
<flexcomp type="grid" count="13 13 1" spacing=".055 .055 .055" pos="-.05 .05 .88"
radius=".004" mass=".25" name="cloth3" dim="2" rgba=".95 .8 .3 1">
<contact selfcollide="auto" passive="true" solref="0.01 1" solimp=".95 .99 .0001"/>
<elasticity young="2e4" poisson=".2" thickness="1e-3" elastic2d="both" damping="1e-4"/>
</flexcomp>
</worldbody>
</mujoco>
model/flex/sphere_passive.xml
-36
View File
@@ -1,36 +0,0 @@
<!-- Copyright 2024 DeepMind Technologies Limited
Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at
http://www.apache.org/licenses/LICENSE-2.0
Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
-->
<mujoco model="Full-flex sphere">
<include file="scene.xml"/>
<option solver="CG" tolerance="1e-6" timestep=".001" integrator="implicitfast"/>
<size memory="10M"/>
<visual>
<map stiffness="500"/>
</visual>
<worldbody>
<geom type="box" pos="1.5 0 0.25" size=".5 2 .25"/>
<geom type="box" pos="0 0 0.25" size="2 2 .05" euler="0 15 0"/>
<flexcomp type="ellipsoid" count="8 8 8" spacing=".07 .07 .07" pos="-.5 0 1" dim="3"
radius=".001" rgba="0 .7 .7 1" mass="5" name="slow">
<edge equality="true"/>
<contact selfcollide="none" internal="false" passive="true"/>
</flexcomp>
</worldbody>
</mujoco>
src/engine/engine_core_constraint.c
+20 -5
View File
@@ -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
src/engine/engine_derivative.c
+257 -5
View File
@@ -17,6 +17,7 @@
#include <mujoco/mjdata.h>
#include <mujoco/mjmodel.h>
#include <mujoco/mjsan.h> // 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)
src/engine/engine_derivative.h
+10 -1
View File
@@ -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);
src/engine/engine_forward.c
+23 -1
View File
@@ -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);
src/engine/engine_passive.c
+5 -4
View File
@@ -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 {
src/engine/engine_setconst.c
+3 -2
View File
@@ -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);
test/engine/engine_derivative_test.cc
+4 -4
View File
@@ -2153,13 +2153,13 @@ TEST_F(DerivativeTest, FlexStiffAssemble) {
std::vector<int> 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<int> colind(nnz);
std::vector<mjtNum> 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<int> 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<int> colind(nnz);
std::vector<mjtNum> 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)
test/engine/engine_forward_test.cc
+62
View File
@@ -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"(
<mujoco>
<option timestep="0.002" integrator="implicitfast" solver="CG" iterations="400"/>
<worldbody>
<flexcomp name="lower" type="grid" dim="2" count="9 9 1" spacing=".04 .04 1"
radius=".004" mass=".3" pos="0 0 .2">
<contact selfcollide="auto" passive="true"/>
<elasticity young="1e5" poisson=".2" thickness="2e-3" elastic2d="both" damping="1e-4"/>
<pin id="0 8 72 80"/>
</flexcomp>
<flexcomp name="upper" type="grid" dim="2" count="5 5 1" spacing=".04 .04 1"
radius=".004" mass=".1" pos="0 0 .27">
<contact selfcollide="auto" passive="true"/>
<elasticity young="1e5" poisson=".2" thickness="2e-3" elastic2d="both" damping="1e-4"/>
</flexcomp>
</worldbody>
</mujoco>
)";
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"(
<mujoco>