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;

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
 

src/xml/xml_base.h

@@ -41,7 +41,8 @@ extern const int colorspace_sz;
 extern const int builtin_sz;
 extern const int mark_sz;
 extern const int dyn_sz;
-extern const int input_sz;
+extern const int inputchart_sz;
+extern const int inputbit_sz;
 extern const int gain_sz;
 extern const int bias_sz;
 extern const int interp_sz;
@@ -76,7 +77,8 @@ extern const mjMap texrole_map[];
 extern const mjMap builtin_map[];
 extern const mjMap mark_map[];
 extern const mjMap dyn_map[];
-extern const mjMap input_map[];
+extern const mjMap inputchart_map[];
+extern const mjMap inputbit_map[];
 extern const mjMap gain_map[];
 extern const mjMap bias_map[];
 extern const mjMap interp_map[];

src/xml/xml_native_reader.cc

@@ -258,7 +258,7 @@ std::vector<const char*> MJCF[nMJCF] = {
             "margin", "stiffness", "damping", "rgba", "user"},
         {"general", "?", "ctrllimited", "forcelimited", "actlimited", "ctrlrange", "forcerange",
             "actrange", "gear", "damping", "armature", "cranklength", "user", "group", "nsample",
-            "interp", "delay", "actdim", "input", "dyntype", "gaintype", "biastype", "dynprm", "gainprm",
+            "interp", "delay", "actdim", "input", "velrange", "ffrange", "dyntype", "gaintype", "biastype", "dynprm", "gainprm",
             "biasprm", "actearly"},
         {"motor", "?", "ctrllimited", "forcelimited", "ctrlrange", "forcerange",
             "gear", "damping", "armature", "cranklength", "user", "group", "nsample", "interp", "delay"},
