Fix implicit integrator derivatives for actearly actuators.

The derivative calculation for actuator velocity in implicit integrators now correctly accounts for the `actearly` flag, using the next activation value when `actearly` is true.

PiperOrigin-RevId: 868598722
Change-Id: Ia180afb15b31a718170aeaf9d4ac514bb9e6073b

doc/changelog.rst

@@ -122,6 +122,8 @@ Documentation
 
 Bug fixes
 ^^^^^^^^^
+- Fixed a bug in :ref:`implicit integrator<geIntegrators>` derivatives where actuator velocity derivatives did not
+  account for the :ref:`actearly<actuator-general-actearly>` flag.
 - Multi threaded mesh processing, enabled by the :ref:`usethread<compiler-usethread>` compiler flag (on by default), was
   in fact disabled by the flag. Fixing this bug speeds up compilation of mesh-heavy models by (up to) the number of
   available cores.

src/engine/engine_derivative.c

@@ -1117,9 +1117,15 @@ void mjd_actuator_vel(const mjModel* m, mjData* d) {
       if (m->actuator_dyntype[i] == mjDYN_NONE) {
         bias_vel += gain_vel * d->ctrl[i];
       } else {
-        int act_first = m->actuator_actadr[i];
-        int act_last = act_first + m->actuator_actnum[i] - 1;
-        bias_vel += gain_vel * d->act[act_last];
+        int act_adr = m->actuator_actadr[i] + m->actuator_actnum[i] - 1;
+        mjtNum act = d->act[act_adr];
+
+        // use next activation if actearly is set (matching forward pass)
+        if (m->actuator_actearly[i]) {
+          act = mj_nextActivation(m, d, i, act_adr, d->act_dot[act_adr]);
+        }
+
+        bias_vel += gain_vel * act;
       }
     }
 

src/engine/engine_forward.c

@@ -260,32 +260,6 @@ void mj_fwdVelocity(const mjModel* m, mjData* d) {
 }
 
 
-// returns the next act given the current act_dot, after clamping
-static mjtNum nextActivation(const mjModel* m, const mjData* d,
-                             int actuator_id, int act_adr, mjtNum act_dot) {
-  mjtNum act = d->act[act_adr];
-
-  if (m->actuator_dyntype[actuator_id] == mjDYN_FILTEREXACT) {
-    // exact filter integration
-    // act_dot(0) = (ctrl-act(0)) / tau
-    // act(h) = act(0) + (ctrl-act(0)) (1 - exp(-h / tau))
-    //        = act(0) + act_dot(0) * tau * (1 - exp(-h / tau))
-    mjtNum tau = mju_max(mjMINVAL, m->actuator_dynprm[actuator_id * mjNDYN]);
-    act = act + act_dot * tau * (1 - mju_exp(-m->opt.timestep / tau));
-  } else {
-    // Euler integration
-    act = act + act_dot * m->opt.timestep;
-  }
-
-  // clamp to actrange
-  if (m->actuator_actlimited[actuator_id]) {
-    mjtNum* actrange = m->actuator_actrange + 2 * actuator_id;
-    act = mju_clip(act, actrange[0], actrange[1]);
-  }
-
-  return act;
-}
-
 
 // clamp vector to range
 static void clampVec(mjtNum* vec, const mjtNum* range, const mjtByte* limited, int n,
@@ -474,7 +448,7 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
 
       mjtNum act;
       if (m->actuator_actearly[i]) {
-        act = nextActivation(m, d, i, act_adr, d->act_dot[act_adr]);
+        act = mj_nextActivation(m, d, i, act_adr, d->act_dot[act_adr]);
       } else {
         act = d->act[act_adr];
       }
@@ -916,7 +890,7 @@ static void mj_advance(const mjModel* m, mjData* d,
       int actadr_end = actadr + m->actuator_actnum[i];
       for (int j=actadr; j < actadr_end; j++) {
         // if disabled, set act_dot to 0
-        d->act[j] = nextActivation(m, d, i, j, mj_actuatorDisabled(m, i) ? 0 : act_dot[j]);
+        d->act[j] = mj_nextActivation(m, d, i, j, mj_actuatorDisabled(m, i) ? 0 : act_dot[j]);
       }
     }
   }

src/engine/engine_support.c

@@ -704,6 +704,34 @@ int mj_actuatorDisabled(const mjModel* m, int i) {
   }
 }
 
