Fix implicit integrator derivatives when actuator force is clamped.

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
+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