Add the pid actuator: setpoint inputs, integral action, slew rate limiting.
<pid kp kv|dampratio [ki imax] [slewmax]> is a PID controller with real position and velocity setpoint inputs on a single force output, plus an optional feedforward input. With a zero velocity setpoint it reproduces <position> bit-exactly; the input signature is any subset of [pos, vel, ff], selected with input="..." and recorded as mjtCtrlInput bits in actuator_ctrlspec; absent setpoint inputs are fixed at zero, so the control vector contains no inert entries. kp and kv are single-sourced in the affine bias parameters (biasprm[1,2]) with no gainprm mirror: every consumer of the position-servo shape (dampratio conversion, inheritrange, qDeriv) reads one location, which is what makes the bit-exact <position> parity possible. Controller state uses dyntype 'pid' with slot-gated activations in the order [slew, integral], following the dcmotor slot idiom: slewmax (dynprm[1]) rate limits the effective position setpoint through an activation holding it; ki (gainprm[0]) integrates the position error -- wrapped on rotational transmissions -- with anti-windup clamping of the integrand at imax (dynprm[0]). Both features require the pos input. Servo input unpacking is shared with the dcmotor controller (unpackServoInputs); per-input ranges are exposed as posrange/velrange/ffrange. This subsumes the functionality of the mujoco.pid plugin with proper activation state: correct under all integrators, visible to keyframes, act sensors and reset. Migration: kp/ki/kd map to kp/ki/kv, plugin imax is in force units (divide by ki), slewmax carries over; the single ctrl becomes input="pos". PiperOrigin-RevId: 957588898 Change-Id: Id2786836ca6e76f58e5b5cc8323fc23be0a53784
This commit is contained in:
committed by
Copybara-Service
parent
7bc1aa9b05
commit
279df98cd0
@@ -1138,8 +1138,9 @@ static void set0(mjModel* m, mjData* d) {
|
||||
mjtNum* biasprm = m->actuator_biasprm + i*mjNBIAS;
|
||||
mjtNum* gainprm = m->actuator_gainprm + i*mjNGAIN;
|
||||
|
||||
// not a position-like actuator: skip
|
||||
if (gainprm[0] != -biasprm[1]) {
|
||||
// not a position-like actuator: skip (PID single-sources kp in biasprm[1])
|
||||
int is_pid = m->actuator_gaintype[i] == mjGAIN_PID;
|
||||
if (!is_pid && gainprm[0] != -biasprm[1]) {
|
||||
continue;
|
||||
}
|
||||
|
||||
@@ -1165,7 +1166,8 @@ static void set0(mjModel* m, mjData* d) {
|
||||
}
|
||||
|
||||
// damping = dampratio * 2 * sqrt(kp * mass)
|
||||
mjtNum damping = biasprm[2] * 2 * mju_sqrt(gainprm[0] * mass);
|
||||
mjtNum kp = is_pid ? -biasprm[1] : gainprm[0];
|
||||
mjtNum damping = biasprm[2] * 2 * mju_sqrt(kp * mass);
|
||||
|
||||
// set biasprm[2] to negative damping
|
||||
biasprm[2] = -damping;
|
||||
|
||||
Reference in New Issue
Block a user