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

src/engine/engine_forward.c

@@ -225,6 +225,16 @@ void mj_fwdVelocity(const mjModel* m, mjData* d) {
 }
 
 
+// unpack servo-family inputs from control block in canonical order [pos, vel, ff]
+// absent input: setpoint 0
+static void unpackServoInputs(const mjtNum* u, int spec, mjtNum out[3]) {
+  int adr = 0;
+  out[0] = (spec & mjINPUT_POS) ? u[adr++] : 0;
+  out[1] = (spec & mjINPUT_VEL) ? u[adr++] : 0;
+  out[2] = (spec & mjINPUT_FF)  ? u[adr]   : 0;
+}
+
+
 // helper for DC motor: computes control voltage from PID state
 static mjtNum dcmotorVoltage(mjtNum ctrl, mjtNum length, mjtNum velocity,
                              mjtNum x_I, const mjtNum* gainprm) {
@@ -285,10 +295,15 @@ static void expmap2Quat(mjtNum quat[4], const mjtNum v[3]) {
 static mjtNum wrapPeriod(const mjModel* m, int i) {
   // servo shape: fixed gain, affine bias, matching kp, setpoint input
   mjtDyn dyntype = m->actuator_dyntype[i];
-  if (m->actuator_gaintype[i] != mjGAIN_FIXED  ||
-      m->actuator_biastype[i] != mjBIAS_AFFINE ||
-      m->actuator_gainprm[mjNGAIN*i] != -m->actuator_biasprm[mjNBIAS*i+1] ||
-      (dyntype != mjDYN_NONE && dyntype != mjDYN_INTEGRATOR)) {
+  int servo = m->actuator_gaintype[i] == mjGAIN_FIXED  &&
+              m->actuator_biastype[i] == mjBIAS_AFFINE &&
+              m->actuator_gainprm[mjNGAIN*i] == -m->actuator_biasprm[mjNBIAS*i+1] &&
+              (dyntype == mjDYN_NONE || dyntype == mjDYN_INTEGRATOR);
+
+  // PID shape: kp and kv are single-sourced in the affine bias
+  int pid = m->actuator_gaintype[i] == mjGAIN_PID;
+
+  if (!servo && !pid) {
     return 0;
   }
 
@@ -318,6 +333,20 @@ static mjtNum wrapSetpoint(mjtNum u, mjtNum length, mjtNum period) {
 }
 
 
+// slew-rate-limit setpoint u given previous effective setpoint u_prev, write act_dot
+// period > 0: wrap u to the representative nearest u_prev before limiting
+static mjtNum slewLimit(mjtNum u, mjtNum u_prev, mjtNum slew_s, mjtNum dt,
+                        mjtNum period, mjtNum* act_dot) {
+  if (period > 0) {
+    u = wrapSetpoint(u, u_prev, period);
+  }
+  mjtNum slew = slew_s * dt;
+  mjtNum u_eff = mju_clip(u, u_prev - slew, u_prev + slew);
+  *act_dot = (u_eff - u_prev) / dt;
+  return u_eff;
+}
+
+
 // (qpos, qvel, ctrl, act) => (qfrc_actuator, actuator_force, act_dot)
 void mj_fwdActuation(const mjModel* m, mjData* d) {
   TM_START;
@@ -419,6 +448,42 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
       d->act_dot[act_last] = mju_muscleDynamics(ctrl[uadr], d->act[act_last], dynprm);
       break;
 
+    case mjDYN_PID: {               // PID controller states, slot order: slew, integral
+      int adr = act_first;
+      mjtNum period = wrapPeriod(m, i);
+
+      // slew rate limiting of the position setpoint
+      mjtNum slew_s = dynprm[1];
+      if (slew_s > 0) {
+        ctrl[uadr] = slewLimit(ctrl[uadr], d->act[adr], slew_s, m->opt.timestep,
+                               period, d->act_dot + adr);
+        adr++;
+      }
+
+      // integral of the position error
+      if (m->actuator_gainprm[mjNGAIN*i] > 0) {
+        mjtNum err = ctrl[uadr] - d->actuator_length[oadr];
+
+        // rotational transmission: error on the circle
+        if (period > 0) {
+          err -= period*mju_round(err/period);
+        }
+
+        // anti-windup: stop accumulating beyond imax
+        mjtNum imax = dynprm[0];
+        if (imax > 0) {
+          mjtNum z = d->act[adr];
+          if (z >= imax) {
+            err = mju_min(err, 0);
+          } else if (z <= -imax) {
+            err = mju_max(err, 0);
+          }
+        }
+        d->act_dot[adr] = err;
+      }
+      break;
+    }
+
     case mjDYN_DCMOTOR: {           // DC motor: up to 5 optional states
       const mjtNum* gainprm = m->actuator_gainprm + mjNGAIN*i;
 
@@ -630,6 +695,10 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
       gain = gainprm[0];
       break;
 
+    case mjGAIN_PID:                // PID servo: input side handled below, state side in bias
+      gain = 0;
+      break;
+
     case mjGAIN_AFFINE:             // affine: prm = [const, kp, kv]
       gain = gainprm[0] + gainprm[1]*d->actuator_length[oadr] +
              gainprm[2]*d->actuator_velocity[oadr];
@@ -693,7 +762,36 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
 
     // DC motor without current state: use ctrl even if other activations exist
     int dcmotor_no_current = (gaintype == mjGAIN_DCMOTOR && dynprm[0] <= 0);
-    if (actnum == 0 || dcmotor_no_current) {
+
+    // PID servo: force = kp*(qref - l) + kv*(vref - l_dot) [+ ff] [+ ki*z]
+    // input-side terms computed here; state-side terms added by the affine bias below
+    if (gaintype == mjGAIN_PID) {
+      const mjtNum* prm = m->actuator_biasprm + mjNBIAS*i;
+
+      // unpack present inputs in canonical order [pos, vel, ff]; absent input: setpoint 0
+      mjtNum u3[3];
+      unpackServoInputs(ctrl + uadr, m->actuator_ctrlspec[i], u3);
+      mjtNum qref = u3[0], vref = u3[1], ff = u3[2];
+
+      // position setpoint: representative nearest the length on rotational transmissions
+      mjtNum period = wrapPeriod(m, i);
+      if (period > 0) {
+        qref = wrapSetpoint(qref, d->actuator_length[oadr], period);
+      }
+
+      // kp and kv are single-sourced in the affine bias parameters
+      force[oadr] = -prm[1]*qref - prm[2]*vref + ff;
+
+      // integral state (last slot): force += ki * z
+      if (actnum && gainprm[0] > 0) {
+        int act_adr = m->actuator_actadr[i] + actnum - 1;
+        mjtNum z = m->actuator_actearly[i]
+                       ? mj_nextActivation(m, d, i, act_adr, d->act_dot[act_adr])
+                       : d->act[act_adr];
+        force[oadr] += gainprm[0]*z;
+      }
+    }
+    else if (actnum == 0 || dcmotor_no_current) {
       mjtNum input = ctrl[uadr];
 
       // rotational setpoint: use representative nearest the length (local, no state change)

src/engine/engine_name.c

@@ -297,5 +297,20 @@ const char* mj_actuatorInputName(const mjModel* m, int id, int input) {
     return m->actuator_ctrlspec[id] == mjCHART_QUAT ? quat[input] : expmap[input];
   }
 
+  // servo family: input names are the present members of [pos, vel, ff]
+  if (m->actuator_gaintype[id] == mjGAIN_PID) {
+    static const char* servo[3] = {"pos", "vel", "ff"};
+    static const int bits[3] = {mjINPUT_POS, mjINPUT_VEL, mjINPUT_FF};
+    int spec = m->actuator_ctrlspec[id];
+    for (int k=0; k < 3; k++) {
+      if (spec & bits[k]) {
+        if (input == 0) {
+          return servo[k];
+        }
+        input--;
+      }
+    }
+  }
+
   return NULL;
 }

src/engine/engine_setconst.c

@@ -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;