From 279df98cd0337c85a6ee36f9bb94800be126e792 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Sat, 1 Aug 2026 04:28:10 -0700 Subject: [PATCH] Add the pid actuator: setpoint inputs, integral action, slew rate limiting. 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 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 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 --- doc/APIreference/APItypes.rst | 10 + doc/APIreference/functions.rst | 9 + doc/XMLreference.rst | 284 +++++++++++++++++-- doc/XMLschema.rst | 240 +++++++++++++++++ doc/changelog.rst | 17 ++ doc/includes/references.h | 12 + include/mujoco/mjspec.h | 2 + include/mujoco/mjspecmacro.h | 2 + include/mujoco/mjtype.h | 9 + include/mujoco/mujoco.h | 5 + python/mujoco/introspect/enums.py | 16 +- python/mujoco/introspect/functions.py | 63 +++++ python/mujoco/introspect/structs.py | 16 ++ python/mujoco/specs.cc | 17 ++ python/mujoco/specs_test.py | 9 + src/engine/engine_forward.c | 108 +++++++- src/engine/engine_name.c | 15 ++ src/engine/engine_setconst.c | 8 +- src/user/user_api.cc | 42 +++ src/user/user_api.h | 5 + src/user/user_init.c | 2 + src/user/user_model.cc | 8 +- src/user/user_objects.cc | 94 ++++++- src/user/user_objects.h | 2 + src/xml/xml_base.h | 6 +- src/xml/xml_native_reader.cc | 115 +++++++- src/xml/xml_native_reader.h | 2 +- src/xml/xml_native_writer.cc | 15 +- test/engine/engine_core_smooth_test.cc | 359 ++++++++++++++++++++++++- test/user/user_api_test.cc | 37 ++- unity/Runtime/Bindings/MjBindings.cs | 11 +- wasm/codegen/generated/bindings.cc | 20 +- wasm/codegen/generated/bindings.h | 6 + 33 files changed, 1504 insertions(+), 62 deletions(-) diff --git a/doc/APIreference/APItypes.rst b/doc/APIreference/APItypes.rst index e9388185..3b03ac64 100644 --- a/doc/APIreference/APItypes.rst +++ b/doc/APIreference/APItypes.rst @@ -357,6 +357,16 @@ Orientation input charts of so3 actuators. These values are used in ``m->actuato .. mujoco-include:: mjtCtrlChart +.. _mjtCtrlInput: + +mjtCtrlInput +~~~~~~~~~~~~ + +Input bitflags of servo-family (pd, dcmotor) actuators. These values are used in ``m->actuator_ctrlspec``. + +.. mujoco-include:: mjtCtrlInput + + .. _mjtObj: mjtObj diff --git a/doc/APIreference/functions.rst b/doc/APIreference/functions.rst index 7ef7d097..b5ba77b8 100644 --- a/doc/APIreference/functions.rst +++ b/doc/APIreference/functions.rst @@ -5315,6 +5315,15 @@ Set actuator to velocity servo; return error if any. Set actuator to orientation servo. +.. _mjs_setToPID: + +`mjs_setToPID <#mjs_setToPID>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +.. mujoco-include:: mjs_setToPID + +Set actuator to PID controller. + .. _mjs_setToDamper: `mjs_setToDamper <#mjs_setToDamper>`__ diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index 8decce54..e4450406 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -5664,25 +5664,27 @@ specify them independently. .. _actuator-general-dyntype: -:at:`dyntype`: :at-val:`[none, integrator, filter, filterexact, muscle, user], "none"` +:at:`dyntype`: :at-val:`[none, integrator, filter, filterexact, pid, muscle, user], "none"` Activation dynamics type for the actuator. The available dynamics types were already described in the :ref:`Actuation model ` section. Repeating that description in somewhat different notation (corresponding to the mjModel and mjData fields involved) we have: - =========== ====================================== - Keyword Description - =========== ====================================== - none No internal state - integrator act_dot = ctrl - filter act_dot = (ctrl - act) / dynprm[0] - filterexact Like filter but with exact integration - muscle act_dot = mju_muscleDynamics(...) - user act_dot = mjcb_act_dyn(...) - =========== ====================================== + ============= ====================================== + Keyword Description + ============= ====================================== + none No internal state + integrator act_dot = ctrl + filter act_dot = (ctrl - act) / dynprm[0] + filterexact Like filter but with exact integration + pid act_dot = position error; see :ref:`pid` + dcmotor DC motor electrical dynamics, see :ref:`dcmotor` + muscle act_dot = mju_muscleDynamics(...) + user act_dot = mjcb_act_dyn(...) + ============= ====================================== .. _actuator-general-gaintype: -:at:`gaintype`: :at-val:`[fixed, affine, muscle, so3, user], "fixed"` +:at:`gaintype`: :at-val:`[fixed, affine, muscle, pid, so3, user], "fixed"` The gain and bias together determine the output of the force generation mechanism, which is currently assumed to be affine. As already explained in :ref:`Actuation model `, the general formula is: scalar_force = gain_term \* (act or ctrl) + bias_term. @@ -5695,6 +5697,7 @@ specify them independently. fixed gain_term = gainprm[0] affine gain_term = gain_prm[0] + gain_prm[1]*length + gain_prm[2]*velocity muscle gain_term = mju_muscleGain(...) + pid PID controller with setpoint inputs, see :ref:`pid` so3 geodesic orientation servo, computed jointly over 3 force outputs, see :ref:`orientation` user gain_term = mjcb_act_gain(...) ======= =============================== @@ -5739,12 +5742,23 @@ specify them independently. so the user can enter as many parameters as needed. These defaults are not compatible with muscle actuators; see :ref:`muscle ` below. +.. _actuator-general-velrange: + +:at:`velrange`: :at-val:`real(2), "0 0"` + Range of the velocity-setpoint input of a :ref:`pid` actuator. + +.. _actuator-general-ffrange: + +:at:`ffrange`: :at-val:`real(2), "0 0"` + Range of the feedforward input of a :ref:`pid` actuator. + .. _actuator-general-input: :at:`input`: :at-val:`string, optional` Input signature of the actuator: which controls make up its control block, recorded in - ``mjModel.actuator_ctrlspec``. Available for gaintype "so3", where it selects the orientation chart: "expmap" - (3 controls, the default) or "quat" (4 controls); see :ref:`orientation/input`. + ``mjModel.actuator_ctrlspec``. For gaintype "so3" it selects the orientation chart: "expmap" (3 controls, the + default) or "quat" (4 controls); see :ref:`orientation/input`. For gaintype "pid" it is + a token list selecting the input subset; see :ref:`pid/input`. .. _actuator-general-actearly: @@ -5954,6 +5968,146 @@ This element has one custom attribute in addition to the common attributes: :ref:`position` attribute and in the :ref:`default class`, saved XMLs always convert it to explicit :at:`ctrlrange` at the actuator. +.. _actuator-pid: + +:el-prefix:`actuator/` |-| **pid** |*| +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +This element creates a PID controller with position and velocity setpoint inputs on a single force output, with optional +integral action and feedforward. With the default input signature ``[pos, vel]`` the force is +:math:`k_p (u_{pos} - l) + k_v (u_{vel} - v)` where :math:`l, v` are the actuator length and velocity; with a zero +velocity setpoint this is identical to :ref:`position`. The input signature is any subset of +``[pos, vel, ff]``, selected by :ref:`input`: an absent setpoint input is fixed at zero, and the +``ff`` input adds a feedforward force. Integral action is enabled by :ref:`ki`: the position error is integrated in +:ref:`act` and contributes :math:`k_i \cdot act` to the force, with anti-windup clamping by +:ref:`imax`. +:ref:`slewmax` limits the rate of change of the effective position setpoint. Each of these +features, when enabled, adds one activation state, in the order [slew, integral]. The underlying +:ref:`general` attributes are set as follows: + +========= ===================== ========= ========= +Attribute Setting Attribute Setting +========= ===================== ========= ========= +dyntype none or pid dynprm imax 0 0 +gaintype pid gainprm ki 0 0 +biastype affine biasprm 0 -kp -kv +========= ===================== ========= ========= + +This element has custom attributes in addition to the common attributes: + +.. _actuator-pid-name: + +.. _actuator-pid-class: + +.. _actuator-pid-group: + +.. _actuator-pid-nsample: + +.. _actuator-pid-interp: + +.. _actuator-pid-delay: + +.. _actuator-pid-ctrllimited: + +.. _actuator-pid-forcelimited: + +.. _actuator-pid-ctrlrange: + +.. _actuator-pid-forcerange: + +.. _actuator-pid-lengthrange: + +.. _actuator-pid-gear: + +.. _actuator-pid-damping: + +.. _actuator-pid-armature: + +.. _actuator-pid-cranklength: + +.. _actuator-pid-user: + +.. _actuator-pid-joint: + +.. _actuator-pid-jointinparent: + +.. _actuator-pid-tendon: + +.. _actuator-pid-slidersite: + +.. _actuator-pid-cranksite: + +.. _actuator-pid-site: + +.. _actuator-pid-refsite: + +.. _actuator-pid-kp: + +:at:`kp`: :at-val:`real, "1"` + Position feedback gain. + +.. _actuator-pid-kv: + +:at:`kv`: :at-val:`real, "0"` + Velocity feedback gain: applied to the velocity error when the ``vel`` input is present, and as pure damping + otherwise. When using this attribute, it is recommended to use the implicitfast or implicit + :ref:`integrators`. + +.. _actuator-pid-dampratio: + +:at:`dampratio`: :at-val:`real, "0"` + Damping applied by the actuator, using damping ratio units, as for + :ref:`position/dampratio`. This attribute is exclusive with :at:`kv`. + +.. _actuator-pid-ki: + +:at:`ki`: :at-val:`real, "0"` + Integral gain. A nonzero value enables integral action: the position error is integrated in + :ref:`act` (:ref:`dyntype` "pid") and contributes :math:`k_i \cdot act` + to the force. + Requires the ``pos`` input. + +.. _actuator-pid-imax: + +:at:`imax`: :at-val:`real, "0"` + Anti-windup limit on the integral state: accumulation stops beyond ±\ :at:`imax`. The default value 0 means + "unclamped". + +.. _actuator-pid-slewmax: + +:at:`slewmax`: :at-val:`real, "0"` + Maximum rate of change of the effective position setpoint. When positive, the commanded setpoint is rate-limited + through an activation state holding the effective setpoint, as for the + :ref:`dcmotor controller`. The default value 0 means "unlimited". + +.. _actuator-pid-input: + +:at:`input`: :at-val:`string, "pos vel"` + Input signature: a space-separated subset of the tokens "pos", "vel" and "ff", packed in this canonical order. + Absent setpoint inputs are fixed at zero, so the control vector contains no inert entries. + +.. _actuator-pid-posrange: + +:at:`posrange`: :at-val:`real(2), "0 0"` + Range of the position-setpoint input; an alias of :ref:`ctrlrange` (the first + input). + +.. _actuator-pid-velrange: + +:at:`velrange`: :at-val:`real(2), "0 0"` + Range of the velocity-setpoint input. + +.. _actuator-pid-ffrange: + +:at:`ffrange`: :at-val:`real(2), "0 0"` + Range of the feedforward input. + +.. _actuator-pid-inheritrange: + +:at:`inheritrange`: :at-val:`real, "0"` + Identical to :ref:`position/inheritrange`, setting :at:`posrange` from the + transmission target's :at:`range`. + .. _actuator-orientation: :el-prefix:`actuator/` |-| **orientation** |*| @@ -5976,7 +6130,7 @@ preserving its direction; the lower bound must be 0. :ref:`Actuator sensors` report one value per force output. The integrator variant, which stores the orientation setpoint in :ref:`act`, is available via :ref:`general` with :ref:`dyntype` "integrator" and is expmap-only. The video on the right shows this `example -model `__. +model `__. The underlying :el:`general` attributes are set as follows: ========= ======= ========= ========= @@ -6056,9 +6210,8 @@ This element has custom attributes in addition to the common attributes: :el-prefix:`actuator/` |-| **velocity** |*| ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -This element creates a velocity servo. Note that in order to create a PD controller, one has to define two actuators: a -position servo and a velocity servo. This is because MuJoCo actuators are SISO while a PD controller takes two control -inputs (reference position and reference velocity). +This element creates a velocity servo. Note that a PD controller with both position and velocity setpoint inputs is +provided by the :ref:`pid` actuator. When using this actuator, it is recommended to use the implicitfast or implicit :ref:`integrators`. The underlying :el:`general` attributes are set as follows: @@ -6802,7 +6955,7 @@ Associate this actuator with an :ref:`engine plugin`. Either :at:`plug .. _actuator-plugin-dyntype: -:at:`dyntype`: :at-val:`[none, integrator, filter, filterexact, muscle, user], "none"` +:at:`dyntype`: :at-val:`[none, integrator, filter, filterexact, pid, muscle, user], "none"` Activation dynamics type for the actuator. The available dynamics types were already described in the :ref:`Actuation model ` section. If :ref:`dyntype` is not "none", an activation variable will be added to the actuator. This variable will be added after any activation state computed by the plugin (see @@ -10020,6 +10173,10 @@ if omitted. .. _default-general-biasprm: +.. _default-general-velrange: + +.. _default-general-ffrange: + .. _default-general-input: .. _default-general-actearly: @@ -10199,6 +10356,95 @@ All :ref:`intvelocity ` attributes are available here exce site, refsite, tendon, slidersite, cranksite. +.. _default-pid: + +.. _default-pid-ctrllimited: + +.. _default-pid-forcelimited: + +.. _default-pid-ctrlrange: + +.. _default-pid-posrange: + +.. _default-pid-velrange: + +.. _default-pid-ffrange: + +.. _default-pid-forcerange: + +.. _default-pid-inheritrange: + +.. _default-pid-gear: + +.. _default-pid-damping: + +.. _default-pid-armature: + +.. _default-pid-cranklength: + +.. _default-pid-user: + +.. _default-pid-group: + +.. _default-pid-nsample: + +.. _default-pid-interp: + +.. _default-pid-delay: + +.. _default-pid-kp: + +.. _default-pid-kv: + +.. _default-pid-dampratio: + +.. _default-pid-ki: + +.. _default-pid-imax: + +.. _default-pid-slewmax: + +.. _default-pid-input: + +:el-prefix:`default/` |-| **pid** |?| +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +All :ref:`pid ` attributes are available here except: name, class, joint, jointinparent, +site, refsite, tendon, slidersite, cranksite. + + +.. _default-orientation: + +.. _default-orientation-forcelimited: + +.. _default-orientation-ctrlrange: + +.. _default-orientation-forcerange: + +.. _default-orientation-user: + +.. _default-orientation-group: + +.. _default-orientation-nsample: + +.. _default-orientation-interp: + +.. _default-orientation-delay: + +.. _default-orientation-kp: + +.. _default-orientation-kv: + +.. _default-orientation-dampratio: + +.. _default-orientation-input: + +:el-prefix:`default/` |-| **orientation** |?| +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +All :ref:`orientation ` attributes are available here except: name, class, joint, site, refsite. + + .. _default-damper: .. _default-damper-forcelimited: diff --git a/doc/XMLschema.rst b/doc/XMLschema.rst index 0fb12668..e940fb5f 100644 --- a/doc/XMLschema.rst +++ b/doc/XMLschema.rst @@ -2378,6 +2378,12 @@ .. grid-item:: :ref:`input` + .. grid-item:: + :ref:`velrange` + + .. grid-item:: + :ref:`ffrange` + .. grid-item:: :ref:`dyntype` @@ -2793,6 +2799,114 @@ :ref:`input` + .. dropdown:: :ref:`pid` |*| + + .. grid:: 2 3 4 4 + :gutter: 0 + + .. grid-item:: + :ref:`name` + + .. grid-item:: + :ref:`class` + + .. grid-item:: + :ref:`group` + + .. grid-item:: + :ref:`nsample` + + .. grid-item:: + :ref:`interp` + + .. grid-item:: + :ref:`delay` + + .. grid-item:: + :ref:`ctrllimited` + + .. grid-item:: + :ref:`forcelimited` + + .. grid-item:: + :ref:`ctrlrange` + + .. grid-item:: + :ref:`posrange` + + .. grid-item:: + :ref:`velrange` + + .. grid-item:: + :ref:`ffrange` + + .. grid-item:: + :ref:`forcerange` + + .. grid-item:: + :ref:`inheritrange` + + .. grid-item:: + :ref:`lengthrange` + + .. grid-item:: + :ref:`gear` + + .. grid-item:: + :ref:`damping` + + .. grid-item:: + :ref:`armature` + + .. grid-item:: + :ref:`cranklength` + + .. grid-item:: + :ref:`user` + + .. grid-item:: + :ref:`joint` + + .. grid-item:: + :ref:`jointinparent` + + .. grid-item:: + :ref:`tendon` + + .. grid-item:: + :ref:`slidersite` + + .. grid-item:: + :ref:`cranksite` + + .. grid-item:: + :ref:`site` + + .. grid-item:: + :ref:`refsite` + + .. grid-item:: + :ref:`kp` + + .. grid-item:: + :ref:`kv` + + .. grid-item:: + :ref:`dampratio` + + .. grid-item:: + :ref:`ki` + + .. grid-item:: + :ref:`imax` + + .. grid-item:: + :ref:`slewmax` + + .. grid-item:: + :ref:`input` + + .. dropdown:: :ref:`damper` |*| .. grid:: 2 3 4 4 @@ -5927,6 +6041,12 @@ .. grid-item:: :ref:`input` + .. grid-item:: + :ref:`velrange` + + .. grid-item:: + :ref:`ffrange` + .. grid-item:: :ref:`dyntype` @@ -6165,6 +6285,126 @@ :ref:`dampratio` + .. dropdown:: :ref:`orientation` :octicon:`dot` + + .. grid:: 2 3 4 4 + :gutter: 0 + + .. grid-item:: + :ref:`forcelimited` + + .. grid-item:: + :ref:`ctrlrange` + + .. grid-item:: + :ref:`forcerange` + + .. grid-item:: + :ref:`user` + + .. grid-item:: + :ref:`group` + + .. grid-item:: + :ref:`nsample` + + .. grid-item:: + :ref:`interp` + + .. grid-item:: + :ref:`delay` + + .. grid-item:: + :ref:`kp` + + .. grid-item:: + :ref:`kv` + + .. grid-item:: + :ref:`dampratio` + + .. grid-item:: + :ref:`input` + + + .. dropdown:: :ref:`pid` :octicon:`dot` + + .. grid:: 2 3 4 4 + :gutter: 0 + + .. grid-item:: + :ref:`ctrllimited` + + .. grid-item:: + :ref:`forcelimited` + + .. grid-item:: + :ref:`ctrlrange` + + .. grid-item:: + :ref:`posrange` + + .. grid-item:: + :ref:`velrange` + + .. grid-item:: + :ref:`ffrange` + + .. grid-item:: + :ref:`forcerange` + + .. grid-item:: + :ref:`inheritrange` + + .. grid-item:: + :ref:`gear` + + .. grid-item:: + :ref:`damping` + + .. grid-item:: + :ref:`armature` + + .. grid-item:: + :ref:`cranklength` + + .. grid-item:: + :ref:`user` + + .. grid-item:: + :ref:`group` + + .. grid-item:: + :ref:`nsample` + + .. grid-item:: + :ref:`interp` + + .. grid-item:: + :ref:`delay` + + .. grid-item:: + :ref:`kp` + + .. grid-item:: + :ref:`kv` + + .. grid-item:: + :ref:`dampratio` + + .. grid-item:: + :ref:`ki` + + .. grid-item:: + :ref:`imax` + + .. grid-item:: + :ref:`slewmax` + + .. grid-item:: + :ref:`input` + + .. dropdown:: :ref:`damper` :octicon:`dot` .. grid:: 2 3 4 4 diff --git a/doc/changelog.rst b/doc/changelog.rst index 1d6a3a6b..beaf5997 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -5,6 +5,23 @@ Changelog Upcoming version (not yet released) ----------------------------------- +Actuation +^^^^^^^^^ +- Added the :ref:`pid` actuator: a PID controller with real position and velocity setpoint inputs, + optional integral action (:ref:`ki`, integrating the position error with + :ref:`imax` anti-windup), setpoint rate limiting (:ref:`slewmax`), and an + optional feedforward input. This subsumes the functionality of the ``mujoco.pid`` plugin with proper activation + state: correct under all integrators and visible to keyframes and sensors. With a zero velocity setpoint it is + identical to :ref:`position`. The input signature is any subset of ``[pos, vel, ff]``, selected + by :ref:`input`; absent setpoint inputs are fixed at zero, so the control vector contains no + inert entries. + +.. admonition:: Breaking ABI changes + :class: caution + + - :ref:`mjsActuator` gained ``velrange`` and ``ffrange`` fields, changing its size and layout. The :ref:`mjtGain` + and :ref:`mjtDyn` enums gained ``pid`` members, shifting the values of ``mjGAIN_USER`` and ``mjDYN_USER``. + Engine ^^^^^^ diff --git a/doc/includes/references.h b/doc/includes/references.h index 8ed1b8cf..9acae221 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -2244,6 +2244,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 @@ -2513,6 +2515,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; typedef enum mjtGain { // type of actuator gain @@ -2521,6 +2524,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; typedef enum mjtBias { // type of actuator bias @@ -2535,6 +2539,11 @@ typedef enum mjtCtrlChart { // so3 input signature (actuator_ctrlspec): or mjCHART_EXPMAP = 1, // exponential-map orientation target: 3 controls mjCHART_QUAT = 2 // quaternion orientation target: 4 controls } 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 @@ -3989,6 +3998,9 @@ const char* mjs_setToIntVelocity(mjsActuator* actuator, double kp, double kv[1], const char* mjs_setToVelocity(mjsActuator* actuator, double kv); const char* mjs_setToOrientation(mjsActuator* actuator, double kp, double kv[1], double dampratio[1], int ctrlspec); +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); const char* mjs_setToDamper(mjsActuator* actuator, double kv); const char* mjs_setToCylinder(mjsActuator* actuator, double timeconst, double bias, double area, double diameter); diff --git a/include/mujoco/mjspec.h b/include/mujoco/mjspec.h index 1d682054..5c0c64a7 100644 --- a/include/mujoco/mjspec.h +++ b/include/mujoco/mjspec.h @@ -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 diff --git a/include/mujoco/mjspecmacro.h b/include/mujoco/mjspecmacro.h index ede71163..b7a9ff10 100644 --- a/include/mujoco/mjspecmacro.h +++ b/include/mujoco/mjspecmacro.h @@ -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 ) \ diff --git a/include/mujoco/mjtype.h b/include/mujoco/mjtype.h index f5697e2f..65c99e15 100644 --- a/include/mujoco/mjtype.h +++ b/include/mujoco/mjtype.h @@ -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 diff --git a/include/mujoco/mujoco.h b/include/mujoco/mujoco.h index 27c33712..3dd2d36b 100644 --- a/include/mujoco/mujoco.h +++ b/include/mujoco/mujoco.h @@ -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); diff --git a/python/mujoco/introspect/enums.py b/python/mujoco/introspect/enums.py index 03ef94c8..b235fcd8 100644 --- a/python/mujoco/introspect/enums.py +++ b/python/mujoco/introspect/enums.py @@ -266,7 +266,8 @@ ENUMS: Mapping[str, EnumDecl] = dict([ ('mjDYN_FILTEREXACT', 3), ('mjDYN_MUSCLE', 4), ('mjDYN_DCMOTOR', 5), - ('mjDYN_USER', 6), + ('mjDYN_PID', 6), + ('mjDYN_USER', 7), ]), )), ('mjtGain', @@ -279,7 +280,8 @@ ENUMS: Mapping[str, EnumDecl] = dict([ ('mjGAIN_MUSCLE', 2), ('mjGAIN_DCMOTOR', 3), ('mjGAIN_SO3', 4), - ('mjGAIN_USER', 5), + ('mjGAIN_PID', 5), + ('mjGAIN_USER', 6), ]), )), ('mjtBias', @@ -304,6 +306,16 @@ ENUMS: Mapping[str, EnumDecl] = dict([ ('mjCHART_QUAT', 2), ]), )), + ('mjtCtrlInput', + EnumDecl( + name='mjtCtrlInput', + declname='enum mjtCtrlInput', + values=dict([ + ('mjINPUT_POS', 1), + ('mjINPUT_VEL', 2), + ('mjINPUT_FF', 4), + ]), + )), ('mjtObj', EnumDecl( name='mjtObj', diff --git a/python/mujoco/introspect/functions.py b/python/mujoco/introspect/functions.py index 00d0d670..4e61aa68 100644 --- a/python/mujoco/introspect/functions.py +++ b/python/mujoco/introspect/functions.py @@ -11035,6 +11035,69 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ ), doc='Set actuator to orientation servo.', )), + ('mjs_setToPID', + FunctionDecl( + name='mjs_setToPID', + return_type=PointerType( + inner_type=ValueType(name='char', is_const=True), + ), + parameters=( + FunctionParameterDecl( + name='actuator', + type=PointerType( + inner_type=ValueType(name='mjsActuator'), + ), + ), + FunctionParameterDecl( + name='kp', + type=ValueType(name='double'), + ), + FunctionParameterDecl( + name='kv', + type=ArrayType( + inner_type=ValueType(name='double'), + extents=(1,), + ), + ), + FunctionParameterDecl( + name='dampratio', + type=ArrayType( + inner_type=ValueType(name='double'), + extents=(1,), + ), + ), + FunctionParameterDecl( + name='ki', + type=ArrayType( + inner_type=ValueType(name='double'), + extents=(1,), + ), + ), + FunctionParameterDecl( + name='imax', + type=ArrayType( + inner_type=ValueType(name='double'), + extents=(1,), + ), + ), + FunctionParameterDecl( + name='slewmax', + type=ArrayType( + inner_type=ValueType(name='double'), + extents=(1,), + ), + ), + FunctionParameterDecl( + name='inheritrange', + type=ValueType(name='double'), + ), + FunctionParameterDecl( + name='ctrlspec', + type=ValueType(name='int'), + ), + ), + doc='Set actuator to PID controller.', + )), ('mjs_setToDamper', FunctionDecl( name='mjs_setToDamper', diff --git a/python/mujoco/introspect/structs.py b/python/mujoco/introspect/structs.py index 5057571c..ddeface3 100644 --- a/python/mujoco/introspect/structs.py +++ b/python/mujoco/introspect/structs.py @@ -9647,6 +9647,22 @@ STRUCTS: Mapping[str, StructDecl] = dict([ type=ValueType(name='int'), doc='input signature, scoped by gaintype; 0: type default', ), + StructFieldDecl( + name='velrange', + type=ArrayType( + inner_type=ValueType(name='double'), + extents=(2,), + ), + doc='range of the velocity-setpoint input (pid)', + ), + StructFieldDecl( + name='ffrange', + type=ArrayType( + inner_type=ValueType(name='double'), + extents=(2,), + ), + doc='range of the feedforward input (pid)', + ), StructFieldDecl( name='actearly', type=ValueType(name='mjtBool'), diff --git a/python/mujoco/specs.cc b/python/mujoco/specs.cc index 94372f5e..68b5c4ac 100644 --- a/python/mujoco/specs.cc +++ b/python/mujoco/specs.cc @@ -1567,6 +1567,23 @@ PYBIND11_MODULE(_specs, m, pybind11::mod_gil_not_used()) { }, py::arg("kp"), py::arg("kv") = -1, py::arg("dampratio") = -1, py::arg("ctrlspec") = 0); + mjsActuator.def( + "set_to_pid", + [](raw::MjsActuator* self, double kp, double kv, double dampratio, + double ki, double imax, double slewmax, double inheritrange, + int ctrlspec) { + std::string err = mjs_setToPID( + self, kp, kv == -1 ? nullptr : &kv, + dampratio == -1 ? nullptr : &dampratio, ki == -1 ? nullptr : &ki, + imax == -1 ? nullptr : &imax, slewmax == -1 ? nullptr : &slewmax, + inheritrange, ctrlspec); + if (!err.empty()) { + throw pybind11::value_error(err); + } + }, + py::arg("kp"), py::arg("kv") = -1, py::arg("dampratio") = -1, + py::arg("ki") = -1, py::arg("imax") = -1, py::arg("slewmax") = -1, + py::arg("inheritrange") = 0, py::arg("ctrlspec") = 0); mjsActuator.def( "set_to_damper", [](raw::MjsActuator* self, double kv) { diff --git a/python/mujoco/specs_test.py b/python/mujoco/specs_test.py index 0b6fa50c..5c107ba3 100644 --- a/python/mujoco/specs_test.py +++ b/python/mujoco/specs_test.py @@ -1596,6 +1596,15 @@ class SpecsTest(absltest.TestCase): self.assertEqual(actuator.biastype, mujoco.mjtBias.mjBIAS_SO3) self.assertEqual(actuator.dyntype, mujoco.mjtDyn.mjDYN_NONE) + actuator.set_to_pid(kp=2.0, kv=3.0, ki=0.5, imax=1.5, slewmax=4.0) + self.assertEqual(actuator.biasprm[1], -2) + self.assertEqual(actuator.biasprm[2], -3) + self.assertEqual(actuator.gainprm[0], 0.5) + self.assertEqual(actuator.dynprm[0], 1.5) + self.assertEqual(actuator.dynprm[1], 4.0) + self.assertEqual(actuator.gaintype, mujoco.mjtGain.mjGAIN_PID) + self.assertEqual(actuator.dyntype, mujoco.mjtDyn.mjDYN_PID) + actuator.set_to_velocity(kv=5.0) self.assertEqual(actuator.gainprm[0], 5) self.assertEqual(actuator.biasprm[2], -5) diff --git a/src/engine/engine_forward.c b/src/engine/engine_forward.c index 15d87ef6..2d3bec9f 100644 --- a/src/engine/engine_forward.c +++ b/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) diff --git a/src/engine/engine_name.c b/src/engine/engine_name.c index caae8bcd..f3bf8c57 100644 --- a/src/engine/engine_name.c +++ b/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; } diff --git a/src/engine/engine_setconst.c b/src/engine/engine_setconst.c index 610f13ee..3d0034ae 100644 --- a/src/engine/engine_setconst.c +++ b/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; diff --git a/src/user/user_api.cc b/src/user/user_api.cc index 1f1bc4ee..d2d51abf 100644 --- a/src/user/user_api.cc +++ b/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) { diff --git a/src/user/user_api.h b/src/user/user_api.h index 0375c033..291b1ab5 100644 --- a/src/user/user_api.h +++ b/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); diff --git a/src/user/user_init.c b/src/user/user_init.c index 4ae97ff7..a1685191 100644 --- a/src/user/user_init.c +++ b/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; diff --git a/src/user/user_model.cc b/src/user/user_model.cc index 9f46d8d4..ce9170ae 100644 --- a/src/user/user_model.cc +++ b/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 diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index f0582cbb..a4dafa2a 100644 --- a/src/user/user_objects.cc +++ b/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]; + } + } } diff --git a/src/user/user_objects.h b/src/user/user_objects.h index 78e1d322..d64da8fe 100644 --- a/src/user/user_objects.h +++ b/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> act_; // act at the previous step std::map ctrl_; // ctrl at the previous step diff --git a/src/xml/xml_base.h b/src/xml/xml_base.h index 00b02e89..2bef9a20 100644 --- a/src/xml/xml_base.h +++ b/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[]; diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index 6901d261..9855c85d 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -258,7 +258,7 @@ std::vector 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 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 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 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" || diff --git a/src/xml/xml_native_reader.h b/src/xml/xml_native_reader.h index 9c9efd11..a70c74d5 100644 --- a/src/xml/xml_native_reader.h +++ b/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 MJCF[nMJCF]; #endif // MUJOCO_SRC_XML_XML_NATIVE_READER_H_ diff --git a/src/xml/xml_native_writer.cc b/src/xml/xml_native_writer.cc index b3a24836..0e7ebc7e 100644 --- a/src/xml/xml_native_writer.cc +++ b/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); diff --git a/test/engine/engine_core_smooth_test.cc b/test/engine/engine_core_smooth_test.cc index 39fe7878..b0f52410 100644 --- a/test/engine/engine_core_smooth_test.cc +++ b/test/engine/engine_core_smooth_test.cc @@ -17,6 +17,7 @@ #include "src/engine/engine_core_smooth.h" #include +#include #include #include #include @@ -597,7 +598,7 @@ TEST_F(CoreSmoothTest, RefsiteConservesMomentum) { ASSERT_THAT(model, NotNull()); mjData* data = mj_makeData(model); - // this test asserts tight momentum conservation: solve exactly, no early termination + // assert tight momentum conservation: solve exactly, no early termination model->opt.tolerance = 0; data->ctrl[0] = 1; @@ -1378,6 +1379,362 @@ TEST_F(CoreSmoothTest, SO3QuatSetpointRequiresStateless) { EXPECT_THAT(error, HasSubstr("dyntype")); } +// PID servo with vref = 0 reproduces the position servo exactly. +TEST_F(CoreSmoothTest, PidMatchesPositionServo) { + static constexpr char xml[] = R"( + + + + + + + + + + + + + + + + + + + )"; + char error[1024]; + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + + // layout: 2 actuators, 1+2 controls, 1+1 outputs + EXPECT_EQ(model->nactuator, 2); + EXPECT_EQ(model->nu, 3); + EXPECT_EQ(model->nout, 2); + int pid = mj_name2id(model.get(), mjOBJ_ACTUATOR, "pid"); + int uadr = model->actuator_ctrladr[pid]; + EXPECT_EQ(model->actuator_ctrlnum[pid], 2); + + mjData* data = mj_makeData(model.get()); + int j_servo = mj_name2id(model.get(), mjOBJ_JOINT, "h_position"); + int j_pd = mj_name2id(model.get(), mjOBJ_JOINT, "h_pd"); + + // ramp the position target on both, v* = 0 on the pid + while (data->time < 3) { + mjtNum target = 0.8 * data->time; + data->ctrl[0] = target; + data->ctrl[uadr] = target; + data->ctrl[uadr+1] = 0; + mj_step(model.get(), data); + ASSERT_EQ(data->warning[mjWARN_BADQACC].number, 0) << "diverged"; + ASSERT_EQ(data->qpos[model->jnt_qposadr[j_servo]], + data->qpos[model->jnt_qposadr[j_pd]]) + << "trajectories diverge at time " << data->time; + } + + mj_deleteData(data); +} + +// Constant velocity setpoint produces steady motion at the commanded rate. +TEST_F(CoreSmoothTest, PidVelocitySetpoint) { + static constexpr char xml[] = R"( + + + + + + + + + + + + + )"; + char error[1024]; + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + mjData* data = mj_makeData(model.get()); + + data->ctrl[1] = 0.7; // v* + while (data->time < 3) { + mj_step(model.get(), data); + ASSERT_EQ(data->warning[mjWARN_BADQACC].number, 0) << "diverged"; + } + EXPECT_NEAR(data->qvel[0], 0.7, MjTol(1e-6, 2e-6)); + + mj_deleteData(data); +} + +// The feedforward input adds directly to the actuator force. +TEST_F(CoreSmoothTest, PidFeedforward) { + static constexpr char xml[] = R"( + + + + + + + + + + + + )"; + char error[1024]; + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + EXPECT_EQ(model->nu, 3); + EXPECT_EQ(model->nout, 1); + + mjData* data = mj_makeData(model.get()); + data->ctrl[2] = 1.25; // ff, with zero position/velocity error + mj_forward(model.get(), data); + EXPECT_EQ(data->actuator_force[0], 1.25); + + mj_deleteData(data); +} + +// Input subsets: present inputs pack in canonical order, absent setpoints are +// zero, making single-input PIDs match the corresponding SISO servos. +TEST_F(CoreSmoothTest, PidInputSubsets) { + static constexpr char xml[] = R"( + + + + + + + + + + + + + + + + + + + + )"; + char error[1024]; + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + mjModel* m = model.get(); + + // layout: subsets shrink the control blocks + EXPECT_EQ(m->nactuator, 5); + EXPECT_EQ(m->nout, 5); + EXPECT_EQ(m->nu, 6); + int expected_ctrlnum[5] = {1, 1, 1, 1, 2}; + for (int i=0; i < 5; i++) { + EXPECT_EQ(m->actuator_ctrlnum[i], expected_ctrlnum[i]) << "actuator " << i; + } + + mjData* data = mj_makeData(m); + data->qpos[0] = 0.2; + data->qpos[1] = -0.3; + data->qvel[0] = 0.5; + data->qvel[1] = -0.4; + data->ctrl[0] = data->ctrl[1] = 0.6; // pos-only pid and position servo + data->ctrl[2] = data->ctrl[3] = -0.8; // vel-only pid and velocity servo + data->ctrl[4] = 0.3; // pos of the [pos, ff] pid + data->ctrl[5] = 0.9; // ff of the [pos, ff] pid + mj_forward(m, data); + + // single-input PIDs match the SISO servos + EXPECT_DOUBLE_EQ(data->actuator_force[0], data->actuator_force[1]); + EXPECT_DOUBLE_EQ(data->actuator_force[2], data->actuator_force[3]); + + // [pos, ff]: kp*(qref - l) - kv*ldot + ff (absent velocity setpoint is zero) + mjtNum expected = 5*(0.3 - data->qpos[1]) - 2*data->qvel[1] + 0.9; + EXPECT_MJTNUM_EQ(data->actuator_force[4], expected); + + // input names skip absent inputs + EXPECT_STREQ(mj_actuatorInputName(m, 4, 0), "pos"); + EXPECT_STREQ(mj_actuatorInputName(m, 4, 1), "ff"); + + mj_deleteData(data); +} + +// Integral action eliminates the steady-state error of a weak P servo under +// gravity; the integral state is clamped by imax. +TEST_F(CoreSmoothTest, PidIntegralAction) { + static constexpr char xml_fmt[] = R"( + + + + + + + + + + + + + )"; + char error[1024]; + char xml[2048]; + + // P-only: gravity induces a steady-state error at target q* = 0 + snprintf(xml, sizeof(xml), xml_fmt, "0"); + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + EXPECT_EQ(model->na, 0); + mjData* data = mj_makeData(model.get()); + while (data->time < 10) { + mj_step(model.get(), data); + ASSERT_EQ(data->warning[mjWARN_BADQACC].number, 0) << "diverged"; + } + mjtNum p_error = mju_abs(data->qpos[0]); + EXPECT_GT(p_error, 0.05); + mj_deleteData(data); + + // with integral action: error is eliminated, integral state is bounded + snprintf(xml, sizeof(xml), xml_fmt, "2"); + model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + EXPECT_EQ(model->na, 1); + data = mj_makeData(model.get()); + while (data->time < 20) { + mj_step(model.get(), data); + ASSERT_LE(mju_abs(data->act[0]), 2 + 1e-10) + << "integral state exceeds imax"; + } + EXPECT_LT(mju_abs(data->qpos[0]), 1e-3); + mj_deleteData(data); +} + +// slewmax limits the effective setpoint rate through an activation state. +TEST_F(CoreSmoothTest, PidSlewRateLimit) { + static constexpr char xml[] = R"( + + + )"; + char error[1024]; + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + EXPECT_EQ(model->actuator_actnum[0], 1); // slew state + mjData* data = mj_makeData(model.get()); + + // step the setpoint to 1: the effective setpoint (act) ramps at slewmax + data->ctrl[0] = 1.0; + mjtNum t_ramp = 1.0 / 0.5; // setpoint distance / slewmax + while (data->time < t_ramp - 0.1) { + mj_step(model.get(), data); + EXPECT_LE(data->act[0], 0.5 * data->time + 1e-9) << "slew exceeded"; + } + while (data->time < 3 * t_ramp) { + mj_step(model.get(), data); + } + + // the effective setpoint reached the command, and the joint tracked it + EXPECT_NEAR(data->act[0], 1.0, 1e-6); + EXPECT_NEAR(data->qpos[0], 1.0, 0.01); + + mj_deleteData(data); +} + +// PID on a rotational transmission: the position setpoint wraps, winding +// targets are tracked smoothly through pi. +TEST_F(CoreSmoothTest, PidTracksWindingTarget) { + static constexpr char xml[] = R"( + + + + + + + + + + + + + )"; + char error[1024]; + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + mjData* data = mj_makeData(model.get()); + + const mjtNum rate = 0.5; + while (data->time < 2*mjPI / rate) { + data->ctrl[0] = rate * data->time; + data->ctrl[1] = rate; // matched velocity setpoint + mj_step(model.get(), data); + mjtNum err = data->ctrl[0] - data->actuator_length[0]; + err -= 2*mjPI * mju_round(err / (2*mjPI)); + ASSERT_LT(mju_abs(err), 0.5) << "tracking lost at time " << data->time; + } + + mj_deleteData(data); +} + +// PID parameters compose through defaults classes; the model round-trips. +TEST_F(CoreSmoothTest, PidDefaultsAndRoundtrip) { + static constexpr char xml[] = R"( + + + + + + + + + + + + + + + + + )"; + char error[1024]; + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + + // inherited: kp, ki, imax, input, velrange + EXPECT_EQ(model->nu, 3); + EXPECT_EQ(model->na, 1); + EXPECT_EQ(model->actuator_gainprm[0], 3); // ki + EXPECT_EQ(model->actuator_biasprm[1], -7); // -kp + EXPECT_EQ(model->actuator_dynprm[0], 1.5); // imax + EXPECT_EQ(model->actuator_dyntype[0], mjDYN_PID); + EXPECT_EQ(model->actuator_ctrlrange[2], -2); // velrange lo, input 1 + EXPECT_EQ(model->actuator_ctrlrange[3], 2); + EXPECT_TRUE(model->actuator_ctrllimited[1]); + + // round-trip preserves everything + std::string saved = SaveAndReadXml(model.get()); + MjModelPtr model2 = LoadModelFromString(saved.c_str(), error, sizeof(error)); + ASSERT_THAT(model2.get(), NotNull()) << error; + EXPECT_EQ(model2->nu, 3); + EXPECT_EQ(model2->na, 1); + EXPECT_EQ(model2->actuator_gainprm[0], 3); + EXPECT_EQ(model2->actuator_ctrlrange[2], -2); +} + static const char* const kInertiaPath = "engine/testdata/inertia.xml"; TEST_F(CoreSmoothTest, FactorI) { diff --git a/test/user/user_api_test.cc b/test/user/user_api_test.cc index bcbb2cdd..4277d546 100644 --- a/test/user/user_api_test.cc +++ b/test/user/user_api_test.cc @@ -18,7 +18,7 @@ #include #include #include -#include +#include // NOLINT #include #include #include @@ -513,6 +513,41 @@ TEST_F(MujocoTest, SetToOrientation) { mj_deleteSpec(spec); } +TEST_F(MujocoTest, SetToPID) { + mjSpec* spec = mj_makeSpec(); + mjsActuator* actuator = mjs_addActuator(spec, 0); + + // stateless PID with kv, default input signature + double kv = 3.0; + const char* err = mjs_setToPID(actuator, 5.0, &kv, nullptr, nullptr, nullptr, + nullptr, 0, 0); + EXPECT_STREQ(err, ""); + EXPECT_EQ(actuator->gaintype, mjGAIN_PID); + EXPECT_EQ(actuator->biastype, mjBIAS_AFFINE); + EXPECT_EQ(actuator->dyntype, mjDYN_NONE); + EXPECT_EQ(actuator->biasprm[1], -5.0); + EXPECT_EQ(actuator->biasprm[2], -3.0); + EXPECT_EQ(actuator->gainprm[0], 0.0); + + // integral action with anti-windup, pos-only signature + double ki = 0.5, imax = 2.0, dampratio = 1.0; + err = mjs_setToPID(actuator, 5.0, nullptr, &dampratio, &ki, &imax, nullptr, 0, + mjINPUT_POS); + EXPECT_STREQ(err, ""); + EXPECT_EQ(actuator->dyntype, mjDYN_PID); + EXPECT_EQ(actuator->gainprm[0], 0.5); + EXPECT_EQ(actuator->dynprm[0], 2.0); + EXPECT_EQ(actuator->biasprm[2], 1.0); + EXPECT_EQ(actuator->ctrlspec, mjINPUT_POS); + + // kv and dampratio are mutually exclusive + err = mjs_setToPID(actuator, 5.0, &kv, &dampratio, nullptr, nullptr, nullptr, + 0, 0); + EXPECT_STREQ(err, "kv and dampratio cannot both be defined"); + + mj_deleteSpec(spec); +} + static constexpr char xml_plugin_1[] = R"( diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index 7f72eaa2..d9de98c4 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -276,7 +276,8 @@ public enum mjtDyn : int{ mjDYN_FILTEREXACT = 3, mjDYN_MUSCLE = 4, mjDYN_DCMOTOR = 5, - mjDYN_USER = 6, + mjDYN_PID = 6, + mjDYN_USER = 7, } public enum mjtGain : int{ mjGAIN_FIXED = 0, @@ -284,7 +285,8 @@ public enum mjtGain : int{ mjGAIN_MUSCLE = 2, mjGAIN_DCMOTOR = 3, mjGAIN_SO3 = 4, - mjGAIN_USER = 5, + mjGAIN_PID = 5, + mjGAIN_USER = 6, } public enum mjtBias : int{ mjBIAS_NONE = 0, @@ -298,6 +300,11 @@ public enum mjtCtrlChart : int{ mjCHART_EXPMAP = 1, mjCHART_QUAT = 2, } +public enum mjtCtrlInput : int{ + mjINPUT_POS = 1, + mjINPUT_VEL = 2, + mjINPUT_FF = 4, +} public enum mjtObj : int{ mjOBJ_UNKNOWN = 0, mjOBJ_BODY = 1, diff --git a/wasm/codegen/generated/bindings.cc b/wasm/codegen/generated/bindings.cc index 3fdd7b12..6f0cb8af 100644 --- a/wasm/codegen/generated/bindings.cc +++ b/wasm/codegen/generated/bindings.cc @@ -2923,6 +2923,15 @@ std::string mjs_setToOrientation_wrapper(MjsActuator& actuator, double kp, const return std::string(mjs_setToOrientation(actuator.get(), kp, kv_.data(), dampratio_.data(), ctrlspec)); } +std::string mjs_setToPID_wrapper(MjsActuator& actuator, double kp, const val& kv, const val& dampratio, const val& ki, const val& imax, const val& slewmax, double inheritrange, int ctrlspec) { + UNPACK_VALUE(double, kv); + UNPACK_VALUE(double, dampratio); + UNPACK_VALUE(double, ki); + UNPACK_VALUE(double, imax); + UNPACK_VALUE(double, slewmax); + return std::string(mjs_setToPID(actuator.get(), kp, kv_.data(), dampratio_.data(), ki_.data(), imax_.data(), slewmax_.data(), inheritrange, ctrlspec)); +} + std::string mjs_setToPosition_wrapper(MjsActuator& actuator, double kp, const val& kv, const val& dampratio, const val& timeconst, double inheritrange) { UNPACK_VALUE(double, kv); UNPACK_VALUE(double, dampratio); @@ -3981,6 +3990,10 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) { enum_("mjtCtrlChart") .value("mjCHART_EXPMAP", mjCHART_EXPMAP) .value("mjCHART_QUAT", mjCHART_QUAT); + enum_("mjtCtrlInput") + .value("mjINPUT_POS", mjINPUT_POS) + .value("mjINPUT_VEL", mjINPUT_VEL) + .value("mjINPUT_FF", mjINPUT_FF); enum_("mjtDataType") .value("mjDATATYPE_REAL", mjDATATYPE_REAL) .value("mjDATATYPE_POSITIVE", mjDATATYPE_POSITIVE) @@ -4018,6 +4031,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) { .value("mjDYN_FILTEREXACT", mjDYN_FILTEREXACT) .value("mjDYN_MUSCLE", mjDYN_MUSCLE) .value("mjDYN_DCMOTOR", mjDYN_DCMOTOR) + .value("mjDYN_PID", mjDYN_PID) .value("mjDYN_USER", mjDYN_USER); enum_("mjtEnableBit") .value("mjENBL_OVERRIDE", mjENBL_OVERRIDE) @@ -4082,6 +4096,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) { .value("mjGAIN_MUSCLE", mjGAIN_MUSCLE) .value("mjGAIN_DCMOTOR", mjGAIN_DCMOTOR) .value("mjGAIN_SO3", mjGAIN_SO3) + .value("mjGAIN_PID", mjGAIN_PID) .value("mjGAIN_USER", mjGAIN_USER); enum_("mjtGeom") .value("mjGEOM_PLANE", mjGEOM_PLANE) @@ -5612,6 +5627,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) { .property("dynprm", &MjsActuator::dynprm) .property("dyntype", &MjsActuator::dyntype, &MjsActuator::set_dyntype, reference()) .property("element", &MjsActuator::element, reference()) + .property("ffrange", &MjsActuator::ffrange) .property("forcelimited", &MjsActuator::forcelimited, &MjsActuator::set_forcelimited, reference()) .property("forcerange", &MjsActuator::forcerange) .property("gainprm", &MjsActuator::gainprm) @@ -5628,7 +5644,8 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) { .property("slidersite", &MjsActuator::slidersite, &MjsActuator::set_slidersite, reference()) .property("target", &MjsActuator::target, &MjsActuator::set_target, reference()) .property("trntype", &MjsActuator::trntype, &MjsActuator::set_trntype, reference()) - .property("userdata", &MjsActuator::userdata, reference()); + .property("userdata", &MjsActuator::userdata, reference()) + .property("velrange", &MjsActuator::velrange); emscripten::class_("MjsAuthored") .property("disableactuator", &MjsAuthored::disableactuator, &MjsAuthored::set_disableactuator, reference()) .property("disableflags", &MjsAuthored::disableflags, &MjsAuthored::set_disableflags, reference()) @@ -6510,6 +6527,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) { function("mjs_setToMotor", &mjs_setToMotor_wrapper); function("mjs_setToMuscle", &mjs_setToMuscle_wrapper); function("mjs_setToOrientation", &mjs_setToOrientation_wrapper); + function("mjs_setToPID", &mjs_setToPID_wrapper); function("mjs_setToPosition", &mjs_setToPosition_wrapper); function("mjs_setToVelocity", &mjs_setToVelocity_wrapper); function("mjs_wrapGeom", &mjs_wrapGeom_wrapper); diff --git a/wasm/codegen/generated/bindings.h b/wasm/codegen/generated/bindings.h index 4c1ad6a0..2f82c8ff 100644 --- a/wasm/codegen/generated/bindings.h +++ b/wasm/codegen/generated/bindings.h @@ -6025,6 +6025,12 @@ struct MjsActuator { void set_ctrlspec(int value) { ptr_->ctrlspec = value; } + emscripten::val velrange() const { + return emscripten::val(emscripten::typed_memory_view(2, ptr_->velrange)); + } + emscripten::val ffrange() const { + return emscripten::val(emscripten::typed_memory_view(2, ptr_->ffrange)); + } mjtBool actearly() const { return ptr_->actearly; }