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

Merge branch 'google-deepmind:main' into usd-integration

Browse Source
This commit is contained in:
Abhishek Joshi
2024-02-21 07:03:55 -08:00
committed by GitHub
parent a50f002bed 7edacd39cd
commit f721d3900d
70 changed files with 5409 additions and 2358 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/APIreference/functions.rst
+15 -6
View File
@@ -1109,14 +1109,14 @@ mj_addFileVFS
Add file to VFS. The directory argument is optional and can be NULL or empty. Returns 0 on success, 1 when VFS is full,
2 on name collision, or -1 when an internal error occurs.
.. _mj_makeEmptyFileVFS:
.. _mj_addBufferVFS:
mj_makeEmptyFileVFS
~~~~~~~~~~~~~~~~~~~
mj_addBufferVFS
~~~~~~~~~~~~~~~
.. mujoco-include:: mj_makeEmptyFileVFS
.. mujoco-include:: mj_addBufferVFS
Make empty file in VFS, return 0: success, 1: full, 2: repeated name.
Add file to VFS from buffer, return 0: success, 1: full, 2: repeated name, -1: failed to load.
.. _mj_findFileVFS:
@@ -1145,6 +1145,15 @@ mj_deleteVFS
Delete all files from VFS.
.. _mj_makeEmptyFileVFS:
mj_makeEmptyFileVFS
~~~~~~~~~~~~~~~~~~~
.. mujoco-include:: mj_makeEmptyFileVFS
deprecated: use mj_copyBufferVFS.
.. _Initialization:
Initialization
@@ -1280,7 +1289,7 @@ mj_resetDataKeyframe
.. mujoco-include:: mj_resetDataKeyframe
Reset data, set fields from specified keyframe.
Reset data. If 0 <= key < nkey, set fields from specified keyframe.
.. _mj_markStack:
doc/XMLreference.rst
+45 -20
View File
@@ -1218,40 +1218,41 @@ construct simple shapes directly in the XML. For example, a pyramid can be creat
.. code-block:: xml
<asset>
<mesh name="tetrahedron" vertex="0 0 0 1 0 0 0 1 0 0 0 1"/>
<mesh name="tetrahedron" vertex="0 0 0 1 0 0 0 1 0 0 0 1"/>
</asset>
Positioning and orienting is complicated by the fact that vertex data are often designed relative to coordinate frames
whose origin is not inside the mesh. In contrast, MuJoCo expects the origin of a geom's local frame to coincide with the
geometric center of the shape. We resolve this discrepancy by pre-processing the mesh in the compiler, so that it is
centered around (0,0,0) and its principal axes of inertia are the coordinate axes. We also save the translation and
rotation offsets needed to achieve such alignment in :ref:`mjModel.mesh_pos<mjModel>` and
:ref:`mjModel.mesh_quat<mjModel>`. These offsets are then applied to the referencing geom's position and orientation; see
also :at:`mesh` attribute of :ref:`geom <body-geom>` below. Fortunately most meshes used in robot models are designed in
a coordinate frame centered at the joint. This makes the corresponding MJCF model intuitive: we set the body frame at the
joint, so that the joint position is (0,0,0) in the body frame, and simply reference the mesh. Below is an MJCF model
fragment of a forearm, containing all the information needed to put the mesh where one would expect it to be. The body
position is specified relative to the parent body, namely the upper arm (not shown). It is offset by 35 cm which is the
typical length of the human upper arm. If the mesh vertex data were not designed in the above convention, we would have
to use the geom position and orientation (or the new refpos, refquat mechanism) to compensate, but in practice this is
Positioning and orienting is complicated by the fact that vertex data in the source asset are often relative to
coordinate frames whose origin is not inside the mesh. In contrast, MuJoCo expects the origin of a geom's local frame to
coincide with the geometric center of the shape. We resolve this discrepancy by pre-processing the mesh in the compiler,
so that it is centered around (0,0,0) and its principal axes of inertia are the coordinate axes. We save the translation
and rotation offsets applied to the source asset in :ref:`mjModel.mesh_pos<mjModel>` and
:ref:`mjModel.mesh_quat<mjModel>`; these are required if one reads vertex data from the source and needs to re-apply the
transform. These offsets are then composed with the referencing geom's position and orientation; see also the :at:`mesh`
attribute of :ref:`geom <body-geom>` below. Fortunately most meshes used in robot models are designed in a coordinate
frame centered at the joint. This makes the corresponding MJCF model intuitive: we set the body frame at the joint, so
that the joint position is (0,0,0) in the body frame, and simply reference the mesh. Below is an MJCF model fragment of
a forearm, containing all the information needed to put the mesh where one would expect it to be. The body position is
specified relative to the parent body, namely the upper arm (not shown). It is offset by 35 cm which is the typical
length of the human upper arm. If the mesh vertex data were not designed in the above convention, we would have to use
the geom position and orientation (or the :at:`refpos`, :at:`refquat`` mechanism) to compensate, but in practice this is
rarely needed.
.. code-block:: xml
<asset>
<mesh file="forearm.stl"/>
<mesh file="forearm.stl"/>
</asset>
<body pos="0 0 0.35"/>
<joint type="hinge" axis="1 0 0"/>
<geom type="mesh" mesh="forearm"/>
<joint type="hinge" axis="1 0 0"/>
<geom type="mesh" mesh="forearm"/>
</body>
The inertial computation mentioned above is part of an algorithm used not only to center and align the mesh, but also to
infer the mass and inertia of the body to which it is attached. This is done by computing the centroid of the triangle
faces, connecting each face with the centroid to form a triangular pyramid, computing the mass and signed inertia of all
pyramids (considered solid or hollow if :at:`shellinertia` is true) and accumulating them. The sign ensures that
pyramids on the outside of the surfaces are subtracted, as it can occur with concave geometries. This algorithm can be
pyramids (considered solid, or hollow if :at:`shellinertia` is true) and accumulating them. The sign ensures that
pyramids on the outside of the surfaces are subtracted, as can occur with concave geometries. This algorithm can be
found in section 1.3.8 of Computational Geometry in C (Second Edition) by Joseph O'Rourke.
The full list of processing steps applied by the compiler to each mesh is as follows:
@@ -5429,6 +5430,20 @@ This element has one custom attribute in addition to the common attributes:
Damping applied by the actuator.
When using this attribute, it is recommended to use the implicitfast or implicit :ref:`integrators<geIntegration>`.
.. _actuator-position-inheritrange:
:at:`inheritrange`: :at-val:`real, "0"`
Automatically set the actuator's :at:`ctrlrange` to match the transmission target's :at:`range`. The default value
means "disabled". A positive value :at-val:`X` sets the :at:`ctrlrange` around the midpoint of the target range,
scaled by :at-val:`X`. For example if the target joint has :at:`range` of :at-val:`[0, 1]`, then a value of
:at-val:`1.0` will set :at:`ctrlrange` to :at-val:`[0, 1]`; values of :at-val:`0.8` and :at-val:`1.2` will set the
:at:`ctrlrange` to :at-val:`[0.1, 0.9]` and :at-val:`[-0.1, 1.1]`, respectively. Values smaller than 1 are useful for
not hitting the limits; values larger than 1 are useful for maintaining control authority at the limits (being able
to push on them). This attribute is exclusive with :at:`ctrlrange` and available only for joint and tendon
transmissions which have :at:`range` defined. Note that while :at:`inheritrange` is available both as a
:ref:`position<actuator-position>` attribute and in the :ref:`default class<default-position-inheritrange>`,
saved XMLs always convert it to explicit :at:`ctrlrange` at the actuator.
.. _actuator-velocity:
:el-prefix:`actuator/` |-| **velocity** (*)
@@ -5577,10 +5592,16 @@ This element has one custom attribute in addition to the common attributes:
Damping applied by the actuator.
When using this attribute, it is recommended to use the implicitfast or implicit :ref:`integrators<geIntegration>`.
.. _actuator-intvelocity-inheritrange:
:at:`inheritrange`: :at-val:`real, "0"`
Identical to :ref:`position/inheritrange<actuator-position-inheritrange>`, but sets :at:`actrange` (which has the same
length semantics as the transmission target) rather than :at:`ctrlrange` (which has velocity semantics).
.. _actuator-damper:
:el-prefix:`actuator/` |-| **damper** (*)
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
This element is an active damper which produces a force proportional to both velocity and control: ``F = - kv * velocity
* control``, where ``kv`` must be nonnegative. :at:`ctrlrange` is required and must also be nonnegative.
@@ -7727,6 +7748,8 @@ tendon, slidersite, cranksite.
.. _default-position-ctrlrange:
.. _default-position-inheritrange:
.. _default-position-forcerange:
.. _default-position-gear:
@@ -7787,6 +7810,8 @@ refsite, tendon, slidersite, cranksite.
.. _default-intvelocity-actrange:
.. _default-intvelocity-inheritrange:
.. _default-intvelocity-gear:
.. _default-intvelocity-cranklength:
doc/XMLschema.rst
+41 -39
View File
@@ -135,32 +135,6 @@
| :ref:`asset<asset>` | | |
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
| |_| asset |br| |_| |L| | | .. table:: |
| :ref:`texture | \* | :class: mjcf-attributes |
| <asset-texture>` | | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`name<asset-texture-name>` | :ref:`type<asset-texture-type>` | :ref:`content_type<asset-texture-content_type>` | :ref:`file<asset-texture-file>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`gridsize<asset-texture-gridsize>` | :ref:`gridlayout<asset-texture-gridlayout>` | :ref:`fileright<asset-texture-fileright>` | :ref:`fileleft<asset-texture-fileleft>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`fileup<asset-texture-fileup>` | :ref:`filedown<asset-texture-filedown>` | :ref:`filefront<asset-texture-filefront>` | :ref:`fileback<asset-texture-fileback>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`builtin<asset-texture-builtin>` | :ref:`rgb1<asset-texture-rgb1>` | :ref:`rgb2<asset-texture-rgb2>` | :ref:`mark<asset-texture-mark>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`markrgb<asset-texture-markrgb>` | :ref:`random<asset-texture-random>` | :ref:`width<asset-texture-width>` | :ref:`height<asset-texture-height>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`hflip<asset-texture-hflip>` | :ref:`vflip<asset-texture-vflip>` | | | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
| |_| asset |br| |_| |L| | | .. table:: |
| :ref:`hfield | \* | :class: mjcf-attributes |
| <asset-hfield>` | | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`name<asset-hfield-name>` | :ref:`content_type<asset-hfield-content_type>` | :ref:`file<asset-hfield-file>` | :ref:`nrow<asset-hfield-nrow>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`ncol<asset-hfield-ncol>` | :ref:`size<asset-hfield-size>` | :ref:`elevation<asset-hfield-elevation>` | | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
| |_| asset |br| |_| |L| | | .. table:: |
| :ref:`mesh | \* | :class: mjcf-attributes |
| <asset-mesh>` | | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
@@ -186,6 +160,15 @@
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
| |_| asset |br| |_| |L| | | .. table:: |
| :ref:`hfield | \* | :class: mjcf-attributes |
| <asset-hfield>` | | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`name<asset-hfield-name>` | :ref:`content_type<asset-hfield-content_type>` | :ref:`file<asset-hfield-file>` | :ref:`nrow<asset-hfield-nrow>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`ncol<asset-hfield-ncol>` | :ref:`size<asset-hfield-size>` | :ref:`elevation<asset-hfield-elevation>` | | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
| |_| asset |br| |_| |L| | | .. table:: |
| :ref:`skin | \* | :class: mjcf-attributes |
| <asset-skin>` | | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
@@ -206,6 +189,23 @@
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
| |_| asset |br| |_| |L| | | .. table:: |
| :ref:`texture | \* | :class: mjcf-attributes |
| <asset-texture>` | | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`name<asset-texture-name>` | :ref:`type<asset-texture-type>` | :ref:`content_type<asset-texture-content_type>` | :ref:`file<asset-texture-file>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`gridsize<asset-texture-gridsize>` | :ref:`gridlayout<asset-texture-gridlayout>` | :ref:`fileright<asset-texture-fileright>` | :ref:`fileleft<asset-texture-fileleft>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`fileup<asset-texture-fileup>` | :ref:`filedown<asset-texture-filedown>` | :ref:`filefront<asset-texture-filefront>` | :ref:`fileback<asset-texture-fileback>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`builtin<asset-texture-builtin>` | :ref:`rgb1<asset-texture-rgb1>` | :ref:`rgb2<asset-texture-rgb2>` | :ref:`mark<asset-texture-mark>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`markrgb<asset-texture-markrgb>` | :ref:`random<asset-texture-random>` | :ref:`width<asset-texture-width>` | :ref:`height<asset-texture-height>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`hflip<asset-texture-hflip>` | :ref:`vflip<asset-texture-vflip>` | | | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
| |_| asset |br| |_| |L| | | .. table:: |
| :ref:`material | \* | :class: mjcf-attributes |
| <asset-material>` | | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
@@ -774,13 +774,15 @@
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`name<actuator-position-name>` | :ref:`class<actuator-position-class>` | :ref:`group<actuator-position-group>` | :ref:`ctrllimited<actuator-position-ctrllimited>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`forcelimited<actuator-position-forcelimited>` | :ref:`ctrlrange<actuator-position-ctrlrange>` | :ref:`forcerange<actuator-position-forcerange>` | :ref:`lengthrange<actuator-position-lengthrange>` | |
| | | | :ref:`forcelimited<actuator-position-forcelimited>` | :ref:`ctrlrange<actuator-position-ctrlrange>` | :ref:`inheritrange<actuator-position-inheritrange>` | :ref:`forcerange<actuator-position-forcerange>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`gear<actuator-position-gear>` | :ref:`cranklength<actuator-position-cranklength>` | :ref:`user<actuator-position-user>` | :ref:`joint<actuator-position-joint>` | |
| | | | :ref:`lengthrange<actuator-position-lengthrange>` | :ref:`gear<actuator-position-gear>` | :ref:`cranklength<actuator-position-cranklength>` | :ref:`user<actuator-position-user>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`jointinparent<actuator-position-jointinparent>` | :ref:`tendon<actuator-position-tendon>` | :ref:`slidersite<actuator-position-slidersite>` | :ref:`cranksite<actuator-position-cranksite>` | |
| | | | :ref:`joint<actuator-position-joint>` | :ref:`jointinparent<actuator-position-jointinparent>` | :ref:`tendon<actuator-position-tendon>` | :ref:`slidersite<actuator-position-slidersite>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`site<actuator-position-site>` | :ref:`refsite<actuator-position-refsite>` | :ref:`kp<actuator-position-kp>` | :ref:`kv<actuator-position-kv>` | |
| | | | :ref:`cranksite<actuator-position-cranksite>` | :ref:`site<actuator-position-site>` | :ref:`refsite<actuator-position-refsite>` | :ref:`kp<actuator-position-kp>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`kv<actuator-position-kv>` | | | | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
| |_| actuator |br| |_| |L| | | .. table:: |
@@ -806,13 +808,13 @@
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`forcelimited<actuator-intvelocity-forcelimited>` | :ref:`ctrlrange<actuator-intvelocity-ctrlrange>` | :ref:`forcerange<actuator-intvelocity-forcerange>` | :ref:`actrange<actuator-intvelocity-actrange>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`lengthrange<actuator-intvelocity-lengthrange>` | :ref:`gear<actuator-intvelocity-gear>` | :ref:`cranklength<actuator-intvelocity-cranklength>` | :ref:`user<actuator-intvelocity-user>` | |
| | | | :ref:`inheritrange<actuator-intvelocity-inheritrange>` | :ref:`lengthrange<actuator-intvelocity-lengthrange>` | :ref:`gear<actuator-intvelocity-gear>` | :ref:`cranklength<actuator-intvelocity-cranklength>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`joint<actuator-intvelocity-joint>` | :ref:`jointinparent<actuator-intvelocity-jointinparent>` | :ref:`tendon<actuator-intvelocity-tendon>` | :ref:`slidersite<actuator-intvelocity-slidersite>` | |
| | | | :ref:`user<actuator-intvelocity-user>` | :ref:`joint<actuator-intvelocity-joint>` | :ref:`jointinparent<actuator-intvelocity-jointinparent>` | :ref:`tendon<actuator-intvelocity-tendon>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`cranksite<actuator-intvelocity-cranksite>` | :ref:`site<actuator-intvelocity-site>` | :ref:`refsite<actuator-intvelocity-refsite>` | :ref:`kp<actuator-intvelocity-kp>` | |
| | | | :ref:`slidersite<actuator-intvelocity-slidersite>` | :ref:`cranksite<actuator-intvelocity-cranksite>` | :ref:`site<actuator-intvelocity-site>` | :ref:`refsite<actuator-intvelocity-refsite>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`kv<actuator-intvelocity-kv>` | | | | |
| | | | :ref:`kp<actuator-intvelocity-kp>` | :ref:`kv<actuator-intvelocity-kv>` | | | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
| |_| actuator |br| |_| |L| | | .. table:: |
@@ -1440,11 +1442,11 @@
| :ref:`position | ? | :class: mjcf-attributes |
| <default-position>` | | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`ctrllimited<default-position-ctrllimited>` | :ref:`forcelimited<default-position-forcelimited>` | :ref:`ctrlrange<default-position-ctrlrange>` | :ref:`forcerange<default-position-forcerange>` | |
| | | | :ref:`ctrllimited<default-position-ctrllimited>` | :ref:`forcelimited<default-position-forcelimited>` | :ref:`ctrlrange<default-position-ctrlrange>` | :ref:`inheritrange<default-position-inheritrange>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`gear<default-position-gear>` | :ref:`cranklength<default-position-cranklength>` | :ref:`user<default-position-user>` | :ref:`group<default-position-group>` | |
| | | | :ref:`forcerange<default-position-forcerange>` | :ref:`gear<default-position-gear>` | :ref:`cranklength<default-position-cranklength>` | :ref:`user<default-position-user>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`kp<default-position-kp>` | :ref:`kv<default-position-kv>` | | | |
| | | | :ref:`group<default-position-group>` | :ref:`kp<default-position-kp>` | :ref:`kv<default-position-kv>` | | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
| |_| default |br| |_| |L| | | .. table:: |
@@ -1464,9 +1466,9 @@
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`ctrllimited<default-intvelocity-ctrllimited>` | :ref:`forcelimited<default-intvelocity-forcelimited>` | :ref:`ctrlrange<default-intvelocity-ctrlrange>` | :ref:`forcerange<default-intvelocity-forcerange>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`actrange<default-intvelocity-actrange>` | :ref:`gear<default-intvelocity-gear>` | :ref:`cranklength<default-intvelocity-cranklength>` | :ref:`user<default-intvelocity-user>` | |
| | | | :ref:`actrange<default-intvelocity-actrange>` | :ref:`inheritrange<default-intvelocity-inheritrange>` | :ref:`gear<default-intvelocity-gear>` | :ref:`cranklength<default-intvelocity-cranklength>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`group<default-intvelocity-group>` | :ref:`kp<default-intvelocity-kp>` | :ref:`kv<default-intvelocity-kv>` | | |
| | | | :ref:`user<default-intvelocity-user>` | :ref:`group<default-intvelocity-group>` | :ref:`kp<default-intvelocity-kp>` | :ref:`kv<default-intvelocity-kv>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
| |_| default |br| |_| |L| | | .. table:: |
doc/changelog.rst
+23 -2
View File
@@ -5,17 +5,38 @@ Changelog
Upcoming version (not yet released)
-----------------------------------
General
^^^^^^^
1. Added the :at:`inheritrange` attribute to :ref:`position<actuator-position>` and
:ref:`intvelocity<actuator-intvelocity>` actuators, allowing convenient setting of the actuator's
:at:`ctrlrange` or :at:`actrange` (respectively), according to the range of the transmission
target (joint or tendon). See :ref:`position/inheritrange<actuator-position-inheritrange>` for
details.
2. Deprecated :ref:`mj_makeEmptyFileVFS` in favor of :ref:`mj_addBufferVFS`. :ref:`mjVFS` now computes checksums of
its internal file buffers. :ref:`mj_addBufferVFS` allocates an empty buffer with a given name in an mjVFS and
copies the data buffer into it, combining and replacing the deprecated two-step process of calling
:ref:`mj_makeEmptyFileVFS` followed by a direct copy into the given mjVFS internal file buffer.
MJX
^^^
1. Improved performance of getting and putting device data.
3. Improved performance of getting and putting device data.
- Use ``tobytes()`` for numpy array serialization, which is orders of magnitude faster than converting to tuples.
- Avoid reallocating host ``mjData`` arrays when array shapes are unchanged.
- Speed up calculation of ``mjx.ncon`` for models with many geoms.
- Avoid calling ``mjx.ncon`` in ``mjx.get_data_into`` when ``nc`` can be derived from ``mjx.Data``.
2. Fixed a bug in ``mjx-viewer`` that prevented it from running. Updated ``mjx-viewer`` to use newer
4. Fixed a bug in ``mjx-viewer`` that prevented it from running. Updated ``mjx-viewer`` to use newer
``mjx.get_data_into`` function call.
5. Fixed a bug in ``mjx.euler`` that applied incorrect damping when using dense mass matrices.
6. Fixed a bug in ``mjx.solve`` that was causing slow convergence when using ``mjSOL_NEWTON`` in :ref:`mjtSolver`.
7. Added support for :ref:`mjOption.impratio<mjOption>` to ``mjx.Model``.
Python bindings
^^^^^^^^^^^^^^^
7. Fixed incorrect data types in the bindings for the ``geom``, ``vert``, ``elem``, and ``flex`` array members
of the ``mjContact`` struct, and all array members of the ``mjrContext`` struct.
Version 3.1.2 (February 05, 2024)
-----------------------------------
doc/conf.py
+2 -2
View File
@@ -118,7 +118,7 @@ SHARED_CSS_VARIABLES = {
html_theme_options = {
'light_css_variables': {
'font-stack--monospace': 'Inconsolata,Consolas,ui-monospace,monospace',
'at-color': '#bc103e',
'at-color': '#830b2b',
'at-val-color': '#bc103e',
'body-color': '#14234b',
'color-highlight-on-target': '#e5e8ed',
@@ -131,7 +131,7 @@ html_theme_options = {
'wy-nav-side-background-color': '#0053d6',
},
'dark_css_variables': {
'at-color': '#ff95a6',
'at-color': '#ffaab7',
'at-val-color': '#ff95a6',
'body-color': '#14234b',
'color-admonition-background': '#1e1e21',
doc/includes/references.h
+11 -8
View File
@@ -691,11 +691,12 @@ struct mjLROpt_ { // options for mj_setLengthRange()
mjtNum tolrange; // convergence tolerance (relative to range)
};
typedef struct mjLROpt_ mjLROpt;
struct mjVFS_ { // virtual file system for loading from memory
int nfile; // number of files present
char filename[mjMAXVFS][mjMAXVFSNAME]; // file name without path
size_t filesize[mjMAXVFS]; // file size in bytes
void* filedata[mjMAXVFS]; // buffer with file data
struct mjVFS_ { // virtual file system for loading from memory
int nfile; // number of files present
char filename[mjMAXVFS][mjMAXVFSNAME]; // file name without path
size_t filesize[mjMAXVFS]; // file size in bytes
void* filedata[mjMAXVFS]; // buffer with file data
uint64_t filestamp[mjMAXVFS]; // checksum of the file data
};
typedef struct mjVFS_ mjVFS;
struct mjOption_ { // physics options
@@ -1154,8 +1155,6 @@ struct mjModel_ {
int* mesh_texcoordadr; // texcoord data address; -1: no texcoord (nmesh x 1)
int* mesh_texcoordnum; // number of texcoord (nmesh x 1)
int* mesh_graphadr; // graph data address; -1: no graph (nmesh x 1)
mjtNum* mesh_pos; // translation applied to asset vertices (nmesh x 3)
mjtNum* mesh_quat; // rotation applied to asset vertices (nmesh x 4)
float* mesh_vert; // vertex positions for all meshes (nmeshvert x 3)
float* mesh_normal; // normals for all meshes (nmeshnormal x 3)
float* mesh_texcoord; // vertex texcoords for all meshes (nmeshtexcoord x 2)
@@ -1163,6 +1162,8 @@ struct mjModel_ {
int* mesh_facenormal; // normal face data (nmeshface x 3)
int* mesh_facetexcoord; // texture face data (nmeshface x 3)
int* mesh_graph; // convex graph data (nmeshgraph x 1)
mjtNum* mesh_pos; // translation applied to asset vertices (nmesh x 3)
mjtNum* mesh_quat; // rotation applied to asset vertices (nmesh x 4)
int* mesh_pathadr; // address of asset path for mesh; -1: none (nmesh x 1)
// skins
@@ -1375,6 +1376,7 @@ typedef struct mjModel_ mjModel;
struct mjResource_ {
char* name; // name of resource (filename, etc)
void* data; // opaque data pointer
char timestamp[512]; // timestamp of the resource
const struct mjpResourceProvider* provider; // pointer to the provider
};
typedef struct mjResource_ mjResource;
@@ -2406,10 +2408,11 @@ typedef struct mjvSceneState_ mjvSceneState;
//----------------------------- MJAPI FUNCTIONS --------------------------------
void mj_defaultVFS(mjVFS* vfs);
int mj_addFileVFS(mjVFS* vfs, const char* directory, const char* filename);
int mj_makeEmptyFileVFS(mjVFS* vfs, const char* filename, int filesize);
int mj_addBufferVFS(mjVFS* vfs, const char* name, const void* buffer, int nbuffer);
int mj_findFileVFS(const mjVFS* vfs, const char* filename);
int mj_deleteFileVFS(mjVFS* vfs, const char* filename);
void mj_deleteVFS(mjVFS* vfs);
int mj_makeEmptyFileVFS(mjVFS* vfs, const char* filename, int filesize);
mjModel* mj_loadXML(const char* filename, const mjVFS* vfs, char* error, int error_sz);
int mj_saveLastXML(const char* filename, const mjModel* m, char* error, int error_sz);
void mj_freeLastXML(void);
include/mujoco/mjmodel.h
+9 -7
View File
@@ -16,6 +16,7 @@
#define MUJOCO_MJMODEL_H_
#include <stddef.h>
#include <stdint.h>
#include <mujoco/mjtnum.h>
@@ -390,11 +391,12 @@ typedef struct mjLROpt_ mjLROpt;
//---------------------------------- mjVFS ---------------------------------------------------------
struct mjVFS_ { // virtual file system for loading from memory
int nfile; // number of files present
char filename[mjMAXVFS][mjMAXVFSNAME]; // file name without path
size_t filesize[mjMAXVFS]; // file size in bytes
void* filedata[mjMAXVFS]; // buffer with file data
struct mjVFS_ { // virtual file system for loading from memory
int nfile; // number of files present
char filename[mjMAXVFS][mjMAXVFSNAME]; // file name without path
size_t filesize[mjMAXVFS]; // file size in bytes
void* filedata[mjMAXVFS]; // buffer with file data
uint64_t filestamp[mjMAXVFS]; // checksum of the file data
};
typedef struct mjVFS_ mjVFS;
@@ -868,8 +870,6 @@ struct mjModel_ {
int* mesh_texcoordadr; // texcoord data address; -1: no texcoord (nmesh x 1)
int* mesh_texcoordnum; // number of texcoord (nmesh x 1)
int* mesh_graphadr; // graph data address; -1: no graph (nmesh x 1)
mjtNum* mesh_pos; // translation applied to asset vertices (nmesh x 3)
mjtNum* mesh_quat; // rotation applied to asset vertices (nmesh x 4)
float* mesh_vert; // vertex positions for all meshes (nmeshvert x 3)
float* mesh_normal; // normals for all meshes (nmeshnormal x 3)
float* mesh_texcoord; // vertex texcoords for all meshes (nmeshtexcoord x 2)
@@ -877,6 +877,8 @@ struct mjModel_ {
int* mesh_facenormal; // normal face data (nmeshface x 3)
int* mesh_facetexcoord; // texture face data (nmeshface x 3)
int* mesh_graph; // convex graph data (nmeshgraph x 1)
mjtNum* mesh_pos; // translation applied to asset vertices (nmesh x 3)
mjtNum* mesh_quat; // rotation applied to asset vertices (nmesh x 4)
int* mesh_pathadr; // address of asset path for mesh; -1: none (nmesh x 1)
// skins
include/mujoco/mjplugin.h
+8 -5
View File
@@ -26,11 +26,12 @@
struct mjResource_ {
char* name; // name of resource (filename, etc)
void* data; // opaque data pointer
char timestamp[512]; // timestamp of the resource
const struct mjpResourceProvider* provider; // pointer to the provider
};
typedef struct mjResource_ mjResource;
// callback for opeing a resource, returns zero on failure
// callback for opening a resource, returns zero on failure
typedef int (*mjfOpenResource)(mjResource* resource);
// callback for reading a resource
@@ -44,10 +45,12 @@ typedef void (*mjfCloseResource)(mjResource* resource);
// sets dir to directory string with ndir being size of directory string
typedef void (*mjfGetResourceDir)(mjResource* resource, const char** dir, int* ndir);
// callback for checking if a resource was modified since last read
// returns > 0 if resource was modified since last open, 0 if resource was not
// modified, and < 0 if inconclusive
typedef int (*mjfResourceModified)(const mjResource* resource);
// callback for checking if the current resource was modified from the time
// specified by the timestamp
// returns 0 if the resource's timestamp matches the provided timestamp
// returns > 0 if the the resource is younger than the given timestamp
// returns < 0 if the resource is older than the given timestamp
typedef int (*mjfResourceModified)(const mjResource* resource, const char* timestamp);
// struct describing a single resource provider
struct mjpResourceProvider {
include/mujoco/mujoco.h
+5 -3
View File
@@ -82,8 +82,8 @@ MJAPI void mj_defaultVFS(mjVFS* vfs);
// Add file to VFS, return 0: success, 1: full, 2: repeated name, -1: failed to load.
MJAPI int mj_addFileVFS(mjVFS* vfs, const char* directory, const char* filename);
// Make empty file in VFS, return 0: success, 1: full, 2: repeated name.
MJAPI int mj_makeEmptyFileVFS(mjVFS* vfs, const char* filename, int filesize);
// Add file to VFS from buffer, return 0: success, 1: full, 2: repeated name, -1: failed to load.
MJAPI int mj_addBufferVFS(mjVFS* vfs, const char* name, const void* buffer, int nbuffer);
// Return file index in VFS, or -1 if not found in VFS.
MJAPI int mj_findFileVFS(const mjVFS* vfs, const char* filename);
@@ -94,6 +94,8 @@ MJAPI int mj_deleteFileVFS(mjVFS* vfs, const char* filename);
// Delete all files from VFS.
MJAPI void mj_deleteVFS(mjVFS* vfs);
// deprecated: use mj_copyBufferVFS.
MJAPI int mj_makeEmptyFileVFS(mjVFS* vfs, const char* filename, int filesize);
//---------------------------------- Parse and compile ---------------------------------------------
@@ -182,7 +184,7 @@ MJAPI void mj_resetData(const mjModel* m, mjData* d);
// Reset data to defaults, fill everything else with debug_value.
MJAPI void mj_resetDataDebug(const mjModel* m, mjData* d, unsigned char debug_value);
// Reset data, set fields from specified keyframe.
// Reset data. If 0 <= key < nkey, set fields from specified keyframe.
MJAPI void mj_resetDataKeyframe(const mjModel* m, mjData* d, int key);
// Mark a new frame on the mjData stack.
introspect/functions.py
+36 -6
View File
@@ -66,9 +66,9 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([
),
doc='Add file to VFS, return 0: success, 1: full, 2: repeated name, -1: failed to load.', # pylint: disable=line-too-long
)),
('mj_makeEmptyFileVFS',
('mj_addBufferVFS',
FunctionDecl(
name='mj_makeEmptyFileVFS',
name='mj_addBufferVFS',
return_type=ValueType(name='int'),
parameters=(
FunctionParameterDecl(
@@ -78,17 +78,23 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([
),
),
FunctionParameterDecl(
name='filename',
name='name',
type=PointerType(
inner_type=ValueType(name='char', is_const=True),
),
),
FunctionParameterDecl(
name='filesize',
name='buffer',
type=PointerType(
inner_type=ValueType(name='void', is_const=True),
),
),
FunctionParameterDecl(
name='nbuffer',
type=ValueType(name='int'),
),
),
doc='Make empty file in VFS, return 0: success, 1: full, 2: repeated name.', # pylint: disable=line-too-long
doc='Add file to VFS from buffer, return 0: success, 1: full, 2: repeated name, -1: failed to load.', # pylint: disable=line-too-long
)),
('mj_findFileVFS',
FunctionDecl(
@@ -144,6 +150,30 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([
),
doc='Delete all files from VFS.',
)),
('mj_makeEmptyFileVFS',
FunctionDecl(
name='mj_makeEmptyFileVFS',
return_type=ValueType(name='int'),
parameters=(
FunctionParameterDecl(
name='vfs',
type=PointerType(
inner_type=ValueType(name='mjVFS'),
),
),
FunctionParameterDecl(
name='filename',
type=PointerType(
inner_type=ValueType(name='char', is_const=True),
),
),
FunctionParameterDecl(
name='filesize',
type=ValueType(name='int'),
),
),
doc='deprecated: use mj_copyBufferVFS.',
)),
('mj_loadXML',
FunctionDecl(
name='mj_loadXML',
@@ -675,7 +705,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([
type=ValueType(name='int'),
),
),
doc='Reset data, set fields from specified keyframe.',
doc='Reset data. If 0 <= key < nkey, set fields from specified keyframe.', # pylint: disable=line-too-long
)),
('mj_markStack',
FunctionDecl(
introspect/structs.py
+22 -14
View File
@@ -121,6 +121,14 @@ STRUCTS: Mapping[str, StructDecl] = dict([
),
doc='buffer with file data',
),
StructFieldDecl(
name='filestamp',
type=ArrayType(
inner_type=ValueType(name='uint64_t'),
extents=(2000,),
),
doc='checksum of the file data',
),
),
)),
('mjOption',
@@ -2591,20 +2599,6 @@ STRUCTS: Mapping[str, StructDecl] = dict([
),
doc='graph data address; -1: no graph (nmesh x 1)',
),
StructFieldDecl(
name='mesh_pos',
type=PointerType(
inner_type=ValueType(name='mjtNum'),
),
doc='translation applied to asset vertices (nmesh x 3)',
),
StructFieldDecl(
name='mesh_quat',
type=PointerType(
inner_type=ValueType(name='mjtNum'),
),
doc='rotation applied to asset vertices (nmesh x 4)',
),
StructFieldDecl(
name='mesh_vert',
type=PointerType(
@@ -2654,6 +2648,20 @@ STRUCTS: Mapping[str, StructDecl] = dict([
),
doc='convex graph data (nmeshgraph x 1)', # pylint: disable=line-too-long
),
StructFieldDecl(
name='mesh_pos',
type=PointerType(
inner_type=ValueType(name='mjtNum'),
),
doc='translation applied to asset vertices (nmesh x 3)',
),
StructFieldDecl(
name='mesh_quat',
type=PointerType(
inner_type=ValueType(name='mjtNum'),
),
doc='rotation applied to asset vertices (nmesh x 4)',
),
StructFieldDecl(
name='mesh_pathadr',
type=PointerType(
mjx/mujoco/mjx/_src/collision_convex.py
+2 -1
View File
@@ -173,7 +173,7 @@ def _manifold_points(
bp = b - poly
dist_bp = jp.abs(bp.dot(bc)) + dist_mask
dist_ap = jp.abs(ap.dot(ac)) + dist_mask
d_idx = jp.concatenate([dist_bp, dist_ap]).argmax() % poly.shape[0]
d_idx = (dist_bp + dist_ap).argmax() % poly.shape[0]
return jp.array([a_idx, b_idx, c_idx, d_idx])
@@ -514,6 +514,7 @@ def plane_convex(plane: GeomInfo, convex: GeomInfo) -> Contact:
frame = jp.stack([math.make_frame(n)] * 4, axis=0)
unique = jp.tril(idx == idx[:, None]).sum(axis=1) == 1
dist = jp.where(unique, -support[idx], 1)
pos = pos - 0.5 * dist[:, None] * n
return dist, pos, frame
mjx/mujoco/mjx/_src/collision_driver_test.py
+30 -2
View File
@@ -265,7 +265,7 @@ class ConvexTest(absltest.TestCase):
<worldbody>
<geom size="40 40 40" type="plane"/>
<body pos="0 0 0.7" euler="45 0 0">
<joint axis="1 0 0" type="free"/>
<freejoint/>
<geom size="0.5 0.5 0.5" type="box"/>
</body>
</worldbody>
@@ -281,7 +281,35 @@ class ConvexTest(absltest.TestCase):
# extract the contact points with penetration
c = jax.tree_map(lambda x: jp.take(x, jp.array([0, 1]), axis=0), dx.contact)
for field in dataclasses.fields(Contact):
_assert_attr_eq(c, d.contact, field.name, 'box_plane', 1e-2)
_assert_attr_eq(c, d.contact, field.name, 'box_plane', 1e-5)
_FLAT_BOX_PLANE = """
<mujoco>
<worldbody>
<geom size="40 40 40" type="plane"/>
<body pos="0 0 0.45">
<freejoint/>
<geom size="0.5 0.5 0.5" type="box"/>
</body>
</worldbody>
</mujoco>
"""
def test_flat_box_plane(self):
"""Tests box collision with a plane."""
with jax.disable_jit():
d, dx = _collide(self._FLAT_BOX_PLANE)
np.testing.assert_array_less(dx.contact.dist, 0)
# sort positions for comparison
idx = np.lexsort((dx.contact.pos[:, 0], dx.contact.pos[:, 1]))
dx = dx.tree_replace({'contact.pos': dx.contact.pos[idx]})
idx = np.lexsort((d.contact.pos[:, 0], d.contact.pos[:, 1]))
d.contact.pos[:] = d.contact.pos[idx]
for field in dataclasses.fields(Contact):
_assert_attr_eq(dx.contact, d.contact, field.name, 'flat_box_plane', 1e-5)
_BOX_BOX = """
<mujoco>
mjx/mujoco/mjx/_src/constraint.py
+1 -1
View File
@@ -297,7 +297,7 @@ def _instantiate_contact(m: Model, d: Data) -> Optional[_Efc]:
for diff_tan, friction in zip(diff_con[1:], c.friction[:2]):
for f in (friction, -friction):
js.append(diff_con[0] + diff_tan * f)
invweights.append((t + f * f * t) * 2 * f * f)
invweights.append((t + f * f * t) * 2 * f * f / m.opt.impratio)
active = dist < 0
j, invweight = jp.stack(js) * active, jp.stack(invweights)
mjx/mujoco/mjx/_src/dataclasses.py
+12 -7
View File
@@ -18,13 +18,23 @@ import copy
import dataclasses
import typing
from typing import Any, Dict, Optional, Sequence, TypeVar
from typing import Any, Dict, Optional, Sequence, TypeVar, Union
import jax
import numpy as np
_T = TypeVar('_T')
def _jax_in_args(typ) -> bool:
if typ is jax.Array:
return True
if dataclasses.is_dataclass(typ):
return any(_jax_in_args(f.type) for f in dataclasses.fields(typ))
if typing.get_origin(typ) in (list, dict, Union, set):
return any(_jax_in_args(t) for t in typing.get_args(typ))
return False
def dataclass(clz: _T) -> _T:
"""Wraps a dataclass with metadata for which fields are pytrees.
@@ -41,12 +51,7 @@ def dataclass(clz: _T) -> _T:
data_clz = dataclasses.dataclass(frozen=True)(clz)
meta_fields, data_fields = [], []
for field in dataclasses.fields(data_clz):
if any((
field.type is jax.Array,
dataclasses.is_dataclass(field.type),
jax.Array in typing.get_args(field.type),
any(dataclasses.is_dataclass(a) for a in typing.get_args(field.type)),
)):
if _jax_in_args(field.type):
data_fields.append(field)
else:
meta_fields.append(field)
mjx/mujoco/mjx/_src/forward.py
+4 -2
View File
@@ -287,8 +287,10 @@ def euler(m: Model, d: Data) -> Data:
# integrate damping implicitly
qacc = d.qacc
if not m.opt.disableflags & DisableBit.EULERDAMP:
# TODO(robotics-simulation): can this be done with a smaller perf hit
dh = d.replace(qM=d.qM.at[m.dof_Madr].add(m.opt.timestep * m.dof_damping))
if support.is_sparse(m):
dh = d.replace(qM=d.qM.at[m.dof_Madr].add(m.opt.timestep * m.dof_damping))
else:
dh = d.replace(qM=d.qM + jp.diag(m.opt.timestep * m.dof_damping))
dh = smooth.factor_m(m, dh)
qfrc = d.qfrc_smooth + d.qfrc_constraint
qacc = smooth.solve_m(m, dh, qfrc)
mjx/mujoco/mjx/_src/forward_test.py
+34 -13
View File
@@ -27,14 +27,14 @@ import numpy as np
_TOLERANCE = 1e-5
def _assert_eq(a, b, name):
tol = _TOLERANCE * 10 # avoid test noise
def _assert_eq(a, b, name, tol=_TOLERANCE):
tol = tol * 10 # avoid test noise
err_msg = f'mismatch: {name}'
np.testing.assert_allclose(a, b, err_msg=err_msg, atol=tol, rtol=tol)
def _assert_attr_eq(a, b, attr):
_assert_eq(getattr(a, attr), getattr(b, attr), attr)
def _assert_attr_eq(a, b, attr, tol=_TOLERANCE):
_assert_eq(getattr(a, attr), getattr(b, attr), attr, tol=tol)
class ForwardTest(absltest.TestCase):
@@ -46,7 +46,7 @@ class ForwardTest(absltest.TestCase):
d.ctrl = np.array([-18, 0.59, 0.47])
d.xfrc_applied[0, 2] = 0.1 # torque
d.xfrc_applied[1, 4] = 0.3 # linear force
mujoco.mj_step(m, d, 100) # get some dynamics going
mujoco.mj_step(m, d, 20) # get some dynamics going
mujoco.mj_forward(m, d)
mx = mjx.put_model(m)
@@ -75,14 +75,13 @@ class ForwardTest(absltest.TestCase):
d.ctrl = np.array([-18, 0.59, 0.47])
d.xfrc_applied[0, 2] = 0.1 # torque
d.xfrc_applied[1, 4] = 0.3 # linear force
mujoco.mj_step(m, d, 100) # get some dynamics going
mujoco.mj_step(m, d, 20) # get some dynamics going
mx = mjx.put_model(m)
dx = jax.jit(mjx.step)(mx, mjx.put_data(m, d))
dx = jax.jit(mjx.step)(mjx.put_model(m), mjx.put_data(m, d))
mujoco.mj_step(m, d)
_assert_attr_eq(d, dx, 'act')
_assert_attr_eq(d, dx, 'time')
_assert_attr_eq(d, dx, 'qvel')
_assert_attr_eq(d, dx, 'qvel', tol=5e-4)
_assert_attr_eq(d, dx, 'qpos')
def test_rk4(self):
@@ -111,8 +110,7 @@ class ForwardTest(absltest.TestCase):
mujoco.mj_step(m, d, 10) # let dynamics get state significantly non-zero
mujoco.mj_forward(m, d)
mx = mjx.put_model(m)
dx = jax.jit(mjx.rungekutta4)(mx, mjx.put_data(m, d))
dx = jax.jit(mjx.rungekutta4)(mjx.put_model(m), mjx.put_data(m, d))
mujoco.mj_RungeKutta(m, d, 4)
_assert_attr_eq(d, dx, 'qvel')
@@ -120,6 +118,30 @@ class ForwardTest(absltest.TestCase):
_assert_attr_eq(d, dx, 'act')
_assert_attr_eq(d, dx, 'time')
def test_eulerdamp(self):
m = test_util.load_test_file('pendula.xml')
self.assertTrue((m.dof_damping > 0).any())
d = mujoco.MjData(m)
d.qvel[:] = 1.0
d.qacc[:] = 1.0
mujoco.mj_forward(m, d)
dx = jax.jit(mjx.euler)(mjx.put_model(m), mjx.put_data(m, d))
mujoco.mj_Euler(m, d)
_assert_attr_eq(d, dx, 'qpos')
# also test sparse
m.opt.jacobian = mujoco.mjtJacobian.mjJAC_SPARSE
d = mujoco.MjData(m)
d.qvel[:] = 1.0
d.qacc[:] = 1.0
mujoco.mj_forward(m, d)
dx = jax.jit(mjx.euler)(mjx.put_model(m), mjx.put_data(m, d))
mujoco.mj_Euler(m, d)
_assert_attr_eq(d, dx, 'qpos')
def test_disable_eulerdamp(self):
m = test_util.load_test_file('pendula.xml')
self.assertTrue((m.dof_damping > 0).any())
@@ -128,8 +150,7 @@ class ForwardTest(absltest.TestCase):
d = mujoco.MjData(m)
d.qvel[:] = 1.0
d.qacc[:] = 1.0
mx = mjx.put_model(m)
dx = jax.jit(mjx.euler)(mx, mjx.put_data(m, d))
dx = jax.jit(mjx.euler)(mjx.put_model(m), mjx.put_data(m, d))
np.testing.assert_allclose(dx.qvel, 1 + m.opt.timestep)
mjx/mujoco/mjx/_src/io.py
+22 -22
View File
@@ -146,23 +146,23 @@ def make_data(m: Union[types.Model, mujoco.MjModel]) -> types.Data:
ne, nf, nl, nc = constraint.count_constraints(m)
nefc = ne + nf + nl + nc
zero_0 = jp.zeros(0, dtype=jp.float32)
zero_nv = jp.zeros(m.nv, dtype=jp.float32)
zero_nv_6 = jp.zeros((m.nv, 6), dtype=jp.float32)
zero_nv_nv = jp.zeros((m.nv, m.nv), dtype=jp.float32)
zero_nbody_3 = jp.zeros((m.nbody, 3), dtype=jp.float32)
zero_nbody_6 = jp.zeros((m.nbody, 6), dtype=jp.float32)
zero_nbody_10 = jp.zeros((m.nbody, 10), dtype=jp.float32)
zero_nbody_3_3 = jp.zeros((m.nbody, 3, 3), dtype=jp.float32)
zero_nefc = jp.zeros(nefc, dtype=jp.float32)
zero_na = jp.zeros(m.na, dtype=jp.float32)
zero_nu = jp.zeros(m.nu, dtype=jp.float32)
zero_njnt_3 = jp.zeros((m.njnt, 3), dtype=jp.float32)
zero_nm = jp.zeros(m.nM, dtype=jp.float32)
zero_0 = jp.zeros(0, dtype=float)
zero_nv = jp.zeros(m.nv, dtype=float)
zero_nv_6 = jp.zeros((m.nv, 6), dtype=float)
zero_nv_nv = jp.zeros((m.nv, m.nv), dtype=float)
zero_nbody_3 = jp.zeros((m.nbody, 3), dtype=float)
zero_nbody_6 = jp.zeros((m.nbody, 6), dtype=float)
zero_nbody_10 = jp.zeros((m.nbody, 10), dtype=float)
zero_nbody_3_3 = jp.zeros((m.nbody, 3, 3), dtype=float)
zero_nefc = jp.zeros(nefc, dtype=float)
zero_na = jp.zeros(m.na, dtype=float)
zero_nu = jp.zeros(m.nu, dtype=float)
zero_njnt_3 = jp.zeros((m.njnt, 3), dtype=float)
zero_nm = jp.zeros(m.nM, dtype=float)
# create first d to get num contacts and nc
d = types.Data(
solver_niter=jp.array(0, dtype=jp.int32),
solver_niter=jp.array(0, dtype=int),
time=jp.array(0.0),
qpos=jp.array(m.qpos0),
qvel=zero_nv,
@@ -171,31 +171,31 @@ def make_data(m: Union[types.Model, mujoco.MjModel]) -> types.Data:
ctrl=zero_nu,
qfrc_applied=zero_nv,
xfrc_applied=zero_nbody_6,
eq_active=jp.zeros(m.neq, dtype=jp.int32),
eq_active=jp.zeros(m.neq, dtype=int),
qacc=zero_nv,
act_dot=zero_na,
xpos=zero_nbody_3,
xquat=jp.zeros((m.nbody, 4), dtype=jp.float32),
xquat=jp.zeros((m.nbody, 4), dtype=float),
xmat=zero_nbody_3_3,
xipos=zero_nbody_3,
ximat=zero_nbody_3_3,
xanchor=zero_njnt_3,
xaxis=zero_njnt_3,
geom_xpos=jp.zeros((m.ngeom, 3), dtype=jp.float32),
geom_xmat=jp.zeros((m.ngeom, 3, 3), dtype=jp.float32),
site_xpos=jp.zeros((m.nsite, 3), dtype=jp.float32),
site_xmat=jp.zeros((m.nsite, 3, 3), dtype=jp.float32),
geom_xpos=jp.zeros((m.ngeom, 3), dtype=float),
geom_xmat=jp.zeros((m.ngeom, 3, 3), dtype=float),
site_xpos=jp.zeros((m.nsite, 3), dtype=float),
site_xmat=jp.zeros((m.nsite, 3, 3), dtype=float),
subtree_com=zero_nbody_3,
cdof=zero_nv_6,
cinert=zero_nbody_10,
actuator_length=zero_nu,
actuator_moment=jp.zeros((m.nu, m.nv), dtype=jp.float32),
actuator_moment=jp.zeros((m.nu, m.nv), dtype=float),
crb=zero_nbody_10,
qM=zero_nm if support.is_sparse(m) else zero_nv_nv,
qLD=zero_nm if support.is_sparse(m) else zero_nv_nv,
qLDiagInv=zero_nv if support.is_sparse(m) else zero_0,
contact=types.Contact.zero(ncon),
efc_J=jp.zeros((nefc, m.nv), dtype=jp.float32),
efc_J=jp.zeros((nefc, m.nv), dtype=float),
efc_frictionloss=zero_nefc,
efc_D=zero_nefc,
actuator_velocity=zero_nu,
mjx/mujoco/mjx/_src/mesh.py
+6 -2
View File
@@ -212,10 +212,14 @@ def get(m: mujoco.MjModel) -> Dict[str, Sequence[Optional[np.ndarray]]]:
"""Derives geom mesh attributes for mjx.Model from MjModel."""
kwargs = {k: [] for k in _DERIVED_ARGS}
verts, faces = _get_faces_verts(m)
geom_con = m.geom_conaffinity | m.geom_contype
for geomid in range(m.ngeom):
dataid = m.geom_dataid[geomid]
typ = m.geom_type[geomid]
if typ == GeomType.BOX:
if not geom_con[geomid]:
# ignore visual-only meshes
kwargs = {k: kwargs[k] + [None] for k in _DERIVED_ARGS}
continue
elif m.geom_type[geomid] == GeomType.BOX:
vert, face = _box(m.geom_size[geomid])
elif dataid >= 0:
vert, face = verts[dataid], faces[dataid]
mjx/mujoco/mjx/_src/solver.py
+6 -6
View File
@@ -218,13 +218,13 @@ def _update_gradient(m: Model, d: Data, ctx: _Context) -> _Context:
mgrad = smooth.solve_m(m, d, grad)
elif m.opt.solver == SolverType.NEWTON:
ne, nf, *_ = constraint.count_constraints(m)
active = (ctx.Jaref < 0).at[:ne + nf].set(True)
h = d.qM + support.make_m(m, d.efc_J.T * d.efc_D * active, d.efc_J.T)
dh = d.replace(qM=h)
dh = smooth.factor_m(m, dh)
mgrad = smooth.solve_m(m, dh, grad)
active = (ctx.Jaref < 0).at[: ne + nf].set(True)
h = (d.efc_J.T * d.efc_D * active) @ d.efc_J
h = support.full_m(m, d) + h
h_ = jax.scipy.linalg.cho_factor(h)
mgrad = jax.scipy.linalg.cho_solve(h_, grad)
else:
raise NotImplementedError(f"unsupported solver type: {m.opt.solver}")
raise NotImplementedError(f'unsupported solver type: {m.opt.solver}')
ctx = ctx.replace(grad=grad, Mgrad=mgrad)
mjx/mujoco/mjx/_src/solver_test.py
+42 -24
View File
@@ -33,8 +33,8 @@ def _assert_eq(a, b, name, tol=_TOLERANCE):
np.testing.assert_allclose(a, b, err_msg=err_msg, atol=tol, rtol=tol)
def _assert_attr_eq(a, b, attr):
_assert_eq(getattr(a, attr), getattr(b, attr), attr)
def _assert_attr_eq(a, b, attr, tol=_TOLERANCE):
_assert_eq(getattr(a, attr), getattr(b, attr), attr, tol=tol)
class SolverTest(absltest.TestCase):
@@ -42,13 +42,20 @@ class SolverTest(absltest.TestCase):
def test_newton(self):
"""Test newton solver."""
m = test_util.load_test_file('constraints.xml')
# it's critical that mgrad is optimally calculated, so lower iterations
# to be sure that MJX is converging as quickly as MuJoCo
m.opt.iterations = 1
d = mujoco.MjData(m)
mujoco.mj_step(m, d, 100) # at 100 steps mix of active/inactive constraints
mujoco.mj_forward(m, d)
mx = mjx.put_model(m)
dx = jax.jit(mjx.solve)(mx, mjx.put_data(m, d))
mujoco.mj_step(m, d, 20) # significant constraint forces at 20 steps
# mj_forward overwrites qacc_warmstart, so let's restore it to what it was
# at the beginning of the step so that MJX does not have a trivial solution
warmstart = d.qacc_warmstart.copy()
mujoco.mj_forward(m, d)
d.qacc_warmstart = warmstart
dx = jax.jit(mjx.solve)(mjx.put_model(m), mjx.put_data(m, d))
_assert_attr_eq(d, dx, 'qacc_warmstart')
_assert_attr_eq(d, dx, 'qacc')
_assert_attr_eq(d, dx, 'qfrc_constraint')
nnz = dx.efc_J.any(axis=1)
@@ -58,23 +65,30 @@ class SolverTest(absltest.TestCase):
"""Test CG solver."""
m = test_util.load_test_file('constraints.xml')
d = mujoco.MjData(m)
mujoco.mj_step(m, d, 100) # at 100 steps mix of active/inactive constraints
m.opt.solver = mujoco.mjtSolver.mjSOL_CG
mujoco.mj_forward(m, d)
mx = mjx.put_model(m)
dx = jax.jit(mjx.solve)(mx, mjx.put_data(m, d))
mujoco.mj_step(m, d, 20) # significant constraint forces at 20 steps
# CG does not converge as quickly as Newton but is cheaper to calculate
m.opt.solver = mujoco.mjtSolver.mjSOL_CG
m.opt.iterations = 8
# mj_forward overwrites qacc_warmstart, so let's restore it to what it was
# at the beginning of the step so that MJX does not have a trivial solution
warmstart = d.qacc_warmstart.copy()
mujoco.mj_forward(m, d)
d.qacc_warmstart = warmstart
dx = jax.jit(mjx.solve)(mjx.put_model(m), mjx.put_data(m, d))
_assert_attr_eq(d, dx, 'qacc_warmstart')
_assert_attr_eq(d, dx, 'qacc')
_assert_attr_eq(d, dx, 'qfrc_constraint')
_assert_attr_eq(d, dx, 'qfrc_constraint', tol=8e-4)
nnz = dx.efc_J.any(axis=1)
_assert_eq(d.efc_force, dx.efc_force[nnz], 'efc_force')
_assert_eq(d.efc_force, dx.efc_force[nnz], 'efc_force', tol=5e-4)
def test_no_warmstart(self):
"""Test no warmstart."""
m = test_util.load_test_file('constraints.xml')
d = mujoco.MjData(m)
mujoco.mj_step(m, d, 100) # at 100 steps mix of active/inactive constraints
mujoco.mj_step(m, d, 20) # significant constraint forces at 20 steps
m.opt.disableflags |= mujoco.mjtDisableBit.mjDSBL_WARMSTART
mujoco.mj_forward(m, d)
mx = mjx.put_model(m)
@@ -83,17 +97,21 @@ class SolverTest(absltest.TestCase):
# without warmstart, the solution is not as close
_assert_eq(d.efc_force, dx.efc_force[nnz], 'efc_force', tol=2e-2)
def test_dense(self):
"""Test solver works with dense mass matrices."""
def test_sparse(self):
"""Test solver works with sparse mass matrices."""
m = test_util.load_test_file('constraints.xml')
m.opt.jacobian = mujoco.mjtJacobian.mjJAC_SPARSE
d = mujoco.MjData(m)
mujoco.mj_step(m, d, 100) # at 100 steps mix of active/inactive constraints
mujoco.mj_forward(m, d)
m.opt.jacobian = mujoco.mjtJacobian.mjJAC_DENSE
mx = mjx.put_model(m)
dx = jax.jit(mjx.solve)(mx, mjx.put_data(m, d))
mujoco.mj_step(m, d, 20) # significant constraint forces at 20 steps
# mj_forward overwrites qacc_warmstart, so let's restore it to what it was
# at the beginning of the step so that MJX does not have a trivial solution
warmstart = d.qacc_warmstart.copy()
mujoco.mj_forward(m, d)
d.qacc_warmstart = warmstart
dx = jax.jit(mjx.solve)(mjx.put_model(m), mjx.put_data(m, d))
_assert_attr_eq(d, dx, 'qacc_warmstart')
_assert_attr_eq(d, dx, 'qacc')
_assert_attr_eq(d, dx, 'qfrc_constraint')
nnz = dx.efc_J.any(axis=1)
mjx/mujoco/mjx/_src/types.py
+11 -8
View File
@@ -15,7 +15,7 @@
"""Base types used in MJX."""
import enum
from typing import Sequence
from typing import List, Optional
import jax
import jax.numpy as jp
@@ -218,6 +218,7 @@ class Option(PyTreeNode):
Attributes:
timestep: timestep
impratio: ratio of friction-to-normal contact impedance
tolerance: main solver tolerance
ls_tolerance: CG/Newton linesearch tolerance
gravity: gravitational acceleration (3,)
@@ -238,9 +239,11 @@ class Option(PyTreeNode):
disableflags: bit flags for disabling standard features
"""
timestep: jax.Array
# unsupported: apirate
impratio: jax.Array
tolerance: jax.Array
ls_tolerance: jax.Array
# unsupported: apirate, impratio, noslip_tolerance, mpr_tolerance
# unsupported: noslip_tolerance, mpr_tolerance
gravity: jax.Array
wind: jax.Array
density: jax.Array
@@ -498,10 +501,10 @@ class Model(PyTreeNode):
pair_dim: np.ndarray
pair_geom1: np.ndarray
pair_geom2: np.ndarray
geom_convex_face: Sequence[jax.Array]
geom_convex_vert: Sequence[jax.Array]
geom_convex_edge: Sequence[jax.Array]
geom_convex_facenormal: Sequence[jax.Array]
geom_convex_face: List[Optional[jax.Array]]
geom_convex_vert: List[Optional[jax.Array]]
geom_convex_edge: List[Optional[jax.Array]]
geom_convex_facenormal: List[Optional[jax.Array]]
pair_solref: jax.Array
pair_solreffriction: jax.Array
pair_solimp: jax.Array
@@ -579,8 +582,8 @@ class Contact(PyTreeNode):
solref=jp.zeros((ncon, mujoco.mjNREF)),
solreffriction=jp.zeros((ncon, mujoco.mjNREF)),
solimp=jp.zeros((ncon, mujoco.mjNIMP,)),
geom1=jp.zeros(ncon, dtype=jp.int32),
geom2=jp.zeros(ncon, dtype=jp.int32),
geom1=jp.zeros(ncon, dtype=int),
geom2=jp.zeros(ncon, dtype=int),
)
mjx/mujoco/mjx/test_data/constraints.xml
+1 -1
View File
@@ -5,7 +5,7 @@
* solref, solimp
-->
<mujoco>
<option timestep="0.02"/>
<option timestep="0.015" impratio="1.5"/>
<default>
<default class="box">
model/humanoid/README.md
+1
View File
@@ -13,6 +13,7 @@ in the DeepMind Control Suite [2], which has fairly realistic actuator gains.
## Changelog
* 20-02-2024: Sorted actuators in the same order as the joints.
* 02-01-2024: Add more keyframes.
* 27-11-2023: Move humanoid geoms to group 1.
* 05-04-2023: Fix typo in texture size.
model/humanoid/humanoid.xml
+3 -3
View File
@@ -200,21 +200,21 @@
</tendon>
<actuator>
<motor name="abdomen_y" gear="40" joint="abdomen_y"/>
<motor name="abdomen_z" gear="40" joint="abdomen_z"/>
<motor name="abdomen_y" gear="40" joint="abdomen_y"/>
<motor name="abdomen_x" gear="40" joint="abdomen_x"/>
<motor name="hip_x_right" gear="40" joint="hip_x_right"/>
<motor name="hip_z_right" gear="40" joint="hip_z_right"/>
<motor name="hip_y_right" gear="120" joint="hip_y_right"/>
<motor name="knee_right" gear="80" joint="knee_right"/>
<motor name="ankle_x_right" gear="20" joint="ankle_x_right"/>
<motor name="ankle_y_right" gear="20" joint="ankle_y_right"/>
<motor name="ankle_x_right" gear="20" joint="ankle_x_right"/>
<motor name="hip_x_left" gear="40" joint="hip_x_left"/>
<motor name="hip_z_left" gear="40" joint="hip_z_left"/>
<motor name="hip_y_left" gear="120" joint="hip_y_left"/>
<motor name="knee_left" gear="80" joint="knee_left"/>
<motor name="ankle_x_left" gear="20" joint="ankle_x_left"/>
<motor name="ankle_y_left" gear="20" joint="ankle_y_left"/>
<motor name="ankle_x_left" gear="20" joint="ankle_x_left"/>
<motor name="shoulder1_right" gear="20" joint="shoulder1_right"/>
<motor name="shoulder2_right" gear="20" joint="shoulder2_right"/>
<motor name="elbow_right" gear="40" joint="elbow_right"/>
python/mujoco/bindings_test.py
+8
View File
@@ -529,6 +529,7 @@ class MuJoCoBindingsTest(parameterized.TestCase):
expected_ncon = 4
mujoco.mj_forward(self.model, self.data)
self.assertLen(self.data.contact, expected_ncon)
np.testing.assert_array_equal(self.data.contact.geom, [[0, 1]] * 4)
expected_pos = []
for contact in self.data.contact:
@@ -551,6 +552,13 @@ class MuJoCoBindingsTest(parameterized.TestCase):
self.assertLen(expected_H, expected_ncon)
np.testing.assert_array_equal(self.data.contact.H, expected_H)
expected_geom = []
for i, contact in enumerate(self.data.contact):
expected_geom.append([i, i + 1])
contact.geom = expected_geom[-1]
self.assertLen(expected_geom, expected_ncon)
np.testing.assert_array_equal(self.data.contact.geom, expected_geom)
def test_realloc_con_efc(self):
self.assertEmpty(self.data.contact)
python/mujoco/minimize.py
+337
View File
@@ -0,0 +1,337 @@
# Copyright 2024 DeepMind Technologies Limited
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
# ==============================================================================
"""Nonlinear Least Squares minimization with box bounds."""
import dataclasses
import enum
import time
from typing import Callable, List, Optional, TextIO, Tuple, Union
import mujoco
import numpy as np
class Verbosity(enum.Enum):
SILENT = 0
FINAL = 1
ITER = 2
FULLITER = 3
class Status(enum.Enum):
FACTORIZATION_FAILED = enum.auto()
NO_IMPORVEMENT = enum.auto()
MAX_ITER = enum.auto()
DX_TOL = enum.auto()
_STATUS_MESSAGE = {
Status.FACTORIZATION_FAILED: 'factorization failed.',
Status.NO_IMPORVEMENT: 'no improvement found.',
Status.MAX_ITER: 'maximum iterations reached.',
Status.DX_TOL: 'norm(step) < tolerance.',
}
@dataclasses.dataclass(frozen=True)
class IterLog:
"""Log of a single iteration of the non-linear least-squares solver.
Attributes:
candidate: Value of the decision variable at the beginning this iteration.
objective: Value of the objective at the candidate.
reduction: Reduction of the objective during this iteration.
regularizer: Value of the regularizer used for this iteration.
residual: Optional value of the residual at the candidate.
jacobian: Optional value of the Jacobian at the candidate.
step: Optional change in decision variable during this iteration.
"""
candidate: np.ndarray
objective: np.float64
reduction: np.float64
regularizer: np.float64
residual: Optional[np.ndarray] = None
jacobian: Optional[np.ndarray] = None
step: Optional[np.ndarray] = None
def jacobian_fd(
residual: Callable[[np.ndarray], np.ndarray],
x: np.ndarray,
r: np.ndarray,
eps: float,
bounds: Optional[List[np.ndarray]] = None,
):
"""Finite-difference Jacobian of a residual function.
Args:
residual: function that returns the residual for a given point.
x: point at which to evaluate the Jacobian.
r: residual at x.
eps: finite-difference step size.
bounds: optional pair of lower and upper bounds of the solution.
Returns:
jac: Jacobian of the residual at x.
"""
nx = x.size
nr = r.size
jac = np.zeros((nr, nx))
xh = x.copy()
for i in range(nx):
if bounds is not None:
# Have bounds: scale eps, don't cross bounds.
lower, upper = bounds
eps_i = eps * (upper[i] - lower[i])
if xh[i] < upper[i] - eps_i:
# Not near upper bound, use forward.
xh[i] += eps_i
rh = residual(xh)
jac[:, i] = (rh - r) / eps_i
else:
# Near upper bound, use backward.
xh[i] -= eps_i
rh = residual(xh)
jac[:, i] = (r - rh) / eps_i
else:
# No bounds, just use forward fin-diff.
xh[i] += eps
rh = residual(xh)
jac[:, i] = (rh - r) / eps
xh[i] = x[i]
return jac
def least_squares(
x0: np.ndarray,
residual: Callable[[np.ndarray], np.ndarray],
bounds: Optional[List[np.ndarray]] = None,
jacobian: Optional[Callable[[np.ndarray, np.ndarray], np.ndarray]] = None,
eps: Optional[float] = -6,
mu_min: Optional[float] = -6,
mu_max: Optional[float] = 8,
mu_delta: Optional[float] = 0.5,
tol: Optional[float] = 1e-7,
max_iter: Optional[int] = 100,
verbose: Optional[Union[Verbosity, int]] = Verbosity.ITER,
output: Optional[TextIO] = None,
) -> Tuple[np.ndarray, List[IterLog]]:
"""Nonlinear Least Squares minimization with box bounds.
Args:
x0: initial guess
residual: function that returns the residual for a given point x.
bounds: optional pair of lower and upper bounds on the solution.
jacobian: optional function that returns Jacobian of the residual at a given
point and residual. If not given, `residual` will be finite-differenced.
eps: log10 of the perurbation used for automatic finite-differencing.
mu_min: log10 of the minimum value of the regularizer.
mu_max: log10 of the maximum value of the regularizer.
mu_delta: log10 of the factor increasing or decreasing the regularizer.
tol: termination tolerance on the step size.
max_iter: maximum number of iterations.
verbose: verbosity level.
output: optional file or StringIO to which to print messages.
Returns:
x: best solution found
trace: sequence of solution iterates.
"""
t_start_total = time.time()
# Convert verbosity to int.
verbose = Verbosity(verbose).value
# Initialize locals.
x = x0.copy()
mu = -np.inf # Optimistically start with no regularization.
n = x.size
i = 0
trace = []
dx = np.zeros((n,))
scratch = np.zeros((n, n + 7))
dx_norm = 0.0
xnew = np.zeros((n,))
status = Status.MAX_ITER
n_res = 0
n_jac = 0
t_res = 0.0
t_jac = 0.0
t_qp = 0.0
# Regularization control functions.
def increase_mu(mu):
return min(mu_max, max(mu_min, mu_delta + mu))
def decrease_mu(mu):
return -np.inf if mu - mu_delta < mu_min else mu - mu_delta
if bounds is not None:
# Checks bounds.
if len(bounds) != 2:
raise ValueError('bounds must have 2 elements.')
if bounds[0].size != n or bounds[1].size != n:
raise ValueError('bounds must have the same size as x0.')
if not np.all(np.isfinite(bounds[0])) or not np.all(np.isfinite(bounds[1])):
raise ValueError('bounds must be finite.')
if not np.all(bounds[0] < bounds[1]):
raise ValueError('bounds[0] must be smaller than bounds[1].')
# Clip.
np.clip(x, bounds[0], bounds[1], out=x)
# Get initial residual.
t_start = time.time()
r = residual(x)
rnew = r
t_res += time.time() - t_start
n_res += 1
# Minimize.
for i in range(max_iter):
if status != Status.MAX_ITER:
break
# Get objective y.
y = 0.5 * r.dot(r)
# Get Jacobian jac.
t_start = time.time()
if jacobian is None:
jac = jacobian_fd(residual, x, r, 10**eps, bounds)
t_res += time.time() - t_start
n_res += n
else:
jac = jacobian(x, r)
t_jac += time.time() - t_start
n_jac += 1
# Get gradient, Gauss-Newton Hessian.
grad = jac.T @ r
hess = jac.T @ jac
gnorm = np.linalg.norm(grad)
# Bounds relative to x
dbounds = [None, None] if bounds is None else [bounds[0] - x, bounds[1] - x]
# Find some reduction.
reduction = -1
while reduction < 0:
# Increase mu until factorizabl.
factorizable = False
while not factorizable:
# Formula from https://arxiv.org/abs/2112.02089
reg = np.sqrt(gnorm * 10**mu) * np.eye(n)
t_start = time.time()
nfree = mujoco.mju_boxQP(
dx, scratch, None, hess + reg, grad, dbounds[0], dbounds[1]
)
t_qp += time.time() - t_start
if nfree > -1:
factorizable = True
elif mu >= mu_max:
status = Status.FACTORIZATION_FAILED
break
else:
mu += mu_delta
if status != Status.MAX_ITER:
break
# New candidate, residual.
xnew = x + dx
t_start = time.time()
rnew = residual(xnew)
t_res += time.time() - t_start
n_res += 1
# New objective, evaluate reduction.
ynew = 0.5 * rnew.dot(rnew)
reduction = y - ynew
if reduction < 0:
if mu >= mu_max:
status = Status.NO_IMPORVEMENT
break
mu = increase_mu(mu)
if status != Status.MAX_ITER:
break
# Compute reduction ratio.
expected_reduction = -(grad.dot(dx) + 0.5 * dx.T @ hess @ dx)
reduction_ratio = 0.0
if expected_reduction == 0:
print('Zero expected reduction: exact minimum found?', file=output)
elif expected_reduction < 0:
print('Negative expected reduction: should not occur.', file=output)
else:
reduction_ratio = reduction / expected_reduction
# Iteration message.
if verbose >= Verbosity.ITER.value:
message = (
f'iter: {i:<3d} y: {y:<8.3g} mu: {mu:>4.1f} '
f'ratio: {reduction_ratio:<5.2g} '
f'dx: {dx_norm:<8.3g} reduction: {reduction:<8.3g}'
)
print(message, file=output)
# Append log to trace.
log = IterLog(candidate=x, objective=y, reduction=reduction, regularizer=mu)
if verbose >= Verbosity.FULLITER.value:
log = dataclasses.replace(log, residual=r, jacobian=jac, step=dx)
trace.append(log)
# Check for success.
dx_norm = np.linalg.norm(dx)
if dx_norm < tol:
status = Status.DX_TOL
break
# Modify regularizer like in (Bazaraa, Sherali, and Shetty)
if reduction_ratio > 0.75:
mu = decrease_mu(mu)
elif reduction_ratio < 0.25:
mu = increase_mu(mu)
# Accept proposal.
x = xnew
r = rnew
# Print final diagnostics.
if verbose > Verbosity.SILENT.value:
message = f'Terminated after {i} iterations: '
message += _STATUS_MESSAGE[status]
message += f' Residual evals: {n_res:d}'
if n_jac > 0:
message += f', Jacobian evals: {n_jac:d}'
print(message, file=output)
time_total = time.time() - t_start_total
if time_total > 0:
qp_percent = 100 * t_qp / time_total
r_percent = 100 * t_res / time_total
time_scale = 1 if time_total > 1 else 1000
time_units = 's' if time_total > 1 else 'ms'
message = f'total time {time_scale * time_total:<.1f}{time_units}'
message += f' of which QP {qp_percent:<.1f}%, residual {r_percent:<.1f}%'
if t_jac > 0:
jac_percent = 100 * t_jac / time_total
message += f' Jacobian {jac_percent:<.1f}%'
print(message, file=output)
return x, trace
python/mujoco/minimize_test.py
+145
View File
@@ -0,0 +1,145 @@
# Copyright 2024 DeepMind Technologies Limited
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
# ==============================================================================
"""Tests for minimize.py."""
import io
from typing import Tuple
from absl.testing import absltest
from mujoco import minimize
import numpy as np
class MinimizeTest(absltest.TestCase):
def test_basic(self) -> None:
def residual(x: np.ndarray) -> float:
return np.array([1 - x[0], 10 * (x[1] - x[0] ** 2)])
out = io.StringIO()
x0 = np.array((0.0, 0.0))
x, _ = minimize.least_squares(x0, residual, output=out)
expected_x = np.array((1.0, 1.0))
np.testing.assert_array_almost_equal(x, expected_x)
self.assertContainsSubsequence(out.getvalue(), 'norm(step) < tol')
def test_start_at_minimum(self) -> None:
def residual(x: np.ndarray) -> float:
return np.array([1 - x[0], 10 * (x[1] - x[0] ** 2)])
out = io.StringIO()
x0 = np.array((1.0, 1.0))
x, _ = minimize.least_squares(x0, residual, output=out)
expected_x = np.array((1.0, 1.0))
np.testing.assert_array_almost_equal(x, expected_x)
self.assertContainsSubsequence(out.getvalue(), 'norm(step) < tol')
self.assertContainsSubsequence(out.getvalue(), 'exact minimum found')
def test_jac_callback(self) -> None:
def residual(x: np.ndarray) -> float:
return np.array([1 - x[0], 10 * (x[1] - x[0] ** 2)])
def jacobian(x: np.ndarray, r: np.ndarray) -> Tuple[float, np.ndarray]:
del r # Unused.
return np.array([[-1, 0], [-20 * x[0], 10]])
x0 = np.array((0.0, 0.0))
out = io.StringIO()
x, _ = minimize.least_squares(x0, residual, jacobian=jacobian, output=out)
expected_x = np.array((1.0, 1.0))
np.testing.assert_array_almost_equal(x, expected_x)
self.assertContainsSubsequence(out.getvalue(), 'norm(step) < tol')
# Try with bad Jacobian, expect no improvement.
def jac_bad1(x: np.ndarray, r: np.ndarray) -> Tuple[float, np.ndarray]:
return -jacobian(x, r)
out1 = io.StringIO()
minimize.least_squares(x0, residual, jacobian=jac_bad1, output=out1)
self.assertContainsSubsequence(out1.getvalue(), 'no improvement found.')
def test_max_iter(self) -> None:
dim = 20 # High-D Rosenbrock
def residual(x: np.ndarray) -> float:
res0 = [1 - x[i] for i in range(dim - 1)]
res1 = [10 * (x[i] - x[i + 1] ** 2) for i in range(dim - 1)]
return np.asarray(res0 + res1)
# Fail to reach minimum after 20 iterations.
x0 = np.zeros(dim)
out = io.StringIO()
minimize.least_squares(x0, residual, max_iter=20, output=out)
self.assertContainsSubsequence(out.getvalue(), 'maximum iterations')
# Succeed after 100 iterations (default).
x, _ = minimize.least_squares(x0, residual)
expected_x = np.ones(20)
np.testing.assert_array_almost_equal(x, expected_x)
def test_bounds(self) -> None:
def residual(x: np.ndarray) -> float:
return np.array([1 - x[0], 10 * (x[1] - x[0] ** 2)])
out = io.StringIO()
x0 = np.array((0.0, 0.0))
expected_x = np.array((1.0, 1.0))
bounds_types = {'inbounds': [np.array((-2.0, -2.0)), np.array((2.0, 2.0))],
'onlower': [np.array((-2.0, 2.0)), np.array((0.5, 3.0))],
'onupper': [np.array((-2.0, -2.0)), np.array((0.5, 2.0))]}
# In bounds finds true minimum.
x, _ = minimize.least_squares(x0, residual, bounds=bounds_types['inbounds'],
output=out)
np.testing.assert_array_almost_equal(x, expected_x)
self.assertContainsSubsequence(out.getvalue(), 'norm(step) < tol')
# Test different bounds conditions.
verbose = minimize.Verbosity.FULLITER
for bounds in bounds_types.values():
out = io.StringIO()
x, trace = minimize.least_squares(x0, residual, bounds=bounds, output=out,
verbose=verbose)
self.assertContainsSubsequence(out.getvalue(), 'norm(step) < tol')
grad = trace[-1].jacobian.T @ trace[-1].residual
# If x_i is on the boundary, gradient points out, otherwise it is 0.
for i, xi in enumerate(x):
if xi == bounds[0][i]:
self.assertGreater(grad[i], 0)
elif xi == bounds[1][i]:
self.assertLess(grad[i], 0)
else:
self.assertAlmostEqual(grad[i], 0, places=4)
def test_bad_bounds(self) -> None:
def residual(x: np.ndarray) -> float:
return np.array([1 - x[0], 10 * (x[1] - x[0] ** 2)])
out = io.StringIO()
x0 = np.array((0.0, 0.0))
bad_bounds = [
[0, 1, 2],
[np.array((-2, 2, 0)), np.array((0.5, 3, 4))],
[np.array((-2, 2, 0)), np.array((0.5, 3, np.inf))],
[np.array((-2, 2, 0)), np.array((-5, 3, 6))],
]
for bounds in bad_bounds:
with self.assertRaises(ValueError):
minimize.least_squares(x0, residual, bounds=bounds, output=out)
if __name__ == '__main__':
absltest.main()
python/mujoco/render.cc
+6 -1
View File
@@ -14,6 +14,8 @@
#include <array>
#include <cstdint>
#include <optional>
#include <type_traits>
#include <Eigen/Core>
#include <mujoco/mjrender.h>
@@ -40,7 +42,10 @@ class MjWrapper<raw::MjrContext> : public WrapperBase<raw::MjrContext> {
void Free();
#define X(var) py_array_or_tuple_t<mjtNum> var
#define X(var) \
py_array_or_tuple_t< \
std::remove_all_extents_t<decltype(raw::MjrContext::var)>> \
var
X(fogRGBA);
X(auxWidth);
X(auxHeight);
python/mujoco/structs.h
+52 -12
View File
@@ -17,12 +17,17 @@
#include <array>
#include <cstddef>
#include <functional>
#include <istream>
#include <memory>
#include <optional>
#include <ostream>
#include <sstream>
#include <string_view>
#include <unordered_map>
#include <string>
#include <type_traits>
#include <utility>
#include <vector>
#include <absl/types/span.h>
@@ -36,6 +41,7 @@
namespace mujoco::python {
namespace _impl {
template <typename T>
class WrapperBase {
public:
@@ -44,7 +50,7 @@ class WrapperBase {
T* get() { return ptr_; }
const T* get() const { return ptr_; }
const pybind11::handle owner() const { return owner_; }
pybind11::handle owner() const { return owner_; }
protected:
static void DefaultCapsuleDestructor(PyObject* pyobj) {
@@ -167,7 +173,10 @@ class MjWrapper<raw::MjOption> : public WrapperBase<raw::MjOption> {
MjWrapper(raw::MjOption* ptr, pybind11::handle owner);
~MjWrapper() = default;
#define X(var, dim) py_array_or_tuple_t<mjtNum> var;
#define X(var, dim) \
py_array_or_tuple_t< \
std::remove_all_extents_t<decltype(raw::MjOption::var)>> \
var;
MJOPTION_VECTORS
#undef X
};
@@ -188,7 +197,10 @@ class MjWrapper<raw::MjVisualHeadlight>
MjWrapper(raw::MjVisualHeadlight* ptr, pybind11::handle owner);
~MjWrapper() = default;
#define X(var) py_array_or_tuple_t<float> var
#define X(var) \
py_array_or_tuple_t< \
std::remove_all_extents_t<decltype(raw::MjVisualHeadlight::var)>> \
var
X(ambient);
X(diffuse);
X(specular);
@@ -209,7 +221,10 @@ class MjWrapper<raw::MjVisualRgba> : public WrapperBase<raw::MjVisualRgba> {
MjWrapper(raw::MjVisualRgba* ptr, pybind11::handle owner);
~MjWrapper() = default;
#define X(var) py_array_or_tuple_t<float> var
#define X(var) \
py_array_or_tuple_t< \
std::remove_all_extents_t<decltype(raw::MjVisualRgba::var)>> \
var
X(fog);
X(haze);
X(force);
@@ -271,7 +286,10 @@ class MjWrapper<raw::MjStatistic> : public WrapperBase<raw::MjStatistic> {
MjWrapper(raw::MjStatistic* ptr, pybind11::handle owner);
~MjWrapper() = default;
#define X(var) py_array_or_tuple_t<mjtNum> var
#define X(var) \
py_array_or_tuple_t< \
std::remove_all_extents_t<decltype(raw::MjStatistic::var)>> \
var
X(center);
#undef X
};
@@ -504,7 +522,10 @@ class MjWrapper<raw::MjContact> : public WrapperBase<raw::MjContact> {
MjWrapper(raw::MjContact* ptr, pybind11::handle owner);
~MjWrapper() = default;
#define X(var) py_array_or_tuple_t<mjtNum> var
#define X(var) \
py_array_or_tuple_t< \
std::remove_all_extents_t<decltype(raw::MjContact::var)>> \
var
X(pos);
X(frame);
X(friction);
@@ -628,7 +649,10 @@ class MjWrapper<raw::MjvPerturb> : public WrapperBase<raw::MjvPerturb> {
MjWrapper(MjWrapper&&) = default;
~MjWrapper() = default;
#define X(var) py_array_or_tuple_t<mjtNum> var
#define X(var) \
py_array_or_tuple_t< \
std::remove_all_extents_t<decltype(raw::MjvPerturb::var)>> \
var
X(refpos);
X(refquat);
X(refselpos);
@@ -650,7 +674,10 @@ class MjWrapper<raw::MjvCamera> : public WrapperBase<raw::MjvCamera> {
MjWrapper(MjWrapper&&) = default;
~MjWrapper() = default;
#define X(var) py_array_or_tuple_t<mjtNum> var
#define X(var) \
py_array_or_tuple_t< \
std::remove_all_extents_t<decltype(raw::MjvCamera::var)>> \
var
X(lookat);
#undef X
};
@@ -671,7 +698,10 @@ class MjWrapper<raw::MjvGLCamera> : public WrapperBase<raw::MjvGLCamera> {
explicit MjWrapper(raw::MjvGLCamera&& other);
~MjWrapper() = default;
#define X(var) py_array_or_tuple_t<float> var
#define X(var) \
py_array_or_tuple_t< \
std::remove_all_extents_t<decltype(raw::MjvGLCamera::var)>> \
var
X(pos);
X(forward);
X(up);
@@ -693,7 +723,10 @@ class MjWrapper<raw::MjvGeom> : public WrapperBase<raw::MjvGeom> {
MjWrapper(raw::MjvGeom* ptr, pybind11::handle owner);
~MjWrapper() = default;
#define X(var) py_array_or_tuple_t<float> var
#define X(var) \
py_array_or_tuple_t< \
std::remove_all_extents_t<decltype(raw::MjvGeom::var)>> \
var
X(texrepeat);
X(size);
X(pos);
@@ -717,7 +750,10 @@ class MjWrapper<raw::MjvLight> : public WrapperBase<raw::MjvLight> {
MjWrapper(raw::MjvLight* ptr, pybind11::handle owner);
~MjWrapper() = default;
#define X(var) py_array_or_tuple_t<float> var
#define X(var) \
py_array_or_tuple_t< \
std::remove_all_extents_t<decltype(raw::MjvLight::var)>> \
var
X(pos);
X(dir);
X(attenuation);
@@ -741,7 +777,10 @@ class MjWrapper<raw::MjvOption> : public WrapperBase<raw::MjvOption> {
MjWrapper(MjWrapper&&) = default;
~MjWrapper() = default;
#define X(var) py_array_or_tuple_t<mjtByte> var
#define X(var) \
py_array_or_tuple_t< \
std::remove_all_extents_t<decltype(raw::MjvOption::var)>> \
var
X(geomgroup);
X(sitegroup);
X(jointgroup);
@@ -862,6 +901,7 @@ class ScopedMsanDisabler {
void* shadow_;
};
#endif
} // namespace _impl
template <typename T>
simulate/simulate.cc
+1 -8
View File
@@ -1969,14 +1969,7 @@ void Simulate::Sync() {
}
if (pending_.load_key) {
int i = this->key;
d_->time = m_->key_time[i];
mju_copy(d_->qpos, m_->key_qpos + i*m_->nq, m_->nq);
mju_copy(d_->qvel, m_->key_qvel + i*m_->nv, m_->nv);
mju_copy(d_->act, m_->key_act + i*m_->na, m_->na);
mju_copy(d_->mocap_pos, m_->key_mpos + i*3*m_->nmocap, 3*m_->nmocap);
mju_copy(d_->mocap_quat, m_->key_mquat + i*4*m_->nmocap, 4*m_->nmocap);
mju_copy(d_->ctrl, m_->key_ctrl + i*m_->nu, m_->nu);
mj_resetDataKeyframe(m_, d_, this->key);
mj_forward(m_, d_);
update_profiler = true;
update_sensor = true;
src/engine/engine_io.c
+5 -3
View File
@@ -722,8 +722,10 @@ mjModel* mj_loadModel(const char* filename, const mjVFS* vfs) {
mjResource* r = NULL;
// first try vfs, otherwise try a provider or OS filesystem
if ((r = mju_openVfsResource(filename, vfs)) == NULL) {
if ((r = mju_openResource(filename)) == NULL) {
if (!(r = mju_openVfsResource(filename, vfs))) {
char error[1024];
if (!(r = mju_openResource(filename, error, 1024))) {
mju_warning("%s", error);
return NULL;
}
}
@@ -1699,7 +1701,7 @@ void mj_resetDataDebug(const mjModel* m, mjData* d, unsigned char debug_value) {
// reset data, set fields from specified keyframe
// Reset data. If 0 <= key < nkey, set fields from specified keyframe.
void mj_resetDataKeyframe(const mjModel* m, mjData* d, int key) {
_resetData(m, d, 0);
src/engine/engine_io.h
+1 -1
View File
@@ -104,7 +104,7 @@ MJAPI void mj_resetData(const mjModel* m, mjData* d);
// set data to defaults, fill everything else with debug_value
MJAPI void mj_resetDataDebug(const mjModel* m, mjData* d, unsigned char debug_value);
// reset data, set fields from specified keyframe
// Reset data. If 0 <= key < nkey, set fields from specified keyframe.
MJAPI void mj_resetDataKeyframe(const mjModel* m, mjData* d, int key);
// mjData arena allocate
src/engine/engine_resource.c
+25 -17
View File
@@ -44,7 +44,12 @@ typedef struct {
// open the given resource; if the name doesn't have a prefix matching with a
// resource provider, then the OS filesystem is used
mjResource* mju_openResource(const char* name) {
mjResource* mju_openResource(const char* name, char* error, size_t error_sz) {
// no error so far
if (error) {
error[0] = '\0';
}
mjResource* resource = (mjResource*) mju_malloc(sizeof(mjResource));
const mjpResourceProvider* provider = NULL;
if (resource == NULL) {
@@ -72,9 +77,12 @@ mjResource* mju_openResource(const char* name) {
return resource;
}
mju_warning("mju_openResource: could not open resource '%s' "
"using a resource provider matching prefix '%s'",
name, provider->prefix);
if (error) {
snprintf(error, error_sz, "could not open '%s'"
"using a resource provider matching prefix '%s'",
name, provider->prefix);
}
mju_closeResource(resource);
return NULL;
}
@@ -85,7 +93,10 @@ mjResource* mju_openResource(const char* name) {
file_buffer* fb = (file_buffer*) resource->data;
fb->buffer = mju_fileToMemory(name, &(fb->nbuffer));
if (fb->buffer == NULL) {
mju_warning("mju_openResource: unknown file '%s'", name);
if (error) {
snprintf(error, error_sz,
"resource not found via provider or OS filesystem: '%s'", name);
}
mju_closeResource(resource);
return NULL;
}
@@ -94,8 +105,9 @@ mjResource* mju_openResource(const char* name) {
memcpy(&fb->mtime, &file_stat.st_mtime, sizeof(time_t));
} else {
memset(&fb->mtime, 0, sizeof(time_t));
resource->timestamp[0] = '\0';
}
strftime(resource->timestamp, 512, "%Y-%m-%d-%H:%M:%S", localtime(&(fb->mtime)));
return resource;
}
@@ -175,24 +187,20 @@ static int mju_isModifiedFile(const char* name, const file_buffer* fb) {
if (stat(name, &file_stat) == 0) {
return difftime(fb->mtime, file_stat.st_mtime) < 0;
}
return -1;
}
return -2;
return 1; // modified (default)
}
// Returns > 0 if resource has been modified since last read, 0 if not, and < 0
// if inconclusive
int mju_isModifiedResource(const mjResource* resource) {
if (resource == NULL) {
return -2;
}
// return 0 if the resource's timestamp matches the provided timestamp
// return > 0 if the the resource is younger than the given timestamp
// return < 0 if the resource is older than the given timestamp
int mju_isModifiedResource(const mjResource* resource, const char* timestamp) {
// provider is not OS filesystem
if (resource->provider) {
if (resource->provider->modified) {
return resource->provider->modified(resource);
return resource->provider->modified(resource, timestamp);
}
return 1; // default (modified)
}
@@ -209,7 +217,7 @@ int mju_dirnamelen(const char* path) {
}
int pos = -1;
for (int i = 0; path[i] && i >= 0; ++i) {
for (int i = 0; path[i]; ++i) {
if (path[i] == '/' || path[i] == '\\') {
pos = i;
}
src/engine/engine_resource.h
+7 -6
View File
@@ -18,7 +18,7 @@
#include <stddef.h>
#include <mujoco/mjexport.h>
#include "engine/engine_plugin.h"
#include <mujoco/mujoco.h>
#ifdef __cplusplus
extern "C" {
@@ -26,7 +26,7 @@ extern "C" {
// open the given resource; if the name doesn't have a prefix matching with a
// resource provider, then the OS filesystem is used
MJAPI mjResource* mju_openResource(const char* name);
MJAPI mjResource* mju_openResource(const char* name, char* error, size_t error_sz);
// close the given resource; no-op if resource is NULL
MJAPI void mju_closeResource(mjResource* resource);
@@ -35,12 +35,13 @@ MJAPI void mju_closeResource(mjResource* resource);
// return negative value if error
MJAPI int mju_readResource(mjResource* resource, const void** buffer);
// sets for a resource with a name partitioned as {dir}{filename}, the dir and ndir pointers
// set for a resource with a name partitioned as {dir}{filename}, the dir and ndir pointers
MJAPI void mju_getResourceDir(mjResource* resource, const char** dir, int* ndir);
// Returns > 0 if resource has been modified since last read, 0 if not, and < 0
// if inconclusive
MJAPI int mju_isModifiedResource(const mjResource* resource);
// return 0 if the resource's timestamp matches the provided timestamp
// return > 0 if the the resource is younger than the given timestamp
// return < 0 if the resource is older than the given timestamp
MJAPI int mju_isModifiedResource(const mjResource* resource, const char* timestamp);
// get the length of the dirname portion of a given path
int mju_dirnamelen(const char* path);
src/engine/engine_vfs.c
+90 -2
View File
@@ -15,7 +15,9 @@
#include "engine/engine_vfs.h"
#include <stddef.h>
#include <stdint.h>
#include <string.h>
#include <stdio.h>
#include "engine/engine_array_safety.h"
#include "engine/engine_resource.h"
@@ -55,6 +57,38 @@ static void vfs_strippath(char* newname, const char* oldname) {
// copies data into a buffer and produces a hash of the data
static uint64_t vfs_memcpy(void* dest, const void* restrict src, size_t n) {
uint64_t hash = 0xcbf29ce484222325; // magic number
uint64_t prime = 0x100000001b3; // magic prime
const uint8_t* bytes = (uint8_t*) src;
uint8_t* buffer = (uint8_t*) dest;
for (size_t i = 0; i < n; i++) {
buffer[i] = bytes[i];
// do FNV-1 hash
hash |= bytes[i];
hash *= prime;
}
return hash;
}
// VFS hash function implemented using the FNV-1 hash
static uint64_t vfs_hash(const void* restrict buffer, size_t n) {
uint64_t hash = 0xcbf29ce484222325; // magic number
uint64_t prime = 0x100000001b3; // magic prime
const uint8_t* bytes = (uint8_t*) buffer;
for (size_t i = 0; i < n; i++) {
hash |= bytes[i];
hash *= prime;
}
return hash;
}
// initialize to empty (no deallocation)
void mj_defaultVFS(mjVFS* vfs) {
memset(vfs, 0, sizeof(mjVFS));
@@ -95,7 +129,9 @@ int mj_addFileVFS(mjVFS* vfs, const char* directory, const char* filename) {
if (!vfs->filedata[vfs->nfile]) {
return -1;
}
// assign size and count
// assign size, count, and checksum
vfs->filestamp[vfs->nfile] = vfs_hash(vfs->filedata[vfs->nfile], filesize);
vfs->filesize[vfs->nfile] = filesize;
vfs->nfile++;
@@ -136,6 +172,7 @@ int mj_makeEmptyFileVFS(mjVFS* vfs, const char* filename, int filesize) {
mjERROR("could not allocate memory");
}
memset(vfs->filedata[vfs->nfile], 0, filesize);
vfs->filestamp[vfs->nfile] = 0;
// assign size and count
vfs->filesize[vfs->nfile] = filesize;
@@ -146,6 +183,51 @@ int mj_makeEmptyFileVFS(mjVFS* vfs, const char* filename, int filesize) {
// add file from buffer into VFS
int mj_addBufferVFS(mjVFS* vfs, const char* name, const void* buffer, int nbuffer) {
if (!vfs || !buffer || !name) {
mjERROR("null pointer");
}
if (vfs->nfile >= mjMAXVFS-1) {
return 1;
}
// check buffer size
if (nbuffer <= 0) {
mjERROR("expects positive buffer size");
}
// strip path
char newname[mjMAXVFSNAME];
vfs_strippath(newname, name);
// check for repeated name
for (int i=0; i < vfs->nfile; i++) {
if (strncmp(newname, vfs->filename[i], mjMAXVFSNAME) == 0) {
return 2;
}
}
// assign name
mjSTRNCPY(vfs->filename[vfs->nfile], newname);
// allocate and clear
vfs->filedata[vfs->nfile] = mju_malloc(nbuffer);
if (!vfs->filedata[vfs->nfile]) {
mjERROR("could not allocate memory");
}
vfs->filestamp[vfs->nfile] = vfs_memcpy(vfs->filedata[vfs->nfile], buffer, nbuffer);
// assign size and count
vfs->filesize[vfs->nfile] = nbuffer;
vfs->nfile++;
return 0;
}
// return file index in VFS, or -1 if not found in VFS
int mj_findFileVFS(const mjVFS* vfs, const char* filename) {
// strip path
@@ -216,7 +298,13 @@ static int vfs_open_callback(mjResource* resource) {
}
const mjVFS* vfs = (const mjVFS*) resource->data;
return mj_findFileVFS(vfs, resource->name) >= 0;
int i = mj_findFileVFS(vfs, resource->name);
resource->timestamp[0] = '\0';
if (i >= 0 && vfs->filestamp[i]) {
mju_encodeBase64(resource->timestamp, (uint8_t*) &vfs->filestamp[i],
sizeof(uint64_t));
}
return i >= 0;
}
src/engine/engine_vfs.h
+6 -1
View File
@@ -15,6 +15,8 @@
#ifndef MUJOCO_SRC_ENGINE_ENGINE_VFS_H_
#define MUJOCO_SRC_ENGINE_ENGINE_VFS_H_
#include <stddef.h>
#include <mujoco/mjexport.h>
#include <mujoco/mjmodel.h>
#include <mujoco/mjplugin.h>
@@ -29,9 +31,12 @@ MJAPI void mj_defaultVFS(mjVFS* vfs);
// add file to VFS, return 0: success, 1: full, 2: repeated name, -1: not found on disk
MJAPI int mj_addFileVFS(mjVFS* vfs, const char* directory, const char* filename);
// make empty file in VFS, return 0: success, 1: full, 2: repeated name
// deprecated: use mj_copyBufferVFS
MJAPI int mj_makeEmptyFileVFS(mjVFS* vfs, const char* filename, int filesize);
// add file from buffer into VFS, return 0: success, 1: full, 2: repeated name, -1: failed to load
MJAPI int mj_copyBufferVFS(mjVFS* vfs, const char* filename, const void* buffer, int nbuffer);
// return file index in VFS, or -1 if not found in VFS
MJAPI int mj_findFileVFS(const mjVFS* vfs, const char* filename);
src/user/user_api.cc
+245 -46
View File
@@ -13,26 +13,30 @@
// limitations under the License.
#include "user/user_api.h"
#include <functional>
#include <map>
#include <string>
#include <utility>
#include <vector>
#include <mujoco/mujoco.h>
#include "user/user_model.h"
#include "user/user_objects.h"
#include "xml/xml_util.h"
// create model
void* mjm_createModel() {
mjmModel* mjm_createModel() {
mjCModel* modelC = new mjCModel();
return modelC;
return &modelC->spec;
}
// delete model
void mjm_deleteModel(void* modelspec) {
mjCModel* model = static_cast<mjCModel*>(modelspec);
void mjm_deleteModel(mjmModel* modelspec) {
mjCModel* model = reinterpret_cast<mjCModel*>(modelspec->element);
delete model;
}
@@ -47,8 +51,8 @@ MJAPI void mjm_finalize(mjElement object) {
// add child body to body, return child spec
mjmBody* mjm_addBody(mjmBody* bodyspec, void* defspec) {
mjCDef* def = static_cast<mjCDef*>(defspec);
mjmBody* mjm_addBody(mjmBody* bodyspec, mjmDefault* defspec) {
mjCDef* def = defspec ? reinterpret_cast<mjCDef*>(defspec->element) : 0;
mjCBody* body = reinterpret_cast<mjCBody*>(bodyspec->element)->AddBody(def);
return &body->spec;
}
@@ -56,8 +60,8 @@ mjmBody* mjm_addBody(mjmBody* bodyspec, void* defspec) {
// add site to body, return site spec
mjmSite* mjm_addSite(mjmBody* bodyspec, void* defspec) {
mjCDef* def = static_cast<mjCDef*>(defspec);
mjmSite* mjm_addSite(mjmBody* bodyspec, mjmDefault* defspec) {
mjCDef* def = defspec ? reinterpret_cast<mjCDef*>(defspec->element) : 0;
mjCBody* body = reinterpret_cast<mjCBody*>(bodyspec->element);
mjCSite* site = body->AddSite(def);
return &site->spec;
@@ -66,8 +70,8 @@ mjmSite* mjm_addSite(mjmBody* bodyspec, void* defspec) {
// add joint to body
mjmJoint* mjm_addJoint(mjmBody* bodyspec, void* defspec) {
mjCDef* def = static_cast<mjCDef*>(defspec);
mjmJoint* mjm_addJoint(mjmBody* bodyspec, mjmDefault* defspec) {
mjCDef* def = defspec ? reinterpret_cast<mjCDef*>(defspec->element) : 0;
mjCBody* body = reinterpret_cast<mjCBody*>(bodyspec->element);
mjCJoint* joint = body->AddJoint(def);
return &joint->spec;
@@ -85,8 +89,8 @@ mjmJoint* mjm_addFreeJoint(mjmBody* bodyspec) {
// add geom to body
mjmGeom* mjm_addGeom(mjmBody* bodyspec, void* defspec) {
mjCDef* def = static_cast<mjCDef*>(defspec);
mjmGeom* mjm_addGeom(mjmBody* bodyspec, mjmDefault* defspec) {
mjCDef* def = defspec ? reinterpret_cast<mjCDef*>(defspec->element) : 0;
mjCBody* body = reinterpret_cast<mjCBody*>(bodyspec->element);
mjCGeom* geom = body->AddGeom(def);
return &geom->spec;
@@ -95,8 +99,8 @@ mjmGeom* mjm_addGeom(mjmBody* bodyspec, void* defspec) {
// add camera to body
mjmCamera* mjm_addCamera(mjmBody* bodyspec, void* defspec) {
mjCDef* def = static_cast<mjCDef*>(defspec);
mjmCamera* mjm_addCamera(mjmBody* bodyspec, mjmDefault* defspec) {
mjCDef* def = defspec ? reinterpret_cast<mjCDef*>(defspec->element) : 0;
mjCBody* body = reinterpret_cast<mjCBody*>(bodyspec->element);
mjCCamera* camera = body->AddCamera(def);
return &camera->spec;
@@ -105,8 +109,8 @@ mjmCamera* mjm_addCamera(mjmBody* bodyspec, void* defspec) {
// add light to body
mjmLight* mjm_addLight(mjmBody* bodyspec, void* defspec) {
mjCDef* def = static_cast<mjCDef*>(defspec);
mjmLight* mjm_addLight(mjmBody* bodyspec, mjmDefault* defspec) {
mjCDef* def = defspec ? reinterpret_cast<mjCDef*>(defspec->element) : 0;
mjCBody* body = reinterpret_cast<mjCBody*>(bodyspec->element);
mjCLight* light = body->AddLight(def);
return &light->spec;
@@ -114,6 +118,15 @@ mjmLight* mjm_addLight(mjmBody* bodyspec, void* defspec) {
// add flex to model
mjmFlex* mjm_addFlex(mjmModel* model) {
mjCModel* modelC = reinterpret_cast<mjCModel*>(model->element);
mjCFlex* flex = modelC->AddFlex();
return &flex->spec;
}
// add frame to body
mjmFrame* mjm_addFrame(mjmBody* bodyspec, mjmFrame* parentframe) {
mjCFrame* parentframeC = 0;
@@ -127,20 +140,76 @@ mjmFrame* mjm_addFrame(mjmBody* bodyspec, mjmFrame* parentframe) {
// Add material to model.
mjmMaterial* mjm_addMaterial(void* model, void* defspec) {
mjCModel* modelC = static_cast<mjCModel*>(model);
mjCDef* def = static_cast<mjCDef*>(defspec);
// add mesh to model
mjmMesh* mjm_addMesh(mjmModel* model, mjmDefault* defspec) {
mjCDef* def = defspec ? reinterpret_cast<mjCDef*>(defspec->element) : 0;
mjCModel* modelC = reinterpret_cast<mjCModel*>(model->element);
mjCMesh* mesh = modelC->AddMesh(def);
return &mesh->spec;
}
// add height field to model
mjmHField* mjm_addHField(mjmModel* model) {
mjCModel* modelC = reinterpret_cast<mjCModel*>(model->element);
mjCHField* heightField = modelC->AddHField();
return &heightField->spec;
}
// add skin to model
mjmSkin* mjm_addSkin(mjmModel* model) {
mjCModel* modelC = reinterpret_cast<mjCModel*>(model->element);
mjCSkin* skin = modelC->AddSkin();
return &skin->spec;
}
// add texture to model
mjmTexture* mjm_addTexture(mjmModel* model) {
mjCModel* modelC = reinterpret_cast<mjCModel*>(model->element);
mjCTexture* texture = modelC->AddTexture();
return &texture->spec;
}
// add material to model
mjmMaterial* mjm_addMaterial(mjmModel* model, mjmDefault* defspec) {
mjCModel* modelC = reinterpret_cast<mjCModel*>(model->element);
mjCDef* def = defspec ? reinterpret_cast<mjCDef*>(defspec->element) : 0;
mjCMaterial* material = modelC->AddMaterial(def);
return &material->spec;
}
// add pair to model
mjmPair* mjm_addPair(mjmModel* model, mjmDefault* defspec) {
mjCModel* modelC = reinterpret_cast<mjCModel*>(model->element);
mjCDef* def = defspec ? reinterpret_cast<mjCDef*>(defspec->element) : 0;
mjCPair* pair = modelC->AddPair(def);
return &pair->spec;
}
// add pair exclusion to model
mjmExclude* mjm_addExclude(mjmModel* model) {
mjCModel* modelC = reinterpret_cast<mjCModel*>(model->element);
mjCBodyPair* bodypair = modelC->AddExclude();
return &bodypair->spec;
}
// add equality to model
mjmEquality* mjm_addEquality(void* model, void* defspec) {
mjCModel* modelC = static_cast<mjCModel*>(model);
mjCDef* def = static_cast<mjCDef*>(defspec);
mjmEquality* mjm_addEquality(mjmModel* model, mjmDefault* defspec) {
mjCModel* modelC = reinterpret_cast<mjCModel*>(model->element);
mjCDef* def = defspec ? reinterpret_cast<mjCDef*>(defspec->element) : 0;
mjCEquality* equality = modelC->AddEquality(def);
return &equality->spec;
}
@@ -148,9 +217,9 @@ mjmEquality* mjm_addEquality(void* model, void* defspec) {
// add tendon to model
mjmTendon* mjm_addTendon(void* model, void* defspec) {
mjCModel* modelC = static_cast<mjCModel*>(model);
mjCDef* def = static_cast<mjCDef*>(defspec);
mjmTendon* mjm_addTendon(mjmModel* model, mjmDefault* defspec) {
mjCModel* modelC = reinterpret_cast<mjCModel*>(model->element);
mjCDef* def = defspec ? reinterpret_cast<mjCDef*>(defspec->element) : 0;
mjCTendon* tendon = modelC->AddTendon(def);
return &tendon->spec;
}
@@ -194,9 +263,9 @@ mjmWrap* mjm_wrapPulley(mjmTendon* tendonspec, double divisor) {
// add actuator to model
mjmActuator* mjm_addActuator(void* model, void* defspec) {
mjCModel* modelC = static_cast<mjCModel*>(model);
mjCDef* def = static_cast<mjCDef*>(defspec);
mjmActuator* mjm_addActuator(mjmModel* model, mjmDefault* defspec) {
mjCModel* modelC = reinterpret_cast<mjCModel*>(model->element);
mjCDef* def = defspec ? reinterpret_cast<mjCDef*>(defspec->element) : 0;
mjCActuator* actuator = modelC->AddActuator(def);
return &actuator->spec;
}
@@ -204,40 +273,90 @@ mjmActuator* mjm_addActuator(void* model, void* defspec) {
// add sensor to model
mjmSensor* mjm_addSensor(void* model) {
mjCModel* modelC = static_cast<mjCModel*>(model);
mjmSensor* mjm_addSensor(mjmModel* model) {
mjCModel* modelC = reinterpret_cast<mjCModel*>(model->element);
mjCSensor* sensor = modelC->AddSensor();
return &sensor->spec;
}
// Add plugin to model.
mjElement mjm_addPlugin(void* model) {
mjCModel* modelC = static_cast<mjCModel*>(model);
// add numeric to model
mjmNumeric* mjm_addNumeric(mjmModel* model) {
mjCModel* modelC = reinterpret_cast<mjCModel*>(model->element);
mjCNumeric* numeric = modelC->AddNumeric();
return &numeric->spec;
}
// add text to model
mjmText* mjm_addText(mjmModel* model) {
mjCModel* modelC = reinterpret_cast<mjCModel*>(model->element);
mjCText* text = modelC->AddText();
return &text->spec;
}
// add tuple to model
mjmTuple* mjm_addTuple(mjmModel* model) {
mjCModel* modelC = reinterpret_cast<mjCModel*>(model->element);
mjCTuple* tuple = modelC->AddTuple();
return &tuple->spec;
}
// add keyframe to model
mjmKey* mjm_addKey(mjmModel* model) {
mjCModel* modelC = reinterpret_cast<mjCModel*>(model->element);
mjCKey* key = modelC->AddKey();
return &key->spec;
}
// add plugin to model
mjmPlugin* mjm_addPlugin(mjmModel* model) {
mjCModel* modelC = reinterpret_cast<mjCModel*>(model->element);
mjCPlugin* plugin = modelC->AddPlugin();
return (mjElement)plugin;
plugin->spec.instance = (mjElement)plugin;
return &plugin->spec;
}
// add default to model
mjmDefault* mjm_addDefault(mjmModel* model, const char* classname, int parentid) {
mjCModel* modelC = reinterpret_cast<mjCModel*>(model->element);
mjCDef* def = modelC->AddDef(classname, parentid);
if (def) {
return &def->spec;
} else {
return nullptr;
}
}
// get objects
void* mjm_getModel(mjmBody* bodyspec) {
return reinterpret_cast<mjCBody*>(bodyspec->element)->model;
mjmModel* mjm_getModel(mjmBody* bodyspec) {
return &(reinterpret_cast<mjCBody*>(bodyspec->element)->model->spec);
}
// get default
void* mjm_getDefault(mjElement element) {
return reinterpret_cast<mjCBase*>(element)->def;
mjmDefault* mjm_getDefault(mjElement element) {
return &(reinterpret_cast<mjCBase*>(element)->def->spec);
}
// find body in model by name
mjmBody* mjm_findBody(void* modelspec, const char* name) {
mjCModel* model = static_cast<mjCModel*>(modelspec);
mjmBody* mjm_findBody(mjmModel* modelspec, const char* name) {
mjCModel* model = reinterpret_cast<mjCModel*>(modelspec->element);
mjCBase* body = model->FindObject(mjOBJ_BODY, std::string(name));
if (!body) {
return 0;
@@ -277,13 +396,16 @@ int mjm_getId(mjElement element) {
}
// set default
void mjm_setDefault(mjElement element, void* defspec) {
void mjm_setDefault(mjElement element, mjmDefault* defspec) {
mjCBase* baseC = reinterpret_cast<mjCBase*>(element);
baseC->def = static_cast<mjCDef*>(defspec);
baseC->def = reinterpret_cast<mjCDef*>(defspec->element);
baseC->def->PointToLocal();
}
// set string
void mjm_setString(mjString dest, const char* text) {
std::string* str = reinterpret_cast<std::string*>(dest);
@@ -292,8 +414,75 @@ void mjm_setString(mjString dest, const char* text) {
// set double array
void mjm_setDouble(mjDouble dest, const double* array, int size) {
// Set specific entry in destination string vector.
mjtByte mjm_setInStringVec(mjStringVec dest, int i, const char* text) {
std::vector<std::string>* v = reinterpret_cast<std::vector<std::string>*>(dest);
if (v->size() <= i) {
mju_error("Requested index in mjm_setInStringVec is out of bounds");
return 0;
}
v->at(i) = std::string(text);
return 1;
}
// split text and copy into string array
void mjm_setStringVec(mjStringVec dest, const char* text) {
std::vector<std::string>* v = reinterpret_cast<std::vector<std::string>*>(dest);
mjXUtil::String2Vector(text, *v);
}
// add text entry to destination string vector
void mjm_appendString(mjStringVec dest, const char* text) {
std::vector<std::string>* v = reinterpret_cast<std::vector<std::string>*>(dest);
v->push_back(std::string(text));
}
// copy int array to vector
void mjm_setInt(mjIntVec dest, const int* array, int size) {
std::vector<int>* v = reinterpret_cast<std::vector<int>*>(dest);
v->assign(size, 0.0);
for (int i = 0; i < size; ++i) {
(*v)[i] = array[i];
}
}
// append int array to vector of arrays
void mjm_appendIntVec(mjIntVecVec dest, const int* array, int size) {
std::vector<std::vector<int>>* v = reinterpret_cast<std::vector<std::vector<int>>*>(dest);
v->push_back(std::vector<int>(array, array + size));
}
// copy float array to vector
void mjm_setFloat(mjFloatVec dest, const float* array, int size) {
std::vector<float>* v = reinterpret_cast<std::vector<float>*>(dest);
v->assign(size, 0.0);
for (int i = 0; i < size; ++i) {
(*v)[i] = array[i];
}
}
// append float array to vector of arrays
void mjm_appendFloatVec(mjFloatVecVec dest, const float* array, int size) {
std::vector<std::vector<float>>* v = reinterpret_cast<std::vector<std::vector<float>>*>(dest);
v->push_back(std::vector<float>(array, array + size));
}
// copy double array to vector
void mjm_setDouble(mjDoubleVec dest, const double* array, int size) {
std::vector<double>* v = reinterpret_cast<std::vector<double>*>(dest);
v->assign(size, 0.0);
for (int i = 0; i < size; ++i) {
@@ -312,7 +501,7 @@ const char* mjm_getString(const mjString source) {
// get double array
const double* mjm_getDouble(const mjDouble source, int* size) {
const double* mjm_getDouble(const mjDoubleVec source, int* size) {
std::vector<double>* v = reinterpret_cast<std::vector<double>*>(source);
if (size) {
*size = v->size();
@@ -322,6 +511,16 @@ const double* mjm_getDouble(const mjDouble source, int* size) {
// set plugin attributes
void mjm_setPluginAttributes(mjmPlugin* plugin, void* attributes) {
mjCPlugin* pluginC = reinterpret_cast<mjCPlugin*>(plugin->instance);
std::map<std::string, std::string, std::less<>>* config_attribs =
reinterpret_cast<std::map<std::string, std::string, std::less<>>*>(attributes);
pluginC->config_attribs = std::move(*config_attribs);
}
// compute full inertia
const char* mjm_setFullInertia(mjmBody* bodyspec, double quat[4], double inertia[3]) {
mjCBody* body = reinterpret_cast<mjCBody*>(bodyspec->element);
src/user/user_api.h
+450 -42
View File
@@ -15,6 +15,7 @@
#ifndef MUJOCO_SRC_USER_USER_API_H_
#define MUJOCO_SRC_USER_USER_API_H_
#include <stddef.h>
#include <mujoco/mjexport.h>
#include <mujoco/mjmodel.h>
#include <mujoco/mjtnum.h>
@@ -30,19 +31,103 @@ extern "C" {
typedef struct _mjElement* mjElement;
typedef struct _mjString* mjString;
typedef struct _mjDouble* mjDouble;
typedef struct _mjStringVec* mjStringVec;
typedef struct _mjIntVec* mjIntVec;
typedef struct _mjIntVecVec* mjIntVecVec;
typedef struct _mjFloatVec* mjFloatVec;
typedef struct _mjFloatVecVec* mjFloatVecVec;
typedef struct _mjDoubleVec* mjDoubleVec;
//---------------------------------- enum types (mjt) ----------------------------------------------
typedef enum _mjtGeomInertia { // type of inertia inference
mjINERTIA_VOLUME, // mass distributed in the volume
mjINERTIA_SHELL, // mass distributed on the surface
typedef enum _mjtGeomInertia { // type of inertia inference
mjINERTIA_VOLUME, // mass distributed in the volume
mjINERTIA_SHELL, // mass distributed on the surface
} mjtGeomInertia;
typedef enum _mjtBuiltin { // type of built-in procedural texture
mjBUILTIN_NONE = 0, // no built-in texture
mjBUILTIN_GRADIENT, // gradient: rgb1->rgb2
mjBUILTIN_CHECKER, // checker pattern: rgb1, rgb2
mjBUILTIN_FLAT // 2d: rgb1; cube: rgb1-up, rgb2-side, rgb3-down
} mjtBuiltin;
typedef enum _mjtMark { // mark type for procedural textures
mjMARK_NONE = 0, // no mark
mjMARK_EDGE, // edges
mjMARK_CROSS, // cross
mjMARK_RANDOM // random dots
} mjtMark;
typedef enum _mjtLimited { // type of limit specification
mjLIMITED_FALSE = 0, // not limited
mjLIMITED_TRUE, // limited
mjLIMITED_AUTO, // limited inferred from presence of range
} mjtLimited;
typedef enum _mjtInertiaFromGeom {
mjINERTIAFROMGEOM_FALSE = 0, // do not use; inertial element required
mjINERTIAFROMGEOM_TRUE, // always use; overwrite inertial element
mjINERTIAFROMGEOM_AUTO // use only if inertial element is missing
} mjtInertiaFromGeom;
//---------------------------------- attribute structs (mjm) ---------------------------------------
typedef struct _mjmModel { // model specification
mjElement element; // internal, do not modify
mjStatistic stat; // statistics override (if defined)
// compiler settings
mjtByte autolimits; // infer "limited" attribute based on range
double boundmass; // enforce minimum body mass
double boundinertia; // enforce minimum body diagonal inertia
double settotalmass; // rescale masses and inertias; <=0: ignore
mjtByte balanceinertia; // automatically impose A + B >= C rule
mjtByte strippath; // automatically strip paths from mesh files
mjtByte fitaabb; // meshfit to aabb instead of inertia box
mjtByte degree; // angles in radians or degrees
char euler[3]; // sequence for euler rotations
mjString meshdir; // mesh and hfield directory
mjString texturedir; // texture directory
mjtByte discardvisual; // discard visual geoms in parser
mjtByte convexhull; // compute mesh convex hulls
mjtByte usethread; // use multiple threads to speed up compiler
mjtByte fusestatic; // fuse static bodies with parent
int inertiafromgeom; // use geom inertias (mjtInertiaFromGeom)
int inertiagrouprange[2]; // range of geom groups used to compute inertia
mjtByte exactmeshinertia; // if false, use old formula
mjLROpt LRopt; // options for lengthrange computation
// engine data
mjString modelname; // model name
mjOption option; // options
mjVisual visual; // visual options
size_t memory; // size of arena+stack memory in bytes
int nemax; // max number of equality constraints
int njmax; // max number of constraints (Jacobian rows)
int nconmax; // max number of detected contacts (mjContact array size)
size_t nstack; // (deprecated) number of fields in mjData stack
int nuserdata; // number extra fields in mjData
int nuser_body; // number of mjtNums in body_user
int nuser_jnt; // number of mjtNums in jnt_user
int nuser_geom; // number of mjtNums in geom_user
int nuser_site; // number of mjtNums in site_user
int nuser_cam; // number of mjtNums in cam_user
int nuser_tendon; // number of mjtNums in tendon_user
int nuser_actuator; // number of mjtNums in actuator_user
int nuser_sensor; // number of mjtNums in sensor_user
// sizes
int nkey; // number of keyframes
} mjmModel;
typedef struct _mjmOrientation { // alternative orientation specifiers
double axisangle[4]; // rotation axis and angle
double xyaxes[6]; // x and y axes
@@ -55,7 +140,9 @@ typedef struct _mjmPlugin { // plugin specification
mjElement instance; // internal, do not modify
mjString name; // name
mjString instance_name; // instance name
bool active; // is the plugin active
int plugin_slot; // global registered slot number of the plugin
mjtByte active; // is the plugin active
mjString info; // message appended to compiler errors
} mjmPlugin;
@@ -80,7 +167,7 @@ typedef struct _mjmBody { // body specification
// other
mjtByte mocap; // is this a mocap body
double gravcomp; // gravity compensation
mjDouble userdata; // user data
mjDoubleVec userdata; // user data
mjtByte explicitinertial; // whether to save the body with explicit inertial clause
mjmPlugin plugin; // passive force plugin
mjString info; // message appended to compiler errors
@@ -113,12 +200,12 @@ typedef struct _mjmJoint { // joint specification
double springdamper[2]; // timeconst, dampratio
// limits
int limited; // does joint have limits
int limited; // does joint have limits (mjtLimited)
double range[2]; // joint limits
double margin; // margin value for joint limit detection
mjtNum solref_limit[mjNREF]; // solver reference: joint limits
mjtNum solimp_limit[mjNIMP]; // solver impedance: joint limits
int actfrclimited; // are actuator forces on joint limited
int actfrclimited; // are actuator forces on joint limited (mjtLimited)
double actfrcrange[2]; // actuator force limits
// dof properties
@@ -131,7 +218,7 @@ typedef struct _mjmJoint { // joint specification
// other
int group; // group
double urdfeffort; // effort (urdf)
mjDouble userdata; // user data
mjDoubleVec userdata; // user data
mjString info; // message appended to compiler errors
} mjmJoint;
@@ -179,7 +266,7 @@ typedef struct _mjmGeom { // geom specification
mjString hfieldname; // heightfield attached to geom
mjString meshname; // mesh attached to geom
double fitscale; // scale mesh uniformly
mjDouble userdata; // user data
mjDoubleVec userdata; // user data
mjmPlugin plugin; // sdf plugin
mjString info; // message appended to compiler errors
} mjmGeom;
@@ -204,7 +291,7 @@ typedef struct _mjmSite { // site specification
float rgba[4]; // rgba when material is omitted
// other
mjDouble userdata; // user data
mjDoubleVec userdata; // user data
mjString info; // message appended to compiler errors
} mjmSite;
@@ -233,7 +320,7 @@ typedef struct _mjmCamera { // camera specification
float principal_pixel[2]; // principal point (pixel)
// other
mjDouble userdata; // user data
mjDoubleVec userdata; // user data
mjString info; // message appended to compiler errors
} mjmCamera;
@@ -265,12 +352,149 @@ typedef struct _mjmLight { // light specification
} mjmLight;
typedef struct _mjmFlex {
mjElement element; // internal, do not modify
mjString name; // name
mjString classname; // class name
// contact properties
int contype; // contact type
int conaffinity; // contact affinity
int condim; // contact dimensionality
int priority; // contact priority
double friction[3]; // one-sided friction coefficients: slide, roll, spin
double solmix; // solver mixing for contact pairs
mjtNum solref[mjNREF]; // solver reference
mjtNum solimp[mjNIMP]; // solver impedance
double margin; // margin for contact detection
double gap; // include in solver if dist<margin-gap
// other properties
int dim; // element dimensionality
double radius; // radius around primitive element
mjtByte internal; // enable internal collisions
mjtByte flatskin; // render flex skin with flat shading
int selfcollide; // mode for flex self colllision
int activelayers; // number of active element layers in 3D
int group; // group for visualizatioh
double edgestiffness; // edge stiffness
double edgedamping; // edge damping
float rgba[4]; // rgba when material is omitted
mjString material; // name of material used for rendering
// mesh properties
mjStringVec vertbody; // vertex body names
mjDoubleVec vert; // vertex positions
mjIntVec elem; // element vertex ids
mjFloatVec texcoord; // vertex texture coordinates
// other
mjString info; // message appended to compiler errors
} mjmFlex;
typedef struct _mjmMesh { // mesh specification
mjElement element; // internal, do not modify
mjString name; // name
mjString classname; // class name
mjString content_type; // content type of file
mjString file; // mesh file
double refpos[3]; // reference position
double refquat[4]; // reference orientation
double scale[3]; // rescale mesh
mjtByte smoothnormal; // do not exclude large-angle faces from normals
mjFloatVec uservert; // user vertex data
mjFloatVec usernormal; // user normal data
mjFloatVec usertexcoord; // user texcoord data
mjIntVec userface; // user vertex indices
mjIntVec userfacenormal; // user normal indices
mjIntVec userfacetexcoord; // user texcoord indices
mjmPlugin plugin; // sdf plugin
mjString info; // message appended to compiler errors
} mjmMesh;
typedef struct _mjmHField { // height field specification
mjElement element; // internal, do not modify
mjString name; // name
mjString content_type; // content type of file
mjString file; // file: (nrow, ncol, [elevation data])
double size[4]; // hfield size (ignore referencing geom size)
int nrow; // number of rows
int ncol; // number of columns
mjFloatVec userdata; // user-provided elevation data
mjString info; // message appended to compiler errors
} mjmHField;
typedef struct _mjmSkin { // skin specification
mjElement element; // internal, do not modify
mjString name; // name
mjString classname; // class name
mjString file; // skin file
mjString material; // name of material used for rendering
float rgba[4]; // rgba when material is omitted
float inflate; // inflate in normal direction
int group; // group for visualization
// mesh
mjFloatVec vert; // vertex positions
mjFloatVec texcoord; // texture coordinates
mjIntVec face; // faces
// skin
mjStringVec bodyname; // body names
mjFloatVec bindpos; // bind pos
mjFloatVec bindquat; // bind quat
mjIntVecVec vertid; // vertex ids
mjFloatVecVec vertweight; // vertex weights
// other
mjString info; // message appended to compiler errors
} mjmSkin;
typedef struct _mjmTexture { // texture specification
mjElement element; // internal, do not modify
mjString name; // name
mjString classname; // class name
mjtTexture type; // texture type
// method 1: builtin
int builtin; // builtin type (mjtBuiltin)
int mark; // mark type (mjtMark)
double rgb1[3]; // first color for builtin
double rgb2[3]; // second color for builtin
double markrgb[3]; // mark color
double random; // probability of random dots
int height; // height in pixels (square for cube and skybox)
int width; // width in pixels
// method 2: single file
mjString content_type; // content type of file
mjString file; // png file to load; use for all sides of cube
int gridsize[2]; // size of grid for composite file; (1,1)-repeat
char gridlayout[13]; // row-major: L,R,F,B,U,D for faces; . for unused
// method 3: separate files
mjStringVec cubefiles; // different file for each side of the cube
// flip options
mjtByte hflip; // horizontal flip
mjtByte vflip; // vertical flip
// other
mjString info; // message appended to compiler errors
} mjmTexture;
typedef struct _mjmMaterial { // material specification
mjElement element; // internal, do not modify
mjString name; // name
mjString classname; // class name
mjString texture; // name of texture (empty: none)
bool texuniform; // make texture cube uniform
mjtByte texuniform; // make texture cube uniform
float texrepeat[2]; // texture repetition for 2D mapping
float emission; // emission
float specular; // specular
@@ -281,6 +505,34 @@ typedef struct _mjmMaterial { // material specification
} mjmMaterial;
typedef struct _mjmPair {
mjElement element; // internal, do not modify
mjString name; // name
mjString classname; // class name
mjString geomname1; // name of geom 1
mjString geomname2; // name of geom 2
// optional parameters: computed from geoms if not set by user
int condim; // contact dimensionality
mjtNum solref[mjNREF]; // solver reference, normal direction
mjtNum solreffriction[mjNREF]; // solver reference, frictional directions
mjtNum solimp[mjNIMP]; // solver impedance
double margin; // margin for contact detection
double gap; // include in solver if dist<margin-gap
double friction[5]; // full contact friction
mjString info; // message appended to errors
} mjmPair;
typedef struct _mjmExclude {
mjElement element; // internal, do not modify
mjString name; // name
mjString bodyname1; // name of geom 1
mjString bodyname2; // name of geom 2
mjString info; // message appended to errors
} mjmExclude;
typedef struct _mjmEquality { // equality specification
mjElement element; // internal, do not modify
mjString name; // name
@@ -310,7 +562,7 @@ typedef struct _mjmTendon { // tendon specification
mjtNum solimp_friction[mjNIMP]; // solver impedance: tendon friction
// length range
int limited; // does tendon have limits: 0 false, 1 true, 2 auto
int limited; // does tendon have limits (mjtLimited)
double range[2]; // length limits
double margin; // margin value for tendon limit detection
mjtNum solref_limit[mjNREF]; // solver reference: tendon limits
@@ -323,7 +575,7 @@ typedef struct _mjmTendon { // tendon specification
int group; // group
// other
mjDouble userdata; // user data
mjDoubleVec userdata; // user data
mjString info; // message appended to errors
} mjmTendon;
@@ -362,18 +614,19 @@ typedef struct _mjmActuator { // actuator specification
mjString slidersite; // site defining cylinder, for slider-crank
double cranklength; // crank length, for slider-crank
double lengthrange[2]; // transmission length range
double inheritrange; // automatic range setting for position and intvelocity
// input/output clamping
int ctrllimited; // are control limits defined: 0 false, 1 true, 2 auto
int ctrllimited; // are control limits defined (mjtLimited)
double ctrlrange[2]; // control range
int forcelimited; // are force limits defined: 0 false, 1 true, 2 auto
int forcelimited; // are force limits defined (mjtLimited)
double forcerange[2]; // force range
int actlimited; // are activation limits defined: 0 false, 1 true, 2 auto
int actlimited; // are activation limits defined (mjtLimited)
double actrange[2]; // activation range
// other
int group; // group
mjDouble userdata; // user data
mjDoubleVec userdata; // user data
mjmPlugin plugin; // actuator plugin
mjString info; // message appended to compiler errors
} mjmActuator;
@@ -401,55 +654,135 @@ typedef struct _mjmSensor { // sensor specfication
double noise; // noise stdev
// other
mjDouble userdata; // user data
mjDoubleVec userdata; // user data
mjmPlugin plugin; // sensor plugin
mjString info; // message appended to compiler errors
} mjmSensor;
typedef struct _mjmNumeric { // custom numeric field specification
mjElement element; // internal, do not modify
mjString name; // name
mjDoubleVec data; // initialization data
int size; // array size, can be bigger than data size
mjString info; // message appended to compiler errors
} mjmNumeric;
typedef struct _mjmText { // custom text specification
mjElement element; // internal, do not modify
mjString name; // name
mjString data; // text string
mjString info; // message appended to compiler errors
} mjmText;
typedef struct _mjmTuple { // tuple specification
mjElement element; // internal, do not modify
mjString name; // name
mjIntVec objtype; // object types
mjStringVec objname; // object names
mjDoubleVec objprm; // object parameters
mjString info; // message appended to compiler errors
} mjmTuple;
typedef struct _mjmKey { // keyframe specification
mjElement element; // internal, do not modify
mjString name; // name
double time; // time
mjDoubleVec qpos; // qpos
mjDoubleVec qvel; // qvel
mjDoubleVec act; // act
mjDoubleVec mpos; // mocap pos
mjDoubleVec mquat; // mocap quat
mjDoubleVec ctrl; // ctrl
mjString info; // message appended to compiler errors
} mjmKey;
typedef struct _mjmDefault { // default specification
mjString name; // name
mjElement element; // internal, do not modify
mjmJoint* joint; // joint defaults
mjmGeom* geom; // geom defaults
mjmSite* site; // site defaults
mjmCamera* camera; // camera defaults
mjmLight* light; // light defaults
mjmFlex* flex; // flex defaults
mjmMesh* mesh; // mesh defaults
mjmMaterial* material; // material defaults
mjmPair* pair; // pair defaults
mjmEquality* equality; // equality defaults
mjmTendon* tendon; // tendon defaults
mjmActuator* actuator; // actuator defaults
} mjmDefault;
//---------------------------------- API functions -------------------------------------------------
// Create model.
MJAPI void* mjm_createModel();
MJAPI mjmModel* mjm_createModel();
// Delete model.
MJAPI void mjm_deleteModel(void* modelspec);
MJAPI void mjm_deleteModel(mjmModel* modelspec);
// Copy spec into private attributes.
MJAPI void mjm_finalize(mjElement object);
// Add child body to body, return child spec.
MJAPI mjmBody* mjm_addBody(mjmBody* body, void* defspec);
MJAPI mjmBody* mjm_addBody(mjmBody* body, mjmDefault* def);
// Add site to body, return site spec.
MJAPI mjmSite* mjm_addSite(mjmBody* body, void* defspec);
MJAPI mjmSite* mjm_addSite(mjmBody* body, mjmDefault* def);
// Add joint to body.
MJAPI mjmJoint* mjm_addJoint(mjmBody* body, void* defspec);
MJAPI mjmJoint* mjm_addJoint(mjmBody* body, mjmDefault* def);
// Add freejoint to body.
MJAPI mjmJoint* mjm_addFreeJoint(mjmBody* body);
// Add geom to body.
MJAPI mjmGeom* mjm_addGeom(mjmBody* body, void* defspec);
MJAPI mjmGeom* mjm_addGeom(mjmBody* body, mjmDefault* def);
// Add camera to body.
MJAPI mjmCamera* mjm_addCamera(mjmBody* body, void* defspec);
MJAPI mjmCamera* mjm_addCamera(mjmBody* body, mjmDefault* def);
// Add light to body.
MJAPI mjmLight* mjm_addLight(mjmBody* body, void* defspec);
MJAPI mjmLight* mjm_addLight(mjmBody* body, mjmDefault* def);
// Add frame to body.
MJAPI mjmFrame* mjm_addFrame(mjmBody* body, mjmFrame* parentframe);
// Add flex to model.
MJAPI mjmFlex* mjm_addFlex(mjmModel* model);
// Add mesh to model.
MJAPI mjmMesh* mjm_addMesh(mjmModel* model, mjmDefault* def);
// Add height field to model.
MJAPI mjmHField* mjm_addHField(mjmModel* model);
// Add skin to model.
MJAPI mjmSkin* mjm_addSkin(mjmModel* model);
// Add texture to model.
MJAPI mjmTexture* mjm_addTexture(mjmModel* model);
// Add material to model.
MJAPI mjmMaterial* mjm_addMaterial(void* model, void* defspec);
MJAPI mjmMaterial* mjm_addMaterial(mjmModel* model, mjmDefault* def);
// Add pair to model.
MJAPI mjmPair* mjm_addPair(mjmModel* model, mjmDefault* def);
// Add excluded body pair to model.
MJAPI mjmExclude* mjm_addExclude(mjmModel *model);
// Add equality to model.
MJAPI mjmEquality* mjm_addEquality(void* model, void* defspec);
MJAPI mjmEquality* mjm_addEquality(mjmModel* model, mjmDefault* def);
// Add tendon to model.
MJAPI mjmTendon* mjm_addTendon(void* model, void* defspec);
MJAPI mjmTendon* mjm_addTendon(mjmModel* model, mjmDefault* def);
// Wrap site using tendon.
MJAPI mjmWrap* mjm_wrapSite(mjmTendon* tendon, const char* name);
@@ -464,22 +797,37 @@ MJAPI mjmWrap* mjm_wrapJoint(mjmTendon* tendon, const char* name, double coef);
MJAPI mjmWrap* mjm_wrapPulley(mjmTendon* tendon, double divisor);
// Add actuator to model.
MJAPI mjmActuator* mjm_addActuator(void* model, void* defspec);
MJAPI mjmActuator* mjm_addActuator(mjmModel* model, mjmDefault* def);
// Add sensor to model.
MJAPI mjmSensor* mjm_addSensor(void* model);
MJAPI mjmSensor* mjm_addSensor(mjmModel* model);
// Add numeric to model.
MJAPI mjmNumeric* mjm_addNumeric(mjmModel* model);
// Add text to model.
MJAPI mjmText* mjm_addText(mjmModel* model);
// Add tuple to model.
MJAPI mjmTuple* mjm_addTuple(mjmModel* model);
// Add keyframe to model.
MJAPI mjmKey* mjm_addKey(mjmModel* model);
// Add plugin to model.
MJAPI mjElement mjm_addPlugin(void* model);
MJAPI mjmPlugin* mjm_addPlugin(mjmModel* model);
// Add default to model.
MJAPI mjmDefault* mjm_addDefault(mjmModel* model, const char* classname, int parentid);
// Get model from body.
MJAPI void* mjm_getModel(mjmBody* body);
MJAPI mjmModel* mjm_getModel(mjmBody* body);
// Get default corresponding to an mjElement.
MJAPI void* mjm_getDefault(mjElement element);
MJAPI mjmDefault* mjm_getDefault(mjElement element);
// Find body in model by name.
MJAPI mjmBody* mjm_findBody(void* modelspec, const char* name);
MJAPI mjmBody* mjm_findBody(mjmModel* modelspec, const char* name);
// Find child body by name.
MJAPI mjmBody* mjm_findChild(mjmBody* body, const char* name);
@@ -487,20 +835,44 @@ MJAPI mjmBody* mjm_findChild(mjmBody* body, const char* name);
// Get element id.
MJAPI int mjm_getId(mjElement element);
// Copy text to destination string.
// Copy text to string.
MJAPI void mjm_setString(mjString dest, const char* text);
// Copy double array to destination vector.
MJAPI void mjm_setDouble(mjDouble dest, const double* array, int size);
// Split text to entries and copy to string vector.
MJAPI void mjm_setStringVec(mjStringVec dest, const char* text);
// Set entry in string vector.
MJAPI mjtByte mjm_setInStringVec(mjStringVec dest, int i, const char* text);
// Append text entry to string vector.
MJAPI void mjm_appendString(mjStringVec dest, const char* text);
// Copy int array to vector.
MJAPI void mjm_setInt(mjIntVec dest, const int* array, int size);
// Append int array to vector of arrays.
MJAPI void mjm_appendIntVec(mjIntVecVec dest, const int* array, int size);
// Copy float array to vector.
MJAPI void mjm_setFloat(mjFloatVec dest, const float* array, int size);
// Append float array to vector of arrays.
MJAPI void mjm_appendFloatVec(mjFloatVecVec dest, const float* array, int size);
// Copy double array to vector.
MJAPI void mjm_setDouble(mjDoubleVec dest, const double* array, int size);
// Get string contents.
MJAPI const char* mjm_getString(mjString source);
// Get double array contents and optionally its size.
MJAPI const double* mjm_getDouble(mjDouble source, int* size);
MJAPI const double* mjm_getDouble(mjDoubleVec source, int* size);
// Set plugin attributes.
MJAPI void mjm_setPluginAttributes(mjmPlugin* plugin, void* attributes);
// Set default.
MJAPI void mjm_setDefault(mjElement element, void* defspec);
MJAPI void mjm_setDefault(mjElement element, mjmDefault* def);
// Set frame.
MJAPI void mjm_setFrame(mjElement dest, mjmFrame* frame);
@@ -511,6 +883,9 @@ MJAPI const char* mjm_setFullInertia(mjmBody* body, double quat[4], double inert
//---------------------------------- Initialization functions --------------------------------------
// Default model attributes.
MJAPI void mjm_defaultModel(mjmModel& model);
// Default body attributes.
MJAPI void mjm_defaultBody(mjmBody& body);
@@ -532,9 +907,27 @@ MJAPI void mjm_defaultCamera(mjmCamera& camera);
// Default light attributes.
MJAPI void mjm_defaultLight(mjmLight& light);
// Default flex attributes.
MJAPI void mjm_defaultFlex(mjmFlex& flex);
// Default mesh attributes.
MJAPI void mjm_defaultMesh(mjmMesh& mesh);
// Default height field attributes.
MJAPI void mjm_defaultHField(mjmHField& hfield);
// Default skin attributes.
MJAPI void mjm_defaultSkin(mjmSkin& skin);
// Default texture attributes.
MJAPI void mjm_defaultTexture(mjmTexture& texture);
// Default material attributes.
MJAPI void mjm_defaultMaterial(mjmMaterial& material);
// Default pair attributes.
MJAPI void mjm_defaultPair(mjmPair& pair);
// Default equality attributes.
MJAPI void mjm_defaultEquality(mjmEquality& equality);
@@ -547,6 +940,21 @@ MJAPI void mjm_defaultActuator(mjmActuator& actuator);
// Default sensor attributes.
MJAPI void mjm_defaultSensor(mjmSensor& sensor);
// Default numeric attributes.
MJAPI void mjm_defaultNumeric(mjmNumeric& numeric);
// Default text attributes.
MJAPI void mjm_defaultText(mjmText& text);
// Default tuple attributes.
MJAPI void mjm_defaultTuple(mjmTuple& tuple);
// Default keyframe attributes.
MJAPI void mjm_defaultKey(mjmKey& key);
// Default plugin attributes.
MJAPI void mjm_defaultPlugin(mjmPlugin& plugin);
#ifdef __cplusplus
}
#endif
src/user/user_composite.cc
+366 -323
View File
File diff suppressed because it is too large Load Diff
src/user/user_composite.h
+18 -8
View File
@@ -20,6 +20,7 @@
#include <vector>
#include <mujoco/mjmodel.h>
#include "user/user_api.h"
#include "user/user_model.h"
#include "user/user_objects.h"
@@ -79,14 +80,14 @@ class mjCComposite {
void MakeSkin2(mjCModel* model, mjtNum inflate);
void MakeSkin2Subgrid(mjCModel* model, mjtNum inflate);
void MakeClothBones(mjCModel* model, mjCSkin* skin);
void MakeClothBonesSubgrid(mjCModel* model, mjCSkin* skin);
void MakeCableBones(mjCModel* model, mjCSkin* skin);
void MakeCableBonesSubgrid(mjCModel* model, mjCSkin* skin);
void MakeClothBones(mjCModel* model, mjmSkin* skin);
void MakeClothBonesSubgrid(mjCModel* model, mjmSkin* skin);
void MakeCableBones(mjCModel* model, mjmSkin* skin);
void MakeCableBonesSubgrid(mjCModel* model, mjmSkin* skin);
void MakeSkin3(mjCModel* model);
void MakeSkin3Box(mjCSkin* skin, int c0, int c1, int side, int& vcnt, const char* format);
void MakeSkin3Smooth(mjCSkin* skin, int c0, int c1, int side,
void MakeSkin3Box(mjmSkin* skin, int c0, int c1, int side, int& vcnt, const char* format);
void MakeSkin3Smooth(mjmSkin* skin, int c0, int c1, int side,
const std::map<std::string, int>& vmap, const char* format);
void BoxProject(double* pos);
@@ -113,10 +114,9 @@ class mjCComposite {
std::vector<std::string> username;
// plugin support
bool is_plugin;
std::string plugin_name;
std::string plugin_instance_name;
mjCPlugin* plugin_instance;
mjmPlugin plugin;
// skin
bool skin; // generate skin
@@ -138,6 +138,16 @@ class mjCComposite {
private:
mjmBody* AddRopeBody(mjCModel* model, mjmBody* body, int ix, int ix1);
mjmBody* AddCableBody(mjCModel* model, mjmBody* body, int ix, mjtNum normal[3], mjtNum prev_quat[4]);
// temporary skin vectors
void CopyIntoSkin(mjmSkin* skin);
std::vector<int> face;
std::vector<float> vert;
std::vector<float> bindpos;
std::vector<float> bindquat;
std::vector<float> texcoord;
std::vector<std::vector<int>> vertid;
std::vector<std::vector<float>> vertweight;
};
#endif // MUJOCO_SRC_USER_USER_COMPOSITE_H_
src/user/user_flexcomp.cc
+67 -50
View File
@@ -71,13 +71,19 @@ mjCFlexcomp::mjCFlexcomp(void) {
mjuu_setvec(quat, 1, 0, 0, 0);
rigid = false;
centered = false;
plugin_instance = nullptr;
mjm_defaultPlugin(plugin);
plugin_name = "";
plugin_instance_name = "";
plugin.name = (mjString)&plugin_name;
plugin.instance_name = (mjString)&plugin_instance_name;
}
// make flexcomp object
bool mjCFlexcomp::Make(mjCModel* model, mjmBody* body, char* error, int error_sz) {
mjmFlex* dflex = def.spec.flex;
bool radial = (type==mjFCOMPTYPE_BOX ||
type==mjFCOMPTYPE_CYLINDER ||
type==mjFCOMPTYPE_ELLIPSOID);
@@ -98,7 +104,7 @@ bool mjCFlexcomp::Make(mjCModel* model, mjmBody* body, char* error, int error_sz
}
// check spacing
double minspace = 2*def.flex.radius + def.flex.margin;
double minspace = 2*dflex->radius + dflex->margin;
if (!direct) {
if (spacing[0]<minspace ||
spacing[1]<minspace ||
@@ -118,7 +124,7 @@ bool mjCFlexcomp::Make(mjCModel* model, mjmBody* body, char* error, int error_sz
}
// compute orientation
const char* alterr = alt.Set(quat, model->degree, model->euler);
const char* alterr = alt.Set(quat, model->spec.degree, model->spec.euler);
if (alterr) {
return comperr(error, alterr, error_sz);
}
@@ -156,7 +162,7 @@ bool mjCFlexcomp::Make(mjCModel* model, mjmBody* body, char* error, int error_sz
}
// get dim and check
int dim = def.flex.dim;
int dim = dflex->dim;
if (dim<1 || dim>3) {
return comperr(error, "Invalid dim, must be between 1 and 3", error_sz);
}
@@ -164,7 +170,7 @@ bool mjCFlexcomp::Make(mjCModel* model, mjmBody* body, char* error, int error_sz
// force flatskin shading for box, cylinder and 3D grid
if (type==mjFCOMPTYPE_BOX || type==mjFCOMPTYPE_CYLINDER ||
(type==mjFCOMPTYPE_GRID && dim==3)) {
def.flex.flatskin = true;
dflex->flatskin = true;
}
// check pin sizes
@@ -368,21 +374,22 @@ bool mjCFlexcomp::Make(mjCModel* model, mjmBody* body, char* error, int error_sz
}
// create flex, copy parameters
mjCFlex* pf = model->AddFlex();
int id = pf->id;
*pf = def.flex;
pf->model = model;
pf->id = id;
pf->name = name;
pf->elem = element;
if (!centered) {
pf->vert = point;
}
pf->texcoord = texcoord;
mjCFlex* flex = model->AddFlex();
mjmFlex* pf = &flex->spec;
int id = flex->id;
*flex = def.flex;
flex->PointToLocal();
flex->model = model;
flex->id = id;
mjm_setString(pf->name, name.c_str());
mjm_setInt(pf->elem, element.data(), element.size());
mjm_setFloat(pf->texcoord, texcoord.data(), texcoord.size());
// rigid: set parent name, nothing else to do
if (rigid) {
pf->vertbody.push_back(std::string(mjm_getString(body->name)).c_str());
mjm_appendString(pf->vertbody, mjm_getString(body->name));
return true;
}
@@ -390,6 +397,12 @@ bool mjCFlexcomp::Make(mjCModel* model, mjmBody* body, char* error, int error_sz
double bodymass = mass/npnt;
double bodyinertia = bodymass*(2.0*inertiabox*inertiabox)/3.0;
// overwrite plugin name
if (plugin.active && plugin_instance_name.empty()) {
plugin_instance_name = "flexcomp_" + name;
((mjCPlugin*)plugin.instance)->name = plugin_instance_name;
}
// create bodies, construct flex vert and vertbody
for (int i=0; i<npnt; i++) {
// not used: skip
@@ -399,15 +412,15 @@ bool mjCFlexcomp::Make(mjCModel* model, mjmBody* body, char* error, int error_sz
// pinned: parent body
if (pinned[i]) {
pf->vertbody.push_back(std::string(mjm_getString(body->name)).c_str());
mjm_appendString(pf->vertbody, mjm_getString(body->name));
// add plugin
if (plugin_instance) {
mjmPlugin* plugin = &body->plugin;
plugin->active = true;
plugin->instance = (mjElement)plugin_instance;
mjm_setString(plugin->name, plugin_name.c_str());
mjm_setString(plugin->instance_name, plugin_instance_name.c_str());
if (plugin.active) {
mjmPlugin* pplugin = &body->plugin;
pplugin->active = true;
pplugin->instance = (mjElement)plugin.instance;
mjm_setString(pplugin->name, mjm_getString(plugin.name));
mjm_setString(pplugin->instance_name, plugin_instance_name.c_str());
}
}
@@ -456,30 +469,34 @@ bool mjCFlexcomp::Make(mjCModel* model, mjmBody* body, char* error, int error_sz
char txt[100];
mju::sprintf_arr(txt, "%s_%d", name.c_str(), i);
mjm_setString(pb->name, txt);
pf->vertbody.push_back(std::string(mjm_getString(pb->name)).c_str());
mjm_appendString(pf->vertbody, mjm_getString(pb->name));
// clear flex vertex coordinates if allocated
if (!centered) {
pf->vert[3*i] = 0;
pf->vert[3*i+1] = 0;
pf->vert[3*i+2] = 0;
point[3*i] = 0;
point[3*i+1] = 0;
point[3*i+2] = 0;
}
// add plugin
if (plugin_instance) {
mjmPlugin* plugin = &pb->plugin;
plugin->active = true;
plugin->instance = (mjElement)plugin_instance;
mjm_setString(plugin->name, plugin_name.c_str());
mjm_setString(plugin->instance_name, plugin_instance_name.c_str());
if (plugin.active) {
mjmPlugin* pplugin = &pb->plugin;
pplugin->active = true;
pplugin->instance = (mjElement)plugin.instance;
mjm_setString(pplugin->name, mjm_getString(plugin.name));
mjm_setString(pplugin->instance_name, plugin_instance_name.c_str());
}
}
}
if (!centered) {
mjm_setDouble(pf->vert, point.data(), point.size());
}
// create edge equality constraint
if (equality) {
mjmEquality* pe = mjm_addEquality(model, &def);
mjm_setDefault(pe->element, model->defaults[0]);
mjmEquality* pe = mjm_addEquality(&model->spec, &def.spec);
mjm_setDefault(pe->element, &model->defaults[0]->spec);
pe->type = mjEQ_FLEX;
pe->active = true;
mjm_setString(pe->name1, name.c_str());
@@ -502,7 +519,7 @@ int mjCFlexcomp::GridID(int ix, int iy, int iz) {
// make grid
bool mjCFlexcomp::MakeGrid(char* error, int error_sz) {
int dim = def.flex.dim;
int dim = def.flex.spec.dim;
bool hastex = texcoord.empty();
// 1D
@@ -683,7 +700,7 @@ bool mjCFlexcomp::MakeBox(char* error, int error_sz) {
double pos[3];
// set 3D
def.flex.dim = 3;
def.spec.flex->dim = 3;
// add center point
point.push_back(0);
@@ -797,7 +814,7 @@ template <typename T> static T* VecToArray(std::vector<T>& vector, bool clear =
// make mesh
bool mjCFlexcomp::MakeMesh(mjCModel* model, char* error, int error_sz) {
// strip path
if (!file.empty() && model->strippath) {
if (!file.empty() && model->spec.strippath) {
file = mjuu_strippath(file);
}
@@ -815,12 +832,12 @@ bool mjCFlexcomp::MakeMesh(mjCModel* model, char* error, int error_sz) {
}
// check dim
if (def.flex.dim!=2) {
if (def.spec.flex->dim!=2) {
return comperr(error, "Flex dim must be 2 in for mesh", error_sz);
}
// load resource
string filename = mjuu_makefullname(model->modelfiledir, model->meshdir, file);
string filename = mjuu_makefullname(model->modelfiledir, mjm_getString(model->spec.meshdir), file);
mjResource* resource = nullptr;
try {
@@ -850,13 +867,13 @@ bool mjCFlexcomp::MakeMesh(mjCModel* model, char* error, int error_sz) {
// LoadOBJ uses userXXX, extra processing needed
if (isobj) {
// check sizes
if (mesh.uservert().empty() || mesh.userface().empty()) {
if (mesh.get_uservert().empty() || mesh.get_userface().empty()) {
return comperr(error, "Vertex and face data required", error_sz);
}
if (mesh.uservert().size()%3) {
if (mesh.get_uservert().size()%3) {
return comperr(error, "Vertex data must be multiple of 3", error_sz);
}
if (mesh.userface().size()%3) {
if (mesh.get_userface().size()%3) {
return comperr(error, "Face data must be multiple of 3", error_sz);
}
@@ -916,7 +933,7 @@ static int findstring(const char* buffer, int buffer_sz, const char* str) {
// load points and elements from GMSH file
bool mjCFlexcomp::MakeGMSH(mjCModel* model, char* error, int error_sz) {
// strip path
if (!file.empty() && model->strippath) {
if (!file.empty() && model->spec.strippath) {
file = mjuu_strippath(file);
}
@@ -926,7 +943,7 @@ bool mjCFlexcomp::MakeGMSH(mjCModel* model, char* error, int error_sz) {
}
// open resource
string filename = mjuu_makefullname(model->modelfiledir, model->meshdir, file);
string filename = mjuu_makefullname(model->modelfiledir, mjm_getString(model->spec.meshdir), file);
mjResource* resource = nullptr;
try {
@@ -1027,7 +1044,7 @@ void mjCFlexcomp::LoadGMSH(mjCModel* model, mjResource* resource) {
if (entityDim<1 || entityDim>3) {
throw mjCError(NULL, "Entity must be 1D, 2D or 3D");
}
def.flex.dim = entityDim;
def.spec.flex->dim = entityDim;
// read and discard node tags; require range from minNodeTag to maxNodeTag
for (size_t i=0; i<numNodes; i++) {
@@ -1079,7 +1096,7 @@ void mjCFlexcomp::LoadGMSH(mjCModel* model, mjResource* resource) {
if (entityDim<1 || entityDim>3) {
throw mjCError(NULL, "Entity must be 1D, 2D or 3D");
}
def.flex.dim = entityDim;
def.spec.flex->dim = entityDim;
// check section byte size
if (nodeend-nodebegin < 52+numNodes*4*8) {
@@ -1127,7 +1144,7 @@ void mjCFlexcomp::LoadGMSH(mjCModel* model, mjResource* resource) {
}
// dimensionality must be same as nodes
if (entityDim!=def.flex.dim) {
if (entityDim!=def.spec.flex->dim) {
throw mjCError(NULL, "Inconsistent dimensionality in Elements");
}
@@ -1176,7 +1193,7 @@ void mjCFlexcomp::LoadGMSH(mjCModel* model, mjResource* resource) {
}
// dimensionality must be same as nodes
if (entityDim!=def.flex.dim) {
if (entityDim!=def.spec.flex->dim) {
throw mjCError(NULL, "Inconsistent dimensionality in Elements");
}
src/user/user_flexcomp.h
+2 -2
View File
@@ -19,6 +19,7 @@
#include <vector>
#include <mujoco/mujoco.h>
#include "user/user_api.h"
#include "user/user_model.h"
#include "user/user_objects.h"
@@ -87,10 +88,9 @@ class mjCFlexcomp {
std::vector<float> texcoord; // vertex texture coordinates
// plugin support
bool is_plugin;
std::string plugin_name;
std::string plugin_instance_name;
mjCPlugin* plugin_instance;
mjmPlugin plugin;
};
#endif // MUJOCO_SRC_USER_USER_FLEXCOMP_H_
src/user/user_init.cc
+176 -7
View File
@@ -15,11 +15,66 @@
#include <cstring>
#include <mujoco/mjmodel.h>
#include <mujoco/mujoco.h>
#include "cc/array_safety.h"
#include "user/user_api.h"
#include "user/user_util.h"
// default model attributes
void mjm_defaultModel(mjmModel& model) {
memset(&model, 0, sizeof(mjmModel));
// default statistics
model.stat.meaninertia = mjNAN;
model.stat.meanmass = mjNAN;
model.stat.meansize = mjNAN;
model.stat.extent = mjNAN;
model.stat.center[0] = mjNAN;
// compiler settings
model.autolimits = true;
model.boundmass = 0;
model.boundinertia = 0;
model.settotalmass = -1;
model.balanceinertia = false;
model.strippath = false;
model.fitaabb = false;
model.degree = true;
model.euler[0] = 'x';
model.euler[1] = 'y';
model.euler[2] = 'z';
model.discardvisual = false;
model.convexhull = true;
model.usethread = true;
model.fusestatic = false;
model.inertiafromgeom = mjINERTIAFROMGEOM_AUTO;
model.inertiagrouprange[0] = 0;
model.inertiagrouprange[1] = mjNGROUP-1;
model.exactmeshinertia = false;
mj_defaultLROpt(&model.LRopt);
// engine data
mj_defaultOption(&model.option);
mj_defaultVisual(&model.visual);
model.memory = -1;
model.nemax = 0;
model.njmax = -1;
model.nconmax = -1;
model.nstack = -1;
model.nuserdata = 0;
model.nuser_body = -1;
model.nuser_jnt = -1;
model.nuser_geom = -1;
model.nuser_site = -1;
model.nuser_cam = -1;
model.nuser_tendon = -1;
model.nuser_actuator = -1;
model.nuser_sensor = -1;
}
// default body attributes
void mjm_defaultBody(mjmBody& body) {
memset(&body, 0, sizeof(mjmBody));
@@ -54,8 +109,8 @@ void mjm_defaultJoint(mjmJoint& joint) {
joint.type = mjJNT_HINGE;
joint.axis[2] = 1;
joint.limited = 2;
joint.actfrclimited = 2;
joint.limited = mjLIMITED_AUTO;
joint.actfrclimited = mjLIMITED_AUTO;
mj_defaultSolRefImp(joint.solref_limit, joint.solimp_limit);
mj_defaultSolRefImp(joint.solref_friction, joint.solimp_friction);
joint.urdfeffort = -1;
@@ -167,7 +222,74 @@ void mjm_defaultLight(mjmLight& light) {
// Default material attributes.
// default flex attributes
void mjm_defaultFlex(mjmFlex& flex) {
memset(&flex, 0, sizeof(mjmFlex));
// set contact defaults
flex.contype = 1;
flex.conaffinity = 1;
flex.condim = 3;
mjuu_setvec(flex.friction, 1, 0.005, 0.0001);
flex.solmix = 1.0;
mj_defaultSolRefImp(flex.solref, flex.solimp);
// set other defaults
flex.dim = 2;
flex.radius = 0.005;
flex.internal = true;
flex.selfcollide = mjFLEXSELF_AUTO;
flex.activelayers = 1;
flex.rgba[0] = flex.rgba[1] = flex.rgba[2] = 0.5f;
flex.rgba[3] = 1.0f;
}
// default mesh attributes
void mjm_defaultMesh(mjmMesh& mesh) {
memset(&mesh, 0, sizeof(mjmMesh));
mjuu_setvec(mesh.refpos, 0, 0, 0);
mjuu_setvec(mesh.refquat, 1, 0, 0, 0);
mjuu_setvec(mesh.scale, 1, 1, 1);
mesh.smoothnormal = false;
}
// default height field attributes
void mjm_defaultHField(mjmHField& hfield) {
memset(&hfield, 0, sizeof(mjmHField));
}
// default skin attributes
void mjm_defaultSkin(mjmSkin& skin) {
memset(&skin, 0, sizeof(mjmSkin));
skin.rgba[0] = skin.rgba[1] = skin.rgba[2] = 0.5f;
skin.rgba[3] = 1.0f;
skin.inflate = 0;
skin.group = 0;
}
// default texture attributes
void mjm_defaultTexture(mjmTexture& texture) {
memset(&texture, 0, sizeof(mjmTexture));
texture.type = mjTEXTURE_CUBE;
mjuu_setvec(texture.rgb1, 0.8, 0.8, 0.8);
mjuu_setvec(texture.rgb2, 0.5, 0.5, 0.5);
mjuu_setvec(texture.markrgb, 0, 0, 0);
texture.random = 0.01;
texture.gridsize[0] = texture.gridsize[1] = 1;
mujoco::util::strcpy_arr(texture.gridlayout, "............");
}
// default material attributes
void mjm_defaultMaterial(mjmMaterial& material) {
memset(&material, 0, sizeof(mjmMaterial));
material.texuniform = false;
@@ -181,6 +303,20 @@ void mjm_defaultMaterial(mjmMaterial& material) {
// default pair attributes
void mjm_defaultPair(mjmPair& pair) {
memset(&pair, 0, sizeof(mjmPair));
pair.condim = 3;
mj_defaultSolRefImp(pair.solref, pair.solimp);
pair.friction[0] = 1;
pair.friction[1] = 1;
pair.friction[2] = 0.005;
pair.friction[3] = 0.0001;
pair.friction[4] = 0.0001;
}
// default equality attributes
void mjm_defaultEquality(mjmEquality& equality) {
memset(&equality, 0, sizeof(mjmEquality));
@@ -196,7 +332,7 @@ void mjm_defaultEquality(mjmEquality& equality) {
// default tendon attributes
void mjm_defaultTendon(mjmTendon& tendon) {
memset(&tendon, 0, sizeof(mjmTendon));
tendon.limited = 2;
tendon.limited = mjLIMITED_AUTO;
tendon.springlength[0] = tendon.springlength[1] = -1;
mj_defaultSolRefImp(tendon.solref_limit, tendon.solimp_limit);
mj_defaultSolRefImp(tendon.solref_friction, tendon.solimp_friction);
@@ -226,9 +362,9 @@ void mjm_defaultActuator(mjmActuator& actuator) {
actuator.gear[0] = 1;
// input/output clamping
actuator.ctrllimited = 2;
actuator.forcelimited = 2;
actuator.actlimited = 2;
actuator.ctrllimited = mjLIMITED_AUTO;
actuator.forcelimited = mjLIMITED_AUTO;
actuator.actlimited = mjLIMITED_AUTO;
}
@@ -243,4 +379,37 @@ void mjm_defaultSensor(mjmSensor& sensor) {
// Default numeric attributes.
void mjm_defaultNumeric(mjmNumeric& numeric) {
memset(&numeric, 0, sizeof(mjmNumeric));
}
// Default text attributes.
void mjm_defaultText(mjmText& text) {
memset(&text, 0, sizeof(mjmText));
}
// Default tuple attributes.
void mjm_defaultTuple(mjmTuple& tuple) {
memset(&tuple, 0, sizeof(mjmTuple));
}
// Default keyframe attributes.
void mjm_defaultKey(mjmKey& key) {
memset(&key, 0, sizeof(mjmKey));
}
// default plugin attributes
void mjm_defaultPlugin(mjmPlugin& plugin) {
memset(&plugin, 0, sizeof(mjmPlugin));
plugin.plugin_slot = -1;
}
src/user/user_mesh.cc
+283 -274
View File
@@ -129,11 +129,7 @@ static void ReadFromBuffer(T* dst, const char* src) {
//------------------ class mjCMesh implementation --------------------------------------------------
mjCMesh::mjCMesh(mjCModel* _model, mjCDef* _def) {
// set defaults
mjuu_setvec(refpos_, 0, 0, 0);
mjuu_setvec(refquat_, 1, 0, 0, 0);
mjuu_setvec(scale_, 1, 1, 1);
smoothnormal_ = false;
mjm_defaultMesh(spec);
// clear internal variables
mjuu_setvec(pos_surface_, 0, 0, 0);
@@ -179,9 +175,50 @@ mjCMesh::mjCMesh(mjCModel* _model, mjCDef* _def) {
model = _model;
def = (_def ? _def : (_model ? _model->defaults[0] : 0));
// in case this body is not compiled
CopyFromSpec();
// point to local (needs to be after defaults)
plugin.name = (mjString)&plugin_name;
plugin.instance_name = (mjString)&plugin_instance_name;
PointToLocal();
}
void mjCMesh::PointToLocal() {
spec.element = (mjElement)this;
spec.name = (mjString)&name;
spec.classname = (mjString)&classname;
spec.file = (mjString)&spec_file_;
spec.content_type = (mjString)&spec_content_type_;
spec.uservert = (mjFloatVec)&spec_uservert_;
spec.usernormal = (mjFloatVec)&spec_usernormal_;
spec.userface = (mjIntVec)&spec_userface_;
spec.usertexcoord = (mjFloatVec)&spec_usertexcoord_;
spec.plugin.name = (mjString)&plugin_name;
spec.plugin.instance_name = (mjString)&plugin_instance_name;
spec.info = (mjString)&info;
}
void mjCMesh::CopyFromSpec() {
*static_cast<mjmMesh*>(this) = spec;
file_ = spec_file_;
content_type_ = spec_content_type_;
uservert_ = spec_uservert_;
usernormal_ = spec_usernormal_;
userface_ = spec_userface_;
usertexcoord_ = spec_usertexcoord_;
file = (mjString)&file_;
content_type = (mjString)&content_type_;
uservert = (mjFloatVec)&uservert_;
usernormal = (mjFloatVec)&usernormal_;
userface = (mjIntVec)&userface_;
usertexcoord = (mjFloatVec)&usertexcoord_;
plugin.active = spec.plugin.active;
plugin.instance = spec.plugin.instance;
plugin.name = spec.plugin.name;
plugin.instance_name = spec.plugin.instance_name;
}
@@ -199,96 +236,6 @@ mjCMesh::~mjCMesh() {
void mjCMesh::set_content_type(std::optional<std::string>&& content_type) {
if (content_type.has_value()) {
content_type_ = std::move(content_type.value());
}
}
void mjCMesh::set_file(std::optional<std::string>&& file) {
if (file.has_value()) {
file_ = std::move(file.value());
}
}
void mjCMesh::set_refpos(std::optional<std::array<double, 3>> refpos) {
if (refpos.has_value()) {
std::copy(refpos.value().begin(), refpos.value().end(), refpos_);
}
}
void mjCMesh::set_refquat(std::optional<std::array<double, 4>> refquat) {
if (refquat.has_value()) {
std::copy(refquat.value().begin(), refquat.value().end(), refquat_);
}
}
void mjCMesh::set_scale(std::optional<std::array<double, 3>> scale) {
if (scale.has_value()) {
set_scale(scale.value());
}
}
void mjCMesh::set_uservert(std::optional<std::vector<float>>&& uservert) {
if (uservert.has_value()) {
uservert_ = std::move(uservert.value());
}
}
void mjCMesh::set_usernormal(std::optional<std::vector<float>>&& usernormal) {
if (usernormal.has_value()) {
usernormal_ = std::move(usernormal.value());
}
}
void mjCMesh::set_usertexcoord(std::optional<std::vector<float>>&& usertexcoord) {
if (usertexcoord.has_value()) {
usertexcoord_ = std::move(usertexcoord.value());
}
}
void mjCMesh::set_userface(std::optional<std::vector<int>>&& userface) {
if (userface.has_value()) {
userface_ = std::move(userface.value());
}
}
void mjCMesh::set_file(const std::string& file) {
file_ = file;
}
void mjCMesh::set_scale(std::array<double, 3> scale) {
std::copy(scale.begin(), scale.end(), scale_);
}
void mjCMesh::set_smoothnormal(bool smoothnormal) {
smoothnormal_ = smoothnormal;
}
void mjCMesh::set_needhull(bool needhull) {
needhull_ = needhull;
}
@@ -303,14 +250,14 @@ void mjCMesh::LoadSDF() {
name.c_str(), id);
}
if (scale_[0] != 1 || scale_[1] != 1 || scale_[2] != 1) {
if (scale[0] != 1 || scale[1] != 1 || scale[2] != 1) {
throw mjCError(this, "attribute scale is not compatible with SDFs in mesh '%s', (id = %d)",
name.c_str(), id);
}
mjCPlugin* plugin_instance = (mjCPlugin*)plugin.instance;
model->ResolvePlugin(this, plugin_name, plugin_instance_name, &plugin_instance);
const mjpPlugin* pplugin = mjp_getPluginAtSlot(plugin_instance->plugin_slot);
const mjpPlugin* pplugin = mjp_getPluginAtSlot(plugin_instance->spec.plugin_slot);
if (!(pplugin->capabilityflags & mjPLUGIN_SDF)) {
throw mjCError(this, "plugin '%s' does not support signed distance fields", pplugin->name);
}
@@ -371,9 +318,9 @@ void mjCMesh::LoadSDF() {
userface.push_back(mesh.indices.at(i));
}
set_uservert(uservert);
set_usernormal(usernormal);
set_userface(userface);
uservert_ = std::move(uservert);
usernormal_ = std::move(usernormal);
userface_ = std::move(userface);
delete[] field;
}
@@ -381,6 +328,8 @@ void mjCMesh::LoadSDF() {
// compiler
void mjCMesh::Compile(const mjVFS* vfs) {
CopyFromSpec();
// load file
if (!file_.empty()) {
// remove path from file if necessary
@@ -398,7 +347,7 @@ void mjCMesh::Compile(const mjVFS* vfs) {
throw mjCError(this, "unsupported content type: '%s'", asset_type.c_str());
}
string filename = mjuu_makefullname(model->modelfiledir, model->meshdir, file_);
string filename = mjuu_makefullname(model->modelfiledir, model->meshdir_, file_);
mjResource* resource = LoadResource(filename, vfs);
try {
@@ -957,7 +906,7 @@ void mjCMesh::LoadOBJ(mjResource* resource) {
if (!objReader.GetShapes().empty()) {
const auto& mesh = objReader.GetShapes()[0].mesh;
bool righthand = (scale_[0]*scale_[1]*scale_[2] > 0);
bool righthand = (scale[0]*scale[1]*scale[2] > 0);
// iterate over mesh faces
std::vector<tinyobj::index_t> face_indices;
@@ -1005,7 +954,7 @@ void mjCMesh::LoadOBJ(mjResource* resource) {
// load STL binary mesh
void mjCMesh::LoadSTL(mjResource* resource) {
bool righthand = (scale_[0]*scale_[1]*scale_[2]>0);
bool righthand = (scale[0]*scale[1]*scale[2]>0);
// get file data in buffer
char* buffer = 0;
@@ -1085,7 +1034,7 @@ void mjCMesh::LoadSTL(mjResource* resource) {
// load MSH binary mesh
void mjCMesh::LoadMSH(mjResource* resource) {
bool righthand = (scale_[0]*scale_[1]*scale_[2]>0);
bool righthand = (scale[0]*scale[1]*scale[2]>0);
// get file data in buffer
char* buffer = 0;
@@ -1193,9 +1142,9 @@ void mjCMesh::ComputeVolume(double CoM[3], mjtGeomInertia type,
// apply transformations
void mjCMesh::ApplyTransformations() {
// translate
if (refpos_[0]!=0 || refpos_[1]!=0 || refpos_[2]!=0) {
if (refpos[0]!=0 || refpos[1]!=0 || refpos[2]!=0) {
// prepare translation
float rp[3] = {(float)refpos_[0], (float)refpos_[1], (float)refpos_[2]};
float rp[3] = {(float)refpos[0], (float)refpos[1], (float)refpos[2]};
// process vertices
for (int i=0; i<nvert_; i++) {
@@ -1206,9 +1155,9 @@ void mjCMesh::ApplyTransformations() {
}
// rotate
if (refquat_[0]!=1 || refquat_[1]!=0 || refquat_[2]!=0 || refquat_[3]!=0) {
if (refquat[0]!=1 || refquat[1]!=0 || refquat[2]!=0 || refquat[3]!=0) {
// prepare rotation
mjtNum quat[4] = {refquat_[0], refquat_[1], refquat_[2], refquat_[3]};
mjtNum quat[4] = {refquat[0], refquat[1], refquat[2], refquat[3]};
mjtNum mat[9];
mju_normalize4(quat);
mju_quat2Mat(mat, quat);
@@ -1233,17 +1182,17 @@ void mjCMesh::ApplyTransformations() {
}
// scale
if (scale_[0]!=1 || scale_[1]!=1 || scale_[2]!=1) {
if (scale[0]!=1 || scale[1]!=1 || scale[2]!=1) {
for (int i=0; i<nvert_; i++) {
vert_[3*i] *= scale_[0];
vert_[3*i+1] *= scale_[1];
vert_[3*i+2] *= scale_[2];
vert_[3*i] *= scale[0];
vert_[3*i+1] *= scale[1];
vert_[3*i+2] *= scale[2];
}
for (int i=0; i<nnormal_; i++) {
normal_[3*i] *= scale_[0];
normal_[3*i+1] *= scale_[1];
normal_[3*i+2] *= scale_[2];
normal_[3*i] *= scale[0];
normal_[3*i+1] *= scale[1];
normal_[3*i+2] *= scale[2];
}
}
@@ -1764,7 +1713,7 @@ void mjCMesh::MakeNormal(void) {
}
// remove large-angle faces
if (!smoothnormal_) {
if (!smoothnormal) {
// allocate removal and clear
float* nremove = (float*) mju_malloc(3*nnormal_*sizeof(float));
memset(nremove, 0, 3*nnormal_*sizeof(float));
@@ -1880,45 +1829,92 @@ void mjCMesh::MakeCenter(void) {
// constructor
mjCSkin::mjCSkin(mjCModel* _model) {
mjm_defaultSkin(spec);
// set model pointer
model = _model;
// clear data
file.clear();
material_.clear();
rgba[0] = rgba[1] = rgba[2] = 0.5f;
rgba[3] = 1.0f;
inflate = 0;
group = 0;
spec_file_.clear();
spec_material_.clear();
spec_vert_.clear();
spec_texcoord_.clear();
spec_face_.clear();
spec_bodyname_.clear();
spec_bindpos_.clear();
spec_bindquat_.clear();
spec_vertid_.clear();
spec_vertweight_.clear();
vert.clear();
texcoord.clear();
face.clear();
bodyname.clear();
bindpos.clear();
bindquat.clear();
vertid.clear();
vertweight.clear();
bodyid.clear();
matid = -1;
// point to local (needs to be after defaults)
PointToLocal();
// in case this camera is not compiled
CopyFromSpec();
}
void mjCSkin::PointToLocal() {
spec.element = (mjElement)this;
spec.name = (mjString)&name;
spec.classname = (mjString)&classname;
spec.file = (mjString)&spec_file_;
spec.material = (mjString)&spec_material_;
spec.vert = (mjFloatVec)&spec_vert_;
spec.texcoord = (mjFloatVec)&spec_texcoord_;
spec.face = (mjIntVec)&spec_face_;
spec.bodyname = (mjStringVec)&spec_bodyname_;
spec.bindpos = (mjFloatVec)&spec_bindpos_;
spec.bindquat = (mjFloatVec)&spec_bindquat_;
spec.vertid = (mjIntVecVec)&spec_vertid_;
spec.vertweight = (mjFloatVecVec)&spec_vertweight_;
spec.info = (mjString)&info;
}
void mjCSkin::CopyFromSpec() {
*static_cast<mjmSkin*>(this) = spec;
file_ = spec_file_;
material_ = spec_material_;
vert_ = spec_vert_;
texcoord_ = spec_texcoord_;
face_ = spec_face_;
bodyname_ = spec_bodyname_;
bindpos_ = spec_bindpos_;
bindquat_ = spec_bindquat_;
vertid_ = spec_vertid_;
vertweight_ = spec_vertweight_;
file = (mjString)&spec_file_;
material = (mjString)&spec_material_;
vert = (mjFloatVec)&spec_vert_;
texcoord = (mjFloatVec)&spec_texcoord_;
face = (mjIntVec)&spec_face_;
bodyname = (mjStringVec)&spec_bodyname_;
bindpos = (mjFloatVec)&spec_bindpos_;
bindquat = (mjFloatVec)&spec_bindquat_;
vertid = (mjIntVecVec)&spec_vertid_;
vertweight = (mjFloatVecVec)&spec_vertweight_;
}
// destructor
mjCSkin::~mjCSkin() {
file.clear();
material_.clear();
vert.clear();
texcoord.clear();
face.clear();
bodyname.clear();
bindpos.clear();
bindquat.clear();
vertid.clear();
vertweight.clear();
spec_file_.clear();
spec_material_.clear();
spec_vert_.clear();
spec_texcoord_.clear();
spec_face_.clear();
spec_bodyname_.clear();
spec_bindpos_.clear();
spec_bindquat_.clear();
spec_vertid_.clear();
spec_vertweight_.clear();
bodyid.clear();
}
@@ -1926,33 +1922,35 @@ mjCSkin::~mjCSkin() {
// compiler
void mjCSkin::Compile(const mjVFS* vfs) {
CopyFromSpec();
// load file
if (!file.empty()) {
if (!file_.empty()) {
// make sure data is not present
if (!vert.empty() ||
!texcoord.empty() ||
!face.empty() ||
!bodyname.empty() ||
!bindpos.empty() ||
!bindquat.empty() ||
!vertid.empty() ||
!vertweight.empty() ||
if (!vert_.empty() ||
!texcoord_.empty() ||
!face_.empty() ||
!bodyname_.empty() ||
!bindpos_.empty() ||
!bindquat_.empty() ||
!vertid_.empty() ||
!vertweight_.empty() ||
!bodyid.empty()) {
throw mjCError(this, "Data already exists, trying to load from skin file: %s", file.c_str());
throw mjCError(this, "Data already exists, trying to load from skin file: %s", file_.c_str());
}
// remove path from file if necessary
if (model->strippath) {
file = mjuu_strippath(file);
file_ = mjuu_strippath(file_);
}
// load SKN
string ext = mjuu_getext(file);
string ext = mjuu_getext(file_);
if (strcasecmp(ext.c_str(), ".skn")) {
throw mjCError(this, "Unknown skin file type: %s", file.c_str());
throw mjCError(this, "Unknown skin file type: %s", file_.c_str());
}
string filename = mjuu_makefullname(model->modelfiledir, model->meshdir, file);
string filename = mjuu_makefullname(model->modelfiledir, model->meshdir_, file_);
mjResource* resource = LoadResource(filename, vfs);
try {
@@ -1965,48 +1963,48 @@ void mjCSkin::Compile(const mjVFS* vfs) {
}
// make sure all data is present
if (vert.empty() ||
face.empty() ||
bodyname.empty() ||
bindpos.empty() ||
bindquat.empty() ||
vertid.empty() ||
vertweight.empty()) {
if (vert_.empty() ||
face_.empty() ||
bodyname_.empty() ||
bindpos_.empty() ||
bindquat_.empty() ||
vertid_.empty() ||
vertweight_.empty()) {
throw mjCError(this, "Missing data in skin");
}
// check mesh sizes
if (vert.size()%3) {
if (vert_.size()%3) {
throw mjCError(this, "Vertex data must be multiple of 3");
}
if (!texcoord.empty() && texcoord.size()!=2*vert.size()/3) {
if (!texcoord_.empty() && texcoord_.size()!=2*vert_.size()/3) {
throw mjCError(this, "Vertex and texcoord data incompatible size");
}
if (face.size()%3) {
if (face_.size()%3) {
throw mjCError(this, "Face data must be multiple of 3");
}
// check bone sizes
size_t nbone = bodyname.size();
if (bindpos.size()!=3*nbone) {
size_t nbone = bodyname_.size();
if (bindpos_.size()!=3*nbone) {
throw mjCError(this, "Unexpected bindpos size in skin");
}
if (bindquat.size()!=4*nbone) {
if (bindquat_.size()!=4*nbone) {
throw mjCError(this, "Unexpected bindquat size in skin");
}
if (vertid.size()!=nbone) {
if (vertid_.size()!=nbone) {
throw mjCError(this, "Unexpected vertid size in skin");
}
if (vertweight.size()!=nbone) {
if (vertweight_.size()!=nbone) {
throw mjCError(this, "Unexpected vertweight size in skin");
}
// resolve body names
bodyid.resize(nbone);
for (int i=0; i<nbone; i++) {
mjCBase* pbody = model->FindObject(mjOBJ_BODY, bodyname[i]);
mjCBase* pbody = model->FindObject(mjOBJ_BODY, bodyname_[i]);
if (!pbody) {
throw mjCError(this, "unknown body '%s' in skin", bodyname[i].c_str());
throw mjCError(this, "unknown body '%s' in skin", bodyname_[i].c_str());
}
bodyid[i] = pbody->id;
}
@@ -2021,28 +2019,28 @@ void mjCSkin::Compile(const mjVFS* vfs) {
// set total vertex weights to 0
vector<float> vw;
size_t nvert = vert.size()/3;
size_t nvert = vert_.size()/3;
vw.resize(nvert);
fill(vw.begin(), vw.end(), 0.0f);
// accumulate vertex weights from all bones
for (int i=0; i<nbone; i++) {
// make sure bone has vertices and sizes match
size_t nbv = vertid[i].size();
if (vertweight[i].size()!=nbv || nbv==0) {
size_t nbv = vertid_[i].size();
if (vertweight_[i].size()!=nbv || nbv==0) {
throw mjCError(this, "vertid and vertweight must have same non-zero size in skin");
}
// accumulate weights in global array
for (int j=0; j<nbv; j++) {
// get index and check range
int jj = vertid[i][j];
int jj = vertid_[i][j];
if (jj<0 || jj>=nvert) {
throw mjCError(this, "vertid %d out of range in skin", NULL, jj);
}
// accumulate
vw[jj] += vertweight[i][j];
vw[jj] += vertweight_[i][j];
}
}
@@ -2055,25 +2053,25 @@ void mjCSkin::Compile(const mjVFS* vfs) {
// normalize vertex weights
for (int i=0; i<nbone; i++) {
for (int j=0; j<vertid[i].size(); j++) {
vertweight[i][j] /= vw[vertid[i][j]];
for (int j=0; j<vertid_[i].size(); j++) {
vertweight_[i][j] /= vw[vertid_[i][j]];
}
}
// normalize bindquat
for (int i=0; i<nbone; i++) {
mjtNum quat[4] = {
(mjtNum)bindquat[4*i],
(mjtNum)bindquat[4*i+1],
(mjtNum)bindquat[4*i+2],
(mjtNum)bindquat[4*i+3]
(mjtNum)bindquat_[4*i],
(mjtNum)bindquat_[4*i+1],
(mjtNum)bindquat_[4*i+2],
(mjtNum)bindquat_[4*i+3]
};
mju_normalize4(quat);
bindquat[4*i] = (float) quat[0];
bindquat[4*i+1] = (float) quat[1];
bindquat[4*i+2] = (float) quat[2];
bindquat[4*i+3] = (float) quat[3];
bindquat_[4*i] = (float) quat[0];
bindquat_[4*i+1] = (float) quat[1];
bindquat_[4*i+2] = (float) quat[2];
bindquat_[4*i+3] = (float) quat[3];
}
}
@@ -2117,31 +2115,31 @@ void mjCSkin::LoadSKN(mjResource* resource) {
// copy vert
if (nvert) {
vert.resize(3*nvert);
memcpy(vert.data(), pdata+cnt, 3*nvert*sizeof(float));
vert_.resize(3*nvert);
memcpy(vert_.data(), pdata+cnt, 3*nvert*sizeof(float));
cnt += 3*nvert;
}
// copy texcoord
if (ntexcoord) {
texcoord.resize(2*ntexcoord);
memcpy(texcoord.data(), pdata+cnt, 2*ntexcoord*sizeof(float));
texcoord_.resize(2*ntexcoord);
memcpy(texcoord_.data(), pdata+cnt, 2*ntexcoord*sizeof(float));
cnt += 2*ntexcoord;
}
// copy face
if (nface) {
face.resize(3*nface);
memcpy(face.data(), pdata+cnt, 3*nface*sizeof(int));
face_.resize(3*nface);
memcpy(face_.data(), pdata+cnt, 3*nface*sizeof(int));
cnt += 3*nface;
}
// allocate bone arrays
bodyname.clear();
bindpos.resize(3*nbone);
bindquat.resize(4*nbone);
vertid.resize(nbone);
vertweight.resize(nbone);
bodyname_.clear();
bindpos_.resize(3*nbone);
bindquat_.resize(4*nbone);
vertid_.resize(nbone);
vertweight_.resize(nbone);
// read bones
for (int i=0; i<nbone; i++) {
@@ -2155,14 +2153,14 @@ void mjCSkin::LoadSKN(mjResource* resource) {
strncpy(txt, (char*)(pdata+cnt), 39);
txt[39] = '\0';
cnt += 10;
bodyname.push_back(txt);
bodyname_.push_back(txt);
// read bindpos
memcpy(bindpos.data()+3*i, pdata+cnt, 3*sizeof(float));
memcpy(bindpos_.data()+3*i, pdata+cnt, 3*sizeof(float));
cnt += 3;
// read bind quat
memcpy(bindquat.data()+4*i, pdata+cnt, 4*sizeof(float));
memcpy(bindquat_.data()+4*i, pdata+cnt, 4*sizeof(float));
cnt += 4;
// read vertex count
@@ -2182,13 +2180,13 @@ void mjCSkin::LoadSKN(mjResource* resource) {
}
// read vertid
vertid[i].resize(vcount);
memcpy(vertid[i].data(), (int*)(pdata+cnt), vcount*sizeof(int));
vertid_[i].resize(vcount);
memcpy(vertid_[i].data(), (int*)(pdata+cnt), vcount*sizeof(int));
cnt += vcount;
// read vertweight
vertweight[i].resize(vcount);
memcpy(vertweight[i].data(), (int*)(pdata+cnt), vcount*sizeof(int));
vertweight_[i].resize(vcount);
memcpy(vertweight_[i].data(), (int*)(pdata+cnt), vcount*sizeof(int));
cnt += vcount;
}
@@ -2222,34 +2220,11 @@ constexpr int eledge[3][6][2] = {{{ 0, 1}, {-1, -1}, {-1, -1},
// constructor
mjCFlex::mjCFlex(mjCModel* _model) {
mjm_defaultFlex(spec);
// set model
model = _model;
// set contact defaults
contype = 1;
conaffinity = 1;
condim = 3;
priority = 0;
mjuu_setvec(friction, 1, 0.005, 0.0001);
solmix = 1.0;
mj_defaultSolRefImp(solref, solimp);
margin = 0;
gap = 0;
// set other defaults
dim = 2;
radius = 0.005;
internal = true;
flatskin = false;
selfcollide = mjFLEXSELF_AUTO;
activelayers = 1;
group = 0;
edgestiffness = 0;
edgedamping = 0;
material_.clear();
rgba[0] = rgba[1] = rgba[2] = 0.5f;
rgba[3] = 1.0f;
// clear internal variables
nvert = 0;
nedge = 0;
@@ -2257,50 +2232,84 @@ mjCFlex::mjCFlex(mjCModel* _model) {
matid = -1;
rigid = false;
centered = false;
PointToLocal();
CopyFromSpec();
}
void mjCFlex::PointToLocal() {
spec.element = (mjElement)this;
spec.name = (mjString)&name;
spec.classname = (mjString)&classname;
spec.material = (mjString)&spec_material_;
spec.vertbody = (mjStringVec)&spec_vertbody_;
spec.vert = (mjDoubleVec)&spec_vert_;
spec.texcoord = (mjFloatVec)&spec_texcoord_;
spec.elem = (mjIntVec)&spec_elem_;
spec.info = (mjString)&info;
}
void mjCFlex::CopyFromSpec() {
*static_cast<mjmFlex*>(this) = spec;
spec.info = (mjString)&info;
material_ = spec_material_;
vertbody_ = spec_vertbody_;
vert_ = spec_vert_;
texcoord_ = spec_texcoord_;
elem_ = spec_elem_;
material = (mjString)&material_;
vertbody = (mjStringVec)&vertbody_;
vert = (mjDoubleVec)&vert_;
texcoord = (mjFloatVec)&texcoord_;
elem = (mjIntVec)&elem_;
}
bool mjCFlex::HasTexcoord() const {
return !texcoord.empty();
return !texcoord_.empty();
}
void mjCFlex::DelTexcoord() {
texcoord.clear();
texcoord_.clear();
}
// compiler
void mjCFlex::Compile(const mjVFS* vfs) {
CopyFromSpec();
// set nelem; check sizes
if (dim<1 || dim>3) {
throw mjCError(this, "dim must be 1, 2 or 3");
}
if (elem.empty()) {
if (elem_.empty()) {
throw mjCError(this, "elem is empty");
}
if (elem.size() % (dim+1)) {
if (elem_.size() % (dim+1)) {
throw mjCError(this, "elem size must be multiple of (dim+1)");
}
if (vertbody.empty()) {
if (vertbody_.empty()) {
throw mjCError(this, "vertbody is empty");
}
if (vert.size() % 3) {
if (vert_.size() % 3) {
throw mjCError(this, "vert size must be a multiple of 3");
}
if (edgestiffness>0 && dim>1) {
throw mjCError(this, "edge stiffness only available for dim=1, please use elasticity plugins");
}
nelem = (int)elem.size()/(dim+1);
nelem = (int)elem_.size()/(dim+1);
// set nvert, rigid, centered; check size
if (vert.empty()) {
if (vert_.empty()) {
centered = true;
nvert = (int)vertbody.size();
nvert = (int)vertbody_.size();
}
else {
nvert = (int)vert.size()/3;
if (vertbody.size()==1) {
nvert = (int)vert_.size()/3;
if (vertbody_.size()==1) {
rigid = true;
}
}
@@ -2309,14 +2318,14 @@ void mjCFlex::Compile(const mjVFS* vfs) {
}
// check elem vertex ids
for (int i=0; i<(int)elem.size(); i++) {
if (elem[i]<0 || elem[i]>=nvert) {
for (int i=0; i<(int)elem_.size(); i++) {
if (elem_[i]<0 || elem_[i]>=nvert) {
throw mjCError(this, "elem vertex id out of range");
}
}
// check texcoord
if (!texcoord.empty() && texcoord.size()!=2*nvert) {
if (!texcoord_.empty() && texcoord_.size()!=2*nvert) {
throw mjCError(this, "two texture coordinates per vertex expected");
}
@@ -2329,20 +2338,20 @@ void mjCFlex::Compile(const mjVFS* vfs) {
}
// resolve body ids
for (int i=0; i<(int)vertbody.size(); i++) {
mjCBase* pbody = model->FindObject(mjOBJ_BODY, vertbody[i]);
for (int i=0; i<(int)vertbody_.size(); i++) {
mjCBase* pbody = model->FindObject(mjOBJ_BODY, vertbody_[i]);
if (pbody) {
vertbodyid.push_back(pbody->id);
} else {
throw mjCError(this, "unkown body '%s' in flex", vertbody[i].c_str());
throw mjCError(this, "unkown body '%s' in flex", vertbody_[i].c_str());
}
}
// process elements
for (int e=0; e<(int)elem.size()/(dim+1); e++) {
for (int e=0; e<(int)elem_.size()/(dim+1); e++) {
// make sorted copy of element
vector<int> el;
el.assign(elem.begin()+e*(dim+1), elem.begin()+(e+1)*(dim+1));
el.assign(elem_.begin()+e*(dim+1), elem_.begin()+(e+1)*(dim+1));
std::sort(el.begin(), el.end());
// check for repeated vertices
@@ -2367,8 +2376,8 @@ void mjCFlex::Compile(const mjVFS* vfs) {
// determine centered if not already set
if (!centered) {
centered = true;
for (int i=0; i<(int)vert.size(); i++) {
if (vert[i]!=0) {
for (int i=0; i<(int)vert_.size(); i++) {
if (vert_[i]!=0) {
centered = false;
break;
}
@@ -2385,7 +2394,7 @@ void mjCFlex::Compile(const mjVFS* vfs) {
// add vertex offset within body if not centered
if (!centered) {
mjtNum offset[3];
mju_rotVecQuat(offset, vert.data()+3*i, model->bodies[b]->xquat0);
mju_rotVecQuat(offset, vert_.data()+3*i, model->bodies[b]->xquat0);
mju_addTo3(vertxpos.data()+3*i, offset);
}
}
@@ -2394,7 +2403,7 @@ void mjCFlex::Compile(const mjVFS* vfs) {
// faces are (0,1,2); (0,2,3); (0,3,1); (1,3,2)
if (dim==3) {
for (int e=0; e<nelem; e++) {
const int* edata = elem.data() + e*(dim+1);
const int* edata = elem_.data() + e*(dim+1);
mjtNum* v0 = vertxpos.data() + 3*edata[0];
mjtNum* v1 = vertxpos.data() + 3*edata[1];
mjtNum* v2 = vertxpos.data() + 3*edata[2];
@@ -2408,22 +2417,22 @@ void mjCFlex::Compile(const mjVFS* vfs) {
mju_cross(nrm, v01, v02);
if (mju_dot3(nrm, v03)>0) {
// flip orientation
int tmp = elem[e*(dim+1)+1];
elem[e*(dim+1)+1] = elem[e*(dim+1)+2];
elem[e*(dim+1)+2] = tmp;
int tmp = elem_[e*(dim+1)+1];
elem_[e*(dim+1)+1] = elem_[e*(dim+1)+2];
elem_[e*(dim+1)+2] = tmp;
}
}
}
// create edges
std::vector<int> edgeidx(elem.size()*kNumEdges[dim-1]);
std::vector<int> edgeidx(elem_.size()*kNumEdges[dim-1]);
// map from edge vertices to their index in `edges` vector
std::unordered_map<std::pair<int, int>, int, PairHash> edge_indices;
// insert local edges into global vector
for (int f = 0; f < (int)elem.size()/(dim+1); f++) {
int* v = elem.data() + f*(dim+1);
for (int f = 0; f < (int)elem_.size()/(dim+1); f++) {
int* v = elem_.data() + f*(dim+1);
for (int e = 0; e < kNumEdges[dim-1]; e++) {
auto pair = std::pair(
std::min(v[eledge[dim-1][e][0]], v[eledge[dim-1][e][1]]),
@@ -2447,7 +2456,7 @@ void mjCFlex::Compile(const mjVFS* vfs) {
// add plugins
std::string userface, useredge;
mjXUtil::Vector2String(userface, elem);
mjXUtil::Vector2String(userface, elem_);
mjXUtil::Vector2String(useredge, edgeidx);
for (int i=0; i<(int)vertbodyid.size(); i++) {
@@ -2472,12 +2481,12 @@ void mjCFlex::CreateBVH(void) {
int nbvh = 0;
// allocate element bounding boxes
elemaabb.resize(6*nelem);
elemaabb_.resize(6*nelem);
tree.AllocateBoundingVolumes(nelem);
// construct element bounding boxes, add to hierarchy
for (int e=0; e<nelem; e++) {
const int* edata = elem.data() + e*(dim+1);
const int* edata = elem_.data() + e*(dim+1);
// skip inactive in 3D
if (dim==3 && elemlayer[e]>=activelayers) {
@@ -2496,12 +2505,12 @@ void mjCFlex::CreateBVH(void) {
}
// compute aabb (center, size)
elemaabb[6*e+0] = 0.5*(xmax[0]+xmin[0]);
elemaabb[6*e+1] = 0.5*(xmax[1]+xmin[1]);
elemaabb[6*e+2] = 0.5*(xmax[2]+xmin[2]);
elemaabb[6*e+3] = 0.5*(xmax[0]-xmin[0]) + radius;
elemaabb[6*e+4] = 0.5*(xmax[1]-xmin[1]) + radius;
elemaabb[6*e+5] = 0.5*(xmax[2]-xmin[2]) + radius;
elemaabb_[6*e+0] = 0.5*(xmax[0]+xmin[0]);
elemaabb_[6*e+1] = 0.5*(xmax[1]+xmin[1]);
elemaabb_[6*e+2] = 0.5*(xmax[2]+xmin[2]);
elemaabb_[6*e+3] = 0.5*(xmax[0]-xmin[0]) + radius;
elemaabb_[6*e+4] = 0.5*(xmax[1]-xmin[1]) + radius;
elemaabb_[6*e+5] = 0.5*(xmax[2]-xmin[2]) + radius;
// add bounding volume for this element
mjCBoundingVolume* bv = tree.GetBoundingVolume(nbvh++);
@@ -2509,7 +2518,7 @@ void mjCFlex::CreateBVH(void) {
bv->conaffinity = conaffinity;
bv->quat = NULL;
bv->SetId(e);
bv->aabb = elemaabb.data() + 6*e;
bv->aabb = elemaabb_.data() + 6*e;
bv->pos = bv->aabb;
}
@@ -2533,7 +2542,7 @@ void mjCFlex::CreateShellPair(void) {
// element vertices in original (unsorted) order
vector<int> el;
el.assign(elem.begin()+n, elem.begin()+n+dim+1);
el.assign(elem_.begin()+n, elem_.begin()+n+dim+1);
// line: 2 vertex fragments
if (dim==1) {
@@ -2702,7 +2711,7 @@ void mjCFlex::CreateShellPair(void) {
// process both elements
for (int ei=0; ei<2; ei++) {
const int* edata = elem.data() + connectspec[n][ei]*(dim+1);
const int* edata = elem_.data() + connectspec[n][ei]*(dim+1);
// find element vertex that is not in the common fragment
for (int i=0; i<=dim; i++) {
src/user/user_model.cc
+112 -131
View File
@@ -41,6 +41,7 @@
#include "engine/engine_util_blas.h"
#include "engine/engine_util_errmem.h"
#include "engine/engine_util_misc.h"
#include "user/user_api.h"
#include "user/user_objects.h"
#include "user/user_util.h"
@@ -82,40 +83,12 @@ static void copyvec(T1* dest, T2* src, int n) {
// constructor
mjCModel::mjCModel() {
mjm_defaultModel(spec);
comment.clear();
modelfiledir.clear();
//------------------------ compiler settings
autolimits = true;
boundmass = 0;
boundinertia = 0;
settotalmass = -1;
balanceinertia = false;
strippath = false;
fitaabb = false;
degree = true;
euler[0] = 'x';
euler[1] = 'y';
euler[2] = 'z';
meshdir.clear();
texturedir.clear();
discardvisual = false;
convexhull = true;
usethread = true;
fusestatic = false;
inertiafromgeom = mjINERTIAFROMGEOM_AUTO;
inertiagrouprange[0] = 0;
inertiagrouprange[1] = mjNGROUP-1;
exactmeshinertia = false;
mj_defaultLROpt(&LRopt);
//------------------------ statistics override
meaninertia = mjNAN;
meanmass = mjNAN;
meansize = mjNAN;
extent = mjNAN;
center[0] = mjNAN;
center[1] = center[2] = 0;
spec_meshdir_.clear();
spec_texturedir_.clear();
spec_modelname_ = "MuJoCo Model";
//------------------------ auto-computed statistics
#ifndef MEMORY_SANITIZER
@@ -127,28 +100,8 @@ mjCModel::mjCModel() {
center_auto[0] = center_auto[1] = center_auto[2] = 0;
#endif
//------------------------ engine data
modelname = "MuJoCo Model";
mj_defaultOption(&option);
mj_defaultVisual(&visual);
memory = -1;
nemax = 0;
njmax = -1;
nconmax = -1;
nstack = -1;
nuserdata = 0;
nkey = 0;
nmocap = 0;
nplugin = 0;
nuser_body = -1;
nuser_jnt = -1;
nuser_geom = -1;
nuser_site = -1;
nuser_cam = -1;
nuser_tendon = -1;
nuser_actuator = -1;
nuser_sensor = -1;
//------------------------ private variables
cameras.clear();
lights.clear();
@@ -214,6 +167,31 @@ mjCModel::mjCModel() {
object_lists[mjOBJ_TUPLE] = (std::vector<mjCBase*>*) &tuples;
object_lists[mjOBJ_KEY] = (std::vector<mjCBase*>*) &keys;
object_lists[mjOBJ_PLUGIN] = (std::vector<mjCBase*>*) &plugins;
// point to model from spec
PointToLocal();
}
void mjCModel::PointToLocal() {
spec.element = (mjElement)this;
spec.modelname = (mjString)&spec_modelname_;
spec.meshdir = (mjString)&spec_meshdir_;
spec.texturedir = (mjString)&spec_texturedir_;
}
void mjCModel::CopyFromSpec() {
*static_cast<mjmModel*>(this) = spec;
modelname_ = spec_modelname_;
meshdir_ = spec_meshdir_;
texturedir_ = spec_texturedir_;
modelname = (mjString)&modelname_;
meshdir = (mjString)&meshdir_;
texturedir = (mjString)&texturedir_;
}
@@ -904,6 +882,7 @@ void mjCModel::SetDefaultNames(std::vector<T*>& assets) {
// use filename if name is missing
for (int i=0; i<assets.size(); i++) {
assets[i]->CopyFromSpec();
if (assets[i]->name.empty()) {
stripped = mjuu_strippath(assets[i]->get_file());
assets[i]->name = mjuu_stripext(stripped);
@@ -1043,12 +1022,12 @@ void mjCModel::SetSizes(void) {
// skin counts
for (int i=0; i<nskin; i++) {
nskinvert += skins[i]->vert.size()/3;
nskintexvert += skins[i]->texcoord.size()/2;
nskinface += skins[i]->face.size()/3;
nskinvert += skins[i]->get_vert().size()/3;
nskintexvert += skins[i]->get_texcoord().size()/2;
nskinface += skins[i]->get_face().size()/3;
nskinbone += skins[i]->bodyid.size();
for (int j=0; j<skins[i]->bodyid.size(); j++) {
nskinbonevert += skins[i]->vertid[j].size();
nskinbonevert += skins[i]->get_vertid()[j].size();
}
}
@@ -1068,16 +1047,16 @@ void mjCModel::SetSizes(void) {
for (int i=0; i<nnumeric; i++) nnumericdata += numerics[i]->size;
// ntextdata
for (int i=0; i<ntext; i++) ntextdata += (int)texts[i]->data.size() + 1;
for (int i=0; i<ntext; i++) ntextdata += (int)texts[i]->data_.size() + 1;
// ntupledata
for (int i=0; i<ntuple; i++) ntupledata += (int)tuples[i]->objtype.size();
for (int i=0; i<ntuple; i++) ntupledata += (int)tuples[i]->objtype_.size();
// npluginattr
for (int i=0; i<nplugin; i++) npluginattr += (int)plugins[i]->flattened_attributes.size();
// nnames
nnames = (int)modelname.size() + 1;
nnames = (int)modelname_.size() + 1;
for (int i=0; i<nbody; i++) nnames += (int)bodies[i]->name.length() + 1;
for (int i=0; i<njnt; i++) nnames += (int)joints[i]->name.length() + 1;
for (int i=0; i<ngeom; i++) nnames += (int)geoms[i]->name.length() + 1;
@@ -1356,9 +1335,9 @@ static int namelist(vector<T*>& list, int adr, int* name_adr, char* names, int*
// copy names, compute name addresses
void mjCModel::CopyNames(mjModel* m) {
// start with model name
int adr = (int)modelname.size()+1;
int adr = (int)modelname_.size()+1;
int* map_adr = m->names_map;
mju_strncpy(m->names, modelname.c_str(), m->nnames);
mju_strncpy(m->names, modelname_.c_str(), m->nnames);
memset(m->names_map, -1, sizeof(int) * m->nnames_map);
// process all lists
@@ -1995,15 +1974,15 @@ void mjCModel::CopyObjects(mjModel* m) {
m->flex_evpairnum[i] = (int)pfl->evpair.size()/2;
memcpy(m->flex_evpair + 2*evpair_adr, pfl->evpair.data(), pfl->evpair.size()*sizeof(int));
}
if (pfl->texcoord.empty()) {
if (pfl->texcoord_.empty()) {
m->flex_texcoordadr[i] = -1;
} else {
m->flex_texcoordadr[i] = texcoord_adr;
memcpy(m->flex_texcoord + 2*texcoord_adr,
pfl->texcoord.data(), pfl->texcoord.size()*sizeof(float));
pfl->texcoord_.data(), pfl->texcoord_.size()*sizeof(float));
}
m->flex_elemnum[i] = pfl->nelem;
memcpy(m->flex_elem + elemdata_adr, pfl->elem.data(), pfl->elem.size()*sizeof(int));
memcpy(m->flex_elem + elemdata_adr, pfl->elem_.data(), pfl->elem_.size()*sizeof(int));
memcpy(m->flex_elemlayer + elem_adr, pfl->elemlayer.data(), pfl->nelem*sizeof(int));
if (m->flex_shellnum[i]) {
memcpy(m->flex_shell + shelldata_adr, pfl->shell.data(), pfl->shell.size()*sizeof(int));
@@ -2042,7 +2021,7 @@ void mjCModel::CopyObjects(mjModel* m) {
mju_zero(m->flex_vert + 3*vert_adr, 3*pfl->nvert);
}
else {
memcpy(m->flex_vert + 3*vert_adr, pfl->vert.data(), 3*pfl->nvert*sizeof(mjtNum));
memcpy(m->flex_vert + 3*vert_adr, pfl->vert_.data(), 3*pfl->nvert*sizeof(mjtNum));
}
// copy or set vertbodyid
@@ -2073,7 +2052,7 @@ void mjCModel::CopyObjects(mjModel* m) {
elemdata_adr += (pfl->dim+1) * pfl->nelem;
shelldata_adr += (int)pfl->shell.size();
evpair_adr += (int)pfl->evpair.size()/2;
texcoord_adr += (int)pfl->texcoord.size()/2;
texcoord_adr += (int)pfl->texcoord_.size()/2;
bvh_adr += pfl->tree.nbvh;
}
@@ -2093,25 +2072,25 @@ void mjCModel::CopyObjects(mjModel* m) {
copyvec(m->skin_rgba+4*i, psk->rgba, 4);
m->skin_inflate[i] = psk->inflate;
m->skin_vertadr[i] = vert_adr;
m->skin_vertnum[i] = psk->vert.size()/3;
m->skin_texcoordadr[i] = (!psk->texcoord.empty() ? texcoord_adr : -1);
m->skin_vertnum[i] = psk->get_vert().size()/3;
m->skin_texcoordadr[i] = (!psk->get_texcoord().empty() ? texcoord_adr : -1);
m->skin_faceadr[i] = face_adr;
m->skin_facenum[i] = psk->face.size()/3;
m->skin_facenum[i] = psk->get_face().size()/3;
m->skin_boneadr[i] = bone_adr;
m->skin_bonenum[i] = psk->bodyid.size();
// copy mesh data
memcpy(m->skin_vert + 3*vert_adr, psk->vert.data(), psk->vert.size()*sizeof(float));
if (!psk->texcoord.empty())
memcpy(m->skin_texcoord + 2*texcoord_adr, psk->texcoord.data(),
psk->texcoord.size()*sizeof(float));
memcpy(m->skin_face + 3*face_adr, psk->face.data(), psk->face.size()*sizeof(int));
memcpy(m->skin_vert + 3*vert_adr, psk->get_vert().data(), psk->get_vert().size()*sizeof(float));
if (!psk->get_texcoord().empty())
memcpy(m->skin_texcoord + 2*texcoord_adr, psk->get_texcoord().data(),
psk->get_texcoord().size()*sizeof(float));
memcpy(m->skin_face + 3*face_adr, psk->get_face().data(), psk->get_face().size()*sizeof(int));
// copy bind poses and body ids
memcpy(m->skin_bonebindpos+3*bone_adr, psk->bindpos.data(),
psk->bindpos.size()*sizeof(float));
memcpy(m->skin_bonebindquat+4*bone_adr, psk->bindquat.data(),
psk->bindquat.size()*sizeof(float));
memcpy(m->skin_bonebindpos+3*bone_adr, psk->get_bindpos().data(),
psk->get_bindpos().size()*sizeof(float));
memcpy(m->skin_bonebindquat+4*bone_adr, psk->get_bindquat().data(),
psk->get_bindquat().size()*sizeof(float));
memcpy(m->skin_bonebodyid+bone_adr, psk->bodyid.data(),
psk->bodyid.size()*sizeof(int));
@@ -2119,13 +2098,13 @@ void mjCModel::CopyObjects(mjModel* m) {
for (int j=0; j<m->skin_bonenum[i]; j++) {
// set fields
m->skin_bonevertadr[bone_adr+j] = bonevert_adr;
m->skin_bonevertnum[bone_adr+j] = (int)psk->vertid[j].size();
m->skin_bonevertnum[bone_adr+j] = (int)psk->get_vertid()[j].size();
// copy data
memcpy(m->skin_bonevertid+bonevert_adr, psk->vertid[j].data(),
psk->vertid[j].size()*sizeof(int));
memcpy(m->skin_bonevertweight+bonevert_adr, psk->vertweight[j].data(),
psk->vertid[j].size()*sizeof(float));
memcpy(m->skin_bonevertid+bonevert_adr, psk->get_vertid()[j].data(),
psk->get_vertid()[j].size()*sizeof(int));
memcpy(m->skin_bonevertweight+bonevert_adr, psk->get_vertweight()[j].data(),
psk->get_vertid()[j].size()*sizeof(float));
// advance counter
bonevert_adr += m->skin_bonevertnum[bone_adr+j];
@@ -2133,7 +2112,7 @@ void mjCModel::CopyObjects(mjModel* m) {
// advance mesh and bone counters
vert_adr += m->skin_vertnum[i];
texcoord_adr += psk->texcoord.size()/2;
texcoord_adr += psk->get_texcoord().size()/2;
face_adr += m->skin_facenum[i];
bone_adr += m->skin_bonenum[i];
}
@@ -2334,10 +2313,10 @@ void mjCModel::CopyObjects(mjModel* m) {
// set fields
m->numeric_adr[i] = adr;
m->numeric_size[i] = pcu->size;
for (int j=0; j<(int)pcu->data.size(); j++) {
m->numeric_data[adr+j] = (mjtNum)pcu->data[j];
for (int j=0; j<(int)pcu->data_.size(); j++) {
m->numeric_data[adr+j] = (mjtNum)pcu->data_[j];
}
for (int j=(int)pcu->data.size(); j<(int)pcu->size; j++) {
for (int j=(int)pcu->data_.size(); j<(int)pcu->size; j++) {
m->numeric_data[adr+j] = 0;
}
@@ -2353,8 +2332,8 @@ void mjCModel::CopyObjects(mjModel* m) {
// set fields
m->text_adr[i] = adr;
m->text_size[i] = (int)pte->data.size()+1;
mju_strncpy(m->text_data + adr, pte->data.c_str(), m->ntextdata - adr);
m->text_size[i] = (int)pte->data_.size()+1;
mju_strncpy(m->text_data + adr, pte->data_.c_str(), m->ntextdata - adr);
// advance address counter
adr += m->text_size[i];
@@ -2368,11 +2347,11 @@ void mjCModel::CopyObjects(mjModel* m) {
// set fields
m->tuple_adr[i] = adr;
m->tuple_size[i] = (int)ptu->objtype.size();
m->tuple_size[i] = (int)ptu->objtype_.size();
for (int j=0; j<m->tuple_size[i]; j++) {
m->tuple_objtype[adr+j] = (int)ptu->objtype[j];
m->tuple_objtype[adr+j] = (int)ptu->objtype_[j];
m->tuple_objid[adr+j] = ptu->obj[j]->id;
m->tuple_objprm[adr+j] = (mjtNum)ptu->objprm[j];
m->tuple_objprm[adr+j] = (mjtNum)ptu->objprm_[j];
}
// advance address counter
@@ -2383,14 +2362,14 @@ void mjCModel::CopyObjects(mjModel* m) {
for (int i=0; i<nkey; i++) {
// copy data
m->key_time[i] = (mjtNum)keys[i]->time;
copyvec(m->key_qpos+i*nq, keys[i]->qpos.data(), nq);
copyvec(m->key_qvel+i*nv, keys[i]->qvel.data(), nv);
copyvec(m->key_qpos+i*nq, keys[i]->qpos_.data(), nq);
copyvec(m->key_qvel+i*nv, keys[i]->qvel_.data(), nv);
if (na) {
copyvec(m->key_act+i*na, keys[i]->act.data(), na);
copyvec(m->key_act+i*na, keys[i]->act_.data(), na);
}
if (nmocap) {
copyvec(m->key_mpos + i*3*nmocap, keys[i]->mpos.data(), 3*nmocap);
copyvec(m->key_mquat + i*4*nmocap, keys[i]->mquat.data(), 4*nmocap);
copyvec(m->key_mpos + i*3*nmocap, keys[i]->mpos_.data(), 3*nmocap);
copyvec(m->key_mquat + i*4*nmocap, keys[i]->mquat_.data(), 4*nmocap);
}
// normalize quaternions in m->key_qpos
@@ -2405,7 +2384,7 @@ void mjCModel::CopyObjects(mjModel* m) {
mju_normalize4(m->key_mquat+i*4*nmocap+4*j);
}
copyvec(m->key_ctrl+i*nu, keys[i]->ctrl.data(), nu);
copyvec(m->key_ctrl+i*nu, keys[i]->ctrl_.data(), nu);
}
// save qpos0 in user model (to recognize changed key_qpos in write)
@@ -2742,6 +2721,8 @@ static void warninghandler(const char* msg) {
// compiler
mjModel* mjCModel::Compile(const mjVFS* vfs) {
CopyFromSpec();
// The volatile keyword is necessary to prevent a possible memory leak due to
// an interaction between longjmp and compiler optimization. Specifically, at
// the point where the setjmp takes places, these pointers have never been
@@ -2821,16 +2802,16 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) {
}
// append directory separator
if (!meshdir.empty()) {
int n = meshdir.length();
if (meshdir[n-1]!='/' && meshdir[n-1]!='\\') {
meshdir += '/';
if (!meshdir_.empty()) {
int n = meshdir_.length();
if (meshdir_[n-1]!='/' && meshdir_[n-1]!='\\') {
meshdir_ += '/';
}
}
if (!texturedir.empty()) {
int n = texturedir.length();
if (texturedir[n-1]!='/' && texturedir[n-1]!='\\') {
texturedir += '/';
if (!texturedir_.empty()) {
int n = texturedir_.length();
if (texturedir_[n-1]!='/' && texturedir_[n-1]!='\\') {
texturedir_ += '/';
}
}
@@ -3037,7 +3018,7 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) {
{
int adr = 0;
for (int i = 0; i < nplugin; ++i) {
m->plugin[i] = plugins[i]->plugin_slot;
m->plugin[i] = plugins[i]->spec.plugin_slot;
const int size = plugins[i]->flattened_attributes.size();
std::memcpy(m->plugin_attr + adr,
plugins[i]->flattened_attributes.data(), size);
@@ -3202,11 +3183,11 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) {
copyvec(center_auto, m->stat.center, 3);
// override model statistics if defined by user
if (mjuu_defined(extent)) m->stat.extent = (mjtNum)extent;
if (mjuu_defined(meaninertia)) m->stat.meaninertia = (mjtNum)meaninertia;
if (mjuu_defined(meanmass)) m->stat.meanmass = (mjtNum)meanmass;
if (mjuu_defined(meansize)) m->stat.meansize = (mjtNum)meansize;
if (mjuu_defined(center[0])) copyvec(m->stat.center, center, 3);
if (mjuu_defined(stat.extent)) m->stat.extent = (mjtNum)stat.extent;
if (mjuu_defined(stat.meaninertia)) m->stat.meaninertia = (mjtNum)stat.meaninertia;
if (mjuu_defined(stat.meanmass)) m->stat.meanmass = (mjtNum)stat.meanmass;
if (mjuu_defined(stat.meansize)) m->stat.meansize = (mjtNum)stat.meansize;
if (mjuu_defined(stat.center[0])) copyvec(m->stat.center, stat.center, 3);
// assert that model has valid references
const char* validationerr = mj_validateReferences(m);
@@ -3278,14 +3259,14 @@ bool mjCModel::CopyBack(const mjModel* m) {
visual = m->vis;
// runtime-modifiable members of mjStatistic, if different from computed values
if (m->stat.meaninertia != meaninertia_auto) meaninertia = m->stat.meaninertia;
if (m->stat.meanmass != meanmass_auto) meanmass = m->stat.meanmass;
if (m->stat.meansize != meansize_auto) meansize = m->stat.meansize;
if (m->stat.extent != extent_auto) extent = m->stat.extent;
if (m->stat.meaninertia != meaninertia_auto) stat.meaninertia = m->stat.meaninertia;
if (m->stat.meanmass != meanmass_auto) stat.meanmass = m->stat.meanmass;
if (m->stat.meansize != meansize_auto) stat.meansize = m->stat.meansize;
if (m->stat.extent != extent_auto) stat.extent = m->stat.extent;
if (m->stat.center[0] != center_auto[0] ||
m->stat.center[1] != center_auto[1] ||
m->stat.center[2] != center_auto[2]) {
mju_copy3(center, m->stat.center);
mju_copy3(stat.center, m->stat.center);
}
// qpos0, qpos_spring
@@ -3386,11 +3367,11 @@ bool mjCModel::CopyBack(const mjModel* m) {
mjCHField* phf;
for (int i=0; i<nhfield; i++) {
phf = hfields[i];
int size = phf->userdata().size();
int size = phf->get_userdata().size();
if (size) {
int nrow = m->hfield_nrow[i];
int ncol = m->hfield_ncol[i];
float* userdata = phf->userdata().data();
float* userdata = phf->get_userdata().data();
float* modeldata = m->hfield_data + m->hfield_adr[i];
// copy back in reverse row order
for (int j=0; j<nrow; j++) {
@@ -3517,14 +3498,14 @@ bool mjCModel::CopyBack(const mjModel* m) {
// numeric data
for (int i=0; i<nnumeric; i++) {
for (int j=0; j<m->numeric_size[i]; j++) {
numerics[i]->data[j] = (double)m->numeric_data[m->numeric_adr[i]+j];
numerics[i]->data_[j] = (double)m->numeric_data[m->numeric_adr[i]+j];
}
}
// tuple data
for (int i=0; i<ntuple; i++) {
for (int j=0; j<m->tuple_size[i]; j++) {
tuples[i]->objprm[j] = (double)m->tuple_objprm[m->tuple_adr[i]+j];
tuples[i]->objprm_[j] = (double)m->tuple_objprm[m->tuple_adr[i]+j];
}
}
@@ -3533,17 +3514,17 @@ bool mjCModel::CopyBack(const mjModel* m) {
mjCKey* pk = keys[i];
pk->time = (double)m->key_time[i];
copyvec(pk->qpos.data(), m->key_qpos + i*nq, nq);
copyvec(pk->qvel.data(), m->key_qvel + i*nv, nv);
copyvec(pk->qpos_.data(), m->key_qpos + i*nq, nq);
copyvec(pk->qvel_.data(), m->key_qvel + i*nv, nv);
if (na) {
copyvec(pk->act.data(), m->key_act + i*na, na);
copyvec(pk->act_.data(), m->key_act + i*na, na);
}
if (nmocap) {
copyvec(pk->mpos.data(), m->key_mpos + i*3*nmocap, 3*nmocap);
copyvec(pk->mquat.data(), m->key_mquat + i*4*nmocap, 4*nmocap);
copyvec(pk->mpos_.data(), m->key_mpos + i*3*nmocap, 3*nmocap);
copyvec(pk->mquat_.data(), m->key_mquat + i*4*nmocap, 4*nmocap);
}
if (nu) {
copyvec(pk->ctrl.data(), m->key_ctrl + i*nu, nu);
copyvec(pk->ctrl_.data(), m->key_ctrl + i*nu, nu);
}
}
@@ -3568,8 +3549,8 @@ void mjCModel::ResolvePlugin(mjCBase* obj, const std::string& plugin_name,
}
// implicit plugin instance
if (*plugin_instance && (*plugin_instance)->plugin_slot == -1) {
(*plugin_instance)->plugin_slot = plugin_slot;
if (*plugin_instance && (*plugin_instance)->spec.plugin_slot == -1) {
(*plugin_instance)->spec.plugin_slot = plugin_slot;
(*plugin_instance)->parent = obj;
}
@@ -3581,10 +3562,10 @@ void mjCModel::ResolvePlugin(mjCBase* obj, const std::string& plugin_name,
throw mjCError(
obj, "unrecognized name '%s' for plugin instance", plugin_instance_name.c_str());
}
if (plugin_slot != -1 && plugin_slot != (*plugin_instance)->plugin_slot) {
if (plugin_slot != -1 && plugin_slot != (*plugin_instance)->spec.plugin_slot) {
throw mjCError(
obj, "'plugin' attribute does not match that of the instance");
}
plugin_slot = (*plugin_instance)->plugin_slot;
plugin_slot = (*plugin_instance)->spec.plugin_slot;
}
}
src/user/user_model.h
+26 -64
View File
@@ -25,14 +25,10 @@
#include <mujoco/mjdata.h>
#include <mujoco/mjmodel.h>
#include <mujoco/mjplugin.h>
#include <mujoco/mjtnum.h>
#include "user/user_api.h"
#include "user/user_objects.h"
typedef enum _mjtInertiaFromGeom {
mjINERTIAFROMGEOM_FALSE = 0, // do not use; inertial element required
mjINERTIAFROMGEOM_TRUE, // always use; overwrite inertial element
mjINERTIAFROMGEOM_AUTO // use only if inertial element is missing
} mjtInertiaFromGeom;
typedef std::map<std::string, int, std::less<> > mjKeyMap;
typedef std::array<mjKeyMap, mjNOBJECT> mjListKeyMap;
@@ -46,31 +42,33 @@ typedef std::array<mjKeyMap, mjNOBJECT> mjListKeyMap;
// constructed, 'Compile' can be called to generate the corresponding mjModel object
// (which is the low-level model). The mjCModel object can then be deleted.
class mjCModel {
class mjCModel : private mjmModel {
friend class mjCBody;
friend class mjCJoint;
friend class mjCCamera;
friend class mjCGeom;
friend class mjCFlex;
friend class mjCHField;
friend class mjCFrame;
friend class mjCJoint;
friend class mjCEquality;
friend class mjCMesh;
friend class mjCSkin;
friend class mjCHField;
friend class mjCPair;
friend class mjCBodyPair;
friend class mjCSite;
friend class mjCEquality;
friend class mjCTendon;
friend class mjCWrap;
friend class mjCTexture;
friend class mjCActuator;
friend class mjCSensor;
friend class mjCNumeric;
friend class mjCTuple;
friend class mjCKey;
friend class mjCDef;
friend class mjXReader;
friend class mjXWriter;
public:
mjCModel(); // constructor
~mjCModel(); // destructor
void CopyFromSpec(); // copy spec to private attributes
void PointToLocal();
mjmModel spec;
mjModel* Compile(const mjVFS* vfs = 0); // COMPILER: construct mjModel
bool CopyBack(const mjModel*); // DECOMPILER: copy numeric back
@@ -119,6 +117,10 @@ class mjCModel {
mjCBase* FindObject(mjtObj type, std::string name); // find object given type and name
bool IsNullPose(const mjtNum* pos, const mjtNum* quat); // detect null pose
//------------------------ getters
std::string get_meshdir(void) const { return meshdir_; }
std::string get_texturedir(void) const { return texturedir_; }
//------------------------ API for plugins
void ResolvePlugin(mjCBase* obj, // resolve plugin instance, create a new one if needed
const std::string& plugin_name,
@@ -131,53 +133,6 @@ class mjCModel {
std::string modelfiledir; // path to model file
std::vector<mjCDef*> defaults; // settings for each defaults class
//------------------------ compiler settings
bool autolimits; // infer "limited" attribute based on range
double boundmass; // enforce minimum body mass
double boundinertia; // enforce minimum body diagonal inertia
double settotalmass; // rescale masses and inertias; <=0: ignore
bool balanceinertia; // automatically impose A + B >= C rule
bool strippath; // automatically strip paths from mesh files
bool fitaabb; // meshfit to aabb instead of inertia box
bool degree; // angles in radians or degrees
char euler[3]; // sequence for euler rotations
std::string meshdir; // mesh and hfield directory
std::string texturedir; // texture directory
bool discardvisual; // discard visual geoms in parser
bool convexhull; // compute mesh convex hulls
bool usethread; // use multiple threads to speed up compiler
bool fusestatic; // fuse static bodies with parent
int inertiafromgeom; // use geom inertias (mjtInertiaFromGeom)
int inertiagrouprange[2]; // range of geom groups used to compute inertia
bool exactmeshinertia; // if false, use old formula
mjLROpt LRopt; // options for lengthrange computation
//------------------------ statistics override (if defined)
double meaninertia; // mean diagonal inertia
double meanmass; // mean body mass
double meansize; // mean body size
double extent; // spatial extent
double center[3]; // center of model
//------------------------ engine data
std::string modelname; // model name
mjOption option; // options
mjVisual visual; // visual options
std::size_t memory; // size of arena+stack memory in bytes
int nemax; // max number of equality constraints
int njmax; // max number of constraints (Jacobian rows)
int nconmax; // max number of detected contacts (mjContact array size)
size_t nstack; // (deprecated) number of fields in mjData stack
int nuserdata; // number extra fields in mjData
int nuser_body; // number of mjtNums in body_user
int nuser_jnt; // number of mjtNums in jnt_user
int nuser_geom; // number of mjtNums in geom_user
int nuser_site; // number of mjtNums in site_user
int nuser_cam; // number of mjtNums in cam_user
int nuser_tendon; // number of mjtNums in tendon_user
int nuser_actuator; // number of mjtNums in actuator_user
int nuser_sensor; // number of mjtNums in sensor_user
private:
void TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs);
mjModel* _Compile(const mjVFS* vfs);
@@ -230,7 +185,6 @@ class mjCModel {
int nnumeric; // number of numeric fields
int ntext; // number of text fields
int ntuple; // number of tuple fields
int nkey; // number of keyframes
int nmocap; // number of mocap bodies
int nplugin; // number of plugin instances
@@ -323,5 +277,13 @@ class mjCModel {
mjCError errInfo; // last error info
int fixCount; // how many bodies have been fixed
std::vector<mjtNum> qpos0; // save qpos0, to recognize changed key_qpos in write
// variable-size attributes
std::string modelname_;
std::string meshdir_;
std::string texturedir_;
std::string spec_modelname_;
std::string spec_meshdir_;
std::string spec_texturedir_;
};
#endif // MUJOCO_SRC_USER_USER_MODEL_H_
src/user/user_objects.cc
+618 -314
View File
File diff suppressed because it is too large Load Diff
src/user/user_objects.h
+261 -213
View File
@@ -68,24 +68,6 @@ class mjXURDF; // defined in xml_urdf
const int mjGEOMINFO[mjNGEOMTYPES] = {3, 0, 1, 2, 3, 2, 3, 0};
// builtin type for procedural textures
typedef enum _mjtBuiltin {
mjBUILTIN_NONE = 0, // no builtin
mjBUILTIN_GRADIENT, // circular gradient: rgb1->rgb2->rgb3
mjBUILTIN_CHECKER, // checker pattern: rgb1, rgb2
mjBUILTIN_FLAT // 2d: rgb1; cube: rgb1-up, rgb2-side, rgb3-down
} mjtBuiltin;
// mark type for procedural textures
typedef enum _mjtMark {
mjMARK_NONE = 0, // no mark
mjMARK_EDGE, // paint edges
mjMARK_CROSS, // paint cross
mjMARK_RANDOM // paint random dots
} mjtMark;
// error information
class [[nodiscard]] mjCError {
public:
@@ -191,10 +173,6 @@ class mjCBase {
mjCModel* model; // pointer to model that created object
mjCFrame* frame; // pointer to frame transformation
// plugin support
mjmPlugin plugin;
std::string plugin_name;
std::string plugin_instance_name;
protected:
mjCBase(); // constructor
virtual ~mjCBase() = default; // destructor
@@ -251,7 +229,6 @@ class mjCBody : public mjCBase, private mjmBody {
using mjCBase::name;
using mjCBase::classname;
using mjCBase::info;
using mjCBase::plugin;
// used by mjXWriter and mjCModel
const std::vector<double>& get_userdata() { return userdata_; }
@@ -296,6 +273,8 @@ class mjCBody : public mjCBase, private mjmBody {
void PointToLocal(void);
// variable-size data
std::string plugin_name;
std::string plugin_instance_name;
std::vector<double> userdata_;
std::vector<double> spec_userdata_;
};
@@ -354,10 +333,10 @@ class mjCJoint : public mjCBase, private mjmJoint {
// used by mjXWriter and mjCModel
const std::vector<double>& get_userdata() { return userdata_; }
const double* get_range() { return range; }
// public getters
bool is_limited() const { return limited_; }
bool is_actfrclimited() const { return actfrclimited_; }
bool is_limited() const;
bool is_actfrclimited() const;
private:
@@ -367,8 +346,7 @@ class mjCJoint : public mjCBase, private mjmJoint {
void PointToLocal(void);
mjCBody* body; // joint's body
bool limited_; // actual (inferred) value of limited
bool actfrclimited_; // actual (inferred) value of actfrclimited
// variable-size data
std::vector<double> userdata_;
std::vector<double> spec_userdata_;
@@ -434,6 +412,8 @@ class mjCGeom : public mjCBase, private mjmGeom {
mjtNum fluid[mjNFLUID]; // compile-time fluid-interaction parameters
// variable-size data
std::string plugin_name;
std::string plugin_instance_name;
std::string hfieldname_;
std::string meshname_;
std::string material_;
@@ -446,7 +426,6 @@ class mjCGeom : public mjCBase, private mjmGeom {
// inherited
using mjCBase::classname;
using mjCBase::info;
using mjCBase::plugin;
};
@@ -554,6 +533,7 @@ class mjCLight : public mjCBase, private mjmLight {
mjCLight(mjCModel* = 0, mjCDef* = 0); // constructor
void Compile(void); // compiler
void CopyFromSpec(void);
void PointToLocal(void);
mjCBody* body; // light's body
int targetbodyid; // id of target body; -1: none
@@ -566,7 +546,7 @@ class mjCLight : public mjCBase, private mjmLight {
//------------------------- class mjCFlex ----------------------------------------------------------
// Describes a flex
class mjCFlex: public mjCBase {
class mjCFlex: public mjCBase, private mjmFlex {
friend class mjCDef;
friend class mjCModel;
friend class mjCFlexcomp;
@@ -574,41 +554,24 @@ class mjCFlex: public mjCBase {
friend class mjXWriter;
public:
void set_material(std::string _material) { material_ = _material; }
std::string& get_material() { return material_; }
mjmFlex spec;
using mjCBase::name;
using mjCBase::classname;
using mjCBase::info;
// contact properties
int contype; // contact type
int conaffinity; // contact affinity
int condim; // contact dimensionality
int priority; // contact priority
double friction[3]; // one-sided friction coefficients: slide, roll, spin
double solmix; // solver mixing for contact pairs
mjtNum solref[mjNREF]; // solver reference
mjtNum solimp[mjNIMP]; // solver impedance
double margin; // margin for contact detection
double gap; // include in solver if dist<margin-gap
void CopyFromSpec(void);
void PointToLocal(void);
// other properties
int dim; // element dimensionality
double radius; // radius around primitive element
bool internal; // enable internal collisions
bool flatskin; // render flex skin with flat shading
int selfcollide; // mode for flex self colllision
int activelayers; // number of active element layers in 3D
int group; // group for visualizatioh
double edgestiffness; // edge stiffness
double edgedamping; // edge damping
float rgba[4]; // rgba when material is omitted
// used by mjXWriter and mjCModel
const std::string& get_material() { return material_; }
const std::vector<std::string>& get_vertbody() { return vertbody_; }
const std::vector<double>& get_vert() { return vert_; }
const std::vector<double>& get_elemaabb() { return elemaabb_; }
const std::vector<int>& get_elem() { return elem_; }
const std::vector<float>& get_texcoord() { return texcoord_; }
std::vector<std::string> vertbody; // vertex body names
std::vector<mjtNum> vert; // vertex positions
std::vector<mjtNum> elemaabb; // element bounding volume
std::vector<int> elem; // element vertex ids
std::vector<float> texcoord; // vertex texture coordinates
bool HasTexcoord() const; // texcoord not null
void DelTexcoord(); // delete texcoord
bool HasTexcoord() const; // texcoord not null
void DelTexcoord(); // delete texcoord
private:
mjCFlex(mjCModel* = 0);
@@ -629,7 +592,20 @@ class mjCFlex: public mjCBase {
std::vector<int> evpair; // element-vertex pairs
std::vector<mjtNum> vertxpos; // global vertex positions
mjCBoundingVolumeHierarchy tree; // bounding volume hierarchy
std::vector<mjtNum> elemaabb_; // element bounding volume
// variable-size data
std::vector<std::string> vertbody_; // vertex body names
std::vector<mjtNum> vert_; // vertex positions
std::vector<int> elem_; // element vertex ids
std::vector<float> texcoord_; // vertex texture coordinates
std::string material_; // name of material used for rendering
std::string spec_material_;
std::vector<std::string> spec_vertbody_;
std::vector<mjtNum> spec_vert_;
std::vector<int> spec_elem_;
std::vector<float> spec_texcoord_;
};
@@ -637,26 +613,35 @@ class mjCFlex: public mjCBase {
//------------------------- class mjCMesh ----------------------------------------------------------
// Describes a mesh
class mjCMesh: public mjCBase {
class mjCMesh: public mjCBase, private mjmMesh {
friend class mjCFlexcomp;
friend class mjXWriter;
public:
mjCMesh(mjCModel* = 0, mjCDef* = 0);
~mjCMesh();
// public getters
const std::string& content_type() const { return content_type_; }
const std::string& file() const { return file_; }
mjmMesh spec;
using mjCBase::name;
using mjCBase::classname;
using mjCBase::info;
void CopyFromSpec(void);
void PointToLocal(void);
// public getters and setters
const std::string& get_content_type() const { return content_type_; }
const std::string& get_file() const { return file_; }
const double* refpos() const { return refpos_; }
const double* refquat() const { return refquat_; }
const double* scale() const { return scale_; }
bool smoothnormal() const { return smoothnormal_; }
const double* get_refpos() const { return refpos; }
const double* get_refquat() const { return refquat; }
const double* get_scale() const { return scale; }
bool get_smoothnormal() const { return smoothnormal; }
void set_needhull(bool needhull);
// public getters for user data
const std::vector<float>& uservert() const { return uservert_; }
const std::vector<float>& usernormal() const { return usernormal_; }
const std::vector<float>& usertexcoord() const { return usertexcoord_; }
const std::vector<int>& userface() const { return userface_; }
const std::vector<float>& get_uservert() const { return uservert_; }
const std::vector<float>& get_usernormal() const { return usernormal_; }
const std::vector<float>& get_usertexcoord() const { return usertexcoord_; }
const std::vector<int>& get_userface() const { return userface_; }
// mesh properties computed by Compile
const double* aamm() const { return aamm_; }
@@ -673,24 +658,6 @@ class mjCMesh: public mjCBase {
// bounding volume hierarchy tree
const mjCBoundingVolumeHierarchy& tree() { return tree_; }
// general setters
void set_file(const std::string& file);
void set_scale(std::array<double, 3> scale);
void set_smoothnormal(bool smoothnormal);
void set_needhull(bool needhull);
// setters used in reading XML attributes (no-op if empty optional)
void set_content_type(std::optional<std::string>&& content_type);
void set_file(std::optional<std::string>&& file);
void set_refpos(std::optional<std::array<double, 3>> refpos);
void set_refquat(std::optional<std::array<double, 4>> refquat);
void set_scale(std::optional<std::array<double, 3>> scale);
void set_uservert(std::optional<std::vector<float>>&& uservert);
void set_usernormal(std::optional<std::vector<float>>&& usernormal);
void set_usertexcoord(std::optional<std::vector<float>>&& usertexcoord);
void set_userface(std::optional<std::vector<int>>&& userface);
void Compile(const mjVFS* vfs); // compiler
double* GetPosPtr(mjtGeomInertia type); // get position
double* GetQuatPtr(mjtGeomInertia type); // get orientation
@@ -716,19 +683,28 @@ class mjCMesh: public mjCBase {
void SetBoundingVolume(int faceid);
private:
bool visual_; // true: the mesh is only visual
std::string content_type_; // content type of file
std::string file_; // mesh file
double refpos_[3]; // reference position (translate)
double refquat_[4]; // reference orientation (rotate)
double scale_[3]; // rescale mesh
bool smoothnormal_; // do not exclude large-angle faces from normals
// variable size attributes
std::string plugin_name;
std::string plugin_instance_name;
std::string content_type_; // content type of file
std::string file_; // mesh file
std::vector<float> uservert_; // user vertex data
std::vector<float> usernormal_; // user normal data
std::vector<float> usertexcoord_; // user texcoord data
std::vector<int> userface_; // user vertex indices
std::vector<int> userfacenormal_; // user normal indices
std::string spec_content_type_;
std::string spec_file_;
std::vector<float> spec_uservert_;
std::vector<float> spec_usernormal_;
std::vector<float> spec_usertexcoord_;
std::vector<int> spec_userface_;
std::vector<int> spec_userfacenormal_;
// used by the compiler
bool visual_; // true: the mesh is only visual
std::vector<int> userfacetexcoord_; // user texcoord indices
std::vector< std::pair<int, int> > useredge_; // user half-edge data
@@ -797,32 +773,30 @@ class mjCMesh: public mjCBase {
//------------------------- class mjCSkin ----------------------------------------------------------
// Describes a skin
class mjCSkin: public mjCBase {
class mjCSkin: public mjCBase, private mjmSkin {
friend class mjCModel;
friend class mjXWriter;
public:
std::string get_file() const { return file; }
void set_material(std::string _material) { material_ = _material; }
mjmSkin spec;
using mjCBase::name;
using mjCBase::classname;
using mjCBase::info;
std::string get_file() const { return file_; }
std::string& get_material() { return material_; }
std::vector<float>& get_vert() { return vert_; }
std::vector<float>& get_texcoord() { return texcoord_; }
std::vector<int>& get_face() { return face_; }
std::vector<std::string>& get_bodyname() { return bodyname_; }
std::vector<float>& get_bindpos() { return bindpos_; }
std::vector<float>& get_bindquat() { return bindquat_; }
std::vector<std::vector<int>>& get_vertid() { return vertid_; }
std::vector<std::vector<float>>& get_vertweight() { return vertweight_; }
void del_material() { material_.clear(); }
std::string file; // skin file
float rgba[4]; // rgba when material is omitted
float inflate; // inflate in normal direction
int group; // group for visualization
// mesh
std::vector<float> vert; // vertex positions
std::vector<float> texcoord; // texture coordinates
std::vector<int> face; // faces
// skin
std::vector<std::string> bodyname; // body names
std::vector<float> bindpos; // bind pos
std::vector<float> bindquat; // bind quat
std::vector<std::vector<int>> vertid; // vertex ids
std::vector<std::vector<float>> vertweight; // vertex weights
void CopyFromSpec();
void PointToLocal();
private:
mjCSkin(mjCModel* = 0); // constructor
@@ -830,7 +804,29 @@ class mjCSkin: public mjCBase {
void Compile(const mjVFS* vfs); // compiler
void LoadSKN(mjResource* resource); // load skin in SKN BIN format
std::string material_; // name of material used for rendering
// variable size attributes
std::string file_;
std::string material_;
std::vector<float> vert_;
std::vector<float> texcoord_;
std::vector<int> face_;
std::vector<std::string> bodyname_;
std::vector<float> bindpos_;
std::vector<float> bindquat_;
std::vector<std::vector<int>> vertid_;
std::vector<std::vector<float>> vertweight_;
std::string spec_file_;
std::string spec_material_;
std::vector<float> spec_vert_;
std::vector<float> spec_texcoord_;
std::vector<int> spec_face_;
std::vector<std::string> spec_bodyname_;
std::vector<float> spec_bindpos_;
std::vector<float> spec_bindquat_;
std::vector<std::vector<int>> spec_vertid_;
std::vector<std::vector<float>> spec_vertweight_;
int matid; // material id
std::vector<int> bodyid; // body ids
};
@@ -840,34 +836,39 @@ class mjCSkin: public mjCBase {
//------------------------- class mjCHField --------------------------------------------------------
// Describes a height field
class mjCHField : public mjCBase {
class mjCHField : public mjCBase, private mjmHField {
friend class mjCGeom;
friend class mjCModel;
friend class mjXWriter;
public:
std::string get_file() const { return file; }
mjmHField spec;
using mjCBase::name;
using mjCBase::info;
std::string content_type; // content type of file
std::string file; // file: (nrow, ncol, [elevation data])
double size[4]; // hfield size (ignore referencing geom size)
int nrow; // number of rows
int ncol; // number of columns
float* data; // elevation data, row-major format
void CopyFromSpec(void);
void PointToLocal(void);
std::string get_file() const { return file_; }
// getter for user data
std::vector<float>& userdata() { return userdata_; }
// setter for user data
void set_userdata(std::optional<std::vector<float>>&& userdata);
std::vector<float>& get_userdata() { return userdata_; }
private:
mjCHField(mjCModel* model); // constructor
~mjCHField(); // destructor
std::vector<float> userdata_; // user-provided elevation data
float* data; // elevation data, row-major format
void Compile(const mjVFS* vfs); // compiler
void LoadCustom(mjResource* resource); // load from custom format
void LoadPNG(mjResource* resource); // load from PNG format
std::string file_;
std::string content_type_;
std::vector<float> userdata_;
std::string spec_file_;
std::string spec_content_type_;
std::vector<float> spec_userdata_;
};
@@ -875,7 +876,7 @@ class mjCHField : public mjCBase {
//------------------------- class mjCTexture -------------------------------------------------------
// Describes a texture
class mjCTexture : public mjCBase {
class mjCTexture : public mjCBase, private mjmTexture {
friend class mjCModel;
friend class mjXReader;
friend class mjXWriter;
@@ -883,32 +884,17 @@ class mjCTexture : public mjCBase {
public:
~mjCTexture(); // destructor
std::string get_file() const { return file; }
mjmTexture spec;
using mjCBase::name;
using mjCBase::classname;
using mjCBase::info;
mjtTexture type; // texture type
void CopyFromSpec(void);
void PointToLocal(void);
// method 1: builtin
mjtBuiltin builtin; // builtin type
mjtMark mark; // mark type
double rgb1[3]; // first color for builtin
double rgb2[3]; // second color for builtin
double markrgb[3]; // mark color
double random; // probability of random dots
int height; // height in pixels (square for cube and skybox)
int width; // width in pixels
// method 2: single file
std::string content_type; // content type of file
std::string file; // png file to load; use for all sides of cube
int gridsize[2]; // size of grid for composite file; (1,1)-repeat
char gridlayout[13]; // row-major: L,R,F,B,U,D for faces; . for unused
// method 3: separate files
std::string cubefiles[6]; // different file for each side of the cube
// flip options
bool hflip; // horizontal flip
bool vflip; // vertical flip
std::string get_file() const { return file_; }
std::string get_content_type() const { return content_type_; }
std::vector<std::string> get_cubefiles() const { return cubefiles_; }
private:
mjCTexture(mjCModel*); // constructor
@@ -932,6 +918,13 @@ class mjCTexture : public mjCBase {
unsigned int& w, unsigned int& h);
mjtByte* rgb; // rgb data
std::string file_;
std::string content_type_;
std::vector<std::string> cubefiles_;
std::string spec_file_;
std::string spec_content_type_;
std::vector<std::string> spec_cubefiles_;
};
@@ -970,24 +963,23 @@ class mjCMaterial : public mjCBase, private mjmMaterial {
//------------------------- class mjCPair ----------------------------------------------------------
// Predefined geom pair for collision detection
class mjCPair : public mjCBase {
class mjCPair : public mjCBase, private mjmPair {
friend class mjCDef;
friend class mjCBody;
friend class mjCModel;
friend class mjXWriter;
public:
// parameters set by user
std::string geomname1; // name of geom 1
std::string geomname2; // name of geom 2
mjmPair spec;
using mjCBase::name;
using mjCBase::classname;
using mjCBase::info;
// optional parameters: computed from geoms if not set by user
int condim; // contact dimensionality
mjtNum solref[mjNREF]; // solver reference, normal direction
mjtNum solreffriction[mjNREF]; // solver reference, frictional directions
mjtNum solimp[mjNIMP]; // solver impedance
double margin; // margin for contact detection
double gap; // include in solver if dist<margin-gap
double friction[5]; // full contact friction
void CopyFromSpec();
void PointToLocal();
std::string get_geomname1() { return geomname1_; }
std::string get_geomname2() { return geomname2_; }
int GetSignature(void) {
return signature;
@@ -1000,6 +992,11 @@ class mjCPair : public mjCBase {
mjCGeom* geom1; // geom1
mjCGeom* geom2; // geom2
int signature; // body1<<16 + body2
std::string geomname1_;
std::string geomname2_;
std::string spec_geomname1_;
std::string spec_geomname2_;
};
@@ -1007,26 +1004,37 @@ class mjCPair : public mjCBase {
//------------------------- class mjCBodyPair ------------------------------------------------------
// Body pair specification, use to exclude pairs
class mjCBodyPair : public mjCBase {
class mjCBodyPair : public mjCBase, private mjmExclude {
friend class mjCBody;
friend class mjCModel;
public:
// parameters set by user
std::string bodyname1; // name of geom 1
std::string bodyname2; // name of geom 2
mjmExclude spec;
using mjCBase::name;
using mjCBase::info;
void CopyFromSpec();
void PointToLocal();
std::string get_bodyname1() const { return bodyname1_; }
std::string get_bodyname2() const { return bodyname2_; }
int GetSignature(void) {
return signature;
}
private:
mjCBodyPair(mjCModel*); // constructor
void Compile(void); // compiler
mjCBodyPair(mjCModel*); // constructor
void Compile(void); // compiler
int body1; // id of body1
int body2; // id of body2
int signature; // body1<<16 + body2
int body1; // id of body1
int body2; // id of body2
int signature; // body1<<16 + body2
std::string bodyname1_; // name of geom 1
std::string bodyname2_; // name of geom 2
std::string spec_bodyname1_;
std::string spec_bodyname2_;
};
@@ -1095,12 +1103,12 @@ class mjCTendon : public mjCBase, private mjmTendon {
// used by mjXWriter and mjCModel
const std::vector<double>& get_userdata() { return userdata_; }
const double* get_range() { return range; }
void CopyFromSpec();
void PointToLocal();
// public getters
bool is_limited() const { return limited_; }
bool is_limited() const;
private:
mjCTendon(mjCModel* = 0, mjCDef* = 0); // constructor
@@ -1108,7 +1116,6 @@ class mjCTendon : public mjCBase, private mjmTendon {
void Compile(void); // compiler
int matid; // material id for rendering
bool limited_; // actual (inferred) value of limited
// variable-size data
std::string material_;
@@ -1157,7 +1164,7 @@ class mjCPlugin : public mjCBase {
friend class mjXWriter;
public:
int plugin_slot; // global registered slot number of the plugin
mjmPlugin spec;
int nstate; // state size for the plugin instance
mjCBase* parent; // parent object (only used when generating error message)
std::map<std::string, std::string, std::less<>> config_attribs; // raw config attributes from XML
@@ -1166,6 +1173,7 @@ class mjCPlugin : public mjCBase {
private:
mjCPlugin(mjCModel*); // constructor
void Compile(void); // compiler
std::string instance_name;
};
@@ -1183,7 +1191,6 @@ class mjCActuator : public mjCBase, private mjmActuator {
using mjCBase::name;
using mjCBase::classname;
using mjCBase::info;
using mjCBase::plugin;
// used by mjXWriter and mjCModel
const std::vector<double>& get_userdata() { return userdata_; }
@@ -1191,23 +1198,21 @@ class mjCActuator : public mjCBase, private mjmActuator {
const std::string& get_slidersite() { return spec_slidersite_; }
const std::string& get_refsite() { return spec_refsite_; }
// public getters
bool is_ctrllimited() const { return ctrllimited_; }
bool is_forcelimited() const { return forcelimited_; }
bool is_actlimited() const { return actlimited_; }
bool is_ctrllimited() const;
bool is_forcelimited() const;
bool is_actlimited() const;
private:
mjCActuator(mjCModel* = 0, mjCDef* = 0); // constructor
void Compile(void); // compiler
void CopyFromSpec();
void MakePointerLocal();
void PointToLocal();
int trnid[2]; // id of transmission target
bool ctrllimited_; // actual (inferred) value of ctrllimited
bool forcelimited_; // actual (inferred) value of forcelimited
bool actlimited_; // actual (inferred) value of actlimited
// variable-size data
std::string plugin_name;
std::string plugin_instance_name;
std::string target_;
std::string slidersite_;
std::string refsite_;
@@ -1233,7 +1238,6 @@ class mjCSensor : public mjCBase, private mjmSensor {
using mjCBase::name;
using mjCBase::classname;
using mjCBase::info;
using mjCBase::plugin;
// used by mjXWriter and mjCModel
const std::vector<double>& get_userdata() { return userdata_; }
@@ -1250,6 +1254,8 @@ class mjCSensor : public mjCBase, private mjmSensor {
int refid; // id of reference frame
// variable-size data
std::string plugin_name;
std::string plugin_instance_name;
std::string objname_;
std::string refname_;
std::vector<double> userdata_;
@@ -1263,18 +1269,25 @@ class mjCSensor : public mjCBase, private mjmSensor {
//------------------------- class mjCNumeric -------------------------------------------------------
// Describes a custom data field
class mjCNumeric : public mjCBase {
class mjCNumeric : public mjCBase, private mjmNumeric {
friend class mjCModel;
friend class mjXWriter;
public:
// variables set by user
std::vector<double> data; // initialization data
int size; // array size, can be bigger than data.size()
mjmNumeric spec;
using mjCBase::name;
using mjCBase::info;
void PointToLocal();
void CopyFromSpec();
private:
mjCNumeric(mjCModel*); // constructor
~mjCNumeric(); // destructor
void Compile(void); // compiler
std::vector<double> data_;
std::vector<double> spec_data_;
};
@@ -1282,17 +1295,25 @@ class mjCNumeric : public mjCBase {
//------------------------- class mjCText ----------------------------------------------------------
// Describes a custom text field
class mjCText : public mjCBase {
class mjCText : public mjCBase, private mjmText {
friend class mjCModel;
friend class mjXWriter;
public:
// variables set by user
std::string data; // string
mjmText spec;
using mjCBase::name;
using mjCBase::info;
void PointToLocal();
void CopyFromSpec();
private:
mjCText(mjCModel*); // constructor
~mjCText(); // destructor
void Compile(void); // compiler
std::string data_;
std::string spec_data_;
};
@@ -1300,14 +1321,17 @@ class mjCText : public mjCBase {
//------------------------- class mjCTuple ---------------------------------------------------------
// Describes a custom tuple field
class mjCTuple : public mjCBase {
class mjCTuple : public mjCBase, private mjmTuple {
friend class mjCModel;
friend class mjXWriter;
public:
// variables set by user
std::vector<mjtObj> objtype; // object types
std::vector<std::string> objname; // object names
std::vector<double> objprm; // object parameters
mjmTuple spec;
using mjCBase::name;
using mjCBase::info;
void PointToLocal();
void CopyFromSpec();
private:
mjCTuple(mjCModel*); // constructor
@@ -1315,6 +1339,14 @@ class mjCTuple : public mjCBase {
void Compile(void); // compiler
std::vector<mjCBase*> obj; // object pointers
// variable-size data
std::vector<mjtObj> objtype_;
std::vector<std::string> objname_;
std::vector<double> objprm_;
std::vector<mjtObj> spec_objtype_;
std::vector<std::string> spec_objname_;
std::vector<double> spec_objprm_;
};
@@ -1322,23 +1354,35 @@ class mjCTuple : public mjCBase {
//------------------------- class mjCKey -----------------------------------------------------------
// Describes a keyframe
class mjCKey : public mjCBase {
class mjCKey : public mjCBase, private mjmKey {
friend class mjCModel;
friend class mjXWriter;
public:
double time; // time
std::vector<double> qpos; // qpos
std::vector<double> qvel; // qvel
std::vector<double> act; // act
std::vector<double> mpos; // mocap pos
std::vector<double> mquat; // mocap quat
std::vector<double> ctrl; // ctrl
mjmKey spec;
using mjCBase::name;
using mjCBase::info;
void PointToLocal();
void CopyFromSpec();
private:
mjCKey(mjCModel*); // constructor
~mjCKey(); // destructor
void Compile(const mjModel* m); // compiler
std::vector<double> qpos_;
std::vector<double> qvel_;
std::vector<double> act_;
std::vector<double> mpos_;
std::vector<double> mquat_;
std::vector<double> ctrl_;
std::vector<double> spec_qpos_;
std::vector<double> spec_qvel_;
std::vector<double> spec_act_;
std::vector<double> spec_mpos_;
std::vector<double> spec_mquat_;
std::vector<double> spec_ctrl_;
};
@@ -1347,6 +1391,8 @@ class mjCKey : public mjCBase {
// Describes one set of defaults
class mjCDef {
friend class mjXWriter;
public:
mjCDef(void); // constructor
void Compile(const mjCModel* model); // compiler
@@ -1357,7 +1403,9 @@ class mjCDef {
int parentid; // id of parent class
std::vector<int> childid; // ids of child classes
// default objects
mjmDefault spec;
// default objects (TODO: they should become private)
mjCJoint joint;
mjCGeom geom;
mjCSite site;
src/xml/xml.cc
+89 -28
View File
@@ -20,8 +20,10 @@
#include <xlocale.h>
#endif
#include <array>
#include <cstdio>
#include <string>
#include <string_view>
#include <unordered_set>
#include "tinyxml2.h"
@@ -33,6 +35,7 @@
#include "engine/engine_resource.h"
#include "engine/engine_vfs.h"
#include "user/user_model.h"
#include "user/user_util.h"
#include "xml/xml_native_reader.h"
#include "xml/xml_native_writer.h"
#include "xml/xml_urdf.h"
@@ -40,7 +43,6 @@
namespace {
using std::string;
using tinyxml2::XMLDocument;
using tinyxml2::XMLElement;
using tinyxml2::XMLNode;
@@ -96,7 +98,7 @@ class LocaleOverride {
} // namespace
// Main writer function - calls mjXWrite
string mjWriteXML(mjCModel* model, char* error, int error_sz) {
std::string mjWriteXML(mjCModel* model, char* error, int error_sz) {
LocaleOverride locale_override;
// check for empty model
@@ -113,13 +115,32 @@ string mjWriteXML(mjCModel* model, char* error, int error_sz) {
// find include elements recursively, replace them with subtree from xml file
static void mjIncludeXML(XMLElement* elem, string dir, const mjVFS* vfs,
std::unordered_set<string>& included) {
static void mjIncludeXML(mjXReader& reader, XMLElement* elem,
std::string_view dir, const mjVFS* vfs,
std::unordered_set<std::string>& included) {
// capture directory defaults on first pass of XML tree
if (!strcasecmp(elem->Value(), "compiler")) {
auto assetdir_attr = mjXUtil::ReadAttrStr(elem, "assetdir");
if (assetdir_attr.has_value()) {
reader.SetAssetDir(assetdir_attr.value());
}
auto texturedir_attr = mjXUtil::ReadAttrStr(elem, "texturedir");
if (texturedir_attr.has_value()) {
reader.SetTextureDir(texturedir_attr.value());
}
auto meshdir_attr = mjXUtil::ReadAttrStr(elem, "meshdir");
if (meshdir_attr.has_value()) {
reader.SetMeshDir(meshdir_attr.value());
}
}
// not an include, recursively go through all children
if (strcasecmp(elem->Value(), "include")) {
XMLElement* child = elem->FirstChildElement();
for (; child; child = child->NextSiblingElement()) {
mjIncludeXML(child, dir, vfs, included);
mjIncludeXML(reader, child, dir, vfs, included);
}
return;
}
@@ -130,24 +151,60 @@ static void mjIncludeXML(XMLElement* elem, string dir, const mjVFS* vfs,
}
// get filename
string filename;
mjXUtil::ReadAttrTxt(elem, "file", filename, true);
filename = dir + filename;
auto file_attr = mjXUtil::ReadAttrStr(elem, "file", true);
if (!file_attr.has_value()) {
throw mjXError(elem, "Include element missing file attribute");
}
std::string filename = file_attr.value();
// block repeated include files
if (included.find(filename) != included.end()) {
throw mjXError(elem, "File '%s' already included", filename.c_str());
}
// get data source
mjResource *resource = nullptr;
if ((resource = mju_openVfsResource(filename.c_str(), vfs)) == nullptr) {
// TODO: b/325905702 - We have a messy wrapper here to remain backwards
// compatible, which will be removed in the near future.
std::string fullname;
if (!mjuu_isabspath(filename)) {
fullname = reader.ModelFileDir() + filename;
} else {
fullname = filename;
}
mjResource *resource = mju_openVfsResource(fullname.c_str(), vfs);
if (!resource) {
// load from provider or OS filesystem
if ((resource = mju_openResource(filename.c_str())) == nullptr) {
throw mjXError(elem, "Could not open file '%s'", filename.c_str());
std::array<char, 1024> error;
resource = mju_openResource(fullname.c_str(), error.data(), error.size());
if (!resource) {
if (!mjuu_isabspath(filename)) {
fullname = std::string(dir) + filename;
} else {
fullname = filename;
}
// load from provider or OS filesystem
std::array<char, 1024> error;
resource = mju_openResource(fullname.c_str(), error.data(), error.size());
if (!resource) {
throw mjXError(elem, "%s", error.data());
}
}
}
if (!mjuu_isabspath(filename)) {
filename = std::string(dir) + filename;
}
const char* include_dir = nullptr;
int ninclude_dir = 0;
mju_getResourceDir(resource, &include_dir, &ninclude_dir);
std::string next_dir = std::string(include_dir, ninclude_dir);
if (!mjuu_isabspath(filename)) {
next_dir = std::string(dir) + next_dir;
}
elem->SetAttribute("dir", next_dir.data());
const char* xmlstring = nullptr;
int buffer_size = mju_readResource(resource, (const void**) &xmlstring);
if (buffer_size < 0) {
@@ -207,14 +264,15 @@ static void mjIncludeXML(XMLElement* elem, string dir, const mjVFS* vfs,
// recursively run include
child = include->FirstChildElement();
for (; child; child = child->NextSiblingElement()) {
mjIncludeXML(child, dir, vfs, included);
mjIncludeXML(reader, child, next_dir, vfs, included);
}
}
// Main parser function
mjCModel* mjParseXML(const char* filename, const mjVFS* vfs, char* error, int error_sz) {
mjCModel* mjParseXML(const char* filename, const mjVFS* vfs,
char* error, int error_sz) {
LocaleOverride locale_override;
// check arguments
@@ -232,14 +290,15 @@ mjCModel* mjParseXML(const char* filename, const mjVFS* vfs, char* error, int er
}
// get data source
mjResource* resource = nullptr;
const char* xmlstring = nullptr;
if ((resource = mju_openVfsResource(filename, vfs)) == nullptr) {
mjResource* resource = mju_openVfsResource(filename, vfs);
if (!resource) {
// load from provider or fallback to OS filesystem
if ((resource = mju_openResource(filename)) == nullptr) {
if (error) {
std::snprintf(error, error_sz, "mjParseXML: could not open file '%s'", filename);
}
std::array<char, 1024> rerror;
resource = mju_openResource(filename, rerror.data(), rerror.size());
if (!resource) {
std::snprintf(error, error_sz, "mjParseXML: %s", rerror.data());
return nullptr;
}
}
@@ -247,7 +306,8 @@ mjCModel* mjParseXML(const char* filename, const mjVFS* vfs, char* error, int er
int buffer_size = mju_readResource(resource, (const void**) &xmlstring);
if (buffer_size < 0) {
if (error) {
std::snprintf(error, error_sz, "mjParseXML: error reading file '%s'", filename);
std::snprintf(error, error_sz,
"mjParseXML: error reading file '%s'", filename);
}
mju_closeResource(resource);
return nullptr;
@@ -300,11 +360,12 @@ mjCModel* mjParseXML(const char* filename, const mjVFS* vfs, char* error, int er
try {
if (!strcasecmp(root->Value(), "mujoco")) {
// find include elements, replace them with subtree from xml file
std::unordered_set<string> included = {filename};
mjIncludeXML(root, model->modelfiledir, vfs, included);
std::unordered_set<std::string> included = {filename};
mjXReader parser;
parser.SetModelFileDir(model->modelfiledir);
mjIncludeXML(parser, root, model->modelfiledir, vfs, included);
// parse MuJoCo model
mjXReader parser;
parser.SetModel(model);
parser.Parse(root);
}
@@ -315,9 +376,9 @@ mjCModel* mjParseXML(const char* filename, const mjVFS* vfs, char* error, int er
// set reasonable default for parsing a URDF
// this is separate from the Parser to allow multiple URDFs to be loaded.
model->strippath = true;
model->fusestatic = true;
model->discardvisual = true;
model->spec.strippath = true;
model->spec.fusestatic = true;
model->spec.discardvisual = true;
parser.SetModel(model);
parser.Parse(root);
src/xml/xml_native_reader.cc
+510 -320
View File
File diff suppressed because it is too large Load Diff
src/xml/xml_native_reader.h
+38 -13
View File
@@ -16,11 +16,15 @@
#define MUJOCO_SRC_XML_XML_NATIVE_READER_H_
#include <sstream>
#include <string>
#include "user/user_model.h"
#include "xml/xml_base.h"
#include "tinyxml2.h"
#include <mujoco/mujoco.h>
#include "user/user_api.h"
#include "xml/xml_base.h"
#include "xml/xml_util.h"
class mjXReader : public mjXBase {
public:
mjXReader(); // constructor
@@ -29,10 +33,18 @@ class mjXReader : public mjXBase {
void Parse(tinyxml2::XMLElement* root); // parse XML document
void PrintSchema(std::stringstream& str, bool html, bool pad); // print text or HTML schema
void SetModelFileDir(std::string modelfiledir);
const std::string& ModelFileDir() const { return modelfiledir_; }
// setters for directory defaults
void SetAssetDir(std::string assetdir);
void SetMeshDir(std::string meshdir);
void SetTextureDir(std::string texturedir);
// XML sections embedded in all formats
static void Compiler(tinyxml2::XMLElement* section, mjCModel* mod); // compiler section
static void Compiler(tinyxml2::XMLElement* section, mjmModel* mod); // compiler section
static void Option(tinyxml2::XMLElement* section, mjOption* opt); // option section
static void Size(tinyxml2::XMLElement* section, mjCModel* mod); // size section
static void Size(tinyxml2::XMLElement* section, mjmModel* mod); // size section
private:
// XML section specific to MJCF
@@ -53,28 +65,41 @@ class mjXReader : public mjXBase {
void Keyframe(tinyxml2::XMLElement* section); // keyframe section
// single element parsers, used in defaults and main body
void OneFlex(tinyxml2::XMLElement* elem, mjCFlex* pflex);
void OneMesh(tinyxml2::XMLElement* elem, mjCMesh* pmesh);
void OneSkin(tinyxml2::XMLElement* elem, mjCSkin* pskin);
void OneFlex(tinyxml2::XMLElement* elem, mjmFlex* pflex);
void OneMesh(tinyxml2::XMLElement* elem, mjmMesh* pmesh);
void OneSkin(tinyxml2::XMLElement* elem, mjmSkin* pskin);
void OneMaterial(tinyxml2::XMLElement* elem, mjmMaterial* pmaterial);
void OneJoint(tinyxml2::XMLElement* elem, mjmJoint* pjoint);
void OneGeom(tinyxml2::XMLElement* elem, mjmGeom* pgeom);
void OneSite(tinyxml2::XMLElement* elem, mjmSite& site);
void OneSite(tinyxml2::XMLElement* elem, mjmSite* site);
void OneCamera(tinyxml2::XMLElement* elem, mjmCamera* pcamera);
void OneLight(tinyxml2::XMLElement* elem, mjmLight* plight);
void OnePair(tinyxml2::XMLElement* elem, mjCPair* ppair);
void OnePair(tinyxml2::XMLElement* elem, mjmPair* ppair);
void OneEquality(tinyxml2::XMLElement* elem, mjmEquality* pequality);
void OneTendon(tinyxml2::XMLElement* elem, mjmTendon* ptendon);
void OneActuator(tinyxml2::XMLElement* elem, mjmActuator* pactuator);
void OneComposite(tinyxml2::XMLElement* elem, mjmBody* pbody, mjCDef* def);
void OneComposite(tinyxml2::XMLElement* elem, mjmBody* pbody, mjmDefault* def);
void OneFlexcomp(tinyxml2::XMLElement* elem, mjmBody* pbody);
void OnePlugin(tinyxml2::XMLElement* elem, mjmPlugin* plugin);
mjXSchema schema; // schema used for validation
mjCDef* GetClass(tinyxml2::XMLElement* section); // get default class name
static void GetXMLPos(tinyxml2::XMLElement* elem, mjCBase* obj); // get xml position
mjmDefault* GetClass(tinyxml2::XMLElement* section); // get default class name
bool readingdefaults; // true while reading defaults
bool readingdefaults; // true while reading defaults
// accessors for directory defaults
std::string AssetDir() const;
std::string MeshDir() const;
std::string TextureDir() const;
std::string modelfiledir_;
std::string assetdir_;
std::string meshdir_;
std::string texturedir_;
};
// MJCF schema
#define nMJCF 227
extern const char* MJCF[nMJCF][mjXATTRNUM];
#endif // MUJOCO_SRC_XML_XML_NATIVE_READER_H_
src/xml/xml_native_writer.cc
+127 -120
View File
@@ -100,20 +100,20 @@ void mjXWriter::OneFlex(XMLElement* elem, mjCFlex* pflex) {
WriteAttrInt(elem, "group", pflex->group, defflex.group);
// data vectors
if (!pflex->vertbody.empty()) {
Vector2String(text, pflex->vertbody);
if (!pflex->get_vertbody().empty()) {
Vector2String(text, pflex->get_vertbody());
WriteAttrTxt(elem, "body", text);
}
if (!pflex->vert.empty()) {
Vector2String(text, pflex->vert);
if (!pflex->get_vert().empty()) {
Vector2String(text, pflex->get_vert());
WriteAttrTxt(elem, "vertex", text);
}
if (!pflex->elem.empty()) {
Vector2String(text, pflex->elem);
if (!pflex->get_elem().empty()) {
Vector2String(text, pflex->get_elem());
WriteAttrTxt(elem, "element", text);
}
if (!pflex->texcoord.empty()) {
Vector2String(text, pflex->texcoord);
if (!pflex->get_texcoord().empty()) {
Vector2String(text, pflex->get_texcoord());
WriteAttrTxt(elem, "texcoord", text);
}
@@ -159,39 +159,40 @@ void mjXWriter::OneMesh(XMLElement* elem, mjCMesh* pmesh, mjCDef* def) {
if (!writingdefaults) {
WriteAttrTxt(elem, "name", pmesh->name);
WriteAttrTxt(elem, "class", pmesh->classname);
WriteAttrTxt(elem, "content_type", pmesh->content_type());
WriteAttrTxt(elem, "file", pmesh->file());
WriteAttrTxt(elem, "content_type", pmesh->get_content_type());
WriteAttrTxt(elem, "file", pmesh->get_file());
// write vertex data
if (!pmesh->uservert().empty()) {
Vector2String(text, pmesh->uservert());
if (!pmesh->get_uservert().empty()) {
Vector2String(text, pmesh->get_uservert());
WriteAttrTxt(elem, "vertex", text);
}
// write normal data
if (!pmesh->usernormal().empty()) {
Vector2String(text, pmesh->usernormal());
if (!pmesh->get_usernormal().empty()) {
Vector2String(text, pmesh->get_usernormal());
WriteAttrTxt(elem, "normal", text);
}
// write texcoord data
if (!pmesh->usertexcoord().empty()) {
Vector2String(text, pmesh->usertexcoord());
if (!pmesh->get_usertexcoord().empty()) {
Vector2String(text, pmesh->get_usertexcoord());
WriteAttrTxt(elem, "texcoord", text);
}
// write face data
if (!pmesh->userface().empty()) {
Vector2String(text, pmesh->userface());
if (!pmesh->get_userface().empty()) {
Vector2String(text, pmesh->get_userface());
WriteAttrTxt(elem, "face", text);
}
}
// defaults and regular
WriteAttr(elem, "refpos", 3, pmesh->refpos(), def->mesh.refpos());
WriteAttr(elem, "refquat", 4, pmesh->refquat(), def->mesh.refquat());
WriteAttr(elem, "scale", 3, pmesh->scale(), def->mesh.scale());
WriteAttrKey(elem, "smoothnormal", bool_map, 2, pmesh->smoothnormal(), def->mesh.smoothnormal());
WriteAttr(elem, "refpos", 3, pmesh->refpos, def->mesh.refpos);
WriteAttr(elem, "refquat", 4, pmesh->refquat, def->mesh.refquat);
WriteAttr(elem, "scale", 3, pmesh->scale, def->mesh.scale);
WriteAttrKey(elem, "smoothnormal", bool_map, 2, pmesh->get_smoothnormal(),
def->mesh.get_smoothnormal());
}
@@ -204,44 +205,44 @@ void mjXWriter::OneSkin(XMLElement* elem, mjCSkin* pskin) {
// write attributes
WriteAttrTxt(elem, "name", pskin->name);
WriteAttrTxt(elem, "file", pskin->file);
WriteAttrTxt(elem, "file", pskin->get_file());
WriteAttrTxt(elem, "material", pskin->get_material());
WriteAttrInt(elem, "group", pskin->group, 0);
WriteAttr(elem, "rgba", 4, pskin->rgba, mydef.geom.rgba);
WriteAttr(elem, "inflate", 1, &pskin->inflate, &zero);
// write data if no file
if (pskin->file.empty()) {
if (pskin->get_file().empty()) {
// mesh vert
Vector2String(text, pskin->vert);
Vector2String(text, pskin->get_vert());
WriteAttrTxt(elem, "vertex", text);
// mesh texcoord
if (!pskin->texcoord.empty()) {
Vector2String(text, pskin->texcoord);
if (!pskin->get_texcoord().empty()) {
Vector2String(text, pskin->get_texcoord());
WriteAttrTxt(elem, "texcoord", text);
}
// mesh face
Vector2String(text, pskin->face);
Vector2String(text, pskin->get_face());
WriteAttrTxt(elem, "face", text);
// bones
for (size_t i=0; i<pskin->bodyname.size(); i++) {
for (size_t i=0; i<pskin->get_bodyname().size(); i++) {
// make bone
XMLElement* bone = InsertEnd(elem, "bone");
// write attributes
WriteAttrTxt(bone, "body", pskin->bodyname[i]);
WriteAttr(bone, "bindpos", 3, pskin->bindpos.data()+3*i);
WriteAttr(bone, "bindquat", 4, pskin->bindquat.data()+4*i);
WriteAttrTxt(bone, "body", pskin->get_bodyname()[i]);
WriteAttr(bone, "bindpos", 3, pskin->get_bindpos().data()+3*i);
WriteAttr(bone, "bindquat", 4, pskin->get_bindquat().data()+4*i);
// write vertid
Vector2String(text, pskin->vertid[i]);
Vector2String(text, pskin->get_vertid()[i]);
WriteAttrTxt(bone, "vertid", text);
// write vertweight
Vector2String(text, pskin->vertweight[i]);
Vector2String(text, pskin->get_vertweight()[i]);
WriteAttrTxt(bone, "vertweight", text);
}
}
@@ -295,10 +296,12 @@ void mjXWriter::OneJoint(XMLElement* elem, mjCJoint* pjoint, mjCDef* def) {
WriteAttrInt(elem, "group", pjoint->group, def->joint.group);
WriteAttr(elem, "ref", 1, &pjoint->ref, &zero);
WriteAttr(elem, "springref", 1, &pjoint->springref, &zero);
WriteAttr(elem, "solreflimit", mjNREF, pjoint->solref_limit, def->joint.solref_limit);
WriteAttr(elem, "solimplimit", mjNIMP, pjoint->solimp_limit, def->joint.solimp_limit);
WriteAttr(elem, "solreffriction", mjNREF, pjoint->solref_friction, def->joint.solref_friction);
WriteAttr(elem, "solimpfriction", mjNIMP, pjoint->solimp_friction, def->joint.solimp_friction);
WriteAttr(elem, "solreflimit", mjNREF, pjoint->solref_limit, def->joint.solref_limit, true);
WriteAttr(elem, "solimplimit", mjNIMP, pjoint->solimp_limit, def->joint.solimp_limit, true);
WriteAttr(elem, "solreffriction", mjNREF, pjoint->solref_friction, def->joint.solref_friction,
true);
WriteAttr(elem, "solimpfriction", mjNIMP, pjoint->solimp_friction, def->joint.solimp_friction,
true);
WriteAttr(elem, "stiffness", 1, &pjoint->stiffness, &def->joint.stiffness);
WriteAttrKey(elem, "limited", TFAuto_map, 3, pjoint->limited, def->joint.limited);
WriteAttr(elem, "range", 2, pjoint->range, def->joint.range);
@@ -372,10 +375,10 @@ void mjXWriter::OneGeom(XMLElement* elem, mjCGeom* pgeom, mjCDef* def) {
WriteAttrInt(elem, "condim", pgeom->condim, def->geom.condim);
WriteAttrInt(elem, "group", pgeom->group, def->geom.group);
WriteAttrInt(elem, "priority", pgeom->priority, def->geom.priority);
WriteAttr(elem, "friction", 3, pgeom->friction, def->geom.friction);
WriteAttr(elem, "friction", 3, pgeom->friction, def->geom.friction, true);
WriteAttr(elem, "solmix", 1, &pgeom->solmix, &def->geom.solmix);
WriteAttr(elem, "solref", mjNREF, pgeom->solref, def->geom.solref);
WriteAttr(elem, "solimp", mjNIMP, pgeom->solimp, def->geom.solimp);
WriteAttr(elem, "solref", mjNREF, pgeom->solref, def->geom.solref, true);
WriteAttr(elem, "solimp", mjNIMP, pgeom->solimp, def->geom.solimp, true);
WriteAttr(elem, "margin", 1, &pgeom->margin, &def->geom.margin);
WriteAttr(elem, "gap", 1, &pgeom->gap, &def->geom.gap);
WriteAttr(elem, "gap", 1, &pgeom->gap, &def->geom.gap);
@@ -409,7 +412,7 @@ void mjXWriter::OneGeom(XMLElement* elem, mjCGeom* pgeom, mjCDef* def) {
// write plugin
if (pgeom->plugin.active) {
OnePlugin(InsertEnd(elem, "plugin"), pgeom);
OnePlugin(InsertEnd(elem, "plugin"), &pgeom->plugin);
}
}
@@ -523,19 +526,20 @@ void mjXWriter::OnePair(XMLElement* elem, mjCPair* ppair, mjCDef* def) {
// regular
if (!writingdefaults) {
WriteAttrTxt(elem, "class", ppair->classname);
WriteAttrTxt(elem, "geom1", ppair->geomname1);
WriteAttrTxt(elem, "geom2", ppair->geomname2);
WriteAttrTxt(elem, "geom1", ppair->get_geomname1());
WriteAttrTxt(elem, "geom2", ppair->get_geomname2());
}
// defaults and regular
WriteAttrTxt(elem, "name", ppair->name);
WriteAttrInt(elem, "condim", ppair->condim, def->pair.condim);
WriteAttr(elem, "margin", 1, &ppair->margin, &def->pair.margin);
WriteAttr(elem, "gap", 1, &ppair->gap, &def->pair.gap);
WriteAttr(elem, "solref", mjNREF, ppair->solref, def->pair.solref);
WriteAttr(elem, "solreffriction", mjNREF, ppair->solreffriction, def->pair.solreffriction);
WriteAttr(elem, "solimp", mjNIMP, ppair->solimp, def->pair.solimp);
WriteAttr(elem, "friction", 5, ppair->friction, def->pair.friction);
WriteAttrInt(elem, "condim", ppair->condim, def->pair.spec.condim);
WriteAttr(elem, "margin", 1, &ppair->margin, &def->pair.spec.margin);
WriteAttr(elem, "gap", 1, &ppair->gap, &def->pair.spec.gap);
WriteAttr(elem, "solref", mjNREF, ppair->solref, def->pair.spec.solref, true);
WriteAttr(elem, "solreffriction", mjNREF, ppair->solreffriction, def->pair.spec.solreffriction,
true);
WriteAttr(elem, "solimp", mjNIMP, ppair->solimp, def->pair.spec.solimp, true);
WriteAttr(elem, "friction", 5, ppair->friction, def->pair.spec.friction); // all 5 values
}
@@ -585,8 +589,8 @@ void mjXWriter::OneEquality(XMLElement* elem, mjCEquality* peq, mjCDef* def) {
// defaults and regular
WriteAttrKey(elem, "active", bool_map, 2, peq->active, def->equality.active);
WriteAttr(elem, "solref", mjNREF, peq->solref, def->equality.solref);
WriteAttr(elem, "solimp", mjNIMP, peq->solimp, def->equality.solimp);
WriteAttr(elem, "solref", mjNREF, peq->solref, def->equality.solref, true);
WriteAttr(elem, "solimp", mjNIMP, peq->solimp, def->equality.solimp, true);
}
@@ -603,10 +607,12 @@ void mjXWriter::OneTendon(XMLElement* elem, mjCTendon* pten, mjCDef* def) {
// defaults and regular
WriteAttrInt(elem, "group", pten->group, def->tendon.group);
WriteAttr(elem, "solreflimit", mjNREF, pten->solref_limit, def->tendon.solref_limit);
WriteAttr(elem, "solimplimit", mjNIMP, pten->solimp_limit, def->tendon.solimp_limit);
WriteAttr(elem, "solreffriction", mjNREF, pten->solref_friction, def->tendon.solref_friction);
WriteAttr(elem, "solimpfriction", mjNIMP, pten->solimp_friction, def->tendon.solimp_friction);
WriteAttr(elem, "solreflimit", mjNREF, pten->solref_limit, def->tendon.solref_limit, true);
WriteAttr(elem, "solimplimit", mjNIMP, pten->solimp_limit, def->tendon.solimp_limit, true);
WriteAttr(elem, "solreffriction", mjNREF, pten->solref_friction, def->tendon.solref_friction,
true);
WriteAttr(elem, "solimpfriction", mjNIMP, pten->solimp_friction, def->tendon.solimp_friction,
true);
WriteAttrKey(elem, "limited", TFAuto_map, 3, pten->limited, def->tendon.limited);
WriteAttr(elem, "range", 2, pten->range, def->tendon.range);
WriteAttr(elem, "margin", 1, &pten->margin, &def->tendon.margin);
@@ -696,7 +702,7 @@ void mjXWriter::OneActuator(XMLElement* elem, mjCActuator* pact, mjCDef* def) {
// plugins: write config attributes
if (pact->plugin.active) {
OnePlugin(elem, pact);
OnePlugin(elem, &pact->plugin);
}
// non-plugins: write actuator parameters
@@ -710,8 +716,8 @@ void mjXWriter::OneActuator(XMLElement* elem, mjCActuator* pact, mjCDef* def) {
}
WriteAttrKey(elem, "gaintype", gain_map, gain_sz, pact->gaintype, def->actuator.gaintype);
WriteAttrKey(elem, "biastype", bias_map, bias_sz, pact->biastype, def->actuator.biastype);
WriteAttr(elem, "gainprm", mjNGAIN, pact->gainprm, def->actuator.gainprm);
WriteAttr(elem, "biasprm", mjNBIAS, pact->biasprm, def->actuator.biasprm);
WriteAttr(elem, "gainprm", mjNGAIN, pact->gainprm, def->actuator.gainprm, true);
WriteAttr(elem, "biasprm", mjNBIAS, pact->biasprm, def->actuator.biasprm, true);
}
// userdata
@@ -725,21 +731,21 @@ void mjXWriter::OneActuator(XMLElement* elem, mjCActuator* pact, mjCDef* def) {
// write plugin
void mjXWriter::OnePlugin(XMLElement* elem, mjCBase* object) {
const std::string instance_name = std::string(mjm_getString(object->plugin.instance_name));
const std::string plugin_name = std::string(mjm_getString(object->plugin.name));
void mjXWriter::OnePlugin(XMLElement* elem, mjmPlugin* plugin) {
const std::string instance_name = std::string(mjm_getString(plugin->instance_name));
const std::string plugin_name = std::string(mjm_getString(plugin->name));
if (!instance_name.empty()) {
WriteAttrTxt(elem, "instance", instance_name);
} else {
WriteAttrTxt(elem, "plugin", plugin_name);
const mjpPlugin* plugin = mjp_getPluginAtSlot(
((mjCPlugin*)object->plugin.instance)->plugin_slot);
const char* c = &((mjCPlugin*)object->plugin.instance)->flattened_attributes[0];
for (int i = 0; i < plugin->nattribute; ++i) {
const mjpPlugin* pplugin = mjp_getPluginAtSlot(
((mjCPlugin*)plugin->instance)->spec.plugin_slot);
const char* c = &((mjCPlugin*)plugin->instance)->flattened_attributes[0];
for (int i = 0; i < pplugin->nattribute; ++i) {
std::string value(c);
if (!value.empty()) {
XMLElement* config_elem = InsertEnd(elem, "config");
WriteAttrTxt(config_elem, "key", plugin->attributes[i]);
WriteAttrTxt(config_elem, "key", pplugin->attributes[i]);
WriteAttrTxt(config_elem, "value", value);
c += value.size();
}
@@ -769,7 +775,7 @@ string mjXWriter::Write(char *error, size_t error_sz) {
// create document and root
XMLDocument doc;
XMLElement* root = doc.NewElement("mujoco");
root->SetAttribute("model", model->modelname.c_str());
root->SetAttribute("model", mjm_getString(model->modelname));
// insert root
doc.InsertFirstChild(root);
@@ -815,11 +821,11 @@ void mjXWriter::Compiler(XMLElement* root) {
WriteAttrTxt(section, "convexhull", FindValue(bool_map, 2, model->convexhull));
}
WriteAttrTxt(section, "angle", "radian");
if (!model->meshdir.empty()) {
WriteAttrTxt(section, "meshdir", model->meshdir);
if (!model->get_meshdir().empty()) {
WriteAttrTxt(section, "meshdir", model->get_meshdir());
}
if (!model->texturedir.empty()) {
WriteAttrTxt(section, "texturedir", model->texturedir);
if (!model->get_texturedir().empty()) {
WriteAttrTxt(section, "texturedir", model->get_texturedir());
}
if (!model->usethread) {
WriteAttrTxt(section, "usethread", "false");
@@ -969,12 +975,13 @@ void mjXWriter::Size(XMLElement* root) {
// statistic section
void mjXWriter::Statistic(XMLElement* root) {
XMLElement* section = InsertEnd(root, "statistic");
mjStatistic* s = &model->stat;
if (mjuu_defined(model->meaninertia)) WriteAttr(section, "meaninertia", 1, &model->meaninertia);
if (mjuu_defined(model->meanmass)) WriteAttr(section, "meanmass", 1, &model->meanmass);
if (mjuu_defined(model->meansize)) WriteAttr(section, "meansize", 1, &model->meansize);
if (mjuu_defined(model->extent)) WriteAttr(section, "extent", 1, &model->extent);
if (mjuu_defined(model->center[0])) WriteAttr(section, "center", 3, model->center);
if (mjuu_defined(s->meaninertia)) WriteAttr(section, "meaninertia", 1, &s->meaninertia);
if (mjuu_defined(s->meanmass)) WriteAttr(section, "meanmass", 1, &s->meanmass);
if (mjuu_defined(s->meansize)) WriteAttr(section, "meansize", 1, &s->meansize);
if (mjuu_defined(s->extent)) WriteAttr(section, "extent", 1, &s->extent);
if (mjuu_defined(s->center[0])) WriteAttr(section, "center", 3, s->center);
// remove entire section if no attributes
if (!section->FirstAttribute()) root->DeleteChild(section);
@@ -1222,7 +1229,7 @@ void mjXWriter::Extension(XMLElement* root) {
}
// check if we need to open a new <plugin> section
const mjpPlugin* plugin = mjp_getPluginAtSlot(pp->plugin_slot);
const mjpPlugin* plugin = mjp_getPluginAtSlot(pp->spec.plugin_slot);
if (plugin != last_plugin) {
plugin_elem = InsertEnd(section, "plugin");
WriteAttrTxt(plugin_elem, "plugin", plugin->name);
@@ -1282,7 +1289,7 @@ void mjXWriter::Custom(XMLElement* root) {
elem = InsertEnd(section, "numeric");
WriteAttrTxt(elem, "name", ptr->name);
WriteAttrInt(elem, "size", ptr->size);
WriteAttr(elem, "data", ptr->size, ptr->data.data());
WriteAttr(elem, "data", ptr->size, ptr->data_.data());
}
// write all texts
@@ -1290,7 +1297,7 @@ void mjXWriter::Custom(XMLElement* root) {
mjCText* ptr = (mjCText*)model->GetObject(mjOBJ_TEXT, i);
elem = InsertEnd(section, "text");
WriteAttrTxt(elem, "name", ptr->name);
WriteAttrTxt(elem, "data", ptr->data.c_str());
WriteAttrTxt(elem, "data", ptr->data_.c_str());
}
// write all tuples
@@ -1300,11 +1307,11 @@ void mjXWriter::Custom(XMLElement* root) {
WriteAttrTxt(elem, "name", ptr->name);
// write objects in tuple
for (int j=0; j<(int)ptr->objtype.size(); j++) {
for (int j=0; j<(int)ptr->objtype_.size(); j++) {
XMLElement* obj = InsertEnd(elem, "element");
WriteAttrTxt(obj, "objtype", mju_type2Str((int)ptr->objtype[j]));
WriteAttrTxt(obj, "objname", ptr->objname[j].c_str());
double oprm = ptr->objprm[j];
WriteAttrTxt(obj, "objtype", mju_type2Str((int)ptr->objtype_[j]));
WriteAttrTxt(obj, "objname", ptr->objname_[j].c_str());
double oprm = ptr->objprm_[j];
if (oprm!=0) {
WriteAttr(obj, "prm", 1, &oprm);
}
@@ -1358,16 +1365,16 @@ void mjXWriter::Asset(XMLElement* root) {
// write textures loaded from files
else {
// write single file
WriteAttrTxt(elem, "content_type", ptex->content_type);
WriteAttrTxt(elem, "file", ptex->file);
WriteAttrTxt(elem, "content_type", ptex->get_content_type());
WriteAttrTxt(elem, "file", ptex->get_file());
// write separate files
WriteAttrTxt(elem, "fileright", ptex->cubefiles[0]);
WriteAttrTxt(elem, "fileleft", ptex->cubefiles[1]);
WriteAttrTxt(elem, "fileup", ptex->cubefiles[2]);
WriteAttrTxt(elem, "filedown", ptex->cubefiles[3]);
WriteAttrTxt(elem, "filefront", ptex->cubefiles[4]);
WriteAttrTxt(elem, "fileback", ptex->cubefiles[5]);
WriteAttrTxt(elem, "fileright", ptex->get_cubefiles()[0]);
WriteAttrTxt(elem, "fileleft", ptex->get_cubefiles()[1]);
WriteAttrTxt(elem, "fileup", ptex->get_cubefiles()[2]);
WriteAttrTxt(elem, "filedown", ptex->get_cubefiles()[3]);
WriteAttrTxt(elem, "filefront", ptex->get_cubefiles()[4]);
WriteAttrTxt(elem, "fileback", ptex->get_cubefiles()[5]);
if (ptex->hflip) {
WriteAttrKey(elem, "hflip", bool_map, 2, 1);
}
@@ -1399,7 +1406,7 @@ void mjXWriter::Asset(XMLElement* root) {
if (pmesh->plugin.active) {
elem = InsertEnd(section, "mesh");
WriteAttrTxt(elem, "name", pmesh->name);
OnePlugin(InsertEnd(elem, "plugin"), pmesh);
OnePlugin(InsertEnd(elem, "plugin"), &pmesh->plugin);
} else{
elem = InsertEnd(section, "mesh");
OneMesh(elem, pmesh, pmesh->def);
@@ -1415,15 +1422,15 @@ void mjXWriter::Asset(XMLElement* root) {
// write attributes
WriteAttrTxt(elem, "name", phf->name);
WriteAttr(elem, "size", 4, phf->size);
if (!phf->file.empty()) {
WriteAttrTxt(elem, "content_type", phf->content_type);
WriteAttrTxt(elem, "file", phf->file);
if (!phf->file_.empty()) {
WriteAttrTxt(elem, "content_type", phf->content_type_);
WriteAttrTxt(elem, "file", phf->file_);
} else {
WriteAttrInt(elem, "nrow", phf->nrow);
WriteAttrInt(elem, "ncol", phf->ncol);
if (!phf->userdata().empty()) {
if (!phf->get_userdata().empty()) {
string text;
Vector2String(text, phf->userdata(), phf->ncol);
Vector2String(text, phf->get_userdata(), phf->ncol);
WriteAttrTxt(elem, "elevation", text);
}
}
@@ -1499,7 +1506,7 @@ void mjXWriter::Body(XMLElement* elem, mjCBody* body) {
// write plugin
if (body->plugin.active) {
OnePlugin(InsertEnd(elem, "plugin"), body);
OnePlugin(InsertEnd(elem, "plugin"), &body->plugin);
}
// write child bodies recursively
@@ -1542,8 +1549,8 @@ void mjXWriter::Contact(XMLElement* root) {
// write attributes
WriteAttrTxt(elem, "name", pexclude->name);
WriteAttrTxt(elem, "body1", pexclude->bodyname1);
WriteAttrTxt(elem, "body2", pexclude->bodyname2);
WriteAttrTxt(elem, "body1", pexclude->get_bodyname1());
WriteAttrTxt(elem, "body2", pexclude->get_bodyname2());
}
}
@@ -1897,7 +1904,7 @@ void mjXWriter::Sensor(XMLElement* root) {
WriteAttrTxt(elem, "objtype", mju_type2Str(psen->objtype));
WriteAttrTxt(elem, "objname", psen->get_objname());
}
OnePlugin(elem, psen);
OnePlugin(elem, &psen->plugin);
break;
// user-defined sensor
@@ -1963,8 +1970,8 @@ void mjXWriter::Keyframe(XMLElement* root) {
// check qpos and write
for (int j=0; j<model->nq; j++) {
if (pk->qpos[j]!=model->qpos0[j]) {
WriteAttr(elem, "qpos", model->nq, pk->qpos.data());
if (pk->qpos_[j]!=model->qpos0[j]) {
WriteAttr(elem, "qpos", model->nq, pk->qpos_.data());
change = true;
break;
}
@@ -1972,8 +1979,8 @@ void mjXWriter::Keyframe(XMLElement* root) {
// check qvel and write
for (int j=0; j<model->nv; j++) {
if (pk->qvel[j]!=0) {
WriteAttr(elem, "qvel", model->nv, pk->qvel.data());
if (pk->qvel_[j]!=0) {
WriteAttr(elem, "qvel", model->nv, pk->qvel_.data());
change = true;
break;
}
@@ -1981,8 +1988,8 @@ void mjXWriter::Keyframe(XMLElement* root) {
// check act and write
for (int j=0; j<model->na; j++) {
if (pk->act[j]!=0) {
WriteAttr(elem, "act", model->na, pk->act.data());
if (pk->act_[j]!=0) {
WriteAttr(elem, "act", model->na, pk->act_.data());
change = true;
break;
}
@@ -1994,10 +2001,10 @@ void mjXWriter::Keyframe(XMLElement* root) {
if (model->bodies[j]->mocap) {
mjCBody* pb = model->bodies[j];
int id = pb->mocapid;
if (pb->pos[0] != pk->mpos[3*id] ||
pb->pos[1] != pk->mpos[3*id+1] ||
pb->pos[2] != pk->mpos[3*id+2]) {
WriteAttr(elem, "mpos", 3*model->nmocap, pk->mpos.data());
if (pb->pos[0] != pk->mpos_[3*id] ||
pb->pos[1] != pk->mpos_[3*id+1] ||
pb->pos[2] != pk->mpos_[3*id+2]) {
WriteAttr(elem, "mpos", 3*model->nmocap, pk->mpos_.data());
change = true;
break;
}
@@ -2011,11 +2018,11 @@ void mjXWriter::Keyframe(XMLElement* root) {
if (model->bodies[j]->mocap) {
mjCBody* pb = model->bodies[j];
int id = pb->mocapid;
if (pb->quat[0] != pk->mquat[4*id] ||
pb->quat[1] != pk->mquat[4*id+1] ||
pb->quat[2] != pk->mquat[4*id+2] ||
pb->quat[3] != pk->mquat[4*id+3]) {
WriteAttr(elem, "mquat", 4*model->nmocap, pk->mquat.data());
if (pb->quat[0] != pk->mquat_[4*id] ||
pb->quat[1] != pk->mquat_[4*id+1] ||
pb->quat[2] != pk->mquat_[4*id+2] ||
pb->quat[3] != pk->mquat_[4*id+3]) {
WriteAttr(elem, "mquat", 4*model->nmocap, pk->mquat_.data());
change = true;
break;
}
@@ -2025,8 +2032,8 @@ void mjXWriter::Keyframe(XMLElement* root) {
// check ctrl and write
for (int j=0; j<model->nu; j++) {
if (pk->ctrl[j]!=0) {
WriteAttr(elem, "ctrl", model->nu, pk->ctrl.data());
if (pk->ctrl_[j]!=0) {
WriteAttr(elem, "ctrl", model->nu, pk->ctrl_.data());
change = true;
break;
}
src/xml/xml_native_writer.h
+1 -1
View File
@@ -66,7 +66,7 @@ class mjXWriter : public mjXBase {
void OneEquality(tinyxml2::XMLElement* elem, mjCEquality* pequality, mjCDef* def);
void OneTendon(tinyxml2::XMLElement* elem, mjCTendon* ptendon, mjCDef* def);
void OneActuator(tinyxml2::XMLElement* elem, mjCActuator* pactuator, mjCDef* def);
void OnePlugin(tinyxml2::XMLElement* elem, mjCBase* object);
void OnePlugin(tinyxml2::XMLElement* elem, mjmPlugin* plugin);
bool writingdefaults; // true during defaults write
};
src/xml/xml_urdf.cc
+38 -26
View File
@@ -93,23 +93,26 @@ void mjXURDF::Parse(
if (mjc) {
XMLElement *section;
if ((section = FindSubElem(mjc, "compiler"))) {
mjXReader::Compiler(section, model);
mjXReader::Compiler(section, &model->spec);
}
if ((section = FindSubElem(mjc, "option"))) {
mjXReader::Option(section, &model->option);
mjXReader::Option(section, &model->spec.option);
}
if ((section = FindSubElem(mjc, "size"))) {
mjXReader::Size(section, model);
mjXReader::Size(section, &model->spec);
}
}
// enforce required compiler defaults for URDF
model->degree = false;
model->spec.degree = false;
// get model name
ReadAttrTxt(root, "name", model->modelname);
std::string modelname;
if (ReadAttrTxt(root, "name", modelname)) {
mjm_setString(model->spec.modelname, modelname.c_str());
}
// find and register all materials
MakeMaterials(root);
@@ -202,7 +205,7 @@ void mjXURDF::Parse(
// override the pose for the base link and add a free joint
for (int i = 0; i < (int)urName.size(); i++) {
if (urParent[i] < 0) {
mjmBody* world = mjm_findBody(model, "world");
mjmBody* world = mjm_findBody(&model->spec, "world");
mjmBody* pbody = mjm_findChild(world, urName[i].c_str());
mjuu_copyvec(pbody->pos, pos, 3);
mjuu_copyvec(pbody->quat, quat, 4);
@@ -228,7 +231,7 @@ void mjXURDF::Body(XMLElement* body_elem) {
// get body name and pointer to mjmBody
ReadAttrTxt(body_elem, "name", name, true);
name = GetPrefixedName(name);
world = mjm_findBody(model, "world");
world = mjm_findBody(&model->spec, "world");
pbody = mjm_findChild(world, name.c_str());
if (!pbody) {
throw mjXError(body_elem, "URDF body not found"); // SHOULD NOT OCCUR
@@ -310,7 +313,7 @@ void mjXURDF::Body(XMLElement* body_elem) {
}
}
// create geom if not discarded
if (!model->discardvisual) {
if (!model->spec.discardvisual) {
pgeom = Geom(elem, pbody, false);
// save color
@@ -386,7 +389,7 @@ void mjXURDF::Joint(XMLElement* joint_elem) {
elem = FindSubElem(joint_elem, "parent", true);
ReadAttrTxt(elem, "link", name, true);
name = GetPrefixedName(name);
world = mjm_findBody(model, "world");
world = mjm_findBody(&model->spec, "world");
parent = mjm_findChild(world, name.c_str());
if (!parent) { // SHOULD NOT OCCUR
throw mjXError(elem, "invalid parent name in URDF joint definition");
@@ -396,7 +399,7 @@ void mjXURDF::Joint(XMLElement* joint_elem) {
elem = FindSubElem(joint_elem, "child", true);
ReadAttrTxt(elem, "link", name, true);
name = GetPrefixedName(name);
world = mjm_findBody(model, "world");
world = mjm_findBody(&model->spec, "world");
pbody = mjm_findChild(world, name.c_str());
if (!pbody) { // SHOULD NOT OCCUR
throw mjXError(elem, "invalid child name in URDF joint definition");
@@ -487,14 +490,15 @@ void mjXURDF::Joint(XMLElement* joint_elem) {
if ((elem = FindSubElem(joint_elem, "limit"))) {
ReadAttr(elem, "lower", 1, pjoint->range, text);
ReadAttr(elem, "upper", 1, pjoint->range+1, text);
pjoint->limited = (mjuu_defined(pjoint->range[0]) &&
mjuu_defined(pjoint->range[1]) &&
pjoint->range[0] < pjoint->range[1]);
bool is_limited = mjuu_defined(pjoint->range[0]) &&
mjuu_defined(pjoint->range[1]) &&
pjoint->range[0] < pjoint->range[1];
pjoint->limited = is_limited ? mjLIMITED_TRUE : mjLIMITED_FALSE;
// ReadAttr(elem, "velocity", 1, &pjoint->maxvel, text); // no maxvel in MuJoCo
ReadAttr(elem, "effort", 1, &pjoint->urdfeffort, text);
} else {
pjoint->limited = 0;
pjoint->limited = mjLIMITED_FALSE;
}
}
@@ -562,7 +566,7 @@ mjmGeom* mjXURDF::Geom(XMLElement* geom_elem, mjmBody* pbody, bool collision) {
.value_or(default_meshscale);
// strip file name if necessary
if (model->strippath) {
if (model->spec.strippath) {
meshfile = mjuu_strippath(meshfile);
}
@@ -571,26 +575,34 @@ mjmGeom* mjXURDF::Geom(XMLElement* geom_elem, mjmBody* pbody, bool collision) {
meshname = mjuu_stripext(meshname);
// look for existing mesh
mjCMesh* pmesh = (mjCMesh*)model->FindObject(mjOBJ_MESH, meshname);
mjCMesh* mesh = (mjCMesh*)model->FindObject(mjOBJ_MESH, meshname);
mjmMesh* pmesh = 0;
// does not exist: create
if (!pmesh) {
pmesh = model->AddMesh();
if (!mesh) {
pmesh = mjm_addMesh(&model->spec, 0);
}
// exists with different scale: append name with '1', create
else if (pmesh->scale()[0]!=meshscale[0] ||
pmesh->scale()[1]!=meshscale[1] ||
pmesh->scale()[2]!=meshscale[2]) {
pmesh = model->AddMesh();
else if (mesh->spec.scale[0]!=meshscale[0] ||
mesh->spec.scale[1]!=meshscale[1] ||
mesh->spec.scale[2]!=meshscale[2]) {
pmesh = mjm_addMesh(&model->spec, 0);
meshname = meshname + "1";
}
// point to already existing spec
else {
pmesh = &mesh->spec;
}
// set fields
pmesh->set_file(meshfile);
pmesh->name = meshname;
mjm_setString(pmesh->file, meshfile.c_str());
mjm_setString(pmesh->name, meshname.c_str());
mjm_setString(pgeom->meshname, meshname.c_str());
pmesh->set_scale(meshscale);
pmesh->scale[0] = meshscale[0];
pmesh->scale[1] = meshscale[1];
pmesh->scale[2] = meshscale[2];
}
else {
@@ -672,7 +684,7 @@ void mjXURDF::AddToTree(int n) {
// get pointer to parent in mjCModel tree
mjmBody *parent = 0, *child = 0, *world = 0;
if (urParent[n]>=0) {
world = mjm_findBody(model, "world");
world = mjm_findBody(&model->spec, "world");
parent = mjm_findChild(world, urName[urParent[n]].c_str());
if (!parent)
src/xml/xml_util.cc
+59 -69
View File
@@ -32,8 +32,11 @@
#include "tinyxml2.h"
#include <mujoco/mujoco.h>
#include "cc/array_safety.h"
#include "engine/engine_resource.h"
#include "engine/engine_util_errmem.h"
#include "user/user_util.h"
#include "xml/xml_util.h"
#include "xml/xml_numeric_format.h"
@@ -156,57 +159,37 @@ XMLElement* NextSiblingElement(XMLElement* e, const char* name) {
return nullptr;
}
// constructor
mjXSchema::mjXSchema(const char* schema[][mjXATTRNUM], unsigned nrow, bool checkptr) {
if (schema[0][0][0] == '<' || schema[0][0][0] == '>') {
throw "expected element, found bracket";
static std::string ResolveFilePath(XMLElement* e, std::string filename,
const std::string& dir) {
std::string path = "";
if (mjuu_isabspath(filename)) {
return filename;
}
// check entire schema for null pointers
if (checkptr) {
char msg[100];
// TODO(kylebayes): We first look in the base model directory for files to
// remain backwards compatible.
std::string full_filename = dir + filename;
mjResource *resource = mju_openResource(full_filename.c_str(), nullptr, 0);
if (resource != nullptr) {
mju_closeResource(resource);
return filename;
}
for (int i=0; i<nrow; i++) {
// base pointers
if (!schema[i][0]) {
mju::sprintf_arr(msg, "null pointer found in row %d", i);
throw msg;
}
// detect element
if (schema[i][0][0]!='<' && schema[i][0][0]!='>') {
// first 3 pointers required
if (!schema[i][1] || !schema[i][2]) {
mju::sprintf_arr(msg, "null pointer in row %d, element %s", i, schema[i][0]);
throw msg;
}
// check type
if (schema[i][1][0]!='!' && schema[i][1][0]!='?' &&
schema[i][1][0]!='*' && schema[i][1][0]!='R') {
mju::sprintf_arr(msg, "invalid type in row %d, element %s", i, schema[i][0]);
throw msg;
}
// number of attributes
int nattr = atoi(schema[i][2]);
if (nattr < 0 || nattr > mjXATTRNUM-3) {
mju::sprintf_arr(msg,
"invalid number of attributes in row %d, element %s", i, schema[i][0]);
throw msg;
}
// attribute pointers
for (int j=0; j<nattr; j++) {
if (!schema[i][3+j]) {
mju::sprintf_arr(msg, "null attribute %d in row %d, element %s", j, i, schema[i][0]);
throw msg;
}
}
XMLElement* parent = e->Parent()->ToElement();
for (; parent; parent = parent->Parent()->ToElement()) {
if (!std::strcmp(parent->Value(), "include")) {
auto file_attr = mjXUtil::ReadAttrStr(parent, "dir", false);
if (file_attr.has_value()) {
path = file_attr.value();
}
break;
}
}
return path + filename;
}
// constructor
mjXSchema::mjXSchema(const char* schema[][mjXATTRNUM], unsigned nrow) {
// set name and type
name_ = schema[0][0];
type_ = schema[0][1][0];
@@ -218,42 +201,32 @@ mjXSchema::mjXSchema(const char* schema[][mjXATTRNUM], unsigned nrow, bool check
}
// process sub-elements of complex element
if (nrow>1) {
// check for bracketed block
if (schema[1][0][0]!='<' || schema[nrow-1][0][0]!='>') {
throw "expected brackets after complex element";
}
if (nrow > 1) {
// parse block into simple and complex elements, create children
int start = 2;
while (start < nrow-1) {
int end = start;
// look for bracketed block at start+1
if (schema[start+1][0][0]=='<') {
if (schema[start+1][0][0] == '<') {
// look for corresponding closing bracket
int cnt = 0;
while (end <= nrow-1) {
if (schema[end][0][0]=='<') {
if (schema[end][0][0] == '<') {
cnt++;
} else if (schema[end][0][0]=='>') {
} else if (schema[end][0][0] == '>') {
cnt--;
if (cnt==0) {
if (cnt == 0) {
break;
}
}
end++;
}
// closing bracket not found
if (end > nrow-1) {
throw "matching closing bracket not found";
}
}
// add element, check for error
subschema_.emplace_back(schema+start, end-start+1, false);
// add child element
subschema_.emplace_back(schema+start, end-start+1);
// proceed with next subelement
start = end+1;
@@ -621,8 +594,8 @@ mjXUtil::ReadAttrVec(XMLElement* elem, const char* attr, bool required);
// if attribute is present, return attribute as a string
std::optional<std::string> mjXUtil::ReadAttrStr(XMLElement* elem, const char* attr,
bool required) {
std::optional<std::string>
mjXUtil::ReadAttrStr(XMLElement* elem, const char* attr, bool required) {
const char* pstr = elem->Attribute(attr);
// check if attribute exists
@@ -637,7 +610,16 @@ std::optional<std::string> mjXUtil::ReadAttrStr(XMLElement* elem, const char* at
return std::string(pstr);
}
// if attribute is present, return attribute as a filename
std::optional<std::string>
mjXUtil::ReadAttrFile(XMLElement* elem, const char* attr,
const std::string& dir, bool required) {
auto maybe_str = ReadAttrStr(elem, attr, required);
if (!maybe_str.has_value()) {
return std::nullopt;
}
return ResolveFilePath(elem, maybe_str.value(), dir);
}
// if attribute is present, return numerical value of attribute
template<typename T>
@@ -963,7 +945,8 @@ static int Round(double x) {
// write attribute
template<typename T>
void mjXUtil::WriteAttr(XMLElement* elem, string name, int n, const T* data, const T* def) {
void mjXUtil::WriteAttr(XMLElement* elem, string name, int n, const T* data, const T* def,
bool trim) {
// make sure all are defined
if constexpr (std::is_floating_point_v<T>) {
for (int i=0; i<n; i++) {
@@ -978,6 +961,13 @@ void mjXUtil::WriteAttr(XMLElement* elem, string name, int n, const T* data, con
return;
}
// trim identical trailing default values
if (trim) {
while (n > 0 && data[n-1] == def[n-1]) {
n--;
}
}
// increase precision for testing
stringstream stream;
stream.precision(mujoco::_mjPRIVATE__get_xml_precision());
@@ -1004,17 +994,17 @@ void mjXUtil::WriteAttr(XMLElement* elem, string name, int n, const T* data, con
template void mjXUtil::WriteAttr(XMLElement* elem, string name, int n,
const double* data, const double* def);
const double* data, const double* def, bool trim);
template void mjXUtil::WriteAttr(XMLElement* elem, string name, int n,
const float* data, const float* def);
const float* data, const float* def, bool trim);
template void mjXUtil::WriteAttr(XMLElement* elem, string name, int n,
const int* data, const int* def);
const int* data, const int* def, bool trim);
template void mjXUtil::WriteAttr(XMLElement* elem, string name, int n,
const unsigned char* data,
const unsigned char* def);
const unsigned char* def, bool trim);
// write vector<double> attribute, default = zero array
src/xml/xml_util.h
+6 -3
View File
@@ -35,7 +35,6 @@ using tinyxml2::XMLElement;
XMLElement* FirstChildElement(XMLElement* e, const char* name = nullptr);
XMLElement* NextSiblingElement(XMLElement* e, const char* name = nullptr);
// XML Error info
class [[nodiscard]] mjXError {
public:
@@ -56,7 +55,7 @@ class [[nodiscard]] mjXError {
// Custom XML file validation
class mjXSchema {
public:
mjXSchema(const char* schema[][mjXATTRNUM], unsigned nrow, bool checkptr = true);
mjXSchema(const char* schema[][mjXATTRNUM], unsigned nrow);
std::string GetError(); // return error
void Print(std::stringstream& str, int level) const; // print schema
@@ -109,6 +108,10 @@ class mjXUtil {
static std::optional<std::string> ReadAttrStr(tinyxml2::XMLElement* elem, const char* attr,
bool required = false);
// if attribute is present, return attribute as a filename
static std::optional<std::string> ReadAttrFile(tinyxml2::XMLElement* elem, const char* attr,
const std::string& dir = "", bool required = false);
// if attribute is present, return numerical value of attribute
template<typename T>
static std::optional<T> ReadAttrNum(tinyxml2::XMLElement* elem, const char* attr,
@@ -195,7 +198,7 @@ class mjXUtil {
// write attribute- any type
template<typename T>
static void WriteAttr(tinyxml2::XMLElement* elem, std::string name, int n, const T* data,
const T* def = 0);
const T* def = 0, bool trim = false);
// write vector<double> attribute, with and without default
static void WriteVector(tinyxml2::XMLElement* elem, std::string name,
test/CMakeLists.txt
+2
View File
@@ -58,6 +58,8 @@ target_link_libraries(
fixture
PUBLIC absl::core_headers
absl::synchronization
absl::flat_hash_map
absl::flat_hash_set
gtest
gmock
mujoco::mujoco
test/engine/engine_resource_test.cc
+10 -14
View File
@@ -14,6 +14,7 @@
// Tests for engine/engine_resource.c
#include <array>
#include <cstring>
#include <gmock/gmock.h>
@@ -228,7 +229,7 @@ TEST_F(ResourceTest, GeneralTest) {
EXPECT_GT(i, 0);
// open resource
mjResource* resource = mju_openResource("str:file");
mjResource* resource = mju_openResource("str:file", nullptr, 0);
ASSERT_THAT(resource, NotNull());
const char* buffer = NULL;
@@ -239,7 +240,7 @@ TEST_F(ResourceTest, GeneralTest) {
mju_closeResource(resource);
}
TEST_F(ResourceTest, GeneralTestFailure) {
TEST_F(ResourceTest, GeneralFailureTest) {
mjpResourceProvider provider = {
"str", open_str, read_str, close_str
};
@@ -248,19 +249,14 @@ TEST_F(ResourceTest, GeneralTestFailure) {
int i = mjp_registerResourceProvider(&provider);
EXPECT_GT(i, 0);
// install warning handler
static char warning[1024];
warning[0] = '\0';
mju_user_warning = [](const char* msg) {
util::strcpy_arr(warning, msg);
};
static std::array<char, 1024> error;
// open resource
mjResource* resource = mju_openResource("str:notfound");
mjResource* resource = mju_openResource("str:notfound",
error.data(), error.size());
ASSERT_THAT(resource, IsNull());
EXPECT_THAT(warning, HasSubstr("could not open"));
EXPECT_THAT(error.data(), HasSubstr("could not open"));
}
TEST_F(ResourceTest, NameWithValidPrefix) {
@@ -281,7 +277,7 @@ TEST_F(ResourceTest, NameWithValidPrefix) {
};
// open resource
mjResource* resource = mju_openResource("nop:found");
mjResource* resource = mju_openResource("nop:found", nullptr, 0);
ASSERT_THAT(resource, NotNull());
mju_closeResource(resource);
}
@@ -304,7 +300,7 @@ TEST_F(ResourceTest, NameWithUpperCasePrefix) {
};
// open resource
mjResource* resource = mju_openResource("NOP:found");
mjResource* resource = mju_openResource("NOP:found", nullptr, 0);
ASSERT_THAT(resource, NotNull());
mju_closeResource(resource);
}
@@ -327,7 +323,7 @@ TEST_F(ResourceTest, NameWithInvalidPrefix) {
};
// open resource
mjResource* resource = mju_openResource("nopfound");
mjResource* resource = mju_openResource("nopfound", nullptr, 0);
ASSERT_THAT(resource, IsNull());
}
test/engine/engine_util_misc_test.cc
-1
View File
@@ -247,7 +247,6 @@ TEST_F(Base64Test, mju_encodeBase64) {
EXPECT_THAT(buffer.data(), StrEq("D4a+//A="));
EXPECT_THAT(n, std::strlen(buffer.data()) + 1);
EXPECT_THAT(n, buffer.size());
}
TEST_F(Base64Test, mju_encodeBase64_align0) {
test/engine/engine_vfs_test.cc
+42 -26
View File
@@ -12,6 +12,9 @@
// See the License for the specific language governing permissions and
// limitations under the License.
#include <array>
#include <cstdio>
#include <memory>
#include <string>
#include <gmock/gmock.h>
@@ -25,7 +28,7 @@ namespace {
using ::testing::NotNull;
using EngineVfsTest = MujocoTest;
TEST_F(EngineVfsTest, AddFileVFS) {
TEST_F(EngineVfsTest, AddFileTest) {
constexpr char path[] = "engine/testdata/actuation/";
const std::string dir = GetTestDataFilePath(path);
std::string file1 = "activation.xml";
@@ -44,38 +47,51 @@ TEST_F(EngineVfsTest, AddFileVFS) {
ASSERT_THAT(fp3, NotNull()) << "Input file3 missing.";
std::fclose(fp3);
auto mj_vfs = std::make_unique<mjVFS>();
mj_defaultVFS(mj_vfs.get());
auto vfs = std::make_unique<mjVFS>();
mj_defaultVFS(vfs.get());
EXPECT_THAT(mj_vfs->nfile, 0);
EXPECT_THAT(mj_addFileVFS(mj_vfs.get(), dir.c_str(), file1.c_str()), 0);
EXPECT_THAT(mj_vfs->nfile, 1);
EXPECT_THAT(mj_vfs->filename[0], file1);
EXPECT_THAT(vfs->nfile, 0);
EXPECT_THAT(mj_addFileVFS(vfs.get(), dir.c_str(), file1.c_str()), 0);
EXPECT_THAT(vfs->nfile, 1);
EXPECT_THAT(vfs->filename[0], file1);
EXPECT_THAT(mj_addFileVFS(mj_vfs.get(), dir.c_str(), file2.c_str()), 0);
EXPECT_THAT(mj_vfs->nfile, 2);
EXPECT_THAT(mj_vfs->filename[0], file1);
EXPECT_THAT(mj_vfs->filename[1], file2);
EXPECT_THAT(mj_addFileVFS(vfs.get(), dir.c_str(), file2.c_str()), 0);
EXPECT_THAT(vfs->nfile, 2);
EXPECT_THAT(vfs->filename[0], file1);
EXPECT_THAT(vfs->filename[1], file2);
EXPECT_THAT(mj_addFileVFS(mj_vfs.get(), dir.c_str(), file3.c_str()), 0);
EXPECT_THAT(mj_vfs->nfile, 3);
EXPECT_THAT(mj_vfs->filename[0], file1);
EXPECT_THAT(mj_vfs->filename[1], file2);
EXPECT_THAT(mj_vfs->filename[2], file3);
EXPECT_THAT(mj_addFileVFS(vfs.get(), dir.c_str(), file3.c_str()), 0);
EXPECT_THAT(vfs->nfile, 3);
EXPECT_THAT(vfs->filename[0], file1);
EXPECT_THAT(vfs->filename[1], file2);
EXPECT_THAT(vfs->filename[2], file3);
mj_deleteFileVFS(mj_vfs.get(), file1.c_str());
EXPECT_THAT(mj_vfs->nfile, 2);
EXPECT_THAT(mj_vfs->filename[0], file2);
EXPECT_THAT(mj_vfs->filename[1], file3);
mj_deleteFileVFS(vfs.get(), file1.c_str());
EXPECT_THAT(vfs->nfile, 2);
EXPECT_THAT(vfs->filename[0], file2);
EXPECT_THAT(vfs->filename[1], file3);
mj_deleteFileVFS(mj_vfs.get(), file3.c_str());
EXPECT_THAT(mj_vfs->nfile, 1);
EXPECT_THAT(mj_vfs->filename[0], file2);
mj_deleteFileVFS(vfs.get(), file3.c_str());
EXPECT_THAT(vfs->nfile, 1);
EXPECT_THAT(vfs->filename[0], file2);
mj_deleteFileVFS(mj_vfs.get(), file2.c_str());
EXPECT_THAT(mj_vfs->nfile, 0);
mj_deleteFileVFS(vfs.get(), file2.c_str());
EXPECT_THAT(vfs->nfile, 0);
mj_deleteVFS(mj_vfs.get());
mj_deleteVFS(vfs.get());
}
TEST_F(EngineVfsTest, AddBufferTest) {
auto vfs = std::make_unique<mjVFS>();
mj_defaultVFS(vfs.get());
std::string buffer = "<mujoco/>";
mj_addBufferVFS(vfs.get(), "model", static_cast<const void*>(buffer.c_str()),
buffer.size());
std::array<char, 1024> error;
mjModel* model = mj_loadXML("model", vfs.get(), error.data(), error.size());
EXPECT_THAT(model, NotNull());
mj_deleteModel(model);
mj_deleteVFS(vfs.get());
}
} // namespace
test/fixture.cc
+121
View File
@@ -14,6 +14,8 @@
#include "test/fixture.h"
#include <array>
#include <cerrno>
#include <cstdio>
#include <cstring>
#include <filesystem>
@@ -21,12 +23,15 @@
#include <sstream>
#include <string>
#include <string_view>
#include <utility>
#include <vector>
#include <gmock/gmock.h>
#include <gtest/gtest.h>
#include <absl/base/const_init.h>
#include <absl/strings/str_cat.h>
#include <absl/strings/str_join.h>
#include <absl/synchronization/mutex.h>
#include <mujoco/mjmodel.h>
#include <mujoco/mujoco.h>
@@ -167,4 +172,120 @@ std::vector<mjtNum> GetCtrlNoise(const mjModel* m, int nsteps,
return ctrl;
}
MockFilesystem::MockFilesystem(std::string unit_test_name) {
prefix_ = absl::StrCat("MjMock.", unit_test_name);
dir_ = "/";
if (mjp_getResourceProvider(prefix_.c_str()) != nullptr) {
return;
}
mjpResourceProvider resourceProvider;
mjp_defaultResourceProvider(&resourceProvider);
resourceProvider.prefix = prefix_.c_str();
resourceProvider.data = (void *) this;
resourceProvider.open = +[](mjResource* resource) {
MockFilesystem *fs = static_cast<MockFilesystem*>(resource->provider->data);
std::string filename = fs->StripPrefix(resource->name);
return fs->FileExists(filename) ? 1 : 0;
};
resourceProvider.read =+[](mjResource* resource, const void** buffer) {
MockFilesystem *fs = static_cast<MockFilesystem*>(resource->provider->data);
std::string filename = fs->StripPrefix(resource->name);
return (int) fs->GetFile(filename, (const unsigned char**) buffer);
};
resourceProvider.getdir = +[](mjResource* resource, const char** dir,
int* ndir) {
MockFilesystem *fs = static_cast<MockFilesystem*>(resource->provider->data);
*dir = resource->name;
// find last directory path separator
int length = fs->Prefix().size() + 1;
for (int i = length; resource->name[i]; ++i) {
if (resource->name[i] == '/' || resource->name[i] == '\\') {
length = i + 1;
}
}
*ndir = length;
};
resourceProvider.close = +[](mjResource* resource) {};
mjp_registerResourceProvider(&resourceProvider);
}
bool MockFilesystem::AddFile(std::string filename, const unsigned char* data,
std::size_t ndata) {
std::string fullfilename = PathReduce(dir_, filename);
auto [it, inserted] = filenames_.insert(fullfilename);
if (inserted) {
data_[fullfilename] = std::vector(data, data + ndata);
}
return inserted;
}
bool MockFilesystem::FileExists(const std::string& filename) {
std::string fullfilename = PathReduce(dir_, filename);
return filenames_.find(fullfilename) != filenames_.end();
}
std::size_t MockFilesystem::GetFile(const std::string& filename,
const unsigned char** buffer) const {
std::string fullfilename = PathReduce(dir_, filename);
auto it = data_.find(fullfilename);
if (it == data_.end()) {
return 0;
}
*buffer = it->second.data();
return it->second.size();
}
void MockFilesystem::ChangeDirectory(std::string dir) {
if (dir.empty()) {
return;
}
dir_ = PathReduce(dir_, dir);
if (dir_.back() != '/') {
dir_ = absl::StrCat(dir_, "/");
}
}
std::string MockFilesystem::FullPath(const std::string& path) const {
return absl::StrCat(prefix_, ":", PathReduce(dir_, path));
}
std::string MockFilesystem::StripPrefix(const char* path) const {
return &path[prefix_.size() + 1];
}
std::string MockFilesystem::PathReduce(const std::string& current_dir,
const std::string& path) {
std::stringstream stream;
if (!path.empty() && path[0] != '/') {
stream = std::stringstream(absl::StrCat(current_dir, path));
} else {
stream = std::stringstream(path);
}
std::string temp;
std::vector<std::string> dirs;
while (std::getline(stream, temp, '/')) {
if (temp == ".." && !dirs.empty()) {
dirs.pop_back();
continue;
}
if (temp != "." && !temp.empty()) {
dirs.push_back(temp);
}
}
if (dirs.empty()) {
return "/";
}
return absl::StrJoin(dirs, "/");
}
} // namespace mujoco
test/fixture.h
+47
View File
@@ -22,6 +22,8 @@
#include <vector>
#include <gtest/gtest.h>
#include <absl/container/flat_hash_map.h>
#include <absl/container/flat_hash_set.h>
#include <mujoco/mjmodel.h>
#include <mujoco/mujoco.h>
@@ -99,6 +101,51 @@ std::string SaveAndReadXml(const mjModel* model);
std::vector<mjtNum> GetCtrlNoise(const mjModel* m, int nsteps,
mjtNum ctrlnoise = 0.01);
// Installs a mock filesystem via a resource provider. To obtain thread safety,
// each filesystem is scoped for individual unit tests with destructive
// operations not permitted.
class MockFilesystem {
public:
// constructs mock filesystem. A unique name (normally the unit test name)
// should be passed in.
MockFilesystem(std::string unit_test_name);
// Move and copy operations are forbidden.
MockFilesystem(MockFilesystem&& other) = delete;
MockFilesystem& operator=(MockFilesystem&& other) = delete;
MockFilesystem(const MockFilesystem& other) = delete;
MockFilesystem& operator=(const MockFilesystem& other) = delete;
// Returns the prefix registered for the resource provider.
const std::string& Prefix() const { return prefix_; }
// Adds file to the current directory. Returns false if file already exists.
bool AddFile(std::string filename, const unsigned char* data,
std::size_t ndata);
// Returns true if mock filesystem has file.
bool FileExists(const std::string& filename);
// Change the current directory.
void ChangeDirectory(std::string dir);
// Helper functions for resource provider callbacks.
std::size_t GetFile(const std::string& filename,
const unsigned char** buffer) const;
std::string FullPath(const std::string& path) const;
private:
std::string StripPrefix(const char* path) const;
static std::string PathReduce(const std::string& current_dir,
const std::string& path);
absl::flat_hash_set<std::string> filenames_;
absl::flat_hash_map<std::string, std::vector<unsigned char>> data_;
std::string prefix_;
std::string dir_; // current directory
};
// Installs all plugins
class PluginTest : public MujocoTest {
public:
test/fixture_test.cc
+19
View File
@@ -14,6 +14,8 @@
#include "test/fixture.h"
#include <array>
#include <gmock/gmock.h>
#include <gtest/gtest-spi.h>
#include <gtest/gtest.h>
@@ -49,5 +51,22 @@ TEST_F(MujocoErrorTestGuardTest, NestedErrorGuards) {
EXPECT_THAT(mju_user_warning, IsNull());
}
TEST_F(MujocoTestTest, MockFilesystemTest) {
MockFilesystem fs("MockFilesystemTest");
std::array<unsigned char, 3> data = {'a', 'b', 'c'};
fs.ChangeDirectory("tmp");
fs.AddFile("../tmp2/file2", data.data(), data.size());
fs.AddFile("./file1", data.data(), data.size());
ASSERT_TRUE(fs.FileExists("/tmp/file1"));
ASSERT_TRUE(fs.FileExists("/tmp2/file2"));
fs.ChangeDirectory("../tmp2");
ASSERT_TRUE(fs.FileExists("../tmp/file1"));
ASSERT_TRUE(fs.FileExists("file2"));
}
} // namespace
} // namespace mujoco
test/user/user_api_test.cc
+1 -1
View File
@@ -29,7 +29,7 @@ using ::testing::HasSubstr;
// ----------------------------- test set/get --------------------------------
TEST_F(MujocoTest, ReadWriteData) {
void* model = mjm_createModel();
mjmModel* model = mjm_createModel();
mjmBody* world = mjm_findBody(model, "world");
mjmBody* body = mjm_addBody(world, 0);
mjmSite* site = mjm_addSite(body, 0);
test/user/user_objects_test.cc
+71
View File
@@ -433,6 +433,28 @@ TEST_F(KeyframeTest, ResetDataKeyframe) {
mj_deleteModel(model);
}
TEST_F(KeyframeTest, ResetDataKeyframeAcceptsNegativeKeyframe) {
const std::string xml_path = GetTestDataFilePath(kKeyframePath);
mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0);
ASSERT_THAT(model, NotNull());
mjData* data = mj_makeData(model);
data->time = data->qpos[0] = data->qvel[0] = data->act[0] = data->ctrl[0] =
data->mocap_pos[0] = data->mocap_quat[0] = 1337.0;
mj_resetDataKeyframe(model, data, -1);
EXPECT_EQ(data->time, 0.0);
EXPECT_EQ(data->qpos[0], 0.0);
EXPECT_EQ(data->qvel[0], 0.0);
EXPECT_EQ(data->act[0], 0.0);
EXPECT_EQ(data->ctrl[0], 0.0);
EXPECT_EQ(data->mocap_pos[0], 0.0);
EXPECT_EQ(data->mocap_quat[0], 1.0);
mj_deleteData(data);
mj_deleteModel(model);
}
TEST_F(KeyframeTest, BadSize) {
static constexpr char xml[] = R"(
<mujoco>
@@ -909,6 +931,55 @@ TEST_F(ActuatorTest, ActuatorOrderDoesntMatter) {
mj_deleteModel(model1);
}
// ------------- test inheritrange attribute ----------------------------------
using InheritrangeTest = MujocoTest;
TEST_F(InheritrangeTest, ErrorIfTargetMissingRange) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<joint name="jnt"/>
<geom size="1"/>
</body>
</worldbody>
<actuator>
<position joint="jnt" inheritrange="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("target 'jnt' has no range defined"));
}
TEST_F(InheritrangeTest, WorksForDegrees) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<joint name="jnt" range="90 180"/>
<geom size="1"/>
</body>
</worldbody>
<actuator>
<position joint="jnt" inheritrange="1"/>
</actuator>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << error.data();
EXPECT_DOUBLE_EQ(model->actuator_ctrlrange[0], mjPI/2);
EXPECT_DOUBLE_EQ(model->actuator_ctrlrange[1], mjPI);
mj_deleteModel(model);
}
// ------------- test actlimited and actrange fields ---------------------------
test/xml/xml_native_reader_test.cc
+269 -4
View File
@@ -437,8 +437,48 @@ TEST_F(XMLReaderTest, InvalidDoubleOrientation) {
}
}
}
TEST_F(XMLReaderTest, RepeatedDefaultName) {
static constexpr char xml[] = R"(
<mujoco>
<default>
<default class="sphere">
<geom type="sphere" size="1"/>
</default>
<default class="sphere">
<geom type="capsule" size="1 1"/>
</default>
</default>
<worldbody>
<body>
<geom class="sphere"/>
</body>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, IsNull()) << error.data();
EXPECT_THAT(error.data(), HasSubstr("repeated default class name"));
}
// ------------------------ test including -------------------------------------
// credit: https://www.mjt.me.uk/posts/smallest-png/
static constexpr unsigned char kTinyPng[] =
{ 0x89, 0x50, 0x4E, 0x47, 0x0D, 0x0A, 0x1A, 0x0A, 0x00,
0x00, 0x00, 0x0D, 0x49, 0x48, 0x44, 0x52, 0x00, 0x00,
0x01, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x03, 0x00,
0x00, 0x00, 0x66, 0xBC, 0x3A, 0x25, 0x00, 0x00, 0x00,
0x03, 0x50, 0x4C, 0x54, 0x45, 0xB5, 0xD0, 0xD0, 0x63,
0x04, 0x16, 0xEA, 0x00, 0x00, 0x00, 0x1F, 0x49, 0x44,
0x41, 0x54, 0x68, 0x81, 0xED, 0xC1, 0x01, 0x0D, 0x00,
0x00, 0x00, 0xC2, 0xA0, 0xF7, 0x4F, 0x6D, 0x0E, 0x37,
0xA0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0xBE, 0x0D, 0x21, 0x00, 0x00, 0x01, 0x9A, 0x60, 0xE1,
0xD5, 0x00, 0x00, 0x00, 0x00, 0x49, 0x45, 0x4E, 0x44,
0xAE, 0x42, 0x60, 0x82 };
TEST_F(XMLReaderTest, IncludeTest) {
static constexpr char xml[] = R"(
<mujoco>
@@ -517,7 +557,6 @@ TEST_F(XMLReaderTest, IncludeSameFileTest) {
<geom name="box" type="box" size="1 1 1"/>
</mujoco>)";
auto vfs = std::make_unique<mjVFS>();
mj_defaultVFS(vfs.get());
@@ -533,7 +572,229 @@ TEST_F(XMLReaderTest, IncludeSameFileTest) {
mj_deleteVFS(vfs.get());
}
TEST_F(XMLReaderTest, IncludePathTest) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<geom name="plane" type="plane" size="1 1 1"/>
<include file="submodels/model1.xml"/>
<include file="submodels/model2.xml"/>
</worldbody>
</mujoco>)";
static constexpr char xml1[] = R"(
<mujoco>
<geom name="box" type="box" size="1 1 1"/>
</mujoco>)";
static constexpr char xml2[]= R"(
<mujoco>
<geom name="ball" type="sphere" size="2"/>
<include file="subsubmodels/model3.xml"/>
</mujoco>)";
static constexpr char xml3[]= R"(
<mujoco>
<geom name="another_box" type="box" size="2 2 2"/>
</mujoco>)";
MockFilesystem fs("IncludePathTest");
fs.AddFile("model.xml", (const unsigned char*) xml, sizeof(xml));
std::string modelpath = fs.FullPath("model.xml");
fs.ChangeDirectory("submodels/");
fs.AddFile("model1.xml", (const unsigned char*) xml1, sizeof(xml1));
fs.AddFile("model2.xml", (const unsigned char*) xml2, sizeof(xml2));
fs.AddFile("subsubmodels/model3.xml", (const unsigned char*) xml3, sizeof(xml3));
fs.ChangeDirectory("/");
mjModel* model = mj_loadXML(modelpath.c_str(), nullptr,
nullptr, 0);
ASSERT_THAT(model, NotNull());
EXPECT_EQ(mj_name2id(model, mjOBJ_GEOM, "ball"), 2);
EXPECT_EQ(mj_name2id(model, mjOBJ_GEOM, "another_box"), 3);
mj_deleteModel(model);
}
TEST_F(XMLReaderTest, FallbackIncludePathTest) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<geom name="plane" type="plane" size="1 1 1"/>
<include file="model1.xml"/>
<include file="submodels/model2.xml"/>
</worldbody>
</mujoco>)";
static constexpr char xml1[] = R"(
<mujoco>
<geom name="box" type="box" size="1 1 1"/>
</mujoco>)";
static constexpr char xml2[]= R"(
<mujoco>
<geom name="ball" type="sphere" size="2"/>
<include file="subsubmodels/model3.xml"/>
</mujoco>)";
static constexpr char xml3[]= R"(
<mujoco>
<geom name="another_box" type="box" size="2 2 2"/>
</mujoco>)";
MockFilesystem fs("FallbackIncludePathTest");
fs.AddFile("model.xml", (const unsigned char*) xml, sizeof(xml));
std::string modelpath = fs.FullPath("model.xml");
fs.AddFile("model1.xml", (const unsigned char*) xml1, sizeof(xml1));
fs.AddFile("submodels/model2.xml", (const unsigned char*) xml2, sizeof(xml2));
fs.AddFile("subsubmodels/model3.xml", (const unsigned char*) xml3,
sizeof(xml3));
std::array<char, 1024> error;
mjModel* model = mj_loadXML(modelpath.c_str(), nullptr,
error.data(), error.size());
ASSERT_THAT(model, NotNull());
EXPECT_EQ(mj_name2id(model, mjOBJ_GEOM, "ball"), 2);
EXPECT_EQ(mj_name2id(model, mjOBJ_GEOM, "another_box"), 3);
mj_deleteModel(model);
}
TEST_F(XMLReaderTest, IncludeAssetsTest) {
static constexpr char xml[] = R"(
<mujoco>
<include file="assets/assets.xml"/>
<worldbody>
<geom type="plane" material="material" size="4 4 4"/>
</worldbody>
</mujoco>
)";
static constexpr char assets[] = R"(
<mujoco>
<asset>
<texture file="tiny.png" type="2d"/>
<material name="material" texture="tiny"/>
<include file="subassets/assets.xml"/>
</asset>
</mujoco>
)";
static constexpr char subassets[] = R"(
<mujoco>
<texture file="subtiny.png" type="2d"/>
<material name="submaterial" texture="subtiny"/>
</mujoco>
)";
MockFilesystem fs("IncludeAssetsTest");
fs.AddFile("assets/tiny.png", kTinyPng, sizeof(kTinyPng));
fs.AddFile("assets/subassets/subtiny.png", kTinyPng, sizeof(kTinyPng));
fs.AddFile("assets/assets.xml", (const unsigned char*) assets,
sizeof(assets));
fs.AddFile("assets/subassets/assets.xml", (const unsigned char*) subassets,
sizeof(subassets));
fs.AddFile("model.xml", (const unsigned char*) xml, sizeof(xml));
std::string modelpath = fs.FullPath("model.xml");
// loading the file should be successful
mjModel* model = mj_loadXML(modelpath.c_str(), nullptr, nullptr, 0);
EXPECT_THAT(model, NotNull());
mj_deleteModel(model);
}
TEST_F(XMLReaderTest, FallbackIncludeAssetsTest) {
static constexpr char xml[] = R"(
<mujoco>
<include file="assets/assets.xml"/>
<worldbody>
<geom type="plane" material="material" size="4 4 4"/>
</worldbody>
</mujoco>
)";
static constexpr char assets[] = R"(
<mujoco>
<asset>
<texture file="tiny.png" type="2d"/>
<material name="material" texture="tiny"/>
<include file="subassets/assets.xml"/>
</asset>
</mujoco>
)";
static constexpr char subassets[] = R"(
<mujoco>
<texture file="subtiny.png" type="2d"/>
<material name="submaterial" texture="subtiny"/>
</mujoco>
)";
MockFilesystem fs("FallbackIncludeAssetsTest");
fs.AddFile("assets/tiny.png", kTinyPng, sizeof(kTinyPng));
// need to fallback for backwards compatibility
fs.AddFile("subtiny.png", kTinyPng, sizeof(kTinyPng));
fs.AddFile("assets/assets.xml", (const unsigned char*) assets,
sizeof(assets));
fs.AddFile("assets/subassets/assets.xml", (const unsigned char*) subassets,
sizeof(subassets));
fs.AddFile("model.xml", (const unsigned char*) xml, sizeof(xml));
std::string modelpath = fs.FullPath("model.xml");
// loading the file should be successful
std::array<char, 1024> error;
mjModel* model = mj_loadXML(modelpath.c_str(), nullptr,
error.data(), error.size());
EXPECT_THAT(model, NotNull());
mj_deleteModel(model);
}
TEST_F(XMLReaderTest, IncludeAbsoluteTest) {
static constexpr char xml[] = R"(
<mujoco>
<include file="assets/assets.xml"/>
<worldbody>
<geom type="plane" material="material" size="4 4 4"/>
</worldbody>
</mujoco>
)";
static constexpr char assets[] = R"(
<mujoco>
<asset>
<texture file="tiny.png" type="2d"/>
<material name="material" texture="tiny"/>
<include file="subassets/assets.xml"/>
</asset>
</mujoco>
)";
static constexpr char subassets[] = R"(
<mujoco>
<texture file="MjMock.IncludeAbsoluteTest:assets/subtiny.png" type="2d"/>
<material name="submaterial" texture="subtiny"/>
</mujoco>
)";
MockFilesystem fs("IncludeAbsoluteTest");
fs.AddFile("assets/tiny.png", kTinyPng, sizeof(kTinyPng));
fs.AddFile("assets/subtiny.png", kTinyPng, sizeof(kTinyPng));
fs.AddFile("assets/assets.xml", (const unsigned char*) assets,
sizeof(assets));
fs.AddFile("assets/subassets/assets.xml", (const unsigned char*) subassets,
sizeof(subassets));
fs.AddFile("model.xml", (const unsigned char*) xml, sizeof(xml));
std::string modelpath = fs.FullPath("model.xml");
std::array<char, 1024> error;
// loading the file should be successful
mjModel* model = mj_loadXML(modelpath.c_str(), nullptr,
error.data(), error.size());
EXPECT_THAT(model, NotNull());
mj_deleteModel(model);
}
// ------------------------ test frame parsing ---------------------------------
TEST_F(XMLReaderTest, ParseFrame) {
static constexpr char xml[] = R"(
<mujoco>
@@ -1040,10 +1301,10 @@ TEST_F(ActuatorParseTest, PositionIntvelocityVelocityDefaultsPropagate) {
<mujoco>
<default>
<default class="position">
<position kp="3" kv="4"/>
<position kp="3" kv="4" inheritrange="2"/>
</default>
<default class="intvelocity">
<intvelocity kp="5" kv="6" actrange="-1 1"/>
<intvelocity kp="5" kv="6" inheritrange="0.5"/>
</default>
<default class="velocity">
<velocity kv="7"/>
@@ -1052,7 +1313,7 @@ TEST_F(ActuatorParseTest, PositionIntvelocityVelocityDefaultsPropagate) {
<worldbody>
<body>
<geom size="1"/>
<joint name="jnt" type="slide" axis="1 0 0"/>
<joint name="jnt" type="slide" axis="1 0 0" range="0 2"/>
</body>
</worldbody>
<actuator>
@@ -1082,6 +1343,10 @@ TEST_F(ActuatorParseTest, PositionIntvelocityVelocityDefaultsPropagate) {
EXPECT_EQ(model->actuator_gainprm[i*mjNGAIN + j], 0.0);
}
}
EXPECT_EQ(model->actuator_ctrlrange[0*2 + 0], -1.0);
EXPECT_EQ(model->actuator_ctrlrange[0*2 + 1], 3.0);
EXPECT_EQ(model->actuator_actrange[1*2 + 0], 0.5);
EXPECT_EQ(model->actuator_actrange[1*2 + 1], 1.5);
mj_deleteModel(model);
}
test/xml/xml_native_writer_test.cc
+118
View File
@@ -694,6 +694,38 @@ TEST_F(XMLWriterTest, WritesDefaults) {
mj_deleteModel(model);
}
TEST_F(XMLWriterTest, WritesActuatorDefaults) {
static constexpr char xml[] = R"(
<mujoco>
<default>
<default class="position">
<position kp="3" kv="4" />
</default>
<default class="intvelocity">
<intvelocity kp="5" kv="6" />
</default>
</default>
<worldbody>
<body>
<geom size="1"/>
<joint name="jnt" type="slide" axis="1 0 0" range="0 2"/>
</body>
</worldbody>
<actuator>
<position joint="jnt" class="position"/>
<intvelocity joint="jnt" actrange="-1 1"/>
</actuator>
</mujoco>
)";
mjModel* model = LoadModelFromString(xml);
ASSERT_THAT(model, NotNull());
std::string saved_xml = SaveAndReadXml(model);
EXPECT_THAT(saved_xml, Not(HasSubstr("mass")));
EXPECT_THAT(saved_xml, HasSubstr(
"<general biastype=\"affine\" gainprm=\"3\""));
mj_deleteModel(model);
}
TEST_F(XMLWriterTest, WritesDensity) {
static constexpr char xml[] = R"(
<mujoco>
@@ -967,6 +999,92 @@ TEST_F(XMLWriterTest, ActdimDefaults) {
mj_deleteModel(model);
}
TEST_F(XMLWriterTest, TrimsDefaults) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<geom size="1" friction="0.9" solref="0.1" solimp="0.1 0.2"/>
<joint name="jnt" type="slide" axis="1 0 0" range="0 2"/>
</body>
</worldbody>
<actuator>
<position joint="jnt" kp="3" kv="4"/>
<damper joint="jnt" kv="10" ctrlrange="0 1"/>
</actuator>
</mujoco>
)";
mjModel* model = LoadModelFromString(xml);
ASSERT_THAT(model, NotNull());
std::string saved_xml = SaveAndReadXml(model);
EXPECT_THAT(saved_xml, HasSubstr("friction=\"0.9\""));
EXPECT_THAT(saved_xml, HasSubstr("solref=\"0.1\""));
EXPECT_THAT(saved_xml, HasSubstr("solimp=\"0.1 0.2\""));
EXPECT_THAT(saved_xml, HasSubstr("gainprm=\"3\" biasprm=\"0 -3 -4\""));
EXPECT_THAT(saved_xml, HasSubstr("gainprm=\"0 0 -10\""));
mj_deleteModel(model);
}
TEST_F(XMLWriterTest, InheritrangeSavedAsRange) {
static constexpr char xml[] = R"(
<mujoco>
<compiler angle="radian"/>
<worldbody>
<body>
<geom size="1"/>
<joint name="slide" type="slide" range="0 2"/>
</body>
<body>
<geom size="1"/>
<joint name="hinge" type="hinge" range="-2 0"/>
</body>
</worldbody>
<actuator>
<position joint="slide" inheritrange="2"/>
<intvelocity joint="hinge" inheritrange="0.5"/>
</actuator>
</mujoco>
)";
mjModel* model = LoadModelFromString(xml);
ASSERT_THAT(model, NotNull());
std::string saved_xml = SaveAndReadXml(model);
EXPECT_THAT(saved_xml, HasSubstr("ctrlrange=\"-1 3\""));
EXPECT_THAT(saved_xml, HasSubstr("actrange=\"-1.5 -0.5\""));
mj_deleteModel(model);
}
TEST_F(XMLWriterTest, InheritedInheritrangeSavedAsRange) {
static constexpr char xml[] = R"(
<mujoco>
<default>
<default class="position">
<position kp="3" kv="4" inheritrange="2"/>
</default>
<default class="intvelocity">
<intvelocity kp="5" kv="6" inheritrange="0.5"/>
</default>
</default>
<worldbody>
<body>
<geom size="1"/>
<joint name="jnt" type="slide" range="0 2"/>
</body>
</worldbody>
<actuator>
<position joint="jnt" class="position"/>
<intvelocity joint="jnt" class="intvelocity"/>
</actuator>
</mujoco>
)";
mjModel* model = LoadModelFromString(xml);
ASSERT_THAT(model, NotNull());
std::string saved_xml = SaveAndReadXml(model);
EXPECT_THAT(saved_xml, Not(HasSubstr("inheritrange")));
EXPECT_THAT(saved_xml, HasSubstr("ctrlrange=\"-1 3\""));
EXPECT_THAT(saved_xml, HasSubstr("actrange=\"0.5 1.5\""));
mj_deleteModel(model);
}
// check that no precision is lost when saving XMLs with FullFloatPrecision
TEST_F(XMLWriterTest, SetPrecision) {
static constexpr char xml[] = R"(
test/xml/xml_schema_test.cc
+135
View File
@@ -0,0 +1,135 @@
// Copyright 2024 DeepMind Technologies Limited
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <optional>
#include <string>
#include <gtest/gtest.h>
#include "src/xml/xml_native_reader.h"
#include "src/xml/xml_util.h"
#include "test/fixture.h"
#include <absl/strings/numbers.h>
#include <absl/strings/str_format.h>
namespace mujoco {
namespace {
using XMLSchemaTest = MujocoTest;
static std::optional<std::string> IsValidSchemaFormat(
const char* schema[][mjXATTRNUM], unsigned nrow) {
if (schema[0][0][0] == '<' || schema[0][0][0] == '>') {
return "expected element, found bracket";
}
// check entire schema for null pointers
for (int i = 0; i < nrow; i++) {
// base pointers
if (!schema[i][0]) {
return "null pointer found in row " + std::to_string(i);
}
// detect element
if (schema[i][0][0] != '<' && schema[i][0][0] != '>') {
// first 3 pointers required
if (!schema[i][1] || !schema[i][2]) {
return absl::StrFormat("expected element, found null pointers"
"in row %d, element %s", i, schema[i][0]);
}
// check type
if (schema[i][1][0] != '!' && schema[i][1][0] != '?' &&
schema[i][1][0] != '*' && schema[i][1][0] != 'R') {
return absl::StrFormat("invalid type in row %d, element %s",
i, schema[i][0]);
}
// number of attributes
int nattr = 0;
if (!absl::SimpleAtoi(schema[i][2], &nattr)) {
return absl::StrFormat("unparseable number of attributes in"
" row %d, element %s", i, schema[i][0]);
} else if (nattr < 0 || nattr > mjXATTRNUM - 3) {
return absl::StrFormat("invalid number of attributes in"
" row %d, element %s", i, schema[i][0]);
}
// attribute pointers
for (int j = 0; j < nattr; j++) {
if (!schema[i][3 + j]) {
return absl::StrFormat("null attribute %d in"
" row %d, element %s", j, i, schema[i][0]);
}
}
}
}
// process sub-elements of complex element
if (nrow > 1) {
// check for bracketed block
if (schema[1][0][0] != '<' || schema[nrow - 1][0][0] != '>') {
return "expected brackets after complex element";
}
// parse block into simple and complex elements
int start = 2;
while (start < nrow - 1) {
int end = start;
// look for bracketed block at start + 1
if (schema[start + 1][0][0] == '<') {
// look for corresponding closing bracket
int cnt = 0;
while (end <= nrow - 1) {
if (schema[end][0][0] == '<') {
cnt++;
} else if (schema[end][0][0] == '>') {
cnt--;
if (cnt == 0) {
break;
}
}
end++;
}
// closing bracket not found
if (end > nrow - 1) {
return "matching closing bracket not found";
}
}
// recursively check for error
auto error = IsValidSchemaFormat(schema + start, end - start + 1);
if (error.has_value()) {
return error;
}
// proceed with next subelement
start = end + 1;
}
}
return std::nullopt;
}
TEST_F(XMLSchemaTest, MjcfSchemaTest) {
auto error = IsValidSchemaFormat(MJCF, nMJCF);
auto error_msg = error.value_or("");
EXPECT_EQ(error_msg, "");
ASSERT_FALSE(error.has_value());
}
} // namespace
} // namespace mujoco
unity/Runtime/Bindings/MjBindings.cs
+7 -3
View File
@@ -4938,6 +4938,7 @@ public unsafe struct _mjVFS
[MarshalAs(UnmanagedType.ByValArray, SizeConst = 2000 * 1000)] public char[] filename;
[MarshalAs(UnmanagedType.ByValArray, SizeConst = 2000)] public UIntPtr[] filesize;
[MarshalAs(UnmanagedType.ByValArray, SizeConst = 2000)] public IntPtr[] filedata;
[MarshalAs(UnmanagedType.ByValArray, SizeConst = 2000)] public UInt64[] filestamp;
}
[StructLayout(LayoutKind.Sequential)]
@@ -5365,8 +5366,6 @@ public unsafe struct mjModel_ {
public int* mesh_texcoordadr;
public int* mesh_texcoordnum;
public int* mesh_graphadr;
public double* mesh_pos;
public double* mesh_quat;
public float* mesh_vert;
public float* mesh_normal;
public float* mesh_texcoord;
@@ -5374,6 +5373,8 @@ public unsafe struct mjModel_ {
public int* mesh_facenormal;
public int* mesh_facetexcoord;
public int* mesh_graph;
public double* mesh_pos;
public double* mesh_quat;
public int* mesh_pathadr;
public int* skin_matid;
public int* skin_group;
@@ -6321,7 +6322,7 @@ public static unsafe extern void mj_defaultVFS(void* vfs);
public static unsafe extern int mj_addFileVFS(void* vfs, [MarshalAs(UnmanagedType.LPStr)]string directory, [MarshalAs(UnmanagedType.LPStr)]string filename);
[DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)]
public static unsafe extern int mj_makeEmptyFileVFS(void* vfs, [MarshalAs(UnmanagedType.LPStr)]string filename, int filesize);
public static unsafe extern int mj_addBufferVFS(void* vfs, [MarshalAs(UnmanagedType.LPStr)]string name, void* buffer, int nbuffer);
[DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)]
public static unsafe extern int mj_findFileVFS(void* vfs, [MarshalAs(UnmanagedType.LPStr)]string filename);
@@ -6332,6 +6333,9 @@ public static unsafe extern int mj_deleteFileVFS(void* vfs, [MarshalAs(Unmanaged
[DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)]
public static unsafe extern void mj_deleteVFS(void* vfs);
[DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)]
public static unsafe extern int mj_makeEmptyFileVFS(void* vfs, [MarshalAs(UnmanagedType.LPStr)]string filename, int filesize);
[DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)]
public static unsafe extern mjModel_* mj_loadXML([MarshalAs(UnmanagedType.LPStr)]string filename, void* vfs, StringBuilder error, int error_sz);