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/user/user_api.cc

@@ -1348,6 +1348,48 @@ const char* mjs_setToOrientation(mjsActuator* actuator, double kp, double kv[1],
 }
 
 
+// Set to PID actuator.
+const char* mjs_setToPID(mjsActuator* actuator, double kp, double kv[1], double dampratio[1],
+                         double ki[1], double imax[1], double slewmax[1], double inheritrange,
+                         int ctrlspec) {
+  if (kv && dampratio) {
+    return "kv and dampratio cannot both be defined";
+  }
+  actuator->biasprm[1] = -kp;
+  if (kv) {
+    if (*kv < 0) return "kv cannot be negative";
+    actuator->biasprm[2] = -(*kv);
+  }
+  if (dampratio) {
+    if (*dampratio < 0) return "dampratio cannot be negative";
+    actuator->biasprm[2] = *dampratio;
+  }
+
+  // controller states: ki in gainprm[0], imax in dynprm[0], slewmax in dynprm[1]
+  double ki_value = ki ? *ki : 0;
+  double slew_value = slewmax ? *slewmax : 0;
+  if (slew_value < 0) return "slewmax cannot be negative";
+  actuator->gainprm[0] = ki_value;
+  actuator->dynprm[1] = slew_value;
+  actuator->dyntype = (ki_value || slew_value) ? mjDYN_PID : mjDYN_NONE;
+  if (ki_value && imax) {
+    actuator->dynprm[0] = *imax;
+  }
+
+  actuator->inheritrange = inheritrange;
+  if (inheritrange > 0) {
+    if (actuator->ctrlrange[0] || actuator->ctrlrange[1]) {
+      return "posrange and inheritrange cannot both be defined";
+    }
+  }
+
+  actuator->ctrlspec = ctrlspec;
+  actuator->gaintype = mjGAIN_PID;
+  actuator->biastype = mjBIAS_AFFINE;
+  return "";
+}
+
+
 
 // Set to velocity actuator.
 const char* mjs_setToVelocity(mjsActuator* actuator, double kv) {

src/user/user_api.h

@@ -189,6 +189,11 @@ MJAPI const char* mjs_setToVelocity(mjsActuator* actuator, double kv);
 MJAPI const char* mjs_setToOrientation(mjsActuator* actuator, double kp, double kv[1],
                                        double dampratio[1], int ctrlspec);
 
+// Set to PID actuator.
+MJAPI const char* mjs_setToPID(mjsActuator* actuator, double kp, double kv[1], double dampratio[1],
+                               double ki[1], double imax[1], double slewmax[1], double inheritrange,
+                               int ctrlspec);
+
 // Set actuator to damper, return error on failure.
 MJAPI const char* mjs_setToDamper(mjsActuator* actuator, double kv);
 

src/user/user_init.c

@@ -342,6 +342,8 @@ void mjs_defaultActuator(mjsActuator* actuator) {
   actuator->dynprm[0] = 1;
   actuator->actdim = -1;
   actuator->ctrlspec = 0;
+  actuator->velrange[0] = actuator->velrange[1] = 0;
+  actuator->ffrange[0] = actuator->ffrange[1] = 0;
 
   // transmission
   actuator->trntype = mjTRN_UNDEFINED;

src/user/user_model.cc

@@ -4066,10 +4066,10 @@ void mjCModel::CopyObjects(mjModel* m) {
     mjuu_copyvec(m->actuator_user+nuser_actuator*i, pac->get_userdata().data(), nuser_actuator);
 
     // per-input arrays, at the actuator's ctrl block
-    for (int j = m->actuator_ctrladr[i];
-         j < m->actuator_ctrladr[i] + m->actuator_ctrlnum[i]; j++) {
-      m->actuator_ctrllimited[j] = (mjtBool)pac->is_ctrllimited();
-      mjuu_copyvec(m->actuator_ctrlrange + 2 * j, pac->ctrlrange, 2);
+    for (int k = 0; k < m->actuator_ctrlnum[i]; k++) {
+      int j = m->actuator_ctrladr[i] + k;
+      m->actuator_ctrllimited[j] = (mjtBool)pac->ctrllimiteds_[k];
+      mjuu_copyvec(m->actuator_ctrlrange + 2 * j, pac->ctrlranges_[k], 2);
     }
 
     // per-output arrays, at the actuator's output block

src/user/user_objects.cc

@@ -7214,12 +7214,69 @@ void mjCActuator::Compile(void) {
     so3_ = true;
   }
 
-  // input signature selection is so3-only
-  if (ctrlspec && gaintype != mjGAIN_SO3) {
-    throw mjCError(this, "input is only available for so3 actuators, actuator '%s' (id = %d)",
+  // PID servo: validate and resolve input block
+  if (gaintype == mjGAIN_PID) {
+    if (biastype != mjBIAS_AFFINE) {
+      throw mjCError(this, "pid requires biastype 'affine' in actuator '%s' (id = %d)",
+                     name.c_str(), id);
+    }
+    if (dyntype != mjDYN_NONE && dyntype != mjDYN_PID) {
+      throw mjCError(this, "pid requires dyntype 'none' or 'pid' in actuator '%s' "
+                     "(id = %d)", name.c_str(), id);
+    }
+    if (dyntype == mjDYN_NONE && gainprm[0]) {
+      throw mjCError(this, "ki (gainprm[0]) requires dyntype 'pid' in actuator '%s' "
+                     "(id = %d)", name.c_str(), id);
+    }
+    if (trntype == mjTRN_BODY) {
+      throw mjCError(this, "pid cannot use body transmission, actuator '%s' (id = %d)",
+                     name.c_str(), id);
+    }
+
+    // controller states, slot order [slew, integral]: gated on slewmax (dynprm[1]) and ki
+    if (dyntype == mjDYN_PID) {
+      if (dynprm[0] < 0) {
+        throw mjCError(this, "imax (dynprm[0]) must be non-negative in actuator '%s' (id = %d)",
+                       name.c_str(), id);
+      }
+      if (dynprm[1] < 0) {
+        throw mjCError(this, "slewmax (dynprm[1]) must be non-negative in actuator '%s' (id = %d)",
+                       name.c_str(), id);
+      }
+      int nslot = (dynprm[1] > 0) + (gainprm[0] > 0);
+      if (actdim > 0 && actdim != nslot) {
+        throw mjCError(this, "pid controller states require matching actdim in actuator '%s' "
+                       "(id = %d)", name.c_str(), id);
+      }
+      actdim = nslot;
+    }
+
+    // input block: any subset of [pos, vel, ff], default [pos, vel]
+    ctrlspec_ = ctrlspec ? ctrlspec : (mjINPUT_POS | mjINPUT_VEL);
+    if (ctrlspec_ & ~(mjINPUT_POS | mjINPUT_VEL | mjINPUT_FF)) {
+      throw mjCError(this, "pid inputs are a subset of [pos, vel, ff] in actuator '%s' (id = %d)",
+                     name.c_str(), id);
+    }
+    if (dyntype == mjDYN_PID && !(ctrlspec_ & mjINPUT_POS)) {
+      throw mjCError(this, "pid controller states require the pos input in actuator '%s' (id = %d)",
+                     name.c_str(), id);
+    }
+    ctrlnum_ = !!(ctrlspec_ & mjINPUT_POS) + !!(ctrlspec_ & mjINPUT_VEL) +
+               !!(ctrlspec_ & mjINPUT_FF);
+  }
+
+  // pid dynamics are pid-only
+  if (dyntype == mjDYN_PID && gaintype != mjGAIN_PID) {
+    throw mjCError(this, "dyntype 'pid' requires gaintype 'pid', actuator '%s' (id = %d)",
                    name.c_str(), id);
   }
 
+  // input signature selection is so3- or pid-only
+  if (ctrlspec && gaintype != mjGAIN_SO3 && gaintype != mjGAIN_PID) {
+    throw mjCError(this, "input is only available for so3 and pid actuators, actuator '%s' "
+                   "(id = %d)", name.c_str(), id);
+  }
+
   // check damping/armature only valid for joint and tendon transmission
   bool has_damping = false;
   for (int i = 0; i < mjNPOLY+1; i++) {
@@ -7242,12 +7299,13 @@ void mjCActuator::Compile(void) {
   }
 
   // handle inheritrange
-  if (gaintype == mjGAIN_FIXED && biastype == mjBIAS_AFFINE &&
-      gainprm[0] == -biasprm[1] && inheritrange > 0) {
+  if (((gaintype == mjGAIN_FIXED && gainprm[0] == -biasprm[1]) || gaintype == mjGAIN_PID) &&
+      biastype == mjBIAS_AFFINE && inheritrange > 0) {
     // semantic of actuator is the same as transmission, inheritrange is applicable
     double* range;
-    if (dyntype == mjDYN_NONE || dyntype == mjDYN_FILTEREXACT) {
-      // position actuator
+    if (dyntype == mjDYN_NONE || dyntype == mjDYN_FILTEREXACT ||
+        dyntype == mjDYN_PID) {
+      // position or pd actuator: range applies to the position input
       range = ctrlrange;
     } else if (dyntype == mjDYN_INTEGRATOR) {
       // intvelocity actuator
@@ -7397,6 +7455,28 @@ void mjCActuator::Compile(void) {
   if (nsample > 16777216) {
     throw mjCError(this, "at most 2^24 samples in history buffer, got %d", nullptr, nsample);
   }
+
+  // resolve per-input control ranges: broadcast ctrlrange, pd overrides vel and ff
+  for (int j=0; j < ctrlnum_ && j < 4; j++) {
+    ctrllimiteds_[j] = (mjtByte)is_ctrllimited();
+    ctrlranges_[j][0] = ctrlrange[0];
+    ctrlranges_[j][1] = ctrlrange[1];
+  }
+  if (gaintype == mjGAIN_PID) {
+    // present inputs pack in canonical order [pos, vel, ff]; pos keeps the ctrlrange broadcast
+    int j = ctrlspec_ & mjINPUT_POS ? 1 : 0;
+    if (ctrlspec_ & mjINPUT_VEL) {
+      ctrllimiteds_[j] = velrange[0] < velrange[1];
+      ctrlranges_[j][0] = velrange[0];
+      ctrlranges_[j][1] = velrange[1];
+      j++;
+    }
+    if (ctrlspec_ & mjINPUT_FF) {
+      ctrllimiteds_[j] = ffrange[0] < ffrange[1];
+      ctrlranges_[j][0] = ffrange[0];
+      ctrlranges_[j][1] = ffrange[1];
+    }
+  }
 }
 
 

src/user/user_objects.h

@@ -1816,6 +1816,8 @@ class mjCActuator_ : public mjCBase {
   int outadr_;                                      // address of first force output
   int outnum_;                                      // number of force outputs, from trntype
   bool so3_;                                        // compiles to an SO3 transmission
+  double ctrlranges_[4][2];                         // resolved per-input control ranges
+  mjtByte ctrllimiteds_[4];                         // resolved per-input limited flags
   std::map<std::string, std::vector<mjtNum>> act_;  // act at the previous step
   std::map<std::string, mjtNum> ctrl_;              // ctrl at the previous step