Add actuator and sensor delays. Fixes #1004
PiperOrigin-RevId: 866478839 Change-Id: Id21a6da0f98454c8fa39ea5af8a5e213d6eae497
This commit is contained in:
committed by
Copybara-Service
parent
84fa527723
commit
6419534bad
@@ -300,6 +300,89 @@ Copy concatenated state components specified by ``sig`` from ``state`` into ``d
|
|||||||
|
|
||||||
Copy state from src to dst.
|
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 <#mj_setKeyframe>`__
|
`mj_setKeyframe <#mj_setKeyframe>`__
|
||||||
|
|||||||
@@ -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
|
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.
|
``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:
|
.. _mj_mulJacVec:
|
||||||
|
|
||||||
This function multiplies the constraint Jacobian mjData.efc_J by a vector. Note that the Jacobian can be either dense or
|
This function multiplies the constraint Jacobian mjData.efc_J by a vector. Note that the Jacobian can be either dense or
|
||||||
|
|||||||
+592
-75
File diff suppressed because it is too large
Load Diff
+753
-18
File diff suppressed because it is too large
Load Diff
@@ -32,6 +32,9 @@ Upcoming version (not yet released)
|
|||||||
General
|
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
|
.. image:: images/changelog/poncho.png
|
||||||
:width: 45%
|
:width: 45%
|
||||||
:align: right
|
:align: right
|
||||||
|
|||||||
File diff suppressed because it is too large
Load Diff
|
After Width: | Height: | Size: 30 KiB |
File diff suppressed because it is too large
Load Diff
|
After Width: | Height: | Size: 30 KiB |
+37
-11
@@ -21,20 +21,21 @@ typedef enum mjtState_ { // state elements
|
|||||||
mjSTATE_QPOS = 1<<1, // position
|
mjSTATE_QPOS = 1<<1, // position
|
||||||
mjSTATE_QVEL = 1<<2, // velocity
|
mjSTATE_QVEL = 1<<2, // velocity
|
||||||
mjSTATE_ACT = 1<<3, // actuator activation
|
mjSTATE_ACT = 1<<3, // actuator activation
|
||||||
mjSTATE_WARMSTART = 1<<4, // acceleration used for warmstart
|
mjSTATE_HISTORY = 1<<4, // history buffers (control, sensor)
|
||||||
mjSTATE_CTRL = 1<<5, // control
|
mjSTATE_WARMSTART = 1<<5, // acceleration used for warmstart
|
||||||
mjSTATE_QFRC_APPLIED = 1<<6, // applied generalized force
|
mjSTATE_CTRL = 1<<6, // control
|
||||||
mjSTATE_XFRC_APPLIED = 1<<7, // applied Cartesian force/torque
|
mjSTATE_QFRC_APPLIED = 1<<7, // applied generalized force
|
||||||
mjSTATE_EQ_ACTIVE = 1<<8, // enable/disable constraints
|
mjSTATE_XFRC_APPLIED = 1<<8, // applied Cartesian force/torque
|
||||||
mjSTATE_MOCAP_POS = 1<<9, // positions of mocap bodies
|
mjSTATE_EQ_ACTIVE = 1<<9, // enable/disable constraints
|
||||||
mjSTATE_MOCAP_QUAT = 1<<10, // orientations of mocap bodies
|
mjSTATE_MOCAP_POS = 1<<10, // positions of mocap bodies
|
||||||
mjSTATE_USERDATA = 1<<11, // user data
|
mjSTATE_MOCAP_QUAT = 1<<11, // orientations of mocap bodies
|
||||||
mjSTATE_PLUGIN = 1<<12, // plugin state
|
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
|
// 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_FULLPHYSICS = mjSTATE_TIME | mjSTATE_PHYSICS | mjSTATE_PLUGIN,
|
||||||
mjSTATE_USER = mjSTATE_CTRL | mjSTATE_QFRC_APPLIED | mjSTATE_XFRC_APPLIED |
|
mjSTATE_USER = mjSTATE_CTRL | mjSTATE_QFRC_APPLIED | mjSTATE_XFRC_APPLIED |
|
||||||
mjSTATE_EQ_ACTIVE | mjSTATE_MOCAP_POS | mjSTATE_MOCAP_QUAT |
|
mjSTATE_EQ_ACTIVE | mjSTATE_MOCAP_POS | mjSTATE_MOCAP_QUAT |
|
||||||
@@ -221,6 +222,7 @@ struct mjData_ {
|
|||||||
mjtNum* qpos; // position (nq x 1)
|
mjtNum* qpos; // position (nq x 1)
|
||||||
mjtNum* qvel; // velocity (nv x 1)
|
mjtNum* qvel; // velocity (nv x 1)
|
||||||
mjtNum* act; // actuator activation (na 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* qacc_warmstart; // acceleration used for warmstart (nv x 1)
|
||||||
mjtNum* plugin_state; // plugin state (npluginstate 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 nuserdata; // number of mjtNums reserved for the user
|
||||||
mjtSize nsensordata; // number of mjtNums in sensor data vector
|
mjtSize nsensordata; // number of mjtNums in sensor data vector
|
||||||
mjtSize npluginstate; // number of mjtNums in plugin state vector
|
mjtSize npluginstate; // number of mjtNums in plugin state vector
|
||||||
|
mjtSize nhistory; // number of mjtNums in history buffer
|
||||||
|
|
||||||
// buffer sizes
|
// buffer sizes
|
||||||
mjtSize narena; // number of bytes in the mjData arena (inclusive of stack)
|
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_actadr; // first activation address; -1: stateless (nu x 1)
|
||||||
int* actuator_actnum; // number of activation variables (nu x 1)
|
int* actuator_actnum; // number of activation variables (nu x 1)
|
||||||
int* actuator_group; // group for visibility (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_ctrllimited; // is control limited (nu x 1)
|
||||||
mjtByte* actuator_forcelimited;// is force limited (nu x 1)
|
mjtByte* actuator_forcelimited;// is force limited (nu x 1)
|
||||||
mjtByte* actuator_actlimited; // is activation 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)
|
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_cutoff; // cutoff for real and positive; 0: ignore (nsensor x 1)
|
||||||
mjtNum* sensor_noise; // noise standard deviation (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)
|
mjtNum* sensor_user; // user data (nsensor x nuser_sensor)
|
||||||
int* sensor_plugin; // plugin instance id; -1: not a plugin (nsensor x 1)
|
int* sensor_plugin; // plugin instance id; -1: not a plugin (nsensor x 1)
|
||||||
|
|
||||||
@@ -2445,6 +2455,9 @@ typedef struct mjsActuator_ { // actuator specification
|
|||||||
|
|
||||||
// other
|
// other
|
||||||
int group; // group
|
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
|
mjDoubleVec* userdata; // user data
|
||||||
mjsPlugin plugin; // actuator plugin
|
mjsPlugin plugin; // actuator plugin
|
||||||
mjString* info; // message appended to compiler errors
|
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 cutoff; // cutoff for real and positive datatypes
|
||||||
double noise; // noise stdev
|
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
|
// other
|
||||||
mjDoubleVec* userdata; // user data
|
mjDoubleVec* userdata; // user data
|
||||||
mjsPlugin plugin; // sensor plugin
|
mjsPlugin plugin; // sensor plugin
|
||||||
@@ -3234,6 +3253,13 @@ void mj_extractState(const mjModel* m, const mjtNum* src, int srcsig,
|
|||||||
mjtNum* dst, int dstsig);
|
mjtNum* dst, int dstsig);
|
||||||
void mj_setState(const mjModel* m, mjData* d, const mjtNum* state, int sig);
|
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);
|
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);
|
void mj_setKeyframe(mjModel* m, const mjData* d, int k);
|
||||||
int mj_addContact(const mjModel* m, mjData* d, const mjContact* con);
|
int mj_addContact(const mjModel* m, mjData* d, const mjContact* con);
|
||||||
int mj_isPyramidal(const mjModel* m);
|
int mj_isPyramidal(const mjModel* m);
|
||||||
|
|||||||
@@ -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.
|
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`
|
:at:`name`: :at-val:`string, optional`
|
||||||
Name of the sensor.
|
Name of the sensor.
|
||||||
|
|
||||||
|
.. _sensor-noise:
|
||||||
|
|
||||||
:at:`noise`: :at-val:`real, "0"`
|
: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
|
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
|
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
|
detailed justification. As of subsequent versions, this attrbute serves as a convenient location for saving standard
|
||||||
deviation information for later use.
|
deviation information for later use.
|
||||||
|
|
||||||
|
.. _sensor-cutoff:
|
||||||
|
|
||||||
:at:`cutoff`: :at-val:`real, "0"`
|
: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
|
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
|
sensor data plots in :ref:`simulate.cc <saSimulate>`. Note that :at:`cutoff` has a different meaning for
|
||||||
:ref:`collision sensors<collision-sensors>`.
|
: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 ..."`
|
:at:`user`: :at-val:`real(nuser_sensor), "0 0 ..."`
|
||||||
See :ref:`User parameters <CUser>`.
|
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:
|
.. _CCamera:
|
||||||
|
|
||||||
Cameras
|
Cameras
|
||||||
|
|||||||
@@ -264,7 +264,7 @@ individual components and combinations of components. These are:
|
|||||||
Physics state
|
Physics state
|
||||||
"""""""""""""
|
"""""""""""""
|
||||||
The *physics state* (:ref:`mjSTATE_PHYSICS<mjtState>`) contains the main quantities which are time-integrated during
|
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``
|
Position: ``qpos``
|
||||||
The configuration in generalized coodinates, denoted in the :ref:`Numerical Integration<geIntegration>` section as
|
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
|
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.
|
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:
|
.. _siFullPhysics:
|
||||||
|
|
||||||
Full physics state
|
Full physics state
|
||||||
|
|||||||
+13
-11
@@ -29,20 +29,21 @@ typedef enum mjtState_ { // state elements
|
|||||||
mjSTATE_QPOS = 1<<1, // position
|
mjSTATE_QPOS = 1<<1, // position
|
||||||
mjSTATE_QVEL = 1<<2, // velocity
|
mjSTATE_QVEL = 1<<2, // velocity
|
||||||
mjSTATE_ACT = 1<<3, // actuator activation
|
mjSTATE_ACT = 1<<3, // actuator activation
|
||||||
mjSTATE_WARMSTART = 1<<4, // acceleration used for warmstart
|
mjSTATE_HISTORY = 1<<4, // history buffers (control, sensor)
|
||||||
mjSTATE_CTRL = 1<<5, // control
|
mjSTATE_WARMSTART = 1<<5, // acceleration used for warmstart
|
||||||
mjSTATE_QFRC_APPLIED = 1<<6, // applied generalized force
|
mjSTATE_CTRL = 1<<6, // control
|
||||||
mjSTATE_XFRC_APPLIED = 1<<7, // applied Cartesian force/torque
|
mjSTATE_QFRC_APPLIED = 1<<7, // applied generalized force
|
||||||
mjSTATE_EQ_ACTIVE = 1<<8, // enable/disable constraints
|
mjSTATE_XFRC_APPLIED = 1<<8, // applied Cartesian force/torque
|
||||||
mjSTATE_MOCAP_POS = 1<<9, // positions of mocap bodies
|
mjSTATE_EQ_ACTIVE = 1<<9, // enable/disable constraints
|
||||||
mjSTATE_MOCAP_QUAT = 1<<10, // orientations of mocap bodies
|
mjSTATE_MOCAP_POS = 1<<10, // positions of mocap bodies
|
||||||
mjSTATE_USERDATA = 1<<11, // user data
|
mjSTATE_MOCAP_QUAT = 1<<11, // orientations of mocap bodies
|
||||||
mjSTATE_PLUGIN = 1<<12, // plugin state
|
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
|
// 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_FULLPHYSICS = mjSTATE_TIME | mjSTATE_PHYSICS | mjSTATE_PLUGIN,
|
||||||
mjSTATE_USER = mjSTATE_CTRL | mjSTATE_QFRC_APPLIED | mjSTATE_XFRC_APPLIED |
|
mjSTATE_USER = mjSTATE_CTRL | mjSTATE_QFRC_APPLIED | mjSTATE_XFRC_APPLIED |
|
||||||
mjSTATE_EQ_ACTIVE | mjSTATE_MOCAP_POS | mjSTATE_MOCAP_QUAT |
|
mjSTATE_EQ_ACTIVE | mjSTATE_MOCAP_POS | mjSTATE_MOCAP_QUAT |
|
||||||
@@ -255,6 +256,7 @@ struct mjData_ {
|
|||||||
mjtNum* qpos; // position (nq x 1)
|
mjtNum* qpos; // position (nq x 1)
|
||||||
mjtNum* qvel; // velocity (nv x 1)
|
mjtNum* qvel; // velocity (nv x 1)
|
||||||
mjtNum* act; // actuator activation (na 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* qacc_warmstart; // acceleration used for warmstart (nv x 1)
|
||||||
mjtNum* plugin_state; // plugin state (npluginstate x 1)
|
mjtNum* plugin_state; // plugin state (npluginstate x 1)
|
||||||
|
|
||||||
|
|||||||
@@ -761,6 +761,7 @@ struct mjModel_ {
|
|||||||
mjtSize nuserdata; // number of mjtNums reserved for the user
|
mjtSize nuserdata; // number of mjtNums reserved for the user
|
||||||
mjtSize nsensordata; // number of mjtNums in sensor data vector
|
mjtSize nsensordata; // number of mjtNums in sensor data vector
|
||||||
mjtSize npluginstate; // number of mjtNums in plugin state vector
|
mjtSize npluginstate; // number of mjtNums in plugin state vector
|
||||||
|
mjtSize nhistory; // number of mjtNums in history buffer
|
||||||
|
|
||||||
// buffer sizes
|
// buffer sizes
|
||||||
mjtSize narena; // number of bytes in the mjData arena (inclusive of stack)
|
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_actadr; // first activation address; -1: stateless (nu x 1)
|
||||||
int* actuator_actnum; // number of activation variables (nu x 1)
|
int* actuator_actnum; // number of activation variables (nu x 1)
|
||||||
int* actuator_group; // group for visibility (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_ctrllimited; // is control limited (nu x 1)
|
||||||
mjtByte* actuator_forcelimited;// is force limited (nu x 1)
|
mjtByte* actuator_forcelimited;// is force limited (nu x 1)
|
||||||
mjtByte* actuator_actlimited; // is activation 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)
|
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_cutoff; // cutoff for real and positive; 0: ignore (nsensor x 1)
|
||||||
mjtNum* sensor_noise; // noise standard deviation (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)
|
mjtNum* sensor_user; // user data (nsensor x nuser_sensor)
|
||||||
int* sensor_plugin; // plugin instance id; -1: not a plugin (nsensor x 1)
|
int* sensor_plugin; // plugin instance id; -1: not a plugin (nsensor x 1)
|
||||||
|
|
||||||
|
|||||||
@@ -698,6 +698,9 @@ typedef struct mjsActuator_ { // actuator specification
|
|||||||
|
|
||||||
// other
|
// other
|
||||||
int group; // group
|
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
|
mjDoubleVec* userdata; // user data
|
||||||
mjsPlugin plugin; // actuator plugin
|
mjsPlugin plugin; // actuator plugin
|
||||||
mjString* info; // message appended to compiler errors
|
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 cutoff; // cutoff for real and positive datatypes
|
||||||
double noise; // noise stdev
|
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
|
// other
|
||||||
mjDoubleVec* userdata; // user data
|
mjDoubleVec* userdata; // user data
|
||||||
mjsPlugin plugin; // sensor plugin
|
mjsPlugin plugin; // sensor plugin
|
||||||
|
|||||||
@@ -153,6 +153,7 @@
|
|||||||
X( nuserdata ) \
|
X( nuserdata ) \
|
||||||
X( nsensordata ) \
|
X( nsensordata ) \
|
||||||
X( npluginstate ) \
|
X( npluginstate ) \
|
||||||
|
X( nhistory ) \
|
||||||
X( narena ) \
|
X( narena ) \
|
||||||
X( nbuffer )
|
X( nbuffer )
|
||||||
|
|
||||||
@@ -563,6 +564,9 @@
|
|||||||
X ( int, actuator_actadr, nu, 1 ) \
|
X ( int, actuator_actadr, nu, 1 ) \
|
||||||
X ( int, actuator_actnum, nu, 1 ) \
|
X ( int, actuator_actnum, nu, 1 ) \
|
||||||
X ( int, actuator_group, 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_ctrllimited, nu, 1 ) \
|
||||||
X ( mjtByte, actuator_forcelimited, nu, 1 ) \
|
X ( mjtByte, actuator_forcelimited, nu, 1 ) \
|
||||||
X ( mjtByte, actuator_actlimited, nu, 1 ) \
|
X ( mjtByte, actuator_actlimited, nu, 1 ) \
|
||||||
@@ -594,6 +598,10 @@
|
|||||||
X ( int, sensor_adr, nsensor, 1 ) \
|
X ( int, sensor_adr, nsensor, 1 ) \
|
||||||
X ( mjtNum, sensor_cutoff, nsensor, 1 ) \
|
X ( mjtNum, sensor_cutoff, nsensor, 1 ) \
|
||||||
X ( mjtNum, sensor_noise, 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 ( mjtNum, sensor_user, nsensor, MJ_M(nuser_sensor) ) \
|
||||||
X ( int, sensor_plugin, nsensor, 1 )
|
X ( int, sensor_plugin, nsensor, 1 )
|
||||||
|
|
||||||
@@ -708,6 +716,7 @@
|
|||||||
X ( mjtNum, qpos, nq, 1 ) \
|
X ( mjtNum, qpos, nq, 1 ) \
|
||||||
X ( mjtNum, qvel, nv, 1 ) \
|
X ( mjtNum, qvel, nv, 1 ) \
|
||||||
X ( mjtNum, act, na, 1 ) \
|
X ( mjtNum, act, na, 1 ) \
|
||||||
|
X ( mjtNum, history, nhistory, 1 ) \
|
||||||
X ( mjtNum, qacc_warmstart, nv, 1 ) \
|
X ( mjtNum, qacc_warmstart, nv, 1 ) \
|
||||||
X ( mjtNum, plugin_state, npluginstate, 1 ) \
|
X ( mjtNum, plugin_state, npluginstate, 1 ) \
|
||||||
X ( mjtNum, ctrl, nu, 1 ) \
|
X ( mjtNum, ctrl, nu, 1 ) \
|
||||||
|
|||||||
@@ -493,6 +493,29 @@ MJAPI void mj_setState(const mjModel* m, mjData* d, const mjtNum* state, int sig
|
|||||||
// Copy state from src to dst.
|
// Copy state from src to dst.
|
||||||
MJAPI void mj_copyState(const mjModel* m, const mjData* src, mjData* dst, int sig);
|
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.
|
// Copy current state to the k-th model keyframe.
|
||||||
MJAPI void mj_setKeyframe(mjModel* m, const mjData* d, int k);
|
MJAPI void mj_setKeyframe(mjModel* m, const mjData* d, int k);
|
||||||
|
|
||||||
|
|||||||
@@ -571,6 +571,7 @@ def _make_data_public_fields(m: types.Model) -> Dict[str, Any]:
|
|||||||
'time': (float_,),
|
'time': (float_,),
|
||||||
'qvel': (m.nv, float_),
|
'qvel': (m.nv, float_),
|
||||||
'act': (m.na, float_),
|
'act': (m.na, float_),
|
||||||
|
'history': (m.nhistory, float_),
|
||||||
'plugin_state': (m.npluginstate, float_),
|
'plugin_state': (m.npluginstate, float_),
|
||||||
'qacc_warmstart': (m.nv, float_),
|
'qacc_warmstart': (m.nv, float_),
|
||||||
'ctrl': (m.nu, float_),
|
'ctrl': (m.nu, float_),
|
||||||
@@ -875,7 +876,7 @@ def _make_data_warp(
|
|||||||
|
|
||||||
fields = _make_data_public_fields(m)
|
fields = _make_data_public_fields(m)
|
||||||
for k in fields:
|
for k in fields:
|
||||||
if k in {'userdata', 'plugin_state'}:
|
if k in {'userdata', 'plugin_state', 'history'}:
|
||||||
continue
|
continue
|
||||||
if not hasattr(dw, k):
|
if not hasattr(dw, k):
|
||||||
raise ValueError(f'Public data field {k} not found in Warp data.')
|
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_QPOS: 'qpos',
|
||||||
mujoco.mjtState.mjSTATE_QVEL: 'qvel',
|
mujoco.mjtState.mjSTATE_QVEL: 'qvel',
|
||||||
mujoco.mjtState.mjSTATE_ACT: 'act',
|
mujoco.mjtState.mjSTATE_ACT: 'act',
|
||||||
|
mujoco.mjtState.mjSTATE_HISTORY: 'history',
|
||||||
mujoco.mjtState.mjSTATE_WARMSTART: 'qacc_warmstart',
|
mujoco.mjtState.mjSTATE_WARMSTART: 'qacc_warmstart',
|
||||||
mujoco.mjtState.mjSTATE_CTRL: 'ctrl',
|
mujoco.mjtState.mjSTATE_CTRL: 'ctrl',
|
||||||
mujoco.mjtState.mjSTATE_QFRC_APPLIED: 'qfrc_applied',
|
mujoco.mjtState.mjSTATE_QFRC_APPLIED: 'qfrc_applied',
|
||||||
@@ -1752,6 +1754,7 @@ def _state_elem_size(m: types.Model, state_enum: mujoco.mjtState) -> int:
|
|||||||
'qpos',
|
'qpos',
|
||||||
'qvel',
|
'qvel',
|
||||||
'act',
|
'act',
|
||||||
|
'history',
|
||||||
'qacc_warmstart',
|
'qacc_warmstart',
|
||||||
'ctrl',
|
'ctrl',
|
||||||
'qfrc_applied',
|
'qfrc_applied',
|
||||||
@@ -1767,6 +1770,7 @@ def _state_elem_size(m: types.Model, state_enum: mujoco.mjtState) -> int:
|
|||||||
'qpos': 'nq',
|
'qpos': 'nq',
|
||||||
'qvel': 'nv',
|
'qvel': 'nv',
|
||||||
'act': 'na',
|
'act': 'na',
|
||||||
|
'history': 'nhistory',
|
||||||
'qacc_warmstart': 'nv',
|
'qacc_warmstart': 'nv',
|
||||||
'ctrl': 'nu',
|
'ctrl': 'nu',
|
||||||
'qfrc_applied': 'nv',
|
'qfrc_applied': 'nv',
|
||||||
|
|||||||
@@ -641,7 +641,14 @@ class ModelC(PyTreeNode):
|
|||||||
tendon_treenum: jax.Array
|
tendon_treenum: jax.Array
|
||||||
tendon_treeid: jax.Array
|
tendon_treeid: jax.Array
|
||||||
actuator_plugin: jax.Array
|
actuator_plugin: jax.Array
|
||||||
|
actuator_history: jax.Array
|
||||||
|
actuator_historyadr: jax.Array
|
||||||
|
actuator_delay: jax.Array
|
||||||
sensor_plugin: 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: jax.Array
|
||||||
plugin_stateadr: jax.Array
|
plugin_stateadr: jax.Array
|
||||||
B_rownnz: jax.Array # pylint:disable=invalid-name
|
B_rownnz: jax.Array # pylint:disable=invalid-name
|
||||||
@@ -719,6 +726,7 @@ class Model(PyTreeNode):
|
|||||||
nuserdata: number of elements in userdata
|
nuserdata: number of elements in userdata
|
||||||
nsensordata: number of elements in sensor data vector
|
nsensordata: number of elements in sensor data vector
|
||||||
npluginstate: number of plugin state values
|
npluginstate: number of plugin state values
|
||||||
|
nhistory: number of history buffer elements
|
||||||
opt: physics options
|
opt: physics options
|
||||||
stat: model statistics
|
stat: model statistics
|
||||||
qpos0: qpos values at default pose
|
qpos0: qpos values at default pose
|
||||||
@@ -768,6 +776,7 @@ class Model(PyTreeNode):
|
|||||||
nuserdata: int
|
nuserdata: int
|
||||||
nsensordata: int
|
nsensordata: int
|
||||||
npluginstate: int
|
npluginstate: int
|
||||||
|
nhistory: int
|
||||||
opt: Option
|
opt: Option
|
||||||
stat: Union[Statistic, StatisticWarp]
|
stat: Union[Statistic, StatisticWarp]
|
||||||
qpos0: jax.Array
|
qpos0: jax.Array
|
||||||
@@ -1206,6 +1215,7 @@ class Data(PyTreeNode):
|
|||||||
qpos: position
|
qpos: position
|
||||||
qvel: velocity
|
qvel: velocity
|
||||||
act: actuator activation
|
act: actuator activation
|
||||||
|
history: actuator history buffer
|
||||||
qacc_warmstart: warm start for solver
|
qacc_warmstart: warm start for solver
|
||||||
plugin_state: plugin state values
|
plugin_state: plugin state values
|
||||||
ctrl: control input
|
ctrl: control input
|
||||||
@@ -1255,6 +1265,7 @@ class Data(PyTreeNode):
|
|||||||
qpos: jax.Array
|
qpos: jax.Array
|
||||||
qvel: jax.Array
|
qvel: jax.Array
|
||||||
act: jax.Array
|
act: jax.Array
|
||||||
|
history: jax.Array
|
||||||
qacc_warmstart: jax.Array
|
qacc_warmstart: jax.Array
|
||||||
plugin_state: jax.Array
|
plugin_state: jax.Array
|
||||||
# control:
|
# control:
|
||||||
|
|||||||
@@ -1369,7 +1369,9 @@ Euler integrator, semi-implicit in velocity.
|
|||||||
for i in range(0, 3):
|
for i in range(0, 3):
|
||||||
self.assertEqual(
|
self.assertEqual(
|
||||||
dist[i],
|
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], geom1)
|
||||||
self.assertEqual(geomid[i], geom_ex[i])
|
self.assertEqual(geomid[i], geom_ex[i])
|
||||||
@@ -1717,6 +1719,168 @@ Euler integrator, semi-implicit in velocity.
|
|||||||
self.assertIn(model_path, dependencies)
|
self.assertIn(model_path, dependencies)
|
||||||
self.assertIn(msh_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):
|
def _assert_attributes_equal(self, actual_obj, expected_obj, attr_to_compare):
|
||||||
for name in attr_to_compare:
|
for name in attr_to_compare:
|
||||||
actual_value = getattr(actual_obj, name)
|
actual_value = getattr(actual_obj, name)
|
||||||
|
|||||||
@@ -91,7 +91,7 @@ PYBIND11_MODULE(_functions, pymodule) {
|
|||||||
DEF_WITH_OMITTED_PY_ARGS(traits::mj_printSchema,
|
DEF_WITH_OMITTED_PY_ARGS(traits::mj_printSchema,
|
||||||
"filename", "buffer", "buffer_sz")(
|
"filename", "buffer", "buffer_sz")(
|
||||||
pymodule, [](bool flg_html, bool flg_pad) {
|
pymodule, [](bool flg_html, bool flg_pad) {
|
||||||
constexpr int kBufferSize = 40000;
|
constexpr int kBufferSize = 60000;
|
||||||
auto buffer = std::unique_ptr<char[]>(new char[kBufferSize]);
|
auto buffer = std::unique_ptr<char[]>(new char[kBufferSize]);
|
||||||
const int out_length = InterceptMjErrors(::mj_printSchema)(
|
const int out_length = InterceptMjErrors(::mj_printSchema)(
|
||||||
nullptr, buffer.get(), kBufferSize, flg_html, flg_pad);
|
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);
|
return InterceptMjErrors(::mj_setState)(m, d, state.data(), sig);
|
||||||
});
|
});
|
||||||
Def<traits::mj_copyState>(pymodule);
|
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_setKeyframe>(pymodule);
|
||||||
Def<traits::mj_addContact>(pymodule);
|
Def<traits::mj_addContact>(pymodule);
|
||||||
Def<traits::mj_isPyramidal>(pymodule);
|
Def<traits::mj_isPyramidal>(pymodule);
|
||||||
|
|||||||
@@ -522,20 +522,21 @@ ENUMS: Mapping[str, EnumDecl] = dict([
|
|||||||
('mjSTATE_QPOS', 2),
|
('mjSTATE_QPOS', 2),
|
||||||
('mjSTATE_QVEL', 4),
|
('mjSTATE_QVEL', 4),
|
||||||
('mjSTATE_ACT', 8),
|
('mjSTATE_ACT', 8),
|
||||||
('mjSTATE_WARMSTART', 16),
|
('mjSTATE_HISTORY', 16),
|
||||||
('mjSTATE_CTRL', 32),
|
('mjSTATE_WARMSTART', 32),
|
||||||
('mjSTATE_QFRC_APPLIED', 64),
|
('mjSTATE_CTRL', 64),
|
||||||
('mjSTATE_XFRC_APPLIED', 128),
|
('mjSTATE_QFRC_APPLIED', 128),
|
||||||
('mjSTATE_EQ_ACTIVE', 256),
|
('mjSTATE_XFRC_APPLIED', 256),
|
||||||
('mjSTATE_MOCAP_POS', 512),
|
('mjSTATE_EQ_ACTIVE', 512),
|
||||||
('mjSTATE_MOCAP_QUAT', 1024),
|
('mjSTATE_MOCAP_POS', 1024),
|
||||||
('mjSTATE_USERDATA', 2048),
|
('mjSTATE_MOCAP_QUAT', 2048),
|
||||||
('mjSTATE_PLUGIN', 4096),
|
('mjSTATE_USERDATA', 4096),
|
||||||
('mjNSTATE', 13),
|
('mjSTATE_PLUGIN', 8192),
|
||||||
('mjSTATE_PHYSICS', 14),
|
('mjNSTATE', 14),
|
||||||
('mjSTATE_FULLPHYSICS', 4111),
|
('mjSTATE_PHYSICS', 30),
|
||||||
('mjSTATE_USER', 4064),
|
('mjSTATE_FULLPHYSICS', 8223),
|
||||||
('mjSTATE_INTEGRATION', 8191),
|
('mjSTATE_USER', 8128),
|
||||||
|
('mjSTATE_INTEGRATION', 16383),
|
||||||
]),
|
]),
|
||||||
)),
|
)),
|
||||||
('mjtConstraint',
|
('mjtConstraint',
|
||||||
|
|||||||
@@ -2636,6 +2636,156 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([
|
|||||||
),
|
),
|
||||||
doc='Copy state from src to dst.',
|
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',
|
('mj_setKeyframe',
|
||||||
FunctionDecl(
|
FunctionDecl(
|
||||||
name='mj_setKeyframe',
|
name='mj_setKeyframe',
|
||||||
|
|||||||
@@ -1277,6 +1277,11 @@ STRUCTS: Mapping[str, StructDecl] = dict([
|
|||||||
type=ValueType(name='mjtSize'),
|
type=ValueType(name='mjtSize'),
|
||||||
doc='number of mjtNums in plugin state vector',
|
doc='number of mjtNums in plugin state vector',
|
||||||
),
|
),
|
||||||
|
StructFieldDecl(
|
||||||
|
name='nhistory',
|
||||||
|
type=ValueType(name='mjtSize'),
|
||||||
|
doc='number of mjtNums in history buffer',
|
||||||
|
),
|
||||||
StructFieldDecl(
|
StructFieldDecl(
|
||||||
name='narena',
|
name='narena',
|
||||||
type=ValueType(name='mjtSize'),
|
type=ValueType(name='mjtSize'),
|
||||||
@@ -4157,6 +4162,30 @@ STRUCTS: Mapping[str, StructDecl] = dict([
|
|||||||
doc='group for visibility',
|
doc='group for visibility',
|
||||||
array_extent=('nu',),
|
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(
|
StructFieldDecl(
|
||||||
name='actuator_ctrllimited',
|
name='actuator_ctrllimited',
|
||||||
type=PointerType(
|
type=PointerType(
|
||||||
@@ -4389,6 +4418,38 @@ STRUCTS: Mapping[str, StructDecl] = dict([
|
|||||||
doc='noise standard deviation',
|
doc='noise standard deviation',
|
||||||
array_extent=('nsensor',),
|
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(
|
StructFieldDecl(
|
||||||
name='sensor_user',
|
name='sensor_user',
|
||||||
type=PointerType(
|
type=PointerType(
|
||||||
@@ -5402,6 +5463,14 @@ STRUCTS: Mapping[str, StructDecl] = dict([
|
|||||||
doc='actuator activation',
|
doc='actuator activation',
|
||||||
array_extent=('na',),
|
array_extent=('na',),
|
||||||
),
|
),
|
||||||
|
StructFieldDecl(
|
||||||
|
name='history',
|
||||||
|
type=PointerType(
|
||||||
|
inner_type=ValueType(name='mjtNum'),
|
||||||
|
),
|
||||||
|
doc='history buffer',
|
||||||
|
array_extent=('nhistory',),
|
||||||
|
),
|
||||||
StructFieldDecl(
|
StructFieldDecl(
|
||||||
name='qacc_warmstart',
|
name='qacc_warmstart',
|
||||||
type=PointerType(
|
type=PointerType(
|
||||||
@@ -9173,6 +9242,21 @@ STRUCTS: Mapping[str, StructDecl] = dict([
|
|||||||
type=ValueType(name='int'),
|
type=ValueType(name='int'),
|
||||||
doc='group',
|
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(
|
StructFieldDecl(
|
||||||
name='userdata',
|
name='userdata',
|
||||||
type=PointerType(
|
type=PointerType(
|
||||||
@@ -9268,6 +9352,29 @@ STRUCTS: Mapping[str, StructDecl] = dict([
|
|||||||
type=ValueType(name='double'),
|
type=ValueType(name='double'),
|
||||||
doc='noise stdev',
|
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(
|
StructFieldDecl(
|
||||||
name='userdata',
|
name='userdata',
|
||||||
type=PointerType(
|
type=PointerType(
|
||||||
|
|||||||
@@ -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,
|
void mjd_stepFD(const mjModel* m, mjData* d, mjtNum eps, mjtByte flg_centered,
|
||||||
mjtNum* DyDq, mjtNum* DyDv, mjtNum* DyDa, mjtNum* DyDu,
|
mjtNum* DyDq, mjtNum* DyDv, mjtNum* DyDa, mjtNum* DyDu,
|
||||||
mjtNum* DsDq, mjtNum* DsDv, mjtNum* DsDa, mjtNum* DsDu) {
|
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 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
|
int ndx = 2*nv+na; // row length of Dy Jacobians
|
||||||
mj_markStack(d);
|
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) {
|
if (m->opt.integrator == mjINT_RK4) {
|
||||||
mjERROR("RK4 integrator is not supported");
|
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 nv = m->nv, na = m->na, nu = m->nu, ns = m->nsensordata;
|
||||||
int ndx = 2*nv+na; // row length of state Jacobians
|
int ndx = 2*nv+na; // row length of state Jacobians
|
||||||
|
|||||||
@@ -320,10 +320,17 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
|
|||||||
// any tendon transmission targets with force limits
|
// any tendon transmission targets with force limits
|
||||||
int tendon_frclimited = 0;
|
int tendon_frclimited = 0;
|
||||||
|
|
||||||
// local, clamped copy of ctrl
|
// local copy of ctrl
|
||||||
mj_markStack(d);
|
mj_markStack(d);
|
||||||
mjtNum *ctrl = mjSTACKALLOC(d, nu, mjtNum);
|
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)) {
|
if (!mjDISABLED(mjDSBL_CLAMPCTRL)) {
|
||||||
clampVec(ctrl, m->actuator_ctrlrange, m->actuator_ctrllimited, nu, NULL);
|
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
|
// advance state and time given activation derivatives, acceleration, and optional velocity
|
||||||
static void mj_advance(const mjModel* m, mjData* d,
|
static void mj_advance(const mjModel* m, mjData* d,
|
||||||
const mjtNum* act_dot, const mjtNum* qacc, const mjtNum* qvel) {
|
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
|
// advance activations
|
||||||
if (m->na && !mjDISABLED(mjDSBL_ACTUATION)) {
|
if (m->na && !mjDISABLED(mjDSBL_ACTUATION)) {
|
||||||
int nu = m->nu;
|
|
||||||
for (int i=0; i < nu; i++) {
|
for (int i=0; i < nu; i++) {
|
||||||
int actadr = m->actuator_actadr[i];
|
int actadr = m->actuator_actadr[i];
|
||||||
int actadr_end = actadr + m->actuator_actnum[i];
|
int actadr_end = actadr + m->actuator_actnum[i];
|
||||||
|
|||||||
+63
-4
@@ -224,8 +224,8 @@ void mj_makeModel(mjModel** dest,
|
|||||||
// CHECK SIZE PARAMETERS
|
// CHECK SIZE PARAMETERS
|
||||||
{
|
{
|
||||||
// dummy variables for MJMODEL_SIZES set after mjModel construction
|
// dummy variables for MJMODEL_SIZES set after mjModel construction
|
||||||
int nnames_map=0, nJmom=0, ngravcomp=0, nemax=0, njmax=0;
|
int nnames_map=0, nJmom=0, ngravcomp=0, nemax=0, njmax=0, nconmax=0;
|
||||||
int nconmax=0, nuserdata=0, nsensordata=0, npluginstate=0, narena=0, nbuffer=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
|
// sizes must be non-negative and fit in int, except for the byte arrays texdata and textdata
|
||||||
#define X(name) \
|
#define X(name) \
|
||||||
@@ -243,8 +243,9 @@ void mj_makeModel(mjModel** dest,
|
|||||||
#undef X
|
#undef X
|
||||||
|
|
||||||
// suppress unused variable warnings
|
// suppress unused variable warnings
|
||||||
(void)nnames_map; (void)nJmom; (void)ngravcomp; (void)nemax; (void)njmax;
|
(void)nnames_map; (void)nJmom; (void)ngravcomp; (void)nemax; (void)njmax; (void)nconmax;
|
||||||
(void)nconmax; (void)nuserdata; (void)nsensordata; (void)npluginstate; (void)narena; (void)nbuffer;
|
(void)nuserdata; (void)nsensordata; (void)npluginstate; (void)nhistory; (void)narena;
|
||||||
|
(void)nbuffer;
|
||||||
}
|
}
|
||||||
|
|
||||||
// nbody should always be positive
|
// nbody should always be positive
|
||||||
@@ -1262,6 +1263,12 @@ mjData* mjv_copyData(mjData* dest, const mjModel* m, const mjData* src) {
|
|||||||
|
|
||||||
// clear data, set defaults
|
// clear data, set defaults
|
||||||
static void _resetData(const mjModel* m, mjData* d, unsigned char debug_value) {
|
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
|
//------------------------------ save plugin state and data
|
||||||
mjtNum* plugin_state;
|
mjtNum* plugin_state;
|
||||||
uintptr_t* plugindata;
|
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_pos, 3*m->nmocap);
|
||||||
mju_zero(d->mocap_quat, 4*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
|
// zero out qM, special case because scattering from M skips simple body off-diagonals
|
||||||
mju_zero(d->qM, m->nM);
|
mju_zero(d->qM, m->nM);
|
||||||
|
|
||||||
|
|||||||
@@ -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
|
// print sparse matrix
|
||||||
static void printSparse(const char* str, const mjtNum* mat, int nr,
|
static void printSparse(const char* str, const mjtNum* mat, int nr,
|
||||||
const int* rownnz, const int* rowadr,
|
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("QPOS", m->nq, 1, d->qpos, fp, float_format);
|
||||||
printArray2d("QVEL", m->nv, 1, d->qvel, fp, float_format);
|
printArray2d("QVEL", m->nv, 1, d->qvel, fp, float_format);
|
||||||
printArray2d("ACT", m->na, 1, d->act, 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("QACC_WARMSTART", m->nv, 1, d->qacc_warmstart, fp, float_format);
|
||||||
printArray2d("CTRL", m->nu, 1, d->ctrl, 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);
|
printArray2d("QFRC_APPLIED", m->nv, 1, d->qfrc_applied, fp, float_format);
|
||||||
|
|||||||
@@ -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
|
// compute user sensors: call user callback and apply cutoff
|
||||||
static void compute_user_sensors(const mjModel* m, mjData* d, mjtStage stage) {
|
static void compute_user_sensors(const mjModel* m, mjData* d, mjtStage stage) {
|
||||||
if (mjcb_sensor) {
|
if (mjcb_sensor) {
|
||||||
@@ -1438,13 +1483,14 @@ void mj_sensorPos(const mjModel* m, mjData* d) {
|
|||||||
|
|
||||||
if (m->sensor_needstage[i] == mjSTAGE_POS) {
|
if (m->sensor_needstage[i] == mjSTAGE_POS) {
|
||||||
int adr = m->sensor_adr[i];
|
int adr = m->sensor_adr[i];
|
||||||
|
mjtNum* sensordata = d->sensordata + adr;
|
||||||
|
|
||||||
if (type == mjSENS_USER) {
|
if (type == mjSENS_USER) {
|
||||||
// clear result, compute later
|
// clear result, compute later
|
||||||
mju_zero(d->sensordata + adr, m->sensor_dim[i]);
|
mju_zero(sensordata, m->sensor_dim[i]);
|
||||||
nusersensor++;
|
nusersensor++;
|
||||||
} else {
|
} 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) {
|
if (m->sensor_needstage[i] == mjSTAGE_VEL) {
|
||||||
mjtSensor type = m->sensor_type[i];
|
mjtSensor type = m->sensor_type[i];
|
||||||
int adr = m->sensor_adr[i];
|
int adr = m->sensor_adr[i];
|
||||||
|
mjtNum* sensordata = d->sensordata + adr;
|
||||||
|
|
||||||
if (type == mjSENS_USER) {
|
if (type == mjSENS_USER) {
|
||||||
// call mj_subtreeVel for user sensors
|
// call mj_subtreeVel for user sensors
|
||||||
@@ -1494,10 +1541,10 @@ void mj_sensorVel(const mjModel* m, mjData* d) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
// clear result, compute later
|
// clear result, compute later
|
||||||
mju_zero(d->sensordata + adr, m->sensor_dim[i]);
|
mju_zero(sensordata, m->sensor_dim[i]);
|
||||||
nusersensor++;
|
nusersensor++;
|
||||||
} else {
|
} 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) {
|
if (m->sensor_needstage[i] == mjSTAGE_ACC) {
|
||||||
mjtSensor type = m->sensor_type[i];
|
mjtSensor type = m->sensor_type[i];
|
||||||
int adr = m->sensor_adr[i];
|
int adr = m->sensor_adr[i];
|
||||||
|
mjtNum* sensordata = d->sensordata + adr;
|
||||||
|
|
||||||
if (type == mjSENS_USER) {
|
if (type == mjSENS_USER) {
|
||||||
// call mj_rnePostConstraint for user sensors
|
// call mj_rnePostConstraint for user sensors
|
||||||
@@ -1547,10 +1595,10 @@ void mj_sensorAcc(const mjModel* m, mjData* d) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
// clear result, compute later
|
// clear result, compute later
|
||||||
mju_zero(d->sensordata + adr, m->sensor_dim[i]);
|
mju_zero(sensordata, m->sensor_dim[i]);
|
||||||
nusersensor++;
|
nusersensor++;
|
||||||
} else {
|
} else {
|
||||||
mj_computeSensor(m, d, i, d->sensordata + adr);
|
compute_or_read_sensor(m, d, i, sensordata);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -25,6 +25,9 @@ extern "C" {
|
|||||||
|
|
||||||
//-------------------------------- sensors ---------------------------------------------------------
|
//-------------------------------- 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
|
// position-dependent sensors
|
||||||
MJAPI void mj_sensorPos(const mjModel* m, mjData* d);
|
MJAPI void mj_sensorPos(const mjModel* m, mjData* d);
|
||||||
|
|
||||||
|
|||||||
+113
-1
@@ -132,6 +132,7 @@ static inline int mj_stateElemSize(const mjModel* m, mjtState sig) {
|
|||||||
case mjSTATE_QPOS: return m->nq;
|
case mjSTATE_QPOS: return m->nq;
|
||||||
case mjSTATE_QVEL: return m->nv;
|
case mjSTATE_QVEL: return m->nv;
|
||||||
case mjSTATE_ACT: return m->na;
|
case mjSTATE_ACT: return m->na;
|
||||||
|
case mjSTATE_HISTORY: return m->nhistory;
|
||||||
case mjSTATE_WARMSTART: return m->nv;
|
case mjSTATE_WARMSTART: return m->nv;
|
||||||
case mjSTATE_CTRL: return m->nu;
|
case mjSTATE_CTRL: return m->nu;
|
||||||
case mjSTATE_QFRC_APPLIED: return m->nv;
|
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_QPOS: return d->qpos;
|
||||||
case mjSTATE_QVEL: return d->qvel;
|
case mjSTATE_QVEL: return d->qvel;
|
||||||
case mjSTATE_ACT: return d->act;
|
case mjSTATE_ACT: return d->act;
|
||||||
|
case mjSTATE_HISTORY: return d->history;
|
||||||
case mjSTATE_WARMSTART: return d->qacc_warmstart;
|
case mjSTATE_WARMSTART: return d->qacc_warmstart;
|
||||||
case mjSTATE_CTRL: return d->ctrl;
|
case mjSTATE_CTRL: return d->ctrl;
|
||||||
case mjSTATE_QFRC_APPLIED: return d->qfrc_applied;
|
case mjSTATE_QFRC_APPLIED: return d->qfrc_applied;
|
||||||
@@ -619,7 +621,7 @@ void mj_differentiatePos(const mjModel* m, mjtNum* qvel, mjtNum dt,
|
|||||||
vadr += 3;
|
vadr += 3;
|
||||||
padr += 3;
|
padr += 3;
|
||||||
|
|
||||||
// continute with rotations
|
// continue with rotations
|
||||||
mjFALLTHROUGH;
|
mjFALLTHROUGH;
|
||||||
|
|
||||||
case mjJNT_BALL:
|
case mjJNT_BALL:
|
||||||
@@ -806,3 +808,113 @@ void mju_camIntrinsics(const mjModel* m, int camid,
|
|||||||
// extent only used for orthographic cameras
|
// extent only used for orthographic cameras
|
||||||
*extent = m->cam_fovy[camid];
|
*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);
|
||||||
|
}
|
||||||
|
|||||||
@@ -130,6 +130,29 @@ void mju_camIntrinsics(const mjModel* m, int camid,
|
|||||||
mjtNum* fx, mjtNum* fy, mjtNum* cx, mjtNum* cy,
|
mjtNum* fx, mjtNum* fy, mjtNum* cx, mjtNum* cy,
|
||||||
mjtNum* ortho_extent);
|
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
|
#ifdef __cplusplus
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|||||||
@@ -2249,6 +2249,20 @@ void mjCModel::SetSizes() {
|
|||||||
nsensordata += sensors_[i]->dim;
|
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
|
// nnumericdata
|
||||||
for (int i=0; i < nnumeric; i++) {
|
for (int i=0; i < nnumeric; i++) {
|
||||||
nnumericdata += numerics_[i]->size;
|
nnumericdata += numerics_[i]->size;
|
||||||
@@ -3226,6 +3240,7 @@ void mjCModel::CopyObjects(mjModel* m) {
|
|||||||
m->njmax = njmax;
|
m->njmax = njmax;
|
||||||
m->nconmax = nconmax;
|
m->nconmax = nconmax;
|
||||||
m->nsensordata = nsensordata;
|
m->nsensordata = nsensordata;
|
||||||
|
m->nhistory = nhistory;
|
||||||
m->nuserdata = nuserdata;
|
m->nuserdata = nuserdata;
|
||||||
m->na = na;
|
m->na = na;
|
||||||
|
|
||||||
@@ -3724,6 +3739,7 @@ void mjCModel::CopyObjects(mjModel* m) {
|
|||||||
|
|
||||||
// actuators
|
// actuators
|
||||||
adr = 0;
|
adr = 0;
|
||||||
|
int delay_adr = 0;
|
||||||
for (int i=0; i < nu; i++) {
|
for (int i=0; i < nu; i++) {
|
||||||
// get pointer
|
// get pointer
|
||||||
mjCActuator* pac = actuators_[i];
|
mjCActuator* pac = actuators_[i];
|
||||||
@@ -3741,6 +3757,18 @@ void mjCModel::CopyObjects(mjModel* m) {
|
|||||||
pac->actdim_ = m->actuator_actnum[i];
|
pac->actdim_ = m->actuator_actnum[i];
|
||||||
adr += m->actuator_actnum[i];
|
adr += m->actuator_actnum[i];
|
||||||
m->actuator_group[i] = pac->group;
|
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_ctrllimited[i] = (mjtByte)pac->is_ctrllimited();
|
||||||
m->actuator_forcelimited[i] = (mjtByte)pac->is_forcelimited();
|
m->actuator_forcelimited[i] = (mjtByte)pac->is_forcelimited();
|
||||||
m->actuator_actlimited[i] = (mjtByte)pac->is_actlimited();
|
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_dim[i] = psen->dim;
|
||||||
m->sensor_cutoff[i] = (mjtNum)psen->cutoff;
|
m->sensor_cutoff[i] = (mjtNum)psen->cutoff;
|
||||||
m->sensor_noise[i] = (mjtNum)psen->noise;
|
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);
|
mjuu_copyvec(m->sensor_user+nuser_sensor*i, psen->get_userdata().data(), nuser_sensor);
|
||||||
|
|
||||||
// calculate address and advance
|
// calculate address and advance
|
||||||
|
|||||||
@@ -118,6 +118,7 @@ class mjCModel_ : public mjsElement {
|
|||||||
mjtSize ntexdata; // number of texture bytes
|
mjtSize ntexdata; // number of texture bytes
|
||||||
mjtSize nwrap; // number of wrap objects in all tendon paths
|
mjtSize nwrap; // number of wrap objects in all tendon paths
|
||||||
mjtSize nsensordata; // number of mjtNums in sensor data vector
|
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 nnumericdata; // number of mjtNums in all custom fields
|
||||||
mjtSize ntextdata; // number of chars in all text fields, including 0
|
mjtSize ntextdata; // number of chars in all text fields, including 0
|
||||||
mjtSize ntupledata; // number of objects in all tuple fields
|
mjtSize ntupledata; // number of objects in all tuple fields
|
||||||
|
|||||||
@@ -6960,6 +6960,17 @@ void mjCActuator::Compile(void) {
|
|||||||
throw mjCError(this, "plugin '%s' does not support actuators", pplugin->name);
|
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");
|
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
|
// Find referenced object
|
||||||
ResolveReferences(model);
|
ResolveReferences(model);
|
||||||
|
|
||||||
|
|||||||
@@ -43,6 +43,7 @@ extern const int mark_sz;
|
|||||||
extern const int dyn_sz;
|
extern const int dyn_sz;
|
||||||
extern const int gain_sz;
|
extern const int gain_sz;
|
||||||
extern const int bias_sz;
|
extern const int bias_sz;
|
||||||
|
extern const int interp_sz;
|
||||||
extern const int stage_sz;
|
extern const int stage_sz;
|
||||||
extern const int datatype_sz;
|
extern const int datatype_sz;
|
||||||
extern const int camout_sz;
|
extern const int camout_sz;
|
||||||
@@ -74,6 +75,7 @@ extern const mjMap mark_map[];
|
|||||||
extern const mjMap dyn_map[];
|
extern const mjMap dyn_map[];
|
||||||
extern const mjMap gain_map[];
|
extern const mjMap gain_map[];
|
||||||
extern const mjMap bias_map[];
|
extern const mjMap bias_map[];
|
||||||
|
extern const mjMap interp_map[];
|
||||||
extern const mjMap stage_map[];
|
extern const mjMap stage_map[];
|
||||||
extern const mjMap datatype_map[];
|
extern const mjMap datatype_map[];
|
||||||
extern const mjMap condata_map[];
|
extern const mjMap condata_map[];
|
||||||
|
|||||||
@@ -182,29 +182,29 @@ std::vector<const char*> MJCF[nMJCF] = {
|
|||||||
"frictionloss", "springlength", "width", "material",
|
"frictionloss", "springlength", "width", "material",
|
||||||
"margin", "stiffness", "damping", "rgba", "user"},
|
"margin", "stiffness", "damping", "rgba", "user"},
|
||||||
{"general", "?", "ctrllimited", "forcelimited", "actlimited", "ctrlrange",
|
{"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"},
|
"dyntype", "gaintype", "biastype", "dynprm", "gainprm", "biasprm", "actearly"},
|
||||||
{"motor", "?", "ctrllimited", "forcelimited", "ctrlrange", "forcerange",
|
{"motor", "?", "ctrllimited", "forcelimited", "ctrlrange", "forcerange",
|
||||||
"gear", "cranklength", "user", "group"},
|
"gear", "cranklength", "user", "group", "nsample", "interp", "delay"},
|
||||||
{"position", "?", "ctrllimited", "forcelimited", "ctrlrange", "inheritrange",
|
{"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",
|
{"velocity", "?", "ctrllimited", "forcelimited", "ctrlrange", "forcerange",
|
||||||
"gear", "cranklength", "user", "group", "kv"},
|
"gear", "cranklength", "user", "group", "nsample", "interp", "delay", "kv"},
|
||||||
{"intvelocity", "?", "ctrllimited", "forcelimited",
|
{"intvelocity", "?", "ctrllimited", "forcelimited",
|
||||||
"ctrlrange", "forcerange", "actrange", "inheritrange",
|
"ctrlrange", "forcerange", "actrange", "inheritrange",
|
||||||
"gear", "cranklength", "user", "group",
|
"gear", "cranklength", "user", "group", "nsample", "interp", "delay",
|
||||||
"kp", "kv", "dampratio"},
|
"kp", "kv", "dampratio"},
|
||||||
{"damper", "?", "forcelimited", "ctrlrange", "forcerange",
|
{"damper", "?", "forcelimited", "ctrlrange", "forcerange",
|
||||||
"gear", "cranklength", "user", "group", "kv"},
|
"gear", "cranklength", "user", "group", "nsample", "interp", "delay", "kv"},
|
||||||
{"cylinder", "?", "ctrllimited", "forcelimited", "ctrlrange", "forcerange",
|
{"cylinder", "?", "ctrllimited", "forcelimited", "ctrlrange", "forcerange",
|
||||||
"gear", "cranklength", "user", "group",
|
"gear", "cranklength", "user", "group", "nsample", "interp", "delay",
|
||||||
"timeconst", "area", "diameter", "bias"},
|
"timeconst", "area", "diameter", "bias"},
|
||||||
{"muscle", "?", "ctrllimited", "forcelimited", "ctrlrange", "forcerange",
|
{"muscle", "?", "ctrllimited", "forcelimited", "ctrlrange", "forcerange",
|
||||||
"gear", "cranklength", "user", "group",
|
"gear", "cranklength", "user", "group", "nsample", "interp", "delay",
|
||||||
"timeconst", "range", "force", "scale",
|
"timeconst", "range", "force", "scale",
|
||||||
"lmin", "lmax", "vmax", "fpmax", "fvmax"},
|
"lmin", "lmax", "vmax", "fpmax", "fvmax"},
|
||||||
{"adhesion", "?", "forcelimited", "ctrlrange", "forcerange",
|
{"adhesion", "?", "forcelimited", "ctrlrange", "forcerange",
|
||||||
"gain", "user", "group"},
|
"gain", "user", "group", "nsample", "interp", "delay"},
|
||||||
{">"},
|
{">"},
|
||||||
|
|
||||||
{"extension", "*"},
|
{"extension", "*"},
|
||||||
@@ -389,51 +389,51 @@ std::vector<const char*> MJCF[nMJCF] = {
|
|||||||
|
|
||||||
{"actuator", "*"},
|
{"actuator", "*"},
|
||||||
{"<"},
|
{"<"},
|
||||||
{"general", "*", "name", "class", "group",
|
{"general", "*", "name", "class", "group", "nsample", "interp", "delay",
|
||||||
"ctrllimited", "forcelimited", "actlimited", "ctrlrange", "forcerange", "actrange",
|
"ctrllimited", "forcelimited", "actlimited", "ctrlrange", "forcerange", "actrange",
|
||||||
"lengthrange", "gear", "cranklength", "user",
|
"lengthrange", "gear", "cranklength", "user",
|
||||||
"joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite",
|
"joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite",
|
||||||
"body", "actdim", "dyntype", "gaintype", "biastype", "dynprm", "gainprm", "biasprm",
|
"body", "actdim", "dyntype", "gaintype", "biastype", "dynprm", "gainprm", "biasprm",
|
||||||
"actearly"},
|
"actearly"},
|
||||||
{"motor", "*", "name", "class", "group",
|
{"motor", "*", "name", "class", "group", "nsample", "interp", "delay",
|
||||||
"ctrllimited", "forcelimited", "ctrlrange", "forcerange",
|
"ctrllimited", "forcelimited", "ctrlrange", "forcerange",
|
||||||
"lengthrange", "gear", "cranklength", "user",
|
"lengthrange", "gear", "cranklength", "user",
|
||||||
"joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite"},
|
"joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite"},
|
||||||
{"position", "*", "name", "class", "group",
|
{"position", "*", "name", "class", "group", "nsample", "interp", "delay",
|
||||||
"ctrllimited", "forcelimited", "ctrlrange", "inheritrange", "forcerange",
|
"ctrllimited", "forcelimited", "ctrlrange", "inheritrange", "forcerange",
|
||||||
"lengthrange", "gear", "cranklength", "user",
|
"lengthrange", "gear", "cranklength", "user",
|
||||||
"joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite",
|
"joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite",
|
||||||
"kp", "kv", "dampratio", "timeconst"},
|
"kp", "kv", "dampratio", "timeconst"},
|
||||||
{"velocity", "*", "name", "class", "group",
|
{"velocity", "*", "name", "class", "group", "nsample", "interp", "delay",
|
||||||
"ctrllimited", "forcelimited", "ctrlrange", "forcerange",
|
"ctrllimited", "forcelimited", "ctrlrange", "forcerange",
|
||||||
"lengthrange", "gear", "cranklength", "user",
|
"lengthrange", "gear", "cranklength", "user",
|
||||||
"joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite",
|
"joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite",
|
||||||
"kv"},
|
"kv"},
|
||||||
{"intvelocity", "*", "name", "class", "group",
|
{"intvelocity", "*", "name", "class", "group", "nsample", "interp", "delay",
|
||||||
"ctrllimited", "forcelimited",
|
"ctrllimited", "forcelimited",
|
||||||
"ctrlrange", "forcerange", "actrange", "inheritrange", "lengthrange",
|
"ctrlrange", "forcerange", "actrange", "inheritrange", "lengthrange",
|
||||||
"gear", "cranklength", "user",
|
"gear", "cranklength", "user",
|
||||||
"joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite",
|
"joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite",
|
||||||
"kp", "kv", "dampratio"},
|
"kp", "kv", "dampratio"},
|
||||||
{"damper", "*", "name", "class", "group",
|
{"damper", "*", "name", "class", "group", "nsample", "interp", "delay",
|
||||||
"forcelimited", "ctrlrange", "forcerange",
|
"forcelimited", "ctrlrange", "forcerange",
|
||||||
"lengthrange", "gear", "cranklength", "user",
|
"lengthrange", "gear", "cranklength", "user",
|
||||||
"joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite",
|
"joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite",
|
||||||
"kv"},
|
"kv"},
|
||||||
{"cylinder", "*", "name", "class", "group",
|
{"cylinder", "*", "name", "class", "group", "nsample", "interp", "delay",
|
||||||
"ctrllimited", "forcelimited", "ctrlrange", "forcerange",
|
"ctrllimited", "forcelimited", "ctrlrange", "forcerange",
|
||||||
"lengthrange", "gear", "cranklength", "user",
|
"lengthrange", "gear", "cranklength", "user",
|
||||||
"joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite",
|
"joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite",
|
||||||
"timeconst", "area", "diameter", "bias"},
|
"timeconst", "area", "diameter", "bias"},
|
||||||
{"muscle", "*", "name", "class", "group",
|
{"muscle", "*", "name", "class", "group", "nsample", "interp", "delay",
|
||||||
"ctrllimited", "forcelimited", "ctrlrange", "forcerange",
|
"ctrllimited", "forcelimited", "ctrlrange", "forcerange",
|
||||||
"lengthrange", "gear", "cranklength", "user",
|
"lengthrange", "gear", "cranklength", "user",
|
||||||
"joint", "jointinparent", "tendon", "slidersite", "cranksite",
|
"joint", "jointinparent", "tendon", "slidersite", "cranksite",
|
||||||
"timeconst", "tausmooth", "range", "force", "scale",
|
"timeconst", "tausmooth", "range", "force", "scale",
|
||||||
"lmin", "lmax", "vmax", "fpmax", "fvmax"},
|
"lmin", "lmax", "vmax", "fpmax", "fvmax"},
|
||||||
{"adhesion", "*", "name", "class", "group",
|
{"adhesion", "*", "name", "class", "group", "nsample", "interp", "delay",
|
||||||
"forcelimited", "ctrlrange", "forcerange", "user", "body", "gain"},
|
"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",
|
"ctrllimited", "forcelimited", "actlimited", "ctrlrange", "forcerange", "actrange",
|
||||||
"lengthrange", "gear", "cranklength", "joint", "jointinparent",
|
"lengthrange", "gear", "cranklength", "joint", "jointinparent",
|
||||||
"site", "actdim", "dyntype", "dynprm", "tendon", "cranksite", "slidersite", "user",
|
"site", "actdim", "dyntype", "dynprm", "tendon", "cranksite", "slidersite", "user",
|
||||||
@@ -445,56 +445,56 @@ std::vector<const char*> MJCF[nMJCF] = {
|
|||||||
|
|
||||||
{"sensor", "*"},
|
{"sensor", "*"},
|
||||||
{"<"},
|
{"<"},
|
||||||
{"touch", "*", "name", "site", "cutoff", "noise", "user"},
|
{"touch", "*", "name", "site", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
|
||||||
{"accelerometer", "*", "name", "site", "cutoff", "noise", "user"},
|
{"accelerometer", "*", "name", "site", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
|
||||||
{"velocimeter", "*", "name", "site", "cutoff", "noise", "user"},
|
{"velocimeter", "*", "name", "site", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
|
||||||
{"gyro", "*", "name", "site", "cutoff", "noise", "user"},
|
{"gyro", "*", "name", "site", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
|
||||||
{"force", "*", "name", "site", "cutoff", "noise", "user"},
|
{"force", "*", "name", "site", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
|
||||||
{"torque", "*", "name", "site", "cutoff", "noise", "user"},
|
{"torque", "*", "name", "site", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
|
||||||
{"magnetometer", "*", "name", "site", "cutoff", "noise", "user"},
|
{"magnetometer", "*", "name", "site", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
|
||||||
{"camprojection", "*", "name", "site", "camera", "cutoff", "noise", "user"},
|
{"camprojection", "*", "name", "site", "camera", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
|
||||||
{"rangefinder", "*", "name", "site", "camera", "data", "cutoff", "noise", "user"},
|
{"rangefinder", "*", "name", "site", "camera", "data", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
|
||||||
{"jointpos", "*", "name", "joint", "cutoff", "noise", "user"},
|
{"jointpos", "*", "name", "joint", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
|
||||||
{"jointvel", "*", "name", "joint", "cutoff", "noise", "user"},
|
{"jointvel", "*", "name", "joint", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
|
||||||
{"tendonpos", "*", "name", "tendon", "cutoff", "noise", "user"},
|
{"tendonpos", "*", "name", "tendon", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
|
||||||
{"tendonvel", "*", "name", "tendon", "cutoff", "noise", "user"},
|
{"tendonvel", "*", "name", "tendon", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
|
||||||
{"actuatorpos", "*", "name", "actuator", "cutoff", "noise", "user"},
|
{"actuatorpos", "*", "name", "actuator", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
|
||||||
{"actuatorvel", "*", "name", "actuator", "cutoff", "noise", "user"},
|
{"actuatorvel", "*", "name", "actuator", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
|
||||||
{"actuatorfrc", "*", "name", "actuator", "cutoff", "noise", "user"},
|
{"actuatorfrc", "*", "name", "actuator", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
|
||||||
{"jointactuatorfrc", "*", "name", "joint", "cutoff", "noise", "user"},
|
{"jointactuatorfrc", "*", "name", "joint", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
|
||||||
{"tendonactuatorfrc", "*", "name", "tendon", "cutoff", "noise", "user"},
|
{"tendonactuatorfrc", "*", "name", "tendon", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
|
||||||
{"ballquat", "*", "name", "joint", "cutoff", "noise", "user"},
|
{"ballquat", "*", "name", "joint", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
|
||||||
{"ballangvel", "*", "name", "joint", "cutoff", "noise", "user"},
|
{"ballangvel", "*", "name", "joint", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
|
||||||
{"jointlimitpos", "*", "name", "joint", "cutoff", "noise", "user"},
|
{"jointlimitpos", "*", "name", "joint", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
|
||||||
{"jointlimitvel", "*", "name", "joint", "cutoff", "noise", "user"},
|
{"jointlimitvel", "*", "name", "joint", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
|
||||||
{"jointlimitfrc", "*", "name", "joint", "cutoff", "noise", "user"},
|
{"jointlimitfrc", "*", "name", "joint", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
|
||||||
{"tendonlimitpos", "*", "name", "tendon", "cutoff", "noise", "user"},
|
{"tendonlimitpos", "*", "name", "tendon", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
|
||||||
{"tendonlimitvel", "*", "name", "tendon", "cutoff", "noise", "user"},
|
{"tendonlimitvel", "*", "name", "tendon", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
|
||||||
{"tendonlimitfrc", "*", "name", "tendon", "cutoff", "noise", "user"},
|
{"tendonlimitfrc", "*", "name", "tendon", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
|
||||||
{"framepos", "*", "name", "objtype", "objname", "reftype", "refname", "cutoff", "noise", "user"},
|
{"framepos", "*", "name", "objtype", "objname", "reftype", "refname", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
|
||||||
{"framequat", "*", "name", "objtype", "objname", "reftype", "refname", "cutoff", "noise", "user"},
|
{"framequat", "*", "name", "objtype", "objname", "reftype", "refname", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
|
||||||
{"framexaxis", "*", "name", "objtype", "objname", "reftype", "refname", "cutoff", "noise", "user"},
|
{"framexaxis", "*", "name", "objtype", "objname", "reftype", "refname", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
|
||||||
{"frameyaxis", "*", "name", "objtype", "objname", "reftype", "refname", "cutoff", "noise", "user"},
|
{"frameyaxis", "*", "name", "objtype", "objname", "reftype", "refname", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
|
||||||
{"framezaxis", "*", "name", "objtype", "objname", "reftype", "refname", "cutoff", "noise", "user"},
|
{"framezaxis", "*", "name", "objtype", "objname", "reftype", "refname", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
|
||||||
{"framelinvel", "*", "name", "objtype", "objname", "reftype", "refname", "cutoff", "noise", "user"},
|
{"framelinvel", "*", "name", "objtype", "objname", "reftype", "refname", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
|
||||||
{"frameangvel", "*", "name", "objtype", "objname", "reftype", "refname", "cutoff", "noise", "user"},
|
{"frameangvel", "*", "name", "objtype", "objname", "reftype", "refname", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
|
||||||
{"framelinacc", "*", "name", "objtype", "objname", "cutoff", "noise", "user"},
|
{"framelinacc", "*", "name", "objtype", "objname", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
|
||||||
{"frameangacc", "*", "name", "objtype", "objname", "cutoff", "noise", "user"},
|
{"frameangacc", "*", "name", "objtype", "objname", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
|
||||||
{"subtreecom", "*", "name", "body", "cutoff", "noise", "user"},
|
{"subtreecom", "*", "name", "body", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
|
||||||
{"subtreelinvel", "*", "name", "body", "cutoff", "noise", "user"},
|
{"subtreelinvel", "*", "name", "body", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
|
||||||
{"subtreeangmom", "*", "name", "body", "cutoff", "noise", "user"},
|
{"subtreeangmom", "*", "name", "body", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
|
||||||
{"insidesite", "*", "name", "site", "objtype", "objname", "cutoff", "noise", "user"},
|
{"insidesite", "*", "name", "site", "objtype", "objname", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
|
||||||
{"distance", "*", "name", "geom1", "geom2", "body1", "body2", "cutoff", "noise", "user"},
|
{"distance", "*", "name", "geom1", "geom2", "body1", "body2", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
|
||||||
{"normal", "*", "name", "geom1", "geom2", "body1", "body2", "cutoff", "noise", "user"},
|
{"normal", "*", "name", "geom1", "geom2", "body1", "body2", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
|
||||||
{"fromto", "*", "name", "geom1", "geom2", "body1", "body2", "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",
|
{"contact", "*", "name", "geom1", "geom2", "body1", "body2", "subtree1", "subtree2", "site",
|
||||||
"num", "data", "reduce", "cutoff", "noise", "user"},
|
"num", "data", "reduce", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
|
||||||
{"e_potential", "*", "name", "cutoff", "noise", "user"},
|
{"e_potential", "*", "name", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
|
||||||
{"e_kinetic", "*", "name", "cutoff", "noise", "user"},
|
{"e_kinetic", "*", "name", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
|
||||||
{"clock", "*", "name", "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",
|
{"user", "*", "name", "objtype", "objname", "datatype", "needstage",
|
||||||
"dim", "cutoff", "noise", "user"},
|
"dim", "cutoff", "noise", "user"},
|
||||||
{"tactile", "*", "name", "geom", "mesh", "user"},
|
|
||||||
{"plugin", "*", "name", "plugin", "instance", "cutoff", "objtype", "objname", "reftype", "refname",
|
{"plugin", "*", "name", "plugin", "instance", "cutoff", "objtype", "objname", "reftype", "refname",
|
||||||
"user"},
|
"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
|
// stage type
|
||||||
const int stage_sz = 4;
|
const int stage_sz = 4;
|
||||||
const mjMap stage_map[stage_sz] = {
|
const mjMap stage_map[stage_sz] = {
|
||||||
@@ -2285,6 +2294,9 @@ void mjXReader::OneActuator(XMLElement* elem, mjsActuator* actuator) {
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
ReadAttrInt(elem, "group", &actuator->group);
|
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, "ctrllimited", &actuator->ctrllimited, TFAuto_map, 3);
|
||||||
MapValue(elem, "forcelimited", &actuator->forcelimited, TFAuto_map, 3);
|
MapValue(elem, "forcelimited", &actuator->forcelimited, TFAuto_map, 3);
|
||||||
MapValue(elem, "actlimited", &actuator->actlimited, 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, "cutoff", 1, &sensor->cutoff, text);
|
||||||
ReadAttr(elem, "noise", 1, &sensor->noise, 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)) {
|
if (ReadVector(elem, "user", userdata, text)) {
|
||||||
mjs_setDouble(sensor->userdata, userdata.data(), userdata.size());
|
mjs_setDouble(sensor->userdata, userdata.data(), userdata.size());
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -825,6 +825,9 @@ void mjXWriter::OneActuator(XMLElement* elem, const mjCActuator* actuator, mjCDe
|
|||||||
|
|
||||||
// defaults and regular
|
// defaults and regular
|
||||||
WriteAttrInt(elem, "group", actuator->group, def->Actuator().group);
|
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);
|
WriteAttrKey(elem, "ctrllimited", TFAuto_map, 3, actuator->ctrllimited, def->Actuator().ctrllimited);
|
||||||
WriteAttr(elem, "ctrlrange", 2, actuator->ctrlrange, def->Actuator().ctrlrange);
|
WriteAttr(elem, "ctrlrange", 2, actuator->ctrlrange, def->Actuator().ctrlrange);
|
||||||
WriteAttrKey(elem, "forcelimited", TFAuto_map, 3, actuator->forcelimited, def->Actuator().forcelimited);
|
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) {
|
if (sensor->type != mjSENS_PLUGIN) {
|
||||||
WriteAttr(elem, "noise", 1, &sensor->noise, &zero);
|
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());
|
WriteVector(elem, "user", sensor->get_userdata());
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -1513,5 +1513,109 @@ TEST_F(ActuatorTest, TendonActuatorForceRange) {
|
|||||||
mj_deleteModel(model);
|
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
|
||||||
} // namespace mujoco
|
} // namespace mujoco
|
||||||
|
|||||||
@@ -24,6 +24,7 @@
|
|||||||
#include <mujoco/mjmodel.h>
|
#include <mujoco/mjmodel.h>
|
||||||
#include <mujoco/mjtnum.h>
|
#include <mujoco/mjtnum.h>
|
||||||
#include <mujoco/mujoco.h>
|
#include <mujoco/mujoco.h>
|
||||||
|
#include "src/engine/engine_support.h"
|
||||||
#include "src/engine/engine_util_blas.h"
|
#include "src/engine/engine_util_blas.h"
|
||||||
#include "src/engine/engine_util_spatial.h"
|
#include "src/engine/engine_util_spatial.h"
|
||||||
#include "test/fixture.h"
|
#include "test/fixture.h"
|
||||||
@@ -1193,5 +1194,402 @@ TEST_F(SensorTest, RFCamera) {
|
|||||||
mj_deleteModel(model);
|
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
|
||||||
} // namespace mujoco
|
} // namespace mujoco
|
||||||
|
|||||||
@@ -692,7 +692,9 @@ TEST_F(SupportTest, DifferentiatePosSubQuat) {
|
|||||||
|
|
||||||
static const char* const kDefaultModel = "testdata/model.xml";
|
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);
|
const std::string xml_path = GetTestDataFilePath(kDefaultModel);
|
||||||
mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0);
|
mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0);
|
||||||
mjData* data = mj_makeData(model);
|
mjData* data = mj_makeData(model);
|
||||||
@@ -745,7 +747,58 @@ TEST_F(SupportTest, GetSetStateStepEqual) {
|
|||||||
mj_deleteModel(model);
|
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);
|
const std::string xml_path = GetTestDataFilePath(kDefaultModel);
|
||||||
mjModel* m = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0);
|
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->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->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->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];
|
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->qpos, m->nq), AsVector(src->qpos, m->nq));
|
||||||
EXPECT_EQ(AsVector(dst->qvel, m->nv), AsVector(src->qvel, m->nv));
|
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->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));
|
EXPECT_EQ(AsVector(dst->eq_active, m->neq), AsVector(src->eq_active, m->neq));
|
||||||
|
|
||||||
// check non-copied components (CTRL not in signature)
|
// check non-copied components (CTRL not in signature)
|
||||||
@@ -790,7 +846,7 @@ TEST_F(SupportTest, CopyState) {
|
|||||||
mj_deleteModel(m);
|
mj_deleteModel(m);
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(SupportTest, ExtractState) {
|
TEST_F(StateTest, ExtractState) {
|
||||||
const std::string xml_path = GetTestDataFilePath(kDefaultModel);
|
const std::string xml_path = GetTestDataFilePath(kDefaultModel);
|
||||||
mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0);
|
mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0);
|
||||||
mjData* data = mj_makeData(model);
|
mjData* data = mj_makeData(model);
|
||||||
@@ -809,7 +865,8 @@ TEST_F(SupportTest, ExtractState) {
|
|||||||
mj_step(model, data);
|
mj_step(model, data);
|
||||||
|
|
||||||
// take a state that will be used as src
|
// 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);
|
int srcsize = mj_stateSize(model, srcsig);
|
||||||
vector<mjtNum> srcstate(srcsize);
|
vector<mjtNum> srcstate(srcsize);
|
||||||
mj_getState(model, data, srcstate.data(), srcsig);
|
mj_getState(model, data, srcstate.data(), srcsig);
|
||||||
@@ -835,6 +892,14 @@ TEST_F(SupportTest, ExtractState) {
|
|||||||
EXPECT_EQ(AsVector(dststate2.data() + model->nq, model->nu),
|
EXPECT_EQ(AsVector(dststate2.data() + model->nq, model->nu),
|
||||||
AsVector(data->ctrl, 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
|
// test that an error is correctly raised if dstsig is not a subset of srcsig
|
||||||
static int error_count;
|
static int error_count;
|
||||||
static char last_error_msg[128];
|
static char last_error_msg[128];
|
||||||
@@ -1202,5 +1267,205 @@ TEST_F(SupportTest, ContactSensorDim) {
|
|||||||
EXPECT_EQ(mju_condataSize(dataSpec), 1+3+1+3+3);
|
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
|
||||||
} // namespace mujoco
|
} // namespace mujoco
|
||||||
|
|
||||||
|
|||||||
Vendored
+20
@@ -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>
|
||||||
@@ -973,6 +973,48 @@ TEST_F(MujocoTest, ConvertSpringdamper) {
|
|||||||
EXPECT_THAT(str.data(), HasSubstr("damping"));
|
EXPECT_THAT(str.data(), HasSubstr("damping"));
|
||||||
EXPECT_THAT(str.data(), HasSubstr("stiffness"));
|
EXPECT_THAT(str.data(), HasSubstr("stiffness"));
|
||||||
mj_deleteModel(model);
|
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
|
} // namespace
|
||||||
|
|||||||
@@ -626,6 +626,66 @@ TEST_F(SensorTest, OjbtypeParsedButNotRequired) {
|
|||||||
mj_deleteModel(model);
|
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 -----------------------------------------
|
// ------------- test capsule inertias -----------------------------------------
|
||||||
|
|
||||||
static const char* const kCapsuleInertiaPath =
|
static const char* const kCapsuleInertiaPath =
|
||||||
|
|||||||
@@ -3267,5 +3267,95 @@ TEST_F(XMLReaderTest, CameraOutputDefault) {
|
|||||||
mj_deleteModel(model);
|
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
|
||||||
} // namespace mujoco
|
} // namespace mujoco
|
||||||
|
|||||||
@@ -1720,5 +1720,76 @@ TEST_F(XMLWriterTest, WritesCameraOutputDefault) {
|
|||||||
mj_deleteModel(model);
|
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
|
||||||
} // namespace mujoco
|
} // namespace mujoco
|
||||||
|
|||||||
@@ -4951,6 +4951,7 @@ public unsafe struct mjData_ {
|
|||||||
public double* qpos;
|
public double* qpos;
|
||||||
public double* qvel;
|
public double* qvel;
|
||||||
public double* act;
|
public double* act;
|
||||||
|
public double* history;
|
||||||
public double* qacc_warmstart;
|
public double* qacc_warmstart;
|
||||||
public double* plugin_state;
|
public double* plugin_state;
|
||||||
public double* ctrl;
|
public double* ctrl;
|
||||||
@@ -5377,6 +5378,7 @@ public unsafe struct mjModel_ {
|
|||||||
public Int64 nuserdata;
|
public Int64 nuserdata;
|
||||||
public Int64 nsensordata;
|
public Int64 nsensordata;
|
||||||
public Int64 npluginstate;
|
public Int64 npluginstate;
|
||||||
|
public Int64 nhistory;
|
||||||
public Int64 narena;
|
public Int64 narena;
|
||||||
public Int64 nbuffer;
|
public Int64 nbuffer;
|
||||||
public mjOption_ opt;
|
public mjOption_ opt;
|
||||||
@@ -5739,6 +5741,9 @@ public unsafe struct mjModel_ {
|
|||||||
public int* actuator_actadr;
|
public int* actuator_actadr;
|
||||||
public int* actuator_actnum;
|
public int* actuator_actnum;
|
||||||
public int* actuator_group;
|
public int* actuator_group;
|
||||||
|
public int* actuator_history;
|
||||||
|
public int* actuator_historyadr;
|
||||||
|
public double* actuator_delay;
|
||||||
public byte* actuator_ctrllimited;
|
public byte* actuator_ctrllimited;
|
||||||
public byte* actuator_forcelimited;
|
public byte* actuator_forcelimited;
|
||||||
public byte* actuator_actlimited;
|
public byte* actuator_actlimited;
|
||||||
@@ -5768,6 +5773,10 @@ public unsafe struct mjModel_ {
|
|||||||
public int* sensor_adr;
|
public int* sensor_adr;
|
||||||
public double* sensor_cutoff;
|
public double* sensor_cutoff;
|
||||||
public double* sensor_noise;
|
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 double* sensor_user;
|
||||||
public int* sensor_plugin;
|
public int* sensor_plugin;
|
||||||
public int* 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)]
|
[DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)]
|
||||||
public static unsafe extern void mj_copyState(mjModel_* m, mjData_* src, mjData_* dst, int sig);
|
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)]
|
[DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)]
|
||||||
public static unsafe extern void mj_setKeyframe(mjModel_* m, mjData_* d, int k);
|
public static unsafe extern void mj_setKeyframe(mjModel_* m, mjData_* d, int k);
|
||||||
|
|
||||||
|
|||||||
@@ -4120,6 +4120,12 @@ struct MjModel {
|
|||||||
void set_npluginstate(int value) {
|
void set_npluginstate(int value) {
|
||||||
ptr_->npluginstate = static_cast<mjtSize>(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 {
|
int narena() const {
|
||||||
return static_cast<int>(ptr_->narena);
|
return static_cast<int>(ptr_->narena);
|
||||||
}
|
}
|
||||||
@@ -5203,6 +5209,15 @@ struct MjModel {
|
|||||||
emscripten::val actuator_group() const {
|
emscripten::val actuator_group() const {
|
||||||
return emscripten::val(emscripten::typed_memory_view(ptr_->nu, ptr_->actuator_group));
|
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 {
|
emscripten::val actuator_ctrllimited() const {
|
||||||
return emscripten::val(emscripten::typed_memory_view(ptr_->nu, ptr_->actuator_ctrllimited));
|
return emscripten::val(emscripten::typed_memory_view(ptr_->nu, ptr_->actuator_ctrllimited));
|
||||||
}
|
}
|
||||||
@@ -5290,6 +5305,18 @@ struct MjModel {
|
|||||||
emscripten::val sensor_noise() const {
|
emscripten::val sensor_noise() const {
|
||||||
return emscripten::val(emscripten::typed_memory_view(ptr_->nsensor, ptr_->sensor_noise));
|
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 {
|
emscripten::val sensor_user() const {
|
||||||
return emscripten::val(emscripten::typed_memory_view(ptr_->nsensor * ptr_->nuser_sensor, ptr_->sensor_user));
|
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) {
|
void set_group(int value) {
|
||||||
ptr_->group = 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 {
|
mjDoubleVec &userdata() const {
|
||||||
return *(ptr_->userdata);
|
return *(ptr_->userdata);
|
||||||
}
|
}
|
||||||
@@ -6222,6 +6267,27 @@ struct MjsSensor {
|
|||||||
void set_noise(double value) {
|
void set_noise(double value) {
|
||||||
ptr_->noise = 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 {
|
mjDoubleVec &userdata() const {
|
||||||
return *(ptr_->userdata);
|
return *(ptr_->userdata);
|
||||||
}
|
}
|
||||||
@@ -6448,6 +6514,9 @@ struct MjData {
|
|||||||
emscripten::val act() const {
|
emscripten::val act() const {
|
||||||
return emscripten::val(emscripten::typed_memory_view(model->na, ptr_->act));
|
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 {
|
emscripten::val qacc_warmstart() const {
|
||||||
return emscripten::val(emscripten::typed_memory_view(model->nv, ptr_->qacc_warmstart));
|
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());
|
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) {
|
void mj_integratePos_wrapper(const MjModel& m, const val& qpos, const NumberArray& qvel, mjtNum dt) {
|
||||||
UNPACK_VALUE(mjtNum, qpos);
|
UNPACK_VALUE(mjtNum, qpos);
|
||||||
UNPACK_ARRAY(mjtNum, qvel);
|
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());
|
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) {
|
void mj_referenceConstraint_wrapper(const MjModel& m, MjData& d) {
|
||||||
mj_referenceConstraint(m.get(), d.get());
|
mj_referenceConstraint(m.get(), d.get());
|
||||||
}
|
}
|
||||||
@@ -11103,6 +11188,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) {
|
|||||||
.value("mjSTATE_QPOS", mjSTATE_QPOS)
|
.value("mjSTATE_QPOS", mjSTATE_QPOS)
|
||||||
.value("mjSTATE_QVEL", mjSTATE_QVEL)
|
.value("mjSTATE_QVEL", mjSTATE_QVEL)
|
||||||
.value("mjSTATE_ACT", mjSTATE_ACT)
|
.value("mjSTATE_ACT", mjSTATE_ACT)
|
||||||
|
.value("mjSTATE_HISTORY", mjSTATE_HISTORY)
|
||||||
.value("mjSTATE_WARMSTART", mjSTATE_WARMSTART)
|
.value("mjSTATE_WARMSTART", mjSTATE_WARMSTART)
|
||||||
.value("mjSTATE_CTRL", mjSTATE_CTRL)
|
.value("mjSTATE_CTRL", mjSTATE_CTRL)
|
||||||
.value("mjSTATE_QFRC_APPLIED", mjSTATE_QFRC_APPLIED)
|
.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("flg_subtreevel", &MjData::flg_subtreevel, &MjData::set_flg_subtreevel, reference())
|
||||||
.property("geom_xmat", &MjData::geom_xmat)
|
.property("geom_xmat", &MjData::geom_xmat)
|
||||||
.property("geom_xpos", &MjData::geom_xpos)
|
.property("geom_xpos", &MjData::geom_xpos)
|
||||||
|
.property("history", &MjData::history)
|
||||||
.property("iLD", &MjData::iLD)
|
.property("iLD", &MjData::iLD)
|
||||||
.property("iLDiagInv", &MjData::iLDiagInv)
|
.property("iLDiagInv", &MjData::iLDiagInv)
|
||||||
.property("iM", &MjData::iM)
|
.property("iM", &MjData::iM)
|
||||||
@@ -11517,6 +11604,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) {
|
|||||||
.property("actuator_cranklength", &MjModel::actuator_cranklength)
|
.property("actuator_cranklength", &MjModel::actuator_cranklength)
|
||||||
.property("actuator_ctrllimited", &MjModel::actuator_ctrllimited)
|
.property("actuator_ctrllimited", &MjModel::actuator_ctrllimited)
|
||||||
.property("actuator_ctrlrange", &MjModel::actuator_ctrlrange)
|
.property("actuator_ctrlrange", &MjModel::actuator_ctrlrange)
|
||||||
|
.property("actuator_delay", &MjModel::actuator_delay)
|
||||||
.property("actuator_dynprm", &MjModel::actuator_dynprm)
|
.property("actuator_dynprm", &MjModel::actuator_dynprm)
|
||||||
.property("actuator_dyntype", &MjModel::actuator_dyntype)
|
.property("actuator_dyntype", &MjModel::actuator_dyntype)
|
||||||
.property("actuator_forcelimited", &MjModel::actuator_forcelimited)
|
.property("actuator_forcelimited", &MjModel::actuator_forcelimited)
|
||||||
@@ -11525,6 +11613,8 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) {
|
|||||||
.property("actuator_gaintype", &MjModel::actuator_gaintype)
|
.property("actuator_gaintype", &MjModel::actuator_gaintype)
|
||||||
.property("actuator_gear", &MjModel::actuator_gear)
|
.property("actuator_gear", &MjModel::actuator_gear)
|
||||||
.property("actuator_group", &MjModel::actuator_group)
|
.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_length0", &MjModel::actuator_length0)
|
||||||
.property("actuator_lengthrange", &MjModel::actuator_lengthrange)
|
.property("actuator_lengthrange", &MjModel::actuator_lengthrange)
|
||||||
.property("actuator_plugin", &MjModel::actuator_plugin)
|
.property("actuator_plugin", &MjModel::actuator_plugin)
|
||||||
@@ -11859,6 +11949,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) {
|
|||||||
.property("ngravcomp", &MjModel::ngravcomp, &MjModel::set_ngravcomp, reference())
|
.property("ngravcomp", &MjModel::ngravcomp, &MjModel::set_ngravcomp, reference())
|
||||||
.property("nhfield", &MjModel::nhfield, &MjModel::set_nhfield, reference())
|
.property("nhfield", &MjModel::nhfield, &MjModel::set_nhfield, reference())
|
||||||
.property("nhfielddata", &MjModel::nhfielddata, &MjModel::set_nhfielddata, 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("njmax", &MjModel::njmax, &MjModel::set_njmax, reference())
|
||||||
.property("njnt", &MjModel::njnt, &MjModel::set_njnt, reference())
|
.property("njnt", &MjModel::njnt, &MjModel::set_njnt, reference())
|
||||||
.property("nkey", &MjModel::nkey, &MjModel::set_nkey, 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_adr", &MjModel::sensor_adr)
|
||||||
.property("sensor_cutoff", &MjModel::sensor_cutoff)
|
.property("sensor_cutoff", &MjModel::sensor_cutoff)
|
||||||
.property("sensor_datatype", &MjModel::sensor_datatype)
|
.property("sensor_datatype", &MjModel::sensor_datatype)
|
||||||
|
.property("sensor_delay", &MjModel::sensor_delay)
|
||||||
.property("sensor_dim", &MjModel::sensor_dim)
|
.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_intprm", &MjModel::sensor_intprm)
|
||||||
.property("sensor_needstage", &MjModel::sensor_needstage)
|
.property("sensor_needstage", &MjModel::sensor_needstage)
|
||||||
.property("sensor_noise", &MjModel::sensor_noise)
|
.property("sensor_noise", &MjModel::sensor_noise)
|
||||||
@@ -12239,6 +12334,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) {
|
|||||||
.property("cranklength", &MjsActuator::cranklength, &MjsActuator::set_cranklength, reference())
|
.property("cranklength", &MjsActuator::cranklength, &MjsActuator::set_cranklength, reference())
|
||||||
.property("ctrllimited", &MjsActuator::ctrllimited, &MjsActuator::set_ctrllimited, reference())
|
.property("ctrllimited", &MjsActuator::ctrllimited, &MjsActuator::set_ctrllimited, reference())
|
||||||
.property("ctrlrange", &MjsActuator::ctrlrange)
|
.property("ctrlrange", &MjsActuator::ctrlrange)
|
||||||
|
.property("delay", &MjsActuator::delay, &MjsActuator::set_delay, reference())
|
||||||
.property("dynprm", &MjsActuator::dynprm)
|
.property("dynprm", &MjsActuator::dynprm)
|
||||||
.property("dyntype", &MjsActuator::dyntype, &MjsActuator::set_dyntype, reference())
|
.property("dyntype", &MjsActuator::dyntype, &MjsActuator::set_dyntype, reference())
|
||||||
.property("element", &MjsActuator::element, reference())
|
.property("element", &MjsActuator::element, reference())
|
||||||
@@ -12250,7 +12346,9 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) {
|
|||||||
.property("group", &MjsActuator::group, &MjsActuator::set_group, reference())
|
.property("group", &MjsActuator::group, &MjsActuator::set_group, reference())
|
||||||
.property("info", &MjsActuator::info, &MjsActuator::set_info, reference())
|
.property("info", &MjsActuator::info, &MjsActuator::set_info, reference())
|
||||||
.property("inheritrange", &MjsActuator::inheritrange, &MjsActuator::set_inheritrange, reference())
|
.property("inheritrange", &MjsActuator::inheritrange, &MjsActuator::set_inheritrange, reference())
|
||||||
|
.property("interp", &MjsActuator::interp, &MjsActuator::set_interp, reference())
|
||||||
.property("lengthrange", &MjsActuator::lengthrange)
|
.property("lengthrange", &MjsActuator::lengthrange)
|
||||||
|
.property("nsample", &MjsActuator::nsample, &MjsActuator::set_nsample, reference())
|
||||||
.property("plugin", &MjsActuator::plugin, reference())
|
.property("plugin", &MjsActuator::plugin, reference())
|
||||||
.property("refsite", &MjsActuator::refsite, &MjsActuator::set_refsite, reference())
|
.property("refsite", &MjsActuator::refsite, &MjsActuator::set_refsite, reference())
|
||||||
.property("slidersite", &MjsActuator::slidersite, &MjsActuator::set_slidersite, reference())
|
.property("slidersite", &MjsActuator::slidersite, &MjsActuator::set_slidersite, reference())
|
||||||
@@ -12556,12 +12654,16 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) {
|
|||||||
emscripten::class_<MjsSensor>("MjsSensor")
|
emscripten::class_<MjsSensor>("MjsSensor")
|
||||||
.property("cutoff", &MjsSensor::cutoff, &MjsSensor::set_cutoff, reference())
|
.property("cutoff", &MjsSensor::cutoff, &MjsSensor::set_cutoff, reference())
|
||||||
.property("datatype", &MjsSensor::datatype, &MjsSensor::set_datatype, 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("dim", &MjsSensor::dim, &MjsSensor::set_dim, reference())
|
||||||
.property("element", &MjsSensor::element, reference())
|
.property("element", &MjsSensor::element, reference())
|
||||||
.property("info", &MjsSensor::info, &MjsSensor::set_info, 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("intprm", &MjsSensor::intprm)
|
||||||
.property("needstage", &MjsSensor::needstage, &MjsSensor::set_needstage, reference())
|
.property("needstage", &MjsSensor::needstage, &MjsSensor::set_needstage, reference())
|
||||||
.property("noise", &MjsSensor::noise, &MjsSensor::set_noise, 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("objname", &MjsSensor::objname, &MjsSensor::set_objname, reference())
|
||||||
.property("objtype", &MjsSensor::objtype, &MjsSensor::set_objtype, reference())
|
.property("objtype", &MjsSensor::objtype, &MjsSensor::set_objtype, reference())
|
||||||
.property("plugin", &MjsSensor::plugin, reference())
|
.property("plugin", &MjsSensor::plugin, reference())
|
||||||
@@ -12908,6 +13010,8 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) {
|
|||||||
function("mj_getTotalmass", &mj_getTotalmass_wrapper);
|
function("mj_getTotalmass", &mj_getTotalmass_wrapper);
|
||||||
function("mj_id2name", &mj_id2name_wrapper);
|
function("mj_id2name", &mj_id2name_wrapper);
|
||||||
function("mj_implicit", &mj_implicit_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_integratePos", &mj_integratePos_wrapper);
|
||||||
function("mj_invConstraint", &mj_invConstraint_wrapper);
|
function("mj_invConstraint", &mj_invConstraint_wrapper);
|
||||||
function("mj_invPosition", &mj_invPosition_wrapper);
|
function("mj_invPosition", &mj_invPosition_wrapper);
|
||||||
@@ -12951,6 +13055,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) {
|
|||||||
function("mj_rayFlex", &mj_rayFlex_wrapper);
|
function("mj_rayFlex", &mj_rayFlex_wrapper);
|
||||||
function("mj_rayHfield", &mj_rayHfield_wrapper);
|
function("mj_rayHfield", &mj_rayHfield_wrapper);
|
||||||
function("mj_rayMesh", &mj_rayMesh_wrapper);
|
function("mj_rayMesh", &mj_rayMesh_wrapper);
|
||||||
|
function("mj_readCtrl", &mj_readCtrl_wrapper);
|
||||||
function("mj_referenceConstraint", &mj_referenceConstraint_wrapper);
|
function("mj_referenceConstraint", &mj_referenceConstraint_wrapper);
|
||||||
function("mj_resetCallbacks", &mj_resetCallbacks);
|
function("mj_resetCallbacks", &mj_resetCallbacks);
|
||||||
function("mj_resetData", &mj_resetData_wrapper);
|
function("mj_resetData", &mj_resetData_wrapper);
|
||||||
|
|||||||
@@ -184,6 +184,7 @@ _SKIPPED_GETTERS_AND_SETTERS: tuple[str, ...] = (
|
|||||||
|
|
||||||
_SKIPPED_UTILITY_FUNCTIONS: tuple[str, ...] = (
|
_SKIPPED_UTILITY_FUNCTIONS: tuple[str, ...] = (
|
||||||
# go/keep-sorted start
|
# go/keep-sorted start
|
||||||
|
"mj_readSensor",
|
||||||
"mju_getXMLDependencies",
|
"mju_getXMLDependencies",
|
||||||
# go/keep-sorted end
|
# go/keep-sorted end
|
||||||
)
|
)
|
||||||
|
|||||||
@@ -1113,7 +1113,7 @@ describe('MuJoCo WASM Bindings', () => {
|
|||||||
mujoco.mj_stateSize(model!, invalidSig);
|
mujoco.mj_stateSize(model!, invalidSig);
|
||||||
})
|
})
|
||||||
.toThrowError(
|
.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 sig = mujoco.mjtState.mjSTATE_INTEGRATION.value;
|
||||||
const size = mujoco.mj_stateSize(model!, sig);
|
const size = mujoco.mj_stateSize(model!, sig);
|
||||||
|
|||||||
Reference in New Issue
Block a user