Correct DC motor derivative calculation and enforce actearly.

PiperOrigin-RevId: 898983657
Change-Id: I008529eacf3e400696d26bd3a52a4cf4c1c12225

src/engine/engine_derivative.c

@@ -1137,14 +1137,26 @@ void mjd_actuator_vel(const mjModel* m, mjData* d) {
     else if (m->actuator_gaintype[i] == mjGAIN_DCMOTOR) {
       const mjtNum* dynprm = m->actuator_dynprm + mjNDYN*i;
       const mjtNum* gainprm = m->actuator_gainprm + mjNGAIN*i;
+      mjtNum te = dynprm[0];
+
+      // controller velocity derivative: dV/dω
       int input_mode = (int)gainprm[8];
-      if (input_mode > 0) {
-        mjtNum R = gainprm[0];
+      mjtNum dVdw = 0;
+      if (input_mode == 1) dVdw = -gainprm[6];       // position: -kd
+      else if (input_mode == 2) dVdw = -gainprm[4];   // velocity: -kp
+
+      if (te > 0) {
+        // stateful current with actearly: d(K*next_act)/dω
+        // includes both back-EMF (-K) and controller (dVdw) through act_dot
+        mjtNum R = mju_max(mjMINVAL, gainprm[0]);
         mjtNum K = gainprm[1];
-        mjtNum gain = (dynprm[0] > 0) ? K : K / mju_max(mjMINVAL, R);
-        mjtNum kp = gainprm[4];
-        mjtNum kd = gainprm[6];
-        bias_vel -= gain * (input_mode == 1 ? kd : kp);
+        mjtNum s = 1 - mju_exp(-m->opt.timestep / te);
+        bias_vel += K * (dVdw - K) * s / R;
+      } else if (dVdw != 0) {
+        // stateless: controller terms only (back-EMF handled in bias block)
+        mjtNum R = mju_max(mjMINVAL, gainprm[0]);
+        mjtNum K = gainprm[1];
+        bias_vel += K * dVdw / R;
       }
 
       // LuGre: force includes -sigma1*z_dot, z_dot = a*z + v

src/user/user_api.cc

@@ -1280,6 +1280,9 @@ const char* mjs_setToDCMotor(mjsActuator* actuator, double motorconst[2], double
   // enforce actlimited = 0; homogeneous bounds are invalid across DC motor states
   actuator->actlimited = 0;
 
+  // DC motor always uses actearly
+  actuator->actearly = 1;
+
   return "";
 }
 

src/user/user_objects.cc

@@ -7238,6 +7238,12 @@ void mjCActuator::Compile(void) {
     actdim = (dyntype != mjDYN_NONE && dyntype != mjDYN_DCMOTOR);
   }
 
+  // DC motor always uses actearly
+  if (dyntype == mjDYN_DCMOTOR && !actearly) {
+    throw mjCError(this, "actearly cannot be false for DC motor actuator '%s' (id = %d)",
+                   name.c_str(), id);
+  }
+
   // check muscle parameters
   for (int i=0; i < 2; i++) {
     // select gain or bias