diff --git a/doc/changelog.rst b/doc/changelog.rst index d6a44aad..d084b356 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -126,6 +126,8 @@ Documentation Bug fixes ^^^^^^^^^ +- Fixed a bug in :ref:`implicit integrator` derivatives where actuator velocity derivatives were + incorrectly computed when the force was clamped by :ref:`forcerange`. - 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 diff --git a/src/engine/engine_derivative.c b/src/engine/engine_derivative.c index 7fc7912f..6c37710f 100644 --- a/src/engine/engine_derivative.c +++ b/src/engine/engine_derivative.c @@ -1089,6 +1089,15 @@ void mjd_actuator_vel(const mjModel* m, mjData* d) { continue; } + // skip if force is clamped by forcerange + if (m->actuator_forcelimited[i]) { + mjtNum force = d->actuator_force[i]; + mjtNum* range = m->actuator_forcerange + 2*i; + if (force <= range[0] || force >= range[1]) { + continue; + } + } + mjtNum bias_vel = 0, gain_vel = 0; // affine bias diff --git a/test/engine/engine_derivative_test.cc b/test/engine/engine_derivative_test.cc index 16287ec0..0c991a85 100644 --- a/test/engine/engine_derivative_test.cc +++ b/test/engine/engine_derivative_test.cc @@ -1080,6 +1080,94 @@ TEST_F(DerivativeTest, quatIntegrate) { } } +// implicit integration is better than Euler with active forcerange clamping +TEST_F(DerivativeTest, ForcerangeClampedDerivative) { + static constexpr char xml[] = R"( + + + )"; + + char error[1024]; + mjModel* m = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(m, NotNull()) << error; + + mjtNum dt_small = 1e-4; + mjtNum dt_large = 1e-2; + mjtNum duration = 1.0; + + mjData* d_gt = mj_makeData(m); + mjData* d_implicit = mj_makeData(m); + mjData* d_euler = mj_makeData(m); + + mj_resetData(m, d_gt); + mj_resetData(m, d_implicit); + mj_resetData(m, d_euler); + + d_gt->ctrl[0] = 0.5; + d_implicit->ctrl[0] = 0.5; + d_euler->ctrl[0] = 0.5; + + mjtNum error_implicit = 0; + mjtNum error_euler = 0; + int nsteps_large = static_cast(duration / dt_large); + int substeps = static_cast(dt_large / dt_small); + + m->opt.timestep = dt_large; + + m->opt.integrator = mjINT_IMPLICITFAST; + mj_resetData(m, d_gt); + d_gt->ctrl[0] = 0.5; + m->opt.timestep = dt_small; + m->opt.integrator = mjINT_EULER; + + for (int i = 0; i < nsteps_large; i++) { + // ground truth: small steps with Euler + m->opt.integrator = mjINT_EULER; + m->opt.timestep = dt_small; + for (int j = 0; j < substeps; j++) { + mj_step(m, d_gt); + } + + // euler at large timestep + m->opt.timestep = dt_large; + mj_step(m, d_euler); + + // implicitfast at large timestep + m->opt.integrator = mjINT_IMPLICITFAST; + mj_step(m, d_implicit); + + // accumulate errors + mjtNum diff_implicit = d_gt->qpos[0] - d_implicit->qpos[0]; + mjtNum diff_euler = d_gt->qpos[0] - d_euler->qpos[0]; + error_implicit += diff_implicit * diff_implicit; + error_euler += diff_euler * diff_euler; + } + + // expect implicitfast to be more accurate than Euler + EXPECT_LT(error_implicit, error_euler) + << "implicitfast should be more accurate than Euler at large timestep " + << "when forcerange derivatives are correctly handled"; + + mj_deleteData(d_euler); + mj_deleteData(d_implicit); + mj_deleteData(d_gt); + mj_deleteModel(m); +} + // implicit derivatives should use next activation when actearly is set TEST_F(DerivativeTest, ActearlyDerivative) { static constexpr char xml[] = R"(