diff --git a/doc/APIreference/functions.rst b/doc/APIreference/functions.rst index 62db7ea4..cfbf9eaf 100644 --- a/doc/APIreference/functions.rst +++ b/doc/APIreference/functions.rst @@ -300,6 +300,89 @@ Copy concatenated state components specified by ``sig`` from ``state`` into ``d Copy state from src to dst. +.. _mj_readCtrl: + +`mj_readCtrl <#mj_readCtrl>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +.. mujoco-include:: mj_readCtrl + +Read the control value for an actuator at a given time, taking delays into account. If no history buffer exists, return +``mjData.ctrl[id]``. If a history buffer exists (:ref:`nsample` > 0), read from the delay +buffer at ``time - actuator_delay[id]`` using the requested interpolation order: + +- ``interp = 0``: Zero-order hold (piecewise constant) +- ``interp = 1``: Piecewise Linear +- ``interp = 2``: Cubic Spline (Catmull-Rom) +- ``interp = -1``: Use the actuator's :ref:`interp` value. + +In all three cases, constant extrapolation outside of buffer bounds. +See :ref:`Delays` for details. + +.. _mj_readSensor: + +`mj_readSensor <#mj_readSensor>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +.. mujoco-include:: mj_readSensor + +Read a sensor value at a given time, taking delays into account. If no history buffer exists, return a pointer to the +sensor's slice of ``mjData.sensordata``. If a history buffer exists (:ref:`nsample` > 0), read from the +history buffer at ``time - sensor_delay[id]``. See :ref:`Delays` for details. + +**Return value semantics:** + +- If no history buffer exists (:ref:`nsample` = 0), returns a pointer to the sensor's slice of + ``mjData.sensordata``. +- If a history buffer exists (:ref:`nsample` > 0) and the requested time matches a stored sample + (always true for ``interp = 0``), returns a pointer to the data in the history buffer. +- If interpolation is required (``interp = 1 or 2``), returns ``NULL`` and writes the interpolated result to + ``result`` (must be of size ``dim``). + +**Interpolation:** + +- ``interp = 0``: Zero-order hold (piecewise constant) +- ``interp = 1``: Piecewise Linear +- ``interp = 2``: Cubic Spline (Catmull-Rom) +- ``interp = -1``: Use the value in :ref:`interp` + +In all three cases, constant extrapolation outside of buffer bounds. + + +**Usage:** + +.. code-block:: C + + // read sensor 0 of data size `dim` at time t + mjtNum result[dim]; + const mjtNum* ptr = mj_readSensor(m, d, 0, t, result, /* interp = */ 1); + const mjtNum* data = ptr ? ptr : result; + +.. _mj_initCtrlHistory: + +`mj_initCtrlHistory <#mj_initCtrlHistory>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +.. mujoco-include:: mj_initCtrlHistory + +Initialize the history buffer for an actuator with custom values. The ``times`` array specifies the timestamps for each +sample (must be length :ref:`nsample`), and ``values`` specifies the control values. If +``times`` is ``NULL``, the existing timestamps in the buffer are used, and only the values are updated. +See :ref:`Delays` for details. + +.. _mj_initSensorHistory: + +`mj_initSensorHistory <#mj_initSensorHistory>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +.. mujoco-include:: mj_initSensorHistory + +Initialize the history buffer for a sensor with custom values. The ``times`` array specifies the timestamps for each +sample (must be length :ref:`nsample`), and ``values`` specifies the sensor values (must be of size +``nsample * dim``). If ``times`` is ``NULL``, the existing timestamps in the buffer are used. +The ``phase`` argument sets the user slot, which stores the last computation time for interval sensors. +See :ref:`Delays` for details. + .. _mj_setKeyframe: `mj_setKeyframe <#mj_setKeyframe>`__ diff --git a/doc/APIreference/functions_override.rst b/doc/APIreference/functions_override.rst index 38629433..8d195b00 100644 --- a/doc/APIreference/functions_override.rst +++ b/doc/APIreference/functions_override.rst @@ -205,6 +205,69 @@ is not a subset of the bits set in ``srcsig``. Copy concatenated state components specified by ``sig`` from ``state`` into ``d``. The bits of the integer ``sig`` correspond to element fields of :ref:`mjtState`. Fails with :ref:`mju_error` if ``sig`` is invalid. +.. _mj_readCtrl: + +Read the control value for an actuator at a given time, taking delays into account. If no history buffer exists, return +``mjData.ctrl[id]``. If a history buffer exists (:ref:`nsample` > 0), read from the delay +buffer at ``time - actuator_delay[id]`` using the requested interpolation order: + +- ``interp = 0``: Zero-order hold (piecewise constant) +- ``interp = 1``: Piecewise Linear +- ``interp = 2``: Cubic Spline (Catmull-Rom) +- ``interp = -1``: Use the actuator's :ref:`interp` value. + +In all three cases, constant extrapolation outside of buffer bounds. +See :ref:`Delays` for details. + +.. _mj_readSensor: + +Read a sensor value at a given time, taking delays into account. If no history buffer exists, return a pointer to the +sensor's slice of ``mjData.sensordata``. If a history buffer exists (:ref:`nsample` > 0), read from the +history buffer at ``time - sensor_delay[id]``. See :ref:`Delays` for details. + +**Return value semantics:** + +- If no history buffer exists (:ref:`nsample` = 0), returns a pointer to the sensor's slice of + ``mjData.sensordata``. +- If a history buffer exists (:ref:`nsample` > 0) and the requested time matches a stored sample + (always true for ``interp = 0``), returns a pointer to the data in the history buffer. +- If interpolation is required (``interp = 1 or 2``), returns ``NULL`` and writes the interpolated result to + ``result`` (must be of size ``dim``). + +**Interpolation:** + +- ``interp = 0``: Zero-order hold (piecewise constant) +- ``interp = 1``: Piecewise Linear +- ``interp = 2``: Cubic Spline (Catmull-Rom) +- ``interp = -1``: Use the value in :ref:`interp` + +In all three cases, constant extrapolation outside of buffer bounds. + + +**Usage:** + +.. code-block:: C + + // read sensor 0 of data size `dim` at time t + mjtNum result[dim]; + const mjtNum* ptr = mj_readSensor(m, d, 0, t, result, /* interp = */ 1); + const mjtNum* data = ptr ? ptr : result; + +.. _mj_initCtrlHistory: + +Initialize the history buffer for an actuator with custom values. The ``times`` array specifies the timestamps for each +sample (must be length :ref:`nsample`), and ``values`` specifies the control values. If +``times`` is ``NULL``, the existing timestamps in the buffer are used, and only the values are updated. +See :ref:`Delays` for details. + +.. _mj_initSensorHistory: + +Initialize the history buffer for a sensor with custom values. The ``times`` array specifies the timestamps for each +sample (must be length :ref:`nsample`), and ``values`` specifies the sensor values (must be of size +``nsample * dim``). If ``times`` is ``NULL``, the existing timestamps in the buffer are used. +The ``phase`` argument sets the user slot, which stores the last computation time for interval sensors. +See :ref:`Delays` for details. + .. _mj_mulJacVec: This function multiplies the constraint Jacobian mjData.efc_J by a vector. Note that the Jacobian can be either dense or diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index 3b1481ee..0787afce 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -5202,6 +5202,37 @@ specify them independently. Integer group to which the actuator belongs. This attribute can be used for custom tags. It is also used by the visualizer to enable and disable the rendering of entire groups of actuators. +.. _actuator-general-nsample: + +:at:`nsample`: :at-val:`int, "0"` + If greater than 0, this attribute creates a time-indexed ring buffer with :at:`nsample` samples of this actuator's + ``ctrl`` history. During state advancement, the current control input is appended to the buffer with timestamp + ``time``, and the oldest sample is removed. Values in the history buffer can be read via :ref:`mj_readCtrl`. + + A positive :at-val:`nsample` is required for :ref:`delay`. + See :ref:`Delays` for details. + +.. _actuator-general-interp: + +:at:`interp`: :at-val:`[zoh, linear, cubic], "zoh"` + The interpolation method used when reading from the history buffer. Corresponds to the ``interp`` argument in + :ref:`mj_readCtrl`. + + - ``zoh``: Zero-order hold (piecewise constant). + - ``linear``: Piecewise linear interpolation. + - ``cubic``: Cubic spline interpolation (Catmull-Rom). + + The :at:`interp` value is for advanced use-cases, see :ref:`Delays` for details. + +.. _actuator-general-delay: + +:at:`delay`: :at-val:`real, "0"` + If greater than 0, then during the forward dynamics, instead of reading the control input to the actuator from + ``mjData.ctrl``, the control input is read from the history buffer using :ref:`mj_readCtrl`. + Requires a history buffer (:ref:`nsample` > 0). + + In the most common case, ``delay = nsample * timestep``. + .. _actuator-general-ctrllimited: :at:`ctrllimited`: :at-val:`[false, true, auto], "auto"` @@ -5470,6 +5501,12 @@ This element does not have custom attributes. It only has common attributes, whi .. _actuator-motor-group: +.. _actuator-motor-delay: + +.. _actuator-motor-nsample: + +.. _actuator-motor-interp: + .. _actuator-motor-ctrllimited: .. _actuator-motor-forcelimited: @@ -5502,9 +5539,9 @@ This element does not have custom attributes. It only has common attributes, whi .. |actuator/motor attrib list| replace:: - :at:`name`, :at:`class`, :at:`group`, :at:`ctrllimited`, :at:`forcelimited`, :at:`ctrlrange`, :at:`forcerange`, - :at:`lengthrange`, :at:`gear`, :at:`cranklength`, :at:`joint`, :at:`jointinparent`, :at:`tendon`, :at:`cranksite`, - :at:`slidersite`, :at:`site`, :at:`refsite`, :at:`user` + :at:`name`, :at:`class`, :at:`group`, :at:`delay`, :at:`ctrllimited`, :at:`forcelimited`, :at:`ctrlrange`, + :at:`forcerange`, :at:`lengthrange`, :at:`gear`, :at:`cranklength`, :at:`joint`, :at:`jointinparent`, :at:`tendon`, + :at:`cranksite`, :at:`slidersite`, :at:`site`, :at:`refsite`, :at:`user` |actuator/motor attrib list| Same as in actuator/ :ref:`general `. @@ -5535,6 +5572,12 @@ This element has one custom attribute in addition to the common attributes: .. _actuator-position-group: +.. _actuator-position-delay: + +.. _actuator-position-nsample: + +.. _actuator-position-interp: + .. _actuator-position-ctrllimited: .. _actuator-position-forcelimited: @@ -5566,9 +5609,9 @@ This element has one custom attribute in addition to the common attributes: .. _actuator-position-user: .. |actuator/position attrib list| replace:: - :at:`name`, :at:`class`, :at:`group`, :at:`ctrllimited`, :at:`forcelimited`, :at:`ctrlrange`, :at:`forcerange`, - :at:`lengthrange`, :at:`gear`, :at:`cranklength`, :at:`joint`, :at:`jointinparent`, :at:`tendon`, :at:`cranksite`, - :at:`slidersite`, :at:`site`, :at:`refsite`, :at:`user` + :at:`name`, :at:`class`, :at:`group`, :at:`delay`, :at:`ctrllimited`, :at:`forcelimited`, :at:`ctrlrange`, + :at:`forcerange`, :at:`lengthrange`, :at:`gear`, :at:`cranklength`, :at:`joint`, :at:`jointinparent`, :at:`tendon`, + :at:`cranksite`, :at:`slidersite`, :at:`site`, :at:`refsite`, :at:`user` |actuator/position attrib list| Same as in actuator/ :ref:`general `. @@ -5648,6 +5691,12 @@ This element has one custom attribute in addition to the common attributes: .. _actuator-velocity-group: +.. _actuator-velocity-delay: + +.. _actuator-velocity-nsample: + +.. _actuator-velocity-interp: + .. _actuator-velocity-ctrllimited: .. _actuator-velocity-forcelimited: @@ -5679,9 +5728,9 @@ This element has one custom attribute in addition to the common attributes: .. _actuator-velocity-user: .. |actuator/velocity attrib list| replace:: - :at:`name`, :at:`class`, :at:`group`, :at:`ctrllimited`, :at:`forcelimited`, :at:`ctrlrange`, :at:`forcerange`, - :at:`lengthrange`, :at:`gear`, :at:`cranklength`, :at:`joint`, :at:`jointinparent`, :at:`tendon`, :at:`cranksite`, - :at:`slidersite`, :at:`site`, :at:`refsite`, :at:`user` + :at:`name`, :at:`class`, :at:`group`, :at:`delay`, :at:`ctrllimited`, :at:`forcelimited`, :at:`ctrlrange`, + :at:`forcerange`, :at:`lengthrange`, :at:`gear`, :at:`cranklength`, :at:`joint`, :at:`jointinparent`, :at:`tendon`, + :at:`cranksite`, :at:`slidersite`, :at:`site`, :at:`refsite`, :at:`user` |actuator/velocity attrib list| Same as in actuator/ :ref:`general `. @@ -5718,6 +5767,12 @@ This element has one custom attribute in addition to the common attributes: .. _actuator-intvelocity-group: +.. _actuator-intvelocity-delay: + +.. _actuator-intvelocity-nsample: + +.. _actuator-intvelocity-interp: + .. _actuator-intvelocity-ctrllimited: .. _actuator-intvelocity-forcelimited: @@ -5751,9 +5806,9 @@ This element has one custom attribute in addition to the common attributes: .. _actuator-intvelocity-user: .. |actuator/intvelocity attrib list| replace:: - :at:`name`, :at:`class`, :at:`group`, :at:`ctrllimited`, :at:`forcelimited`, :at:`ctrlrange`, :at:`forcerange`, - :at:`actrange`, :at:`lengthrange`, :at:`gear`, :at:`cranklength`, :at:`joint`, :at:`jointinparent`, :at:`tendon`, - :at:`cranksite`, :at:`slidersite`, :at:`site`, :at:`refsite`, :at:`user` + :at:`name`, :at:`class`, :at:`group`, :at:`delay`, :at:`ctrllimited`, :at:`forcelimited`, :at:`ctrlrange`, + :at:`forcerange`, :at:`actrange`, :at:`lengthrange`, :at:`gear`, :at:`cranklength`, :at:`joint`, :at:`jointinparent`, + :at:`tendon`, :at:`cranksite`, :at:`slidersite`, :at:`site`, :at:`refsite`, :at:`user` |actuator/intvelocity attrib list| Same as in actuator/ :ref:`general `. @@ -5777,8 +5832,8 @@ This element has one custom attribute in addition to the common attributes: .. _actuator-intvelocity-inheritrange: :at:`inheritrange`: :at-val:`real, "0"` - Identical to :ref:`position/inheritrange`, but sets :at:`actrange` (which has the same - length semantics as the transmission target) rather than :at:`ctrlrange` (which has velocity semantics). + Identical to :ref:`position/inheritrange`, but sets :at:`actrange` (which has the + same length semantics as the transmission target) rather than :at:`ctrlrange` (which has velocity semantics). .. _actuator-damper: @@ -5808,6 +5863,12 @@ This element has one custom attribute in addition to the common attributes: .. _actuator-damper-group: +.. _actuator-damper-delay: + +.. _actuator-damper-nsample: + +.. _actuator-damper-interp: + .. _actuator-damper-ctrllimited: .. _actuator-damper-forcelimited: @@ -5838,9 +5899,10 @@ This element has one custom attribute in addition to the common attributes: .. _actuator-damper-user: -.. |actuator/damper attrib list| replace:: :at:`name`, :at:`class`, :at:`group`, :at:`ctrllimited`, :at:`forcelimited`, - :at:`ctrlrange`, :at:`forcerange`, :at:`lengthrange`, :at:`gear`, :at:`cranklength`, :at:`joint`, - :at:`jointinparent`, :at:`tendon`, :at:`cranksite`, :at:`slidersite`, :at:`site`, :at:`refsite`, :at:`user` +.. |actuator/damper attrib list| replace:: + :at:`name`, :at:`class`, :at:`group`, :at:`delay`, :at:`ctrllimited`, :at:`forcelimited`, :at:`ctrlrange`, + :at:`forcerange`, :at:`lengthrange`, :at:`gear`, :at:`cranklength`, :at:`joint`, :at:`jointinparent`, :at:`tendon`, + :at:`cranksite`, :at:`slidersite`, :at:`site`, :at:`refsite`, :at:`user` |actuator/damper attrib list| Same as in actuator/ :ref:`general `. @@ -5876,6 +5938,12 @@ This element has four custom attributes in addition to the common attributes: .. _actuator-cylinder-group: +.. _actuator-cylinder-delay: + +.. _actuator-cylinder-nsample: + +.. _actuator-cylinder-interp: + .. _actuator-cylinder-ctrllimited: .. _actuator-cylinder-forcelimited: @@ -5907,9 +5975,9 @@ This element has four custom attributes in addition to the common attributes: .. _actuator-cylinder-user: .. |actuator/cylinder attrib list| replace:: - :at:`name`, :at:`class`, :at:`group`, :at:`ctrllimited`, :at:`forcelimited`, :at:`ctrlrange`, :at:`forcerange`, - :at:`lengthrange`, :at:`gear`, :at:`cranklength`, :at:`joint`, :at:`jointinparent`, :at:`tendon`, :at:`cranksite`, - :at:`slidersite`, :at:`site`, :at:`refsite`, :at:`user` + :at:`name`, :at:`class`, :at:`group`, :at:`delay`, :at:`ctrllimited`, :at:`forcelimited`, :at:`ctrlrange`, + :at:`forcerange`, :at:`lengthrange`, :at:`gear`, :at:`cranklength`, :at:`joint`, :at:`jointinparent`, :at:`tendon`, + :at:`cranksite`, :at:`slidersite`, :at:`site`, :at:`refsite`, :at:`user` |actuator/cylinder attrib list| Same as in actuator/ :ref:`general `. @@ -5960,6 +6028,12 @@ This element has nine custom attributes in addition to the common attributes: .. _actuator-muscle-group: +.. _actuator-muscle-delay: + +.. _actuator-muscle-nsample: + +.. _actuator-muscle-interp: + .. _actuator-muscle-ctrllimited: .. _actuator-muscle-forcelimited: @@ -5988,9 +6062,9 @@ This element has nine custom attributes in addition to the common attributes: .. |actuator/muscle attrib list| replace:: - :at:`name`, :at:`class`, :at:`group`, :at:`ctrllimited`, :at:`forcelimited`, :at:`ctrlrange`, :at:`forcerange`, - :at:`lengthrange`, :at:`gear`, :at:`cranklength`, :at:`joint`, :at:`jointinparent`, :at:`tendon`, :at:`cranksite`, - :at:`slidersite`, :at:`user` + :at:`name`, :at:`class`, :at:`group`, :at:`delay`, :at:`ctrllimited`, :at:`forcelimited`, :at:`ctrlrange`, + :at:`forcerange`, :at:`lengthrange`, :at:`gear`, :at:`cranklength`, :at:`joint`, :at:`jointinparent`, :at:`tendon`, + :at:`cranksite`, :at:`slidersite`, :at:`user` |actuator/muscle attrib list| Same as in actuator/ :ref:`general `. @@ -6090,6 +6164,12 @@ This element has a subset of the common attributes and two custom attributes. .. _actuator-adhesion-group: +.. _actuator-adhesion-delay: + +.. _actuator-adhesion-nsample: + +.. _actuator-adhesion-interp: + .. _actuator-adhesion-forcelimited: .. _actuator-adhesion-ctrlrange: @@ -6098,7 +6178,7 @@ This element has a subset of the common attributes and two custom attributes. .. _actuator-adhesion-user: -.. |actuator/adhesion attrib list| replace:: :at:`name`, :at:`class`, :at:`group`, +.. |actuator/adhesion attrib list| replace:: :at:`name`, :at:`class`, :at:`group`, :at:`delay`, :at:`forcelimited`, :at:`ctrlrange`, :at:`forcerange`, :at:`user` |actuator/adhesion attrib list| @@ -6155,6 +6235,12 @@ Associate this actuator with an :ref:`engine plugin`. Either :at:`plug .. _actuator-plugin-group: +.. _actuator-plugin-delay: + +.. _actuator-plugin-nsample: + +.. _actuator-plugin-interp: + .. _actuator-plugin-actlimited: .. _actuator-plugin-ctrllimited: @@ -6191,10 +6277,11 @@ Associate this actuator with an :ref:`engine plugin`. Either :at:`plug .. _actuator-plugin-actearly: -.. |actuator/plugin attrib list| replace:: :at:`name`, :at:`class`, :at:`group`, :at:`actlimited`, :at:`ctrllimited`, - :at:`forcelimited`, :at:`ctrlrange`, :at:`forcerange`, :at:`lengthrange`, :at:`gear`, :at:`cranklength`, - :at:`joint`, :at:`jointinparent`, :at:`site`, :at:`tendon`, :at:`cranksite`, :at:`slidersite`, :at:`user`, - :at:`actdim`, :at:`dynprm`, :at:`actearly` +.. |actuator/plugin attrib list| replace:: + :at:`name`, :at:`class`, :at:`group`, :at:`delay`, :at:`actlimited`, :at:`ctrllimited`, :at:`forcelimited`, + :at:`ctrlrange`, :at:`forcerange`, :at:`lengthrange`, :at:`gear`, :at:`cranklength`, :at:`joint`, + :at:`jointinparent`, :at:`site`, :at:`tendon`, :at:`cranksite`, :at:`slidersite`, :at:`user`, :at:`actdim`, + :at:`dynprm`, :at:`actearly` |actuator/plugin attrib list| Same as in actuator/ :ref:`general `. @@ -6234,9 +6321,17 @@ scalar. It is computed by adding up the (scalar) normal forces from all included .. _sensor-touch-cutoff: +.. _sensor-touch-nsample: + +.. _sensor-touch-interp: + +.. _sensor-touch-interval: + +.. _sensor-touch-delay: + .. _sensor-touch-user: -:at:`name`, :at:`noise`, :at:`cutoff`, :at:`user` +:at:`name`, :at:`noise`, :at:`cutoff`, :at:`nsample`, :at:`interval`, :at:`delay`, :at:`user` See :ref:`CSensor`. .. _sensor-touch-site: @@ -6262,9 +6357,17 @@ The presence of this sensor in a model triggers a call to :ref:`mj_rnePostConstr .. _sensor-accelerometer-cutoff: +.. _sensor-accelerometer-nsample: + +.. _sensor-accelerometer-interp: + +.. _sensor-accelerometer-interval: + +.. _sensor-accelerometer-delay: + .. _sensor-accelerometer-user: -:at:`name`, :at:`noise`, :at:`cutoff`, :at:`user` +:at:`name`, :at:`noise`, :at:`cutoff`, :at:`nsample`, :at:`interval`, :at:`delay`, :at:`user` See :ref:`CSensor`. .. _sensor-accelerometer-site: @@ -6287,9 +6390,17 @@ the site frame. This sensor outputs three numbers, which are the linear velocity .. _sensor-velocimeter-cutoff: +.. _sensor-velocimeter-nsample: + +.. _sensor-velocimeter-interp: + +.. _sensor-velocimeter-interval: + +.. _sensor-velocimeter-delay: + .. _sensor-velocimeter-user: -:at:`name`, :at:`noise`, :at:`cutoff`, :at:`user` +:at:`name`, :at:`noise`, :at:`cutoff`, :at:`nsample`, :at:`interval`, :at:`delay`, :at:`user` See :ref:`CSensor`. .. _sensor-velocimeter-site: @@ -6314,9 +6425,17 @@ simulate an inertial measurement unit (IMU). .. _sensor-gyro-cutoff: +.. _sensor-gyro-nsample: + +.. _sensor-gyro-interp: + +.. _sensor-gyro-interval: + +.. _sensor-gyro-delay: + .. _sensor-gyro-user: -:at:`name`, :at:`noise`, :at:`cutoff`, :at:`user` +:at:`name`, :at:`noise`, :at:`cutoff`, :at:`nsample`, :at:`interval`, :at:`delay`, :at:`user` See :ref:`CSensor`. .. _sensor-gyro-site: @@ -6344,9 +6463,17 @@ The presence of this sensor in a model triggers a call to :ref:`mj_rnePostConstr .. _sensor-force-cutoff: +.. _sensor-force-nsample: + +.. _sensor-force-interp: + +.. _sensor-force-interval: + +.. _sensor-force-delay: + .. _sensor-force-user: -:at:`name`, :at:`noise`, :at:`cutoff`, :at:`user` +:at:`name`, :at:`noise`, :at:`cutoff`, :at:`nsample`, :at:`interval`, :at:`delay`, :at:`user` See :ref:`CSensor`. .. _sensor-force-site: @@ -6375,9 +6502,17 @@ The presence of this sensor in a model triggers a call to :ref:`mj_rnePostConstr .. _sensor-torque-cutoff: +.. _sensor-torque-nsample: + +.. _sensor-torque-interp: + +.. _sensor-torque-interval: + +.. _sensor-torque-delay: + .. _sensor-torque-user: -:at:`name`, :at:`noise`, :at:`cutoff`, :at:`user` +:at:`name`, :at:`noise`, :at:`cutoff`, :at:`nsample`, :at:`interval`, :at:`delay`, :at:`user` See :ref:`CSensor`. .. _sensor-torque-site: @@ -6401,9 +6536,17 @@ site frame. The output is a 3D vector. .. _sensor-magnetometer-cutoff: +.. _sensor-magnetometer-nsample: + +.. _sensor-magnetometer-interp: + +.. _sensor-magnetometer-interval: + +.. _sensor-magnetometer-delay: + .. _sensor-magnetometer-user: -:at:`name`, :at:`noise`, :at:`cutoff`, :at:`user` +:at:`name`, :at:`noise`, :at:`cutoff`, :at:`nsample`, :at:`interval`, :at:`delay`, :at:`user` See :ref:`CSensor`. .. _sensor-magnetometer-site: @@ -6471,9 +6614,17 @@ points (spheres) and the surface normals (arrows). .. _sensor-rangefinder-cutoff: +.. _sensor-rangefinder-nsample: + +.. _sensor-rangefinder-interp: + +.. _sensor-rangefinder-interval: + +.. _sensor-rangefinder-delay: + .. _sensor-rangefinder-user: -:at:`name`, :at:`noise`, :at:`cutoff`, :at:`user` +:at:`name`, :at:`noise`, :at:`cutoff`, :at:`nsample`, :at:`interval`, :at:`delay`, :at:`user` See :ref:`CSensor`. .. _sensor-rangefinder-site: @@ -6514,9 +6665,17 @@ z-coordinate indicates a location in front of/behind the camera plane, respectiv .. _sensor-camprojection-cutoff: +.. _sensor-camprojection-nsample: + +.. _sensor-camprojection-interp: + +.. _sensor-camprojection-interval: + +.. _sensor-camprojection-delay: + .. _sensor-camprojection-user: -:at:`name`, :at:`noise`, :at:`cutoff`, :at:`user` +:at:`name`, :at:`noise`, :at:`cutoff`, :at:`nsample`, :at:`interval`, :at:`delay`, :at:`user` See :ref:`CSensor`. .. _sensor-jointpos: @@ -6534,9 +6693,17 @@ attached to scalar joints (slide or hinge). Its output is scalar. .. _sensor-jointpos-cutoff: +.. _sensor-jointpos-nsample: + +.. _sensor-jointpos-interp: + +.. _sensor-jointpos-interval: + +.. _sensor-jointpos-delay: + .. _sensor-jointpos-user: -:at:`name`, :at:`noise`, :at:`cutoff`, :at:`user` +:at:`name`, :at:`noise`, :at:`cutoff`, :at:`nsample`, :at:`interval`, :at:`delay`, :at:`user` See :ref:`CSensor`. .. _sensor-jointpos-joint: @@ -6560,9 +6727,17 @@ scalar. .. _sensor-jointvel-cutoff: +.. _sensor-jointvel-nsample: + +.. _sensor-jointvel-interp: + +.. _sensor-jointvel-interval: + +.. _sensor-jointvel-delay: + .. _sensor-jointvel-user: -:at:`name`, :at:`noise`, :at:`cutoff`, :at:`user` +:at:`name`, :at:`noise`, :at:`cutoff`, :at:`nsample`, :at:`interval`, :at:`delay`, :at:`user` See :ref:`CSensor`. .. _sensor-jointvel-joint: @@ -6585,9 +6760,17 @@ This element creates a tendon length sensor. It can be attached to both spatial .. _sensor-tendonpos-cutoff: +.. _sensor-tendonpos-nsample: + +.. _sensor-tendonpos-interp: + +.. _sensor-tendonpos-interval: + +.. _sensor-tendonpos-delay: + .. _sensor-tendonpos-user: -:at:`name`, :at:`noise`, :at:`cutoff`, :at:`user` +:at:`name`, :at:`noise`, :at:`cutoff`, :at:`nsample`, :at:`interval`, :at:`delay`, :at:`user` See :ref:`CSensor`. .. _sensor-tendonpos-tendon: @@ -6610,9 +6793,17 @@ scalar. .. _sensor-tendonvel-cutoff: +.. _sensor-tendonvel-nsample: + +.. _sensor-tendonvel-interp: + +.. _sensor-tendonvel-interval: + +.. _sensor-tendonvel-delay: + .. _sensor-tendonvel-user: -:at:`name`, :at:`noise`, :at:`cutoff`, :at:`user` +:at:`name`, :at:`noise`, :at:`cutoff`, :at:`nsample`, :at:`interval`, :at:`delay`, :at:`user` See :ref:`CSensor`. .. _sensor-tendonvel-tendon: @@ -6635,9 +6826,17 @@ sensor can be attached to any actuator. Its output is scalar. .. _sensor-actuatorpos-cutoff: +.. _sensor-actuatorpos-nsample: + +.. _sensor-actuatorpos-interp: + +.. _sensor-actuatorpos-interval: + +.. _sensor-actuatorpos-delay: + .. _sensor-actuatorpos-user: -:at:`name`, :at:`noise`, :at:`cutoff`, :at:`user` +:at:`name`, :at:`noise`, :at:`cutoff`, :at:`nsample`, :at:`interval`, :at:`delay`, :at:`user` See :ref:`CSensor`. .. _sensor-actuatorpos-actuator: @@ -6659,9 +6858,17 @@ This element creates an actuator velocity sensor. This sensor can be attached to .. _sensor-actuatorvel-cutoff: +.. _sensor-actuatorvel-nsample: + +.. _sensor-actuatorvel-interp: + +.. _sensor-actuatorvel-interval: + +.. _sensor-actuatorvel-delay: + .. _sensor-actuatorvel-user: -:at:`name`, :at:`noise`, :at:`cutoff`, :at:`user` +:at:`name`, :at:`noise`, :at:`cutoff`, :at:`nsample`, :at:`interval`, :at:`delay`, :at:`user` See :ref:`CSensor`. .. _sensor-actuatorvel-actuator: @@ -6685,9 +6892,17 @@ arms determined by the transmission). This sensor can be attached to any actuato .. _sensor-actuatorfrc-cutoff: +.. _sensor-actuatorfrc-nsample: + +.. _sensor-actuatorfrc-interp: + +.. _sensor-actuatorfrc-interval: + +.. _sensor-actuatorfrc-delay: + .. _sensor-actuatorfrc-user: -:at:`name`, :at:`noise`, :at:`cutoff`, :at:`user` +:at:`name`, :at:`noise`, :at:`cutoff`, :at:`nsample`, :at:`interval`, :at:`delay`, :at:`user` See :ref:`CSensor`. .. _sensor-actuatorfrc-actuator: @@ -6715,9 +6930,17 @@ joint or when a single actuator acts on multiple joints. See :ref:`CForceRange` .. _sensor-jointactuatorfrc-cutoff: +.. _sensor-jointactuatorfrc-nsample: + +.. _sensor-jointactuatorfrc-interp: + +.. _sensor-jointactuatorfrc-interval: + +.. _sensor-jointactuatorfrc-delay: + .. _sensor-jointactuatorfrc-user: -:at:`name`, :at:`noise`, :at:`cutoff`, :at:`user` +:at:`name`, :at:`noise`, :at:`cutoff`, :at:`nsample`, :at:`interval`, :at:`delay`, :at:`user` See :ref:`CSensor`. .. _sensor-jointactuatorfrc-joint: @@ -6742,9 +6965,17 @@ single tendon. See :ref:`CForceRange` for details. .. _sensor-tendonactuatorfrc-cutoff: +.. _sensor-tendonactuatorfrc-nsample: + +.. _sensor-tendonactuatorfrc-interp: + +.. _sensor-tendonactuatorfrc-interval: + +.. _sensor-tendonactuatorfrc-delay: + .. _sensor-tendonactuatorfrc-user: -:at:`name`, :at:`noise`, :at:`cutoff`, :at:`user` +:at:`name`, :at:`noise`, :at:`cutoff`, :at:`nsample`, :at:`interval`, :at:`delay`, :at:`user` See :ref:`CSensor`. .. _sensor-tendonactuatorfrc-tendon: @@ -6766,9 +6997,17 @@ This element creates a quaternion sensor for a ball joints. It outputs 4 numbers .. _sensor-ballquat-cutoff: +.. _sensor-ballquat-nsample: + +.. _sensor-ballquat-interp: + +.. _sensor-ballquat-interval: + +.. _sensor-ballquat-delay: + .. _sensor-ballquat-user: -:at:`name`, :at:`noise`, :at:`cutoff`, :at:`user` +:at:`name`, :at:`noise`, :at:`cutoff`, :at:`nsample`, :at:`interval`, :at:`delay`, :at:`user` See :ref:`CSensor`. .. _sensor-ballquat-joint: @@ -6792,9 +7031,17 @@ rotation takes place. .. _sensor-ballangvel-cutoff: +.. _sensor-ballangvel-nsample: + +.. _sensor-ballangvel-interp: + +.. _sensor-ballangvel-interval: + +.. _sensor-ballangvel-delay: + .. _sensor-ballangvel-user: -:at:`name`, :at:`noise`, :at:`cutoff`, :at:`user` +:at:`name`, :at:`noise`, :at:`cutoff`, :at:`nsample`, :at:`interval`, :at:`delay`, :at:`user` See :ref:`CSensor`. .. _sensor-ballangvel-joint: @@ -6816,9 +7063,17 @@ This element creates a joint limit sensor for position. .. _sensor-jointlimitpos-cutoff: +.. _sensor-jointlimitpos-nsample: + +.. _sensor-jointlimitpos-interp: + +.. _sensor-jointlimitpos-interval: + +.. _sensor-jointlimitpos-delay: + .. _sensor-jointlimitpos-user: -:at:`name`, :at:`noise`, :at:`cutoff`, :at:`user` +:at:`name`, :at:`noise`, :at:`cutoff`, :at:`nsample`, :at:`interval`, :at:`delay`, :at:`user` See :ref:`CSensor`. .. _sensor-jointlimitpos-joint: @@ -6843,9 +7098,17 @@ This element creates a joint limit sensor for velocity. .. _sensor-jointlimitvel-cutoff: +.. _sensor-jointlimitvel-nsample: + +.. _sensor-jointlimitvel-interp: + +.. _sensor-jointlimitvel-interval: + +.. _sensor-jointlimitvel-delay: + .. _sensor-jointlimitvel-user: -:at:`name`, :at:`noise`, :at:`cutoff`, :at:`user` +:at:`name`, :at:`noise`, :at:`cutoff`, :at:`nsample`, :at:`interval`, :at:`delay`, :at:`user` See :ref:`CSensor`. .. _sensor-jointlimitvel-joint: @@ -6868,9 +7131,17 @@ This element creates a joint limit sensor for constraint force. .. _sensor-jointlimitfrc-cutoff: +.. _sensor-jointlimitfrc-nsample: + +.. _sensor-jointlimitfrc-interp: + +.. _sensor-jointlimitfrc-interval: + +.. _sensor-jointlimitfrc-delay: + .. _sensor-jointlimitfrc-user: -:at:`name`, :at:`noise`, :at:`cutoff`, :at:`user` +:at:`name`, :at:`noise`, :at:`cutoff`, :at:`nsample`, :at:`interval`, :at:`delay`, :at:`user` See :ref:`CSensor`. .. _sensor-jointlimitfrc-joint: @@ -6893,9 +7164,17 @@ This element creates a tendon limit sensor for position. .. _sensor-tendonlimitpos-cutoff: +.. _sensor-tendonlimitpos-nsample: + +.. _sensor-tendonlimitpos-interp: + +.. _sensor-tendonlimitpos-interval: + +.. _sensor-tendonlimitpos-delay: + .. _sensor-tendonlimitpos-user: -:at:`name`, :at:`noise`, :at:`cutoff`, :at:`user` +:at:`name`, :at:`noise`, :at:`cutoff`, :at:`nsample`, :at:`interval`, :at:`delay`, :at:`user` See :ref:`CSensor`. .. _sensor-tendonlimitpos-tendon: @@ -6918,9 +7197,17 @@ This element creates a tendon limit sensor for velocity. .. _sensor-tendonlimitvel-cutoff: +.. _sensor-tendonlimitvel-nsample: + +.. _sensor-tendonlimitvel-interp: + +.. _sensor-tendonlimitvel-interval: + +.. _sensor-tendonlimitvel-delay: + .. _sensor-tendonlimitvel-user: -:at:`name`, :at:`noise`, :at:`cutoff`, :at:`user` +:at:`name`, :at:`noise`, :at:`cutoff`, :at:`nsample`, :at:`interval`, :at:`delay`, :at:`user` See :ref:`CSensor`. .. _sensor-tendonlimitvel-tendon: @@ -6943,9 +7230,17 @@ This element creates a tendon limit sensor for constraint force. .. _sensor-tendonlimitfrc-cutoff: +.. _sensor-tendonlimitfrc-nsample: + +.. _sensor-tendonlimitfrc-interp: + +.. _sensor-tendonlimitfrc-interval: + +.. _sensor-tendonlimitfrc-delay: + .. _sensor-tendonlimitfrc-user: -:at:`name`, :at:`noise`, :at:`cutoff`, :at:`user` +:at:`name`, :at:`noise`, :at:`cutoff`, :at:`nsample`, :at:`interval`, :at:`delay`, :at:`user` See :ref:`CSensor`. .. _sensor-tendonlimitfrc-tendon: @@ -6969,9 +7264,17 @@ optionally with respect to a given frame-of-reference. .. _sensor-framepos-cutoff: +.. _sensor-framepos-nsample: + +.. _sensor-framepos-interp: + +.. _sensor-framepos-interval: + +.. _sensor-framepos-delay: + .. _sensor-framepos-user: -:at:`name`, :at:`noise`, :at:`cutoff`, :at:`user` +:at:`name`, :at:`noise`, :at:`cutoff`, :at:`nsample`, :at:`interval`, :at:`delay`, :at:`user` See :ref:`CSensor`. .. _sensor-framepos-objtype: @@ -7013,9 +7316,17 @@ object, in global coordinates. .. _sensor-framequat-cutoff: +.. _sensor-framequat-nsample: + +.. _sensor-framequat-interp: + +.. _sensor-framequat-interval: + +.. _sensor-framequat-delay: + .. _sensor-framequat-user: -:at:`name`, :at:`noise`, :at:`cutoff`, :at:`user` +:at:`name`, :at:`noise`, :at:`cutoff`, :at:`nsample`, :at:`interval`, :at:`delay`, :at:`user` See :ref:`CSensor`. .. _sensor-framequat-objtype: @@ -7053,9 +7364,17 @@ object, in global coordinates. .. _sensor-framexaxis-cutoff: +.. _sensor-framexaxis-nsample: + +.. _sensor-framexaxis-interp: + +.. _sensor-framexaxis-interval: + +.. _sensor-framexaxis-delay: + .. _sensor-framexaxis-user: -:at:`name`, :at:`noise`, :at:`cutoff`, :at:`user` +:at:`name`, :at:`noise`, :at:`cutoff`, :at:`nsample`, :at:`interval`, :at:`delay`, :at:`user` See :ref:`CSensor`. .. _sensor-framexaxis-objtype: @@ -7093,9 +7412,17 @@ object, in global coordinates. .. _sensor-frameyaxis-cutoff: +.. _sensor-frameyaxis-nsample: + +.. _sensor-frameyaxis-interp: + +.. _sensor-frameyaxis-interval: + +.. _sensor-frameyaxis-delay: + .. _sensor-frameyaxis-user: -:at:`name`, :at:`noise`, :at:`cutoff`, :at:`user` +:at:`name`, :at:`noise`, :at:`cutoff`, :at:`nsample`, :at:`interval`, :at:`delay`, :at:`user` See :ref:`CSensor`. .. _sensor-frameyaxis-objtype: @@ -7133,9 +7460,17 @@ object, in global coordinates. .. _sensor-framezaxis-cutoff: +.. _sensor-framezaxis-nsample: + +.. _sensor-framezaxis-interp: + +.. _sensor-framezaxis-interval: + +.. _sensor-framezaxis-delay: + .. _sensor-framezaxis-user: -:at:`name`, :at:`noise`, :at:`cutoff`, :at:`user` +:at:`name`, :at:`noise`, :at:`cutoff`, :at:`nsample`, :at:`interval`, :at:`delay`, :at:`user` See :ref:`CSensor`. .. _sensor-framezaxis-objtype: @@ -7173,9 +7508,17 @@ coordinates. .. _sensor-framelinvel-cutoff: +.. _sensor-framelinvel-nsample: + +.. _sensor-framelinvel-interp: + +.. _sensor-framelinvel-interval: + +.. _sensor-framelinvel-delay: + .. _sensor-framelinvel-user: -:at:`name`, :at:`noise`, :at:`cutoff`, :at:`user` +:at:`name`, :at:`noise`, :at:`cutoff`, :at:`nsample`, :at:`interval`, :at:`delay`, :at:`user` See :ref:`CSensor`. .. _sensor-framelinvel-objtype: @@ -7213,9 +7556,17 @@ coordinates. .. _sensor-frameangvel-cutoff: +.. _sensor-frameangvel-nsample: + +.. _sensor-frameangvel-interp: + +.. _sensor-frameangvel-interval: + +.. _sensor-frameangvel-delay: + .. _sensor-frameangvel-user: -:at:`name`, :at:`noise`, :at:`cutoff`, :at:`user` +:at:`name`, :at:`noise`, :at:`cutoff`, :at:`nsample`, :at:`interval`, :at:`delay`, :at:`user` See :ref:`CSensor`. .. _sensor-frameangvel-objtype: @@ -7255,9 +7606,17 @@ The presence of this sensor in a model triggers a call to :ref:`mj_rnePostConstr .. _sensor-framelinacc-cutoff: +.. _sensor-framelinacc-nsample: + +.. _sensor-framelinacc-interp: + +.. _sensor-framelinacc-interval: + +.. _sensor-framelinacc-delay: + .. _sensor-framelinacc-user: -:at:`name`, :at:`noise`, :at:`cutoff`, :at:`user` +:at:`name`, :at:`noise`, :at:`cutoff`, :at:`nsample`, :at:`interval`, :at:`delay`, :at:`user` See :ref:`CSensor`. .. _sensor-framelinacc-objtype: @@ -7287,9 +7646,17 @@ The presence of this sensor in a model triggers a call to :ref:`mj_rnePostConstr .. _sensor-frameangacc-cutoff: +.. _sensor-frameangacc-nsample: + +.. _sensor-frameangacc-interp: + +.. _sensor-frameangacc-interval: + +.. _sensor-frameangacc-delay: + .. _sensor-frameangacc-user: -:at:`name`, :at:`noise`, :at:`cutoff`, :at:`user` +:at:`name`, :at:`noise`, :at:`cutoff`, :at:`nsample`, :at:`interval`, :at:`delay`, :at:`user` See :ref:`CSensor`. .. _sensor-frameangacc-objtype: @@ -7317,9 +7684,17 @@ global coordinates. .. _sensor-subtreecom-cutoff: +.. _sensor-subtreecom-nsample: + +.. _sensor-subtreecom-interp: + +.. _sensor-subtreecom-interval: + +.. _sensor-subtreecom-delay: + .. _sensor-subtreecom-user: -:at:`name`, :at:`noise`, :at:`cutoff`, :at:`user` +:at:`name`, :at:`noise`, :at:`cutoff`, :at:`nsample`, :at:`interval`, :at:`delay`, :at:`user` See :ref:`CSensor`. .. _sensor-subtreecom-body: @@ -7344,9 +7719,17 @@ The presence of this sensor in a model triggers a call to :ref:`mj_subtreeVel` d .. _sensor-subtreelinvel-cutoff: +.. _sensor-subtreelinvel-nsample: + +.. _sensor-subtreelinvel-interp: + +.. _sensor-subtreelinvel-interval: + +.. _sensor-subtreelinvel-delay: + .. _sensor-subtreelinvel-user: -:at:`name`, :at:`noise`, :at:`cutoff`, :at:`user` +:at:`name`, :at:`noise`, :at:`cutoff`, :at:`nsample`, :at:`interval`, :at:`delay`, :at:`user` See :ref:`CSensor`. .. _sensor-subtreelinvel-body: @@ -7371,9 +7754,17 @@ The presence of this sensor in a model triggers a call to :ref:`mj_subtreeVel` d .. _sensor-subtreeangmom-cutoff: +.. _sensor-subtreeangmom-nsample: + +.. _sensor-subtreeangmom-interp: + +.. _sensor-subtreeangmom-interval: + +.. _sensor-subtreeangmom-delay: + .. _sensor-subtreeangmom-user: -:at:`name`, :at:`noise`, :at:`cutoff`, :at:`user` +:at:`name`, :at:`noise`, :at:`cutoff`, :at:`nsample`, :at:`interval`, :at:`delay`, :at:`user` See :ref:`CSensor`. .. _sensor-subtreeangmom-body: @@ -7396,9 +7787,17 @@ See `example model ` + .. grid-item:: + :ref:`nsample` + + .. grid-item:: + :ref:`interp` + + .. grid-item:: + :ref:`delay` + .. grid-item:: :ref:`ctrllimited` @@ -2329,6 +2338,15 @@ .. grid-item:: :ref:`group` + .. grid-item:: + :ref:`nsample` + + .. grid-item:: + :ref:`interp` + + .. grid-item:: + :ref:`delay` + .. grid-item:: :ref:`ctrllimited` @@ -2389,6 +2407,15 @@ .. grid-item:: :ref:`group` + .. grid-item:: + :ref:`nsample` + + .. grid-item:: + :ref:`interp` + + .. grid-item:: + :ref:`delay` + .. grid-item:: :ref:`ctrllimited` @@ -2464,6 +2491,15 @@ .. grid-item:: :ref:`group` + .. grid-item:: + :ref:`nsample` + + .. grid-item:: + :ref:`interp` + + .. grid-item:: + :ref:`delay` + .. grid-item:: :ref:`ctrllimited` @@ -2527,6 +2563,15 @@ .. grid-item:: :ref:`group` + .. grid-item:: + :ref:`nsample` + + .. grid-item:: + :ref:`interp` + + .. grid-item:: + :ref:`delay` + .. grid-item:: :ref:`ctrllimited` @@ -2602,6 +2647,15 @@ .. grid-item:: :ref:`group` + .. grid-item:: + :ref:`nsample` + + .. grid-item:: + :ref:`interp` + + .. grid-item:: + :ref:`delay` + .. grid-item:: :ref:`forcelimited` @@ -2662,6 +2716,15 @@ .. grid-item:: :ref:`group` + .. grid-item:: + :ref:`nsample` + + .. grid-item:: + :ref:`interp` + + .. grid-item:: + :ref:`delay` + .. grid-item:: :ref:`ctrllimited` @@ -2734,6 +2797,15 @@ .. grid-item:: :ref:`group` + .. grid-item:: + :ref:`nsample` + + .. grid-item:: + :ref:`interp` + + .. grid-item:: + :ref:`delay` + .. grid-item:: :ref:`ctrllimited` @@ -2818,6 +2890,15 @@ .. grid-item:: :ref:`group` + .. grid-item:: + :ref:`nsample` + + .. grid-item:: + :ref:`interp` + + .. grid-item:: + :ref:`delay` + .. grid-item:: :ref:`forcelimited` @@ -2857,6 +2938,15 @@ .. grid-item:: :ref:`group` + .. grid-item:: + :ref:`nsample` + + .. grid-item:: + :ref:`interp` + + .. grid-item:: + :ref:`delay` + .. grid-item:: :ref:`ctrllimited` @@ -2944,6 +3034,18 @@ .. grid-item:: :ref:`site` + .. grid-item:: + :ref:`nsample` + + .. grid-item:: + :ref:`interp` + + .. grid-item:: + :ref:`delay` + + .. grid-item:: + :ref:`interval` + .. grid-item:: :ref:`cutoff` @@ -2965,6 +3067,18 @@ .. grid-item:: :ref:`site` + .. grid-item:: + :ref:`nsample` + + .. grid-item:: + :ref:`interp` + + .. grid-item:: + :ref:`delay` + + .. grid-item:: + :ref:`interval` + .. grid-item:: :ref:`cutoff` @@ -2986,6 +3100,18 @@ .. grid-item:: :ref:`site` + .. grid-item:: + :ref:`nsample` + + .. grid-item:: + :ref:`interp` + + .. grid-item:: + :ref:`delay` + + .. grid-item:: + :ref:`interval` + .. grid-item:: :ref:`cutoff` @@ -3007,6 +3133,18 @@ .. grid-item:: :ref:`site` + .. grid-item:: + :ref:`nsample` + + .. grid-item:: + :ref:`interp` + + .. grid-item:: + :ref:`delay` + + .. grid-item:: + :ref:`interval` + .. grid-item:: :ref:`cutoff` @@ -3028,6 +3166,18 @@ .. grid-item:: :ref:`site` + .. grid-item:: + :ref:`nsample` + + .. grid-item:: + :ref:`interp` + + .. grid-item:: + :ref:`delay` + + .. grid-item:: + :ref:`interval` + .. grid-item:: :ref:`cutoff` @@ -3049,6 +3199,18 @@ .. grid-item:: :ref:`site` + .. grid-item:: + :ref:`nsample` + + .. grid-item:: + :ref:`interp` + + .. grid-item:: + :ref:`delay` + + .. grid-item:: + :ref:`interval` + .. grid-item:: :ref:`cutoff` @@ -3070,6 +3232,18 @@ .. grid-item:: :ref:`site` + .. grid-item:: + :ref:`nsample` + + .. grid-item:: + :ref:`interp` + + .. grid-item:: + :ref:`delay` + + .. grid-item:: + :ref:`interval` + .. grid-item:: :ref:`cutoff` @@ -3094,6 +3268,18 @@ .. grid-item:: :ref:`camera` + .. grid-item:: + :ref:`nsample` + + .. grid-item:: + :ref:`interp` + + .. grid-item:: + :ref:`delay` + + .. grid-item:: + :ref:`interval` + .. grid-item:: :ref:`cutoff` @@ -3121,6 +3307,18 @@ .. grid-item:: :ref:`data` + .. grid-item:: + :ref:`nsample` + + .. grid-item:: + :ref:`interp` + + .. grid-item:: + :ref:`delay` + + .. grid-item:: + :ref:`interval` + .. grid-item:: :ref:`cutoff` @@ -3142,6 +3340,18 @@ .. grid-item:: :ref:`joint` + .. grid-item:: + :ref:`nsample` + + .. grid-item:: + :ref:`interp` + + .. grid-item:: + :ref:`delay` + + .. grid-item:: + :ref:`interval` + .. grid-item:: :ref:`cutoff` @@ -3163,6 +3373,18 @@ .. grid-item:: :ref:`joint` + .. grid-item:: + :ref:`nsample` + + .. grid-item:: + :ref:`interp` + + .. grid-item:: + :ref:`delay` + + .. grid-item:: + :ref:`interval` + .. grid-item:: :ref:`cutoff` @@ -3184,6 +3406,18 @@ .. grid-item:: :ref:`tendon` + .. grid-item:: + :ref:`nsample` + + .. grid-item:: + :ref:`interp` + + .. grid-item:: + :ref:`delay` + + .. grid-item:: + :ref:`interval` + .. grid-item:: :ref:`cutoff` @@ -3205,6 +3439,18 @@ .. grid-item:: :ref:`tendon` + .. grid-item:: + :ref:`nsample` + + .. grid-item:: + :ref:`interp` + + .. grid-item:: + :ref:`delay` + + .. grid-item:: + :ref:`interval` + .. grid-item:: :ref:`cutoff` @@ -3226,6 +3472,18 @@ .. grid-item:: :ref:`actuator` + .. grid-item:: + :ref:`nsample` + + .. grid-item:: + :ref:`interp` + + .. grid-item:: + :ref:`delay` + + .. grid-item:: + :ref:`interval` + .. grid-item:: :ref:`cutoff` @@ -3247,6 +3505,18 @@ .. grid-item:: :ref:`actuator` + .. grid-item:: + :ref:`nsample` + + .. grid-item:: + :ref:`interp` + + .. grid-item:: + :ref:`delay` + + .. grid-item:: + :ref:`interval` + .. grid-item:: :ref:`cutoff` @@ -3268,6 +3538,18 @@ .. grid-item:: :ref:`actuator` + .. grid-item:: + :ref:`nsample` + + .. grid-item:: + :ref:`interp` + + .. grid-item:: + :ref:`delay` + + .. grid-item:: + :ref:`interval` + .. grid-item:: :ref:`cutoff` @@ -3289,6 +3571,18 @@ .. grid-item:: :ref:`joint` + .. grid-item:: + :ref:`nsample` + + .. grid-item:: + :ref:`interp` + + .. grid-item:: + :ref:`delay` + + .. grid-item:: + :ref:`interval` + .. grid-item:: :ref:`cutoff` @@ -3310,6 +3604,18 @@ .. grid-item:: :ref:`tendon` + .. grid-item:: + :ref:`nsample` + + .. grid-item:: + :ref:`interp` + + .. grid-item:: + :ref:`delay` + + .. grid-item:: + :ref:`interval` + .. grid-item:: :ref:`cutoff` @@ -3331,6 +3637,18 @@ .. grid-item:: :ref:`joint` + .. grid-item:: + :ref:`nsample` + + .. grid-item:: + :ref:`interp` + + .. grid-item:: + :ref:`delay` + + .. grid-item:: + :ref:`interval` + .. grid-item:: :ref:`cutoff` @@ -3352,6 +3670,18 @@ .. grid-item:: :ref:`joint` + .. grid-item:: + :ref:`nsample` + + .. grid-item:: + :ref:`interp` + + .. grid-item:: + :ref:`delay` + + .. grid-item:: + :ref:`interval` + .. grid-item:: :ref:`cutoff` @@ -3373,6 +3703,18 @@ .. grid-item:: :ref:`joint` + .. grid-item:: + :ref:`nsample` + + .. grid-item:: + :ref:`interp` + + .. grid-item:: + :ref:`delay` + + .. grid-item:: + :ref:`interval` + .. grid-item:: :ref:`cutoff` @@ -3394,6 +3736,18 @@ .. grid-item:: :ref:`joint` + .. grid-item:: + :ref:`nsample` + + .. grid-item:: + :ref:`interp` + + .. grid-item:: + :ref:`delay` + + .. grid-item:: + :ref:`interval` + .. grid-item:: :ref:`cutoff` @@ -3415,6 +3769,18 @@ .. grid-item:: :ref:`joint` + .. grid-item:: + :ref:`nsample` + + .. grid-item:: + :ref:`interp` + + .. grid-item:: + :ref:`delay` + + .. grid-item:: + :ref:`interval` + .. grid-item:: :ref:`cutoff` @@ -3436,6 +3802,18 @@ .. grid-item:: :ref:`tendon` + .. grid-item:: + :ref:`nsample` + + .. grid-item:: + :ref:`interp` + + .. grid-item:: + :ref:`delay` + + .. grid-item:: + :ref:`interval` + .. grid-item:: :ref:`cutoff` @@ -3457,6 +3835,18 @@ .. grid-item:: :ref:`tendon` + .. grid-item:: + :ref:`nsample` + + .. grid-item:: + :ref:`interp` + + .. grid-item:: + :ref:`delay` + + .. grid-item:: + :ref:`interval` + .. grid-item:: :ref:`cutoff` @@ -3478,6 +3868,18 @@ .. grid-item:: :ref:`tendon` + .. grid-item:: + :ref:`nsample` + + .. grid-item:: + :ref:`interp` + + .. grid-item:: + :ref:`delay` + + .. grid-item:: + :ref:`interval` + .. grid-item:: :ref:`cutoff` @@ -3508,6 +3910,18 @@ .. grid-item:: :ref:`refname` + .. grid-item:: + :ref:`nsample` + + .. grid-item:: + :ref:`interp` + + .. grid-item:: + :ref:`delay` + + .. grid-item:: + :ref:`interval` + .. grid-item:: :ref:`cutoff` @@ -3538,6 +3952,18 @@ .. grid-item:: :ref:`refname` + .. grid-item:: + :ref:`nsample` + + .. grid-item:: + :ref:`interp` + + .. grid-item:: + :ref:`delay` + + .. grid-item:: + :ref:`interval` + .. grid-item:: :ref:`cutoff` @@ -3568,6 +3994,18 @@ .. grid-item:: :ref:`refname` + .. grid-item:: + :ref:`nsample` + + .. grid-item:: + :ref:`interp` + + .. grid-item:: + :ref:`delay` + + .. grid-item:: + :ref:`interval` + .. grid-item:: :ref:`cutoff` @@ -3598,6 +4036,18 @@ .. grid-item:: :ref:`refname` + .. grid-item:: + :ref:`nsample` + + .. grid-item:: + :ref:`interp` + + .. grid-item:: + :ref:`delay` + + .. grid-item:: + :ref:`interval` + .. grid-item:: :ref:`cutoff` @@ -3628,6 +4078,18 @@ .. grid-item:: :ref:`refname` + .. grid-item:: + :ref:`nsample` + + .. grid-item:: + :ref:`interp` + + .. grid-item:: + :ref:`delay` + + .. grid-item:: + :ref:`interval` + .. grid-item:: :ref:`cutoff` @@ -3658,6 +4120,18 @@ .. grid-item:: :ref:`refname` + .. grid-item:: + :ref:`nsample` + + .. grid-item:: + :ref:`interp` + + .. grid-item:: + :ref:`delay` + + .. grid-item:: + :ref:`interval` + .. grid-item:: :ref:`cutoff` @@ -3688,6 +4162,18 @@ .. grid-item:: :ref:`refname` + .. grid-item:: + :ref:`nsample` + + .. grid-item:: + :ref:`interp` + + .. grid-item:: + :ref:`delay` + + .. grid-item:: + :ref:`interval` + .. grid-item:: :ref:`cutoff` @@ -3712,6 +4198,18 @@ .. grid-item:: :ref:`objname` + .. grid-item:: + :ref:`nsample` + + .. grid-item:: + :ref:`interp` + + .. grid-item:: + :ref:`delay` + + .. grid-item:: + :ref:`interval` + .. grid-item:: :ref:`cutoff` @@ -3736,6 +4234,18 @@ .. grid-item:: :ref:`objname` + .. grid-item:: + :ref:`nsample` + + .. grid-item:: + :ref:`interp` + + .. grid-item:: + :ref:`delay` + + .. grid-item:: + :ref:`interval` + .. grid-item:: :ref:`cutoff` @@ -3757,6 +4267,18 @@ .. grid-item:: :ref:`body` + .. grid-item:: + :ref:`nsample` + + .. grid-item:: + :ref:`interp` + + .. grid-item:: + :ref:`delay` + + .. grid-item:: + :ref:`interval` + .. grid-item:: :ref:`cutoff` @@ -3778,6 +4300,18 @@ .. grid-item:: :ref:`body` + .. grid-item:: + :ref:`nsample` + + .. grid-item:: + :ref:`interp` + + .. grid-item:: + :ref:`delay` + + .. grid-item:: + :ref:`interval` + .. grid-item:: :ref:`cutoff` @@ -3799,6 +4333,18 @@ .. grid-item:: :ref:`body` + .. grid-item:: + :ref:`nsample` + + .. grid-item:: + :ref:`interp` + + .. grid-item:: + :ref:`delay` + + .. grid-item:: + :ref:`interval` + .. grid-item:: :ref:`cutoff` @@ -3826,6 +4372,18 @@ .. grid-item:: :ref:`objname` + .. grid-item:: + :ref:`nsample` + + .. grid-item:: + :ref:`interp` + + .. grid-item:: + :ref:`delay` + + .. grid-item:: + :ref:`interval` + .. grid-item:: :ref:`cutoff` @@ -3856,6 +4414,18 @@ .. grid-item:: :ref:`body2` + .. grid-item:: + :ref:`nsample` + + .. grid-item:: + :ref:`interp` + + .. grid-item:: + :ref:`delay` + + .. grid-item:: + :ref:`interval` + .. grid-item:: :ref:`cutoff` @@ -3886,6 +4456,18 @@ .. grid-item:: :ref:`body2` + .. grid-item:: + :ref:`nsample` + + .. grid-item:: + :ref:`interp` + + .. grid-item:: + :ref:`delay` + + .. grid-item:: + :ref:`interval` + .. grid-item:: :ref:`cutoff` @@ -3916,6 +4498,18 @@ .. grid-item:: :ref:`body2` + .. grid-item:: + :ref:`nsample` + + .. grid-item:: + :ref:`interp` + + .. grid-item:: + :ref:`delay` + + .. grid-item:: + :ref:`interval` + .. grid-item:: :ref:`cutoff` @@ -3964,6 +4558,18 @@ .. grid-item:: :ref:`reduce` + .. grid-item:: + :ref:`nsample` + + .. grid-item:: + :ref:`interp` + + .. grid-item:: + :ref:`delay` + + .. grid-item:: + :ref:`interval` + .. grid-item:: :ref:`cutoff` @@ -3982,6 +4588,18 @@ .. grid-item:: :ref:`name` + .. grid-item:: + :ref:`nsample` + + .. grid-item:: + :ref:`interp` + + .. grid-item:: + :ref:`delay` + + .. grid-item:: + :ref:`interval` + .. grid-item:: :ref:`cutoff` @@ -4000,6 +4618,18 @@ .. grid-item:: :ref:`name` + .. grid-item:: + :ref:`nsample` + + .. grid-item:: + :ref:`interp` + + .. grid-item:: + :ref:`delay` + + .. grid-item:: + :ref:`interval` + .. grid-item:: :ref:`cutoff` @@ -4018,6 +4648,18 @@ .. grid-item:: :ref:`name` + .. grid-item:: + :ref:`nsample` + + .. grid-item:: + :ref:`interp` + + .. grid-item:: + :ref:`delay` + + .. grid-item:: + :ref:`interval` + .. grid-item:: :ref:`cutoff` @@ -4028,6 +4670,36 @@ :ref:`user` + .. dropdown:: :ref:`tactile` |*| + + .. grid:: 2 3 4 4 + :gutter: 0 + + .. grid-item:: + :ref:`name` + + .. grid-item:: + :ref:`geom` + + .. grid-item:: + :ref:`mesh` + + .. grid-item:: + :ref:`nsample` + + .. grid-item:: + :ref:`interp` + + .. grid-item:: + :ref:`delay` + + .. grid-item:: + :ref:`interval` + + .. grid-item:: + :ref:`user` + + .. dropdown:: :ref:`user` |*| .. grid:: 2 3 4 4 @@ -4061,24 +4733,6 @@ :ref:`user` - .. dropdown:: :ref:`tactile` |*| - - .. grid:: 2 3 4 4 - :gutter: 0 - - .. grid-item:: - :ref:`name` - - .. grid-item:: - :ref:`geom` - - .. grid-item:: - :ref:`mesh` - - .. grid-item:: - :ref:`user` - - .. dropdown:: :ref:`plugin` |*| .. grid:: 2 3 4 4 @@ -4960,6 +5614,15 @@ .. grid-item:: :ref:`group` + .. grid-item:: + :ref:`nsample` + + .. grid-item:: + :ref:`interp` + + .. grid-item:: + :ref:`delay` + .. grid-item:: :ref:`actdim` @@ -5014,6 +5677,15 @@ .. grid-item:: :ref:`group` + .. grid-item:: + :ref:`nsample` + + .. grid-item:: + :ref:`interp` + + .. grid-item:: + :ref:`delay` + .. dropdown:: :ref:`position` :octicon:`dot` @@ -5047,6 +5719,15 @@ .. grid-item:: :ref:`group` + .. grid-item:: + :ref:`nsample` + + .. grid-item:: + :ref:`interp` + + .. grid-item:: + :ref:`delay` + .. grid-item:: :ref:`kp` @@ -5089,6 +5770,15 @@ .. grid-item:: :ref:`group` + .. grid-item:: + :ref:`nsample` + + .. grid-item:: + :ref:`interp` + + .. grid-item:: + :ref:`delay` + .. grid-item:: :ref:`kv` @@ -5128,6 +5818,15 @@ .. grid-item:: :ref:`group` + .. grid-item:: + :ref:`nsample` + + .. grid-item:: + :ref:`interp` + + .. grid-item:: + :ref:`delay` + .. grid-item:: :ref:`kp` @@ -5164,6 +5863,15 @@ .. grid-item:: :ref:`group` + .. grid-item:: + :ref:`nsample` + + .. grid-item:: + :ref:`interp` + + .. grid-item:: + :ref:`delay` + .. grid-item:: :ref:`kv` @@ -5197,6 +5905,15 @@ .. grid-item:: :ref:`group` + .. grid-item:: + :ref:`nsample` + + .. grid-item:: + :ref:`interp` + + .. grid-item:: + :ref:`delay` + .. grid-item:: :ref:`timeconst` @@ -5239,6 +5956,15 @@ .. grid-item:: :ref:`group` + .. grid-item:: + :ref:`nsample` + + .. grid-item:: + :ref:`interp` + + .. grid-item:: + :ref:`delay` + .. grid-item:: :ref:`timeconst` @@ -5290,6 +6016,15 @@ .. grid-item:: :ref:`group` + .. grid-item:: + :ref:`nsample` + + .. grid-item:: + :ref:`interp` + + .. grid-item:: + :ref:`delay` + .. dropdown:: :ref:`custom` |*| diff --git a/doc/changelog.rst b/doc/changelog.rst index e0e5d114..b86f7258 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -32,6 +32,9 @@ Upcoming version (not yet released) General ^^^^^^^ +- Actuators and sensors now support arbitrary delays, see :ref:`Delays` for details. Adding + delays introduces a new ``mjData.history`` variable to the :ref:`Physics state`. + .. image:: images/changelog/poncho.png :width: 45% :align: right diff --git a/doc/images/modeling/delay_buffer_dark.svg b/doc/images/modeling/delay_buffer_dark.svg new file mode 100644 index 00000000..0c192ffb --- /dev/null +++ b/doc/images/modeling/delay_buffer_dark.svg @@ -0,0 +1,1129 @@ + + + + + + + + 2026-02-04T15:25:12.458847 + image/svg+xml + + + Matplotlib v3.9.1, https://matplotlib.org/ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/doc/images/modeling/delay_buffer_light.svg b/doc/images/modeling/delay_buffer_light.svg new file mode 100644 index 00000000..196a8ebb --- /dev/null +++ b/doc/images/modeling/delay_buffer_light.svg @@ -0,0 +1,1127 @@ + + + + + + + + 2026-02-04T15:25:12.277721 + image/svg+xml + + + Matplotlib v3.9.1, https://matplotlib.org/ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/doc/includes/references.h b/doc/includes/references.h index 7950ca98..e435325e 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -21,20 +21,21 @@ typedef enum mjtState_ { // state elements mjSTATE_QPOS = 1<<1, // position mjSTATE_QVEL = 1<<2, // velocity mjSTATE_ACT = 1<<3, // actuator activation - mjSTATE_WARMSTART = 1<<4, // acceleration used for warmstart - mjSTATE_CTRL = 1<<5, // control - mjSTATE_QFRC_APPLIED = 1<<6, // applied generalized force - mjSTATE_XFRC_APPLIED = 1<<7, // applied Cartesian force/torque - mjSTATE_EQ_ACTIVE = 1<<8, // enable/disable constraints - mjSTATE_MOCAP_POS = 1<<9, // positions of mocap bodies - mjSTATE_MOCAP_QUAT = 1<<10, // orientations of mocap bodies - mjSTATE_USERDATA = 1<<11, // user data - mjSTATE_PLUGIN = 1<<12, // plugin state + mjSTATE_HISTORY = 1<<4, // history buffers (control, sensor) + mjSTATE_WARMSTART = 1<<5, // acceleration used for warmstart + mjSTATE_CTRL = 1<<6, // control + mjSTATE_QFRC_APPLIED = 1<<7, // applied generalized force + mjSTATE_XFRC_APPLIED = 1<<8, // applied Cartesian force/torque + mjSTATE_EQ_ACTIVE = 1<<9, // enable/disable constraints + mjSTATE_MOCAP_POS = 1<<10, // positions of mocap bodies + mjSTATE_MOCAP_QUAT = 1<<11, // orientations of mocap bodies + mjSTATE_USERDATA = 1<<12, // user data + mjSTATE_PLUGIN = 1<<13, // plugin state - mjNSTATE = 13, // number of state elements + mjNSTATE = 14, // number of state elements // convenience values for commonly used state specifications - mjSTATE_PHYSICS = mjSTATE_QPOS | mjSTATE_QVEL | mjSTATE_ACT, + mjSTATE_PHYSICS = mjSTATE_QPOS | mjSTATE_QVEL | mjSTATE_ACT | mjSTATE_HISTORY, mjSTATE_FULLPHYSICS = mjSTATE_TIME | mjSTATE_PHYSICS | mjSTATE_PLUGIN, mjSTATE_USER = mjSTATE_CTRL | mjSTATE_QFRC_APPLIED | mjSTATE_XFRC_APPLIED | mjSTATE_EQ_ACTIVE | mjSTATE_MOCAP_POS | mjSTATE_MOCAP_QUAT | @@ -221,6 +222,7 @@ struct mjData_ { mjtNum* qpos; // position (nq x 1) mjtNum* qvel; // velocity (nv x 1) mjtNum* act; // actuator activation (na x 1) + mjtNum* history; // history buffer (nhistory x 1) mjtNum* qacc_warmstart; // acceleration used for warmstart (nv x 1) mjtNum* plugin_state; // plugin state (npluginstate x 1) @@ -1101,6 +1103,7 @@ struct mjModel_ { mjtSize nuserdata; // number of mjtNums reserved for the user mjtSize nsensordata; // number of mjtNums in sensor data vector mjtSize npluginstate; // number of mjtNums in plugin state vector + mjtSize nhistory; // number of mjtNums in history buffer // buffer sizes mjtSize narena; // number of bytes in the mjData arena (inclusive of stack) @@ -1520,6 +1523,9 @@ struct mjModel_ { int* actuator_actadr; // first activation address; -1: stateless (nu x 1) int* actuator_actnum; // number of activation variables (nu x 1) int* actuator_group; // group for visibility (nu x 1) + int* actuator_history; // history buffer: [nsample, interp] (nu x 2) + int* actuator_historyadr; // address in history buffer; -1: none (nu x 1) + mjtNum* actuator_delay; // delay time in seconds; 0: no delay (nu x 1) mjtByte* actuator_ctrllimited; // is control limited (nu x 1) mjtByte* actuator_forcelimited;// is force limited (nu x 1) mjtByte* actuator_actlimited; // is activation limited (nu x 1) @@ -1551,6 +1557,10 @@ struct mjModel_ { int* sensor_adr; // address in sensor array (nsensor x 1) mjtNum* sensor_cutoff; // cutoff for real and positive; 0: ignore (nsensor x 1) mjtNum* sensor_noise; // noise standard deviation (nsensor x 1) + int* sensor_history; // history buffer: [nsample, interp] (nsensor x 2) + int* sensor_historyadr; // address in history buffer; -1: none (nsensor x 1) + mjtNum* sensor_delay; // delay time in seconds; 0: no delay (nsensor x 1) + mjtNum* sensor_interval; // interval: [period, phase] in seconds (nsensor x 2) mjtNum* sensor_user; // user data (nsensor x nuser_sensor) int* sensor_plugin; // plugin instance id; -1: not a plugin (nsensor x 1) @@ -2445,6 +2455,9 @@ typedef struct mjsActuator_ { // actuator specification // other int group; // group + int nsample; // number of samples in history buffer + int interp; // interpolation order (0=ZOH, 1=linear, 2=cubic) + double delay; // delay time in seconds; 0: no delay mjDoubleVec* userdata; // user data mjsPlugin plugin; // actuator plugin mjString* info; // message appended to compiler errors @@ -2469,6 +2482,12 @@ typedef struct mjsSensor_ { // sensor specification double cutoff; // cutoff for real and positive datatypes double noise; // noise stdev + // history buffer + int nsample; // number of samples in history buffer + int interp; // interpolation order (0=ZOH, 1=linear, 2=cubic) + double delay; // delay time in seconds + double interval[2]; // [period, time_prev] in seconds + // other mjDoubleVec* userdata; // user data mjsPlugin plugin; // sensor plugin @@ -3234,6 +3253,13 @@ void mj_extractState(const mjModel* m, const mjtNum* src, int srcsig, mjtNum* dst, int dstsig); void mj_setState(const mjModel* m, mjData* d, const mjtNum* state, int sig); void mj_copyState(const mjModel* m, const mjData* src, mjData* dst, int sig); +mjtNum mj_readCtrl(const mjModel* m, const mjData* d, int id, mjtNum time, int interp); +const mjtNum* mj_readSensor(const mjModel* m, const mjData* d, int id, mjtNum time, + mjtNum* result, int interp); +void mj_initCtrlHistory(const mjModel* m, mjData* d, int id, + const mjtNum* times, const mjtNum* values); +void mj_initSensorHistory(const mjModel* m, mjData* d, int id, + const mjtNum* times, const mjtNum* values, mjtNum phase); void mj_setKeyframe(mjModel* m, const mjData* d, int k); int mj_addContact(const mjModel* m, mjData* d, const mjContact* con); int mj_isPyramidal(const mjModel* m); diff --git a/doc/modeling.rst b/doc/modeling.rst index ab480e91..5016fc29 100644 --- a/doc/modeling.rst +++ b/doc/modeling.rst @@ -1081,20 +1081,186 @@ copied in the array mjData.sensordata and are available for user processing. Here we describe the XML attributes common to all sensor types, so as to avoid repetition later. +.. _sensor-name: + :at:`name`: :at-val:`string, optional` Name of the sensor. + +.. _sensor-noise: + :at:`noise`: :at-val:`real, "0"` The standard deviation of the noise model of this sensor. In versions prior to 3.1.4, this would lead to noise being added to the sensors. In release 3.1.4 this feature was removed, see :doc:`3.1.4 changelog ` for a detailed justification. As of subsequent versions, this attrbute serves as a convenient location for saving standard deviation information for later use. + +.. _sensor-cutoff: + :at:`cutoff`: :at-val:`real, "0"` When this value is positive, it limits the absolute value of the sensor output. It is also used to normalize the sensor data plots in :ref:`simulate.cc `. Note that :at:`cutoff` has a different meaning for :ref:`collision sensors`. + +.. _sensor-nsample: + +:at:`nsample`: :at-val:`int, "0"` + If :at-val:`nsample` is greater than 0, creates a time-indexed ring buffer with :at:`nsample` slots of sensor data. + During state advancement, the current sensor data is appended to the buffer with timestamp ``time``, and the oldest + sample is removed. Values in the history buffer can be read via :ref:`mj_readSensor`. A positive :at-val:`nsample` + is required for both :ref:`delay` and :ref:`interval` features. + + See :ref:`Delays` for details. + +.. _sensor-interp: + +:at:`interp`: :at-val:`[zoh, linear, cubic], "zoh"` + The interpolation method used when reading from the history buffer. Corresponds to the ``interp`` argument in + :ref:`mj_readSensor`. + + - ``zoh``: Zero-order hold (piecewise constant). + - ``linear``: Piecewise linear interpolation. + - ``cubic``: Cubic spline interpolation (Catmull-Rom). + + The :at:`interp` value is for advanced use-cases, see :ref:`Delays` for details. + +.. _sensor-delay: + +:at:`delay`: :at-val:`real, "0"` + If greater than 0, sensor values in ``mjData.sensordata`` are read from the history buffer at ``time - delay`` rather + than computed directly. Requires positive :ref:`nsample`, cannot be negative. + + In the most common case, ``delay = nsample * timestep``, see :ref:`Delays` for details. + +.. _sensor-interval: + +:at:`interval`: :at-val:`real, "0 0"` + This attribute controls how often sensor values are recomputed. It is useful for modeling sensors that have a larger + sampling period than the simulation timestep. Requires a history buffer (:ref:`nsample ` > 0). + + This attribute is defined by two real-valued numbers, both in units of time, called :at:`interval` = + ":at-val:`period` :at-val:`phase`". It is possible to only specify the :at-val:`period`, in which case the + :at-val:`phase` is assumed to be 0. + + The :at-val:`period` specifies the interval period between recomputations. The default value of 0 has the special + meaning "every simulation timestep". Note that the period is not required to be an integer multiple of the timestep. + For example, if the simulation timestep is 1.0, and :at-val:`period` is 2.5, the sensor will be computed at times + 0.0, 3.0, 5.0, 8.0, 10.0, 13.0, ... with the actual interval alternating between 2 and 3 timesteps. :at-val:`period` + cannot be negative. Note that only ``period > timestep`` values make sense; values smaller than or equal to the + timestep will not lead to an error but merely cause the sensor to be recomputed at every timestep. + + The :at-val:`phase` only takes effect during history buffer initialization in :ref:`mj_resetData`. It specifies the + last time that the sensor was computed "before the simulation started" in continuous time (i.e., disregarding the + quantization of timesteps). It is useful for precisely controlling the *relative phase* of sensor computation and + simulation time, when interval is used. The default value of 0 has the special meaning ":at-val:`-period`", i.e. + specifying that the sensor should be computed at the first timestep of the simulation. Continuing our example from + earlier, if the timestep is 1.0 and interval is ":at-val:`2.5 -1.5`", the sensor will be computed at times 1.0, 4.0, + 6.0, 9.0, 11.0, 14.0, etc. :at-val:`phase` must be in the range :math:`(-\text{period}, 0]`. + :at:`user`: :at-val:`real(nuser_sensor), "0 0 ..."` See :ref:`User parameters `. +.. _CDelay: + +Delays +~~~~~~ + +Both actuators and sensors support time delays via a ring buffer that stores timestamped samples. When the integer +attribute :at:`nsample` (:ref:`actuators`, :ref:`sensors`) is positive, a +buffer with :at:`nsample` slots is included in the :ref:`physics state` component ``mjData.history`` +and the samples and current timestamps are written into the buffer upon state advancement. + +If additionally the real-valued :at:`delay` attribute (:ref:`actuators`, +:ref:`sensors`) is positive, then during the forward dynamics the control or sensor values are read from +the history buffer (instead of being read from ``ctrl`` or recomputed, respectively). Positive :at:`delay` requires +positive :at:`nsample`. + +Note that since reading happens before writing, the minimum positive delay is effectively one timestep, despite +:at:`delay` being real-valued. + +Delayed reading in the engine is triggered by positive :at:`delay`, and performed by the API functions +:ref:`mj_readCtrl` and :ref:`mj_readSensor`, which read from the buffer at ``time - delay``, effectively implementing +the requested delay. These functions take ``time`` as an argument and can be used whenever :at:`nsample` is positive, +allowing the user to inspect the contents of the history buffer at any time, including in a "history-only" mode +(:at:`nsample` > 0, :at:`delay` = 0), where past values are accessible via the API but the simulation is unaffected. + +**Sensor Modes** + +Sensors support both :ref:`delay` and :ref:`interval` attributes. +The combination determines behavior: + +.. list-table:: + :header-rows: 1 + :widths: 10 10 80 + + * - delay + - interval + - Write / Read behavior + * - = 0 + - = 0 + - History-only: computed every step, written to ``sensordata``, pushed into history buffer + * - > 0 + - = 0 + - Delayed: computed every step, ``sensordata`` contains delayed reading (read from buffer) + * - = 0 + - > 0 + - Periodic: computed on interval, ``sensordata`` contains last computed value (no delay) + * - > 0 + - > 0 + - Periodic + Delayed: computed on interval, ``sensordata`` contains delayed reading (read from buffer) + +**Initialization** + +History buffers are initialized by :ref:`mj_resetData` as follows: + +- **Values**: Always initialized to zero. For custom initialization after reset, call :ref:`mj_initCtrlHistory` + and :ref:`mj_initSensorHistory`. + +- **Actuator timestamps**: ``[..., -2*dt, -dt]``. + +- **Sensor timestamps** without :ref:`interval`: ``[..., -2*dt, -dt]``. + +- **Sensor timestamps** with :ref:`interval`: Samples are spaced at ``period`` intervals rather than + ``dt``. The continuous-time timestamps ``[..., phase-2*period, phase-period, phase]`` are rounded up to the nearest + multiple of ``dt``, since that is when samples could have been computed. If ``phase = 0`` (the default), it is + interpreted as ``-period``, meaning the first sample will be computed at ``t = 0``. + +**Causality and interpolation** + +The most common positive delay value is ``delay = timestep * nsample``, which implements a simple +history buffer, with no causality issues. + +.. warning:: + + If ``delay > timestep * nsample``, then data will be read before the earliest buffer bound, resulting in non-causal + extrapolation: using a value from before it was actually recorded. This scenario will not lead to a runtime error, + so it is up to the user to avoid it. + +Setting ``delay < timestep * nsample`` is not problematic and can be useful for system identification and stochastic +delays. In these use cases, one should choose a maximum possible ``delay_max`` and set ``nsample = ceil(delay_max / +timestep)``. Then at run-time or sysID-time, the :ref:`mjModel` fields ``actuator_delay`` or ``sensor_delay`` can be +safely modified, so long as ``delay_max`` is not exceeded. + +.. image:: images/modeling/delay_buffer_light.svg + :width: 50% + :align: right + :class: only-light + +.. image:: images/modeling/delay_buffer_dark.svg + :width: 50% + :align: right + :class: only-dark + +These two use cases are the reason for including the :at:`interp` attribute (:ref:`actuators`, +:ref:`sensors`). While real-world exogenous delays are generally a zero-order-hold phenomenon, this +implies discontinuity: a small change in the delay has no effect, until the timestep threshold is crossed. For example +with ``dt = 0.1`` and ``nsample = 2``, there is no functional difference between ``delay = 0.2`` and ``delay = 0.101`` +(both read from the oldest sample), but stepping from ``delay = 0.101`` to ``delay = 0.1`` crosses a threshold and +changes behavior. By allowing higher order interpolation, the effect of delays becomes continuous (``interp = linear``) +and differentiable (``interp = cubic``). + +Note that interpolation does not makes sense for some types of sensors, for example sensors that report integer values +(e.g. :ref:`insidesite`). + .. _CCamera: Cameras diff --git a/doc/programming/simulation.rst b/doc/programming/simulation.rst index 502e34cf..da6c2b16 100644 --- a/doc/programming/simulation.rst +++ b/doc/programming/simulation.rst @@ -264,7 +264,7 @@ individual components and combinations of components. These are: Physics state """"""""""""" The *physics state* (:ref:`mjSTATE_PHYSICS`) contains the main quantities which are time-integrated during -stepping. These are ``mjData.{qpos, qvel, act}``: +stepping. These are ``mjData.{qpos, qvel, act, history}``: Position: ``qpos`` The configuration in generalized coodinates, denoted in the :ref:`Numerical Integration` section as @@ -281,6 +281,11 @@ Actuator activation: ``act`` actuators (such as biological muscles) that have their own activation states assembled in ``mjData.act``, denoted as :math:`w` in the :ref:`Numerical Integration` section. +History buffer: ``history`` + When actuators or sensors have a positive :at:`nsample` attribute (:ref:`actuators`, + :ref:`sensors`), this buffer stores timestamped samples of previous + control or sensor values. See :ref:`Delays` for details. + .. _siFullPhysics: Full physics state diff --git a/include/mujoco/mjdata.h b/include/mujoco/mjdata.h index 01c1736b..839602d5 100644 --- a/include/mujoco/mjdata.h +++ b/include/mujoco/mjdata.h @@ -29,20 +29,21 @@ typedef enum mjtState_ { // state elements mjSTATE_QPOS = 1<<1, // position mjSTATE_QVEL = 1<<2, // velocity mjSTATE_ACT = 1<<3, // actuator activation - mjSTATE_WARMSTART = 1<<4, // acceleration used for warmstart - mjSTATE_CTRL = 1<<5, // control - mjSTATE_QFRC_APPLIED = 1<<6, // applied generalized force - mjSTATE_XFRC_APPLIED = 1<<7, // applied Cartesian force/torque - mjSTATE_EQ_ACTIVE = 1<<8, // enable/disable constraints - mjSTATE_MOCAP_POS = 1<<9, // positions of mocap bodies - mjSTATE_MOCAP_QUAT = 1<<10, // orientations of mocap bodies - mjSTATE_USERDATA = 1<<11, // user data - mjSTATE_PLUGIN = 1<<12, // plugin state + mjSTATE_HISTORY = 1<<4, // history buffers (control, sensor) + mjSTATE_WARMSTART = 1<<5, // acceleration used for warmstart + mjSTATE_CTRL = 1<<6, // control + mjSTATE_QFRC_APPLIED = 1<<7, // applied generalized force + mjSTATE_XFRC_APPLIED = 1<<8, // applied Cartesian force/torque + mjSTATE_EQ_ACTIVE = 1<<9, // enable/disable constraints + mjSTATE_MOCAP_POS = 1<<10, // positions of mocap bodies + mjSTATE_MOCAP_QUAT = 1<<11, // orientations of mocap bodies + mjSTATE_USERDATA = 1<<12, // user data + mjSTATE_PLUGIN = 1<<13, // plugin state - mjNSTATE = 13, // number of state elements + mjNSTATE = 14, // number of state elements // convenience values for commonly used state specifications - mjSTATE_PHYSICS = mjSTATE_QPOS | mjSTATE_QVEL | mjSTATE_ACT, + mjSTATE_PHYSICS = mjSTATE_QPOS | mjSTATE_QVEL | mjSTATE_ACT | mjSTATE_HISTORY, mjSTATE_FULLPHYSICS = mjSTATE_TIME | mjSTATE_PHYSICS | mjSTATE_PLUGIN, mjSTATE_USER = mjSTATE_CTRL | mjSTATE_QFRC_APPLIED | mjSTATE_XFRC_APPLIED | mjSTATE_EQ_ACTIVE | mjSTATE_MOCAP_POS | mjSTATE_MOCAP_QUAT | @@ -255,6 +256,7 @@ struct mjData_ { mjtNum* qpos; // position (nq x 1) mjtNum* qvel; // velocity (nv x 1) mjtNum* act; // actuator activation (na x 1) + mjtNum* history; // history buffer (nhistory x 1) mjtNum* qacc_warmstart; // acceleration used for warmstart (nv x 1) mjtNum* plugin_state; // plugin state (npluginstate x 1) diff --git a/include/mujoco/mjmodel.h b/include/mujoco/mjmodel.h index f19d5ea0..42638b97 100644 --- a/include/mujoco/mjmodel.h +++ b/include/mujoco/mjmodel.h @@ -761,6 +761,7 @@ struct mjModel_ { mjtSize nuserdata; // number of mjtNums reserved for the user mjtSize nsensordata; // number of mjtNums in sensor data vector mjtSize npluginstate; // number of mjtNums in plugin state vector + mjtSize nhistory; // number of mjtNums in history buffer // buffer sizes mjtSize narena; // number of bytes in the mjData arena (inclusive of stack) @@ -1180,6 +1181,9 @@ struct mjModel_ { int* actuator_actadr; // first activation address; -1: stateless (nu x 1) int* actuator_actnum; // number of activation variables (nu x 1) int* actuator_group; // group for visibility (nu x 1) + int* actuator_history; // history buffer: [nsample, interp] (nu x 2) + int* actuator_historyadr; // address in history buffer; -1: none (nu x 1) + mjtNum* actuator_delay; // delay time in seconds; 0: no delay (nu x 1) mjtByte* actuator_ctrllimited; // is control limited (nu x 1) mjtByte* actuator_forcelimited;// is force limited (nu x 1) mjtByte* actuator_actlimited; // is activation limited (nu x 1) @@ -1211,6 +1215,10 @@ struct mjModel_ { int* sensor_adr; // address in sensor array (nsensor x 1) mjtNum* sensor_cutoff; // cutoff for real and positive; 0: ignore (nsensor x 1) mjtNum* sensor_noise; // noise standard deviation (nsensor x 1) + int* sensor_history; // history buffer: [nsample, interp] (nsensor x 2) + int* sensor_historyadr; // address in history buffer; -1: none (nsensor x 1) + mjtNum* sensor_delay; // delay time in seconds; 0: no delay (nsensor x 1) + mjtNum* sensor_interval; // interval: [period, phase] in seconds (nsensor x 2) mjtNum* sensor_user; // user data (nsensor x nuser_sensor) int* sensor_plugin; // plugin instance id; -1: not a plugin (nsensor x 1) diff --git a/include/mujoco/mjspec.h b/include/mujoco/mjspec.h index 1aebbd67..30857b16 100644 --- a/include/mujoco/mjspec.h +++ b/include/mujoco/mjspec.h @@ -698,6 +698,9 @@ typedef struct mjsActuator_ { // actuator specification // other int group; // group + int nsample; // number of samples in history buffer + int interp; // interpolation order (0=ZOH, 1=linear, 2=cubic) + double delay; // delay time in seconds; 0: no delay mjDoubleVec* userdata; // user data mjsPlugin plugin; // actuator plugin mjString* info; // message appended to compiler errors @@ -724,6 +727,12 @@ typedef struct mjsSensor_ { // sensor specification double cutoff; // cutoff for real and positive datatypes double noise; // noise stdev + // history buffer + int nsample; // number of samples in history buffer + int interp; // interpolation order (0=ZOH, 1=linear, 2=cubic) + double delay; // delay time in seconds + double interval[2]; // [period, time_prev] in seconds + // other mjDoubleVec* userdata; // user data mjsPlugin plugin; // sensor plugin diff --git a/include/mujoco/mjxmacro.h b/include/mujoco/mjxmacro.h index 6b876a94..3136e1c9 100644 --- a/include/mujoco/mjxmacro.h +++ b/include/mujoco/mjxmacro.h @@ -153,6 +153,7 @@ X( nuserdata ) \ X( nsensordata ) \ X( npluginstate ) \ + X( nhistory ) \ X( narena ) \ X( nbuffer ) @@ -563,6 +564,9 @@ X ( int, actuator_actadr, nu, 1 ) \ X ( int, actuator_actnum, nu, 1 ) \ X ( int, actuator_group, nu, 1 ) \ + X ( int, actuator_history, nu, 2 ) \ + X ( int, actuator_historyadr, nu, 1 ) \ + X ( mjtNum, actuator_delay, nu, 1 ) \ X ( mjtByte, actuator_ctrllimited, nu, 1 ) \ X ( mjtByte, actuator_forcelimited, nu, 1 ) \ X ( mjtByte, actuator_actlimited, nu, 1 ) \ @@ -594,6 +598,10 @@ X ( int, sensor_adr, nsensor, 1 ) \ X ( mjtNum, sensor_cutoff, nsensor, 1 ) \ X ( mjtNum, sensor_noise, nsensor, 1 ) \ + X ( int, sensor_history, nsensor, 2 ) \ + X ( int, sensor_historyadr, nsensor, 1 ) \ + X ( mjtNum, sensor_delay, nsensor, 1 ) \ + X ( mjtNum, sensor_interval, nsensor, 2 ) \ X ( mjtNum, sensor_user, nsensor, MJ_M(nuser_sensor) ) \ X ( int, sensor_plugin, nsensor, 1 ) @@ -708,6 +716,7 @@ X ( mjtNum, qpos, nq, 1 ) \ X ( mjtNum, qvel, nv, 1 ) \ X ( mjtNum, act, na, 1 ) \ + X ( mjtNum, history, nhistory, 1 ) \ X ( mjtNum, qacc_warmstart, nv, 1 ) \ X ( mjtNum, plugin_state, npluginstate, 1 ) \ X ( mjtNum, ctrl, nu, 1 ) \ diff --git a/include/mujoco/mujoco.h b/include/mujoco/mujoco.h index 197d42f1..997a33ba 100644 --- a/include/mujoco/mujoco.h +++ b/include/mujoco/mujoco.h @@ -493,6 +493,29 @@ MJAPI void mj_setState(const mjModel* m, mjData* d, const mjtNum* state, int sig // Copy state from src to dst. MJAPI void mj_copyState(const mjModel* m, const mjData* src, mjData* dst, int sig); +// Read ctrl value for actuator at given time. +// Returns d->ctrl[id] if no history, otherwise reads from history buffer. +// interp: 0=zero-order-hold, 1=linear, 2=cubic spline. +MJAPI mjtNum mj_readCtrl(const mjModel* m, const mjData* d, int id, mjtNum time, int interp); + +// Read sensor value from history buffer at given time. +// Returns pointer to sensordata (no history) or history buffer (exact match), +// or NULL if interpolation performed (writes to result). +// interp: 0=zero-order-hold, 1=linear, 2=cubic spline. +MJAPI const mjtNum* mj_readSensor(const mjModel* m, const mjData* d, int id, mjtNum time, + mjtNum* result, int interp); + +// Initialize history buffer for actuator; if times is NULL, uses existing buffer timestamps. +// Nullable: times +MJAPI void mj_initCtrlHistory(const mjModel* m, mjData* d, int id, + const mjtNum* times, const mjtNum* values); + +// Initialize history buffer for sensor; if times is NULL, uses existing buffer timestamps. +// phase sets the user slot (last computation time for interval sensors). +// Nullable: times +MJAPI void mj_initSensorHistory(const mjModel* m, mjData* d, int id, + const mjtNum* times, const mjtNum* values, mjtNum phase); + // Copy current state to the k-th model keyframe. MJAPI void mj_setKeyframe(mjModel* m, const mjData* d, int k); diff --git a/mjx/mujoco/mjx/_src/io.py b/mjx/mujoco/mjx/_src/io.py index 48f149c0..ba8c91c4 100644 --- a/mjx/mujoco/mjx/_src/io.py +++ b/mjx/mujoco/mjx/_src/io.py @@ -571,6 +571,7 @@ def _make_data_public_fields(m: types.Model) -> Dict[str, Any]: 'time': (float_,), 'qvel': (m.nv, float_), 'act': (m.na, float_), + 'history': (m.nhistory, float_), 'plugin_state': (m.npluginstate, float_), 'qacc_warmstart': (m.nv, float_), 'ctrl': (m.nu, float_), @@ -875,7 +876,7 @@ def _make_data_warp( fields = _make_data_public_fields(m) for k in fields: - if k in {'userdata', 'plugin_state'}: + if k in {'userdata', 'plugin_state', 'history'}: continue if not hasattr(dw, k): raise ValueError(f'Public data field {k} not found in Warp data.') @@ -1729,6 +1730,7 @@ _STATE_MAP = { mujoco.mjtState.mjSTATE_QPOS: 'qpos', mujoco.mjtState.mjSTATE_QVEL: 'qvel', mujoco.mjtState.mjSTATE_ACT: 'act', + mujoco.mjtState.mjSTATE_HISTORY: 'history', mujoco.mjtState.mjSTATE_WARMSTART: 'qacc_warmstart', mujoco.mjtState.mjSTATE_CTRL: 'ctrl', mujoco.mjtState.mjSTATE_QFRC_APPLIED: 'qfrc_applied', @@ -1752,6 +1754,7 @@ def _state_elem_size(m: types.Model, state_enum: mujoco.mjtState) -> int: 'qpos', 'qvel', 'act', + 'history', 'qacc_warmstart', 'ctrl', 'qfrc_applied', @@ -1767,6 +1770,7 @@ def _state_elem_size(m: types.Model, state_enum: mujoco.mjtState) -> int: 'qpos': 'nq', 'qvel': 'nv', 'act': 'na', + 'history': 'nhistory', 'qacc_warmstart': 'nv', 'ctrl': 'nu', 'qfrc_applied': 'nv', diff --git a/mjx/mujoco/mjx/_src/types.py b/mjx/mujoco/mjx/_src/types.py index a85f6a76..3965a010 100644 --- a/mjx/mujoco/mjx/_src/types.py +++ b/mjx/mujoco/mjx/_src/types.py @@ -641,7 +641,14 @@ class ModelC(PyTreeNode): tendon_treenum: jax.Array tendon_treeid: jax.Array actuator_plugin: jax.Array + actuator_history: jax.Array + actuator_historyadr: jax.Array + actuator_delay: jax.Array sensor_plugin: jax.Array + sensor_history: jax.Array + sensor_historyadr: jax.Array + sensor_delay: jax.Array + sensor_interval: jax.Array plugin: jax.Array plugin_stateadr: jax.Array B_rownnz: jax.Array # pylint:disable=invalid-name @@ -719,6 +726,7 @@ class Model(PyTreeNode): nuserdata: number of elements in userdata nsensordata: number of elements in sensor data vector npluginstate: number of plugin state values + nhistory: number of history buffer elements opt: physics options stat: model statistics qpos0: qpos values at default pose @@ -768,6 +776,7 @@ class Model(PyTreeNode): nuserdata: int nsensordata: int npluginstate: int + nhistory: int opt: Option stat: Union[Statistic, StatisticWarp] qpos0: jax.Array @@ -1206,6 +1215,7 @@ class Data(PyTreeNode): qpos: position qvel: velocity act: actuator activation + history: actuator history buffer qacc_warmstart: warm start for solver plugin_state: plugin state values ctrl: control input @@ -1255,6 +1265,7 @@ class Data(PyTreeNode): qpos: jax.Array qvel: jax.Array act: jax.Array + history: jax.Array qacc_warmstart: jax.Array plugin_state: jax.Array # control: diff --git a/python/mujoco/bindings_test.py b/python/mujoco/bindings_test.py index 321188a6..020c68a6 100644 --- a/python/mujoco/bindings_test.py +++ b/python/mujoco/bindings_test.py @@ -1369,7 +1369,9 @@ Euler integrator, semi-implicit in velocity. for i in range(0, 3): self.assertEqual( dist[i], - mujoco.mj_ray(self.model, self.data, pnt, vec[i], None, 1, -1, geom1, None), + mujoco.mj_ray( + self.model, self.data, pnt, vec[i], None, 1, -1, geom1, None + ), ) self.assertEqual(geomid[i], geom1) self.assertEqual(geomid[i], geom_ex[i]) @@ -1717,6 +1719,168 @@ Euler integrator, semi-implicit in velocity. self.assertIn(model_path, dependencies) self.assertIn(msh_path, dependencies) + def test_mj_read_ctrl_and_init_ctrl_delay(self): + xml = r""" + + + + + + + + + + + +""" + model = mujoco.MjModel.from_xml_string(xml) + data = mujoco.MjData(model) + mujoco.mj_forward(model, data) + + # Initialize the delay buffer with known values + # actuator_history[i, 0] = nsample, actuator_history[i, 1] = interp + nhistory = model.actuator_history[0, 0] + self.assertEqual(nhistory, 4) + times = np.array([0.0, 0.01, 0.02, 0.03]) + values = np.array([1.0, 2.0, 3.0, 4.0]) + mujoco.mj_initCtrlHistory(model, data, 0, times, values) + + # Read back a value using zero-order hold + # mj_readCtrl auto-subtracts delay: lookup_time = read_time - delay + # delay = 0.01, so: + # read_time=0.02 -> lookup at 0.01 -> value 2.0 + # read_time=0.03 -> lookup at 0.02 -> value 3.0 + result = mujoco.mj_readCtrl(model, data, 0, 0.02, interp=0) + self.assertEqual(result, 2.0) # ZOH returns value at t=0.01 + + # Test with times=None (uses existing timestamps) + new_values = np.array([5.0, 6.0, 7.0, 8.0]) + mujoco.mj_initCtrlHistory(model, data, 0, None, new_values) + # read_time=0.02 -> lookup at 0.01 -> value 6.0 + result = mujoco.mj_readCtrl(model, data, 0, 0.02, interp=0) + self.assertEqual(result, 6.0) + + # Test dimension validation errors + with self.assertRaises(TypeError): + # wrong times + mujoco.mj_initCtrlHistory(model, data, 0, np.zeros(3), values) + with self.assertRaises(TypeError): + # wrong values + mujoco.mj_initCtrlHistory(model, data, 0, times, np.zeros(5)) + + def test_mj_read_sensor_and_init_sensor_delay(self): + xml = r""" + + + + + + + + + + + + +""" + model = mujoco.MjModel.from_xml_string(xml) + data = mujoco.MjData(model) + mujoco.mj_forward(model, data) + + # Initialize the delay buffer with known values + # sensor_history[i, 0] = nsample, sensor_history[i, 1] = interp + nhistory = model.sensor_history[0, 0] + dim = model.sensor_dim[0] + self.assertEqual(nhistory, 3) + self.assertEqual(dim, 3) # accelerometer has dim=3 + times = np.array([0.0, 0.01, 0.02]) + values = np.array([[1, 2, 3], [4, 5, 6], [7, 8, 9]], dtype=np.float64) + mujoco.mj_initSensorHistory(model, data, 0, times, values, phase=0.0) + + # Read back a value using zero-order hold + # mj_readSensor auto-subtracts delay: lookup_time = read_time - delay + # delay = 0.01, so: + # read_time=0.02 -> lookup at 0.01 -> value [4, 5, 6] + result = np.zeros(dim) + mujoco.mj_readSensor(model, data, 0, 0.02, result, interp=0) + # ZOH returns value at t=0.01 + np.testing.assert_array_equal(result, [4, 5, 6]) + + # Test with times=None (uses existing timestamps) + new_values = np.array([ + [10, 11, 12], [13, 14, 15], [16, 17, 18]], dtype=np.float64) + mujoco.mj_initSensorHistory(model, data, 0, None, new_values, phase=0.0) + # read_time=0.02 -> lookup at 0.01 -> value [13, 14, 15] + mujoco.mj_readSensor(model, data, 0, 0.02, result, interp=0) + np.testing.assert_array_equal(result, [13, 14, 15]) + + # Test dimension validation errors + with self.assertRaises(TypeError): + # wrong result size + mujoco.mj_readSensor(model, data, 0, 0.02, np.zeros(2), interp=0) + with self.assertRaises(TypeError): + # wrong times size + mujoco.mj_initSensorHistory(model, data, 0, np.zeros(2), values, 0.0) + with self.assertRaises(TypeError): + # wrong values rows + mujoco.mj_initSensorHistory(model, data, 0, times, np.zeros((4, 3)), 0.0) + with self.assertRaises(TypeError): + # wrong values cols + mujoco.mj_initSensorHistory(model, data, 0, times, np.zeros((3, 2)), 0.0) + + def test_init_sensor_history_pedagogical(self): + # A framequat sensor reports body orientation as a unit quaternion. + # Quaternions are never zero: the identity quaternion is [1, 0, 0, 0]. + # This test demonstrates why mj_initSensorHistory is needed: after + # mj_makeData, the history buffer is filled with zeros, which is invalid. + xml = r""" + + + + + + + + + + + +""" + model = mujoco.MjModel.from_xml_string(xml) + data = mujoco.MjData(model) + dim = model.sensor_dim[0] + nsample = model.sensor_history[0, 0] + delay = model.sensor_delay[0] + self.assertEqual(dim, 4) + self.assertEqual(nsample, 5) + self.assertEqual(delay, 0.01) + + # After mj_makeData, reading from the delay buffer gives all zeros. + # For a quaternion sensor, this is invalid data. + result = np.zeros(dim) + mujoco.mj_readSensor(model, data, 0, delay, result, interp=0) + np.testing.assert_array_equal(result, [0, 0, 0, 0]) + + # To get valid sensor values, we temporarily set delay to 0 so that + # mj_forward populates sensordata directly (without using the delay + # buffer), then restore the original delay value. + saved_delay = model.sensor_delay.copy() + model.sensor_delay[:] = 0 + mujoco.mj_forward(model, data) + model.sensor_delay[:] = saved_delay + + # Now sensordata contains the valid identity quaternion. + np.testing.assert_array_equal(data.sensordata, [1, 0, 0, 0]) + + # Use mj_initSensorHistory to fill the buffer with valid quaternion values. + # Passing None for times keeps the existing timestamps in the buffer. + values = np.tile(data.sensordata, (nsample, 1)) + mujoco.mj_initSensorHistory(model, data, 0, None, values, phase=0.0) + + # Now reading from the delay buffer gives the valid identity quaternion. + mujoco.mj_readSensor(model, data, 0, delay, result, interp=0) + np.testing.assert_array_equal(result, [1, 0, 0, 0]) + def _assert_attributes_equal(self, actual_obj, expected_obj, attr_to_compare): for name in attr_to_compare: actual_value = getattr(actual_obj, name) diff --git a/python/mujoco/functions.cc b/python/mujoco/functions.cc index fd83692a..776e8ae4 100644 --- a/python/mujoco/functions.cc +++ b/python/mujoco/functions.cc @@ -91,7 +91,7 @@ PYBIND11_MODULE(_functions, pymodule) { DEF_WITH_OMITTED_PY_ARGS(traits::mj_printSchema, "filename", "buffer", "buffer_sz")( pymodule, [](bool flg_html, bool flg_pad) { - constexpr int kBufferSize = 40000; + constexpr int kBufferSize = 60000; auto buffer = std::unique_ptr(new char[kBufferSize]); const int out_length = InterceptMjErrors(::mj_printSchema)( nullptr, buffer.get(), kBufferSize, flg_html, flg_pad); @@ -357,6 +357,60 @@ PYBIND11_MODULE(_functions, pymodule) { return InterceptMjErrors(::mj_setState)(m, d, state.data(), sig); }); Def(pymodule); + Def(pymodule); + Def( + pymodule, + [](const raw::MjModel* m, const raw::MjData* d, int id, mjtNum time, + Eigen::Ref result, int order) { + int dim = m->sensor_dim[id]; + if (result.size() != dim) { + throw py::type_error("result should have length sensor_dim[id]"); + } + const mjtNum* ptr = InterceptMjErrors(::mj_readSensor)( + m, d, id, time, result.data(), order); + if (ptr && ptr != result.data()) { + for (int i = 0; i < dim; ++i) { + result[i] = ptr[i]; + } + } + return result; + }); + Def( + pymodule, + [](const raw::MjModel* m, raw::MjData* d, int id, + std::optional> times, + Eigen::Ref values) { + int nhistory = m->actuator_history[2*id]; + if (times.has_value() && times->size() != nhistory) { + throw py::type_error( + "times should have length actuator_history[2*id]"); + } + if (values.size() != nhistory) { + throw py::type_error( + "values should have length actuator_history[2*id]"); + } + return InterceptMjErrors(::mj_initCtrlHistory)( + m, d, id, + times.has_value() ? times->data() : nullptr, values.data()); + }); + Def( + pymodule, [](const raw::MjModel* m, raw::MjData* d, int id, + std::optional> times, + Eigen::Ref values, mjtNum phase) { + int nhistory = m->sensor_history[2 * id]; + int dim = m->sensor_dim[id]; + if (times.has_value() && times->size() != nhistory) { + throw py::type_error("times should have length sensor_history[2*id]"); + } + if (values.rows() != nhistory || values.cols() != dim) { + throw py::type_error( + "values should have shape (sensor_history[2*id], " + "sensor_dim[id])"); + } + return InterceptMjErrors(::mj_initSensorHistory)( + m, d, id, times.has_value() ? times->data() : nullptr, + values.data(), phase); + }); Def(pymodule); Def(pymodule); Def(pymodule); diff --git a/python/mujoco/introspect/enums.py b/python/mujoco/introspect/enums.py index 1e8fa84e..711b0ee2 100644 --- a/python/mujoco/introspect/enums.py +++ b/python/mujoco/introspect/enums.py @@ -522,20 +522,21 @@ ENUMS: Mapping[str, EnumDecl] = dict([ ('mjSTATE_QPOS', 2), ('mjSTATE_QVEL', 4), ('mjSTATE_ACT', 8), - ('mjSTATE_WARMSTART', 16), - ('mjSTATE_CTRL', 32), - ('mjSTATE_QFRC_APPLIED', 64), - ('mjSTATE_XFRC_APPLIED', 128), - ('mjSTATE_EQ_ACTIVE', 256), - ('mjSTATE_MOCAP_POS', 512), - ('mjSTATE_MOCAP_QUAT', 1024), - ('mjSTATE_USERDATA', 2048), - ('mjSTATE_PLUGIN', 4096), - ('mjNSTATE', 13), - ('mjSTATE_PHYSICS', 14), - ('mjSTATE_FULLPHYSICS', 4111), - ('mjSTATE_USER', 4064), - ('mjSTATE_INTEGRATION', 8191), + ('mjSTATE_HISTORY', 16), + ('mjSTATE_WARMSTART', 32), + ('mjSTATE_CTRL', 64), + ('mjSTATE_QFRC_APPLIED', 128), + ('mjSTATE_XFRC_APPLIED', 256), + ('mjSTATE_EQ_ACTIVE', 512), + ('mjSTATE_MOCAP_POS', 1024), + ('mjSTATE_MOCAP_QUAT', 2048), + ('mjSTATE_USERDATA', 4096), + ('mjSTATE_PLUGIN', 8192), + ('mjNSTATE', 14), + ('mjSTATE_PHYSICS', 30), + ('mjSTATE_FULLPHYSICS', 8223), + ('mjSTATE_USER', 8128), + ('mjSTATE_INTEGRATION', 16383), ]), )), ('mjtConstraint', diff --git a/python/mujoco/introspect/functions.py b/python/mujoco/introspect/functions.py index 33f48e46..cfd60c74 100644 --- a/python/mujoco/introspect/functions.py +++ b/python/mujoco/introspect/functions.py @@ -2636,6 +2636,156 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ ), doc='Copy state from src to dst.', )), + ('mj_readCtrl', + FunctionDecl( + name='mj_readCtrl', + return_type=ValueType(name='mjtNum'), + parameters=( + FunctionParameterDecl( + name='m', + type=PointerType( + inner_type=ValueType(name='mjModel', is_const=True), + ), + ), + FunctionParameterDecl( + name='d', + type=PointerType( + inner_type=ValueType(name='mjData', is_const=True), + ), + ), + FunctionParameterDecl( + name='id', + type=ValueType(name='int'), + ), + FunctionParameterDecl( + name='time', + type=ValueType(name='mjtNum'), + ), + FunctionParameterDecl( + name='interp', + type=ValueType(name='int'), + ), + ), + doc='Read ctrl value for actuator at given time. Returns d->ctrl[id] if no history, otherwise reads from history buffer. interp: 0=zero-order-hold, 1=linear, 2=cubic spline.', # pylint: disable=line-too-long + )), + ('mj_readSensor', + FunctionDecl( + name='mj_readSensor', + return_type=PointerType( + inner_type=ValueType(name='mjtNum', is_const=True), + ), + parameters=( + FunctionParameterDecl( + name='m', + type=PointerType( + inner_type=ValueType(name='mjModel', is_const=True), + ), + ), + FunctionParameterDecl( + name='d', + type=PointerType( + inner_type=ValueType(name='mjData', is_const=True), + ), + ), + FunctionParameterDecl( + name='id', + type=ValueType(name='int'), + ), + FunctionParameterDecl( + name='time', + type=ValueType(name='mjtNum'), + ), + FunctionParameterDecl( + name='result', + type=PointerType( + inner_type=ValueType(name='mjtNum'), + ), + ), + FunctionParameterDecl( + name='interp', + type=ValueType(name='int'), + ), + ), + doc='Read sensor value from history buffer at given time. Returns pointer to sensordata (no history) or history buffer (exact match), or NULL if interpolation performed (writes to result). interp: 0=zero-order-hold, 1=linear, 2=cubic spline.', # pylint: disable=line-too-long + )), + ('mj_initCtrlHistory', + FunctionDecl( + name='mj_initCtrlHistory', + return_type=ValueType(name='void'), + parameters=( + FunctionParameterDecl( + name='m', + type=PointerType( + inner_type=ValueType(name='mjModel', is_const=True), + ), + ), + FunctionParameterDecl( + name='d', + type=PointerType( + inner_type=ValueType(name='mjData'), + ), + ), + FunctionParameterDecl( + name='id', + type=ValueType(name='int'), + ), + FunctionParameterDecl( + name='times', + type=PointerType( + inner_type=ValueType(name='mjtNum', is_const=True), + ), + nullable=True, + ), + FunctionParameterDecl( + name='values', + type=PointerType( + inner_type=ValueType(name='mjtNum', is_const=True), + ), + ), + ), + doc='Initialize history buffer for actuator; if times is NULL, uses existing buffer timestamps.', # pylint: disable=line-too-long + )), + ('mj_initSensorHistory', + FunctionDecl( + name='mj_initSensorHistory', + return_type=ValueType(name='void'), + parameters=( + FunctionParameterDecl( + name='m', + type=PointerType( + inner_type=ValueType(name='mjModel', is_const=True), + ), + ), + FunctionParameterDecl( + name='d', + type=PointerType( + inner_type=ValueType(name='mjData'), + ), + ), + FunctionParameterDecl( + name='id', + type=ValueType(name='int'), + ), + FunctionParameterDecl( + name='times', + type=PointerType( + inner_type=ValueType(name='mjtNum', is_const=True), + ), + nullable=True, + ), + FunctionParameterDecl( + name='values', + type=PointerType( + inner_type=ValueType(name='mjtNum', is_const=True), + ), + ), + FunctionParameterDecl( + name='phase', + type=ValueType(name='mjtNum'), + ), + ), + doc='Initialize history buffer for sensor; if times is NULL, uses existing buffer timestamps. phase sets the user slot (last computation time for interval sensors).', # pylint: disable=line-too-long + )), ('mj_setKeyframe', FunctionDecl( name='mj_setKeyframe', diff --git a/python/mujoco/introspect/structs.py b/python/mujoco/introspect/structs.py index ac7bbdda..c20f26d6 100644 --- a/python/mujoco/introspect/structs.py +++ b/python/mujoco/introspect/structs.py @@ -1277,6 +1277,11 @@ STRUCTS: Mapping[str, StructDecl] = dict([ type=ValueType(name='mjtSize'), doc='number of mjtNums in plugin state vector', ), + StructFieldDecl( + name='nhistory', + type=ValueType(name='mjtSize'), + doc='number of mjtNums in history buffer', + ), StructFieldDecl( name='narena', type=ValueType(name='mjtSize'), @@ -4157,6 +4162,30 @@ STRUCTS: Mapping[str, StructDecl] = dict([ doc='group for visibility', array_extent=('nu',), ), + StructFieldDecl( + name='actuator_history', + type=PointerType( + inner_type=ValueType(name='int'), + ), + doc='history buffer: [nsample, interp]', + array_extent=('nu', 2), + ), + StructFieldDecl( + name='actuator_historyadr', + type=PointerType( + inner_type=ValueType(name='int'), + ), + doc='address in history buffer; -1: none', + array_extent=('nu',), + ), + StructFieldDecl( + name='actuator_delay', + type=PointerType( + inner_type=ValueType(name='mjtNum'), + ), + doc='delay time in seconds; 0: no delay', + array_extent=('nu',), + ), StructFieldDecl( name='actuator_ctrllimited', type=PointerType( @@ -4389,6 +4418,38 @@ STRUCTS: Mapping[str, StructDecl] = dict([ doc='noise standard deviation', array_extent=('nsensor',), ), + StructFieldDecl( + name='sensor_history', + type=PointerType( + inner_type=ValueType(name='int'), + ), + doc='history buffer: [nsample, interp]', + array_extent=('nsensor', 2), + ), + StructFieldDecl( + name='sensor_historyadr', + type=PointerType( + inner_type=ValueType(name='int'), + ), + doc='address in history buffer; -1: none', + array_extent=('nsensor',), + ), + StructFieldDecl( + name='sensor_delay', + type=PointerType( + inner_type=ValueType(name='mjtNum'), + ), + doc='delay time in seconds; 0: no delay', + array_extent=('nsensor',), + ), + StructFieldDecl( + name='sensor_interval', + type=PointerType( + inner_type=ValueType(name='mjtNum'), + ), + doc='interval: [period, phase] in seconds', + array_extent=('nsensor', 2), + ), StructFieldDecl( name='sensor_user', type=PointerType( @@ -5402,6 +5463,14 @@ STRUCTS: Mapping[str, StructDecl] = dict([ doc='actuator activation', array_extent=('na',), ), + StructFieldDecl( + name='history', + type=PointerType( + inner_type=ValueType(name='mjtNum'), + ), + doc='history buffer', + array_extent=('nhistory',), + ), StructFieldDecl( name='qacc_warmstart', type=PointerType( @@ -9173,6 +9242,21 @@ STRUCTS: Mapping[str, StructDecl] = dict([ type=ValueType(name='int'), doc='group', ), + StructFieldDecl( + name='nsample', + type=ValueType(name='int'), + doc='number of samples in history buffer', + ), + StructFieldDecl( + name='interp', + type=ValueType(name='int'), + doc='interpolation order (0=ZOH, 1=linear, 2=cubic)', + ), + StructFieldDecl( + name='delay', + type=ValueType(name='double'), + doc='delay time in seconds; 0: no delay', + ), StructFieldDecl( name='userdata', type=PointerType( @@ -9268,6 +9352,29 @@ STRUCTS: Mapping[str, StructDecl] = dict([ type=ValueType(name='double'), doc='noise stdev', ), + StructFieldDecl( + name='nsample', + type=ValueType(name='int'), + doc='number of samples in history buffer', + ), + StructFieldDecl( + name='interp', + type=ValueType(name='int'), + doc='interpolation order (0=ZOH, 1=linear, 2=cubic)', + ), + StructFieldDecl( + name='delay', + type=ValueType(name='double'), + doc='delay time in seconds', + ), + StructFieldDecl( + name='interval', + type=ArrayType( + inner_type=ValueType(name='double'), + extents=(2,), + ), + doc='[period, time_prev] in seconds', + ), StructFieldDecl( name='userdata', type=PointerType( diff --git a/src/engine/engine_derivative_fd.c b/src/engine/engine_derivative_fd.c index 95905b91..93fbaa76 100644 --- a/src/engine/engine_derivative_fd.c +++ b/src/engine/engine_derivative_fd.c @@ -295,6 +295,10 @@ void mjd_smooth_velFD(const mjModel* m, mjData* d, mjtNum eps) { void mjd_stepFD(const mjModel* m, mjData* d, mjtNum eps, mjtByte flg_centered, mjtNum* DyDq, mjtNum* DyDv, mjtNum* DyDa, mjtNum* DyDu, mjtNum* DsDq, mjtNum* DsDv, mjtNum* DsDa, mjtNum* DsDu) { + if (m->nhistory) { + mjERROR("delays are not supported"); + } + int nq = m->nq, nv = m->nv, na = m->na, nu = m->nu, ns = m->nsensordata; int ndx = 2*nv+na; // row length of Dy Jacobians mj_markStack(d); @@ -540,6 +544,9 @@ void mjd_transitionFD(const mjModel* m, mjData* d, mjtNum eps, mjtByte flg_cente if (m->opt.integrator == mjINT_RK4) { mjERROR("RK4 integrator is not supported"); } + if (m->nhistory) { + mjERROR("delays are not supported"); + } int nv = m->nv, na = m->na, nu = m->nu, ns = m->nsensordata; int ndx = 2*nv+na; // row length of state Jacobians diff --git a/src/engine/engine_forward.c b/src/engine/engine_forward.c index eae2a198..7d0f2736 100644 --- a/src/engine/engine_forward.c +++ b/src/engine/engine_forward.c @@ -320,10 +320,17 @@ void mj_fwdActuation(const mjModel* m, mjData* d) { // any tendon transmission targets with force limits int tendon_frclimited = 0; - // local, clamped copy of ctrl + // local copy of ctrl mj_markStack(d); mjtNum *ctrl = mjSTACKALLOC(d, nu, mjtNum); - mju_copy(ctrl, d->ctrl, nu); + + // read from ctrl or history buffer for delayed actuators + for (int i = 0; i < nu; i++) { + int interp = m->actuator_history[2*i+1]; + ctrl[i] = m->actuator_delay[i] ? mj_readCtrl(m, d, i, d->time, interp) : d->ctrl[i]; + } + + // clamp local copy if (!mjDISABLED(mjDSBL_CLAMPCTRL)) { clampVec(ctrl, m->actuator_ctrlrange, m->actuator_ctrllimited, nu, NULL); } @@ -846,14 +853,64 @@ void mj_fwdConstraint(const mjModel* m, mjData* d) { } -//-------------------------- integrators ---------------------------------------------------------- +//-------------------------- state advancement and integration ------------------------------------ // advance state and time given activation derivatives, acceleration, and optional velocity static void mj_advance(const mjModel* m, mjData* d, const mjtNum* act_dot, const mjtNum* qacc, const mjtNum* qvel) { + int nu = m->nu, nsensor = m->nsensor; + + // advance history buffers + if (m->nhistory > 0) { + // advance ctrl history buffers + for (int i = 0; i < nu; i++) { + int nsample = m->actuator_history[2*i]; + if (nsample == 0) continue; + + // get history buffer pointer and insert ctrl at current time + mjtNum* buf = d->history + m->actuator_historyadr[i]; + *mju_delayInsert(buf, nsample, /*dim=*/1, d->time) = d->ctrl[i]; + } + + // advance sensor history buffers + for (int i = 0; i < nsensor; i++) { + int nsample = m->sensor_history[2*i]; + if (nsample == 0) continue; + + // get history buffer parameters + int dim = m->sensor_dim[i]; + mjtNum* buf = d->history + m->sensor_historyadr[i]; + mjtNum delay = m->sensor_delay[i]; + mjtNum interval = m->sensor_interval[2*i]; + + if (interval > 0) { + // interval mode: if condition is satisfied, compute; otherwise copy + mjtNum time_prev = buf[0]; // first slot stores previous sensor tick + if (time_prev + interval <= d->time) { + buf[0] += interval; // advance by exact interval (continuous time) + mjtNum* slot = mju_delayInsert(buf, nsample, dim, d->time); + if (delay > 0) { + // have delay, compute sensor + mj_computeSensor(m, d, i, slot); + } else { + // no delay, copy from sensordata (already computed) + mju_copy(slot, d->sensordata + m->sensor_adr[i], dim); + } + } + } else if (delay > 0) { + // delay-only mode: always compute and insert + mjtNum* slot = mju_delayInsert(buf, nsample, dim, d->time); + mj_computeSensor(m, d, i, slot); + } else { + // history-only mode: copy from sensordata (already computed) + mjtNum* slot = mju_delayInsert(buf, nsample, dim, d->time); + mju_copy(slot, d->sensordata + m->sensor_adr[i], dim); + } + } + } + // advance activations if (m->na && !mjDISABLED(mjDSBL_ACTUATION)) { - int nu = m->nu; for (int i=0; i < nu; i++) { int actadr = m->actuator_actadr[i]; int actadr_end = actadr + m->actuator_actnum[i]; diff --git a/src/engine/engine_io.c b/src/engine/engine_io.c index 51e4d833..8bb97d15 100644 --- a/src/engine/engine_io.c +++ b/src/engine/engine_io.c @@ -224,8 +224,8 @@ void mj_makeModel(mjModel** dest, // CHECK SIZE PARAMETERS { // dummy variables for MJMODEL_SIZES set after mjModel construction - int nnames_map=0, nJmom=0, ngravcomp=0, nemax=0, njmax=0; - int nconmax=0, nuserdata=0, nsensordata=0, npluginstate=0, narena=0, nbuffer=0; + int nnames_map=0, nJmom=0, ngravcomp=0, nemax=0, njmax=0, nconmax=0; + int nuserdata=0, nsensordata=0, npluginstate=0, nhistory=0, narena=0, nbuffer=0; // sizes must be non-negative and fit in int, except for the byte arrays texdata and textdata #define X(name) \ @@ -243,8 +243,9 @@ void mj_makeModel(mjModel** dest, #undef X // suppress unused variable warnings - (void)nnames_map; (void)nJmom; (void)ngravcomp; (void)nemax; (void)njmax; - (void)nconmax; (void)nuserdata; (void)nsensordata; (void)npluginstate; (void)narena; (void)nbuffer; + (void)nnames_map; (void)nJmom; (void)ngravcomp; (void)nemax; (void)njmax; (void)nconmax; + (void)nuserdata; (void)nsensordata; (void)npluginstate; (void)nhistory; (void)narena; + (void)nbuffer; } // nbody should always be positive @@ -1262,6 +1263,12 @@ mjData* mjv_copyData(mjData* dest, const mjModel* m, const mjData* src) { // clear data, set defaults static void _resetData(const mjModel* m, mjData* d, unsigned char debug_value) { + // error early if history buffers cannot be initialized + mjtNum dt = m->opt.timestep; + if (m->nhistory && dt <= 0) { + mjERROR("history buffers require positive timestep, got %g", dt); + } + //------------------------------ save plugin state and data mjtNum* plugin_state; uintptr_t* plugindata; @@ -1367,6 +1374,58 @@ static void _resetData(const mjModel* m, mjData* d, unsigned char debug_value) { mju_zero(d->mocap_pos, 3*m->nmocap); mju_zero(d->mocap_quat, 4*m->nmocap); + // initialize ctrl history buffers: timestamps at [-n*dt, ..., -dt] + for (int i = 0; i < m->nu; i++) { + int n = m->actuator_history[2*i]; + if (n > 0) { + mjtNum* buf = d->history + m->actuator_historyadr[i]; + buf[0] = 0; // user slot + buf[1] = n - 1; // cursor: newest at logical index n-1 + mjtNum* times = buf + 2; + for (int j = 0; j < n; j++) { + times[j] = -(n-j)*dt; + } + + // clear values + mjtNum* values = buf + 2 + n; + mju_zero(values, n); + } + } + + // initialize sensor history buffers + for (int i = 0; i < m->nsensor; i++) { + int n = m->sensor_history[2*i]; + if (n > 0) { + int dim = m->sensor_dim[i]; + mjtNum period = m->sensor_interval[2*i]; + mjtNum phase = m->sensor_interval[2*i+1]; + mjtNum* buf = d->history + m->sensor_historyadr[i]; + + // user slot: last compute time (phase=0 means -period, i.e. first compute at t=0) + buf[0] = (period > 0) ? (phase == 0 ? -period : phase) : -dt; + buf[1] = n - 1; // cursor: newest at logical index n-1 + + mjtNum* times = buf + 2; + if (period > 0) { + // samples spaced at period intervals, rounded up to dt grid + mjtNum t0 = (phase == 0) ? -period : phase; + for (int j = 0; j < n; j++) { + mjtNum continuous_t = t0 - (n-1-j)*period; + times[j] = mju_ceil(continuous_t / dt) * dt; + } + } else { + // no period: timestamps at [-n*dt, ..., -dt] + for (int j = 0; j < n; j++) { + times[j] = -(n-j)*dt; + } + } + + // clear values + mjtNum* values = buf + 2 + n; + mju_zero(values, n*dim); + } + } + // zero out qM, special case because scattering from M skips simple body off-diagonals mju_zero(d->qM, m->nM); diff --git a/src/engine/engine_print.c b/src/engine/engine_print.c index 66291819..b2143c9c 100644 --- a/src/engine/engine_print.c +++ b/src/engine/engine_print.c @@ -120,6 +120,51 @@ static void printArray2dInt(const char* str, int nr, int nc, const int* data, FI } +// print history buffer with semantic labels +static void printDelayBuffer(const char* name, const mjtNum* buf, int nhistory, int dim, + FILE* fp, const char* float_format) { + if (!buf || nhistory <= 0) { + return; + } + fprintf(fp, " %s:\n", name); + + // user value (first slot) + fprintf(fp, " phase = "); + fprintf(fp, float_format, buf[0]); + fprintf(fp, "\n"); + + // cursor (second slot, stored as mjtNum but is an integer) + fprintf(fp, " cursor = %d\n", (int)buf[1]); + + // timestamps + const mjtNum* times = buf + 2; + fprintf(fp, " times = "); + for (int i = 0; i < nhistory; i++) { + fprintf(fp, float_format, times[i]); + } + fprintf(fp, "\n"); + + // values + const mjtNum* values = times + nhistory; + if (dim == 1) { + fprintf(fp, " values = "); + for (int i = 0; i < nhistory; i++) { + fprintf(fp, float_format, values[i]); + } + fprintf(fp, "\n"); + } else { + fprintf(fp, " values:\n"); + for (int i = 0; i < nhistory; i++) { + fprintf(fp, " [%d] =", i); + for (int j = 0; j < dim; j++) { + fprintf(fp, float_format, values[i*dim + j]); + } + fprintf(fp, "\n"); + } + } +} + + // print sparse matrix static void printSparse(const char* str, const mjtNum* mat, int nr, const int* rownnz, const int* rowadr, @@ -1241,6 +1286,36 @@ void mj_printFormattedData(const mjModel* m, const mjData* d, const char* filena printArray2d("QPOS", m->nq, 1, d->qpos, fp, float_format); printArray2d("QVEL", m->nv, 1, d->qvel, fp, float_format); printArray2d("ACT", m->na, 1, d->act, fp, float_format); + + // print history buffers with semantic structure + if (m->nhistory) { + fprintf(fp, "DELAY\n"); + + // actuator history buffers + for (int i = 0; i < m->nu; i++) { + int adr = m->actuator_historyadr[i]; + if (adr >= 0) { + char name[100]; + const char* actuator_name = mj_id2name(m, mjOBJ_ACTUATOR, i); + snprintf(name, sizeof(name), "actuator %d '%s'", i, actuator_name ? actuator_name : ""); + printDelayBuffer(name, d->history + adr, m->actuator_history[2*i], 1, fp, float_format); + } + } + + // sensor history buffers + for (int i = 0; i < m->nsensor; i++) { + int adr = m->sensor_historyadr[i]; + if (adr >= 0) { + char name[100]; + const char* sensor_name = mj_id2name(m, mjOBJ_SENSOR, i); + snprintf(name, sizeof(name), "sensor %d '%s'", i, sensor_name ? sensor_name : ""); + printDelayBuffer(name, d->history + adr, m->sensor_history[2*i], m->sensor_dim[i], + fp, float_format); + } + } + + fprintf(fp, "\n"); + } printArray2d("QACC_WARMSTART", m->nv, 1, d->qacc_warmstart, fp, float_format); printArray2d("CTRL", m->nu, 1, d->ctrl, fp, float_format); printArray2d("QFRC_APPLIED", m->nv, 1, d->qfrc_applied, fp, float_format); diff --git a/src/engine/engine_sensor.c b/src/engine/engine_sensor.c index 508f0c5d..b011cba6 100644 --- a/src/engine/engine_sensor.c +++ b/src/engine/engine_sensor.c @@ -1343,6 +1343,51 @@ void mj_computeSensor(const mjModel* m, mjData* d, int i, mjtNum* sensordata) { } +// compute sensor or read from history buffer (handles delay and interval logic) +static void compute_or_read_sensor(const mjModel* m, mjData* d, int i, mjtNum* sensordata) { + int nsample = m->sensor_history[2*i]; + + // no history: compute directly + if (nsample <= 0) { + mj_computeSensor(m, d, i, sensordata); + return; + } + + mjtNum delay = m->sensor_delay[i]; + int dim = m->sensor_dim[i]; + + // delay > 0: read delayed value from buffer + if (delay > 0) { + int interp = m->sensor_history[2*i+1]; + const mjtNum* ptr = mj_readSensor(m, d, i, d->time, sensordata, interp); + if (ptr) mju_copy(sensordata, ptr, dim); + return; + } + + // interval > 0: compute if interval condition satisfied, else read from buffer + mjtNum interval = m->sensor_interval[2*i]; + if (interval > 0) { + int historyadr = m->sensor_historyadr[i]; + mjtNum* buf = d->history + historyadr; + mjtNum time_prev = buf[0]; // first slot stores time_prev + + if (time_prev + interval <= d->time) { + // interval condition satisfied: compute new sensor value + mj_computeSensor(m, d, i, sensordata); + } else { + // interval condition not satisfied: read from buffer + int interp = m->sensor_history[2*i+1]; + const mjtNum* ptr = mj_readSensor(m, d, i, d->time, sensordata, interp); + if (ptr) mju_copy(sensordata, ptr, dim); + } + return; + } + + // history only, no delay or interval: compute directly + mj_computeSensor(m, d, i, sensordata); +} + + // compute user sensors: call user callback and apply cutoff static void compute_user_sensors(const mjModel* m, mjData* d, mjtStage stage) { if (mjcb_sensor) { @@ -1438,13 +1483,14 @@ void mj_sensorPos(const mjModel* m, mjData* d) { if (m->sensor_needstage[i] == mjSTAGE_POS) { int adr = m->sensor_adr[i]; + mjtNum* sensordata = d->sensordata + adr; if (type == mjSENS_USER) { // clear result, compute later - mju_zero(d->sensordata + adr, m->sensor_dim[i]); + mju_zero(sensordata, m->sensor_dim[i]); nusersensor++; } else { - mj_computeSensor(m, d, i, d->sensordata + adr); + compute_or_read_sensor(m, d, i, sensordata); } } } @@ -1486,6 +1532,7 @@ void mj_sensorVel(const mjModel* m, mjData* d) { if (m->sensor_needstage[i] == mjSTAGE_VEL) { mjtSensor type = m->sensor_type[i]; int adr = m->sensor_adr[i]; + mjtNum* sensordata = d->sensordata + adr; if (type == mjSENS_USER) { // call mj_subtreeVel for user sensors @@ -1494,10 +1541,10 @@ void mj_sensorVel(const mjModel* m, mjData* d) { } // clear result, compute later - mju_zero(d->sensordata + adr, m->sensor_dim[i]); + mju_zero(sensordata, m->sensor_dim[i]); nusersensor++; } else { - mj_computeSensor(m, d, i, d->sensordata + adr); + compute_or_read_sensor(m, d, i, sensordata); } } } @@ -1539,6 +1586,7 @@ void mj_sensorAcc(const mjModel* m, mjData* d) { if (m->sensor_needstage[i] == mjSTAGE_ACC) { mjtSensor type = m->sensor_type[i]; int adr = m->sensor_adr[i]; + mjtNum* sensordata = d->sensordata + adr; if (type == mjSENS_USER) { // call mj_rnePostConstraint for user sensors @@ -1547,10 +1595,10 @@ void mj_sensorAcc(const mjModel* m, mjData* d) { } // clear result, compute later - mju_zero(d->sensordata + adr, m->sensor_dim[i]); + mju_zero(sensordata, m->sensor_dim[i]); nusersensor++; } else { - mj_computeSensor(m, d, i, d->sensordata + adr); + compute_or_read_sensor(m, d, i, sensordata); } } } diff --git a/src/engine/engine_sensor.h b/src/engine/engine_sensor.h index 377481e7..deef11f8 100644 --- a/src/engine/engine_sensor.h +++ b/src/engine/engine_sensor.h @@ -25,6 +25,9 @@ extern "C" { //-------------------------------- sensors --------------------------------------------------------- +// compute value for one sensor, write to sensordata, apply cutoff +void mj_computeSensor(const mjModel* m, mjData* d, int i, mjtNum* sensordata); + // position-dependent sensors MJAPI void mj_sensorPos(const mjModel* m, mjData* d); diff --git a/src/engine/engine_support.c b/src/engine/engine_support.c index 91a36e82..a75bb405 100644 --- a/src/engine/engine_support.c +++ b/src/engine/engine_support.c @@ -132,6 +132,7 @@ static inline int mj_stateElemSize(const mjModel* m, mjtState sig) { case mjSTATE_QPOS: return m->nq; case mjSTATE_QVEL: return m->nv; case mjSTATE_ACT: return m->na; + case mjSTATE_HISTORY: return m->nhistory; case mjSTATE_WARMSTART: return m->nv; case mjSTATE_CTRL: return m->nu; case mjSTATE_QFRC_APPLIED: return m->nv; @@ -155,6 +156,7 @@ static inline mjtNum* mj_stateElemPtr(const mjModel* m, mjData* d, mjtState sig) case mjSTATE_QPOS: return d->qpos; case mjSTATE_QVEL: return d->qvel; case mjSTATE_ACT: return d->act; + case mjSTATE_HISTORY: return d->history; case mjSTATE_WARMSTART: return d->qacc_warmstart; case mjSTATE_CTRL: return d->ctrl; case mjSTATE_QFRC_APPLIED: return d->qfrc_applied; @@ -619,7 +621,7 @@ void mj_differentiatePos(const mjModel* m, mjtNum* qvel, mjtNum dt, vadr += 3; padr += 3; - // continute with rotations + // continue with rotations mjFALLTHROUGH; case mjJNT_BALL: @@ -806,3 +808,113 @@ void mju_camIntrinsics(const mjModel* m, int camid, // extent only used for orthographic cameras *extent = m->cam_fovy[camid]; } + + +// read delayed ctrl value for actuator at given time +mjtNum mj_readCtrl(const mjModel* m, const mjData* d, int id, mjtNum time, int interp) { + // validate actuator id + if (id < 0 || id >= m->nu) { + mjERROR("invalid actuator id %d", id); + return 0; + } + + // no delay: return current ctrl value + int nsample = m->actuator_history[2*id]; + if (nsample == 0) { + return d->ctrl[id]; + } + + // resolve interpolation order: use model's interp if argument is -1 + if (interp < 0) interp = m->actuator_history[2*id+1]; + + // get buffer pointer and read from history buffer + mjtNum delay = m->actuator_delay[id]; + const mjtNum* buf = d->history + m->actuator_historyadr[id]; + mjtNum res; + const mjtNum* ptr = mju_delayRead(buf, nsample, /*dim=*/1, &res, time - delay, interp); + return ptr ? *ptr : res; +} + + +// read sensor value from history buffer at given time +const mjtNum* mj_readSensor(const mjModel* m, const mjData* d, int id, mjtNum time, + mjtNum* result, int interp) { + // validate sensor id + if (id < 0 || id >= m->nsensor) { + mjERROR("invalid sensor id %d", id); + return NULL; + } + + // no history: return current sensor value + int nsample = m->sensor_history[2*id]; + if (nsample == 0) { + return d->sensordata + m->sensor_adr[id]; + } + + // resolve interpolation order: use model's interp if argument is -1 + if (interp < 0) interp = m->sensor_history[2*id+1]; + + // get buffer pointer and read from history buffer + int dim = m->sensor_dim[id]; + mjtNum delay = m->sensor_delay[id]; + const mjtNum* buf = d->history + m->sensor_historyadr[id]; + return mju_delayRead(buf, nsample, dim, result, time - delay, interp); +} + + +// initialize history buffer for actuator +void mj_initCtrlHistory(const mjModel* m, mjData* d, int id, + const mjtNum* times, const mjtNum* values) { + // validate actuator id + if (id < 0 || id >= m->nu) { + mjERROR("invalid actuator id %d", id); + return; + } + + // check that actuator has a history buffer + int nsample = m->actuator_history[2*id]; + if (nsample == 0) { + mjERROR("actuator %d has no history buffer", id); + return; + } + + // get buffer pointer + mjtNum* buf = d->history + m->actuator_historyadr[id]; + + // if times is NULL, use existing buffer times + const mjtNum* buf_times = times ? times : buf + 2; + + // get existing user value (preserve it) + mjtNum user = buf[0]; + + // initialize history buffer + mju_delayInit(buf, nsample, 1, buf_times, values, user); +} + + +// initialize history buffer for sensor +void mj_initSensorHistory(const mjModel* m, mjData* d, int id, + const mjtNum* times, const mjtNum* values, mjtNum phase) { + // validate sensor id + if (id < 0 || id >= m->nsensor) { + mjERROR("invalid sensor id %d", id); + return; + } + + // check that sensor has a history buffer + int nsample = m->sensor_history[2*id]; + if (nsample == 0) { + mjERROR("sensor %d has no history buffer", id); + return; + } + + // get buffer pointer and dimension + mjtNum* buf = d->history + m->sensor_historyadr[id]; + int dim = m->sensor_dim[id]; + + // if times is NULL, use existing buffer times + const mjtNum* buf_times = times ? times : buf + 2; + + // initialize history buffer with provided phase + mju_delayInit(buf, nsample, dim, buf_times, values, phase); +} diff --git a/src/engine/engine_support.h b/src/engine/engine_support.h index 552f1ba4..280f6003 100644 --- a/src/engine/engine_support.h +++ b/src/engine/engine_support.h @@ -130,6 +130,29 @@ void mju_camIntrinsics(const mjModel* m, int camid, mjtNum* fx, mjtNum* fy, mjtNum* cx, mjtNum* cy, mjtNum* ortho_extent); +// read ctrl value for actuator at given time +// returns d->ctrl[id] if no history, otherwise reads from history buffer +// interp: 0=zero-order-hold, 1=linear, 2=cubic spline +MJAPI mjtNum mj_readCtrl(const mjModel* m, const mjData* d, int id, mjtNum time, int interp); + +// read sensor value from history buffer at given time +// returns pointer to sensordata (no history) or history buffer (exact match), +// or NULL if interpolation performed (writes to result) +// interp: 0=zero-order-hold, 1=linear, 2=cubic spline +MJAPI const mjtNum* mj_readSensor(const mjModel* m, const mjData* d, int id, mjtNum time, + mjtNum* result, int interp); + +// initialize history buffer for actuator with given values +// if times is NULL, uses existing buffer timestamps +MJAPI void mj_initCtrlHistory(const mjModel* m, mjData* d, int id, + const mjtNum* times, const mjtNum* values); + +// initialize history buffer for sensor with given values +// if times is NULL, uses existing buffer timestamps +// phase sets the user slot (last computation time for interval sensors) +MJAPI void mj_initSensorHistory(const mjModel* m, mjData* d, int id, + const mjtNum* times, const mjtNum* values, mjtNum phase); + #ifdef __cplusplus } #endif diff --git a/src/user/user_model.cc b/src/user/user_model.cc index 588a514e..a7649762 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -2249,6 +2249,20 @@ void mjCModel::SetSizes() { nsensordata += sensors_[i]->dim; } + // nhistory: layout is [user, cursor, times(n), values(n*dim)] = 2+2n per actuator (dim=1) + nhistory = 0; + for (int i=0; i < actuators_.size(); i++) { + if (actuators_[i]->nsample > 0) { + nhistory += 2 + 2 * actuators_[i]->nsample; + } + } + // sensor delay: layout is [user, cursor, times(n), values(n*dim)] = 2 + n + n*dim + for (int i=0; i < sensors_.size(); i++) { + if (sensors_[i]->nsample > 0) { + nhistory += 2 + sensors_[i]->nsample + sensors_[i]->nsample * sensors_[i]->dim; + } + } + // nnumericdata for (int i=0; i < nnumeric; i++) { nnumericdata += numerics_[i]->size; @@ -3226,6 +3240,7 @@ void mjCModel::CopyObjects(mjModel* m) { m->njmax = njmax; m->nconmax = nconmax; m->nsensordata = nsensordata; + m->nhistory = nhistory; m->nuserdata = nuserdata; m->na = na; @@ -3724,6 +3739,7 @@ void mjCModel::CopyObjects(mjModel* m) { // actuators adr = 0; + int delay_adr = 0; for (int i=0; i < nu; i++) { // get pointer mjCActuator* pac = actuators_[i]; @@ -3741,6 +3757,18 @@ void mjCModel::CopyObjects(mjModel* m) { pac->actdim_ = m->actuator_actnum[i]; adr += m->actuator_actnum[i]; m->actuator_group[i] = pac->group; + + // historyadr + m->actuator_delay[i] = (mjtNum)pac->delay; + m->actuator_history[2*i] = pac->nsample; + m->actuator_history[2*i+1] = pac->interp; + if (pac->nsample > 0) { + m->actuator_historyadr[i] = delay_adr; + delay_adr += 2 + 2 * pac->nsample; // [user, cursor, times, values] + } else { + m->actuator_historyadr[i] = -1; + } + m->actuator_ctrllimited[i] = (mjtByte)pac->is_ctrllimited(); m->actuator_forcelimited[i] = (mjtByte)pac->is_forcelimited(); m->actuator_actlimited[i] = (mjtByte)pac->is_actlimited(); @@ -3775,6 +3803,21 @@ void mjCModel::CopyObjects(mjModel* m) { m->sensor_dim[i] = psen->dim; m->sensor_cutoff[i] = (mjtNum)psen->cutoff; m->sensor_noise[i] = (mjtNum)psen->noise; + + // history buffer + m->sensor_delay[i] = (mjtNum)psen->delay; + m->sensor_history[2*i] = psen->nsample; + m->sensor_history[2*i+1] = psen->interp; + m->sensor_interval[2*i] = (mjtNum)psen->interval[0]; + m->sensor_interval[2*i+1] = (mjtNum)psen->interval[1]; + if (psen->nsample > 0) { + m->sensor_historyadr[i] = delay_adr; + int dim = psen->dim; + delay_adr += 2 + psen->nsample + psen->nsample * dim; // [user, cursor, times(n), values(n*dim)] + } else { + m->sensor_historyadr[i] = -1; + } + mjuu_copyvec(m->sensor_user+nuser_sensor*i, psen->get_userdata().data(), nuser_sensor); // calculate address and advance diff --git a/src/user/user_model.h b/src/user/user_model.h index 579ab8f1..e2377016 100644 --- a/src/user/user_model.h +++ b/src/user/user_model.h @@ -118,6 +118,7 @@ class mjCModel_ : public mjsElement { mjtSize ntexdata; // number of texture bytes mjtSize nwrap; // number of wrap objects in all tendon paths mjtSize nsensordata; // number of mjtNums in sensor data vector + mjtSize nhistory; // number of mjtNums in history buffer mjtSize nnumericdata; // number of mjtNums in all custom fields mjtSize ntextdata; // number of chars in all text fields, including 0 mjtSize ntupledata; // number of objects in all tuple fields diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index a304aaef..9dfa3346 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -6960,6 +6960,17 @@ void mjCActuator::Compile(void) { throw mjCError(this, "plugin '%s' does not support actuators", pplugin->name); } } + + // validate delay + if (delay > 0 && nsample <= 0) { + throw mjCError(this, "setting delay > 0 without a history buffer"); + } + + // nsample is limited to 2^24 because the cursor is stored as an mjtNum, which may be a float + // single-precision floats can represent all integers up to 2^24 exactly + if (nsample > 16777216) { + throw mjCError(this, "at most 2^24 samples in history buffer, got %d", nullptr, nsample); + } } @@ -7291,6 +7302,31 @@ void mjCSensor::Compile(void) { throw mjCError(this, "negative cutoff in sensor"); } + // require non-negative interval + if (interval[0] < 0) { + throw mjCError(this, "negative interval in sensor"); + } + + // require non-positive phase + if (interval[1] > 0) { + throw mjCError(this, "positive phase in sensor"); + } + + // require phase > -period (values outside this are equivalent modulo period) + if (interval[0] > 0 && interval[1] <= -interval[0]) { + throw mjCError(this, "phase must be greater than -period in sensor"); + } + + // require nsample for delay + if (delay > 0 && nsample <= 0) { + throw mjCError(this, "setting delay > 0 without a history buffer"); + } + + // validate nsample size (max 2^24) + if (nsample > 16777216) { + throw mjCError(this, "at most 2^24 samples in sensor history buffer, got %d", nullptr, nsample); + } + // Find referenced object ResolveReferences(model); diff --git a/src/xml/xml_base.h b/src/xml/xml_base.h index 3a1bed5b..af636702 100644 --- a/src/xml/xml_base.h +++ b/src/xml/xml_base.h @@ -43,6 +43,7 @@ extern const int mark_sz; extern const int dyn_sz; extern const int gain_sz; extern const int bias_sz; +extern const int interp_sz; extern const int stage_sz; extern const int datatype_sz; extern const int camout_sz; @@ -74,6 +75,7 @@ extern const mjMap mark_map[]; extern const mjMap dyn_map[]; extern const mjMap gain_map[]; extern const mjMap bias_map[]; +extern const mjMap interp_map[]; extern const mjMap stage_map[]; extern const mjMap datatype_map[]; extern const mjMap condata_map[]; diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index 1572644d..430d1d4a 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -182,29 +182,29 @@ std::vector MJCF[nMJCF] = { "frictionloss", "springlength", "width", "material", "margin", "stiffness", "damping", "rgba", "user"}, {"general", "?", "ctrllimited", "forcelimited", "actlimited", "ctrlrange", - "forcerange", "actrange", "gear", "cranklength", "user", "group", "actdim", + "forcerange", "actrange", "gear", "cranklength", "user", "group", "nsample", "interp", "delay", "actdim", "dyntype", "gaintype", "biastype", "dynprm", "gainprm", "biasprm", "actearly"}, {"motor", "?", "ctrllimited", "forcelimited", "ctrlrange", "forcerange", - "gear", "cranklength", "user", "group"}, + "gear", "cranklength", "user", "group", "nsample", "interp", "delay"}, {"position", "?", "ctrllimited", "forcelimited", "ctrlrange", "inheritrange", - "forcerange", "gear", "cranklength", "user", "group", "kp", "kv", "dampratio", "timeconst"}, + "forcerange", "gear", "cranklength", "user", "group", "nsample", "interp", "delay", "kp", "kv", "dampratio", "timeconst"}, {"velocity", "?", "ctrllimited", "forcelimited", "ctrlrange", "forcerange", - "gear", "cranklength", "user", "group", "kv"}, + "gear", "cranklength", "user", "group", "nsample", "interp", "delay", "kv"}, {"intvelocity", "?", "ctrllimited", "forcelimited", "ctrlrange", "forcerange", "actrange", "inheritrange", - "gear", "cranklength", "user", "group", + "gear", "cranklength", "user", "group", "nsample", "interp", "delay", "kp", "kv", "dampratio"}, {"damper", "?", "forcelimited", "ctrlrange", "forcerange", - "gear", "cranklength", "user", "group", "kv"}, + "gear", "cranklength", "user", "group", "nsample", "interp", "delay", "kv"}, {"cylinder", "?", "ctrllimited", "forcelimited", "ctrlrange", "forcerange", - "gear", "cranklength", "user", "group", + "gear", "cranklength", "user", "group", "nsample", "interp", "delay", "timeconst", "area", "diameter", "bias"}, {"muscle", "?", "ctrllimited", "forcelimited", "ctrlrange", "forcerange", - "gear", "cranklength", "user", "group", + "gear", "cranklength", "user", "group", "nsample", "interp", "delay", "timeconst", "range", "force", "scale", "lmin", "lmax", "vmax", "fpmax", "fvmax"}, {"adhesion", "?", "forcelimited", "ctrlrange", "forcerange", - "gain", "user", "group"}, + "gain", "user", "group", "nsample", "interp", "delay"}, {">"}, {"extension", "*"}, @@ -389,51 +389,51 @@ std::vector MJCF[nMJCF] = { {"actuator", "*"}, {"<"}, - {"general", "*", "name", "class", "group", + {"general", "*", "name", "class", "group", "nsample", "interp", "delay", "ctrllimited", "forcelimited", "actlimited", "ctrlrange", "forcerange", "actrange", "lengthrange", "gear", "cranklength", "user", "joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite", "body", "actdim", "dyntype", "gaintype", "biastype", "dynprm", "gainprm", "biasprm", "actearly"}, - {"motor", "*", "name", "class", "group", + {"motor", "*", "name", "class", "group", "nsample", "interp", "delay", "ctrllimited", "forcelimited", "ctrlrange", "forcerange", "lengthrange", "gear", "cranklength", "user", "joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite"}, - {"position", "*", "name", "class", "group", + {"position", "*", "name", "class", "group", "nsample", "interp", "delay", "ctrllimited", "forcelimited", "ctrlrange", "inheritrange", "forcerange", "lengthrange", "gear", "cranklength", "user", "joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite", "kp", "kv", "dampratio", "timeconst"}, - {"velocity", "*", "name", "class", "group", + {"velocity", "*", "name", "class", "group", "nsample", "interp", "delay", "ctrllimited", "forcelimited", "ctrlrange", "forcerange", "lengthrange", "gear", "cranklength", "user", "joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite", "kv"}, - {"intvelocity", "*", "name", "class", "group", + {"intvelocity", "*", "name", "class", "group", "nsample", "interp", "delay", "ctrllimited", "forcelimited", "ctrlrange", "forcerange", "actrange", "inheritrange", "lengthrange", "gear", "cranklength", "user", "joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite", "kp", "kv", "dampratio"}, - {"damper", "*", "name", "class", "group", + {"damper", "*", "name", "class", "group", "nsample", "interp", "delay", "forcelimited", "ctrlrange", "forcerange", "lengthrange", "gear", "cranklength", "user", "joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite", "kv"}, - {"cylinder", "*", "name", "class", "group", + {"cylinder", "*", "name", "class", "group", "nsample", "interp", "delay", "ctrllimited", "forcelimited", "ctrlrange", "forcerange", "lengthrange", "gear", "cranklength", "user", "joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite", "timeconst", "area", "diameter", "bias"}, - {"muscle", "*", "name", "class", "group", + {"muscle", "*", "name", "class", "group", "nsample", "interp", "delay", "ctrllimited", "forcelimited", "ctrlrange", "forcerange", "lengthrange", "gear", "cranklength", "user", "joint", "jointinparent", "tendon", "slidersite", "cranksite", "timeconst", "tausmooth", "range", "force", "scale", "lmin", "lmax", "vmax", "fpmax", "fvmax"}, - {"adhesion", "*", "name", "class", "group", + {"adhesion", "*", "name", "class", "group", "nsample", "interp", "delay", "forcelimited", "ctrlrange", "forcerange", "user", "body", "gain"}, - {"plugin", "*", "name", "class", "plugin", "instance", "group", + {"plugin", "*", "name", "class", "plugin", "instance", "group", "nsample", "interp", "delay", "ctrllimited", "forcelimited", "actlimited", "ctrlrange", "forcerange", "actrange", "lengthrange", "gear", "cranklength", "joint", "jointinparent", "site", "actdim", "dyntype", "dynprm", "tendon", "cranksite", "slidersite", "user", @@ -445,56 +445,56 @@ std::vector MJCF[nMJCF] = { {"sensor", "*"}, {"<"}, - {"touch", "*", "name", "site", "cutoff", "noise", "user"}, - {"accelerometer", "*", "name", "site", "cutoff", "noise", "user"}, - {"velocimeter", "*", "name", "site", "cutoff", "noise", "user"}, - {"gyro", "*", "name", "site", "cutoff", "noise", "user"}, - {"force", "*", "name", "site", "cutoff", "noise", "user"}, - {"torque", "*", "name", "site", "cutoff", "noise", "user"}, - {"magnetometer", "*", "name", "site", "cutoff", "noise", "user"}, - {"camprojection", "*", "name", "site", "camera", "cutoff", "noise", "user"}, - {"rangefinder", "*", "name", "site", "camera", "data", "cutoff", "noise", "user"}, - {"jointpos", "*", "name", "joint", "cutoff", "noise", "user"}, - {"jointvel", "*", "name", "joint", "cutoff", "noise", "user"}, - {"tendonpos", "*", "name", "tendon", "cutoff", "noise", "user"}, - {"tendonvel", "*", "name", "tendon", "cutoff", "noise", "user"}, - {"actuatorpos", "*", "name", "actuator", "cutoff", "noise", "user"}, - {"actuatorvel", "*", "name", "actuator", "cutoff", "noise", "user"}, - {"actuatorfrc", "*", "name", "actuator", "cutoff", "noise", "user"}, - {"jointactuatorfrc", "*", "name", "joint", "cutoff", "noise", "user"}, - {"tendonactuatorfrc", "*", "name", "tendon", "cutoff", "noise", "user"}, - {"ballquat", "*", "name", "joint", "cutoff", "noise", "user"}, - {"ballangvel", "*", "name", "joint", "cutoff", "noise", "user"}, - {"jointlimitpos", "*", "name", "joint", "cutoff", "noise", "user"}, - {"jointlimitvel", "*", "name", "joint", "cutoff", "noise", "user"}, - {"jointlimitfrc", "*", "name", "joint", "cutoff", "noise", "user"}, - {"tendonlimitpos", "*", "name", "tendon", "cutoff", "noise", "user"}, - {"tendonlimitvel", "*", "name", "tendon", "cutoff", "noise", "user"}, - {"tendonlimitfrc", "*", "name", "tendon", "cutoff", "noise", "user"}, - {"framepos", "*", "name", "objtype", "objname", "reftype", "refname", "cutoff", "noise", "user"}, - {"framequat", "*", "name", "objtype", "objname", "reftype", "refname", "cutoff", "noise", "user"}, - {"framexaxis", "*", "name", "objtype", "objname", "reftype", "refname", "cutoff", "noise", "user"}, - {"frameyaxis", "*", "name", "objtype", "objname", "reftype", "refname", "cutoff", "noise", "user"}, - {"framezaxis", "*", "name", "objtype", "objname", "reftype", "refname", "cutoff", "noise", "user"}, - {"framelinvel", "*", "name", "objtype", "objname", "reftype", "refname", "cutoff", "noise", "user"}, - {"frameangvel", "*", "name", "objtype", "objname", "reftype", "refname", "cutoff", "noise", "user"}, - {"framelinacc", "*", "name", "objtype", "objname", "cutoff", "noise", "user"}, - {"frameangacc", "*", "name", "objtype", "objname", "cutoff", "noise", "user"}, - {"subtreecom", "*", "name", "body", "cutoff", "noise", "user"}, - {"subtreelinvel", "*", "name", "body", "cutoff", "noise", "user"}, - {"subtreeangmom", "*", "name", "body", "cutoff", "noise", "user"}, - {"insidesite", "*", "name", "site", "objtype", "objname", "cutoff", "noise", "user"}, - {"distance", "*", "name", "geom1", "geom2", "body1", "body2", "cutoff", "noise", "user"}, - {"normal", "*", "name", "geom1", "geom2", "body1", "body2", "cutoff", "noise", "user"}, - {"fromto", "*", "name", "geom1", "geom2", "body1", "body2", "cutoff", "noise", "user"}, + {"touch", "*", "name", "site", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"}, + {"accelerometer", "*", "name", "site", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"}, + {"velocimeter", "*", "name", "site", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"}, + {"gyro", "*", "name", "site", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"}, + {"force", "*", "name", "site", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"}, + {"torque", "*", "name", "site", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"}, + {"magnetometer", "*", "name", "site", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"}, + {"camprojection", "*", "name", "site", "camera", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"}, + {"rangefinder", "*", "name", "site", "camera", "data", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"}, + {"jointpos", "*", "name", "joint", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"}, + {"jointvel", "*", "name", "joint", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"}, + {"tendonpos", "*", "name", "tendon", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"}, + {"tendonvel", "*", "name", "tendon", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"}, + {"actuatorpos", "*", "name", "actuator", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"}, + {"actuatorvel", "*", "name", "actuator", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"}, + {"actuatorfrc", "*", "name", "actuator", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"}, + {"jointactuatorfrc", "*", "name", "joint", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"}, + {"tendonactuatorfrc", "*", "name", "tendon", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"}, + {"ballquat", "*", "name", "joint", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"}, + {"ballangvel", "*", "name", "joint", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"}, + {"jointlimitpos", "*", "name", "joint", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"}, + {"jointlimitvel", "*", "name", "joint", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"}, + {"jointlimitfrc", "*", "name", "joint", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"}, + {"tendonlimitpos", "*", "name", "tendon", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"}, + {"tendonlimitvel", "*", "name", "tendon", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"}, + {"tendonlimitfrc", "*", "name", "tendon", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"}, + {"framepos", "*", "name", "objtype", "objname", "reftype", "refname", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"}, + {"framequat", "*", "name", "objtype", "objname", "reftype", "refname", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"}, + {"framexaxis", "*", "name", "objtype", "objname", "reftype", "refname", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"}, + {"frameyaxis", "*", "name", "objtype", "objname", "reftype", "refname", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"}, + {"framezaxis", "*", "name", "objtype", "objname", "reftype", "refname", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"}, + {"framelinvel", "*", "name", "objtype", "objname", "reftype", "refname", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"}, + {"frameangvel", "*", "name", "objtype", "objname", "reftype", "refname", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"}, + {"framelinacc", "*", "name", "objtype", "objname", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"}, + {"frameangacc", "*", "name", "objtype", "objname", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"}, + {"subtreecom", "*", "name", "body", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"}, + {"subtreelinvel", "*", "name", "body", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"}, + {"subtreeangmom", "*", "name", "body", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"}, + {"insidesite", "*", "name", "site", "objtype", "objname", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"}, + {"distance", "*", "name", "geom1", "geom2", "body1", "body2", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"}, + {"normal", "*", "name", "geom1", "geom2", "body1", "body2", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"}, + {"fromto", "*", "name", "geom1", "geom2", "body1", "body2", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"}, {"contact", "*", "name", "geom1", "geom2", "body1", "body2", "subtree1", "subtree2", "site", - "num", "data", "reduce", "cutoff", "noise", "user"}, - {"e_potential", "*", "name", "cutoff", "noise", "user"}, - {"e_kinetic", "*", "name", "cutoff", "noise", "user"}, - {"clock", "*", "name", "cutoff", "noise", "user"}, + "num", "data", "reduce", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"}, + {"e_potential", "*", "name", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"}, + {"e_kinetic", "*", "name", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"}, + {"clock", "*", "name", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"}, + {"tactile", "*", "name", "geom", "mesh", "nsample", "interp", "delay", "interval", "user"}, {"user", "*", "name", "objtype", "objname", "datatype", "needstage", "dim", "cutoff", "noise", "user"}, - {"tactile", "*", "name", "geom", "mesh", "user"}, {"plugin", "*", "name", "plugin", "instance", "cutoff", "objtype", "objname", "reftype", "refname", "user"}, {"<"}, @@ -751,6 +751,15 @@ const mjMap bias_map[bias_sz] = { }; +// interpolation type +const int interp_sz = 3; +const mjMap interp_map[interp_sz] = { + {"zoh", 0}, + {"linear", 1}, + {"cubic", 2} +}; + + // stage type const int stage_sz = 4; const mjMap stage_map[stage_sz] = { @@ -2285,6 +2294,9 @@ void mjXReader::OneActuator(XMLElement* elem, mjsActuator* actuator) { } } ReadAttrInt(elem, "group", &actuator->group); + ReadAttrInt(elem, "nsample", &actuator->nsample); + MapValue(elem, "interp", &actuator->interp, interp_map, interp_sz); + ReadAttr(elem, "delay", 1, &actuator->delay, text); MapValue(elem, "ctrllimited", &actuator->ctrllimited, TFAuto_map, 3); MapValue(elem, "forcelimited", &actuator->forcelimited, TFAuto_map, 3); MapValue(elem, "actlimited", &actuator->actlimited, TFAuto_map, 3); @@ -4037,6 +4049,10 @@ void mjXReader::Sensor(XMLElement* section) { } ReadAttr(elem, "cutoff", 1, &sensor->cutoff, text); ReadAttr(elem, "noise", 1, &sensor->noise, text); + ReadAttrInt(elem, "nsample", &sensor->nsample); + MapValue(elem, "interp", &sensor->interp, interp_map, interp_sz); + ReadAttr(elem, "delay", 1, &sensor->delay, text); + ReadAttr(elem, "interval", 2, sensor->interval, text, /*required=*/false, /*exact=*/false); if (ReadVector(elem, "user", userdata, text)) { mjs_setDouble(sensor->userdata, userdata.data(), userdata.size()); } diff --git a/src/xml/xml_native_writer.cc b/src/xml/xml_native_writer.cc index 07d8cbce..199f15aa 100644 --- a/src/xml/xml_native_writer.cc +++ b/src/xml/xml_native_writer.cc @@ -825,6 +825,9 @@ void mjXWriter::OneActuator(XMLElement* elem, const mjCActuator* actuator, mjCDe // defaults and regular WriteAttrInt(elem, "group", actuator->group, def->Actuator().group); + WriteAttrInt(elem, "nsample", actuator->nsample, def->Actuator().nsample); + WriteAttrKey(elem, "interp", interp_map, interp_sz, actuator->interp, def->Actuator().interp); + WriteAttr(elem, "delay", 1, &actuator->delay, &def->Actuator().delay); WriteAttrKey(elem, "ctrllimited", TFAuto_map, 3, actuator->ctrllimited, def->Actuator().ctrllimited); WriteAttr(elem, "ctrlrange", 2, actuator->ctrlrange, def->Actuator().ctrlrange); WriteAttrKey(elem, "forcelimited", TFAuto_map, 3, actuator->forcelimited, def->Actuator().forcelimited); @@ -2338,6 +2341,11 @@ void mjXWriter::Sensor(XMLElement* root) { if (sensor->type != mjSENS_PLUGIN) { WriteAttr(elem, "noise", 1, &sensor->noise, &zero); } + WriteAttrInt(elem, "nsample", sensor->nsample, 0); + WriteAttrKey(elem, "interp", interp_map, interp_sz, sensor->interp, 0); + WriteAttr(elem, "delay", 1, &sensor->delay, &zero); + double zeros[2] = {0, 0}; + WriteAttr(elem, "interval", 2, sensor->interval, zeros); WriteVector(elem, "user", sensor->get_userdata()); } diff --git a/test/engine/engine_forward_test.cc b/test/engine/engine_forward_test.cc index 63f9e0d2..c4f73c1f 100644 --- a/test/engine/engine_forward_test.cc +++ b/test/engine/engine_forward_test.cc @@ -1513,5 +1513,109 @@ TEST_F(ActuatorTest, TendonActuatorForceRange) { mj_deleteModel(model); } +// ----------------------------- actuator delays ------------------------------- + +TEST_F(ForwardTest, ActuatorDelay) { + static constexpr char xml[] = R"( + + + )"; + char error[1024]; + mjModel* model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model, NotNull()) << error; + mjData* data = mj_makeData(model); + + // delay = 0.02 seconds, timestep = 0.01, so ndelay = ceil(0.02/0.01) = 2 + EXPECT_EQ(model->actuator_history[0], 2); + + // set ctrl to a nonzero value + data->ctrl[0] = 10.0; + + // step once: the new ctrl is appended but won't be read for 2 timesteps + mj_step(model, data); + // actuator_force should still be 0 (delayed value from buffer init) + EXPECT_NEAR(data->actuator_force[0], 0.0, 1e-10); + + // step again + mj_step(model, data); + // still reading old values + EXPECT_NEAR(data->actuator_force[0], 0.0, 1e-10); + + // step a third time - now the delayed ctrl should arrive + mj_step(model, data); + // actuator_force should now be 10.0 + EXPECT_NEAR(data->actuator_force[0], 10.0, 1e-10); + + mj_deleteData(data); + mj_deleteModel(model); +} + +// Test actuator delay with linear interpolation (interp=1) +// Uses delay = 1.5*timestep so interpolation is meaningful +TEST_F(ForwardTest, ActuatorDelayLinearInterp) { + constexpr char xml[] = R"( + + + )"; + char error[1024]; + mjModel* model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model, NotNull()) << error; + mjData* data = mj_makeData(model); + + // delay = 0.015 seconds = 1.5*timestep, nsample=3, interp=1 (linear) + EXPECT_EQ(model->actuator_history[0], 3); + EXPECT_EQ(model->actuator_history[1], 1); // interp=1 (linear) + EXPECT_NEAR(model->actuator_delay[0], 0.015, 1e-10); + + // Set increasing ctrl values + // Buffer has samples at times: -0.02, -0.01, 0 with values 0, 0, 0 + // After step 0 at time=0.01: buffer has times -0.01, 0, 0.01 with values 0, 0, ctrl[0] + // Read at time 0.01 - 0.015 = -0.005: interpolate between t=-0.01 and t=0 + // Since both values are 0, expected actuator_force = 0 + + data->ctrl[0] = 10.0; + mj_step(model, data); + EXPECT_NEAR(data->actuator_force[0], 0.0, 1e-10) << "step 0"; + + // After step 1 at time=0.02: buffer has times 0, 0.01, 0.02 with values 0, 10, 20 + // Read at time 0.02 - 0.015 = 0.005: interpolate between t=0 (val=0) and t=0.01 (val=10) + // Expected: 0 * 0.5 + 10 * 0.5 = 5 + + data->ctrl[0] = 20.0; + mj_step(model, data); + EXPECT_NEAR(data->actuator_force[0], 5.0, 1e-10) << "step 1"; + + // After step 2 at time=0.03: buffer has times 0.01, 0.02, 0.03 with values 10, 20, 30 + // Read at 0.03 - 0.015 = 0.015: interpolate between t=0.01 (val=10) and t=0.02 (val=20) + // Expected: 10 * 0.5 + 20 * 0.5 = 15 + + data->ctrl[0] = 30.0; + mj_step(model, data); + EXPECT_NEAR(data->actuator_force[0], 15.0, 1e-10) << "step 2"; + + mj_deleteData(data); + mj_deleteModel(model); +} + } // namespace } // namespace mujoco diff --git a/test/engine/engine_sensor_test.cc b/test/engine/engine_sensor_test.cc index 3f061aec..498ec4a2 100644 --- a/test/engine/engine_sensor_test.cc +++ b/test/engine/engine_sensor_test.cc @@ -24,6 +24,7 @@ #include #include #include +#include "src/engine/engine_support.h" #include "src/engine/engine_util_blas.h" #include "src/engine/engine_util_spatial.h" #include "test/fixture.h" @@ -1193,5 +1194,402 @@ TEST_F(SensorTest, RFCamera) { mj_deleteModel(model); } +// ------------------------------- sensor delays ------------------------------- + +TEST_F(SensorTest, SensorDelay) { + constexpr char xml[] = R"( + + + )"; + char error[1024]; + mjModel* model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model, NotNull()) << error; + mjData* data = mj_makeData(model); + + // delay = 0.02 seconds, timestep = 0.01 + // history = 3 (more than delay/timestep=2) to ensure buffer coverage + EXPECT_EQ(model->sensor_history[0], 3); + EXPECT_NEAR(model->sensor_delay[0], 0.02, 1e-10); + + // Use different values to verify exact delay timing. + // With delay=0.02 and timestep=0.01, we expect 2-step delay: + // - At step N, sensordata should reflect qpos from step N-2. + + // step 0: qpos=10, read from initial buffer + data->qpos[0] = 10.0; + mj_step(model, data); + EXPECT_NEAR(data->sensordata[0], 0.0, 1e-10) << "step 0"; + + // step 1: qpos=20, still reading initial buffer + data->qpos[0] = 20.0; + mj_step(model, data); + EXPECT_NEAR(data->sensordata[0], 0.0, 1e-10) << "step 1"; + + // step 2: qpos=30, read value from step 0 (delay=2 steps) + data->qpos[0] = 30.0; + mj_step(model, data); + EXPECT_NEAR(data->sensordata[0], 10.0, 1e-10) << "step 2"; + + // step 3: qpos=40, read value from step 1 (delay=2 steps) + data->qpos[0] = 40.0; + mj_step(model, data); + EXPECT_NEAR(data->sensordata[0], 20.0, 1e-10) << "step 3"; + + // step 4: qpos=50, read value from step 2 (delay=2 steps) + data->qpos[0] = 50.0; + mj_step(model, data); + EXPECT_NEAR(data->sensordata[0], 30.0, 1e-10) << "step 4"; + + mj_deleteData(data); + mj_deleteModel(model); +} + +// Test sensor delay with linear interpolation (interp=1) +// Uses delay = 1.5*timestep so interpolation is meaningful +TEST_F(SensorTest, SensorDelayLinearInterp) { + constexpr char xml[] = R"( + + + )"; + char error[1024]; + mjModel* model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model, NotNull()) << error; + mjData* data = mj_makeData(model); + + // delay = 0.015 seconds = 1.5*timestep, nsample=3, interp=1 (linear) + // With linear interpolation and 1.5*timestep delay, the read time falls + // exactly between two buffer samples, so we should get the average. + EXPECT_EQ(model->sensor_history[0], 3); + EXPECT_EQ(model->sensor_history[1], 1); // interp=1 (linear) + EXPECT_NEAR(model->sensor_delay[0], 0.015, 1e-10); + + // Set increasing qpos values: step i -> qpos = (i+1)*10 + // Buffer has samples at times: -0.02, -0.01, 0 (initialized) + // After step 0 at time=0.01: buffer has times -0.01, 0, 0.01 with values 0, 0, 10 + // Read at time 0.01 - 0.015 = -0.005: interpolate between t=-0.01 (val=0) and t=0 (val=0) + // Expected: 0 * 0.5 + 0 * 0.5 = 0 + + data->qpos[0] = 10.0; + mj_step(model, data); + EXPECT_NEAR(data->sensordata[0], 0.0, 1e-10) << "step 0"; + + // After step 1 at time=0.02: buffer has times 0, 0.01, 0.02 with values 0, 10, 20 + // Read at time 0.02 - 0.015 = 0.005: interpolate between t=0 (val=0) and t=0.01 (val=10) + // Expected: 0 * 0.5 + 10 * 0.5 = 5 + + data->qpos[0] = 20.0; + mj_step(model, data); + EXPECT_NEAR(data->sensordata[0], 5.0, 1e-10) << "step 1"; + + // After step 2 at time=0.03: buffer has times 0.01, 0.02, 0.03 with values 10, 20, 30 + // Read at 0.03 - 0.015 = 0.015: interpolate between t=0.01 (val=10) and t=0.02 (val=20) + // Expected: 10 * 0.5 + 20 * 0.5 = 15 + + data->qpos[0] = 30.0; + mj_step(model, data); + EXPECT_NEAR(data->sensordata[0], 15.0, 1e-10) << "step 2"; + + mj_deleteData(data); + mj_deleteModel(model); +} + +TEST_F(SensorTest, SensorInterval) { + // This test uses the exact values from the documentation for interval: + // timestep=1, interval=2.5, producing times 0, 3, 5, 8, 10, 13, ... + // with interval="2.5 -1.5", producing times 1, 4, 6, 9, 11, 14, ... + constexpr char xml[] = R"( + + + )"; + char error[1024]; + mjModel* model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model, NotNull()) << error; + mjData* data = mj_makeData(model); + + int sensor0 = mj_name2id(model, mjOBJ_SENSOR, "default_phase"); + int sensor1 = mj_name2id(model, mjOBJ_SENSOR, "offset_phase"); + int adr0 = model->sensor_adr[sensor0]; + int adr1 = model->sensor_adr[sensor1]; + + // Verify initial buffer timestamps (after mj_makeData/mj_resetData) + // With period=2.5, dt=1.0, nsample=10: + // sensor0 (phase = -period = -2.5): continuous times are -2.5, -5, -7.5, ... + // rounded up to dt: -2, -5, -7, -10, -12, -15, -17, -20, -22, -25 + // sensor1 (phase = -1.5): continuous times are -1.5, -4, -6.5, ... + // rounded up to dt: -1, -4, -6, -9, -11, -14, -16, -19, -21, -24 + int n0 = model->sensor_history[2*sensor0]; + int n1 = model->sensor_history[2*sensor1]; + mjtNum* buf0 = data->history + model->sensor_historyadr[sensor0]; + mjtNum* buf1 = data->history + model->sensor_historyadr[sensor1]; + mjtNum* times0 = buf0 + 2; + mjtNum* times1 = buf1 + 2; + mjtNum expected_times0[] = {-25, -22, -20, -17, -15, -12, -10, -7, -5, -2}; + mjtNum expected_times1[] = {-24, -21, -19, -16, -14, -11, -9, -6, -4, -1}; + for (int i = 0; i < n0; i++) { + EXPECT_NEAR(times0[i], expected_times0[i], 1e-10); + } + for (int i = 0; i < n1; i++) { + EXPECT_NEAR(times1[i], expected_times1[i], 1e-10); + } + + // sensor0: interval="2.5 0" -> time_prev starts at -2.5 + // triggers at: 0, 3, 5, 8, 10, 13, ... (gaps: 3,2,3,2,3,...) + // sensor1: interval="2.5 -1.5" -> time_prev starts at -1.5 + // triggers at: 1, 4, 6, 9, 11, 14, ... (gaps: 3,2,3,2,3,...) + + // Arrays tracking when each sensor triggers (1=triggers, 0=holds) + // Times: 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 + int triggers0[] = {1, 0, 0, 1, 0, 1, 0, 0, 1, 0, 1, 0, 0, 1, 0}; + int triggers1[] = {0, 1, 0, 0, 1, 0, 1, 0, 0, 1, 0, 1, 0, 0, 1}; + + mjtNum value0 = 0, value1 = 0; + for (int t = 0; t < 15; t++) { + // set position to current time (so we can track when sensor was computed) + data->qpos[0] = t; + mj_step(model, data); + + // update expected values based on trigger pattern + if (triggers0[t]) value0 = t; + if (triggers1[t]) value1 = t; + + EXPECT_NEAR(data->sensordata[adr0], value0, 1e-10) + << "sensor0 at t=" << t; + EXPECT_NEAR(data->sensordata[adr1], value1, 1e-10) + << "sensor1 at t=" << t; + } + + mj_deleteData(data); + mj_deleteModel(model); +} + +TEST_F(SensorTest, SensorDelayInterval) { + constexpr char xml[] = R"( + + + )"; + char error[1024]; + mjModel* model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model, NotNull()) << error; + mjData* data = mj_makeData(model); + + // Combined delay and interval + EXPECT_EQ(model->sensor_history[0], 5); + EXPECT_NEAR(model->sensor_delay[0], 0.02, 1e-10); + EXPECT_NEAR(model->sensor_interval[2*0], 0.03, 1e-10); + + // Verify initial buffer timestamps (after mj_makeData/mj_resetData) + // With period=0.03, dt=0.01, nsample=5, phase=0 (means -period=-0.03): + // continuous times: -0.03, -0.06, -0.09, -0.12, -0.15 + // rounded up to dt: -0.03, -0.06, -0.09, -0.12, -0.15 (multiples of dt) + int n = model->sensor_history[0]; + mjtNum* buf = data->history + model->sensor_historyadr[0]; + mjtNum* times = buf + 2; + mjtNum expected_times[] = {-0.15, -0.12, -0.09, -0.06, -0.03}; + for (int i = 0; i < n; i++) { + EXPECT_NEAR(times[i], expected_times[i], 1e-10); + } + + // set position + data->qpos[0] = 5.0; + + // initial steps: reading from buffer (initially 0) + // With delay=0.02, interval=0.03: + // - At t=0, interval satisfied: compute 5.0, insert at t=0 (current time) + // - Reading happens at d->time - delay; at t=0.02, reads at t=0.00 (5.0) + for (int i = 0; i < 2; i++) { + mj_step(model, data); + // sensor reads delayed value (0.0 from initial buffer) + EXPECT_NEAR(data->sensordata[0], 0.0, 1e-10) << "step " << i; + } + + // step 3 (i=2): reading at t=0.00 now returns the inserted value 5.0 + mj_step(model, data); + EXPECT_NEAR(data->sensordata[0], 5.0, 1e-10); + + mj_deleteData(data); + mj_deleteModel(model); +} + +TEST_F(SensorTest, SensorHistoryOnly) { + constexpr char xml[] = R"( + + + )"; + char error[1024]; + mjModel* model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model, NotNull()) << error; + mjData* data = mj_makeData(model); + + // history only, no delay or interval + EXPECT_EQ(model->sensor_history[0], 5); + EXPECT_NEAR(model->sensor_delay[0], 0.0, 1e-10); + EXPECT_NEAR(model->sensor_interval[0], 0.0, 1e-10); + + // set position + data->qpos[0] = 3.0; + + // without delay, sensordata reflects current value immediately + mj_step(model, data); + EXPECT_NEAR(data->sensordata[0], 3.0, 1e-10); + + // change position, check again + data->qpos[0] = 7.0; + mj_step(model, data); + EXPECT_NEAR(data->sensordata[0], 7.0, 1e-10); + + mj_deleteData(data); + mj_deleteModel(model); +} + +TEST_F(SensorTest, SensorDelayMultiDim) { + constexpr char xml[] = R"( + + + )"; + char error[1024]; + mjModel* model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model, NotNull()) << error; + mjData* data = mj_makeData(model); + + // ballangvel is 3D + EXPECT_EQ(model->sensor_dim[0], 3); + EXPECT_EQ(model->sensor_history[0], 2); + + // set angular velocity + data->qvel[0] = 1.0; + data->qvel[1] = 2.0; + data->qvel[2] = 3.0; + + // step: reading delayed value (initially 0) + mj_step(model, data); + EXPECT_NEAR(data->sensordata[0], 0.0, 1e-10); + EXPECT_NEAR(data->sensordata[1], 0.0, 1e-10); + EXPECT_NEAR(data->sensordata[2], 0.0, 1e-10); + + // after delay, values should propagate + mj_step(model, data); + mj_step(model, data); + mj_step(model, data); + // angular velocity is affected by dynamics, just check the buffer works + EXPECT_THAT(AsVector(data->sensordata, 3), Not(ElementsAre(0.0, 0.0, 0.0))); + + mj_deleteData(data); + mj_deleteModel(model); +} + +TEST_F(SensorTest, ReadSensor) { + constexpr char xml[] = R"( + + + )"; + char error[1024]; + mjModel* model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model, NotNull()) << error; + mjData* data = mj_makeData(model); + + // step with different qpos values to populate buffer + // mj_advance inserts at current time, then time advances + data->qpos[0] = 1.0; + mj_step(model, data); // inserts 1.0 at t=0, time -> 0.01 + + data->qpos[0] = 2.0; + mj_step(model, data); // inserts 2.0 at t=0.01, time -> 0.02 + + data->qpos[0] = 3.0; + mj_step(model, data); // inserts 3.0 at t=0.02, time -> 0.03 + + // now time=0.03, buffer has: [t=0: 1.0, t=0.01: 2.0, t=0.02: 3.0] + + // read at different times from history + mjtNum result[1]; + const mjtNum* ptr; + + // read at t=0 -> returns 1.0 + ptr = mj_readSensor(model, data, 0, 0.0, result, /*order=*/0); + EXPECT_NEAR(*ptr, 1.0, 1e-10); + + // read at t=0.01 -> returns 2.0 (ZOH: exactly at insertion time) + ptr = mj_readSensor(model, data, 0, 0.01, result, /*order=*/0); + EXPECT_NEAR(*ptr, 2.0, 1e-10); + + // read at t=0.02 -> returns 3.0 + ptr = mj_readSensor(model, data, 0, 0.02, result, /*order=*/0); + EXPECT_NEAR(*ptr, 3.0, 1e-10); + + mj_deleteData(data); + mj_deleteModel(model); +} + } // namespace } // namespace mujoco diff --git a/test/engine/engine_support_test.cc b/test/engine/engine_support_test.cc index 891a6f31..5c196239 100644 --- a/test/engine/engine_support_test.cc +++ b/test/engine/engine_support_test.cc @@ -692,7 +692,9 @@ TEST_F(SupportTest, DifferentiatePosSubQuat) { static const char* const kDefaultModel = "testdata/model.xml"; -TEST_F(SupportTest, GetSetStateStepEqual) { +using StateTest = MujocoTest; + +TEST_F(StateTest, GetSetStateStepEqual) { const std::string xml_path = GetTestDataFilePath(kDefaultModel); mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0); mjData* data = mj_makeData(model); @@ -745,7 +747,58 @@ TEST_F(SupportTest, GetSetStateStepEqual) { mj_deleteModel(model); } -TEST_F(SupportTest, CopyState) { +TEST_F(StateTest, GetSetStateDelay) { + static constexpr char xml[] = R"( + + + )"; + + char error[1024]; + mjModel* model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model, NotNull()) << error; + mjData* data = mj_makeData(model); + + // verify history buffer exists: nhistory = 2 + 2*5 = 12 + EXPECT_EQ(model->nhistory, 12); // [user, cursor, times(5), values(5)] + + // state size should include history buffer + int size = mj_stateSize(model, mjSTATE_HISTORY); + EXPECT_EQ(size, model->nhistory); + + // step to populate history buffer + data->ctrl[0] = 1.0; + mj_step(model, data); + data->ctrl[0] = 2.0; + mj_step(model, data); + + // get history state + vector history_state(size); + mj_getState(model, data, history_state.data(), mjSTATE_HISTORY); + + // modify the history buffer manually (value at index 7 = 2+5 = after times) + data->history[7] = 99.0; // first value + + // set history state back - should restore original + mj_setState(model, data, history_state.data(), mjSTATE_HISTORY); + + // verify restoration + EXPECT_NE(data->history[7], 99.0); + + mj_deleteData(data); + mj_deleteModel(model); +} + +TEST_F(StateTest, CopyState) { const std::string xml_path = GetTestDataFilePath(kDefaultModel); mjModel* m = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0); @@ -762,6 +815,7 @@ TEST_F(SupportTest, CopyState) { for (int i=0; i < m->nv; ++i) src->qvel[i] = i*0.2; for (int i=0; i < m->na; ++i) src->act[i] = i*0.3; for (int i=0; i < m->nu; ++i) src->ctrl[i] = i*0.4; + for (int i=0; i < m->nhistory; ++i) src->history[i] = i*0.5; for (int i=0; i < m->neq; ++i) src->eq_active[i] = 1 - m->eq_active0[i]; @@ -779,6 +833,8 @@ TEST_F(SupportTest, CopyState) { EXPECT_EQ(AsVector(dst->qpos, m->nq), AsVector(src->qpos, m->nq)); EXPECT_EQ(AsVector(dst->qvel, m->nv), AsVector(src->qvel, m->nv)); EXPECT_EQ(AsVector(dst->act, m->na), AsVector(src->act, m->na)); + EXPECT_EQ(AsVector(dst->history, m->nhistory), + AsVector(src->history, m->nhistory)); EXPECT_EQ(AsVector(dst->eq_active, m->neq), AsVector(src->eq_active, m->neq)); // check non-copied components (CTRL not in signature) @@ -790,7 +846,7 @@ TEST_F(SupportTest, CopyState) { mj_deleteModel(m); } -TEST_F(SupportTest, ExtractState) { +TEST_F(StateTest, ExtractState) { const std::string xml_path = GetTestDataFilePath(kDefaultModel); mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0); mjData* data = mj_makeData(model); @@ -809,7 +865,8 @@ TEST_F(SupportTest, ExtractState) { mj_step(model, data); // take a state that will be used as src - int srcsig = mjSTATE_TIME | mjSTATE_QPOS | mjSTATE_QVEL | mjSTATE_CTRL; + int srcsig = mjSTATE_TIME | mjSTATE_QPOS | mjSTATE_QVEL | mjSTATE_CTRL | + mjSTATE_HISTORY; int srcsize = mj_stateSize(model, srcsig); vector srcstate(srcsize); mj_getState(model, data, srcstate.data(), srcsig); @@ -835,6 +892,14 @@ TEST_F(SupportTest, ExtractState) { EXPECT_EQ(AsVector(dststate2.data() + model->nq, model->nu), AsVector(data->ctrl, model->nu)); + // extract history state + int dstsig3 = mjSTATE_HISTORY; + int dstsize3 = mj_stateSize(model, dstsig3); + EXPECT_EQ(dstsize3, model->nhistory); + vector dststate3(dstsize3); + mj_extractState(model, srcstate.data(), srcsig, dststate3.data(), dstsig3); + EXPECT_EQ(dststate3, AsVector(data->history, model->nhistory)); + // test that an error is correctly raised if dstsig is not a subset of srcsig static int error_count; static char last_error_msg[128]; @@ -1202,5 +1267,205 @@ TEST_F(SupportTest, ContactSensorDim) { EXPECT_EQ(mju_condataSize(dataSpec), 1+3+1+3+3); } +// ------------------------------ ctrl delays -------------------------------- + +TEST_F(SupportTest, ReadCtrlNoDelay) { + static constexpr char xml[] = R"( + + + + + + + + + + + + )"; + mjModel* model = LoadModelFromString(xml); + ASSERT_THAT(model, NotNull()); + mjData* data = mj_makeData(model); + + // no delay: should return current ctrl value + data->ctrl[0] = 42.0; + EXPECT_EQ(mj_readCtrl(model, data, 0, data->time, /*order=*/0), 42.0); + + mj_deleteData(data); + mj_deleteModel(model); +} + +TEST_F(SupportTest, ReadCtrlWithDelay) { + static constexpr char xml[] = R"( + + + )"; + char error[1024]; + mjModel* model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model, NotNull()) << error; + mjData* data = mj_makeData(model); + + // model should have delay configured + // delay = 0.03 seconds, timestep = 0.01, so ndelay = ceil(0.03/0.01) = 3 + EXPECT_EQ(model->actuator_history[0], 3); + EXPECT_NEAR(model->actuator_delay[0], 0.03, 1e-10); + EXPECT_GE(model->actuator_historyadr[0], 0); + + // initially, buffer should be filled with constant value (from init) + // reading at current time should return the init value + mjtNum val = mj_readCtrl(model, data, 0, data->time, /*order=*/0); + EXPECT_EQ(val, data->ctrl[0]); + + mj_deleteData(data); + mj_deleteModel(model); +} + +TEST_F(SupportTest, InitCtrlDelay) { + static constexpr char xml[] = R"( + + + )"; + char error[1024]; + mjModel* model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model, NotNull()) << error; + mjData* data = mj_makeData(model); + + // verify nhistory + EXPECT_EQ(model->actuator_history[0], 3); + + // initialize with custom times and values + // buffer stores: time 0.0 -> value 1.0, time 0.01 -> value 2.0, time 0.02 -> value 3.0 + mjtNum times[3] = {0.0, 0.01, 0.02}; + mjtNum values[3] = {1.0, 2.0, 3.0}; + mj_initCtrlHistory(model, data, 0, times, values); + + // mj_readCtrl now auto-subtracts delay: lookup_time = time - delay + // delay = 0.02, so: + // time=0.04 -> lookup at 0.02 -> value 3.0 + // time=0.03 -> lookup at 0.01 -> value 2.0 + // time=0.02 -> lookup at 0.00 -> value 1.0 + mjtNum val = mj_readCtrl(model, data, 0, 0.04, /*order=*/0); + EXPECT_EQ(val, 3.0); + + val = mj_readCtrl(model, data, 0, 0.03, /*order=*/0); + EXPECT_EQ(val, 2.0); + + val = mj_readCtrl(model, data, 0, 0.02, /*order=*/0); + EXPECT_EQ(val, 1.0); + + mj_deleteData(data); + mj_deleteModel(model); +} + +TEST_F(SupportTest, InitCtrlDelayNullTimes) { + static constexpr char xml[] = R"( + + + )"; + char error[1024]; + mjModel* model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model, NotNull()) << error; + mjData* data = mj_makeData(model); + + // get existing times from buffer + int adr = model->actuator_historyadr[0]; + mjtNum* buf = data->history + adr; + mjtNum existing_times[3] = {buf[2], buf[3], buf[4]}; + + // initialize with NULL times (use existing) and new values + mjtNum values[3] = {10.0, 20.0, 30.0}; + mj_initCtrlHistory(model, data, 0, nullptr, values); + + // verify times are unchanged + EXPECT_EQ(buf[2], existing_times[0]); + EXPECT_EQ(buf[3], existing_times[1]); + EXPECT_EQ(buf[4], existing_times[2]); + + // verify values are updated + EXPECT_EQ(buf[5], 10.0); + EXPECT_EQ(buf[6], 20.0); + EXPECT_EQ(buf[7], 30.0); + + mj_deleteData(data); + mj_deleteModel(model); +} + +TEST_F(SupportTest, InitSensorDelay) { + static constexpr char xml[] = R"( + + + )"; + char error[1024]; + mjModel* model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model, NotNull()) << error; + mjData* data = mj_makeData(model); + + // verify nsample for sensor + EXPECT_EQ(model->sensor_history[0], 3); + + // initialize with custom times and values, phase=0 + // buffer stores: time 0.0 -> value 0.5, time 0.01 -> value 0.6, time 0.02 -> value 0.7 + mjtNum times[3] = {0.0, 0.01, 0.02}; + mjtNum values[3] = {0.5, 0.6, 0.7}; + mj_initSensorHistory(model, data, 0, times, values, /*phase=*/0.0); + + // mj_readSensor now auto-subtracts delay: lookup_time = time - delay + // delay = 0.02, so: + // time=0.04 -> lookup at 0.02 -> value 0.7 + // time=0.03 -> lookup at 0.01 -> value 0.6 + mjtNum result = 0; + const mjtNum* ptr = mj_readSensor(model, data, 0, 0.04, &result, /*order=*/0); + mjtNum val = ptr ? *ptr : result; + EXPECT_EQ(val, 0.7); + + ptr = mj_readSensor(model, data, 0, 0.03, &result, /*order=*/0); + val = ptr ? *ptr : result; + EXPECT_EQ(val, 0.6); + + mj_deleteData(data); + mj_deleteModel(model); +} + } // namespace } // namespace mujoco + diff --git a/test/testdata/delay.xml b/test/testdata/delay.xml new file mode 100644 index 00000000..b4166ca4 --- /dev/null +++ b/test/testdata/delay.xml @@ -0,0 +1,20 @@ + + diff --git a/test/user/user_model_test.cc b/test/user/user_model_test.cc index 7deb8894..57c2ee94 100644 --- a/test/user/user_model_test.cc +++ b/test/user/user_model_test.cc @@ -973,6 +973,48 @@ TEST_F(MujocoTest, ConvertSpringdamper) { EXPECT_THAT(str.data(), HasSubstr("damping")); EXPECT_THAT(str.data(), HasSubstr("stiffness")); mj_deleteModel(model); + mj_deleteSpec(spec); +} + +// ------------- test history buffer computation ------------------------------- + +using DelayBufferTest = MujocoTest; + +TEST_F(DelayBufferTest, ActuatorDelayBufferSizes) { + static constexpr char xml[] = R"( + + + )"; + mjModel* m = LoadModelFromString(xml); + ASSERT_THAT(m, NotNull()); + ASSERT_EQ(m->nu, 3); + + // nhistory = (2+2*3) + (2+2*10) = 8 + 22 = 30 + EXPECT_EQ(m->nhistory, 30); + + // verify per-actuator delay and addresses + EXPECT_EQ(m->actuator_history[0], 0); + EXPECT_EQ(m->actuator_history[2], 3); + EXPECT_EQ(m->actuator_history[4], 10); + EXPECT_EQ(m->actuator_historyadr[0], -1); + EXPECT_EQ(m->actuator_historyadr[1], 0); + EXPECT_EQ(m->actuator_historyadr[2], 8); + + mj_deleteModel(m); } } // namespace diff --git a/test/user/user_objects_test.cc b/test/user/user_objects_test.cc index 57a66d91..41528d23 100644 --- a/test/user/user_objects_test.cc +++ b/test/user/user_objects_test.cc @@ -626,6 +626,66 @@ TEST_F(SensorTest, OjbtypeParsedButNotRequired) { mj_deleteModel(model); } +TEST_F(SensorTest, NegativeIntervalError) { + static constexpr char xml[] = R"( + + + + + + + + + + + + )"; + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(model, IsNull()); + EXPECT_THAT(error.data(), HasSubstr("negative interval")); +} + +TEST_F(SensorTest, PositivePhaseError) { + static constexpr char xml[] = R"( + + + + + + + + + + + + )"; + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(model, IsNull()); + EXPECT_THAT(error.data(), HasSubstr("positive phase")); +} + +TEST_F(SensorTest, DelayWithoutHistoryError) { + static constexpr char xml[] = R"( + + + + + + + + + + + + )"; + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(model, IsNull()); + EXPECT_THAT(error.data(), HasSubstr("delay > 0 without a history buffer")); +} + // ------------- test capsule inertias ----------------------------------------- static const char* const kCapsuleInertiaPath = diff --git a/test/xml/xml_native_reader_test.cc b/test/xml/xml_native_reader_test.cc index 3df1656d..5ef94983 100644 --- a/test/xml/xml_native_reader_test.cc +++ b/test/xml/xml_native_reader_test.cc @@ -3267,5 +3267,95 @@ TEST_F(XMLReaderTest, CameraOutputDefault) { mj_deleteModel(model); } +// ------------- test delay attribute parsing ---------------------------------- + +TEST_F(ActuatorParseTest, ActuatorDelayParsed) { + static constexpr char xml[] = R"( + + + )"; + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, NotNull()) << error.data(); + ASSERT_EQ(model->nu, 3); + // actuator_history[2*i] = nsample, actuator_history[2*i+1] = interp + EXPECT_EQ(model->actuator_history[0], 0); // jnt1 nsample + EXPECT_EQ(model->actuator_history[1], 0); // jnt1 interp (no buffer, default 0) + EXPECT_EQ(model->actuator_history[2], 3); // jnt2 nsample + EXPECT_EQ(model->actuator_history[3], 0); // jnt2 interp (ZOH) + EXPECT_EQ(model->actuator_history[4], 10); // jnt3 nsample + EXPECT_EQ(model->actuator_history[5], 2); // jnt3 interp (cubic) + EXPECT_EQ(model->actuator_historyadr[0], -1); + EXPECT_EQ(model->actuator_historyadr[1], 0); + EXPECT_EQ(model->actuator_historyadr[2], 8); // 2+2*3 = 8 + mj_deleteModel(model); +} + +TEST_F(ActuatorParseTest, ActuatorDelayDefault) { + static constexpr char xml[] = R"( + + + )"; + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, NotNull()) << error.data(); + ASSERT_EQ(model->nu, 2); + EXPECT_EQ(model->actuator_history[0], 5); + EXPECT_EQ(model->actuator_history[2], 0); + EXPECT_EQ(model->actuator_historyadr[0], 0); + EXPECT_EQ(model->actuator_historyadr[1], -1); + mj_deleteModel(model); +} + +TEST_F(ActuatorParseTest, ActuatorDelayRequiresHistory) { + static constexpr char xml[] = R"( + + + + + + + + + + + + )"; + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, IsNull()); + EXPECT_THAT(error.data(), + HasSubstr("setting delay > 0 without a history buffer")); +} + } // namespace } // namespace mujoco diff --git a/test/xml/xml_native_writer_test.cc b/test/xml/xml_native_writer_test.cc index d5181652..71c250e6 100644 --- a/test/xml/xml_native_writer_test.cc +++ b/test/xml/xml_native_writer_test.cc @@ -1720,5 +1720,76 @@ TEST_F(XMLWriterTest, WritesCameraOutputDefault) { mj_deleteModel(model); } +TEST_F(XMLWriterTest, WritesSensorDelayIntervalHistory) { + static constexpr char xml[] = R"( + + + + + + + + + + + + )"; + mjModel* model = LoadModelFromString(xml); + ASSERT_THAT(model, NotNull()); + std::string saved_xml = SaveAndReadXml(model); + EXPECT_THAT(saved_xml, HasSubstr("delay=\"0.05\"")); + EXPECT_THAT(saved_xml, HasSubstr("interval=\"0.1 -0.02\"")); + EXPECT_THAT(saved_xml, HasSubstr("nsample=\"20\"")); + EXPECT_THAT(saved_xml, HasSubstr("interp=\"linear\"")); + mj_deleteModel(model); +} + +TEST_F(XMLWriterTest, DoesNotWriteDefaultSensorAttributes) { + static constexpr char xml[] = R"( + + + + + + + + + + + + )"; + mjModel* model = LoadModelFromString(xml); + ASSERT_THAT(model, NotNull()); + std::string saved_xml = SaveAndReadXml(model); + EXPECT_THAT(saved_xml, Not(HasSubstr("delay="))); + EXPECT_THAT(saved_xml, Not(HasSubstr("interval="))); + EXPECT_THAT(saved_xml, Not(HasSubstr("nsample="))); + EXPECT_THAT(saved_xml, Not(HasSubstr("interp="))); + mj_deleteModel(model); +} + +TEST_F(XMLWriterTest, WritesActuatorDelayHistory) { + static constexpr char xml[] = R"( + + + + + + + + + + + + )"; + mjModel* model = LoadModelFromString(xml); + ASSERT_THAT(model, NotNull()); + std::string saved_xml = SaveAndReadXml(model); + EXPECT_THAT(saved_xml, HasSubstr("delay=\"0.03\"")); + EXPECT_THAT(saved_xml, HasSubstr("nsample=\"15\"")); + EXPECT_THAT(saved_xml, HasSubstr("interp=\"cubic\"")); + mj_deleteModel(model); +} + } // namespace } // namespace mujoco diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index 732a7d31..119a2310 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -4951,6 +4951,7 @@ public unsafe struct mjData_ { public double* qpos; public double* qvel; public double* act; + public double* history; public double* qacc_warmstart; public double* plugin_state; public double* ctrl; @@ -5377,6 +5378,7 @@ public unsafe struct mjModel_ { public Int64 nuserdata; public Int64 nsensordata; public Int64 npluginstate; + public Int64 nhistory; public Int64 narena; public Int64 nbuffer; public mjOption_ opt; @@ -5739,6 +5741,9 @@ public unsafe struct mjModel_ { public int* actuator_actadr; public int* actuator_actnum; public int* actuator_group; + public int* actuator_history; + public int* actuator_historyadr; + public double* actuator_delay; public byte* actuator_ctrllimited; public byte* actuator_forcelimited; public byte* actuator_actlimited; @@ -5768,6 +5773,10 @@ public unsafe struct mjModel_ { public int* sensor_adr; public double* sensor_cutoff; public double* sensor_noise; + public int* sensor_history; + public int* sensor_historyadr; + public double* sensor_delay; + public double* sensor_interval; public double* sensor_user; public int* sensor_plugin; public int* plugin; @@ -6709,6 +6718,18 @@ public static unsafe extern void mj_setState(mjModel_* m, mjData_* d, double* st [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] public static unsafe extern void mj_copyState(mjModel_* m, mjData_* src, mjData_* dst, int sig); +[DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] +public static unsafe extern double mj_readCtrl(mjModel_* m, mjData_* d, int id, double time, int interp); + +[DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] +public static unsafe extern double* mj_readSensor(mjModel_* m, mjData_* d, int id, double time, double* result, int interp); + +[DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] +public static unsafe extern void mj_initCtrlHistory(mjModel_* m, mjData_* d, int id, double* times, double* values); + +[DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] +public static unsafe extern void mj_initSensorHistory(mjModel_* m, mjData_* d, int id, double* times, double* values, double phase); + [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] public static unsafe extern void mj_setKeyframe(mjModel_* m, mjData_* d, int k); diff --git a/wasm/codegen/generated/bindings.cc b/wasm/codegen/generated/bindings.cc index 1c4a7896..48ff8013 100644 --- a/wasm/codegen/generated/bindings.cc +++ b/wasm/codegen/generated/bindings.cc @@ -4120,6 +4120,12 @@ struct MjModel { void set_npluginstate(int value) { ptr_->npluginstate = static_cast(value); } + int nhistory() const { + return static_cast(ptr_->nhistory); + } + void set_nhistory(int value) { + ptr_->nhistory = static_cast(value); + } int narena() const { return static_cast(ptr_->narena); } @@ -5203,6 +5209,15 @@ struct MjModel { emscripten::val actuator_group() const { return emscripten::val(emscripten::typed_memory_view(ptr_->nu, ptr_->actuator_group)); } + emscripten::val actuator_history() const { + return emscripten::val(emscripten::typed_memory_view(ptr_->nu * 2, ptr_->actuator_history)); + } + emscripten::val actuator_historyadr() const { + return emscripten::val(emscripten::typed_memory_view(ptr_->nu, ptr_->actuator_historyadr)); + } + emscripten::val actuator_delay() const { + return emscripten::val(emscripten::typed_memory_view(ptr_->nu, ptr_->actuator_delay)); + } emscripten::val actuator_ctrllimited() const { return emscripten::val(emscripten::typed_memory_view(ptr_->nu, ptr_->actuator_ctrllimited)); } @@ -5290,6 +5305,18 @@ struct MjModel { emscripten::val sensor_noise() const { return emscripten::val(emscripten::typed_memory_view(ptr_->nsensor, ptr_->sensor_noise)); } + emscripten::val sensor_history() const { + return emscripten::val(emscripten::typed_memory_view(ptr_->nsensor * 2, ptr_->sensor_history)); + } + emscripten::val sensor_historyadr() const { + return emscripten::val(emscripten::typed_memory_view(ptr_->nsensor, ptr_->sensor_historyadr)); + } + emscripten::val sensor_delay() const { + return emscripten::val(emscripten::typed_memory_view(ptr_->nsensor, ptr_->sensor_delay)); + } + emscripten::val sensor_interval() const { + return emscripten::val(emscripten::typed_memory_view(ptr_->nsensor * 2, ptr_->sensor_interval)); + } emscripten::val sensor_user() const { return emscripten::val(emscripten::typed_memory_view(ptr_->nsensor * ptr_->nuser_sensor, ptr_->sensor_user)); } @@ -5793,6 +5820,24 @@ struct MjsActuator { void set_group(int value) { ptr_->group = value; } + int nsample() const { + return ptr_->nsample; + } + void set_nsample(int value) { + ptr_->nsample = value; + } + int interp() const { + return ptr_->interp; + } + void set_interp(int value) { + ptr_->interp = value; + } + double delay() const { + return ptr_->delay; + } + void set_delay(double value) { + ptr_->delay = value; + } mjDoubleVec &userdata() const { return *(ptr_->userdata); } @@ -6222,6 +6267,27 @@ struct MjsSensor { void set_noise(double value) { ptr_->noise = value; } + int nsample() const { + return ptr_->nsample; + } + void set_nsample(int value) { + ptr_->nsample = value; + } + int interp() const { + return ptr_->interp; + } + void set_interp(int value) { + ptr_->interp = value; + } + double delay() const { + return ptr_->delay; + } + void set_delay(double value) { + ptr_->delay = value; + } + emscripten::val interval() const { + return emscripten::val(emscripten::typed_memory_view(2, ptr_->interval)); + } mjDoubleVec &userdata() const { return *(ptr_->userdata); } @@ -6448,6 +6514,9 @@ struct MjData { emscripten::val act() const { return emscripten::val(emscripten::typed_memory_view(model->na, ptr_->act)); } + emscripten::val history() const { + return emscripten::val(emscripten::typed_memory_view(model->nhistory, ptr_->history)); + } emscripten::val qacc_warmstart() const { return emscripten::val(emscripten::typed_memory_view(model->nv, ptr_->qacc_warmstart)); } @@ -8539,6 +8608,18 @@ void mj_implicit_wrapper(const MjModel& m, MjData& d) { mj_implicit(m.get(), d.get()); } +void mj_initCtrlHistory_wrapper(const MjModel& m, MjData& d, int id, const NumberArray& times, const NumberArray& values) { + UNPACK_NULLABLE_ARRAY(mjtNum, times); + UNPACK_ARRAY(mjtNum, values); + mj_initCtrlHistory(m.get(), d.get(), id, times_.data(), values_.data()); +} + +void mj_initSensorHistory_wrapper(const MjModel& m, MjData& d, int id, const NumberArray& times, const NumberArray& values, mjtNum phase) { + UNPACK_NULLABLE_ARRAY(mjtNum, times); + UNPACK_ARRAY(mjtNum, values); + mj_initSensorHistory(m.get(), d.get(), id, times_.data(), values_.data(), phase); +} + void mj_integratePos_wrapper(const MjModel& m, const val& qpos, const NumberArray& qvel, mjtNum dt) { UNPACK_VALUE(mjtNum, qpos); UNPACK_ARRAY(mjtNum, qvel); @@ -8811,6 +8892,10 @@ mjtNum mj_rayMesh_wrapper(const MjModel& m, const MjData& d, int geomid, const N return mj_rayMesh(m.get(), d.get(), geomid, pnt_.data(), vec_.data(), normal_.data()); } +mjtNum mj_readCtrl_wrapper(const MjModel& m, const MjData& d, int id, mjtNum time, int interp) { + return mj_readCtrl(m.get(), d.get(), id, time, interp); +} + void mj_referenceConstraint_wrapper(const MjModel& m, MjData& d) { mj_referenceConstraint(m.get(), d.get()); } @@ -11103,6 +11188,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) { .value("mjSTATE_QPOS", mjSTATE_QPOS) .value("mjSTATE_QVEL", mjSTATE_QVEL) .value("mjSTATE_ACT", mjSTATE_ACT) + .value("mjSTATE_HISTORY", mjSTATE_HISTORY) .value("mjSTATE_WARMSTART", mjSTATE_WARMSTART) .value("mjSTATE_CTRL", mjSTATE_CTRL) .value("mjSTATE_QFRC_APPLIED", mjSTATE_QFRC_APPLIED) @@ -11341,6 +11427,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) { .property("flg_subtreevel", &MjData::flg_subtreevel, &MjData::set_flg_subtreevel, reference()) .property("geom_xmat", &MjData::geom_xmat) .property("geom_xpos", &MjData::geom_xpos) + .property("history", &MjData::history) .property("iLD", &MjData::iLD) .property("iLDiagInv", &MjData::iLDiagInv) .property("iM", &MjData::iM) @@ -11517,6 +11604,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) { .property("actuator_cranklength", &MjModel::actuator_cranklength) .property("actuator_ctrllimited", &MjModel::actuator_ctrllimited) .property("actuator_ctrlrange", &MjModel::actuator_ctrlrange) + .property("actuator_delay", &MjModel::actuator_delay) .property("actuator_dynprm", &MjModel::actuator_dynprm) .property("actuator_dyntype", &MjModel::actuator_dyntype) .property("actuator_forcelimited", &MjModel::actuator_forcelimited) @@ -11525,6 +11613,8 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) { .property("actuator_gaintype", &MjModel::actuator_gaintype) .property("actuator_gear", &MjModel::actuator_gear) .property("actuator_group", &MjModel::actuator_group) + .property("actuator_history", &MjModel::actuator_history) + .property("actuator_historyadr", &MjModel::actuator_historyadr) .property("actuator_length0", &MjModel::actuator_length0) .property("actuator_lengthrange", &MjModel::actuator_lengthrange) .property("actuator_plugin", &MjModel::actuator_plugin) @@ -11859,6 +11949,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) { .property("ngravcomp", &MjModel::ngravcomp, &MjModel::set_ngravcomp, reference()) .property("nhfield", &MjModel::nhfield, &MjModel::set_nhfield, reference()) .property("nhfielddata", &MjModel::nhfielddata, &MjModel::set_nhfielddata, reference()) + .property("nhistory", &MjModel::nhistory, &MjModel::set_nhistory, reference()) .property("njmax", &MjModel::njmax, &MjModel::set_njmax, reference()) .property("njnt", &MjModel::njnt, &MjModel::set_njnt, reference()) .property("nkey", &MjModel::nkey, &MjModel::set_nkey, reference()) @@ -11943,7 +12034,11 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) { .property("sensor_adr", &MjModel::sensor_adr) .property("sensor_cutoff", &MjModel::sensor_cutoff) .property("sensor_datatype", &MjModel::sensor_datatype) + .property("sensor_delay", &MjModel::sensor_delay) .property("sensor_dim", &MjModel::sensor_dim) + .property("sensor_history", &MjModel::sensor_history) + .property("sensor_historyadr", &MjModel::sensor_historyadr) + .property("sensor_interval", &MjModel::sensor_interval) .property("sensor_intprm", &MjModel::sensor_intprm) .property("sensor_needstage", &MjModel::sensor_needstage) .property("sensor_noise", &MjModel::sensor_noise) @@ -12239,6 +12334,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) { .property("cranklength", &MjsActuator::cranklength, &MjsActuator::set_cranklength, reference()) .property("ctrllimited", &MjsActuator::ctrllimited, &MjsActuator::set_ctrllimited, reference()) .property("ctrlrange", &MjsActuator::ctrlrange) + .property("delay", &MjsActuator::delay, &MjsActuator::set_delay, reference()) .property("dynprm", &MjsActuator::dynprm) .property("dyntype", &MjsActuator::dyntype, &MjsActuator::set_dyntype, reference()) .property("element", &MjsActuator::element, reference()) @@ -12250,7 +12346,9 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) { .property("group", &MjsActuator::group, &MjsActuator::set_group, reference()) .property("info", &MjsActuator::info, &MjsActuator::set_info, reference()) .property("inheritrange", &MjsActuator::inheritrange, &MjsActuator::set_inheritrange, reference()) + .property("interp", &MjsActuator::interp, &MjsActuator::set_interp, reference()) .property("lengthrange", &MjsActuator::lengthrange) + .property("nsample", &MjsActuator::nsample, &MjsActuator::set_nsample, reference()) .property("plugin", &MjsActuator::plugin, reference()) .property("refsite", &MjsActuator::refsite, &MjsActuator::set_refsite, reference()) .property("slidersite", &MjsActuator::slidersite, &MjsActuator::set_slidersite, reference()) @@ -12556,12 +12654,16 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) { emscripten::class_("MjsSensor") .property("cutoff", &MjsSensor::cutoff, &MjsSensor::set_cutoff, reference()) .property("datatype", &MjsSensor::datatype, &MjsSensor::set_datatype, reference()) + .property("delay", &MjsSensor::delay, &MjsSensor::set_delay, reference()) .property("dim", &MjsSensor::dim, &MjsSensor::set_dim, reference()) .property("element", &MjsSensor::element, reference()) .property("info", &MjsSensor::info, &MjsSensor::set_info, reference()) + .property("interp", &MjsSensor::interp, &MjsSensor::set_interp, reference()) + .property("interval", &MjsSensor::interval) .property("intprm", &MjsSensor::intprm) .property("needstage", &MjsSensor::needstage, &MjsSensor::set_needstage, reference()) .property("noise", &MjsSensor::noise, &MjsSensor::set_noise, reference()) + .property("nsample", &MjsSensor::nsample, &MjsSensor::set_nsample, reference()) .property("objname", &MjsSensor::objname, &MjsSensor::set_objname, reference()) .property("objtype", &MjsSensor::objtype, &MjsSensor::set_objtype, reference()) .property("plugin", &MjsSensor::plugin, reference()) @@ -12908,6 +13010,8 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) { function("mj_getTotalmass", &mj_getTotalmass_wrapper); function("mj_id2name", &mj_id2name_wrapper); function("mj_implicit", &mj_implicit_wrapper); + function("mj_initCtrlHistory", &mj_initCtrlHistory_wrapper); + function("mj_initSensorHistory", &mj_initSensorHistory_wrapper); function("mj_integratePos", &mj_integratePos_wrapper); function("mj_invConstraint", &mj_invConstraint_wrapper); function("mj_invPosition", &mj_invPosition_wrapper); @@ -12951,6 +13055,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) { function("mj_rayFlex", &mj_rayFlex_wrapper); function("mj_rayHfield", &mj_rayHfield_wrapper); function("mj_rayMesh", &mj_rayMesh_wrapper); + function("mj_readCtrl", &mj_readCtrl_wrapper); function("mj_referenceConstraint", &mj_referenceConstraint_wrapper); function("mj_resetCallbacks", &mj_resetCallbacks); function("mj_resetData", &mj_resetData_wrapper); diff --git a/wasm/codegen/generators/constants.py b/wasm/codegen/generators/constants.py index dedbf5a6..f7063d1d 100644 --- a/wasm/codegen/generators/constants.py +++ b/wasm/codegen/generators/constants.py @@ -184,6 +184,7 @@ _SKIPPED_GETTERS_AND_SETTERS: tuple[str, ...] = ( _SKIPPED_UTILITY_FUNCTIONS: tuple[str, ...] = ( # go/keep-sorted start + "mj_readSensor", "mju_getXMLDependencies", # go/keep-sorted end ) diff --git a/wasm/tests/bindings_test.ts b/wasm/tests/bindings_test.ts index d7dd0b41..9c60fbd1 100644 --- a/wasm/tests/bindings_test.ts +++ b/wasm/tests/bindings_test.ts @@ -1113,7 +1113,7 @@ describe('MuJoCo WASM Bindings', () => { mujoco.mj_stateSize(model!, invalidSig); }) .toThrowError( - 'MuJoCo Error: mj_stateSize: invalid state signature 8192 >= 2^mjNSTATE'); + 'MuJoCo Error: mj_stateSize: invalid state signature 16384 >= 2^mjNSTATE'); const sig = mujoco.mjtState.mjSTATE_INTEGRATION.value; const size = mujoco.mj_stateSize(model!, sig);