Add flag for disabling implicit integration of joint damping in the Euler integrator.

PiperOrigin-RevId: 559398414
Change-Id: I43b23c5e75a2830a64b1898daeaa16f19e011718

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<geIntegration>` section for more details.
+
 .. _option-flag-override:
 
 :at:`override`: :at-val:`[disable, enable], "disable"`

doc/XMLschema.rst

@@ -239,9 +239,11 @@
 |                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
 |                                    |    |    | :ref:`warmstart<option-flag-warmstart>`                         | :ref:`filterparent<option-flag-filterparent>`                   | :ref:`actuation<option-flag-actuation>`                         | :ref:`refsafe<option-flag-refsafe>`                             | |
 |                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
-|                                    |    |    | :ref:`sensor<option-flag-sensor>`                               | :ref:`override<option-flag-override>`                           | :ref:`energy<option-flag-energy>`                               | :ref:`fwdinv<option-flag-fwdinv>`                               | |
+|                                    |    |    | :ref:`sensor<option-flag-sensor>`                               | :ref:`midphase<option-flag-midphase>`                           | :ref:`eulerdamp<option-flag-eulerdamp>`                         | :ref:`override<option-flag-override>`                           | |
 |                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
-|                                    |    |    | :ref:`sensornoise<option-flag-sensornoise>`                     | :ref:`multiccd<option-flag-multiccd>`                           | :ref:`island<option-flag-island>`                               |                                                                 | |
+|                                    |    |    | :ref:`energy<option-flag-energy>`                               | :ref:`fwdinv<option-flag-fwdinv>`                               | :ref:`sensornoise<option-flag-sensornoise>`                     | :ref:`multiccd<option-flag-multiccd>`                           | |
+|                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+|                                    |    |    | :ref:`island<option-flag-island>`                               |                                                                 |                                                                 |                                                                 | |
 |                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
 +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
 | mujoco |br| |L|                    |    | .. table::                                                                                                                                                                                                                                                                   |

doc/changelog.rst

@@ -34,11 +34,13 @@ General
 #. Renamed ``actuatorforcerange`` and ``actuatorforcelimited``, introduced in the previous version to
    :ref:`actuatorfrcrange<body-joint-actuatorfrcrange>` and
    :ref:`actuatorfrclimited<body-joint-actuatorfrclimited>`, respectively.
+#. Added the flag :ref:`eulerdamp<option-flag-eulerdamp>`, which disables implicit integration of joint damping in the
+   Euler integrator. See the :ref:`Numerical Integration<geIntegration>` section for more details.
 
 Python bindings
 ^^^^^^^^^^^^^^^
 
-4. Fixed `#870 <https://github.com/deepmind/mujoco/issues/870>`__ where calling ``update_scene`` with an invalid
+7. Fixed `#870 <https://github.com/deepmind/mujoco/issues/870>`__ where calling ``update_scene`` with an invalid
    camera name used the default camera.
 
 

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<option-flag-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<mjtState>` 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:
 

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

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;
 
 

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',

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 (!mjDISABLED(mjDSBL_EULERDAMP)) {
-    if (m->dof_damping[i] > 0) {
+    for (int i=0; i < nv; i++) {
-      dof_damping = 1;
+      if (m->dof_damping[i] > 0) {
-      break;
+        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);
   }

src/engine/engine_support.c

@@ -56,7 +56,8 @@ const char* mjDISABLESTRING[mjNDISABLE] = {
   "Actuation",
   "Refsafe",
   "Sensor",
-  "Midphase"
+  "Midphase",
+  "Eulerdamp"
 };
 
 

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) \

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) \

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"(
+  <mujoco>
+    <option>
+      <flag eulerdamp="disable"/>
+    </option>
+
+    <worldbody>
+      <body>
+        <joint axis="1 0 0" damping="2"/>
+        <geom type="capsule" size=".01" fromto="0 0 0 0 .1 0"/>
+        <body pos="0 .1 0">
+          <joint axis="0 1 0" damping="1"/>
+          <geom type="capsule" size=".01" fromto="0 0 0 .1 0 0"/>
+        </body>
+      </body>
+    </worldbody>
+  </mujoco>
+  )";
+
+  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<mjtNum> qvel = AsVector(data->qvel, model->nv);
+  std::vector<mjtNum> qacc = AsVector(data->qacc, model->nv);
+
+  // second step
+  mj_step(model, data);
+
+  // compute finite-difference acceleration
+  std::vector<mjtNum> 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<mjtNum> 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"(
+  <mujoco>
+    <worldbody>
+      <body>
+        <joint axis="1 0 0" damping="2"/>
+        <geom type="capsule" size=".01" fromto="0 0 0 0 .1 0"/>
+        <body pos="0 .1 0">
+          <joint axis="0 1 0" damping="1"/>
+          <geom type="capsule" size=".01" fromto="0 0 0 .1 0 0"/>
+        </body>
+      </body>
+    </worldbody>
+  </mujoco>
+  )";
+
+  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<mjtNum> 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<mjtNum> 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"(

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,