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 dVdw = 0;
-        mjtNum R = gainprm[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 s = 1 - mju_exp(-m->opt.timestep / te);
-        mjtNum kp = gainprm[4];
+        bias_vel += K * (dVdw - K) * s / R;
-        mjtNum kd = gainprm[6];
+      } else if (dVdw != 0) {
-        bias_vel -= gain * (input_mode == 1 ? kd : kp);
+        // 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

test/engine/engine_derivative_test.cc

@@ -1308,6 +1308,123 @@ TEST_F(DerivativeTest, ActearlyDerivative) {
   mj_deleteModel(m);
 }
 
+
+// verify stateful DC motor derivative matches analytical formula
+TEST_F(DerivativeTest, DCMotorStatefulDerivative) {
+  static constexpr char xml[] = R"(
+  <mujoco>
+    <option timestep="0.002"/>
+    <worldbody>
+      <body>
+        <joint name="j" type="slide"/>
+        <geom type="sphere" size="0.1" mass="1"/>
+      </body>
+    </worldbody>
+    <actuator>
+      <dcmotor name="dc" joint="j" motorconst="2.0" resistance="0.5"
+               inductance="0 0.001" input="position" controller="10 0 5"/>
+    </actuator>
+  </mujoco>
+  )";
+
+  char error[1024];
+  mjModel* m = LoadModelFromString(xml, error, sizeof(error));
+  ASSERT_THAT(m, NotNull()) << error;
+  mjData* d = mj_makeData(m);
+
+  // set nonzero velocity and ctrl
+  d->qvel[0] = 1.0;
+  d->ctrl[0] = 0.5;
+
+  // forward to compute act_dot, etc.
+  mj_forward(m, d);
+
+  // compute analytical derivatives
+  mjd_smooth_vel(m, d, /* flg_bias = */ 1);
+
+  // extract diagonal of qDeriv
+  mjtNum qDeriv_diag = d->qDeriv[m->D_rowadr[0] + m->D_rownnz[0] - 1];
+
+  // expected: K*(dVdw - K)*(1 - exp(-h/te))/R
+  // with K=2, R=0.5, te=0.001, h=0.002, kd=5, dVdw=-5
+  mjtNum K = 2.0, R = 0.5, te = 0.001, h = 0.002, kd = 5.0;
+  mjtNum expected = K * (-kd - K) * (1 - mju_exp(-h / te)) / R;
+  EXPECT_NEAR(qDeriv_diag, expected, 1e-10)
+      << "stateful DC motor derivative should match analytical formula";
+
+  mj_deleteData(d);
+  mj_deleteModel(m);
+}
+
+
+// verify that stateful DC motor derivative converges to stateless as te -> 0
+TEST_F(DerivativeTest, DCMotorStatefulConvergesToStateless) {
+  // stateless DC motor with position controller
+  static constexpr char xml_stateless[] = R"(
+  <mujoco>
+    <option timestep="0.002"/>
+    <worldbody>
+      <body>
+        <joint name="j" type="slide"/>
+        <geom type="sphere" size="0.1" mass="1"/>
+      </body>
+    </worldbody>
+    <actuator>
+      <dcmotor name="dc" joint="j" motorconst="1.0" resistance="1.0"
+               input="position" controller="10 0 5"/>
+    </actuator>
+  </mujoco>
+  )";
+
+  // stateful DC motor with very small te
+  static constexpr char xml_stateful[] = R"(
+  <mujoco>
+    <option timestep="0.002"/>
+    <worldbody>
+      <body>
+        <joint name="j" type="slide"/>
+        <geom type="sphere" size="0.1" mass="1"/>
+      </body>
+    </worldbody>
+    <actuator>
+      <dcmotor name="dc" joint="j" motorconst="1.0" resistance="1.0"
+               inductance="0 1e-8" input="position" controller="10 0 5"/>
+    </actuator>
+  </mujoco>
+  )";
+
+  char error[1024];
+  mjModel* m_sl = LoadModelFromString(xml_stateless, error, sizeof(error));
+  ASSERT_THAT(m_sl, NotNull()) << error;
+  mjData* d_sl = mj_makeData(m_sl);
+
+  mjModel* m_sf = LoadModelFromString(xml_stateful, error, sizeof(error));
+  ASSERT_THAT(m_sf, NotNull()) << error;
+  mjData* d_sf = mj_makeData(m_sf);
+
+  // set identical state
+  d_sl->qvel[0] = d_sf->qvel[0] = 1.0;
+  d_sl->ctrl[0] = d_sf->ctrl[0] = 0.5;
+
+  // forward and compute derivatives
+  mj_forward(m_sl, d_sl);
+  mj_forward(m_sf, d_sf);
+  mjd_smooth_vel(m_sl, d_sl, 1);
+  mjd_smooth_vel(m_sf, d_sf, 1);
+
+  // extract diagonals
+  mjtNum diag_sl = d_sl->qDeriv[m_sl->D_rowadr[0] + m_sl->D_rownnz[0] - 1];
+  mjtNum diag_sf = d_sf->qDeriv[m_sf->D_rowadr[0] + m_sf->D_rownnz[0] - 1];
+
+  EXPECT_NEAR(diag_sf, diag_sl, 1e-6)
+      << "stateful derivative should converge to stateless as te -> 0";
+
+  mj_deleteData(d_sf);
+  mj_deleteModel(m_sf);
+  mj_deleteData(d_sl);
+  mj_deleteModel(m_sl);
+}
+
 // Utility: Rotate flex grid
 void RotateFlexGrid(mjModel* model, mjData* data, const char* flex_name,
                     double angle) {

test/engine/engine_forward_test.cc

@@ -1805,8 +1805,12 @@ TEST_F(DCMotorTest, StatefulPositionWithCurrentMode) {
   // di/dt = (1.485 - 0.0125 - 0.5) / 0.5 = 0.9725 / 0.5 = 1.945
   EXPECT_NEAR(data->act_dot[adr+2], 1.945, MjTol(1e-12, 1e-5));
 
-  // Force is just K * current since current is stateful
+  // Force is K * next_activation (actearly is always on for DC motors)
-  EXPECT_NEAR(data->actuator_force[0], 0.05 * 0.5, MjTol(1e-12, 1e-5));
+  // Inline mj_nextActivation for te = 0.5
+  mjtNum te = 0.5;
+  mjtNum h = model->opt.timestep;
+  mjtNum next_i = 0.5 + data->act_dot[adr+2] * te * (1 - mju_exp(-h / te));
+  EXPECT_NEAR(data->actuator_force[0], 0.05 * next_i, MjTol(1e-12, 1e-5));
 
   mj_deleteData(data);
   mj_deleteModel(model);
@@ -1902,7 +1906,11 @@ TEST_F(DCMotorTest, CurrentPlusThermal) {
   data->ctrl[0] = V;
   mj_forward(model, data);
 
-  EXPECT_NEAR(data->actuator_force[0], K * current, MjTol(1e-12, 1e-5));
+  // Force uses next_activation (actearly is always on for DC motors)
+  // Inline mj_nextActivation for te = 0.01 / R = 0.005
+  mjtNum h = model->opt.timestep;
+  mjtNum next_i = current + data->act_dot[adr+1] * te * (1 - mju_exp(-h / te));
+  EXPECT_NEAR(data->actuator_force[0], K * next_i, MjTol(1e-12, 1e-5));
 
   double R_hot = R * (1 + 0.004 * dT);
   double T_dot = (R_hot * current * current - dT / RT) / C;

test/engine/testdata/derivative/dcmotor.xml

@@ -16,6 +16,18 @@
       <joint name="joint4"/>
       <geom size=".03"/>
     </body>
+    <body name="motor5" pos="0 0.5 0">
+      <joint name="slide5" type="slide" axis="0 0 1"/>
+      <geom size=".03"/>
+    </body>
+    <body name="motor6" pos="0 0.6 0">
+      <joint name="slide6" type="slide" axis="0 0 1"/>
+      <geom size=".03"/>
+    </body>
+    <body name="motor7" pos="0 0.7 0">
+      <joint name="slide7" type="slide" axis="0 0 1"/>
+      <geom size=".03"/>
+    </body>
   </worldbody>
 
   <actuator>
@@ -31,5 +43,19 @@
     <!-- DC motor with LuGre friction (sigma1 micro-damping) -->
     <dcmotor name="dc_lugre" joint="joint4" motorconst="0.05" resistance="2.0"
              damping="0.001" lugre="1e4 100 0.005 0.008 0.1"/>
+
+    <!-- Stateful current, voltage mode (back-EMF only through act_dot) -->
+    <dcmotor name="dc_stateful_v" joint="slide5"
+             motorconst="2.0" resistance="0.5" inductance="0 0.001"/>
+
+    <!-- Stateful current, position mode (controller + back-EMF through act_dot) -->
+    <dcmotor name="dc_stateful_pos" joint="slide6"
+             motorconst="1.0" resistance="1.0" inductance="0 0.001"
+             input="position" controller="10 0 5"/>
+
+    <!-- Stateful current, velocity mode -->
+    <dcmotor name="dc_stateful_vel" joint="slide7"
+             motorconst="1.0" resistance="1.0" inductance="0 0.001"
+             input="velocity" controller="5 0"/>
   </actuator>
 </mujoco>