From b7cf479abeed434d7d5caa1f5f74856e3a45848f Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Wed, 23 Aug 2023 05:50:52 -0700 Subject: [PATCH] Add flag for disabling implicit integration of joint damping in the Euler integrator. PiperOrigin-RevId: 559398414 Change-Id: I43b23c5e75a2830a64b1898daeaa16f19e011718 --- doc/XMLreference.rst | 6 ++ doc/XMLschema.rst | 6 +- doc/changelog.rst | 4 +- doc/computation.rst | 6 +- doc/includes/references.h | 3 +- include/mujoco/mjmodel.h | 3 +- introspect/enums.py | 3 +- src/engine/engine_forward.c | 14 ++-- src/engine/engine_support.c | 3 +- src/xml/xml_native_reader.cc | 5 +- src/xml/xml_native_writer.cc | 1 + test/engine/engine_forward_test.cc | 117 +++++++++++++++++++++++++++ unity/Runtime/Bindings/MjBindings.cs | 3 +- 13 files changed, 156 insertions(+), 18 deletions(-) diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index 98f241e9..be8754b3 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -2104,6 +2104,12 @@ from its default. This flag disables the mid-phase collision filtering using a static AABB bounding volume hierarchy (a BVH binary tree). If disabled, all geoms pairs that are allowed to collide are checked for collisions. +.. _option-flag-eulerdamp: + +:at:`eulerdamp`: :at-val:`[disable, enable], "enable"` + This flag disables implicit integration with respect to joint damping in the Euler integrator. See the + :ref:`Numerical Integration` section for more details. + .. _option-flag-override: :at:`override`: :at-val:`[disable, enable], "disable"` diff --git a/doc/XMLschema.rst b/doc/XMLschema.rst index 7f045807..a63b7e75 100644 --- a/doc/XMLschema.rst +++ b/doc/XMLschema.rst @@ -239,9 +239,11 @@ | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | | | | | :ref:`warmstart` | :ref:`filterparent` | :ref:`actuation` | :ref:`refsafe` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`sensor` | :ref:`override` | :ref:`energy` | :ref:`fwdinv` | | +| | | | :ref:`sensor` | :ref:`midphase` | :ref:`eulerdamp` | :ref:`override` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`sensornoise` | :ref:`multiccd` | :ref:`island` | | | +| | | | :ref:`energy` | :ref:`fwdinv` | :ref:`sensornoise` | :ref:`multiccd` | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +| | | | :ref:`island` | | | | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ | mujoco |br| |L| | | .. table:: | diff --git a/doc/changelog.rst b/doc/changelog.rst index b4a82128..d4ac43d9 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -34,11 +34,13 @@ General #. Renamed ``actuatorforcerange`` and ``actuatorforcelimited``, introduced in the previous version to :ref:`actuatorfrcrange` and :ref:`actuatorfrclimited`, respectively. +#. Added the flag :ref:`eulerdamp`, which disables implicit integration of joint damping in the + Euler integrator. See the :ref:`Numerical Integration` section for more details. Python bindings ^^^^^^^^^^^^^^^ -4. Fixed `#870 `__ where calling ``update_scene`` with an invalid +7. Fixed `#870 `__ where calling ``update_scene`` with an invalid camera name used the default camera. diff --git a/doc/computation.rst b/doc/computation.rst index 61631f39..f1acf6d3 100644 --- a/doc/computation.rst +++ b/doc/computation.rst @@ -539,7 +539,9 @@ All three single-step integrators in MuJoCo use the update :eq:`eq_implicit_upda Semi-implicit with implicit joint damping (``Euler``) For this method, :math:`D` only includes derivatives of joint damping. Note that in this case :math:`D` is diagonal - and :math:`M-h D` is symmetric, so Cholesky decomposition can be used. + and :math:`M-h D` is symmetric, so Cholesky decomposition can be used. If the model has no joint damping or the + :ref:`eulerdamp` disable-flag is set, implicit damping is disabled and the semi-implicit + update :eq:`eq_semimplicit` is used, rather than :eq:`eq_implicit_update`. Implicit-in-velocity (``implicit``) For this method, :math:`D` includes derivatives of all forces except the constraint forces :math:`J^T f(v)`. These @@ -690,7 +692,7 @@ also treated as an input variable. All other :ref:`mjData` fields are functions Note that the full integration state as given by :ref:`mjSTATE_INTEGRATION` is maximalist and includes fields which are often unused. If a small state size is desired, it might be sensible to avoid saving unused fields. -In particular `xfrc_applied`` can be quite large (``6 x nbody``) yet is often unused. +In particular ``xfrc_applied`` can be quite large (``6 x nbody``) yet is often unused. .. _geSimulationState: diff --git a/doc/includes/references.h b/doc/includes/references.h index 6235de11..f02d7410 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -387,8 +387,9 @@ typedef enum mjtDisableBit_ { // disable default feature bitflags mjDSBL_REFSAFE = 1<<11, // integrator safety: make ref[0]>=2*timestep mjDSBL_SENSOR = 1<<12, // sensors mjDSBL_MIDPHASE = 1<<13, // mid-phase collision filtering + mjDSBL_EULERDAMP = 1<<14, // implicit integration of joint damping in Euler integrator - mjNDISABLE = 14 // number of disable flags + mjNDISABLE = 15 // number of disable flags } mjtDisableBit; typedef enum mjtEnableBit_ { // enable optional feature bitflags mjENBL_OVERRIDE = 1<<0, // override contact parameters diff --git a/include/mujoco/mjmodel.h b/include/mujoco/mjmodel.h index f988580a..15c822ff 100644 --- a/include/mujoco/mjmodel.h +++ b/include/mujoco/mjmodel.h @@ -58,8 +58,9 @@ typedef enum mjtDisableBit_ { // disable default feature bitflags mjDSBL_REFSAFE = 1<<11, // integrator safety: make ref[0]>=2*timestep mjDSBL_SENSOR = 1<<12, // sensors mjDSBL_MIDPHASE = 1<<13, // mid-phase collision filtering + mjDSBL_EULERDAMP = 1<<14, // implicit integration of joint damping in Euler integrator - mjNDISABLE = 14 // number of disable flags + mjNDISABLE = 15 // number of disable flags } mjtDisableBit; diff --git a/introspect/enums.py b/introspect/enums.py index 2042d428..32273f7b 100644 --- a/introspect/enums.py +++ b/introspect/enums.py @@ -41,7 +41,8 @@ ENUMS: Mapping[str, EnumDecl] = dict([ ('mjDSBL_REFSAFE', 2048), ('mjDSBL_SENSOR', 4096), ('mjDSBL_MIDPHASE', 8192), - ('mjNDISABLE', 14), + ('mjDSBL_EULERDAMP', 16384), + ('mjNDISABLE', 15), ]), )), ('mjtEnableBit', diff --git a/src/engine/engine_forward.c b/src/engine/engine_forward.c index ba3b5bbe..fa174af2 100644 --- a/src/engine/engine_forward.c +++ b/src/engine/engine_forward.c @@ -593,16 +593,18 @@ void mj_EulerSkip(const mjModel* m, mjData* d, int skipfactor) { mjtNum* qfrc = mj_stackAlloc(d, nv); mjtNum* qacc = mj_stackAlloc(d, nv); - // check for dof damping + // check for dof damping if disable flag is not set int dof_damping = 0; - for (int i=0; i < nv; i++) { - if (m->dof_damping[i] > 0) { - dof_damping = 1; - break; + if (!mjDISABLED(mjDSBL_EULERDAMP)) { + for (int i=0; i < nv; i++) { + if (m->dof_damping[i] > 0) { + dof_damping = 1; + break; + } } } - // no damping: explicit velocity integration + // no damping or disabled: explicit velocity integration if (!dof_damping) { mju_copy(qacc, d->qacc, nv); } diff --git a/src/engine/engine_support.c b/src/engine/engine_support.c index 787750b8..a230890d 100644 --- a/src/engine/engine_support.c +++ b/src/engine/engine_support.c @@ -56,7 +56,8 @@ const char* mjDISABLESTRING[mjNDISABLE] = { "Actuation", "Refsafe", "Sensor", - "Midphase" + "Midphase", + "Eulerdamp" }; diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index d098fc04..81b56dd9 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -103,9 +103,9 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = { "solver", "iterations", "noslip_iterations", "mpr_iterations", "sdf_iterations", "sdf_initpoints"}, {"<"}, - {"flag", "?", "19", "constraint", "equality", "frictionloss", "limit", "contact", + {"flag", "?", "21", "constraint", "equality", "frictionloss", "limit", "contact", "passive", "gravity", "clampctrl", "warmstart", - "filterparent", "actuation", "refsafe", "sensor", + "filterparent", "actuation", "refsafe", "sensor", "midphase", "eulerdamp", "override", "energy", "fwdinv", "sensornoise", "multiccd", "island"}, {">"}, @@ -1003,6 +1003,7 @@ void mjXReader::Option(XMLElement* section, mjOption* opt) { READDSBL("refsafe", mjDSBL_REFSAFE) READDSBL("sensor", mjDSBL_SENSOR) READDSBL("midphase", mjDSBL_MIDPHASE) + READDSBL("eulerdamp", mjDSBL_EULERDAMP) #undef READDSBL #define READENBL(NAME, MASK) \ diff --git a/src/xml/xml_native_writer.cc b/src/xml/xml_native_writer.cc index 1491ef8e..4de50b1d 100644 --- a/src/xml/xml_native_writer.cc +++ b/src/xml/xml_native_writer.cc @@ -836,6 +836,7 @@ void mjXWriter::Option(XMLElement* root) { WRITEDSBL("refsafe", mjDSBL_REFSAFE) WRITEDSBL("sensor", mjDSBL_SENSOR) WRITEDSBL("midphase", mjDSBL_MIDPHASE) + WRITEDSBL("eulerdamp", mjDSBL_EULERDAMP) #undef WRITEDSBL #define WRITEENBL(NAME, MASK) \ diff --git a/test/engine/engine_forward_test.cc b/test/engine/engine_forward_test.cc index afe28abb..a3f8abfa 100644 --- a/test/engine/engine_forward_test.cc +++ b/test/engine/engine_forward_test.cc @@ -164,6 +164,123 @@ TEST_F(ForwardTest, DamperDampens) { using ImplicitIntegratorTest = MujocoTest; +// Disabling implicit joint damping works as expected +TEST_F(ImplicitIntegratorTest, EulerDampDisable) { + static constexpr char xml[] = R"( + + + + + + + + + + + + + + + )"; + + mjModel* model = LoadModelFromString(xml); + mjData* data = mj_makeData(model); + + // step once, call mj_forward, save qvel and qacc + mj_step(model, data); + mj_forward(model, data); + std::vector qvel = AsVector(data->qvel, model->nv); + std::vector qacc = AsVector(data->qacc, model->nv); + + // second step + mj_step(model, data); + + // compute finite-difference acceleration + std::vector qacc_fd(model->nv); + for (int i=0; i < model->nv; i++) { + qacc_fd[i] = (data->qvel[i] - qvel[i]) / model->opt.timestep; + } + // expect finite-differenced qacc to match to high precision + EXPECT_THAT(qacc_fd, Pointwise(DoubleNear(1e-14), qacc)); + + // reach the the same initial state + mj_resetData(model, data); + mj_step(model, data); + + // second step again, but with implicit integration of joint damping + model->opt.disableflags &= ~mjDSBL_EULERDAMP; + mj_step(model, data); + + // compute finite-difference acceleration difference + std::vector dqacc(model->nv); + for (int i=0; i < model->nv; i++) { + dqacc[i] = (data->qvel[i] - qvel[i]) / model->opt.timestep; + } + // expect finite-differenced qacc to not match + EXPECT_GT(mju_norm(dqacc.data(), model->nv), 1); + + mj_deleteData(data); + mj_deleteModel(model); +} + +// Reducing timesteps reduces the difference between implicit/explicit +TEST_F(ImplicitIntegratorTest, EulerDampLimit) { + static constexpr char xml[] = R"( + + + + + + + + + + + + + )"; + + mjModel* model = LoadModelFromString(xml); + mjData* data = mj_makeData(model); + + mjtNum diff_norm_prev = -1; + for (const mjtNum dt : {1e-2, 1e-3, 1e-4, 1e-5, 1e-6, 1e-7, 1e-8}) { + // set timestep + model->opt.timestep = dt; + + // step twice with implicit damping, save qvel + model->opt.disableflags &= ~mjDSBL_EULERDAMP; + mj_resetData(model, data); + mj_step(model, data); + mj_step(model, data); + std::vector qvel_imp = AsVector(data->qvel, model->nv); + + // step once, step again without implicit damping, save qvel + mj_resetData(model, data); + mj_step(model, data); + model->opt.disableflags |= mjDSBL_EULERDAMP; + mj_step(model, data); + std::vector qvel_exp = AsVector(data->qvel, model->nv); + + mjtNum diff_norm = 0; + for (int i=0; i < model->nv; i++) { + diff_norm += (qvel_imp[i] - qvel_exp[i]) * (qvel_imp[i] - qvel_exp[i]); + } + diff_norm = mju_sqrt(diff_norm); + + if (diff_norm_prev != -1){ + EXPECT_LT(diff_norm, diff_norm_prev); + } + + diff_norm_prev = diff_norm; + } + + mj_deleteData(data); + mj_deleteModel(model); +} + // Euler and implicit should be equivalent if there is only joint damping TEST_F(ImplicitIntegratorTest, EulerImplicitEqivalent) { static constexpr char xml[] = R"( diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index 035ea508..88514ba1 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -151,7 +151,8 @@ public enum mjtDisableBit : int{ mjDSBL_REFSAFE = 2048, mjDSBL_SENSOR = 4096, mjDSBL_MIDPHASE = 8192, - mjNDISABLE = 14, + mjDSBL_EULERDAMP = 16384, + mjNDISABLE = 15, } public enum mjtEnableBit : int{ mjENBL_OVERRIDE = 1,