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:
committed by
Copybara-Service
parent
d011285380
commit
b466829482
@@ -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
|
||||
|
||||
Reference in New Issue
Block a user