@@ -270,6 +270,12 @@ std::vector<const char*> MJCF[nMJCF] = {
         {"intvelocity", "?", "ctrllimited", "forcelimited", "actlimited", "ctrlrange", "forcerange",
              "actrange", "inheritrange", "gear", "damping", "armature", "cranklength", "user", "group",
             "nsample", "interp", "delay", "kp", "kv", "dampratio"},
+        {"orientation", "?", "forcelimited", "ctrlrange", "forcerange", "user", "group",
+            "nsample", "interp", "delay", "kp", "kv", "dampratio", "input"},
+        {"pid", "?", "ctrllimited", "forcelimited", "ctrlrange", "posrange", "velrange", "ffrange",
+            "forcerange", "inheritrange", "gear", "damping", "armature", "cranklength", "user",
+            "group", "nsample", "interp", "delay", "kp", "kv", "dampratio", "ki", "imax", "slewmax",
+            "input"},
         {"damper", "?", "forcelimited", "ctrlrange", "forcerange",
             "gear", "damping", "armature", "cranklength", "user", "group", "nsample", "interp", "delay", "kv"},
         {"cylinder", "?", "ctrllimited", "forcelimited", "ctrlrange", "forcerange",
@@ -476,7 +482,7 @@ std::vector<const char*> MJCF[nMJCF] = {
             "ctrllimited", "forcelimited", "actlimited", "ctrlrange", "forcerange", "actrange",
             "lengthrange", "gear", "damping", "armature", "cranklength", "user",
             "joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite",
-            "body", "actdim", "input", "dyntype", "gaintype", "biastype", "dynprm", "gainprm", "biasprm",
+            "body", "actdim", "input", "velrange", "ffrange", "dyntype", "gaintype", "biastype", "dynprm", "gainprm", "biasprm",
             "actearly"},
         {"motor", "*", "name", "class", "group", "nsample", "interp", "delay",
             "ctrllimited", "forcelimited", "ctrlrange", "forcerange",
@@ -502,6 +508,12 @@ std::vector<const char*> MJCF[nMJCF] = {
             "forcelimited", "ctrlrange", "forcerange", "user",
             "joint", "site", "refsite",
             "kp", "kv", "dampratio", "input"},
+        {"pid", "*", "name", "class", "group", "nsample", "interp", "delay",
+            "ctrllimited", "forcelimited", "ctrlrange", "posrange", "velrange", "ffrange",
+            "forcerange", "inheritrange", "lengthrange", "gear", "damping", "armature",
+            "cranklength", "user",
+            "joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite",
+            "kp", "kv", "dampratio", "ki", "imax", "slewmax", "input"},
         {"damper", "*", "name", "class", "group", "nsample", "interp", "delay",
             "forcelimited", "ctrlrange", "forcerange",
             "lengthrange", "gear", "damping", "armature", "cranklength", "user",
@@ -822,7 +834,7 @@ const mjMap mark_map[mark_sz] = {
 
 
 // dyn type
-const int dyn_sz = 7;
+const int dyn_sz = 8;
 const mjMap dyn_map[dyn_sz] = {
   {"none",          mjDYN_NONE},
   {"integrator",    mjDYN_INTEGRATOR},
@@ -830,6 +842,7 @@ const mjMap dyn_map[dyn_sz] = {
   {"filterexact",   mjDYN_FILTEREXACT},
   {"muscle",        mjDYN_MUSCLE},
   {"dcmotor",       mjDYN_DCMOTOR},
+  {"pid",           mjDYN_PID},
   {"user",          mjDYN_USER}
 };
 
@@ -844,25 +857,61 @@ const mjMap dcmotorinput_map[dcmotorinput_sz] = {
 
 
 // gain type
-const int gain_sz = 6;
+const int gain_sz = 7;
 const mjMap gain_map[gain_sz] = {
   {"fixed",         mjGAIN_FIXED},
   {"affine",        mjGAIN_AFFINE},
   {"muscle",        mjGAIN_MUSCLE},
   {"dcmotor",       mjGAIN_DCMOTOR},
   {"so3",           mjGAIN_SO3},
+  {"pid",           mjGAIN_PID},
   {"user",          mjGAIN_USER}
 };
 
 
 // so3 input chart
-const int input_sz = 2;
-const mjMap input_map[input_sz] = {
+const int inputchart_sz = 2;
+const mjMap inputchart_map[inputchart_sz] = {
   {"expmap",        mjCHART_EXPMAP},
   {"quat",          mjCHART_QUAT}
 };
 
 
+// servo-family input tokens
+const int inputbit_sz = 3;
+const mjMap inputbit_map[inputbit_sz] = {
+  {"pos",           mjINPUT_POS},
+  {"vel",           mjINPUT_VEL},
+  {"ff",            mjINPUT_FF}
+};
+
+
+// read the "input" attribute: so3 chart keyword, or servo input token list
+static bool ReadInputSpec(tinyxml2::XMLElement* elem, int* ctrlspec) {
+  std::string text;
+  if (!mjXUtil::ReadAttrTxt(elem, "input", text)) {
+    return false;
+  }
+
+  // so3 chart keyword
+  int chart = mjXUtil::FindKey(inputchart_map, inputchart_sz, text);
+  if (chart >= 0) {
+    *ctrlspec = chart;
+    return true;
+  }
+
+  // servo input tokens
+  int bits[inputbit_sz];
+  int nbit = mjXUtil::MapValues(elem, "input", bits, inputbit_map, inputbit_sz);
+  int spec = 0;
+  for (int k=0; k < nbit; k++) {
+    spec |= bits[k];
+  }
+  *ctrlspec = spec;
+  return true;
+}
+
+
 // bias type
 const int bias_sz = 6;
 const mjMap bias_map[bias_sz] = {
@@ -2528,9 +2577,9 @@ void mjXReader::OneActuator(XMLElement* elem, mjsActuator* actuator) {
     ReadAttr(elem, "gainprm", mjNGAIN, actuator->gainprm, text, false, false);
     ReadAttr(elem, "biasprm", mjNBIAS, actuator->biasprm, text, false, false);
     ReadAttrInt(elem, "actdim", &actuator->actdim);
-    if (MapValue(elem, "input", &n, input_map, input_sz)) {
-      actuator->ctrlspec = n;
-    }
+    ReadInputSpec(elem, &actuator->ctrlspec);
+    ReadAttr(elem, "velrange", 2, actuator->velrange, text);
+    ReadAttr(elem, "ffrange", 2, actuator->ffrange, text);
   }
 
   // direct drive motor
@@ -2593,14 +2642,53 @@ void mjXReader::OneActuator(XMLElement* elem, mjsActuator* actuator) {
     }
 
     // input chart: expmap (default) or quat
-    int n;
-    if (MapValue(elem, "input", &n, input_map, input_sz)) {
-      actuator->ctrlspec = n;
-    }
+    ReadInputSpec(elem, &actuator->ctrlspec);
 
     err = mjs_setToOrientation(actuator, kp, kv, dampratio, actuator->ctrlspec);
   }
 
+  // PID servo: inputs are position and velocity setpoints
+  else if (type == "pid") {
+    // kp: default inherited via -biasprm[1]
+    double kp = -actuator->biasprm[1];
+    ReadAttr(elem, "kp", 1, &kp, text);
+
+    double kv_data;
+    double *kv = &kv_data;
+    if (!ReadAttr(elem, "kv", 1, kv, text)) {
+      kv = nullptr;
+    }
+
+    double dampratio_data;
+    double *dampratio = &dampratio_data;
+    if (!ReadAttr(elem, "dampratio", 1, dampratio, text)) {
+      dampratio = nullptr;
+    }
+
+    // controller parameters: ki (gainprm[0]), imax (dynprm[0]), slewmax (dynprm[1]); inherited
+    double ki = actuator->gainprm[0] * (actuator->dyntype == mjDYN_PID);
+    ReadAttr(elem, "ki", 1, &ki, text);
+    double imax = actuator->dynprm[0] * (actuator->dyntype == mjDYN_PID);
+    ReadAttr(elem, "imax", 1, &imax, text);
+    double slewmax = actuator->dynprm[1] * (actuator->dyntype == mjDYN_PID);
+    ReadAttr(elem, "slewmax", 1, &slewmax, text);
+
+    // input subset selection
+    ReadInputSpec(elem, &actuator->ctrlspec);
+
+    // per-input ranges; posrange is an alias of ctrlrange (input 0)
+    ReadAttr(elem, "posrange", 2, actuator->ctrlrange, text);
+    ReadAttr(elem, "velrange", 2, actuator->velrange, text);
+    ReadAttr(elem, "ffrange", 2, actuator->ffrange, text);
+
+    // handle inheritrange
+    double inheritrange = actuator->inheritrange;
+    ReadAttr(elem, "inheritrange", 1, &inheritrange, text);
+
+    err = mjs_setToPID(actuator, kp, kv, dampratio, &ki, &imax, &slewmax, inheritrange,
+                      actuator->ctrlspec);
+  }
+
   // velocity servo
   else if (type == "velocity") {
     double kv = actuator->gainprm[0];
@@ -3168,6 +3256,7 @@ void mjXReader::Default(XMLElement* section, const mjsDefault* def, const mjVFS*
              name == "damper"      ||
              name == "intvelocity" ||
              name == "orientation" ||
+             name == "pid"         ||
              name == "cylinder"    ||
              name == "muscle"      ||
              name == "adhesion"    ||

src/xml/xml_native_reader.h

@@ -102,7 +102,7 @@ class mjXReader : public mjXBase {
 };
 
 // MJCF schema
-#define nMJCF 249
+#define nMJCF 252
 extern std::vector<const char*> MJCF[nMJCF];
 
 #endif  // MUJOCO_SRC_XML_XML_NATIVE_READER_H_

src/xml/xml_native_writer.cc

@@ -899,7 +899,20 @@ void mjXWriter::OneActuator(XMLElement* elem, const mjCActuator* actuator, mjCDe
   // non-plugins: write actuator parameters
   else {
     WriteAttrKey(elem, "gaintype", gain_map, gain_sz, actuator->gaintype, def->Actuator().gaintype);
-    WriteAttrKey(elem, "input", input_map, input_sz, actuator->ctrlspec, def->Actuator().ctrlspec);
+    if (actuator->gaintype == mjGAIN_SO3) {
+      WriteAttrKey(elem, "input", inputchart_map, inputchart_sz, actuator->ctrlspec,
+                   def->Actuator().ctrlspec);
+    } else if (actuator->ctrlspec != def->Actuator().ctrlspec) {
+      std::string tokens;
+      for (int k=0; k < inputbit_sz; k++) {
+        if (actuator->ctrlspec & inputbit_map[k].value) {
+          tokens += std::string(tokens.empty() ? "" : " ") + inputbit_map[k].key;
+        }
+      }
+      WriteAttrTxt(elem, "input", tokens);
+    }
+    WriteAttr(elem, "velrange", 2, actuator->velrange, def->Actuator().velrange);
+    WriteAttr(elem, "ffrange", 2, actuator->ffrange, def->Actuator().ffrange);
     WriteAttrKey(elem, "biastype", bias_map, bias_sz, actuator->biastype, def->Actuator().biastype);
     WriteAttr(elem, "gainprm", mjNGAIN, actuator->gainprm, def->Actuator().gainprm, true);
     WriteAttr(elem, "biasprm", mjNBIAS, actuator->biasprm, def->Actuator().biasprm, true);