Skip disabled actuators in mjd_actuator_vel.

Fixes a bug where the derivative of the actuator force with respect to the generalized velocities (used in implicit and implicitfast integrators) was failing to take into account disabled actuators.

Fixes #1838

PiperOrigin-RevId: 657193960
Change-Id: Id8c0ab863a39e2a01cd2703774f460e9731b4807

doc/XMLreference.rst

@@ -1751,7 +1751,7 @@ properties are grouped together.
 .. _material-rgb:
 
 :el-prefix:`material/` |-| **rgb** (?)
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
 This element references a texture asset used to specify base color / albedo values.
 
@@ -1763,7 +1763,7 @@ This element references a texture asset used to specify base color / albedo valu
 .. _material-normal:
 
 :el-prefix:`material/` |-| **normal** (?)
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
 This element references a texture asset used to specify the bump map (surface normals).
 
@@ -1775,7 +1775,7 @@ This element references a texture asset used to specify the bump map (surface no
 .. _material-occlusion:
 
 :el-prefix:`material/` |-| **occlusion** (?)
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
 This element references a texture asset used to specify ambient occlusion.
 
@@ -1787,7 +1787,7 @@ This element references a texture asset used to specify ambient occlusion.
 .. _material-roughness:
 
 :el-prefix:`material/` |-| **roughness** (?)
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
 This element references a texture asset used to specify the roughness map.
 
@@ -1799,7 +1799,7 @@ This element references a texture asset used to specify the roughness map.
 .. _material-metallic:
 
 :el-prefix:`material/` |-| **metallic** (?)
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
 This element references a texture asset used to specify the metallic map.
 
@@ -1811,7 +1811,7 @@ This element references a texture asset used to specify the metallic map.
 .. _material-opacity:
 
 :el-prefix:`material/` |-| **opacity** (?)
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
 This element references a texture asset used to specify the opacity map (alpha channel, transparency).
 
@@ -1823,7 +1823,7 @@ This element references a texture asset used to specify the opacity map (alpha c
 .. _material-emissive:
 
 :el-prefix:`material/` |-| **emissive** (?)
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
 This element references a texture asset used to specify light emission.
 
@@ -1835,7 +1835,7 @@ This element references a texture asset used to specify light emission.
 .. _material-orm:
 
 :el-prefix:`material/` |-| **orm** (?)
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
 This element references a texture asset used to specify a packed ORM map, where occlusion, roughness, and metallic
 are joined into the corresponding RGB values of a single texture.
@@ -1848,7 +1848,7 @@ are joined into the corresponding RGB values of a single texture.
 .. _material-rgba:
 
 :el-prefix:`material/` |-| **rgba** (?)
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
 This element references a texture asset used to specify a packed map where albedo and opacity are joined into the same
 4-channel texture.

doc/changelog.rst

@@ -7,16 +7,14 @@ Upcoming version (not yet released)
 
 General
 ^^^^^^^
-
 1. Renamed ``mjModel.tex_rbg`` to ``mjModel.tex_data``.
 2. Added a new disable flag ``mjDSBL_AUTORESETNAN`` to disable automatic reset when NaNs or infinities are produced.
 3. Added sub-elements to the MJCF :ref:`material<asset-material>` element, to allow specification of multiple textures
-for rendering (e.g., :ref:`occlusion-roughness-metallic<material-orm>`). Note that the MuJoCo renderer doesn't support
+   for rendering (e.g., :ref:`occlusion-roughness-metallic<material-orm>`). Note that the MuJoCo renderer doesn't
-these new features, and they are made available for use with external renderers.
+   support these new features, and they are made available for use with external renderers.
 
 MJX
 ^^^
-
 4. Added more fields to ``mjx.Model`` and ``mjx.Data`` for further compatibility with the corresponding MuJoCo structs.
 5. Added support for :ref:`fixed tendons <tendon-fixed>`.
 6. Added support for tendon length limits (``mjCNSTR_LIMIT_TENDON`` in :ref:`mjtConstraint`).
@@ -28,6 +26,10 @@ Python bindings
 9. Added support for asset dictionary argument in ``mujoco.spec.from_file``, ``mujoco.spec.from_string`` and
    ``mujoco.spec.compile``.
 
+Bug fixes
+^^^^^^^^^
+10. Fixed a bug where implicit integrators did not take into account disabled actuators (:github:issue:`1838`).
+
 Version 3.2.0 (Jul 15, 2024)
 ----------------------------
 

src/engine/engine_derivative.c

@@ -834,6 +834,11 @@ void mjd_actuator_vel(const mjModel* m, mjData* d) {
 
   // process actuators
   for (int i=0; i < m->nu; i++) {
+    // skip if disabled
+    if (mj_actuatorDisabled(m, i)) {
+      continue;
+    }
+
     mjtNum bias_vel = 0, gain_vel = 0;
 
     // affine bias

test/engine/engine_derivative_test.cc

@@ -159,6 +159,70 @@ TEST_F(DerivativeTest, SmoothDvel) {
   }
 }
 
+// disabled actuators do not contribute to d_qfrc_actuator/d_qvel
+TEST_F(DerivativeTest, DisabledActuators) {
+  // model with only a position actuator
+  static constexpr char xml1[] = R"(
+  <mujoco>
+    <option integrator="implicitfast"/>
+
+    <worldbody>
+      <body>
+        <joint name="joint" type="slide"/>
+        <geom size=".1"/>
+      </body>
+    </worldbody>
+
+    <actuator>
+      <position joint="joint" group="1" kp="2000" kv="200"/>
+    </actuator>
+  </mujoco>
+  )";
+
+  mjModel* m1 = LoadModelFromString(xml1);
+  mjData* d1 = mj_makeData(m1);
+
+  d1->ctrl[0] = 6;
+  while (d1->time < 1)
+    mj_step(m1, d1);
+
+  // model with a position actuator and an intvelocity actuator
+  static constexpr char xml2[] = R"(
+  <mujoco>
+    <option integrator="implicitfast" actuatorgroupdisable="2"/>
+
+    <worldbody>
+      <body>
+        <joint name="joint" type="slide"/>
+        <geom size=".1"/>
+      </body>
+    </worldbody>
+
+    <actuator>
+      <position joint="joint" group="1" kp="2000" kv="200"/>
+      <intvelocity joint="joint" group="2" kp="2000" kv="200" actrange="-6 6"/>
+    </actuator>
+  </mujoco>
+  )";
+
+  mjModel* m2 = LoadModelFromString(xml2);
+  mjData* d2 = mj_makeData(m2);
+
+  d2->ctrl[0] = 6;
+  d2->ctrl[1] = 6;
+
+  while (d2->time < 1)
+    mj_step(m2, d2);
+
+  // expect same qvel in both models
+  EXPECT_EQ(d1->qvel[0], d2->qvel[0]);
+
+  mj_deleteData(d2);
+  mj_deleteModel(m2);
+  mj_deleteData(d1);
+  mj_deleteModel(m1);
+}
+
 // compare analytic and fin-diff d_qfrc_passive/d_qvel
 TEST_F(DerivativeTest, PassiveDvel) {
   for (const char* local_path : {kTumblingThinObjectPath,