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
@@ -725,6 +725,8 @@ typedef struct mjsActuator_ { // actuator specification
|
||||
double dynprm[mjNDYN]; // dynamics parameters
|
||||
int actdim; // number of activation variables
|
||||
int ctrlspec; // input signature, scoped by gaintype; 0: type default
|
||||
double velrange[2]; // range of the velocity-setpoint input (pid)
|
||||
double ffrange[2]; // range of the feedforward input (pid)
|
||||
mjtBool actearly; // apply next activations to qfrc
|
||||
|
||||
// transmission
|
||||
|
||||
@@ -497,6 +497,8 @@
|
||||
XVEC( double, dynprm, mjNDYN ) \
|
||||
X ( int, actdim, 1 ) \
|
||||
X ( int, ctrlspec, 1 ) \
|
||||
XVEC( double, velrange, 2 ) \
|
||||
XVEC( double, ffrange, 2 ) \
|
||||
X ( mjtBool, actearly, 1 ) \
|
||||
X ( mjtTrn, trntype, 1 ) \
|
||||
XVEC( double, gear, 6 ) \
|
||||
|
||||
@@ -247,6 +247,7 @@ typedef enum mjtDyn { // type of actuator dynamics
|
||||
mjDYN_FILTEREXACT, // linear filter: da/dt = (u-a) / tau, with exact integration
|
||||
mjDYN_MUSCLE, // piecewise linear filter with two time constants
|
||||
mjDYN_DCMOTOR, // DC motor electrical dynamics
|
||||
mjDYN_PID, // PID controller states: slew, integral
|
||||
mjDYN_USER // user-defined dynamics type
|
||||
} mjtDyn;
|
||||
|
||||
@@ -257,6 +258,7 @@ typedef enum mjtGain { // type of actuator gain
|
||||
mjGAIN_MUSCLE, // muscle FLV curve computed by mju_muscleGain()
|
||||
mjGAIN_DCMOTOR, // DC motor gain: K or K/R
|
||||
mjGAIN_SO3, // geodesic servo on an SO3 transmission: force = kp * log(error)
|
||||
mjGAIN_PID, // PID controller: position and velocity setpoint inputs
|
||||
mjGAIN_USER // user-defined gain type
|
||||
} mjtGain;
|
||||
|
||||
@@ -277,6 +279,13 @@ typedef enum mjtCtrlChart { // so3 input signature (actuator_ctrlspec): or
|
||||
} mjtCtrlChart;
|
||||
|
||||
|
||||
typedef enum mjtCtrlInput { // servo input signature (actuator_ctrlspec): present-input bits
|
||||
mjINPUT_POS = 1, // position setpoint input
|
||||
mjINPUT_VEL = 2, // velocity setpoint input
|
||||
mjINPUT_FF = 4 // feedforward input
|
||||
} mjtCtrlInput;
|
||||
|
||||
|
||||
typedef enum mjtObj { // type of MujoCo object
|
||||
mjOBJ_UNKNOWN = 0, // unknown object type
|
||||
mjOBJ_BODY, // body
|
||||
|
||||
@@ -1760,6 +1760,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 actuator to PID controller.
|
||||
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 activate damper; return error if any.
|
||||
MJAPI const char* mjs_setToDamper(mjsActuator* actuator, double kv);
|
||||
|
||||
|
||||
Reference in New Issue
Block a user