+
+// returns the next activation given current act_dot, after clamping
+mjtNum mj_nextActivation(const mjModel* m, const mjData* d,
+                         int actuator_id, int act_adr, mjtNum act_dot) {
+  mjtNum act = d->act[act_adr];
+
+  if (m->actuator_dyntype[actuator_id] == mjDYN_FILTEREXACT) {
+    // exact filter integration
+    // act_dot(0) = (ctrl-act(0)) / tau
+    // act(h) = act(0) + (ctrl-act(0)) (1 - exp(-h / tau))
+    //        = act(0) + act_dot(0) * tau * (1 - exp(-h / tau))
+    mjtNum tau = mju_max(mjMINVAL, m->actuator_dynprm[actuator_id*mjNDYN]);
+    act = act + act_dot * tau * (1 - mju_exp(-m->opt.timestep / tau));
+  } else {
+    // Euler integration
+    act = act + act_dot * m->opt.timestep;
+  }
+
+  // clamp to actrange
+  if (m->actuator_actlimited[actuator_id]) {
+    mjtNum* actrange = m->actuator_actrange + 2*actuator_id;
+    act = mju_clip(act, actrange[0], actrange[1]);
+  }
+
+  return act;
+}
+
+
 // sum all body masses
 mjtNum mj_getTotalmass(const mjModel* m) {
   mjtNum res = 0;

src/engine/engine_support.h

@@ -106,6 +106,10 @@ MJAPI void mj_normalizeQuat(const mjModel* m, mjtNum* qpos);
 // return 1 if actuator i is disabled, 0 otherwise
 MJAPI int mj_actuatorDisabled(const mjModel* m, int i);
 
+// returns the next activation given current act_dot, after clamping
+mjtNum mj_nextActivation(const mjModel* m, const mjData* d,
+                         int actuator_id, int act_adr, mjtNum act_dot);
+
 // sum all body masses
 MJAPI mjtNum mj_getTotalmass(const mjModel* m);
 

test/engine/engine_derivative_test.cc

@@ -1080,6 +1080,69 @@ TEST_F(DerivativeTest, quatIntegrate) {
   }
 }
 
+// implicit derivatives should use next activation when actearly is set
+TEST_F(DerivativeTest, ActearlyDerivative) {
+  static constexpr char xml[] = R"(
+  <mujoco>
+    <option timestep="1" integrator="implicitfast"/>
+
+    <worldbody>
+      <body>
+        <joint name="early" type="slide"/>
+        <geom type="sphere" size="0.1" mass="1"/>
+      </body>
+      <body pos="1 0 0">
+        <joint name="late" type="slide"/>
+        <geom type="sphere" size="0.1" mass="1"/>
+      </body>
+    </worldbody>
+
+    <actuator>
+      <general joint="early" dyntype="integrator" gaintype="affine"
+               gainprm="1 0 1" actearly="true"/>
+      <general joint="late" dyntype="integrator" gaintype="affine"
+               gainprm="1 0 1" actearly="false"/>
+    </actuator>
+  </mujoco>
+  )";
+
+  char error[1024];
+  mjModel* m = LoadModelFromString(xml, error, sizeof(error));
+  ASSERT_THAT(m, NotNull()) << error;
+  mjData* d = mj_makeData(m);
+
+  // set identical ctrl with zero initial activation
+  d->ctrl[0] = 1.0;
+  d->ctrl[1] = 1.0;
+  d->act[0] = 0.0;
+  d->act[1] = 0.0;
+
+  // step computes derivatives during implicit integration
+  mj_step(m, d);
+
+  // both should have same act_dot
+  EXPECT_EQ(d->act_dot[0], d->act_dot[1]);
+
+  // with actearly=true and nonzero act_dot, derivative should differ
+  // because actearly uses next activation: act + act_dot*dt
+  // for our model: next_act = 0 + 1*1 = 1, current_act = 0
+  // derivative adds gain_vel * act to qDeriv diagonal
+  // for independent bodies, D is diagonal, so diag[i] is at D_rowadr[i]
+  int diag0 = m->D_rowadr[0];  // first joint's diagonal
+  int diag1 = m->D_rowadr[1];  // second joint's diagonal
+  EXPECT_NE(d->qDeriv[diag0], d->qDeriv[diag1])
+      << "actearly=true should use next activation in derivative";
+
+  // verify specific values: gain_vel=1, next_act=1, current_act=0
+  EXPECT_NEAR(d->qDeriv[diag0], 1.0, 1e-10)
+      << "actearly=true should use next_act=1";
+  EXPECT_NEAR(d->qDeriv[diag1], 0.0, 1e-10)
+      << "actearly=false should use current_act=0";
+
+  mj_deleteData(d);
+  mj_deleteModel(m);
+}
+
 // Utility: Rotate flex grid
 void RotateFlexGrid(mjModel* model, mjData* data, const char* flex_name,
                     double angle) {