chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions 6 Packages Projects Releases Wiki Activity

Fix implicit integrator derivatives when actuator force is clamped.

Browse Source 
The derivatives of actuator velocity with respect to generalized velocities were incorrectly computed when the actuator force was limited by `forcerange`. This CL adds a check to zero out these derivatives when the actuator force is at its upper or lower limit, as the force is no longer a function of velocity in this clamped state.

PiperOrigin-RevId: 868818281
Change-Id: I89742a3f04989dd11f9cfada107200015b1472a7
This commit is contained in:
Yuval Tassa
2026-02-11 13:30:15 -08:00
committed by Copybara-Service
parent d011285380
commit b466829482
3 changed files with 99 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/changelog.rst
+2
View File
@@ -126,6 +126,8 @@ Documentation
Bug fixes
^^^^^^^^^
- Fixed a bug in :ref:`implicit integrator<geIntegrators>` derivatives where actuator velocity derivatives were
incorrectly computed when the force was clamped by :ref:`forcerange<actuator-general-forcerange>`.
- Fixed a bug in :ref:`implicit integrator<geIntegrators>` derivatives where actuator velocity derivatives did not
account for the :ref:`actearly<actuator-general-actearly>` flag.
- Multi threaded mesh processing, enabled by the :ref:`usethread<compiler-usethread>` compiler flag (on by default), was
src/engine/engine_derivative.c
+9
View File
@@ -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
test/engine/engine_derivative_test.cc
+88
View File
@@ -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"(
<mujoco>
<option timestep="0.01" integrator="implicitfast"/>
<worldbody>
<geom name="plane" type="plane" size="2 2 0.1"/>
<light pos="0 0 3"/>
<body name="1" pos="0 0 1">
<joint name="1" type="slide" axis="1 0 0"/>
<geom type="sphere" size="0.1" mass="1"/>
</body>
</worldbody>
<actuator>
<position joint="1" kp="10000" kv="1000" forcerange="-10 10"/>
</actuator>
</mujoco>
)";
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<int>(duration / dt_large);
int substeps = static_cast<int>(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"(