diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index 838c1d1a..5111bdf9 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -2797,6 +2797,29 @@ helps clarify the role of bodies and geoms in MuJoCo. additionally displays an arrow along the tangential surface velocity at the contact point. This attribute can be modified at runtime. +.. _body-geom-adhesion: + +.. youtube:: GioWwB36XHI + :align: right + :width: 40% + +:at:`adhesion`: :at-val:`real, "0"` + Adhesive force of contacts with this geom, in units of force. Geometrically, the friction cone is translated down + along the normal so that the force origin lies strictly inside it: each contact can pull with up to ``adhesion`` + before breaking, and the friction budget becomes :math:`\mu(f_N + \text{adhesion})`. Contacts resist sliding even + under zero normal force, the defining property of cohesive materials. This is useful for sticky materials (tape, + gecko feet, tacky rubber) and as a physical stabilizer for grasping. The adhesion of a contact is the sum of the + values of the two contacting geoms, or the value of the higher-:ref:`priority` geom if priorities + differ; an explicit contact :ref:`pair` overrides both. Note that adhesion is *per contact*: a box face + resting on a plane generates four contact points and therefore four times the pull-off force of a single-point + contact. To let adhesion act across a small separation (attraction at a distance), set :ref:`gap` to + the desired interaction range. This can be used to model magnets. Resting penetration is unaffected by adhesion (the + compression behavior of the contact is unchanged; only a tensile branch is added), and :ref:`mj_contactForce` reports + the net interface force, whose normal component can be negative under tension. The underlying model is described in + the :ref:`Computation chapter`. For adhesion as a *controlled* force — switched on and off like a vacuum + gripper, dividing a total force between a body's contacts and pressing the bodies together — see the :ref:`adhesion + actuator`. + .. _body-geom-fromto: :at:`fromto`: :at-val:`real(6), optional` @@ -3488,10 +3511,12 @@ joints and tendons have different sets of attributes, while all geoms in the com .. _composite-geom-surfacevel: +.. _composite-geom-adhesion: + .. |body/composite/geom attrib list| replace:: :at:`type`, :at:`contype`, :at:`conaffinity`, :at:`condim`, :at:`group`, :at:`priority`, :at:`size`, :at:`material`, :at:`rgba`, :at:`friction`, :at:`mass`, :at:`density`, :at:`solmix`, :at:`solref`, :at:`solimp`, :at:`margin`, - :at:`gap`, :at:`surfacevel` + :at:`gap`, :at:`surfacevel`, :at:`adhesion` |body/composite/geom attrib list| Same meaning as regular :ref:`geom ` attributes. @@ -4224,6 +4249,12 @@ friction can only be created with this element. ``margin`` and ``margin + gap`` are included in ``mjData.contact`` as inactive contacts but no contact forces are generated. +.. _contact-pair-adhesion: + +:at:`adhesion`: :at-val:`real, "0"` + Adhesive force of contacts generated by this pair, overriding the sum of the geoms' + :ref:`adhesion` values. See there for detailed semantics. + .. _contact-exclude: @@ -6415,7 +6446,9 @@ geckos and insects rather than an industrial vacuum gripper. In order to enable :ref:`gap` attribute of the body's geoms to a positive value. This creates a layer around each geom where contacts are detected but no contact forces are generated, and the adhesive force can act across this gap. In the video above, such inactive contacts are blue, while active contacts are orange. An adhesion actuator's length is always 0. -:at:`ctrlrange` is required and must also be nonnegative (no repulsive forces are allowed). The underlying :el:`general` +:at:`ctrlrange` is required and must also be nonnegative (no repulsive forces are allowed). For adhesion as a +*passive* property of the contacting surfaces — always on, per contact, and leaving resting penetration unaffected — +see the :ref:`geom/adhesion` attribute. The underlying :el:`general` attributes are set as follows: =========== ======= =========== ======== @@ -9586,6 +9619,8 @@ if omitted. .. _default-geom-surfacevel: +.. _default-geom-adhesion: + .. _default-geom-fromto: .. _default-geom-axisangle: @@ -9754,6 +9789,8 @@ if omitted. .. _default-pair-margin: +.. _default-pair-adhesion: + :el-prefix:`default/` |-| **pair** |?| ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ diff --git a/doc/XMLschema.rst b/doc/XMLschema.rst index 643af31e..c83e7afa 100644 --- a/doc/XMLschema.rst +++ b/doc/XMLschema.rst @@ -911,6 +911,9 @@ .. grid-item:: :ref:`surfacevel` + .. grid-item:: + :ref:`adhesion` + .. grid-item:: :ref:`fromto` @@ -1379,6 +1382,9 @@ .. grid-item:: :ref:`surfacevel` + .. grid-item:: + :ref:`adhesion` + .. dropdown:: :ref:`site` :octicon:`dot` @@ -1871,6 +1877,9 @@ .. grid-item:: :ref:`margin` + .. grid-item:: + :ref:`adhesion` + .. dropdown:: :ref:`exclude` |*| @@ -5504,6 +5513,9 @@ .. grid-item:: :ref:`surfacevel` + .. grid-item:: + :ref:`adhesion` + .. grid-item:: :ref:`fromto` @@ -5726,6 +5738,9 @@ .. grid-item:: :ref:`margin` + .. grid-item:: + :ref:`adhesion` + .. dropdown:: :ref:`equality` :octicon:`dot` diff --git a/doc/changelog.rst b/doc/changelog.rst index 17fb6db3..a8d3af11 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -21,6 +21,16 @@ Engine velocity relative to the moving surface rather than to the geom, since that is the quantity the constraint acts on; for geoms without :at:`surfacevel` the two are identical. Contact-point visualization draws an arrow along the surface velocity at contacts with moving surfaces. + +.. youtube:: GioWwB36XHI + :align: right + :width: 35% + +- Added :ref:`geom/adhesion` and :ref:`pair/adhesion`: an adhesive force + associated with a contact, useful for modeling sticky materials. Contacts can pull with up to the given force before + breaking, and the friction budget becomes :math:`\mu(f_N + \text{adhesion})`. Combined with :ref:`gap`, + adhesive contacts apply "adhesion at a distance", useful for modeling magnets. Resting penetration is unaffected by + adhesion. :ref:`mj_contactForce` reports the net interface force, whose normal component can now be negative. - Replaced midpoint integration of free bodies with :ref:`gyroscopic derivatives` in the ``implicitfast`` :ref:`integrator`: the bias-force derivative of every standalone free body is applied via a local unsymmetric solve of its decoupled block, making ``implicitfast`` identical to ``implicit`` for such bodies. @@ -56,7 +66,8 @@ Engine .. admonition:: Breaking ABI changes :class: caution - - Added ``texid``, ``texuniform`` and ``texrepeat`` fields to ``mjvGeom``. + - Added ``texid``, ``texuniform`` and ``texrepeat`` fields to :ref:`mjvGeom`. + - The :ref:`mjContact`` struct gained an ``adhesion`` member, changing its size and layout. .. admonition:: Bug fixes :class: admonition diff --git a/doc/computation/index.rst b/doc/computation/index.rst index b4efde27..d060ff54 100644 --- a/doc/computation/index.rst +++ b/doc/computation/index.rst @@ -1507,17 +1507,18 @@ here is to construct a sensible and intuitive parameterization of the constraint .. _soExactDiag: -**Diagonal approximation:** The approximation has three sources of error: (i) it is frozen at ``qpos0`` rather than -evaluated at the current configuration; (ii) it averages the directional inverse inertia into a scalar, assuming -isotropy; and (iii) it treats the contributions of different bodies as independent, ignoring kinematic coupling through -shared DOFs. These errors are usually modest, but can become significant for models with highly anisotropic inertias or -long kinematic chains that operate far from ``qpos0``. In severe cases — particularly when the averaged inertia becomes -near-zero despite finite directional inertia — the regularizer :math:`R` becomes near-zero, making constraints -infinitely hard and causing divergence. The :ref:`diagexact` flag replaces the approximation with -the exact diagonal :math:`A_{ii} = \|Y_i\|^2`, where :math:`Y = J M^{-1/2}` is the whitened Jacobian, computed at the -current configuration. This eliminates all three sources of error at a modest runtime cost: computing :math:`Y` requires -a back-substitution with the Cholesky factor of the mass matrix for each active constraint row; if -:ref:`dual solvers` are used (PGS or NoSlip), the cost is negligible since :math:`Y` is computed anyway. +Diagonal approximation + The approximation has three sources of error: (i) it is frozen at ``qpos0`` rather than evaluated at the current + configuration; (ii) it averages the directional inverse inertia into a scalar, assuming isotropy; and (iii) it treats + the contributions of different bodies as independent, ignoring kinematic coupling through shared DOFs. These errors + are usually modest, but can become significant for models with highly anisotropic inertias or long kinematic chains + that operate far from ``qpos0``. In severe cases — particularly when the averaged inertia becomes near-zero despite + finite directional inertia — the regularizer :math:`R` becomes near-zero, making constraints infinitely hard and + causing divergence. The :ref:`diagexact` flag replaces the approximation with the exact + diagonal :math:`A_{ii} = \|Y_i\|^2`, where :math:`Y = J M^{-1/2}` is the whitened Jacobian, computed at the current + configuration. This eliminates all three sources of error at a modest runtime cost: computing :math:`Y` requires a + back-substitution with the Cholesky factor of the mass matrix for each active constraint row; if :ref:`dual + solvers` are used (PGS or NoSlip), the cost is negligible since :math:`Y` is computed anyway. Next we explain how the reference acceleration is computed. As already mentioned, we use a spring-damper model parameterized by *damping* and *stiffness* coefficients element-wise: @@ -1537,6 +1538,28 @@ velocity of the surface material, so that the reference acceleration drives the surface; this is how conveyor belts and turntables are implemented, and it is also the quantity reported in the contact rows of ``mjData.efc_vel``. +.. _soAdhesion: + +Adhesion + Contacts of geoms with nonzero :ref:`adhesion` force :math:`\delta` can pull: the feasible force + set is the friction cone *translated down the contact normal* by :math:`\delta`. This is implemented with an exact + factorization which leaves the cone machinery untouched. A constant attractive force :math:`\delta` along the contact + normal is accumulated into the passive force ``mjData.qfrc_adhesion``, and the reference acceleration of the contact's + normal row is biased: + + .. math:: + \ar \rightarrow \ar + R \, \delta + + (for pyramidal cones the bias is distributed equally over the :math:`2(\mathrm{dim}-1)` edges). To see that this + factorization is exactly cone translation, combine :math:`f = (A+R)^{-1}(\ar - \au)` with :eq:`eq:identity` to obtain + the force relation :math:`R f = \ar - \ac`, and consider the net interface force :math:`f - \delta`: the passive + attraction cancels :math:`A \delta` in :eq:`eq:identity` while the bias cancels :math:`R \delta` in the force + relation, so the pair :math:`(f - \delta, \ac)` satisfies exactly the unbiased equations, with the cone membership of + :math:`f` becoming membership of the translated cone for :math:`f - \delta`. Consequently the compression branch of + the net contact force is independent of adhesion — resting penetration is unaffected — while a tensile branch of depth + :math:`\delta` is added. Adhesive contacts remain active when separated within the :ref:`gap` band and + the biased reference acceleration continues to pull the geoms together across this distance. + To summarize, the constraint behavior is determined by three per-constraint quantities: impedance :math:`0 0`, and stiffness :math:`k \geq 0`. These are computed from the :at:`solimp` and :at:`solref` attributes as described in the :ref:`solver parameters ` section of the Modeling chapter, which also offers additional diff --git a/doc/includes/references.h b/doc/includes/references.h index c8648e66..953abab1 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -34,6 +34,7 @@ typedef struct mjContact_ { // result of collision detection functions mjtNum solref[mjNREF]; // constraint solver reference, normal direction mjtNum solreffriction[mjNREF]; // constraint solver reference, friction directions mjtNum solimp[mjNIMP]; // constraint solver impedance + mjtNum adhesion; // adhesive force along the contact normal // internal storage used by solver mjtNum mu; // friction of regularized cone, set by mj_makeConstraint @@ -263,6 +264,7 @@ typedef struct mjData_ { mjtNum* qfrc_damper; // passive damper force (nv x 1) mjtNum* qfrc_gravcomp; // passive gravity compensation force (nv x 1) mjtNum* qfrc_fluid; // passive fluid force (nv x 1) + mjtNum* qfrc_adhesion; // passive contact adhesion force (nv x 1) mjtNum* qfrc_passive; // total passive force (nv x 1) // computed by mj_sensorVel/mj_subtreeVel if needed @@ -679,6 +681,7 @@ typedef struct mjModel_ { mjtBool flg_gravcomp; // whether any body has nonzero gravcomp mjtBool flg_surfacevel; // whether any geom has nonzero surfacevel + mjtBool flg_adhesion; // whether any geom or pair has nonzero adhesion // ------------------------------- options and statistics @@ -807,6 +810,7 @@ typedef struct mjModel_ { mjtNum* geom_margin; // geometric inflation for contact (ngeom x 1) mjtNum* geom_gap; // additional contact detection buffer (ngeom x 1) mjtNum* geom_surfacevel; // surface velocity in local frame: lin,ang (ngeom x 6) + mjtNum* geom_adhesion; // adhesive force of contacts (ngeom x 1) mjtNum* geom_fluid; // fluid interaction parameters (ngeom x mjNFLUID) mjtNum* geom_user; // user data (ngeom x nuser_geom) float* geom_rgba; // rgba when material is omitted (ngeom x 4) @@ -1050,6 +1054,7 @@ typedef struct mjModel_ { mjtNum* pair_solimp; // solver impedance: contact (npair x mjNIMP) mjtNum* pair_margin; // geometric inflation for contact (npair x 1) mjtNum* pair_gap; // additional contact detection buffer (npair x 1) + mjtNum* pair_adhesion; // adhesive force of contacts (npair x 1) mjtNum* pair_friction; // tangent1, 2, spin, roll1, 2 (npair x 5) // excluded body pairs for collision detection @@ -1902,6 +1907,7 @@ typedef struct mjsGeom_ { // geom specification double margin; // margin for contact detection double gap; // additional contact detection buffer double surfacevel[6]; // surface velocity in local frame: linear, angular + double adhesion; // adhesive force of contacts // inertia inference double mass; // used to compute density @@ -2164,6 +2170,7 @@ typedef struct mjsPair_ { // pair specification mjtNum solimp[mjNIMP]; // solver impedance double margin; // margin for contact detection double gap; // additional contact detection buffer + double adhesion; // adhesive force of contacts double friction[5]; // full contact friction mjString* info; // message appended to errors } mjsPair; diff --git a/include/mujoco/mjdata.h b/include/mujoco/mjdata.h index 621b1129..c2fddb81 100644 --- a/include/mujoco/mjdata.h +++ b/include/mujoco/mjdata.h @@ -46,6 +46,7 @@ typedef struct mjContact_ { // result of collision detection functions mjtNum solref[mjNREF]; // constraint solver reference, normal direction mjtNum solreffriction[mjNREF]; // constraint solver reference, friction directions mjtNum solimp[mjNIMP]; // constraint solver impedance + mjtNum adhesion; // adhesive force along the contact normal // internal storage used by solver mjtNum mu; // friction of regularized cone, set by mj_makeConstraint @@ -287,6 +288,7 @@ typedef struct mjData_ { mjtNum* qfrc_damper; // passive damper force (nv x 1) mjtNum* qfrc_gravcomp; // passive gravity compensation force (nv x 1) mjtNum* qfrc_fluid; // passive fluid force (nv x 1) + mjtNum* qfrc_adhesion; // passive contact adhesion force (nv x 1) mjtNum* qfrc_passive; // total passive force (nv x 1) // computed by mj_sensorVel/mj_subtreeVel if needed diff --git a/include/mujoco/mjmodel.h b/include/mujoco/mjmodel.h index 52adfc8e..49e1d30b 100644 --- a/include/mujoco/mjmodel.h +++ b/include/mujoco/mjmodel.h @@ -350,6 +350,7 @@ typedef struct mjModel_ { mjtBool flg_gravcomp; // whether any body has nonzero gravcomp mjtBool flg_surfacevel; // whether any geom has nonzero surfacevel + mjtBool flg_adhesion; // whether any geom or pair has nonzero adhesion // ------------------------------- options and statistics @@ -478,6 +479,7 @@ typedef struct mjModel_ { mjtNum* geom_margin; // geometric inflation for contact (ngeom x 1) mjtNum* geom_gap; // additional contact detection buffer (ngeom x 1) mjtNum* geom_surfacevel; // surface velocity in local frame: lin,ang (ngeom x 6) + mjtNum* geom_adhesion; // adhesive force of contacts (ngeom x 1) mjtNum* geom_fluid; // fluid interaction parameters (ngeom x mjNFLUID) mjtNum* geom_user; // user data (ngeom x nuser_geom) float* geom_rgba; // rgba when material is omitted (ngeom x 4) @@ -721,6 +723,7 @@ typedef struct mjModel_ { mjtNum* pair_solimp; // solver impedance: contact (npair x mjNIMP) mjtNum* pair_margin; // geometric inflation for contact (npair x 1) mjtNum* pair_gap; // additional contact detection buffer (npair x 1) + mjtNum* pair_adhesion; // adhesive force of contacts (npair x 1) mjtNum* pair_friction; // tangent1, 2, spin, roll1, 2 (npair x 5) // excluded body pairs for collision detection diff --git a/include/mujoco/mjspec.h b/include/mujoco/mjspec.h index b2d74e68..77e71a22 100644 --- a/include/mujoco/mjspec.h +++ b/include/mujoco/mjspec.h @@ -359,6 +359,7 @@ typedef struct mjsGeom_ { // geom specification double margin; // margin for contact detection double gap; // additional contact detection buffer double surfacevel[6]; // surface velocity in local frame: linear, angular + double adhesion; // adhesive force of contacts // inertia inference double mass; // used to compute density @@ -642,6 +643,7 @@ typedef struct mjsPair_ { // pair specification mjtNum solimp[mjNIMP]; // solver impedance double margin; // margin for contact detection double gap; // additional contact detection buffer + double adhesion; // adhesive force of contacts double friction[5]; // full contact friction mjString* info; // message appended to errors } mjsPair; diff --git a/include/mujoco/mjspecmacro.h b/include/mujoco/mjspecmacro.h index 45aa974d..c2f4b600 100644 --- a/include/mujoco/mjspecmacro.h +++ b/include/mujoco/mjspecmacro.h @@ -183,6 +183,7 @@ X ( double, margin, 1 ) \ X ( double, gap, 1 ) \ XVEC( double, surfacevel, 6 ) \ + X ( double, adhesion, 1 ) \ X ( double, mass, 1 ) \ X ( double, density, 1 ) \ X ( mjtGeomInertia, typeinertia, 1 ) \ @@ -421,6 +422,7 @@ XVEC( mjtNum, solimp, mjNIMP ) \ X ( double, margin, 1 ) \ X ( double, gap, 1 ) \ + X ( double, adhesion, 1 ) \ XVEC( double, friction, 5 ) \ X ( mjString*, info, 1 ) diff --git a/include/mujoco/mjxmacro.h b/include/mujoco/mjxmacro.h index 94ed8884..8f36c166 100644 --- a/include/mujoco/mjxmacro.h +++ b/include/mujoco/mjxmacro.h @@ -386,6 +386,7 @@ X ( mjtNum, geom_margin, ngeom, 1 ) \ X ( mjtNum, geom_gap, ngeom, 1 ) \ X ( mjtNum, geom_surfacevel, ngeom, 6 ) \ + X ( mjtNum, geom_adhesion, ngeom, 1 ) \ XNV ( mjtNum, geom_fluid, ngeom, mjNFLUID ) \ X ( mjtNum, geom_user, ngeom, MJ_M(nuser_geom) ) \ X ( float, geom_rgba, ngeom, 4 ) @@ -627,6 +628,7 @@ X ( mjtNum, pair_solimp, npair, mjNIMP ) \ X ( mjtNum, pair_margin, npair, 1 ) \ X ( mjtNum, pair_gap, npair, 1 ) \ + X ( mjtNum, pair_adhesion, npair, 1 ) \ X ( mjtNum, pair_friction, npair, 5 ) #define MJMODEL_POINTERS_EXCLUDE \ @@ -911,6 +913,7 @@ X ( mjtNum, qfrc_damper, nv, 1 ) \ X ( mjtNum, qfrc_gravcomp, nv, 1 ) \ X ( mjtNum, qfrc_fluid, nv, 1 ) \ + X ( mjtNum, qfrc_adhesion, nv, 1 ) \ X ( mjtNum, qfrc_passive, nv, 1 ) \ X ( mjtNum, subtree_linvel, nbody, 3 ) \ X ( mjtNum, subtree_angmom, nbody, 3 ) \ diff --git a/model/adhesion/fridge_door.xml b/model/adhesion/fridge_door.xml new file mode 100644 index 00000000..004f7e32 --- /dev/null +++ b/model/adhesion/fridge_door.xml @@ -0,0 +1,54 @@ + + + + diff --git a/model/adhesion/sand_castle.xml b/model/adhesion/sand_castle.xml new file mode 100644 index 00000000..43fe99d1 --- /dev/null +++ b/model/adhesion/sand_castle.xml @@ -0,0 +1,115 @@ + + + + diff --git a/python/mujoco/introspect/structs.py b/python/mujoco/introspect/structs.py index 68dfd3bf..f10405c7 100644 --- a/python/mujoco/introspect/structs.py +++ b/python/mujoco/introspect/structs.py @@ -1433,6 +1433,11 @@ STRUCTS: Mapping[str, StructDecl] = dict([ type=ValueType(name='mjtBool'), doc='whether any geom has nonzero surfacevel', ), + StructFieldDecl( + name='flg_adhesion', + type=ValueType(name='mjtBool'), + doc='whether any geom or pair has nonzero adhesion', + ), StructFieldDecl( name='opt', type=ValueType(name='mjOption'), @@ -2263,6 +2268,14 @@ STRUCTS: Mapping[str, StructDecl] = dict([ doc='surface velocity in local frame: lin,ang', array_extent=('ngeom', 6), ), + StructFieldDecl( + name='geom_adhesion', + type=PointerType( + inner_type=ValueType(name='mjtNum'), + ), + doc='adhesive force of contacts', + array_extent=('ngeom',), + ), StructFieldDecl( name='geom_fluid', type=PointerType( @@ -4031,6 +4044,14 @@ STRUCTS: Mapping[str, StructDecl] = dict([ doc='additional contact detection buffer', array_extent=('npair',), ), + StructFieldDecl( + name='pair_adhesion', + type=PointerType( + inner_type=ValueType(name='mjtNum'), + ), + doc='adhesive force of contacts', + array_extent=('npair',), + ), StructFieldDecl( name='pair_friction', type=PointerType( @@ -5409,6 +5430,11 @@ STRUCTS: Mapping[str, StructDecl] = dict([ ), doc='constraint solver impedance', ), + StructFieldDecl( + name='adhesion', + type=ValueType(name='mjtNum'), + doc='adhesive force along the contact normal', + ), StructFieldDecl( name='mu', type=ValueType(name='mjtNum'), @@ -6415,6 +6441,14 @@ STRUCTS: Mapping[str, StructDecl] = dict([ doc='passive fluid force', array_extent=('nv',), ), + StructFieldDecl( + name='qfrc_adhesion', + type=PointerType( + inner_type=ValueType(name='mjtNum'), + ), + doc='passive contact adhesion force', + array_extent=('nv',), + ), StructFieldDecl( name='qfrc_passive', type=PointerType( @@ -7983,6 +8017,11 @@ STRUCTS: Mapping[str, StructDecl] = dict([ ), doc='surface velocity in local frame: linear, angular', ), + StructFieldDecl( + name='adhesion', + type=ValueType(name='double'), + doc='adhesive force of contacts', + ), StructFieldDecl( name='mass', type=ValueType(name='double'), @@ -9256,6 +9295,11 @@ STRUCTS: Mapping[str, StructDecl] = dict([ type=ValueType(name='double'), doc='additional contact detection buffer', ), + StructFieldDecl( + name='adhesion', + type=ValueType(name='double'), + doc='adhesive force of contacts', + ), StructFieldDecl( name='friction', type=ArrayType( diff --git a/python/mujoco/structs.cc b/python/mujoco/structs.cc index 5687c518..bf0f249c 100644 --- a/python/mujoco/structs.cc +++ b/python/mujoco/structs.cc @@ -365,6 +365,13 @@ This is useful for example when the MJB is not available as a file on disk.)")); m.get()->flg_surfacevel = val; }); + mjModel.def_property( + "flg_adhesion", + [](const MjModelWrapper& m) { return m.get()->flg_adhesion; }, + [](MjModelWrapper& m, bool val) { + m.get()->flg_adhesion = val; + }); + #define X(var) \ mjModel.def_property_readonly( \ #var, [](const MjModelWrapper& m) { return m.get()->var; }); @@ -770,6 +777,7 @@ This is useful for example when the MJB is not available as a file on disk.)")); }) X(dist); X(includemargin); + X(adhesion); X(mu); X(dim); X(geom1); @@ -821,6 +829,7 @@ This is useful for example when the MJB is not available as a file on disk.)")); XN(mjtNum, solref); XN(mjtNum, solreffriction); XN(mjtNum, solimp); + X(mjtNum, adhesion); X(mjtNum, mu); XN(mjtNum, H); X(int, dim); diff --git a/src/engine/engine_collision_driver.c b/src/engine/engine_collision_driver.c index 291b822e..295c0aaa 100644 --- a/src/engine/engine_collision_driver.c +++ b/src/engine/engine_collision_driver.c @@ -1724,10 +1724,10 @@ int mj_broadphase(const mjModel* m, mjData* d, int* bfpair, int maxpair) { //----------------------------- narrow-phase collision detection ----------------------------------- -// compute contact condim, solref, solimp, friction +// compute contact condim, solref, solimp, friction, adhesion static void mj_contactParam(const mjModel* m, int* condim, mjtNum* solref, mjtNum* solimp, mjtNum* friction, - int g1, int g2, int f1, int f2) { + mjtNum* adhesion, int g1, int g2, int f1, int f2) { mjtNum fri[3]; // get parameters from geom1 or flex1 @@ -1737,6 +1737,7 @@ static void mj_contactParam(const mjModel* m, int* condim, const mjtNum* solref1 = (f1 < 0) ? m->geom_solref+g1*mjNREF : m->flex_solref+f1*mjNREF; const mjtNum* solimp1 = (f1 < 0) ? m->geom_solimp+g1*mjNIMP : m->flex_solimp+f1*mjNIMP; const mjtNum* friction1 = (f1 < 0) ? m->geom_friction+g1*3 : m->flex_friction+f1*3; + mjtNum adhesion1 = (f1 < 0) ? m->geom_adhesion[g1] : 0; // get parameters from geom2 or flex2 int priority2 = (f2 < 0) ? m->geom_priority[g2] : m->flex_priority[f2]; @@ -1745,6 +1746,10 @@ static void mj_contactParam(const mjModel* m, int* condim, const mjtNum* solref2 = (f2 < 0) ? m->geom_solref+g2*mjNREF : m->flex_solref+f2*mjNREF; const mjtNum* solimp2 = (f2 < 0) ? m->geom_solimp+g2*mjNIMP : m->flex_solimp+f2*mjNIMP; const mjtNum* friction2 = (f2 < 0) ? m->geom_friction+g2*3 : m->flex_friction+f2*3; + mjtNum adhesion2 = (f2 < 0) ? m->geom_adhesion[g2] : 0; + + // adhesion: each surface contributes its own attraction + *adhesion = adhesion1 + adhesion2; // different priority: copy from item with higher priority if (priority1 > priority2) { @@ -1752,12 +1757,14 @@ static void mj_contactParam(const mjModel* m, int* condim, mju_copy(solref, solref1, mjNREF); mju_copy(solimp, solimp1, mjNIMP); mju_copy(fri, friction1, 3); + *adhesion = adhesion1; } else if (priority1 < priority2) { *condim = condim2; mju_copy(solref, solref2, mjNREF); mju_copy(solimp, solimp2, mjNIMP); mju_copy(fri, friction2, 3); + *adhesion = adhesion2; } // same priority @@ -1820,17 +1827,27 @@ static void mj_contactParam(const mjModel* m, int* condim, static void mj_setContact(const mjModel* m, mjContact* con, int condim, mjtNum includemargin, const mjtNum* solref, const mjtNum* solreffriction, - const mjtNum* solimp, const mjtNum* friction) { + const mjtNum* solimp, const mjtNum* friction, + mjtNum adhesion) { // set parameters con->dim = condim; con->includemargin = includemargin; + con->adhesion = adhesion; mj_assignRef(m, con->solref, solref); mj_assignRef(m, con->solreffriction, solreffriction); mj_assignImp(m, con->solimp, solimp); mj_assignFriction(m, con->friction, friction); - // exclude in gap - con->exclude = (con->dist >= includemargin); + // exclude in gap; adhesive contacts remain active in the gap, producing + // rows with positive violation whose reference acceleration pulls (tether) + con->exclude = (con->dist >= includemargin) && !adhesion; + + // in-gap adhesive contacts have no surface contact and therefore no + // friction: reduce to normal-only rows (with tangential rows present, fast + // slip would recruit normal force and brake tangential motion at a distance) + if (adhesion && con->dist >= includemargin) { + con->dim = 1; + } // complete frame mju_makeFrame(con->frame); @@ -2010,7 +2027,7 @@ static void mj_narrowphase(const mjModel* m, mjData* d, const mjcPair* buffer, i continue; int condim; - mjtNum friction[5], solref[mjNREF], solimp[mjNIMP]; + mjtNum friction[5], solref[mjNREF], solimp[mjNIMP], adhesion; mjtNum solreffriction[mjNREF] = {0}; int g1 = pairbuffer[i].geom_geom.g1; int g2 = pairbuffer[i].geom_geom.g2; @@ -2021,11 +2038,12 @@ static void mj_narrowphase(const mjModel* m, mjData* d, const mjcPair* buffer, i mju_copy(solref, m->pair_solref+mjNREF*ipair, mjNREF); mju_copy(solimp, m->pair_solimp+mjNIMP*ipair, mjNIMP); mju_copy(friction, m->pair_friction+5*ipair, 5); + adhesion = m->pair_adhesion[ipair]; if (m->pair_solreffriction[mjNREF*ipair] || m->pair_solreffriction[mjNREF*ipair + 1]) { mju_copy(solreffriction, m->pair_solreffriction+mjNREF*ipair, mjNREF); } } else { - mj_contactParam(m, &condim, solref, solimp, friction, g1, g2, -1, -1); + mj_contactParam(m, &condim, solref, solimp, friction, &adhesion, g1, g2, -1, -1); } mjPreContact* bc = arg.conbuffer + pairbuffer[i].conpos; @@ -2044,7 +2062,7 @@ static void mj_narrowphase(const mjModel* m, mjData* d, const mjcPair* buffer, i c->elem[1] = -1; c->vert[0] = -1; c->vert[1] = -1; - mj_setContact(m, c, condim, margin, solref, solreffriction, solimp, friction); + mj_setContact(m, c, condim, margin, solref, solreffriction, solimp, friction, adhesion); } conpos += ncon; } @@ -2065,9 +2083,9 @@ static void mj_collidePlaneFlex(const mjModel* m, mjData* d, int g, int f) { int condim; int flex_vertnum = m->flex_vertnum[f]; mjtNum gap = m->geom_gap[g] + m->flex_gap[f]; - mjtNum solref[mjNREF], solimp[mjNIMP], friction[5]; + mjtNum solref[mjNREF], solimp[mjNIMP], friction[5], adhesion; mjtNum solreffriction[mjNREF] = {0}; - mj_contactParam(m, &condim, solref, solimp, friction, g, -1, -1, f); + mj_contactParam(m, &condim, solref, solimp, friction, &adhesion, g, -1, -1, f); // collide all flex vertices with plane for (int i=0; i < flex_vertnum; i++) { @@ -2099,7 +2117,7 @@ static void mj_collidePlaneFlex(const mjModel* m, mjData* d, int g, int f) { con.vert[1] = i; // set remaining contact parameters - mj_setContact(m, &con, condim, margin, solref, solreffriction, solimp, friction); + mj_setContact(m, &con, condim, margin, solref, solreffriction, solimp, friction, adhesion); // add to mjData, abort if too many contacts if (mj_addContact(m, d, &con)) { @@ -2120,9 +2138,9 @@ static void mj_collideSdfFlex(const mjModel* m, mjData* d, int g, int f) { mjtNum margin = mj_assignMargin(m, m->geom_margin[g] + m->flex_margin[f]); mjtNum gap = m->geom_gap[g] + m->flex_gap[f]; int condim; - mjtNum solref[mjNREF], solimp[mjNIMP], friction[5]; + mjtNum solref[mjNREF], solimp[mjNIMP], friction[5], adhesion; mjtNum solreffriction[mjNREF] = {0}; - mj_contactParam(m, &condim, solref, solimp, friction, g, -1, -1, f); + mj_contactParam(m, &condim, solref, solimp, friction, &adhesion, g, -1, -1, f); // allocate temporary contact array on stack (zero-initialized) mj_markStack(d); @@ -2153,7 +2171,7 @@ static void mj_collideSdfFlex(const mjModel* m, mjData* d, int g, int f) { con.vert[1] = -1; // set remaining contact parameters - mj_setContact(m, &con, condim, margin, solref, solreffriction, solimp, friction); + mj_setContact(m, &con, condim, margin, solref, solreffriction, solimp, friction, adhesion); // add to mjData, abort if too many contacts if (mj_addContact(m, d, &con)) { @@ -2221,9 +2239,9 @@ static void mj_collideFlexInternal(const mjModel* m, mjData* d, int f) { int condim; int flex_elemnum = m->flex_elemnum[f]; mjtNum radius = m->flex_radius[f]; - mjtNum solref[mjNREF], solimp[mjNIMP], friction[5]; + mjtNum solref[mjNREF], solimp[mjNIMP], friction[5], adhesion; mjtNum solreffriction[mjNREF] = {0}; - mj_contactParam(m, &condim, solref, solimp, friction, -1, -1, f, f); + mj_contactParam(m, &condim, solref, solimp, friction, &adhesion, -1, -1, f, f); condim = 1; // process all elements @@ -2240,7 +2258,7 @@ static void mj_collideFlexInternal(const mjModel* m, mjData* d, int f) { mju_copy3(con.pos, precon.pos); mju_copy3(con.frame, precon.normal); mju_copy3(con.frame + 3, precon.tangent); - mj_setContact(m, &con, condim, 0, solref, solreffriction, solimp, friction); + mj_setContact(m, &con, condim, 0, solref, solreffriction, solimp, friction, adhesion); if (mj_addContact(m, d, &con)) return; } @@ -2251,7 +2269,7 @@ static void mj_collideFlexInternal(const mjModel* m, mjData* d, int f) { mju_copy3(con.pos, precon.pos); mju_copy3(con.frame, precon.normal); mju_copy3(con.frame + 3, precon.tangent); - mj_setContact(m, &con, condim, 0, solref, solreffriction, solimp, friction); + mj_setContact(m, &con, condim, 0, solref, solreffriction, solimp, friction, adhesion); if (mj_addContact(m, d, &con)) return; } @@ -2262,7 +2280,7 @@ static void mj_collideFlexInternal(const mjModel* m, mjData* d, int f) { mju_copy3(con.pos, precon.pos); mju_copy3(con.frame, precon.normal); mju_copy3(con.frame + 3, precon.tangent); - mj_setContact(m, &con, condim, 0, solref, solreffriction, solimp, friction); + mj_setContact(m, &con, condim, 0, solref, solreffriction, solimp, friction, adhesion); if (mj_addContact(m, d, &con)) return; } @@ -2273,7 +2291,7 @@ static void mj_collideFlexInternal(const mjModel* m, mjData* d, int f) { mju_copy3(con.pos, precon.pos); mju_copy3(con.frame, precon.normal); mju_copy3(con.frame + 3, precon.tangent); - mj_setContact(m, &con, condim, 0, solref, solreffriction, solimp, friction); + mj_setContact(m, &con, condim, 0, solref, solreffriction, solimp, friction, adhesion); if (mj_addContact(m, d, &con)) return; } } @@ -2439,9 +2457,9 @@ void mj_collideGeomElem(const mjModel* m, mjData* d, int g, int f, int e) { // get contact parameters int condim; - mjtNum friction[5], solref[mjNREF], solimp[mjNIMP]; + mjtNum friction[5], solref[mjNREF], solimp[mjNIMP], adhesion; mjtNum solreffriction[mjNREF] = {0}; - mj_contactParam(m, &condim, solref, solimp, friction, g, -1, -1, f); + mj_contactParam(m, &condim, solref, solimp, friction, &adhesion, g, -1, -1, f); // allocate mjContact[num] on the arena mjContact* con = @@ -2475,7 +2493,7 @@ void mj_collideGeomElem(const mjModel* m, mjData* d, int g, int f, int e) { } // set remaining contact parameters - mj_setContact(m, con + i, condim, margin, solref, solreffriction, solimp, friction); + mj_setContact(m, con + i, condim, margin, solref, solreffriction, solimp, friction, adhesion); } // add to ncon @@ -2548,9 +2566,9 @@ void mj_collideElems(const mjModel* m, mjData* d, int f1, int e1, int f2, int e2 // get contact parameters int condim; - mjtNum friction[5], solref[mjNREF], solimp[mjNIMP]; + mjtNum friction[5], solref[mjNREF], solimp[mjNIMP], adhesion; mjtNum solreffriction[mjNREF] = {0}; - mj_contactParam(m, &condim, solref, solimp, friction, -1, -1, f1, f2); + mj_contactParam(m, &condim, solref, solimp, friction, &adhesion, -1, -1, f1, f2); mjContact* con = (mjContact*) mj_arenaAllocByte(d, sizeof(mjContact) * num, _Alignof(mjContact)); if (!con) { @@ -2577,7 +2595,7 @@ void mj_collideElems(const mjModel* m, mjData* d, int f1, int e1, int f2, int e2 mju_copy3(con[i].frame + 3, precon[i].tangent); // set remaining contact parameters - mj_setContact(m, con + i, condim, margin, solref, solreffriction, solimp, friction); + mj_setContact(m, con + i, condim, margin, solref, solreffriction, solimp, friction, adhesion); } // add to ncon @@ -2638,9 +2656,9 @@ void mj_collideElemVert(const mjModel* m, mjData* d, int f, int e, int v) { // get contact parameters int condim; - mjtNum friction[5], solref[mjNREF], solimp[mjNIMP]; + mjtNum friction[5], solref[mjNREF], solimp[mjNIMP], adhesion; mjtNum solreffriction[mjNREF] = {0}; - mj_contactParam(m, &condim, solref, solimp, friction, -1, -1, f, f); + mj_contactParam(m, &condim, solref, solimp, friction, &adhesion, -1, -1, f, f); // allocate mjContact[num] on the arena @@ -2671,7 +2689,7 @@ void mj_collideElemVert(const mjModel* m, mjData* d, int f, int e, int v) { mju_copy3(con[i].frame + 3, precon[i].tangent); // set remaining contact parameters - mj_setContact(m, con + i, condim, 0, solref, solreffriction, solimp, friction); + mj_setContact(m, con + i, condim, 0, solref, solreffriction, solimp, friction, adhesion); } // add to ncon diff --git a/src/engine/engine_core_constraint.c b/src/engine/engine_core_constraint.c index 69755042..f0e002e9 100644 --- a/src/engine/engine_core_constraint.c +++ b/src/engine/engine_core_constraint.c @@ -3199,6 +3199,38 @@ static void mj_addSurfaceVel(const mjModel* m, mjData* d) { +// bias aref of adhesive contact rows: makes the compression branch of the net +// force-penetration curve independent of adhesion (exact translated-cone semantics) +static void mj_adhesionRef(const mjModel* m, mjData* d) { + // no adhesion on any geom or pair: quick return + if (!m->flg_adhesion) { + return; + } + + int ispyramid = mj_isPyramidal(m), ncon = d->ncon; + + for (int i=0; i < ncon; i++) { + const mjContact* con = d->contact + i; + if (!con->adhesion || con->efc_address < 0) { + continue; + } + + int adr = con->efc_address, dim = con->dim; + if (dim == 1 || !ispyramid) { + // normal row + d->efc_aref[adr] += d->efc_R[adr] * con->adhesion; + } else { + // pyramid rows: the normal decomposes equally over the 2*(dim-1) edges + mjtNum edge = con->adhesion / (2*(dim-1)); + for (int j=0; j < 2*(dim-1); j++) { + d->efc_aref[adr+j] += d->efc_R[adr+j] * edge; + } + } + } +} + + + // compute efc_vel, efc_aref void mj_referenceConstraint(const mjModel* m, mjData* d) { int nefc = d->nefc; @@ -3216,6 +3248,9 @@ void mj_referenceConstraint(const mjModel* m, mjData* d) { -KBIP[4*i]*KBIP[4*i+2]*(d->efc_pos[i]-d->efc_margin[i]); } + // bias adhesive contact rows + mj_adhesionRef(m, d); + // subtract Jdot*v correction for connect/weld equality constraints if (d->ne > 0) { mj_Jdotv(m, d, d->efc_aref); diff --git a/src/engine/engine_core_util.c b/src/engine/engine_core_util.c index 5853215d..dc1031b8 100644 --- a/src/engine/engine_core_util.c +++ b/src/engine/engine_core_util.c @@ -1077,6 +1077,9 @@ void mj_contactForce(const mjModel* m, const mjData* d, int id, mjtNum result[6] } else { mju_copy(result, d->efc_force + con->efc_address, con->dim); } + + // report the net interface force: the solver's cone force minus the adhesive pull + result[0] -= con->adhesion; } } diff --git a/src/engine/engine_passive.c b/src/engine/engine_passive.c index ef3cd8cd..bd6680bf 100644 --- a/src/engine/engine_passive.c +++ b/src/engine/engine_passive.c @@ -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++) { diff --git a/src/engine/engine_passive.h b/src/engine/engine_passive.h index 096cfefb..eca7c927 100644 --- a/src/engine/engine_passive.h +++ b/src/engine/engine_passive.h @@ -28,6 +28,9 @@ extern "C" { // all passive forces MJAPI void mj_passive(const mjModel* m, mjData* d); +// adhesion forces: constant attraction along the normals of adhesive contacts +int mj_adhesion(const mjModel* m, mjData* d); + diff --git a/src/engine/engine_setconst.c b/src/engine/engine_setconst.c index 13ea8501..11d4e9b0 100644 --- a/src/engine/engine_setconst.c +++ b/src/engine/engine_setconst.c @@ -223,6 +223,21 @@ static void setFixed(mjModel* m, mjData* d) { } m->flg_surfacevel = flg_surfacevel; + // compute flg_adhesion: whether any geom or pair has nonzero adhesion + mjtBool flg_adhesion = 0; + for (int i=0; i < m->ngeom; i++) { + if (m->geom_adhesion[i]) { + flg_adhesion = 1; + break; + } + } + for (int i=0; i < m->npair && !flg_adhesion; i++) { + if (m->pair_adhesion[i]) { + flg_adhesion = 1; + } + } + m->flg_adhesion = flg_adhesion; + // set jnt_actuatorid and tendon_actuatorid mju_fillInt(m->jnt_actuatorid, -1, m->njnt); mju_fillInt(m->tendon_actuatorid, -1, m->ntendon); diff --git a/src/user/user_model.cc b/src/user/user_model.cc index 43384de4..de5d81be 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -3063,6 +3063,7 @@ void mjCModel::CopyTree(mjModel* m) { m->geom_margin[gid] = (mjtNum)pg->margin; m->geom_gap[gid] = (mjtNum)pg->gap; mjuu_copyvec(m->geom_surfacevel+6*gid, pg->surfacevel, 6); + m->geom_adhesion[gid] = (mjtNum)pg->adhesion; mjuu_copyvec(m->geom_fluid+mjNFLUID*gid, pg->fluid, mjNFLUID); mjuu_copyvec(m->geom_user+nuser_geom*gid, pg->get_userdata().data(), nuser_geom); mjuu_copyvec(m->geom_rgba+4*gid, pg->rgba, 4); @@ -3927,6 +3928,7 @@ void mjCModel::CopyObjects(mjModel* m) { mjuu_copyvec(m->pair_solimp+mjNIMP*i, pairs_[i]->solimp, mjNIMP); m->pair_margin[i] = (mjtNum)pairs_[i]->margin; m->pair_gap[i] = (mjtNum)pairs_[i]->gap; + m->pair_adhesion[i] = (mjtNum)pairs_[i]->adhesion; mjuu_copyvec(m->pair_friction+5*i, pairs_[i]->friction, 5); } @@ -5843,6 +5845,7 @@ bool mjCModel::CopyBack(const mjModel* m) { pg->margin = (double)m->geom_margin[i]; pg->gap = (double)m->geom_gap[i]; mjuu_copyvec(pg->surfacevel, m->geom_surfacevel+6*i, 6); + pg->adhesion = (double)m->geom_adhesion[i]; if (nuser_geom) { mjuu_copyvec(pg->userdata_.data(), m->geom_user + nuser_geom*i, nuser_geom); @@ -5926,6 +5929,7 @@ bool mjCModel::CopyBack(const mjModel* m) { mjuu_copyvec(pairs_[i]->solimp, m->pair_solimp+mjNIMP*i, mjNIMP); pairs_[i]->margin = (double)m->pair_margin[i]; pairs_[i]->gap = (double)m->pair_gap[i]; + pairs_[i]->adhesion = (double)m->pair_adhesion[i]; mjuu_copyvec(pairs_[i]->friction, m->pair_friction+5*i, 5); } diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index 0eba322e..c4ab9140 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -238,7 +238,8 @@ std::vector MJCF[nMJCF] = { {"geom", "?", "type", "pos", "quat", "contype", "conaffinity", "condim", "group", "priority", "size", "material", "friction", "mass", "density", "shellinertia", "solmix", "solref", "solimp", - "margin", "gap", "surfacevel", "fromto", "axisangle", "xyaxes", "zaxis", "euler", + "margin", "gap", "surfacevel", "adhesion", "fromto", "axisangle", "xyaxes", "zaxis", + "euler", "hfield", "mesh", "fitscale", "rgba", "fluidshape", "fluidcoef", "user"}, {"site", "?", "type", "group", "pos", "quat", "material", "size", "fromto", "axisangle", "xyaxes", "zaxis", "euler", "rgba", "user"}, @@ -249,7 +250,7 @@ std::vector MJCF[nMJCF] = { "directional", "type", "castshadow", "active", "attenuation", "cutoff", "exponent", "ambient", "diffuse", "specular", "mode"}, {"pair", "?", "condim", "friction", "solref", "solreffriction", "solimp", - "gap", "margin"}, + "gap", "margin", "adhesion"}, {"equality", "?", "active", "solref", "solimp"}, {"tendon", "?", "group", "limited", "range", "solreflimit", "solimplimit", "solreffriction", "solimpfriction", @@ -349,9 +350,9 @@ std::vector MJCF[nMJCF] = { {"geom", "*", "name", "class", "type", "contype", "conaffinity", "condim", "group", "priority", "size", "material", "friction", "mass", "density", "shellinertia", "solmix", "solref", "solimp", - "margin", "gap", "surfacevel", "fromto", "pos", "quat", "axisangle", "xyaxes", - "zaxis", "euler", "hfield", "mesh", "fitscale", "rgba", "fluidshape", "fluidcoef", - "user"}, + "margin", "gap", "surfacevel", "adhesion", "fromto", "pos", "quat", "axisangle", + "xyaxes", "zaxis", "euler", "hfield", "mesh", "fitscale", "rgba", "fluidshape", + "fluidcoef", "user"}, {"<"}, {"plugin", "*", "plugin", "instance"}, {"<"}, @@ -381,7 +382,8 @@ std::vector MJCF[nMJCF] = { {"skin", "?", "texcoord", "material", "group", "rgba", "inflate", "subgrid"}, {"geom", "?", "type", "contype", "conaffinity", "condim", "group", "priority", "size", "material", "rgba", "friction", "mass", - "density", "solmix", "solref", "solimp", "margin", "gap", "surfacevel"}, + "density", "solmix", "solref", "solimp", "margin", "gap", "surfacevel", + "adhesion"}, {"site", "?", "group", "size", "material", "rgba"}, {"plugin", "*", "plugin", "instance"}, {"<"}, @@ -427,7 +429,7 @@ std::vector MJCF[nMJCF] = { {"contact", "*"}, {"<"}, {"pair", "*", "name", "class", "geom1", "geom2", "condim", "friction", - "solref", "solreffriction", "solimp", "gap", "margin"}, + "solref", "solreffriction", "solimp", "gap", "margin", "adhesion"}, {"exclude", "*", "name", "body1", "body2"}, {">"}, @@ -1975,6 +1977,7 @@ void mjXReader::OneGeom(XMLElement* elem, mjsGeom* geom) { ReadAttr(elem, "margin", 1, &geom->margin, text); ReadAttr(elem, "gap", 1, &geom->gap, text); ReadAttr(elem, "surfacevel", 6, geom->surfacevel, text, false, false); + ReadAttr(elem, "adhesion", 1, &geom->adhesion, text); if (ReadAttrTxt(elem, "hfield", hfieldname)) { mjs_setString(geom->hfieldname, hfieldname.c_str()); } @@ -2197,6 +2200,7 @@ void mjXReader::OnePair(XMLElement* elem, mjsPair* pair) { ReadAttr(elem, "solimp", mjNIMP, pair->solimp, text, false, false); ReadAttr(elem, "margin", 1, &pair->margin, text); ReadAttr(elem, "gap", 1, &pair->gap, text); + ReadAttr(elem, "adhesion", 1, &pair->adhesion, text); ReadAttr(elem, "friction", 5, pair->friction, text, false, false); // write error info @@ -2785,6 +2789,7 @@ void mjXReader::OneComposite(XMLElement* elem, mjsBody* body, mjsFrame* frame, c ReadAttr(egeom, "margin", 1, &dgeom.margin, text); ReadAttr(egeom, "gap", 1, &dgeom.gap, text); ReadAttr(egeom, "surfacevel", 6, dgeom.surfacevel, text, false, false); + ReadAttr(egeom, "adhesion", 1, &dgeom.adhesion, text); if (ReadAttrTxt(egeom, "material", material)) { mjs_setString(dgeom.material, material.c_str()); } diff --git a/src/xml/xml_native_writer.cc b/src/xml/xml_native_writer.cc index 97f3654c..0848ae77 100644 --- a/src/xml/xml_native_writer.cc +++ b/src/xml/xml_native_writer.cc @@ -498,6 +498,7 @@ void mjXWriter::OneGeom(XMLElement* elem, const mjCGeom* geom, mjCDef* def, stri WriteAttr(elem, "margin", 1, &geom->margin, &def->Geom().margin); WriteAttr(elem, "gap", 1, &geom->gap, &def->Geom().gap); WriteAttr(elem, "surfacevel", 6, geom->surfacevel, def->Geom().surfacevel, true); + WriteAttr(elem, "adhesion", 1, &geom->adhesion, &def->Geom().adhesion); WriteAttrKey(elem, "fluidshape", fluid_map, 2, geom->fluid_ellipsoid, def->Geom().fluid_ellipsoid); WriteAttr(elem, "fluidcoef", 5, geom->fluid_coefs, def->Geom().fluid_coefs); @@ -672,6 +673,7 @@ void mjXWriter::OnePair(XMLElement* elem, const mjCPair* pair, mjCDef* def) { WriteAttrInt(elem, "condim", pair->condim, def->Pair().spec.condim); WriteAttr(elem, "margin", 1, &pair->margin, &def->Pair().spec.margin); WriteAttr(elem, "gap", 1, &pair->gap, &def->Pair().spec.gap); + WriteAttr(elem, "adhesion", 1, &pair->adhesion, &def->Pair().spec.adhesion); WriteAttr(elem, "solref", mjNREF, pair->solref, def->Pair().spec.solref, true); WriteAttr(elem, "solreffriction", mjNREF, pair->solreffriction, def->Pair().spec.solreffriction, true); diff --git a/test/engine/engine_core_constraint_test.cc b/test/engine/engine_core_constraint_test.cc index ffe7f31f..f463ac54 100644 --- a/test/engine/engine_core_constraint_test.cc +++ b/test/engine/engine_core_constraint_test.cc @@ -1345,5 +1345,184 @@ TEST_F(CoreConstraintTest, ShellModeContactJacobian) { << "Interior nodes should not receive contact force"; } + +// ------------------------------- adhesion ------------------------------------ + +static const char* kAdhereCeiling = R"( + + + + + + + + + +)"; + +// a box sticks to an adhesive ceiling, holds up to ncon*delta, then detaches +TEST_F(CoreConstraintTest, AdhesionPulloff) { + char error[1024]; + MjModelPtr model = LoadModelFromString(kAdhereCeiling, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + MjDataPtr data = MakeData(model); + + // hangs at rest + while (data->time < 2) { + mj_step(model.get(), data.get()); + } + ASSERT_EQ(data->ncon, 4); + EXPECT_GT(data->qpos[2], 0.9); + + // holds a load below 4*delta - mg + data->xfrc_applied[6*1 + 2] = -60; + while (data->time < 4) { + mj_step(model.get(), data.get()); + } + EXPECT_GT(data->qpos[2], 0.9); + + // detaches above 4*delta + data->xfrc_applied[6*1 + 2] = -85; + while (data->time < 5) { + mj_step(model.get(), data.get()); + } + EXPECT_LT(data->qpos[2], 0.5); +} + +// under tension the box hangs at positive separation which grows with load +TEST_F(CoreConstraintTest, AdhesionTether) { + char error[1024]; + MjModelPtr model = LoadModelFromString(kAdhereCeiling, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + MjDataPtr data = MakeData(model); + + mjtNum dist[2]; + for (int k=0; k < 2; k++) { + data->xfrc_applied[6*1 + 2] = k ? -50 : 0; + mjtNum tend = data->time + 3; + while (data->time < tend) { + mj_step(model.get(), data.get()); + } + ASSERT_GT(data->ncon, 0); + dist[k] = data->contact[0].dist; + } + EXPECT_GT(dist[0], 0); // hanging in tension at positive separation + EXPECT_GT(dist[1], dist[0]); // separation grows with load +} + +// a box released inside the adhesion band is captured into steady contact +TEST_F(CoreConstraintTest, AdhesionCapture) { + std::string xml = kAdhereCeiling; + size_t pos = xml.find("0 0 .93"); + xml.replace(pos, 7, "0 0 .89"); + char error[1024]; + MjModelPtr model = LoadModelFromString(xml.c_str(), error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + MjDataPtr data = MakeData(model); + + while (data->time < 3) { + mj_step(model.get(), data.get()); + } + mjtNum zmin = 10, zmax = -10; + while (data->time < 4) { + mj_step(model.get(), data.get()); + zmin = mju_min(zmin, data->qpos[2]); + zmax = mju_max(zmax, data->qpos[2]); + } + EXPECT_GT(zmax, 0.928); // captured up to the ceiling + EXPECT_LT(zmax - zmin, 1e-4); // and steady +} + +// resting penetration is independent of adhesion (the R*delta correction) +TEST_F(CoreConstraintTest, AdhesionRestingPenetration) { + constexpr char xml[] = R"( + + + + + + + + + + )"; + char error[1024]; + mjtNum depth[2]; + for (int k=0; k < 2; k++) { + std::string xml2 = xml; + size_t pos = xml2.find("ADH"); + xml2.replace(pos, 3, k ? "50" : "0"); + MjModelPtr model = LoadModelFromString(xml2.c_str(), error, sizeof(error)); + ASSERT_THAT(model.get(), testing::NotNull()) << error; + MjDataPtr data = MakeData(model); + while (data->time < 3) { + mj_step(model.get(), data.get()); + } + ASSERT_GT(data->ncon, 0); + depth[k] = -data->contact[0].dist; + + // net reported force sums to the weight + if (k) { + mjtNum total = 0, f[6]; + for (int i=0; i < data->ncon; i++) { + mj_contactForce(model.get(), data.get(), i, f); + total += f[0]; + } + EXPECT_NEAR(total, 9.81, 1e-2); + } + } + EXPECT_NEAR(depth[0], depth[1], MjTol(1e-9, 1e-6)); +} + +// adhesion combines by sum, with pair override +TEST_F(CoreConstraintTest, AdhesionCombination) { + constexpr char xml[] = R"( + + + + + + + + + + + + + )"; + char error[1024]; + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), testing::NotNull()) << error; + MjDataPtr data = MakeData(model); + mj_forward(model.get(), data.get()); + ASSERT_GT(data->ncon, 0); + + // explicit pair overrides: 7, not 10+5 + EXPECT_EQ(data->contact[0].adhesion, 7); +} + +TEST_F(CoreConstraintTest, AdhesionPriority) { + constexpr char xml[] = R"( + + + + + + + + + + )"; + char error[1024]; + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), testing::NotNull()) << error; + MjDataPtr data = MakeData(model); + mj_forward(model.get(), data.get()); + ASSERT_GT(data->ncon, 0); + + // higher-priority geom wins: 10, not 10+5 + EXPECT_EQ(data->contact[0].adhesion, 10); +} + } // namespace } // namespace mujoco diff --git a/test/user/user_composite_test.cc b/test/user/user_composite_test.cc index 7944a48a..63bca1be 100644 --- a/test/user/user_composite_test.cc +++ b/test/user/user_composite_test.cc @@ -72,6 +72,26 @@ TEST_F(UserCompositeTest, GeomSurfacevel) { } } +TEST_F(UserCompositeTest, GeomAdhesion) { + static constexpr char xml[] = R"( + + + + + + + + )"; + std::array error; + MjModelPtr m = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(m.get(), NotNull()) << error.data(); + for (int g = 0; g < m->ngeom; g++) { + if (m->geom_type[g] == mjGEOM_CAPSULE) { + EXPECT_EQ(m->geom_adhesion[g], 2); + } + } +} + TEST_F(UserCompositeTest, InvalidShape) { static constexpr char xml[] = R"( diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index 2c206ec5..e982ac26 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -1118,6 +1118,7 @@ public unsafe struct mjModel_ { public UInt64 nbuffer; public byte flg_gravcomp; public byte flg_surfacevel; + public byte flg_adhesion; public mjOption_ opt; public mjVisual_ vis; public mjStatistic_ stat; @@ -1223,6 +1224,7 @@ public unsafe struct mjModel_ { public double* geom_margin; public double* geom_gap; public double* geom_surfacevel; + public double* geom_adhesion; public double* geom_fluid; public double* geom_user; public float* geom_rgba; @@ -1444,6 +1446,7 @@ public unsafe struct mjModel_ { public double* pair_solimp; public double* pair_margin; public double* pair_gap; + public double* pair_adhesion; public double* pair_friction; public int* exclude_signature; public int* eq_type; @@ -1624,6 +1627,7 @@ public unsafe struct mjContact_ { public fixed double solref[2]; public fixed double solreffriction[2]; public fixed double solimp[5]; + public double adhesion; public double mu; public fixed double H[36]; public int dim; @@ -5801,6 +5805,7 @@ public unsafe struct mjData_ { public double* qfrc_damper; public double* qfrc_gravcomp; public double* qfrc_fluid; + public double* qfrc_adhesion; public double* qfrc_passive; public double* subtree_linvel; public double* subtree_angmom; diff --git a/wasm/codegen/generated/bindings.cc b/wasm/codegen/generated/bindings.cc index 64e3954e..ff9f4419 100644 --- a/wasm/codegen/generated/bindings.cc +++ b/wasm/codegen/generated/bindings.cc @@ -4502,6 +4502,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) { .constructor<>() .function("copy", &MjContact::copy, take_ownership()) .property("H", &MjContact::H) + .property("adhesion", &MjContact::adhesion, &MjContact::set_adhesion, reference()) .property("dim", &MjContact::dim, &MjContact::set_dim, reference()) .property("dist", &MjContact::dist, &MjContact::set_dist, reference()) .property("efc_address", &MjContact::efc_address, &MjContact::set_efc_address, reference()) @@ -4685,6 +4686,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) { .property("qacc_smooth", &MjData::qacc_smooth) .property("qacc_warmstart", &MjData::qacc_warmstart) .property("qfrc_actuator", &MjData::qfrc_actuator) + .property("qfrc_adhesion", &MjData::qfrc_adhesion) .property("qfrc_applied", &MjData::qfrc_applied) .property("qfrc_bias", &MjData::qfrc_bias) .property("qfrc_constraint", &MjData::qfrc_constraint) @@ -4982,9 +4984,11 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) { .property("flexvert_J_colind", &MjModel::flexvert_J_colind) .property("flexvert_J_rowadr", &MjModel::flexvert_J_rowadr) .property("flexvert_J_rownnz", &MjModel::flexvert_J_rownnz) + .property("flg_adhesion", &MjModel::flg_adhesion, &MjModel::set_flg_adhesion, reference()) .property("flg_gravcomp", &MjModel::flg_gravcomp, &MjModel::set_flg_gravcomp, reference()) .property("flg_surfacevel", &MjModel::flg_surfacevel, &MjModel::set_flg_surfacevel, reference()) .property("geom_aabb", &MjModel::geom_aabb) + .property("geom_adhesion", &MjModel::geom_adhesion) .property("geom_bodyid", &MjModel::geom_bodyid) .property("geom_conaffinity", &MjModel::geom_conaffinity) .property("geom_condim", &MjModel::geom_condim) @@ -5240,6 +5244,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) { .property("oct_coeff", &MjModel::oct_coeff) .property("oct_depth", &MjModel::oct_depth) .property("opt", &MjModel::opt, reference()) + .property("pair_adhesion", &MjModel::pair_adhesion) .property("pair_dim", &MjModel::pair_dim) .property("pair_friction", &MjModel::pair_friction) .property("pair_gap", &MjModel::pair_gap) @@ -5752,6 +5757,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) { .property("pos", &MjsFrame::pos) .property("quat", &MjsFrame::quat); emscripten::class_("MjsGeom") + .property("adhesion", &MjsGeom::adhesion, &MjsGeom::set_adhesion, reference()) .property("alt", &MjsGeom::alt, reference()) .property("conaffinity", &MjsGeom::conaffinity, &MjsGeom::set_conaffinity, reference()) .property("condim", &MjsGeom::condim, &MjsGeom::set_condim, reference()) @@ -5895,6 +5901,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) { .property("xyaxes", &MjsOrientation::xyaxes) .property("zaxis", &MjsOrientation::zaxis); emscripten::class_("MjsPair") + .property("adhesion", &MjsPair::adhesion, &MjsPair::set_adhesion, reference()) .property("condim", &MjsPair::condim, &MjsPair::set_condim, reference()) .property("element", &MjsPair::element, reference()) .property("friction", &MjsPair::friction) diff --git a/wasm/codegen/generated/bindings.h b/wasm/codegen/generated/bindings.h index ba1c0ad4..8e5f2cff 100644 --- a/wasm/codegen/generated/bindings.h +++ b/wasm/codegen/generated/bindings.h @@ -377,6 +377,12 @@ struct MjContact { emscripten::val solimp() const { return emscripten::val(emscripten::typed_memory_view(5, ptr_->solimp)); } + mjtNum adhesion() const { + return ptr_->adhesion; + } + void set_adhesion(mjtNum value) { + ptr_->adhesion = value; + } mjtNum mu() const { return ptr_->mu; } @@ -3091,6 +3097,12 @@ struct MjsPair { void set_gap(double value) { ptr_->gap = value; } + double adhesion() const { + return ptr_->adhesion; + } + void set_adhesion(double value) { + ptr_->adhesion = value; + } emscripten::val friction() const { return emscripten::val(emscripten::typed_memory_view(5, ptr_->friction)); } @@ -4320,6 +4332,12 @@ struct MjModel { void set_flg_surfacevel(mjtBool value) { ptr_->flg_surfacevel = value; } + mjtBool flg_adhesion() const { + return ptr_->flg_adhesion; + } + void set_flg_adhesion(mjtBool value) { + ptr_->flg_adhesion = value; + } emscripten::val buffer() const { return emscripten::val(emscripten::typed_memory_view(ptr_->nbuffer, static_cast(ptr_->buffer))); } @@ -4626,6 +4644,9 @@ struct MjModel { emscripten::val geom_surfacevel() const { return emscripten::val(emscripten::typed_memory_view(ptr_->ngeom * 6, ptr_->geom_surfacevel)); } + emscripten::val geom_adhesion() const { + return emscripten::val(emscripten::typed_memory_view(ptr_->ngeom, ptr_->geom_adhesion)); + } emscripten::val geom_fluid() const { return emscripten::val(emscripten::typed_memory_view(ptr_->ngeom * mjNFLUID, ptr_->geom_fluid)); } @@ -5289,6 +5310,9 @@ struct MjModel { emscripten::val pair_gap() const { return emscripten::val(emscripten::typed_memory_view(ptr_->npair, ptr_->pair_gap)); } + emscripten::val pair_adhesion() const { + return emscripten::val(emscripten::typed_memory_view(ptr_->npair, ptr_->pair_adhesion)); + } emscripten::val pair_friction() const { return emscripten::val(emscripten::typed_memory_view(ptr_->npair * 5, ptr_->pair_friction)); } @@ -6290,6 +6314,12 @@ struct MjsGeom { emscripten::val surfacevel() const { return emscripten::val(emscripten::typed_memory_view(6, ptr_->surfacevel)); } + double adhesion() const { + return ptr_->adhesion; + } + void set_adhesion(double value) { + ptr_->adhesion = value; + } double mass() const { return ptr_->mass; } @@ -7055,6 +7085,9 @@ struct MjData { emscripten::val qfrc_fluid() const { return emscripten::val(emscripten::typed_memory_view(model->nv, ptr_->qfrc_fluid)); } + emscripten::val qfrc_adhesion() const { + return emscripten::val(emscripten::typed_memory_view(model->nv, ptr_->qfrc_adhesion)); + } emscripten::val qfrc_passive() const { return emscripten::val(emscripten::typed_memory_view(model->nv, ptr_->qfrc_passive)); }