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

Add actuator and sensor delays. Fixes #1004

Browse Source 
PiperOrigin-RevId: 866478839
Change-Id: Id21a6da0f98454c8fa39ea5af8a5e213d6eae497
This commit is contained in:
Yuval Tassa
2026-02-06 08:46:45 -08:00
committed by Copybara-Service
parent 84fa527723
commit 6419534bad
48 changed files with 6282 additions and 219 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/APIreference/functions.rst
+83
View File
@@ -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<actuator-general-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<actuator-general-interp>` value.
In all three cases, constant extrapolation outside of buffer bounds.
See :ref:`Delays<CDelay>` 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<sensor-nsample>` > 0), read from the
history buffer at ``time - sensor_delay[id]``. See :ref:`Delays<CDelay>` for details.
**Return value semantics:**
- If no history buffer exists (:ref:`nsample<sensor-nsample>` = 0), returns a pointer to the sensor's slice of
``mjData.sensordata``.
- If a history buffer exists (:ref:`nsample<sensor-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<sensor-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<actuator-general-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<CDelay>` 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<sensor-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<CDelay>` for details.
.. _mj_setKeyframe:
`mj_setKeyframe <#mj_setKeyframe>`__
doc/APIreference/functions_override.rst
+63
View File
@@ -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<actuator-general-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<actuator-general-interp>` value.
In all three cases, constant extrapolation outside of buffer bounds.
See :ref:`Delays<CDelay>` 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<sensor-nsample>` > 0), read from the
history buffer at ``time - sensor_delay[id]``. See :ref:`Delays<CDelay>` for details.
**Return value semantics:**
- If no history buffer exists (:ref:`nsample<sensor-nsample>` = 0), returns a pointer to the sensor's slice of
``mjData.sensordata``.
- If a history buffer exists (:ref:`nsample<sensor-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<sensor-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<actuator-general-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<CDelay>` 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<sensor-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<CDelay>` 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
doc/XMLreference.rst
+592 -75
View File
File diff suppressed because it is too large Load Diff
doc/XMLschema.rst
+753 -18
View File
File diff suppressed because it is too large Load Diff
doc/changelog.rst
+3
View File
@@ -32,6 +32,9 @@ Upcoming version (not yet released)
General
^^^^^^^
- Actuators and sensors now support arbitrary delays, see :ref:`Delays<CDelay>` for details. Adding
delays introduces a new ``mjData.history`` variable to the :ref:`Physics state<siPhysicsState>`.
.. image:: images/changelog/poncho.png
:width: 45%
:align: right
doc/images/modeling/delay_buffer_dark.svg
+1129
View File
File diff suppressed because it is too large Load Diff
Side by Side

After

Width:  |  Height:  |  Size: 30 KiB

doc/images/modeling/delay_buffer_light.svg
+1127
View File
File diff suppressed because it is too large Load Diff
Side by Side

After

Width:  |  Height:  |  Size: 30 KiB

doc/includes/references.h
+37 -11
View File
@@ -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);
doc/modeling.rst
+166
View File
@@ -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 <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 <saSimulate>`. Note that :at:`cutoff` has a different meaning for
:ref:`collision sensors<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<sensor-delay>` and :ref:`interval<sensor-interval>` features.
See :ref:`Delays<CDelay>` 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<CDelay>` 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<sensor-nsample>`, cannot be negative.
In the most common case, ``delay = nsample * timestep``, see :ref:`Delays<CDelay>` 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 <sensor-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 <CUser>`.
.. _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<actuator-general-nsample>`, :ref:`sensors<sensor-nsample>`) is positive, a
buffer with :at:`nsample` slots is included in the :ref:`physics state<siPhysicsState>` 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<actuator-general-delay>`,
:ref:`sensors<sensor-delay>`) 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<sensor-delay>` and :ref:`interval<sensor-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<sensor-interval>`: ``[..., -2*dt, -dt]``.
- **Sensor timestamps** with :ref:`interval<sensor-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<actuator-general-interp>`,
:ref:`sensors<sensor-interp>`). 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<sensor-insidesite>`).
.. _CCamera:
Cameras
doc/programming/simulation.rst
+6 -1
View File
@@ -264,7 +264,7 @@ individual components and combinations of components. These are:
Physics state
"""""""""""""
The *physics state* (:ref:`mjSTATE_PHYSICS<mjtState>`) 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<geIntegration>` 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<geIntegration>` section.
History buffer: ``history``
When actuators or sensors have a positive :at:`nsample` attribute (:ref:`actuators<actuator-general-nsample>`,
:ref:`sensors<sensor-nsample>`), this buffer stores timestamped samples of previous
control or sensor values. See :ref:`Delays<CDelay>` for details.
.. _siFullPhysics:
Full physics state
include/mujoco/mjdata.h
+13 -11
View File
@@ -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)
include/mujoco/mjmodel.h
+8
View File
@@ -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)
include/mujoco/mjspec.h
+9
View File
@@ -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
include/mujoco/mjxmacro.h
+9
View File
@@ -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 ) \
include/mujoco/mujoco.h
+23
View File
@@ -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);
mjx/mujoco/mjx/_src/io.py
+5 -1
View File
@@ -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',
mjx/mujoco/mjx/_src/types.py
+11
View File
@@ -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:
python/mujoco/bindings_test.py
+165 -1
View File
@@ -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"""
<mujoco>
<worldbody>
<body>
<geom type="sphere" size="0.1"/>
<joint name="hinge" type="hinge"/>
</body>
</worldbody>
<actuator>
<position name="actuator" joint="hinge" delay="0.01" nsample="4"/>
</actuator>
</mujoco>
"""
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"""
<mujoco>
<worldbody>
<body>
<geom type="sphere" size="0.1"/>
<joint name="hinge" type="hinge"/>
<site name="site"/>
</body>
</worldbody>
<sensor>
<accelerometer name="accel" site="site" delay="0.01" nsample="3"/>
</sensor>
</mujoco>
"""
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"""
<mujoco>
<worldbody>
<body name="body">
<freejoint/>
<geom type="sphere" size="0.1"/>
</body>
</worldbody>
<sensor>
<framequat name="quat" objtype="body" objname="body" delay="0.01" nsample="5"/>
</sensor>
</mujoco>
"""
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)
python/mujoco/functions.cc
+55 -1
View File
@@ -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<char[]>(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<traits::mj_copyState>(pymodule);
Def<traits::mj_readCtrl>(pymodule);
Def<traits::mj_readSensor>(
pymodule,
[](const raw::MjModel* m, const raw::MjData* d, int id, mjtNum time,
Eigen::Ref<EigenVectorX> 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<traits::mj_initCtrlHistory>(
pymodule,
[](const raw::MjModel* m, raw::MjData* d, int id,
std::optional<Eigen::Ref<const EigenVectorX>> times,
Eigen::Ref<const EigenVectorX> 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<traits::mj_initSensorHistory>(
pymodule, [](const raw::MjModel* m, raw::MjData* d, int id,
std::optional<Eigen::Ref<const EigenVectorX>> times,
Eigen::Ref<const EigenArrayXX> 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<traits::mj_setKeyframe>(pymodule);
Def<traits::mj_addContact>(pymodule);
Def<traits::mj_isPyramidal>(pymodule);
python/mujoco/introspect/enums.py
+15 -14
View File
@@ -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',
python/mujoco/introspect/functions.py
+150
View File
@@ -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',
python/mujoco/introspect/structs.py
+107
View File
@@ -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(
src/engine/engine_derivative_fd.c
+7
View File
@@ -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
src/engine/engine_forward.c
+61 -4
View File
@@ -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];
src/engine/engine_io.c
+63 -4
View File
@@ -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);
src/engine/engine_print.c
+75
View File
@@ -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);
src/engine/engine_sensor.c
+54 -6
View File
@@ -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);
}
}
}
src/engine/engine_sensor.h
+3
View File
@@ -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);
src/engine/engine_support.c
+113 -1
View File
@@ -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);
}
src/engine/engine_support.h
+23
View File
@@ -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
src/user/user_model.cc
+43
View File
@@ -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
src/user/user_model.h
+1
View File
@@ -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
src/user/user_objects.cc
+36
View File
@@ -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);
src/xml/xml_base.h
+2
View File
@@ -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[];
src/xml/xml_native_reader.cc
+82 -66
View File
@@ -182,29 +182,29 @@ std::vector<const char*> 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<const char*> 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<const char*> 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());
}
src/xml/xml_native_writer.cc
+8
View File
@@ -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());
}
test/engine/engine_forward_test.cc
+104
View File
@@ -1513,5 +1513,109 @@ TEST_F(ActuatorTest, TendonActuatorForceRange) {
mj_deleteModel(model);
}
// ----------------------------- actuator delays -------------------------------
TEST_F(ForwardTest, ActuatorDelay) {
static constexpr char xml[] = R"(
<mujoco>
<option timestep="0.01"/>
<worldbody>
<body>
<joint name="slide" type="slide"/>
<geom size="0.1" mass="1"/>
</body>
</worldbody>
<actuator>
<motor joint="slide" delay="0.02" nsample="2"/>
</actuator>
</mujoco>
)";
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"(
<mujoco>
<option timestep="0.01"/>
<worldbody>
<body>
<joint name="slide" type="slide"/>
<geom size="0.1"/>
</body>
</worldbody>
<actuator>
<motor joint="slide" delay="0.015" nsample="3" interp="linear"/>
</actuator>
</mujoco>
)";
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
test/engine/engine_sensor_test.cc
+398
View File
@@ -24,6 +24,7 @@
#include <mujoco/mjmodel.h>
#include <mujoco/mjtnum.h>
#include <mujoco/mujoco.h>
#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"(
<mujoco>
<option timestep="0.01" gravity="0 0 0"/>
<worldbody>
<body>
<joint name="slide" type="slide"/>
<geom size="0.1"/>
</body>
</worldbody>
<sensor>
<jointpos joint="slide" delay="0.02" nsample="3"/>
</sensor>
</mujoco>
)";
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"(
<mujoco>
<option timestep="0.01" gravity="0 0 0"/>
<worldbody>
<body>
<joint name="slide" type="slide"/>
<geom size="0.1"/>
</body>
</worldbody>
<sensor>
<jointpos joint="slide" delay="0.015" nsample="3" interp="linear"/>
</sensor>
</mujoco>
)";
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"(
<mujoco>
<option timestep="1" gravity="0 0 0"/>
<worldbody>
<body>
<joint name="slide" type="slide"/>
<geom size="0.1"/>
</body>
</worldbody>
<sensor>
<jointpos name="default_phase" joint="slide" interval="2.5 0" nsample="10"/>
<jointpos name="offset_phase" joint="slide" interval="2.5 -1.5" nsample="10"/>
</sensor>
</mujoco>
)";
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"(
<mujoco>
<option timestep="0.01" gravity="0 0 0"/>
<worldbody>
<body>
<joint name="slide" type="slide"/>
<geom size="0.1"/>
</body>
</worldbody>
<sensor>
<jointpos joint="slide" delay="0.02" interval="0.03 0" nsample="5"/>
</sensor>
</mujoco>
)";
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"(
<mujoco>
<option timestep="0.01"/>
<worldbody>
<body>
<joint name="slide" type="slide"/>
<geom size="0.1"/>
</body>
</worldbody>
<sensor>
<jointpos joint="slide" nsample="5"/>
</sensor>
</mujoco>
)";
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"(
<mujoco>
<option timestep="0.01"/>
<worldbody>
<body>
<joint name="ball" type="ball"/>
<geom size="0.1"/>
</body>
</worldbody>
<sensor>
<ballangvel joint="ball" delay="0.02" nsample="2"/>
</sensor>
</mujoco>
)";
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"(
<mujoco>
<option timestep="0.01" gravity="0 0 0"/>
<worldbody>
<body>
<joint name="slide" type="slide"/>
<geom size="0.1"/>
</body>
</worldbody>
<sensor>
<jointpos joint="slide" nsample="5"/>
</sensor>
</mujoco>
)";
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
test/engine/engine_support_test.cc
+269 -4
View File
@@ -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"(
<mujoco>
<option timestep="0.01"/>
<worldbody>
<body>
<joint name="slide" type="slide"/>
<geom size="0.1"/>
</body>
</worldbody>
<actuator>
<motor joint="slide" delay="0.05" nsample="5"/>
</actuator>
</mujoco>
)";
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<mjtNum> 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<mjtNum> 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<mjtNum> 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"(
<mujoco>
<worldbody>
<body>
<joint name="slide" type="slide"/>
<geom size="1"/>
</body>
</worldbody>
<actuator>
<motor joint="slide"/>
</actuator>
</mujoco>
)";
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"(
<mujoco>
<option timestep="0.01"/>
<worldbody>
<body>
<joint name="slide" type="slide"/>
<geom size="1"/>
</body>
</worldbody>
<actuator>
<motor joint="slide" delay="0.03" nsample="3"/>
</actuator>
</mujoco>
)";
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"(
<mujoco>
<option timestep="0.01"/>
<worldbody>
<body>
<joint name="slide" type="slide"/>
<geom size="1"/>
</body>
</worldbody>
<actuator>
<motor joint="slide" delay="0.02" nsample="3"/>
</actuator>
</mujoco>
)";
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"(
<mujoco>
<option timestep="0.01"/>
<worldbody>
<body>
<joint name="slide" type="slide"/>
<geom size="1"/>
</body>
</worldbody>
<actuator>
<motor joint="slide" delay="0.02" nsample="3"/>
</actuator>
</mujoco>
)";
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"(
<mujoco>
<option timestep="0.01"/>
<worldbody>
<body>
<joint name="slide" type="slide"/>
<geom size="1"/>
</body>
</worldbody>
<sensor>
<jointpos joint="slide" delay="0.02" nsample="3"/>
</sensor>
</mujoco>
)";
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
test/testdata/delay.xml
Vendored
+20
View File
@@ -0,0 +1,20 @@
<mujoco>
<option timestep="0.01"/>
<worldbody>
<body name="body">
<joint name="slide" type="slide"/>
<geom size=".1"/>
</body>
</worldbody>
<actuator>
<motor name="motor0" joint="slide" delay="0.05" nsample="6"/>
</actuator>
<sensor>
<jointpos name="jointpos0" joint="slide" delay="0.025" interp="linear" nsample="4"/>
<jointpos name="jointpos1" joint="slide" interval="0.025" nsample="3"/>
<framepos name="framepos0" objtype="body" objname="body" nsample="5"/>
</sensor>
</mujoco>
test/user/user_model_test.cc
+42
View File
@@ -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"(
<mujoco>
<option timestep="1"/>
<worldbody>
<body>
<geom size="1"/>
<joint name="jnt1"/>
<joint name="jnt2"/>
<joint name="jnt3"/>
</body>
</worldbody>
<actuator>
<motor joint="jnt1"/>
<motor joint="jnt2" delay="3" nsample="3"/>
<motor joint="jnt3" delay="10" nsample="10"/>
</actuator>
</mujoco>
)";
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
test/user/user_objects_test.cc
+60
View File
@@ -626,6 +626,66 @@ TEST_F(SensorTest, OjbtypeParsedButNotRequired) {
mj_deleteModel(model);
}
TEST_F(SensorTest, NegativeIntervalError) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<joint name="j"/>
<geom size="1"/>
</body>
</worldbody>
<sensor>
<jointpos joint="j" interval="-1"/>
</sensor>
</mujoco>
)";
std::array<char, 1024> 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"(
<mujoco>
<worldbody>
<body>
<joint name="j"/>
<geom size="1"/>
</body>
</worldbody>
<sensor>
<jointpos joint="j" interval="0.1 0.05"/>
</sensor>
</mujoco>
)";
std::array<char, 1024> 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"(
<mujoco>
<worldbody>
<body>
<joint name="j"/>
<geom size="1"/>
</body>
</worldbody>
<sensor>
<jointpos joint="j" delay="0.1"/>
</sensor>
</mujoco>
)";
std::array<char, 1024> 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 =
test/xml/xml_native_reader_test.cc
+90
View File
@@ -3267,5 +3267,95 @@ TEST_F(XMLReaderTest, CameraOutputDefault) {
mj_deleteModel(model);
}
// ------------- test delay attribute parsing ----------------------------------
TEST_F(ActuatorParseTest, ActuatorDelayParsed) {
static constexpr char xml[] = R"(
<mujoco>
<option timestep="1"/>
<worldbody>
<body>
<geom size="1"/>
<joint name="jnt1"/>
<joint name="jnt2"/>
<joint name="jnt3"/>
</body>
</worldbody>
<actuator>
<motor joint="jnt1"/>
<motor joint="jnt2" delay="3" nsample="3" interp="zoh"/>
<motor joint="jnt3" delay="10" nsample="10" interp="cubic"/>
</actuator>
</mujoco>
)";
std::array<char, 1024> 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"(
<mujoco>
<option timestep="1"/>
<default>
<motor delay="5" nsample="5"/>
</default>
<worldbody>
<body>
<geom size="1"/>
<joint name="jnt1"/>
<joint name="jnt2"/>
</body>
</worldbody>
<actuator>
<motor joint="jnt1"/>
<motor joint="jnt2" delay="0" nsample="0"/>
</actuator>
</mujoco>
)";
std::array<char, 1024> 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"(
<mujoco>
<worldbody>
<body>
<geom size="1"/>
<joint name="jnt"/>
</body>
</worldbody>
<actuator>
<motor joint="jnt" delay="1"/>
</actuator>
</mujoco>
)";
std::array<char, 1024> 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
test/xml/xml_native_writer_test.cc
+71
View File
@@ -1720,5 +1720,76 @@ TEST_F(XMLWriterTest, WritesCameraOutputDefault) {
mj_deleteModel(model);
}
TEST_F(XMLWriterTest, WritesSensorDelayIntervalHistory) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<joint name="slide" type="slide"/>
<geom size="0.1"/>
</body>
</worldbody>
<sensor>
<jointpos joint="slide" delay="0.05" interval="0.1 -0.02" nsample="20" interp="linear"/>
</sensor>
</mujoco>
)";
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"(
<mujoco>
<worldbody>
<body>
<joint name="slide" type="slide"/>
<geom size="0.1"/>
</body>
</worldbody>
<sensor>
<jointpos joint="slide"/>
</sensor>
</mujoco>
)";
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"(
<mujoco>
<worldbody>
<body>
<joint name="slide" type="slide"/>
<geom size="0.1"/>
</body>
</worldbody>
<actuator>
<motor joint="slide" delay="0.03" nsample="15" interp="cubic"/>
</actuator>
</mujoco>
)";
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
unity/Runtime/Bindings/MjBindings.cs
+21
View File
@@ -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);
wasm/codegen/generated/bindings.cc
+105
View File
@@ -4120,6 +4120,12 @@ struct MjModel {
void set_npluginstate(int value) {
ptr_->npluginstate = static_cast<mjtSize>(value);
}
int nhistory() const {
return static_cast<int>(ptr_->nhistory);
}
void set_nhistory(int value) {
ptr_->nhistory = static_cast<mjtSize>(value);
}
int narena() const {
return static_cast<int>(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>("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);
wasm/codegen/generators/constants.py
+1
View File
@@ -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
)
wasm/tests/bindings_test.ts
+1 -1
View File
@@ -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);