Correct DC motor derivative calculation and enforce actearly.

PiperOrigin-RevId: 898983657
Change-Id: I008529eacf3e400696d26bd3a52a4cf4c1c12225

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
-  EXPECT_NEAR(data->actuator_force[0], 0.05 * 0.5, MjTol(1e-12, 1e-5));
+  // Force is K * next_activation (actearly is always on for DC motors)
+  // 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>