From c004d144d1f659ab37458bad9f04b9caef138449 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Mon, 13 Apr 2026 07:18:28 -0700 Subject: [PATCH] Correct DC motor derivative calculation and enforce `actearly`. PiperOrigin-RevId: 898983657 Change-Id: I008529eacf3e400696d26bd3a52a4cf4c1c12225 --- src/engine/engine_derivative.c | 24 +++- src/user/user_api.cc | 3 + src/user/user_objects.cc | 6 + test/engine/engine_derivative_test.cc | 117 ++++++++++++++++++++ test/engine/engine_forward_test.cc | 14 ++- test/engine/testdata/derivative/dcmotor.xml | 26 +++++ 6 files changed, 181 insertions(+), 9 deletions(-) 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 @@ + + + + + + + + + + + + @@ -31,5 +43,19 @@ + + + + + + + + +