diff --git a/src/engine/engine_derivative.c b/src/engine/engine_derivative.c
index 00267a3e..23781e1d 100644
--- a/src/engine/engine_derivative.c
+++ b/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
diff --git a/src/user/user_api.cc b/src/user/user_api.cc
index 13b9168c..4b5effaf 100644
--- a/src/user/user_api.cc
+++ b/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 "";
}
diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc
index 5198cc20..f83bd387 100644
--- a/src/user/user_objects.cc
+++ b/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
diff --git a/test/engine/engine_derivative_test.cc b/test/engine/engine_derivative_test.cc
index c32e6b1e..18ae65b3 100644
--- a/test/engine/engine_derivative_test.cc
+++ b/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"(
+
+
+
+
+
+
+
+
+
+
+
+
+ )";
+
+ 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"(
+
+
+
+
+
+
+
+
+
+
+
+
+ )";
+
+ // stateful DC motor with very small te
+ static constexpr char xml_stateful[] = R"(
+
+
+
+
+
+
+
+
+
+
+
+
+ )";
+
+ 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) {
diff --git a/test/engine/engine_forward_test.cc b/test/engine/engine_forward_test.cc
index 3c9a05d4..fce6cf68 100644
--- a/test/engine/engine_forward_test.cc
+++ b/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;
diff --git a/test/engine/testdata/derivative/dcmotor.xml b/test/engine/testdata/derivative/dcmotor.xml
index a1053bb6..3d9e895a 100644
--- a/test/engine/testdata/derivative/dcmotor.xml
+++ b/test/engine/testdata/derivative/dcmotor.xml
@@ -16,6 +16,18 @@
+