chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Add the pid actuator: setpoint inputs, integral action, slew rate limiting.

Browse Source 
<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:
Yuval Tassa
2026-08-01 04:28:10 -07:00
committed by Copybara-Service
parent 7bc1aa9b05
commit 279df98cd0
33 changed files with 1504 additions and 62 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/APIreference/APItypes.rst
+10
View File
@@ -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
doc/APIreference/functions.rst
+9
View File
@@ -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>`__
doc/XMLreference.rst
+265 -19
View File
@@ -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 <geActuation>` 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<actuator-pid>`
dcmotor DC motor electrical dynamics, see :ref:`dcmotor<actuator-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 <geActuation>`, 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<actuator-pid>`
so3 geodesic orientation servo, computed jointly over 3 force outputs, see :ref:`orientation<actuator-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 <actuator-muscle>` below.
.. _actuator-general-velrange:
:at:`velrange`: :at-val:`real(2), "0 0"`
Range of the velocity-setpoint input of a :ref:`pid<actuator-pid>` actuator.
.. _actuator-general-ffrange:
:at:`ffrange`: :at-val:`real(2), "0 0"`
Range of the feedforward input of a :ref:`pid<actuator-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<actuator-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<actuator-orientation-input>`. For gaintype "pid" it is
a token list selecting the input subset; see :ref:`pid/input<actuator-pid-input>`.
.. _actuator-general-actearly:
@@ -5954,6 +5968,146 @@ This element has one custom attribute in addition to the common attributes:
:ref:`position<actuator-position>` attribute and in the :ref:`default class<default-position-inheritrange>`,
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<actuator-position>`. The input signature is any subset of
``[pos, vel, ff]``, selected by :ref:`input<actuator-pid-input>`: an absent setpoint input is fixed at zero, and the
``ff`` input adds a feedforward force. Integral action is enabled by :ref:`ki<actuator-pid-ki>`: the position error is integrated in
:ref:`act<siPhysicsState>` and contributes :math:`k_i \cdot act` to the force, with anti-windup clamping by
:ref:`imax<actuator-pid-imax>`.
:ref:`slewmax<actuator-pid-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<actuator-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<geIntegration>`.
.. _actuator-pid-dampratio:
:at:`dampratio`: :at-val:`real, "0"`
Damping applied by the actuator, using damping ratio units, as for
:ref:`position/dampratio<actuator-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<siPhysicsState>` (:ref:`dyntype<actuator-general-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<actuator-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<actuator-general-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<actuator-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<sensor-actuatorpos>` report one value per force output. The integrator variant, which
stores the orientation setpoint in :ref:`act<siPhysicsState>`, is available via :ref:`general<actuator-general>` with
:ref:`dyntype<actuator-general-dyntype>` "integrator" and is expmap-only. The video on the right shows this `example
model <https://github.com/google-deepmind/mujoco/blob/main/test/engine/testdata/sensor/actuation/orientation.xml>`__.
model <https://github.com/google-deepmind/mujoco/blob/main/test/engine/testdata/actuation/orientation.xml>`__.
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-pid>` actuator.
When using this actuator, it is recommended to use the implicitfast or implicit :ref:`integrators<geIntegration>`.
The underlying :el:`general` attributes are set as follows:
@@ -6802,7 +6955,7 @@ Associate this actuator with an :ref:`engine plugin<exPlugin>`. 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 <geActuation>` section. If :ref:`dyntype<actuator-general-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 <actuator-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 <actuator-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 <actuator-orientation>` attributes are available here except: name, class, joint, site, refsite.
.. _default-damper:
.. _default-damper-forcelimited:
doc/XMLschema.rst
+240
View File
@@ -2378,6 +2378,12 @@
.. grid-item::
:ref:`input<actuator-general-input>`
.. grid-item::
:ref:`velrange<actuator-general-velrange>`
.. grid-item::
:ref:`ffrange<actuator-general-ffrange>`
.. grid-item::
:ref:`dyntype<actuator-general-dyntype>`
@@ -2793,6 +2799,114 @@
:ref:`input<actuator-orientation-input>`
.. dropdown:: :ref:`pid<actuator-pid>` |*|
.. grid:: 2 3 4 4
:gutter: 0
.. grid-item::
:ref:`name<actuator-pid-name>`
.. grid-item::
:ref:`class<actuator-pid-class>`
.. grid-item::
:ref:`group<actuator-pid-group>`
.. grid-item::
:ref:`nsample<actuator-pid-nsample>`
.. grid-item::
:ref:`interp<actuator-pid-interp>`
.. grid-item::
:ref:`delay<actuator-pid-delay>`
.. grid-item::
:ref:`ctrllimited<actuator-pid-ctrllimited>`
.. grid-item::
:ref:`forcelimited<actuator-pid-forcelimited>`
.. grid-item::
:ref:`ctrlrange<actuator-pid-ctrlrange>`
.. grid-item::
:ref:`posrange<actuator-pid-posrange>`
.. grid-item::
:ref:`velrange<actuator-pid-velrange>`
.. grid-item::
:ref:`ffrange<actuator-pid-ffrange>`
.. grid-item::
:ref:`forcerange<actuator-pid-forcerange>`
.. grid-item::
:ref:`inheritrange<actuator-pid-inheritrange>`
.. grid-item::
:ref:`lengthrange<actuator-pid-lengthrange>`
.. grid-item::
:ref:`gear<actuator-pid-gear>`
.. grid-item::
:ref:`damping<actuator-pid-damping>`
.. grid-item::
:ref:`armature<actuator-pid-armature>`
.. grid-item::
:ref:`cranklength<actuator-pid-cranklength>`
.. grid-item::
:ref:`user<actuator-pid-user>`
.. grid-item::
:ref:`joint<actuator-pid-joint>`
.. grid-item::
:ref:`jointinparent<actuator-pid-jointinparent>`
.. grid-item::
:ref:`tendon<actuator-pid-tendon>`
.. grid-item::
:ref:`slidersite<actuator-pid-slidersite>`
.. grid-item::
:ref:`cranksite<actuator-pid-cranksite>`
.. grid-item::
:ref:`site<actuator-pid-site>`
.. grid-item::
:ref:`refsite<actuator-pid-refsite>`
.. grid-item::
:ref:`kp<actuator-pid-kp>`
.. grid-item::
:ref:`kv<actuator-pid-kv>`
.. grid-item::
:ref:`dampratio<actuator-pid-dampratio>`
.. grid-item::
:ref:`ki<actuator-pid-ki>`
.. grid-item::
:ref:`imax<actuator-pid-imax>`
.. grid-item::
:ref:`slewmax<actuator-pid-slewmax>`
.. grid-item::
:ref:`input<actuator-pid-input>`
.. dropdown:: :ref:`damper<actuator-damper>` |*|
.. grid:: 2 3 4 4
@@ -5927,6 +6041,12 @@
.. grid-item::
:ref:`input<default-general-input>`
.. grid-item::
:ref:`velrange<default-general-velrange>`
.. grid-item::
:ref:`ffrange<default-general-ffrange>`
.. grid-item::
:ref:`dyntype<default-general-dyntype>`
@@ -6165,6 +6285,126 @@
:ref:`dampratio<default-intvelocity-dampratio>`
.. dropdown:: :ref:`orientation<default-orientation>` :octicon:`dot`
.. grid:: 2 3 4 4
:gutter: 0
.. grid-item::
:ref:`forcelimited<default-orientation-forcelimited>`
.. grid-item::
:ref:`ctrlrange<default-orientation-ctrlrange>`
.. grid-item::
:ref:`forcerange<default-orientation-forcerange>`
.. grid-item::
:ref:`user<default-orientation-user>`
.. grid-item::
:ref:`group<default-orientation-group>`
.. grid-item::
:ref:`nsample<default-orientation-nsample>`
.. grid-item::
:ref:`interp<default-orientation-interp>`
.. grid-item::
:ref:`delay<default-orientation-delay>`
.. grid-item::
:ref:`kp<default-orientation-kp>`
.. grid-item::
:ref:`kv<default-orientation-kv>`
.. grid-item::
:ref:`dampratio<default-orientation-dampratio>`
.. grid-item::
:ref:`input<default-orientation-input>`
.. dropdown:: :ref:`pid<default-pid>` :octicon:`dot`
.. grid:: 2 3 4 4
:gutter: 0
.. grid-item::
:ref:`ctrllimited<default-pid-ctrllimited>`
.. grid-item::
:ref:`forcelimited<default-pid-forcelimited>`
.. grid-item::
:ref:`ctrlrange<default-pid-ctrlrange>`
.. grid-item::
:ref:`posrange<default-pid-posrange>`
.. grid-item::
:ref:`velrange<default-pid-velrange>`
.. grid-item::
:ref:`ffrange<default-pid-ffrange>`
.. grid-item::
:ref:`forcerange<default-pid-forcerange>`
.. grid-item::
:ref:`inheritrange<default-pid-inheritrange>`
.. grid-item::
:ref:`gear<default-pid-gear>`
.. grid-item::
:ref:`damping<default-pid-damping>`
.. grid-item::
:ref:`armature<default-pid-armature>`
.. grid-item::
:ref:`cranklength<default-pid-cranklength>`
.. grid-item::
:ref:`user<default-pid-user>`
.. grid-item::
:ref:`group<default-pid-group>`
.. grid-item::
:ref:`nsample<default-pid-nsample>`
.. grid-item::
:ref:`interp<default-pid-interp>`
.. grid-item::
:ref:`delay<default-pid-delay>`
.. grid-item::
:ref:`kp<default-pid-kp>`
.. grid-item::
:ref:`kv<default-pid-kv>`
.. grid-item::
:ref:`dampratio<default-pid-dampratio>`
.. grid-item::
:ref:`ki<default-pid-ki>`
.. grid-item::
:ref:`imax<default-pid-imax>`
.. grid-item::
:ref:`slewmax<default-pid-slewmax>`
.. grid-item::
:ref:`input<default-pid-input>`
.. dropdown:: :ref:`damper<default-damper>` :octicon:`dot`
.. grid:: 2 3 4 4
doc/changelog.rst
+17
View File
@@ -5,6 +5,23 @@ Changelog
Upcoming version (not yet released)
-----------------------------------
Actuation
^^^^^^^^^
- Added the :ref:`pid<actuator-pid>` actuator: a PID controller with real position and velocity setpoint inputs,
optional integral action (:ref:`ki<actuator-pid-ki>`, integrating the position error with
:ref:`imax<actuator-pid-imax>` anti-windup), setpoint rate limiting (:ref:`slewmax<actuator-pid-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<actuator-position>`. The input signature is any subset of ``[pos, vel, ff]``, selected
by :ref:`input<actuator-pid-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
^^^^^^
doc/includes/references.h
+12
View File
@@ -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);
include/mujoco/mjspec.h
+2
View File
@@ -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
include/mujoco/mjspecmacro.h
+2
View File
@@ -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 ) \
include/mujoco/mjtype.h
+9
View File
@@ -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
include/mujoco/mujoco.h
+5
View File
@@ -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);
python/mujoco/introspect/enums.py
+14 -2
View File
@@ -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',
python/mujoco/introspect/functions.py
+63
View File
@@ -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',
python/mujoco/introspect/structs.py
+16
View File
@@ -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'),
python/mujoco/specs.cc
+17
View File
@@ -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) {
python/mujoco/specs_test.py
+9
View File
@@ -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)
src/engine/engine_forward.c
+103 -5
View File
@@ -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
+15
View File
@@ -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
+5 -3
View File
@@ -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
+42
View File
@@ -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
+5
View File
@@ -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
+2
View File
@@ -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
+4 -4
View File
@@ -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
+87 -7
View File
@@ -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
+2
View File
@@ -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
+4 -2
View File
@@ -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
+102 -13
View File
@@ -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
+1 -1
View File
@@ -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
+14 -1
View File
@@ -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);
test/engine/engine_core_smooth_test.cc
+358 -1
View File
@@ -17,6 +17,7 @@
#include "src/engine/engine_core_smooth.h"
#include <algorithm>
#include <cstdio>
#include <string>
#include <string_view>
#include <vector>
@@ -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"(
<mujoco>
<option integrator="implicitfast">
<flag contact="disable"/>
</option>
<worldbody>
<!-- identical overlapping bodies (contacts disabled): world-frame
arithmetic is bit-identical, so the trajectories must be too -->
<body pos="0 0 .2">
<joint name="h_position" axis="0 1 0"/>
<geom type="capsule" size=".02" fromto="0 0 0 .2 0 0"/>
</body>
<body pos="0 0 .2">
<joint name="h_pd" axis="0 1 0"/>
<geom type="capsule" size=".02" fromto="0 0 0 .2 0 0"/>
</body>
</worldbody>
<actuator>
<position name="servo" joint="h_position" kp="10" dampratio="1"/>
<pid name="pid" joint="h_pd" kp="10" dampratio="1"/>
</actuator>
</mujoco>
)";
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"(
<mujoco>
<option integrator="implicitfast">
<flag contact="disable" gravity="disable"/>
</option>
<worldbody>
<body>
<joint name="slide" type="slide" axis="1 0 0"/>
<geom type="box" size=".05 .05 .05"/>
</body>
</worldbody>
<actuator>
<pid name="pid" joint="slide" kp="0" kv="10"/>
</actuator>
</mujoco>
)";
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"(
<mujoco>
<worldbody>
<body>
<joint name="slide" type="slide" axis="1 0 0"/>
<geom type="box" size=".05 .05 .05"/>
</body>
</worldbody>
<actuator>
<pid name="pid" joint="slide" kp="1" kv="1" input="pos vel ff"/>
</actuator>
</mujoco>
)";
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"(
<mujoco>
<worldbody>
<body>
<joint name="j1" type="slide" axis="1 0 0"/>
<geom type="box" size=".05 .05 .05"/>
</body>
<body>
<joint name="j2" type="slide" axis="1 0 0"/>
<geom type="box" size=".05 .05 .05"/>
</body>
</worldbody>
<actuator>
<pid joint="j1" kp="7" input="pos"/>
<position joint="j1" kp="7"/>
<pid joint="j2" kv="3" input="vel"/>
<velocity joint="j2" kv="3"/>
<pid joint="j2" kp="5" kv="2" input="pos ff"/>
</actuator>
</mujoco>
)";
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"(
<mujoco>
<option integrator="implicitfast">
<flag contact="disable"/>
</option>
<worldbody>
<body pos="0 0 .5">
<joint name="hinge" axis="0 1 0"/>
<geom type="capsule" size=".02" fromto="0 0 0 .2 0 0"/>
</body>
</worldbody>
<actuator>
<pid name="pid" joint="hinge" kp="1" dampratio="1" ki="%s" imax="2"/>
</actuator>
</mujoco>
)";
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"(
<mujoco>
<option timestep="0.001"/>
<worldbody>
<body>
<joint name="slide" type="slide" axis="1 0 0"/>
<geom type="box" size=".1 .1 .1" mass="1"/>
</body>
</worldbody>
<actuator>
<pid name="servo" joint="slide" kp="200" kv="30" slewmax="0.5"/>
</actuator>
</mujoco>
)";
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"(
<mujoco>
<option integrator="implicitfast">
<flag contact="disable" gravity="disable"/>
</option>
<worldbody>
<body>
<joint name="ball" type="ball"/>
<geom type="box" size=".05 .07 .03"/>
</body>
</worldbody>
<actuator>
<pid name="rz" joint="ball" gear="0 0 1" kp="1" dampratio="1"/>
</actuator>
</mujoco>
)";
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"(
<mujoco>
<default>
<default class="arm">
<pid kp="7" ki="3" imax="1.5" velrange="-2 2" input="pos vel ff"/>
</default>
</default>
<worldbody>
<body>
<joint name="slide" type="slide" axis="1 0 0"/>
<geom type="box" size=".05 .05 .05"/>
</body>
</worldbody>
<actuator>
<pid name="pid" joint="slide" class="arm"/>
</actuator>
</mujoco>
)";
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) {
test/user/user_api_test.cc
+36 -1
View File
@@ -18,7 +18,7 @@
#include <cstddef>
#include <cstdint>
#include <cstring>
#include <filesystem>
#include <filesystem> // NOLINT
#include <functional>
#include <map>
#include <memory>
@@ -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"(
<mujoco model="MuJoCo Model">
<worldbody>
unity/Runtime/Bindings/MjBindings.cs
+9 -2
View File
@@ -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,
wasm/codegen/generated/bindings.cc
+19 -1
View File
@@ -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>("mjtCtrlChart")
.value("mjCHART_EXPMAP", mjCHART_EXPMAP)
.value("mjCHART_QUAT", mjCHART_QUAT);
enum_<mjtCtrlInput>("mjtCtrlInput")
.value("mjINPUT_POS", mjINPUT_POS)
.value("mjINPUT_VEL", mjINPUT_VEL)
.value("mjINPUT_FF", mjINPUT_FF);
enum_<mjtDataType>("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>("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>("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>("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);
wasm/codegen/generated/bindings.h
+6
View File
@@ -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;
}