From 8acd83f3173df98da3379f2aa96f8d76b85c3741 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Mon, 1 Sep 2025 04:13:07 -0700 Subject: [PATCH] Add `passive` collision mode for flexes. PiperOrigin-RevId: 801754907 Change-Id: I4147af2ee2596519d42cb9a03cf8d5488f0bc3e8 --- doc/XMLreference.rst | 8 + doc/XMLschema.rst | 4 +- doc/changelog.rst | 1 + doc/includes/references.h | 4 +- include/mujoco/mjdata.h | 2 +- include/mujoco/mjmodel.h | 1 + include/mujoco/mjspec.h | 1 + include/mujoco/mjxmacro.h | 1 + mjx/mujoco/mjx/_src/types.py | 1 + model/flex/sphere_passive.xml | 36 ++++ python/mujoco/introspect/structs.py | 15 +- src/engine/engine_core_constraint.c | 263 ++++++++++++++------------- src/engine/engine_core_constraint.h | 8 +- src/engine/engine_passive.c | 93 +++++++++- src/user/user_model.cc | 1 + src/xml/xml_native_reader.cc | 14 +- unity/Runtime/Bindings/MjBindings.cs | 1 + 17 files changed, 320 insertions(+), 134 deletions(-) create mode 100644 model/flex/sphere_passive.xml diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index a25874c0..131132a6 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -3735,6 +3735,7 @@ saving the XML: .. _flexcomp-contact-solimp: .. _flexcomp-contact-margin: .. _flexcomp-contact-gap: +.. _flexcomp-contact-passive: .. |body/flexcomp/contact attrib list| replace:: :at:`internal`, :at:`selfcollide`, :at:`vertcollide`, :at:`activelayers`, :at:`contype`, :at:`conaffinity`, @@ -4298,6 +4299,13 @@ extensions specific to flexes. |deformable/flex/contact attrib list| Same meaning as regular :ref:`geom ` attributes. +.. _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 and might change in future releases. + .. _deformable-skin: diff --git a/doc/XMLschema.rst b/doc/XMLschema.rst index a8515f32..538e1b62 100644 --- a/doc/XMLschema.rst +++ b/doc/XMLschema.rst @@ -458,7 +458,7 @@ | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | | | | | :ref:`margin` | :ref:`gap` | :ref:`internal` | :ref:`selfcollide` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`activelayers` | :ref:`vertcollide` | | | | +| | | | :ref:`activelayers` | :ref:`vertcollide` | :ref:`passive` | | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ | |_2| flexcomp |br| |_2| |L| | | .. table:: | @@ -508,7 +508,7 @@ | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | | | | | :ref:`margin` | :ref:`gap` | :ref:`internal` | :ref:`selfcollide` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`activelayers` | :ref:`vertcollide` | | | | +| | | | :ref:`activelayers` | :ref:`vertcollide` | :ref:`passive` | | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ | |_2| flex |br| |_2| |L| | | .. table:: | diff --git a/doc/changelog.rst b/doc/changelog.rst index d960749a..dd3326d9 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -54,6 +54,7 @@ General - Added support for shells with a curved reference configuration. See this `example `__. +- Added experimental option for :ref:`passive` contacts involving flexes. - Added support for assigning a default material to a mesh asset using the :ref:`mesh/material ` attribute. diff --git a/doc/includes/references.h b/doc/includes/references.h index f9535447..90914e75 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -123,7 +123,7 @@ struct mjContact_ { // result of collision detection functions int vert[2]; // vertex ids; -1 for geom or flex element // flag set by mj_setContact or mj_instantiateContact - int exclude; // 0: include, 1: in gap, 2: fused, 3: no dofs + int exclude; // 0: include, 1: in gap, 2: fused, 3: no dofs, 4: passive // address computed by mj_instantiateContact int efc_address; // address in efc; -1: not included @@ -1220,6 +1220,7 @@ struct mjModel_ { mjtByte* flex_internal; // internal flex collision enabled (nflex x 1) int* flex_selfcollide; // self collision mode (mjtFlexSelf) (nflex x 1) int* flex_activelayers; // number of active element layers, 3D only (nflex x 1) + int* flex_passive; // passive collisions enabled (nflex x 1) // flexes: other properties int* flex_dim; // 1: lines, 2: triangles, 3: tetrahedra (nflex x 1) @@ -2104,6 +2105,7 @@ typedef struct mjsFlex_ { // flex specification mjtByte flatskin; // render flex skin with flat shading int selfcollide; // mode for flex self collision int vertcollide; // mode for vertex collision + int passive; // mode for passive collisions int activelayers; // number of active element layers in 3D int group; // group for visualizatioh double edgestiffness; // edge stiffness diff --git a/include/mujoco/mjdata.h b/include/mujoco/mjdata.h index 60e8a11b..13ddf7a8 100644 --- a/include/mujoco/mjdata.h +++ b/include/mujoco/mjdata.h @@ -143,7 +143,7 @@ struct mjContact_ { // result of collision detection functions int vert[2]; // vertex ids; -1 for geom or flex element // flag set by mj_setContact or mj_instantiateContact - int exclude; // 0: include, 1: in gap, 2: fused, 3: no dofs + int exclude; // 0: include, 1: in gap, 2: fused, 3: no dofs, 4: passive // address computed by mj_instantiateContact int efc_address; // address in efc; -1: not included diff --git a/include/mujoco/mjmodel.h b/include/mujoco/mjmodel.h index 8ee5d111..c50f6554 100644 --- a/include/mujoco/mjmodel.h +++ b/include/mujoco/mjmodel.h @@ -899,6 +899,7 @@ struct mjModel_ { mjtByte* flex_internal; // internal flex collision enabled (nflex x 1) int* flex_selfcollide; // self collision mode (mjtFlexSelf) (nflex x 1) int* flex_activelayers; // number of active element layers, 3D only (nflex x 1) + int* flex_passive; // passive collisions enabled (nflex x 1) // flexes: other properties int* flex_dim; // 1: lines, 2: triangles, 3: tetrahedra (nflex x 1) diff --git a/include/mujoco/mjspec.h b/include/mujoco/mjspec.h index a8aa6f4c..5b2f42d8 100644 --- a/include/mujoco/mjspec.h +++ b/include/mujoco/mjspec.h @@ -437,6 +437,7 @@ typedef struct mjsFlex_ { // flex specification mjtByte flatskin; // render flex skin with flat shading int selfcollide; // mode for flex self collision int vertcollide; // mode for vertex collision + int passive; // mode for passive collisions int activelayers; // number of active element layers in 3D int group; // group for visualizatioh double edgestiffness; // edge stiffness diff --git a/include/mujoco/mjxmacro.h b/include/mujoco/mjxmacro.h index 32cec8e6..1ee6bd97 100644 --- a/include/mujoco/mjxmacro.h +++ b/include/mujoco/mjxmacro.h @@ -335,6 +335,7 @@ X ( mjtByte, flex_internal, nflex, 1 ) \ X ( int, flex_selfcollide, nflex, 1 ) \ X ( int, flex_activelayers, nflex, 1 ) \ + X ( int, flex_passive, nflex, 1 ) \ X ( int, flex_dim, nflex, 1 ) \ X ( int, flex_matid, nflex, 1 ) \ X ( int, flex_group, nflex, 1 ) \ diff --git a/mjx/mujoco/mjx/_src/types.py b/mjx/mujoco/mjx/_src/types.py index a40f55a4..b4a906e6 100644 --- a/mjx/mujoco/mjx/_src/types.py +++ b/mjx/mujoco/mjx/_src/types.py @@ -570,6 +570,7 @@ class ModelC(PyTreeNode): flex_internal: jax.Array flex_selfcollide: jax.Array flex_activelayers: jax.Array + flex_passive: jax.Array flex_dim: jax.Array flex_vertadr: jax.Array flex_vertnum: jax.Array diff --git a/model/flex/sphere_passive.xml b/model/flex/sphere_passive.xml new file mode 100644 index 00000000..3ca4d0d2 --- /dev/null +++ b/model/flex/sphere_passive.xml @@ -0,0 +1,36 @@ + + + + + + diff --git a/python/mujoco/introspect/structs.py b/python/mujoco/introspect/structs.py index 98b39032..6a0f43c1 100644 --- a/python/mujoco/introspect/structs.py +++ b/python/mujoco/introspect/structs.py @@ -2509,6 +2509,14 @@ STRUCTS: Mapping[str, StructDecl] = dict([ doc='number of active element layers, 3D only', array_extent=('nflex',), ), + StructFieldDecl( + name='flex_passive', + type=PointerType( + inner_type=ValueType(name='int'), + ), + doc='passive collisions enabled', + array_extent=('nflex',), + ), StructFieldDecl( name='flex_dim', type=PointerType( @@ -4878,7 +4886,7 @@ STRUCTS: Mapping[str, StructDecl] = dict([ StructFieldDecl( name='exclude', type=ValueType(name='int'), - doc='0: include, 1: in gap, 2: fused, 3: no dofs', + doc='0: include, 1: in gap, 2: fused, 3: no dofs, 4: passive', ), StructFieldDecl( name='efc_address', @@ -9125,6 +9133,11 @@ STRUCTS: Mapping[str, StructDecl] = dict([ type=ValueType(name='int'), doc='mode for vertex collision', ), + StructFieldDecl( + name='passive', + type=ValueType(name='int'), + doc='mode for passive collisions', + ), StructFieldDecl( name='activelayers', type=ValueType(name='int'), diff --git a/src/engine/engine_core_constraint.c b/src/engine/engine_core_constraint.c index 6f145161..40f37813 100644 --- a/src/engine/engine_core_constraint.c +++ b/src/engine/engine_core_constraint.c @@ -852,6 +852,86 @@ void mj_instantiateLimit(const mjModel* m, mjData* d) { +// compute Jacobian for contact, return number of DOFs affected +int mj_contactJacobian(const mjModel* m, mjData* d, const mjContact* con, int dim, + mjtNum* jac, mjtNum* jacdif, mjtNum* jacdifp, + mjtNum* jacdifr, mjtNum* jac1p, mjtNum* jac2p, + mjtNum* jac1r, mjtNum* jac2r, int* chain) { + // special case: single body on each side + if ((con->geom[0] >= 0 || (con->vert[0] >= 0 && m->flex_interp[con->flex[0]] == 0)) && + (con->geom[1] >= 0 || (con->vert[1] >= 0 && m->flex_interp[con->flex[1]] == 0))) { + // get bodies + int bid[2]; + for (int side=0; side < 2; side++) { + bid[side] = (con->geom[side] >= 0) ? + m->geom_bodyid[con->geom[side]] : + m->flex_vertbodyid[m->flex_vertadr[con->flex[side]] + con->vert[side]]; + } + + // compute Jacobian differences + if (dim > 3) { + return mj_jacDifPair(m, d, chain, bid[0], bid[1], con->pos, con->pos, + jac1p, jac2p, jacdifp, jac1r, jac2r, jacdifr); + } else { + return mj_jacDifPair(m, d, chain, bid[0], bid[1], con->pos, con->pos, + jac1p, jac2p, jacdifp, NULL, NULL, NULL); + } + } + + // general case: flex elements involved + else { + // get bodies and weights + int nb = 0; + int bid[64]; + mjtNum bweight[64]; + for (int side=0; side < 2; side++) { + int nw = 0; + int vid[4]; + mjtNum bw[4]; + + // geom + if (con->geom[side] >= 0) { + bid[nb] = m->geom_bodyid[con->geom[side]]; + bweight[nb] = side ? +1 : -1; + nb++; + } + + // flex vert + else if (con->vert[side] >= 0) { + vid[0] = m->flex_vertadr[con->flex[side]] + con->vert[side]; + bw[0] = side ? +1 : -1; + nw = 1; + } + + // flex elem + else { + nw = mj_elemBodyWeight(m, d, con->flex[side], con->elem[side], + con->vert[1-side], con->pos, vid, bw); + + // negative sign for first side of contact + if (side == 0) { + mju_scl(bw, bw, -1, nw); + } + } + + // get body or node ids and weights + for (int k=0; k < nw; k++) { + if (m->flex_interp[con->flex[side]] == 0) { + bid[nb] = m->flex_vertbodyid[vid[k]]; + bweight[nb] = bw[k]; + nb++; + } else { + nb += mj_vertBodyWeight(m, d, con->flex[side], vid[k], + con->pos, bid+nb, bweight+nb, bw[k]); + } + } + } + + // combine weighted Jacobians + return mj_jacSum(m, d, chain, nb, bid, bweight, con->pos, jacdif, dim > 3); + } +} + // frictionless and frictional contacts void mj_instantiateContact(const mjModel* m, mjData* d) { int ispyramid = mj_isPyramidal(m), issparse = mj_isSparse(m), ncon = d->ncon; @@ -880,142 +960,72 @@ void mj_instantiateContact(const mjModel* m, mjData* d) { // find contacts to be included for (int i=0; i < ncon; i++) { - if (!d->contact[i].exclude) { - // get contact info, safe efc_address - con = d->contact + i; - dim = con->dim; - con->efc_address = d->nefc; + if (d->contact[i].exclude) { + continue; + } - // special case: single body on each side - if ((con->geom[0] >= 0 || (con->vert[0] >= 0 && m->flex_interp[con->flex[0]] == 0)) && - (con->geom[1] >= 0 || (con->vert[1] >= 0 && m->flex_interp[con->flex[1]] == 0))) { - // get bodies - int bid[2]; - for (int side=0; side < 2; side++) { - bid[side] = (con->geom[side] >= 0) ? - m->geom_bodyid[con->geom[side]] : - m->flex_vertbodyid[m->flex_vertadr[con->flex[side]] + con->vert[side]]; - } + // get contact info, save efc_address + con = d->contact + i; + dim = con->dim; + con->efc_address = d->nefc; + NV = mj_contactJacobian(m, d, con, dim, jac, jacdif, jacdifp, jacdifr, + jac1p, jac2p, jac1r, jac2r, chain); - // compute Jacobian differences - if (dim > 3) { - NV = mj_jacDifPair(m, d, chain, bid[0], bid[1], con->pos, con->pos, - jac1p, jac2p, jacdifp, jac1r, jac2r, jacdifr); - } else { - NV = mj_jacDifPair(m, d, chain, bid[0], bid[1], con->pos, con->pos, - jac1p, jac2p, jacdifp, NULL, NULL, NULL); - } - } + // skip contact if no DOFs affected + if (NV == 0) { + con->efc_address = -1; + con->exclude = 3; + continue; + } - // general case: flex elements involved - else { - // get bodies and weights - int nb = 0; - int bid[64]; - mjtNum bweight[64]; - for (int side=0; side < 2; side++) { - int nw = 0; - int vid[4]; - mjtNum bw[4]; + // rotate Jacobian differences to contact frame + mju_mulMatMat(jac, con->frame, jacdifp, dim > 1 ? 3 : 1, 3, NV); + if (dim > 3) { + mju_mulMatMat(jac + 3*NV, con->frame, jacdifr, dim-3, 3, NV); + } - // geom - if (con->geom[side] >= 0) { - bid[nb] = m->geom_bodyid[con->geom[side]]; - bweight[nb] = side ? +1 : -1; - nb++; - } + // make frictionless contact + if (dim == 1) { + // add constraint + mj_addConstraint(m, d, jac, &(con->dist), &(con->includemargin), 0, + 1, mjCNSTR_CONTACT_FRICTIONLESS, i, + issparse ? NV : 0, + issparse ? chain : NULL); + } - // flex vert - else if (con->vert[side] >= 0) { - vid[0] = m->flex_vertadr[con->flex[side]] + con->vert[side]; - bw[0] = side ? +1 : -1; - nw = 1; - } + // make pyramidal friction cone + else if (ispyramid) { + // pos = dist + cpos[0] = cpos[1] = con->dist; + cmargin[0] = cmargin[1] = con->includemargin; - // flex elem - else { - nw = mj_elemBodyWeight(m, d, con->flex[side], con->elem[side], - con->vert[1-side], con->pos, vid, bw); + // one pair per friction dimension + for (int k=1; k < con->dim; k++) { + // Jacobian for pair of opposing pyramid edges + mju_addScl(jacdifp, jac, jac + k*NV, con->friction[k-1], NV); + mju_addScl(jacdifp + NV, jac, jac + k*NV, -con->friction[k-1], NV); - // negative sign for first side of contact - if (side == 0) { - mju_scl(bw, bw, -1, nw); - } - } - - // get body or node ids and weights - for (int k=0; k < nw; k++) { - if (m->flex_interp[con->flex[side]] == 0) { - bid[nb] = m->flex_vertbodyid[vid[k]]; - bweight[nb] = bw[k]; - nb++; - } else { - nb += mj_vertBodyWeight(m, d, con->flex[side], vid[k], - con->pos, bid+nb, bweight+nb, bw[k]); - } - } - } - - // combine weighted Jacobians - NV = mj_jacSum(m, d, chain, nb, bid, bweight, con->pos, jacdif, dim > 3); - } - - // skip contact if no DOFs affected - if (NV == 0) { - con->efc_address = -1; - con->exclude = 3; - continue; - } - - // rotate Jacobian differences to contact frame - mju_mulMatMat(jac, con->frame, jacdifp, dim > 1 ? 3 : 1, 3, NV); - if (dim > 3) { - mju_mulMatMat(jac + 3*NV, con->frame, jacdifr, dim-3, 3, NV); - } - - // make frictionless contact - if (dim == 1) { // add constraint - mj_addConstraint(m, d, jac, &(con->dist), &(con->includemargin), 0, - 1, mjCNSTR_CONTACT_FRICTIONLESS, i, + mj_addConstraint(m, d, jacdifp, cpos, cmargin, 0, + 2, mjCNSTR_CONTACT_PYRAMIDAL, i, issparse ? NV : 0, issparse ? chain : NULL); } + } - // make pyramidal friction cone - else if (ispyramid) { - // pos = dist - cpos[0] = cpos[1] = con->dist; - cmargin[0] = cmargin[1] = con->includemargin; + // make elliptic friction cone + else { + // normal pos = dist, all others 0 + mju_zero(cpos, con->dim); + mju_zero(cmargin, con->dim); + cpos[0] = con->dist; + cmargin[0] = con->includemargin; - // one pair per friction dimension - for (int k=1; k < con->dim; k++) { - // Jacobian for pair of opposing pyramid edges - mju_addScl(jacdifp, jac, jac + k*NV, con->friction[k-1], NV); - mju_addScl(jacdifp + NV, jac, jac + k*NV, -con->friction[k-1], NV); - - // add constraint - mj_addConstraint(m, d, jacdifp, cpos, cmargin, 0, - 2, mjCNSTR_CONTACT_PYRAMIDAL, i, - issparse ? NV : 0, - issparse ? chain : NULL); - } - } - - // make elliptic friction cone - else { - // normal pos = dist, all others 0 - mju_zero(cpos, con->dim); - mju_zero(cmargin, con->dim); - cpos[0] = con->dist; - cmargin[0] = con->includemargin; - - // add constraint - mj_addConstraint(m, d, jac, cpos, cmargin, 0, - con->dim, mjCNSTR_CONTACT_ELLIPTIC, i, - issparse ? NV : 0, - issparse ? chain : NULL); - } + // add constraint + mj_addConstraint(m, d, jac, cpos, cmargin, 0, + con->dim, mjCNSTR_CONTACT_ELLIPTIC, i, + issparse ? NV : 0, + issparse ? chain : NULL); } } @@ -1814,6 +1824,13 @@ 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; + } + // skip if excluded if (con->exclude) { continue; diff --git a/src/engine/engine_core_constraint.h b/src/engine/engine_core_constraint.h index 32043f92..514abc3d 100644 --- a/src/engine/engine_core_constraint.h +++ b/src/engine/engine_core_constraint.h @@ -65,9 +65,15 @@ void mj_instantiateFriction(const mjModel* m, mjData* d); // joint and tendon limits void mj_instantiateLimit(const mjModel* m, mjData* d); -// frictionelss and frictional contacts +// frictionless and frictional contacts void mj_instantiateContact(const mjModel* m, mjData* d); +// compute Jacobian for contact, return number of DOFs affected +int mj_contactJacobian(const mjModel* m, mjData* d, const mjContact* con, int dim, + mjtNum* jac, mjtNum* jacdif, mjtNum* jacdifp, + mjtNum* jacdifr, mjtNum* jac1p, mjtNum* jac2p, + mjtNum* jac1r, mjtNum* jac2r, int* chain); + //------------------------ parameter computation/extraction ---------------------------------------- diff --git a/src/engine/engine_passive.c b/src/engine/engine_passive.c index 61fc46da..45b58cdb 100644 --- a/src/engine/engine_passive.c +++ b/src/engine/engine_passive.c @@ -36,6 +36,9 @@ //----------------------------- 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 static const int edges[2][6][2] = {{{1, 2}, {2, 0}, {0, 1}, {0, 0}, {0, 0}, {0, 0}}, @@ -515,6 +518,89 @@ static int mj_fluid(const mjModel* m, mjData* d) { +// passive contact forces +int mj_contactPassive(const mjModel* m, mjData* d) { + int ncon = d->ncon, issparse = mj_isSparse(m); + int dim, NV, nv = m->nv, *chain = NULL; + mjtNum *jac, *jacdif, *jacdifp, *jacdifr, *jac1p, *jac2p, *jac1r, *jac2r, *qfrc; + mjContact* con; + int has_contact = 0; + + if (mjDISABLED(mjDSBL_CONTACT) || ncon == 0 || nv == 0) { + return 0; + } + + // early return if no contact to be included + for (int i=0; i < ncon; i++) { + if (d->contact[i].exclude != 4) { + continue; + } + has_contact = 1; + } + + if (!has_contact) { + return 0; + } + + // allocate Jacobian + mj_markStack(d); + jac = mjSTACKALLOC(d, 6*nv, mjtNum); + jacdif = mjSTACKALLOC(d, 6*nv, mjtNum); + jacdifp = jacdif; + jacdifr = jacdif + 3*nv; + jac1p = mjSTACKALLOC(d, 3*nv, mjtNum); + jac2p = mjSTACKALLOC(d, 3*nv, mjtNum); + jac1r = mjSTACKALLOC(d, 3*nv, mjtNum); + jac2r = mjSTACKALLOC(d, 3*nv, mjtNum); + qfrc = mjSTACKALLOC(d, nv, mjtNum); + if (issparse) { + chain = mjSTACKALLOC(d, nv, int); + } + + // find contacts to be included + for (int i=0; i < ncon; i++) { + if (d->contact[i].exclude != 4) { + continue; + } + + // get contact info, safe efc_address + con = d->contact + i; + dim = con->dim; + con->efc_address = -1; + NV = mj_contactJacobian(m, d, con, dim, jac, jacdif, jacdifp, jacdifr, + jac1p, jac2p, jac1r, jac2r, chain); + + // skip contact if no DOFs affected + if (NV == 0) { + con->efc_address = -1; + con->exclude = 3; + continue; + } + + // rotate Jacobian differences to contact frame + mju_mulMatMat(jac, con->frame, jacdifp, dim > 1 ? 3 : 1, 3, NV); + if (dim > 3) { + mju_mulMatMat(jac + 3*NV, con->frame, jacdifr, dim-3, 3, NV); + } + + // compute passive contact force (dim = 1) + mjtNum scl = -kContactStiffness*con->dist; + if (!issparse) { + mju_addToScl(d->qfrc_spring, jac, scl, nv); + } else { + mju_scl(qfrc, jac, scl, NV); + for (int j=0; j < NV; j++) { + d->qfrc_spring[chain[j]] += qfrc[j]; + } + } + } + + mj_freeStack(d); + return has_contact; +} + + + // all passive forces void mj_passive(const mjModel* m, mjData* d) { int nv = m->nv; @@ -540,9 +626,14 @@ void mj_passive(const mjModel* m, mjData* d) { // fluid forces int has_fluid = mj_fluid(m, d); + // contact forces + mj_contactPassive(m, d); + // add passive forces into qfrc_passive mju_add(d->qfrc_passive, d->qfrc_spring, d->qfrc_damper, nv); - if (has_fluid) mju_addTo(d->qfrc_passive, d->qfrc_fluid, nv); + if (has_fluid) { + mju_addTo(d->qfrc_passive, d->qfrc_fluid, nv); + } if (has_gravcomp) { int njnt = m->njnt; for (int i=0; i < njnt; i++) { diff --git a/src/user/user_model.cc b/src/user/user_model.cc index 40b6b7e9..964e6e5b 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -3321,6 +3321,7 @@ void mjCModel::CopyObjects(mjModel* m) { m->flex_flatskin[i] = pfl->flatskin; m->flex_selfcollide[i] = pfl->selfcollide; m->flex_activelayers[i] = pfl->activelayers; + m->flex_passive[i] = pfl->passive; m->flex_bvhnum[i] = pfl->tree.Nbvh(); m->flex_bvhadr[i] = pfl->tree.Nbvh() ? bvh_adr : -1; diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index b2acf2d0..ee2cbb0d 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -316,9 +316,9 @@ const char* MJCF[nMJCF][mjXATTRNUM] = { {"<"}, {"edge", "?", "5", "equality", "solref", "solimp", "stiffness", "damping"}, {"elasticity", "?", "5", "young", "poisson", "damping", "thickness", "elastic2d"}, - {"contact", "?", "14", "contype", "conaffinity", "condim", "priority", + {"contact", "?", "15", "contype", "conaffinity", "condim", "priority", "friction", "solmix", "solref", "solimp", "margin", "gap", - "internal", "selfcollide", "activelayers", "vertcollide"}, + "internal", "selfcollide", "activelayers", "vertcollide", "passive"}, {"pin", "*", "4", "id", "range", "grid", "gridrange"}, {"plugin", "*", "2", "plugin", "instance"}, {"<"}, @@ -332,9 +332,9 @@ const char* MJCF[nMJCF][mjXATTRNUM] = { {"flex", "*", "13", "name", "group", "dim", "radius", "material", "rgba", "flatskin", "body", "vertex", "element", "texcoord", "elemtexcoord", "node"}, {"<"}, - {"contact", "?", "14", "contype", "conaffinity", "condim", "priority", + {"contact", "?", "15", "contype", "conaffinity", "condim", "priority", "friction", "solmix", "solref", "solimp", "margin", "gap", - "internal", "selfcollide", "activelayers", "vertcollide"}, + "internal", "selfcollide", "activelayers", "vertcollide", "passive"}, {"edge", "?", "2", "stiffness", "damping"}, {"elasticity", "?", "5", "young", "poisson", "damping", "thickness", "elastic2d"}, {">"}, @@ -1460,6 +1460,9 @@ void mjXReader::OneFlex(XMLElement* elem, mjsFlex* flex) { if (MapValue(cont, "vertcollide", &flex->vertcollide, bool_map, 2)) { flex->vertcollide = (n == 1); } + if (MapValue(cont, "passive", &flex->passive, bool_map, 2)) { + flex->passive = (n == 1); + } ReadAttrInt(cont, "activelayers", &flex->activelayers); } @@ -2721,6 +2724,9 @@ void mjXReader::OneFlexcomp(XMLElement* elem, mjsBody* body, const mjVFS* vfs) { if (MapValue(cont, "vertcollide", &n, bool_map, 2)) { dflex.vertcollide = (n == 1); } + if (MapValue(cont, "passive", &n, bool_map, 2)) { + dflex.passive = (n == 1); + } ReadAttrInt(cont, "activelayers", &dflex.activelayers); } diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index 0a760f0c..15846de8 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -5477,6 +5477,7 @@ public unsafe struct mjModel_ { public byte* flex_internal; public int* flex_selfcollide; public int* flex_activelayers; + public int* flex_passive; public int* flex_dim; public int* flex_matid; public int* flex_group;