From d82f5ce5a444856f4cd9cc3dac15d9d586b248d1 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Wed, 7 Jun 2023 09:39:28 -0700 Subject: [PATCH] Add `mjContact.solreffriction`, `mjModel.pair_solreffriction` and related implementation, allowing contact reference acceleration to be different for normal and frictional contact directions. This is required for e.g. elastic frictional collisions. PiperOrigin-RevId: 538513357 Change-Id: Id9eb910fe791cf898c712030cb2c0fc481389c91 --- doc/XMLreference.rst | 16 ++++++++++ doc/XMLschema.rst | 8 +++-- doc/changelog.rst | 7 ++++ doc/includes/references.h | 36 +++++++++++---------- include/mujoco/mjdata.h | 31 +++++++++--------- include/mujoco/mjmodel.h | 5 +-- include/mujoco/mjxmacro.h | 1 + introspect/structs.py | 21 ++++++++++-- python/mujoco/structs.cc | 4 +++ python/mujoco/structs.h | 1 + src/engine/engine_collision_driver.c | 9 +++++- src/engine/engine_core_constraint.c | 48 ++++++++++++++++++++-------- src/user/user_model.cc | 2 ++ src/user/user_objects.cc | 1 + src/user/user_objects.h | 3 +- src/xml/xml_native_reader.cc | 8 +++-- src/xml/xml_native_writer.cc | 1 + test/engine/testdata/spin_recoil.xml | 42 ++++++++++++++++++++++++ unity/Runtime/Bindings/MjBindings.cs | 2 ++ 19 files changed, 187 insertions(+), 59 deletions(-) create mode 100644 test/engine/testdata/spin_recoil.xml diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index ee849901..cd637571 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -1630,6 +1630,8 @@ if omitted. .. _default-pair-solref: +.. _default-pair-solreffriction: + .. _default-pair-solimp: .. _default-pair-gap: @@ -4299,6 +4301,20 @@ element. :at:`solref`, :at:`solimp` Constraint solver parameters for contact simulation. See :ref:`CSolver`. +.. _contact-pair-solreffriction: + +:at:`solreffriction`: :at-val:`real, "0 0"` + Contact reference acceleration, in the friction dimensions. This attribute has the same semantics as other + :at:`solref` attributes (described in :ref:`CSolver`), with two important distictions: + + - The default :at-val:`"0 0"` means "use the same values as :at:`solref`". + - This attribute only takes effect for :ref:`elliptic friction cones`, since pyramidal cones mix normal + and frictional forces. + + Note that as with other :at:`solreffriction` attributes, the constraint violation is identically 0. Therefore, when + using positive semantics :at:`solreffriction[1]` is ignored, while for negative semantics :at:`solreffriction[0]` is + ignored. See :ref:`CSolver` for more details. + .. _contact-pair-margin: :at:`margin`: :at-val:`real, "0"` diff --git a/doc/XMLschema.rst b/doc/XMLschema.rst index 4be0f69a..11f2eb14 100644 --- a/doc/XMLschema.rst +++ b/doc/XMLschema.rst @@ -305,7 +305,9 @@ | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | | | | | :ref:`condim` | :ref:`friction` | :ref:`solref` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`solimp` | :ref:`gap` | :ref:`margin` | | +| | | | :ref:`solreffriction` | :ref:`solimp` | :ref:`gap` | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +| | | | :ref:`margin` | | | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ | |_| default |br| |_| |L| | | .. table:: | @@ -827,9 +829,9 @@ | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | | | | | :ref:`geom2` | :ref:`condim` | :ref:`friction` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`solref` | :ref:`solimp` | :ref:`gap` | | +| | | | :ref:`solref` | :ref:`solreffriction` | :ref:`solimp` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`margin` | | | | +| | | | :ref:`gap` | :ref:`margin` | | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ | |_| contact |br| |_| |L| | | .. table:: | diff --git a/doc/changelog.rst b/doc/changelog.rst index 55c9a1d1..cf4ebdb6 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -63,6 +63,13 @@ Simulate General ^^^^^^^ +- Added :ref:`mjContact.solreffriction`, allowing different :ref:`solref` parameters for the normal + and frictional axes of contacts when using :ref:`elliptic friction cones`. This attribute is required + for elastic frictional collisions, see associated + `example model `__ mimicking the + spin-bounce recoil behaviour of `elastic rubber balls `__. + This is an advanced option currently only supported by explicit :ref:`contact pairs`, using the + :ref:`solreffriction` attribute. - Added :ref:`mjd_inverseFD` for finite-differenced inverse-dynamics derivatives. - Added functions for operations on banded-then-dense "arrowhead" matrices. Such matrices are common when doing direct trajectory optimization. See :ref:`mju_cholFactorBand` documentation for details. diff --git a/doc/includes/references.h b/doc/includes/references.h index ed4166b5..b4ac36af 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -50,32 +50,33 @@ typedef enum mjtTimer_ { // internal timers mjNTIMER // number of timers } mjtTimer; -struct mjContact_ { // result of collision detection functions +struct mjContact_ { // result of collision detection functions // contact parameters set by geom-specific collision detector - mjtNum dist; // distance between nearest points; neg: penetration - mjtNum pos[3]; // position of contact point: midpoint between geoms - mjtNum frame[9]; // normal is in [0-2] + mjtNum dist; // distance between nearest points; neg: penetration + mjtNum pos[3]; // position of contact point: midpoint between geoms + mjtNum frame[9]; // normal is in [0-2] // contact parameters set by mj_collideGeoms - mjtNum includemargin; // include if dist : public WrapperBase { X(frame); X(friction); X(solref); + X(solreffriction); X(solimp); X(H); #undef X diff --git a/src/engine/engine_collision_driver.c b/src/engine/engine_collision_driver.c index 6ae2cd41..229aeefe 100644 --- a/src/engine/engine_collision_driver.c +++ b/src/engine/engine_collision_driver.c @@ -847,6 +847,7 @@ endbroad: void mj_collideGeoms(const mjModel* m, mjData* d, int g1, int g2, int flg_user, mjtNum usermargin) { int num, type1, type2, condim; mjtNum margin, gap, mix, friction[5], solref[mjNREF], solimp[mjNIMP]; + mjtNum solreffriction[mjNREF] = {0}; mjContact con[mjMAXCONPAIR]; int ipair = (g2 < 0 ? g1 : -1); @@ -1045,9 +1046,14 @@ void mj_collideGeoms(const mjModel* m, mjData* d, int g1, int g2, int flg_user, friction[i] = m->pair_friction[5*ipair+i]; } - // reference + // reference, normal direction mju_copy(solref, m->pair_solref+mjNREF*ipair, mjNREF); + // reference, friction directions + if (m->pair_solreffriction[mjNREF*ipair] || m->pair_solreffriction[mjNREF*ipair + 1]) { + mju_copy(solreffriction, m->pair_solreffriction+mjNREF*ipair, mjNREF); + } + // impedance mju_copy(solimp, m->pair_solimp+mjNIMP*ipair, mjNIMP); } @@ -1069,6 +1075,7 @@ void mj_collideGeoms(const mjModel* m, mjData* d, int g1, int g2, int flg_user, con[i].includemargin = margin-gap; mju_copy(con[i].friction, friction, 5); mj_assignRef(m, con[i].solref, solref); + mj_assignRef(m, con[i].solreffriction, solreffriction); mj_assignImp(m, con[i].solimp, solimp); // exclude in gap diff --git a/src/engine/engine_core_constraint.c b/src/engine/engine_core_constraint.c index c0e9f52c..7b37377b 100644 --- a/src/engine/engine_core_constraint.c +++ b/src/engine/engine_core_constraint.c @@ -993,10 +993,14 @@ void mj_diagApprox(const mjModel* m, mjData* d) { // get solref, solimp for specified constraint -static void getsolparam(const mjModel* m, const mjData* d, int i, mjtNum* solref, mjtNum* solimp) { +static void getsolparam(const mjModel* m, const mjData* d, int i, + mjtNum* solref, mjtNum* solreffriction, mjtNum* solimp) { // get constraint id int id = d->efc_id[i]; + // clear solreffriction (applies only to contacts) + mju_zero(solreffriction, mjNREF); + // extract solver parameters from corresponding model element switch (d->efc_type[i]) { case mjCNSTR_EQUALITY: @@ -1028,6 +1032,7 @@ static void getsolparam(const mjModel* m, const mjData* d, int i, mjtNum* solref case mjCNSTR_CONTACT_PYRAMIDAL: case mjCNSTR_CONTACT_ELLIPTIC: mju_copy(solref, d->contact[id].solref, mjNREF); + mju_copy(solreffriction, d->contact[id].solreffriction, mjNREF); mju_copy(solimp, d->contact[id].solimp, mjNIMP); } @@ -1042,6 +1047,17 @@ static void getsolparam(const mjModel* m, const mjData* d, int i, mjtNum* solref solref[0] = mju_max(solref[0], 2*m->opt.timestep); } + // check reference format: standard or direct, cannot be mixed + if ((solreffriction[0] > 0) ^ (solreffriction[1] > 0)) { + mju_warning("solreffriction values should have the same sign, replacing with default"); + mju_zero(solreffriction, mjNREF); // default solreffriction is (0, 0) + } + + // integrator safety: impose ref[0]>=2*timestep for standard format + if (!mjDISABLED(mjDSBL_REFSAFE) && solreffriction[0] > 0) { + solreffriction[0] = mju_max(solreffriction[0], 2*m->opt.timestep); + } + // enforce constraints on solimp solimp[0] = mju_min(mjMAXIMP, mju_max(mjMINIMP, solimp[0])); solimp[1] = mju_min(mjMAXIMP, mju_max(mjMINIMP, solimp[1])); @@ -1149,12 +1165,12 @@ static void getimpedance(const mjtNum* solimp, mjtNum pos, mjtNum margin, void mj_makeImpedance(const mjModel* m, mjData* d) { int dim, nefc = d->nefc; mjtNum *R = d->efc_R, *KBIP = d->efc_KBIP; - mjtNum pos, imp, impP, Rpy, solref[mjNREF], solimp[mjNIMP]; + mjtNum pos, imp, impP, Rpy, solref[mjNREF], solreffriction[mjNREF], solimp[mjNIMP]; // set efc_R, efc_KBIP for (int i=0; i < nefc; i++) { // get solref and solimp - getsolparam(m, d, i, solref, solimp); + getsolparam(m, d, i, solref, solreffriction, solimp); // get pos and dim getposdim(m, d, i, &pos, &dim); @@ -1167,32 +1183,36 @@ void mj_makeImpedance(const mjModel* m, mjData* d) { // R = (1-imp)/imp * diagApprox R[i+j] = mju_max(mjMINVAL, (1-imp)*d->efc_diagApprox[i+j]/imp); - // friction: K = 0 + // constraint type int tp = d->efc_type[i+j]; - if (tp == mjCNSTR_FRICTION_DOF || - tp == mjCNSTR_FRICTION_TENDON || - (tp == mjCNSTR_CONTACT_ELLIPTIC && j > 0)) { + + // elliptic contacts use solreffriction in non-normal directions, if non-zero + int elliptic_friction = (tp == mjCNSTR_CONTACT_ELLIPTIC) && (j > 0); + mjtNum* ref = elliptic_friction && (solreffriction[0] || solreffriction[1]) ? + solreffriction : solref; + + // friction: K = 0 + if (tp == mjCNSTR_FRICTION_DOF || tp == mjCNSTR_FRICTION_TENDON || elliptic_friction) { KBIP[4*(i+j)] = 0; } // standard: K = 1 / (dmax^2 * timeconst^2 * dampratio^2) - else if (solref[0] > 0) - KBIP[4*(i+j)] = 1 / mju_max(mjMINVAL, - solimp[1]*solimp[1] * solref[0]*solref[0] * solref[1]*solref[1]); + else if (ref[0] > 0) + KBIP[4*(i+j)] = 1 / mju_max(mjMINVAL, solimp[1]*solimp[1] * ref[0]*ref[0] * ref[1]*ref[1]); // direct: K = -solref[0] / dmax^2 else { - KBIP[4*(i+j)] = -solref[0] / mju_max(mjMINVAL, solimp[1]*solimp[1]); + KBIP[4*(i+j)] = -ref[0] / mju_max(mjMINVAL, solimp[1]*solimp[1]); } // standard: B = 2 / (dmax*timeconst) - if (solref[1] > 0) { - KBIP[4*(i+j)+1] = 2 / mju_max(mjMINVAL, solimp[1]*solref[0]); + if (ref[1] > 0) { + KBIP[4*(i+j)+1] = 2 / mju_max(mjMINVAL, solimp[1]*ref[0]); } // direct: B = -solref[1] / dmax else { - KBIP[4*(i+j)+1] = -solref[1] / mju_max(mjMINVAL, solimp[1]); + KBIP[4*(i+j)+1] = -ref[1] / mju_max(mjMINVAL, solimp[1]); } // I = imp, P = imp' diff --git a/src/user/user_model.cc b/src/user/user_model.cc index df357577..3f670a85 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -1871,6 +1871,7 @@ void mjCModel::CopyObjects(mjModel* m) { m->pair_geom2[i] = pairs[i]->geom2; m->pair_signature[i] = pairs[i]->signature; copyvec(m->pair_solref+mjNREF*i, pairs[i]->solref, mjNREF); + copyvec(m->pair_solreffriction+mjNREF*i, pairs[i]->solreffriction, mjNREF); 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; @@ -3037,6 +3038,7 @@ bool mjCModel::CopyBack(const mjModel* m) { // pairs for (int i=0; isolref, m->pair_solref+mjNREF*i, mjNREF); + copyvec(pairs[i]->solreffriction, m->pair_solreffriction+mjNREF*i, mjNREF); 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]; diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index 89fb7a33..df1bcbfb 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -2931,6 +2931,7 @@ mjCPair::mjCPair(mjCModel* _model, mjCDef* _def) { condim = 3; mj_defaultSolRefImp(solref, solimp); + mju_zero(solreffriction, mjNREF); margin = 0; gap = 0; friction[0] = 1; diff --git a/src/user/user_objects.h b/src/user/user_objects.h index ff1a5202..556548cd 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -772,7 +772,8 @@ class mjCPair : public mjCBase { // optional parameters: computed from geoms if not set by user int condim; // contact dimensionality - mjtNum solref[mjNREF]; // solver reference + mjtNum solref[mjNREF]; // solver reference, normal direction + mjtNum solreffriction[mjNREF]; // solver reference, frictional directions mjtNum solimp[mjNIMP]; // solver impedance double margin; // margin for contact detection double gap; // include in solver if dist"}, @@ -1495,6 +1496,7 @@ void mjXReader::OnePair(XMLElement* elem, mjCPair* ppair) { ReadAttrTxt(elem, "name", ppair->name); ReadAttrInt(elem, "condim", &ppair->condim); ReadAttr(elem, "solref", mjNREF, ppair->solref, text, false, false); + ReadAttr(elem, "solreffriction", mjNREF, ppair->solreffriction, text, false, false); ReadAttr(elem, "solimp", mjNIMP, ppair->solimp, text, false, false); ReadAttr(elem, "margin", 1, &ppair->margin, text); ReadAttr(elem, "gap", 1, &ppair->gap, text); diff --git a/src/xml/xml_native_writer.cc b/src/xml/xml_native_writer.cc index b4c21052..4e7c77c5 100644 --- a/src/xml/xml_native_writer.cc +++ b/src/xml/xml_native_writer.cc @@ -443,6 +443,7 @@ void mjXWriter::OnePair(XMLElement* elem, mjCPair* ppair, mjCDef* def) { WriteAttr(elem, "margin", 1, &ppair->margin, &def->pair.margin); WriteAttr(elem, "gap", 1, &ppair->gap, &def->pair.gap); WriteAttr(elem, "solref", mjNREF, ppair->solref, def->pair.solref); + WriteAttr(elem, "solreffriction", mjNREF, ppair->solreffriction, def->pair.solreffriction); WriteAttr(elem, "solimp", mjNIMP, ppair->solimp, def->pair.solimp); WriteAttr(elem, "friction", 5, ppair->friction, def->pair.friction); } diff --git a/test/engine/testdata/spin_recoil.xml b/test/engine/testdata/spin_recoil.xml new file mode 100644 index 00000000..ddfde108 --- /dev/null +++ b/test/engine/testdata/spin_recoil.xml @@ -0,0 +1,42 @@ + + diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index f22f8d59..7265c834 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -524,6 +524,7 @@ public unsafe struct mjContact_ { public double includemargin; public fixed double friction[5]; public fixed double solref[2]; + public fixed double solreffriction[2]; public fixed double solimp[5]; public double mu; public fixed double H[36]; @@ -2130,6 +2131,7 @@ public unsafe struct mjModel_ { public int* pair_geom2; public int* pair_signature; public double* pair_solref; + public double* pair_solreffriction; public double* pair_solimp; public double* pair_margin; public double* pair_gap;