From 7f74487a26551962115bd0efa82362f8b4ee5227 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Wed, 11 Feb 2026 04:02:43 -0800 Subject: [PATCH] Fix implicit integrator derivatives for `actearly` actuators. The derivative calculation for actuator velocity in implicit integrators now correctly accounts for the `actearly` flag, using the next activation value when `actearly` is true. PiperOrigin-RevId: 868598722 Change-Id: Ia180afb15b31a718170aeaf9d4ac514bb9e6073b --- doc/changelog.rst | 2 + src/engine/engine_derivative.c | 12 +++-- src/engine/engine_forward.c | 30 +------------ src/engine/engine_support.c | 28 ++++++++++++ src/engine/engine_support.h | 4 ++ test/engine/engine_derivative_test.cc | 63 +++++++++++++++++++++++++++ 6 files changed, 108 insertions(+), 31 deletions(-) diff --git a/doc/changelog.rst b/doc/changelog.rst index 5f1c46cd..3d01044e 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -122,6 +122,8 @@ Documentation Bug fixes ^^^^^^^^^ +- Fixed a bug in :ref:`implicit integrator` derivatives where actuator velocity derivatives did not + account for the :ref:`actearly` flag. - Multi threaded mesh processing, enabled by the :ref:`usethread` compiler flag (on by default), was in fact disabled by the flag. Fixing this bug speeds up compilation of mesh-heavy models by (up to) the number of available cores. diff --git a/src/engine/engine_derivative.c b/src/engine/engine_derivative.c index 47552341..7fc7912f 100644 --- a/src/engine/engine_derivative.c +++ b/src/engine/engine_derivative.c @@ -1117,9 +1117,15 @@ void mjd_actuator_vel(const mjModel* m, mjData* d) { if (m->actuator_dyntype[i] == mjDYN_NONE) { bias_vel += gain_vel * d->ctrl[i]; } else { - int act_first = m->actuator_actadr[i]; - int act_last = act_first + m->actuator_actnum[i] - 1; - bias_vel += gain_vel * d->act[act_last]; + int act_adr = m->actuator_actadr[i] + m->actuator_actnum[i] - 1; + mjtNum act = d->act[act_adr]; + + // use next activation if actearly is set (matching forward pass) + if (m->actuator_actearly[i]) { + act = mj_nextActivation(m, d, i, act_adr, d->act_dot[act_adr]); + } + + bias_vel += gain_vel * act; } } diff --git a/src/engine/engine_forward.c b/src/engine/engine_forward.c index 34408b75..5b336890 100644 --- a/src/engine/engine_forward.c +++ b/src/engine/engine_forward.c @@ -260,32 +260,6 @@ void mj_fwdVelocity(const mjModel* m, mjData* d) { } -// returns the next act given the current act_dot, after clamping -static mjtNum nextActivation(const mjModel* m, const mjData* d, - int actuator_id, int act_adr, mjtNum act_dot) { - mjtNum act = d->act[act_adr]; - - if (m->actuator_dyntype[actuator_id] == mjDYN_FILTEREXACT) { - // exact filter integration - // act_dot(0) = (ctrl-act(0)) / tau - // act(h) = act(0) + (ctrl-act(0)) (1 - exp(-h / tau)) - // = act(0) + act_dot(0) * tau * (1 - exp(-h / tau)) - mjtNum tau = mju_max(mjMINVAL, m->actuator_dynprm[actuator_id * mjNDYN]); - act = act + act_dot * tau * (1 - mju_exp(-m->opt.timestep / tau)); - } else { - // Euler integration - act = act + act_dot * m->opt.timestep; - } - - // clamp to actrange - if (m->actuator_actlimited[actuator_id]) { - mjtNum* actrange = m->actuator_actrange + 2 * actuator_id; - act = mju_clip(act, actrange[0], actrange[1]); - } - - return act; -} - // clamp vector to range static void clampVec(mjtNum* vec, const mjtNum* range, const mjtByte* limited, int n, @@ -474,7 +448,7 @@ void mj_fwdActuation(const mjModel* m, mjData* d) { mjtNum act; if (m->actuator_actearly[i]) { - act = nextActivation(m, d, i, act_adr, d->act_dot[act_adr]); + act = mj_nextActivation(m, d, i, act_adr, d->act_dot[act_adr]); } else { act = d->act[act_adr]; } @@ -916,7 +890,7 @@ static void mj_advance(const mjModel* m, mjData* d, int actadr_end = actadr + m->actuator_actnum[i]; for (int j=actadr; j < actadr_end; j++) { // if disabled, set act_dot to 0 - d->act[j] = nextActivation(m, d, i, j, mj_actuatorDisabled(m, i) ? 0 : act_dot[j]); + d->act[j] = mj_nextActivation(m, d, i, j, mj_actuatorDisabled(m, i) ? 0 : act_dot[j]); } } } diff --git a/src/engine/engine_support.c b/src/engine/engine_support.c index 4ad4663e..9f74c0b9 100644 --- a/src/engine/engine_support.c +++ b/src/engine/engine_support.c @@ -704,6 +704,34 @@ int mj_actuatorDisabled(const mjModel* m, int i) { } } + +// returns the next activation given current act_dot, after clamping +mjtNum mj_nextActivation(const mjModel* m, const mjData* d, + int actuator_id, int act_adr, mjtNum act_dot) { + mjtNum act = d->act[act_adr]; + + if (m->actuator_dyntype[actuator_id] == mjDYN_FILTEREXACT) { + // exact filter integration + // act_dot(0) = (ctrl-act(0)) / tau + // act(h) = act(0) + (ctrl-act(0)) (1 - exp(-h / tau)) + // = act(0) + act_dot(0) * tau * (1 - exp(-h / tau)) + mjtNum tau = mju_max(mjMINVAL, m->actuator_dynprm[actuator_id*mjNDYN]); + act = act + act_dot * tau * (1 - mju_exp(-m->opt.timestep / tau)); + } else { + // Euler integration + act = act + act_dot * m->opt.timestep; + } + + // clamp to actrange + if (m->actuator_actlimited[actuator_id]) { + mjtNum* actrange = m->actuator_actrange + 2*actuator_id; + act = mju_clip(act, actrange[0], actrange[1]); + } + + return act; +} + + // sum all body masses mjtNum mj_getTotalmass(const mjModel* m) { mjtNum res = 0; diff --git a/src/engine/engine_support.h b/src/engine/engine_support.h index 280f6003..04125ed3 100644 --- a/src/engine/engine_support.h +++ b/src/engine/engine_support.h @@ -106,6 +106,10 @@ MJAPI void mj_normalizeQuat(const mjModel* m, mjtNum* qpos); // return 1 if actuator i is disabled, 0 otherwise MJAPI int mj_actuatorDisabled(const mjModel* m, int i); +// returns the next activation given current act_dot, after clamping +mjtNum mj_nextActivation(const mjModel* m, const mjData* d, + int actuator_id, int act_adr, mjtNum act_dot); + // sum all body masses MJAPI mjtNum mj_getTotalmass(const mjModel* m); diff --git a/test/engine/engine_derivative_test.cc b/test/engine/engine_derivative_test.cc index 1a050338..16287ec0 100644 --- a/test/engine/engine_derivative_test.cc +++ b/test/engine/engine_derivative_test.cc @@ -1080,6 +1080,69 @@ TEST_F(DerivativeTest, quatIntegrate) { } } +// implicit derivatives should use next activation when actearly is set +TEST_F(DerivativeTest, ActearlyDerivative) { + 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 identical ctrl with zero initial activation + d->ctrl[0] = 1.0; + d->ctrl[1] = 1.0; + d->act[0] = 0.0; + d->act[1] = 0.0; + + // step computes derivatives during implicit integration + mj_step(m, d); + + // both should have same act_dot + EXPECT_EQ(d->act_dot[0], d->act_dot[1]); + + // with actearly=true and nonzero act_dot, derivative should differ + // because actearly uses next activation: act + act_dot*dt + // for our model: next_act = 0 + 1*1 = 1, current_act = 0 + // derivative adds gain_vel * act to qDeriv diagonal + // for independent bodies, D is diagonal, so diag[i] is at D_rowadr[i] + int diag0 = m->D_rowadr[0]; // first joint's diagonal + int diag1 = m->D_rowadr[1]; // second joint's diagonal + EXPECT_NE(d->qDeriv[diag0], d->qDeriv[diag1]) + << "actearly=true should use next activation in derivative"; + + // verify specific values: gain_vel=1, next_act=1, current_act=0 + EXPECT_NEAR(d->qDeriv[diag0], 1.0, 1e-10) + << "actearly=true should use next_act=1"; + EXPECT_NEAR(d->qDeriv[diag1], 0.0, 1e-10) + << "actearly=false should use current_act=0"; + + mj_deleteData(d); + mj_deleteModel(m); +} + // Utility: Rotate flex grid void RotateFlexGrid(mjModel* model, mjData* data, const char* flex_name, double angle) {