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, 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. 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: .. _mj_findFileVFS:
@@ -1145,6 +1145,15 @@ mj_deleteVFS
Delete all files from VFS. Delete all files from VFS.
.. _mj_makeEmptyFileVFS:
mj_makeEmptyFileVFS
~~~~~~~~~~~~~~~~~~~
.. mujoco-include:: mj_makeEmptyFileVFS
deprecated: use mj_copyBufferVFS.
.. _Initialization: .. _Initialization:
Initialization Initialization
@@ -1280,7 +1289,7 @@ mj_resetDataKeyframe
.. mujoco-include:: 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: .. _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 .. code-block:: xml
<asset> <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> </asset>
Positioning and orienting is complicated by the fact that vertex data are often designed relative to coordinate frames Positioning and orienting is complicated by the fact that vertex data in the source asset are often relative to
whose origin is not inside the mesh. In contrast, MuJoCo expects the origin of a geom's local frame to coincide with the coordinate frames whose origin is not inside the mesh. In contrast, MuJoCo expects the origin of a geom's local frame to
geometric center of the shape. We resolve this discrepancy by pre-processing the mesh in the compiler, so that it is coincide with the geometric center of the shape. We resolve this discrepancy by pre-processing the mesh in the compiler,
centered around (0,0,0) and its principal axes of inertia are the coordinate axes. We also save the translation and so that it is centered around (0,0,0) and its principal axes of inertia are the coordinate axes. We save the translation
rotation offsets needed to achieve such alignment in :ref:`mjModel.mesh_pos<mjModel>` and and rotation offsets applied to the source asset 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 :ref:`mjModel.mesh_quat<mjModel>`; these are required if one reads vertex data from the source and needs to re-apply the
also :at:`mesh` attribute of :ref:`geom <body-geom>` below. Fortunately most meshes used in robot models are designed in transform. These offsets are then composed with the referencing geom's position and orientation; see also the :at:`mesh`
a coordinate frame centered at the joint. This makes the corresponding MJCF model intuitive: we set the body frame at the attribute of :ref:`geom <body-geom>` below. Fortunately most meshes used in robot models are designed in a coordinate
joint, so that the joint position is (0,0,0) in the body frame, and simply reference the mesh. Below is an MJCF model frame centered at the joint. This makes the corresponding MJCF model intuitive: we set the body frame at the joint, so
fragment of a forearm, containing all the information needed to put the mesh where one would expect it to be. The body that the joint position is (0,0,0) in the body frame, and simply reference the mesh. Below is an MJCF model fragment of
position is specified relative to the parent body, namely the upper arm (not shown). It is offset by 35 cm which is the a forearm, containing all the information needed to put the mesh where one would expect it to be. The body position is
typical length of the human upper arm. If the mesh vertex data were not designed in the above convention, we would have specified relative to the parent body, namely the upper arm (not shown). It is offset by 35 cm which is the typical
to use the geom position and orientation (or the new refpos, refquat mechanism) to compensate, but in practice this is 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. rarely needed.
.. code-block:: xml .. code-block:: xml
<asset> <asset>
<mesh file="forearm.stl"/> <mesh file="forearm.stl"/>
</asset> </asset>
<body pos="0 0 0.35"/> <body pos="0 0 0.35"/>
<joint type="hinge" axis="1 0 0"/> <joint type="hinge" axis="1 0 0"/>
<geom type="mesh" mesh="forearm"/> <geom type="mesh" mesh="forearm"/>
</body> </body>
The inertial computation mentioned above is part of an algorithm used not only to center and align the mesh, but also to 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 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 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 (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 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. 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: 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. Damping applied by the actuator.
When using this attribute, it is recommended to use the implicitfast or implicit :ref:`integrators<geIntegration>`. 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: .. _actuator-velocity:
:el-prefix:`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. Damping applied by the actuator.
When using this attribute, it is recommended to use the implicitfast or implicit :ref:`integrators<geIntegration>`. 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: .. _actuator-damper:
:el-prefix:`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 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. * 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-ctrlrange:
.. _default-position-inheritrange:
.. _default-position-forcerange: .. _default-position-forcerange:
.. _default-position-gear: .. _default-position-gear:
@@ -7787,6 +7810,8 @@ refsite, tendon, slidersite, cranksite.
.. _default-intvelocity-actrange: .. _default-intvelocity-actrange:
.. _default-intvelocity-inheritrange:
.. _default-intvelocity-gear: .. _default-intvelocity-gear:
.. _default-intvelocity-cranklength: .. _default-intvelocity-cranklength:
doc/XMLschema.rst
+41 -39
View File
@@ -135,32 +135,6 @@
| :ref:`asset<asset>` | | | | :ref:`asset<asset>` | | |
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
| |_| asset |br| |_| |L| | | .. table:: | | |_| 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 | | :ref:`mesh | \* | :class: mjcf-attributes |
| <asset-mesh>` | | | | <asset-mesh>` | | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
@@ -186,6 +160,15 @@
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
| |_| asset |br| |_| |L| | | .. table:: | | |_| 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 | | :ref:`skin | \* | :class: mjcf-attributes |
| <asset-skin>` | | | | <asset-skin>` | | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
@@ -206,6 +189,23 @@
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
| |_| asset |br| |_| |L| | | .. table:: | | |_| 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 | | :ref:`material | \* | :class: mjcf-attributes |
| <asset-material>` | | | | <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:`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:: | | |_| 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:`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:: | | |_| actuator |br| |_| |L| | | .. table:: |
@@ -1440,11 +1442,11 @@
| :ref:`position | ? | :class: mjcf-attributes | | :ref:`position | ? | :class: mjcf-attributes |
| <default-position>` | | | | <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:: | | |_| 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:`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:: | | |_| default |br| |_| |L| | | .. table:: |
doc/changelog.rst
+23 -2
View File
@@ -5,17 +5,38 @@ Changelog
Upcoming version (not yet released) 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 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. - 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. - Avoid reallocating host ``mjData`` arrays when array shapes are unchanged.
- Speed up calculation of ``mjx.ncon`` for models with many geoms. - 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``. - 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. ``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) Version 3.1.2 (February 05, 2024)
----------------------------------- -----------------------------------
doc/conf.py
+2 -2
View File
@@ -118,7 +118,7 @@ SHARED_CSS_VARIABLES = {
html_theme_options = { html_theme_options = {
'light_css_variables': { 'light_css_variables': {
'font-stack--monospace': 'Inconsolata,Consolas,ui-monospace,monospace', 'font-stack--monospace': 'Inconsolata,Consolas,ui-monospace,monospace',
'at-color': '#bc103e', 'at-color': '#830b2b',
'at-val-color': '#bc103e', 'at-val-color': '#bc103e',
'body-color': '#14234b', 'body-color': '#14234b',
'color-highlight-on-target': '#e5e8ed', 'color-highlight-on-target': '#e5e8ed',
@@ -131,7 +131,7 @@ html_theme_options = {
'wy-nav-side-background-color': '#0053d6', 'wy-nav-side-background-color': '#0053d6',
}, },
'dark_css_variables': { 'dark_css_variables': {
'at-color': '#ff95a6', 'at-color': '#ffaab7',
'at-val-color': '#ff95a6', 'at-val-color': '#ff95a6',
'body-color': '#14234b', 'body-color': '#14234b',
'color-admonition-background': '#1e1e21', '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) mjtNum tolrange; // convergence tolerance (relative to range)
}; };
typedef struct mjLROpt_ mjLROpt; typedef struct mjLROpt_ mjLROpt;
struct mjVFS_ { // virtual file system for loading from memory struct mjVFS_ { // virtual file system for loading from memory
int nfile; // number of files present int nfile; // number of files present
char filename[mjMAXVFS][mjMAXVFSNAME]; // file name without path char filename[mjMAXVFS][mjMAXVFSNAME]; // file name without path
size_t filesize[mjMAXVFS]; // file size in bytes size_t filesize[mjMAXVFS]; // file size in bytes
void* filedata[mjMAXVFS]; // buffer with file data void* filedata[mjMAXVFS]; // buffer with file data
uint64_t filestamp[mjMAXVFS]; // checksum of the file data
}; };
typedef struct mjVFS_ mjVFS; typedef struct mjVFS_ mjVFS;
struct mjOption_ { // physics options struct mjOption_ { // physics options
@@ -1154,8 +1155,6 @@ struct mjModel_ {
int* mesh_texcoordadr; // texcoord data address; -1: no texcoord (nmesh x 1) int* mesh_texcoordadr; // texcoord data address; -1: no texcoord (nmesh x 1)
int* mesh_texcoordnum; // number of 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) 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_vert; // vertex positions for all meshes (nmeshvert x 3)
float* mesh_normal; // normals for all meshes (nmeshnormal x 3) float* mesh_normal; // normals for all meshes (nmeshnormal x 3)
float* mesh_texcoord; // vertex texcoords for all meshes (nmeshtexcoord x 2) 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_facenormal; // normal face data (nmeshface x 3)
int* mesh_facetexcoord; // texture face data (nmeshface x 3) int* mesh_facetexcoord; // texture face data (nmeshface x 3)
int* mesh_graph; // convex graph data (nmeshgraph x 1) 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) int* mesh_pathadr; // address of asset path for mesh; -1: none (nmesh x 1)
// skins // skins
@@ -1375,6 +1376,7 @@ typedef struct mjModel_ mjModel;
struct mjResource_ { struct mjResource_ {
char* name; // name of resource (filename, etc) char* name; // name of resource (filename, etc)
void* data; // opaque data pointer void* data; // opaque data pointer
char timestamp[512]; // timestamp of the resource
const struct mjpResourceProvider* provider; // pointer to the provider const struct mjpResourceProvider* provider; // pointer to the provider
}; };
typedef struct mjResource_ mjResource; typedef struct mjResource_ mjResource;
@@ -2406,10 +2408,11 @@ typedef struct mjvSceneState_ mjvSceneState;
//----------------------------- MJAPI FUNCTIONS -------------------------------- //----------------------------- MJAPI FUNCTIONS --------------------------------
void mj_defaultVFS(mjVFS* vfs); void mj_defaultVFS(mjVFS* vfs);
int mj_addFileVFS(mjVFS* vfs, const char* directory, const char* filename); 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_findFileVFS(const mjVFS* vfs, const char* filename);
int mj_deleteFileVFS(mjVFS* vfs, const char* filename); int mj_deleteFileVFS(mjVFS* vfs, const char* filename);
void mj_deleteVFS(mjVFS* vfs); 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); 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); int mj_saveLastXML(const char* filename, const mjModel* m, char* error, int error_sz);
void mj_freeLastXML(void); void mj_freeLastXML(void);
include/mujoco/mjmodel.h
+9 -7
View File
@@ -16,6 +16,7 @@
#define MUJOCO_MJMODEL_H_ #define MUJOCO_MJMODEL_H_
#include <stddef.h> #include <stddef.h>
#include <stdint.h>
#include <mujoco/mjtnum.h> #include <mujoco/mjtnum.h>
@@ -390,11 +391,12 @@ typedef struct mjLROpt_ mjLROpt;
//---------------------------------- mjVFS --------------------------------------------------------- //---------------------------------- mjVFS ---------------------------------------------------------
struct mjVFS_ { // virtual file system for loading from memory struct mjVFS_ { // virtual file system for loading from memory
int nfile; // number of files present int nfile; // number of files present
char filename[mjMAXVFS][mjMAXVFSNAME]; // file name without path char filename[mjMAXVFS][mjMAXVFSNAME]; // file name without path
size_t filesize[mjMAXVFS]; // file size in bytes size_t filesize[mjMAXVFS]; // file size in bytes
void* filedata[mjMAXVFS]; // buffer with file data void* filedata[mjMAXVFS]; // buffer with file data
uint64_t filestamp[mjMAXVFS]; // checksum of the file data
}; };
typedef struct mjVFS_ mjVFS; typedef struct mjVFS_ mjVFS;
@@ -868,8 +870,6 @@ struct mjModel_ {
int* mesh_texcoordadr; // texcoord data address; -1: no texcoord (nmesh x 1) int* mesh_texcoordadr; // texcoord data address; -1: no texcoord (nmesh x 1)
int* mesh_texcoordnum; // number of 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) 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_vert; // vertex positions for all meshes (nmeshvert x 3)
float* mesh_normal; // normals for all meshes (nmeshnormal x 3) float* mesh_normal; // normals for all meshes (nmeshnormal x 3)
float* mesh_texcoord; // vertex texcoords for all meshes (nmeshtexcoord x 2) 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_facenormal; // normal face data (nmeshface x 3)
int* mesh_facetexcoord; // texture face data (nmeshface x 3) int* mesh_facetexcoord; // texture face data (nmeshface x 3)
int* mesh_graph; // convex graph data (nmeshgraph x 1) 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) int* mesh_pathadr; // address of asset path for mesh; -1: none (nmesh x 1)
// skins // skins
include/mujoco/mjplugin.h
+8 -5
View File
@@ -26,11 +26,12 @@
struct mjResource_ { struct mjResource_ {
char* name; // name of resource (filename, etc) char* name; // name of resource (filename, etc)
void* data; // opaque data pointer void* data; // opaque data pointer
char timestamp[512]; // timestamp of the resource
const struct mjpResourceProvider* provider; // pointer to the provider const struct mjpResourceProvider* provider; // pointer to the provider
}; };
typedef struct mjResource_ mjResource; 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); typedef int (*mjfOpenResource)(mjResource* resource);
// callback for reading a 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 // sets dir to directory string with ndir being size of directory string
typedef void (*mjfGetResourceDir)(mjResource* resource, const char** dir, int* ndir); typedef void (*mjfGetResourceDir)(mjResource* resource, const char** dir, int* ndir);
// callback for checking if a resource was modified since last read // callback for checking if the current resource was modified from the time
// returns > 0 if resource was modified since last open, 0 if resource was not // specified by the timestamp
// modified, and < 0 if inconclusive // returns 0 if the resource's timestamp matches the provided timestamp
typedef int (*mjfResourceModified)(const mjResource* resource); // 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 describing a single resource provider
struct mjpResourceProvider { 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. // 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); 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. // Add file to VFS from buffer, return 0: success, 1: full, 2: repeated name, -1: failed to load.
MJAPI int mj_makeEmptyFileVFS(mjVFS* vfs, const char* filename, int filesize); 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. // Return file index in VFS, or -1 if not found in VFS.
MJAPI int mj_findFileVFS(const mjVFS* vfs, const char* filename); 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. // Delete all files from VFS.
MJAPI void mj_deleteVFS(mjVFS* 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 --------------------------------------------- //---------------------------------- 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. // Reset data to defaults, fill everything else with debug_value.
MJAPI void mj_resetDataDebug(const mjModel* m, mjData* d, unsigned char 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); MJAPI void mj_resetDataKeyframe(const mjModel* m, mjData* d, int key);
// Mark a new frame on the mjData stack. // 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 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( FunctionDecl(
name='mj_makeEmptyFileVFS', name='mj_addBufferVFS',
return_type=ValueType(name='int'), return_type=ValueType(name='int'),
parameters=( parameters=(
FunctionParameterDecl( FunctionParameterDecl(
@@ -78,17 +78,23 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([
), ),
), ),
FunctionParameterDecl( FunctionParameterDecl(
name='filename', name='name',
type=PointerType( type=PointerType(
inner_type=ValueType(name='char', is_const=True), inner_type=ValueType(name='char', is_const=True),
), ),
), ),
FunctionParameterDecl( FunctionParameterDecl(
name='filesize', name='buffer',
type=PointerType(
inner_type=ValueType(name='void', is_const=True),
),
),
FunctionParameterDecl(
name='nbuffer',
type=ValueType(name='int'), 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', ('mj_findFileVFS',
FunctionDecl( FunctionDecl(
@@ -144,6 +150,30 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([
), ),
doc='Delete all files from VFS.', 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', ('mj_loadXML',
FunctionDecl( FunctionDecl(
name='mj_loadXML', name='mj_loadXML',
@@ -675,7 +705,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([
type=ValueType(name='int'), 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', ('mj_markStack',
FunctionDecl( FunctionDecl(
introspect/structs.py
+22 -14
View File
@@ -121,6 +121,14 @@ STRUCTS: Mapping[str, StructDecl] = dict([
), ),
doc='buffer with file data', 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', ('mjOption',
@@ -2591,20 +2599,6 @@ STRUCTS: Mapping[str, StructDecl] = dict([
), ),
doc='graph data address; -1: no graph (nmesh x 1)', 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( StructFieldDecl(
name='mesh_vert', name='mesh_vert',
type=PointerType( type=PointerType(
@@ -2654,6 +2648,20 @@ STRUCTS: Mapping[str, StructDecl] = dict([
), ),
doc='convex graph data (nmeshgraph x 1)', # pylint: disable=line-too-long 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( StructFieldDecl(
name='mesh_pathadr', name='mesh_pathadr',
type=PointerType( type=PointerType(
mjx/mujoco/mjx/_src/collision_convex.py
+2 -1
View File
@@ -173,7 +173,7 @@ def _manifold_points(
bp = b - poly bp = b - poly
dist_bp = jp.abs(bp.dot(bc)) + dist_mask dist_bp = jp.abs(bp.dot(bc)) + dist_mask
dist_ap = jp.abs(ap.dot(ac)) + 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]) 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) frame = jp.stack([math.make_frame(n)] * 4, axis=0)
unique = jp.tril(idx == idx[:, None]).sum(axis=1) == 1 unique = jp.tril(idx == idx[:, None]).sum(axis=1) == 1
dist = jp.where(unique, -support[idx], 1) dist = jp.where(unique, -support[idx], 1)
pos = pos - 0.5 * dist[:, None] * n
return dist, pos, frame 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> <worldbody>
<geom size="40 40 40" type="plane"/> <geom size="40 40 40" type="plane"/>
<body pos="0 0 0.7" euler="45 0 0"> <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"/> <geom size="0.5 0.5 0.5" type="box"/>
</body> </body>
</worldbody> </worldbody>
@@ -281,7 +281,35 @@ class ConvexTest(absltest.TestCase):
# extract the contact points with penetration # extract the contact points with penetration
c = jax.tree_map(lambda x: jp.take(x, jp.array([0, 1]), axis=0), dx.contact) c = jax.tree_map(lambda x: jp.take(x, jp.array([0, 1]), axis=0), dx.contact)
for field in dataclasses.fields(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 = """ _BOX_BOX = """
<mujoco> <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 diff_tan, friction in zip(diff_con[1:], c.friction[:2]):
for f in (friction, -friction): for f in (friction, -friction):
js.append(diff_con[0] + diff_tan * f) 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 active = dist < 0
j, invweight = jp.stack(js) * active, jp.stack(invweights) 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 dataclasses
import typing import typing
from typing import Any, Dict, Optional, Sequence, TypeVar from typing import Any, Dict, Optional, Sequence, TypeVar, Union
import jax import jax
import numpy as np import numpy as np
_T = TypeVar('_T') _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: def dataclass(clz: _T) -> _T:
"""Wraps a dataclass with metadata for which fields are pytrees. """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) data_clz = dataclasses.dataclass(frozen=True)(clz)
meta_fields, data_fields = [], [] meta_fields, data_fields = [], []
for field in dataclasses.fields(data_clz): for field in dataclasses.fields(data_clz):
if any(( if _jax_in_args(field.type):
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)),
)):
data_fields.append(field) data_fields.append(field)
else: else:
meta_fields.append(field) 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 # integrate damping implicitly
qacc = d.qacc qacc = d.qacc
if not m.opt.disableflags & DisableBit.EULERDAMP: if not m.opt.disableflags & DisableBit.EULERDAMP:
# TODO(robotics-simulation): can this be done with a smaller perf hit if support.is_sparse(m):
dh = d.replace(qM=d.qM.at[m.dof_Madr].add(m.opt.timestep * m.dof_damping)) 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) dh = smooth.factor_m(m, dh)
qfrc = d.qfrc_smooth + d.qfrc_constraint qfrc = d.qfrc_smooth + d.qfrc_constraint
qacc = smooth.solve_m(m, dh, qfrc) 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 _TOLERANCE = 1e-5
def _assert_eq(a, b, name): def _assert_eq(a, b, name, tol=_TOLERANCE):
tol = _TOLERANCE * 10 # avoid test noise tol = tol * 10 # avoid test noise
err_msg = f'mismatch: {name}' err_msg = f'mismatch: {name}'
np.testing.assert_allclose(a, b, err_msg=err_msg, atol=tol, rtol=tol) np.testing.assert_allclose(a, b, err_msg=err_msg, atol=tol, rtol=tol)
def _assert_attr_eq(a, b, attr): def _assert_attr_eq(a, b, attr, tol=_TOLERANCE):
_assert_eq(getattr(a, attr), getattr(b, attr), attr) _assert_eq(getattr(a, attr), getattr(b, attr), attr, tol=tol)
class ForwardTest(absltest.TestCase): class ForwardTest(absltest.TestCase):
@@ -46,7 +46,7 @@ class ForwardTest(absltest.TestCase):
d.ctrl = np.array([-18, 0.59, 0.47]) d.ctrl = np.array([-18, 0.59, 0.47])
d.xfrc_applied[0, 2] = 0.1 # torque d.xfrc_applied[0, 2] = 0.1 # torque
d.xfrc_applied[1, 4] = 0.3 # linear force 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) mujoco.mj_forward(m, d)
mx = mjx.put_model(m) mx = mjx.put_model(m)
@@ -75,14 +75,13 @@ class ForwardTest(absltest.TestCase):
d.ctrl = np.array([-18, 0.59, 0.47]) d.ctrl = np.array([-18, 0.59, 0.47])
d.xfrc_applied[0, 2] = 0.1 # torque d.xfrc_applied[0, 2] = 0.1 # torque
d.xfrc_applied[1, 4] = 0.3 # linear force 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)(mjx.put_model(m), mjx.put_data(m, d))
dx = jax.jit(mjx.step)(mx, mjx.put_data(m, d))
mujoco.mj_step(m, d) mujoco.mj_step(m, d)
_assert_attr_eq(d, dx, 'act') _assert_attr_eq(d, dx, 'act')
_assert_attr_eq(d, dx, 'time') _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') _assert_attr_eq(d, dx, 'qpos')
def test_rk4(self): 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_step(m, d, 10) # let dynamics get state significantly non-zero
mujoco.mj_forward(m, d) mujoco.mj_forward(m, d)
mx = mjx.put_model(m) dx = jax.jit(mjx.rungekutta4)(mjx.put_model(m), mjx.put_data(m, d))
dx = jax.jit(mjx.rungekutta4)(mx, mjx.put_data(m, d))
mujoco.mj_RungeKutta(m, d, 4) mujoco.mj_RungeKutta(m, d, 4)
_assert_attr_eq(d, dx, 'qvel') _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, 'act')
_assert_attr_eq(d, dx, 'time') _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): def test_disable_eulerdamp(self):
m = test_util.load_test_file('pendula.xml') m = test_util.load_test_file('pendula.xml')
self.assertTrue((m.dof_damping > 0).any()) self.assertTrue((m.dof_damping > 0).any())
@@ -128,8 +150,7 @@ class ForwardTest(absltest.TestCase):
d = mujoco.MjData(m) d = mujoco.MjData(m)
d.qvel[:] = 1.0 d.qvel[:] = 1.0
d.qacc[:] = 1.0 d.qacc[:] = 1.0
mx = mjx.put_model(m) dx = jax.jit(mjx.euler)(mjx.put_model(m), mjx.put_data(m, d))
dx = jax.jit(mjx.euler)(mx, mjx.put_data(m, d))
np.testing.assert_allclose(dx.qvel, 1 + m.opt.timestep) 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) ne, nf, nl, nc = constraint.count_constraints(m)
nefc = ne + nf + nl + nc nefc = ne + nf + nl + nc
zero_0 = jp.zeros(0, dtype=jp.float32) zero_0 = jp.zeros(0, dtype=float)
zero_nv = jp.zeros(m.nv, dtype=jp.float32) zero_nv = jp.zeros(m.nv, dtype=float)
zero_nv_6 = jp.zeros((m.nv, 6), dtype=jp.float32) zero_nv_6 = jp.zeros((m.nv, 6), dtype=float)
zero_nv_nv = jp.zeros((m.nv, m.nv), dtype=jp.float32) zero_nv_nv = jp.zeros((m.nv, m.nv), dtype=float)
zero_nbody_3 = jp.zeros((m.nbody, 3), dtype=jp.float32) zero_nbody_3 = jp.zeros((m.nbody, 3), dtype=float)
zero_nbody_6 = jp.zeros((m.nbody, 6), dtype=jp.float32) zero_nbody_6 = jp.zeros((m.nbody, 6), dtype=float)
zero_nbody_10 = jp.zeros((m.nbody, 10), dtype=jp.float32) zero_nbody_10 = jp.zeros((m.nbody, 10), dtype=float)
zero_nbody_3_3 = jp.zeros((m.nbody, 3, 3), dtype=jp.float32) zero_nbody_3_3 = jp.zeros((m.nbody, 3, 3), dtype=float)
zero_nefc = jp.zeros(nefc, dtype=jp.float32) zero_nefc = jp.zeros(nefc, dtype=float)
zero_na = jp.zeros(m.na, dtype=jp.float32) zero_na = jp.zeros(m.na, dtype=float)
zero_nu = jp.zeros(m.nu, dtype=jp.float32) zero_nu = jp.zeros(m.nu, dtype=float)
zero_njnt_3 = jp.zeros((m.njnt, 3), dtype=jp.float32) zero_njnt_3 = jp.zeros((m.njnt, 3), dtype=float)
zero_nm = jp.zeros(m.nM, dtype=jp.float32) zero_nm = jp.zeros(m.nM, dtype=float)
# create first d to get num contacts and nc # create first d to get num contacts and nc
d = types.Data( d = types.Data(
solver_niter=jp.array(0, dtype=jp.int32), solver_niter=jp.array(0, dtype=int),
time=jp.array(0.0), time=jp.array(0.0),
qpos=jp.array(m.qpos0), qpos=jp.array(m.qpos0),
qvel=zero_nv, qvel=zero_nv,
@@ -171,31 +171,31 @@ def make_data(m: Union[types.Model, mujoco.MjModel]) -> types.Data:
ctrl=zero_nu, ctrl=zero_nu,
qfrc_applied=zero_nv, qfrc_applied=zero_nv,
xfrc_applied=zero_nbody_6, 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, qacc=zero_nv,
act_dot=zero_na, act_dot=zero_na,
xpos=zero_nbody_3, 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, xmat=zero_nbody_3_3,
xipos=zero_nbody_3, xipos=zero_nbody_3,
ximat=zero_nbody_3_3, ximat=zero_nbody_3_3,
xanchor=zero_njnt_3, xanchor=zero_njnt_3,
xaxis=zero_njnt_3, xaxis=zero_njnt_3,
geom_xpos=jp.zeros((m.ngeom, 3), dtype=jp.float32), geom_xpos=jp.zeros((m.ngeom, 3), dtype=float),
geom_xmat=jp.zeros((m.ngeom, 3, 3), dtype=jp.float32), geom_xmat=jp.zeros((m.ngeom, 3, 3), dtype=float),
site_xpos=jp.zeros((m.nsite, 3), dtype=jp.float32), site_xpos=jp.zeros((m.nsite, 3), dtype=float),
site_xmat=jp.zeros((m.nsite, 3, 3), dtype=jp.float32), site_xmat=jp.zeros((m.nsite, 3, 3), dtype=float),
subtree_com=zero_nbody_3, subtree_com=zero_nbody_3,
cdof=zero_nv_6, cdof=zero_nv_6,
cinert=zero_nbody_10, cinert=zero_nbody_10,
actuator_length=zero_nu, 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, crb=zero_nbody_10,
qM=zero_nm if support.is_sparse(m) else zero_nv_nv, qM=zero_nm if support.is_sparse(m) else zero_nv_nv,
qLD=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, qLDiagInv=zero_nv if support.is_sparse(m) else zero_0,
contact=types.Contact.zero(ncon), 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_frictionloss=zero_nefc,
efc_D=zero_nefc, efc_D=zero_nefc,
actuator_velocity=zero_nu, 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.""" """Derives geom mesh attributes for mjx.Model from MjModel."""
kwargs = {k: [] for k in _DERIVED_ARGS} kwargs = {k: [] for k in _DERIVED_ARGS}
verts, faces = _get_faces_verts(m) verts, faces = _get_faces_verts(m)
geom_con = m.geom_conaffinity | m.geom_contype
for geomid in range(m.ngeom): for geomid in range(m.ngeom):
dataid = m.geom_dataid[geomid] dataid = m.geom_dataid[geomid]
typ = m.geom_type[geomid] if not geom_con[geomid]:
if typ == GeomType.BOX: # 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]) vert, face = _box(m.geom_size[geomid])
elif dataid >= 0: elif dataid >= 0:
vert, face = verts[dataid], faces[dataid] 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) mgrad = smooth.solve_m(m, d, grad)
elif m.opt.solver == SolverType.NEWTON: elif m.opt.solver == SolverType.NEWTON:
ne, nf, *_ = constraint.count_constraints(m) ne, nf, *_ = constraint.count_constraints(m)
active = (ctx.Jaref < 0).at[:ne + nf].set(True) 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) h = (d.efc_J.T * d.efc_D * active) @ d.efc_J
dh = d.replace(qM=h) h = support.full_m(m, d) + h
dh = smooth.factor_m(m, dh) h_ = jax.scipy.linalg.cho_factor(h)
mgrad = smooth.solve_m(m, dh, grad) mgrad = jax.scipy.linalg.cho_solve(h_, grad)
else: 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) 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) np.testing.assert_allclose(a, b, err_msg=err_msg, atol=tol, rtol=tol)
def _assert_attr_eq(a, b, attr): def _assert_attr_eq(a, b, attr, tol=_TOLERANCE):
_assert_eq(getattr(a, attr), getattr(b, attr), attr) _assert_eq(getattr(a, attr), getattr(b, attr), attr, tol=tol)
class SolverTest(absltest.TestCase): class SolverTest(absltest.TestCase):
@@ -42,13 +42,20 @@ class SolverTest(absltest.TestCase):
def test_newton(self): def test_newton(self):
"""Test newton solver.""" """Test newton solver."""
m = test_util.load_test_file('constraints.xml') 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) 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
mujoco.mj_forward(m, d)
mx = mjx.put_model(m) # mj_forward overwrites qacc_warmstart, so let's restore it to what it was
dx = jax.jit(mjx.solve)(mx, mjx.put_data(m, d)) # 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, 'qacc')
_assert_attr_eq(d, dx, 'qfrc_constraint') _assert_attr_eq(d, dx, 'qfrc_constraint')
nnz = dx.efc_J.any(axis=1) nnz = dx.efc_J.any(axis=1)
@@ -58,23 +65,30 @@ class SolverTest(absltest.TestCase):
"""Test CG solver.""" """Test CG solver."""
m = test_util.load_test_file('constraints.xml') m = test_util.load_test_file('constraints.xml')
d = mujoco.MjData(m) 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.solver = mujoco.mjtSolver.mjSOL_CG
mujoco.mj_forward(m, d) # CG does not converge as quickly as Newton but is cheaper to calculate
mx = mjx.put_model(m) m.opt.solver = mujoco.mjtSolver.mjSOL_CG
dx = jax.jit(mjx.solve)(mx, mjx.put_data(m, d)) 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, '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) 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): def test_no_warmstart(self):
"""Test no warmstart.""" """Test no warmstart."""
m = test_util.load_test_file('constraints.xml') m = test_util.load_test_file('constraints.xml')
d = mujoco.MjData(m) 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 m.opt.disableflags |= mujoco.mjtDisableBit.mjDSBL_WARMSTART
mujoco.mj_forward(m, d) mujoco.mj_forward(m, d)
mx = mjx.put_model(m) mx = mjx.put_model(m)
@@ -83,17 +97,21 @@ class SolverTest(absltest.TestCase):
# without warmstart, the solution is not as close # without warmstart, the solution is not as close
_assert_eq(d.efc_force, dx.efc_force[nnz], 'efc_force', tol=2e-2) _assert_eq(d.efc_force, dx.efc_force[nnz], 'efc_force', tol=2e-2)
def test_dense(self): def test_sparse(self):
"""Test solver works with dense mass matrices.""" """Test solver works with sparse mass matrices."""
m = test_util.load_test_file('constraints.xml') m = test_util.load_test_file('constraints.xml')
m.opt.jacobian = mujoco.mjtJacobian.mjJAC_SPARSE
d = mujoco.MjData(m) 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
mujoco.mj_forward(m, d)
m.opt.jacobian = mujoco.mjtJacobian.mjJAC_DENSE # mj_forward overwrites qacc_warmstart, so let's restore it to what it was
mx = mjx.put_model(m) # at the beginning of the step so that MJX does not have a trivial solution
dx = jax.jit(mjx.solve)(mx, mjx.put_data(m, d)) 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, 'qacc')
_assert_attr_eq(d, dx, 'qfrc_constraint') _assert_attr_eq(d, dx, 'qfrc_constraint')
nnz = dx.efc_J.any(axis=1) 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.""" """Base types used in MJX."""
import enum import enum
from typing import Sequence from typing import List, Optional
import jax import jax
import jax.numpy as jp import jax.numpy as jp
@@ -218,6 +218,7 @@ class Option(PyTreeNode):
Attributes: Attributes:
timestep: timestep timestep: timestep
impratio: ratio of friction-to-normal contact impedance
tolerance: main solver tolerance tolerance: main solver tolerance
ls_tolerance: CG/Newton linesearch tolerance ls_tolerance: CG/Newton linesearch tolerance
gravity: gravitational acceleration (3,) gravity: gravitational acceleration (3,)
@@ -238,9 +239,11 @@ class Option(PyTreeNode):
disableflags: bit flags for disabling standard features disableflags: bit flags for disabling standard features
""" """
timestep: jax.Array timestep: jax.Array
# unsupported: apirate
impratio: jax.Array
tolerance: jax.Array tolerance: jax.Array
ls_tolerance: jax.Array ls_tolerance: jax.Array
# unsupported: apirate, impratio, noslip_tolerance, mpr_tolerance # unsupported: noslip_tolerance, mpr_tolerance
gravity: jax.Array gravity: jax.Array
wind: jax.Array wind: jax.Array
density: jax.Array density: jax.Array
@@ -498,10 +501,10 @@ class Model(PyTreeNode):
pair_dim: np.ndarray pair_dim: np.ndarray
pair_geom1: np.ndarray pair_geom1: np.ndarray
pair_geom2: np.ndarray pair_geom2: np.ndarray
geom_convex_face: Sequence[jax.Array] geom_convex_face: List[Optional[jax.Array]]
geom_convex_vert: Sequence[jax.Array] geom_convex_vert: List[Optional[jax.Array]]
geom_convex_edge: Sequence[jax.Array] geom_convex_edge: List[Optional[jax.Array]]
geom_convex_facenormal: Sequence[jax.Array] geom_convex_facenormal: List[Optional[jax.Array]]
pair_solref: jax.Array pair_solref: jax.Array
pair_solreffriction: jax.Array pair_solreffriction: jax.Array
pair_solimp: jax.Array pair_solimp: jax.Array
@@ -579,8 +582,8 @@ class Contact(PyTreeNode):
solref=jp.zeros((ncon, mujoco.mjNREF)), solref=jp.zeros((ncon, mujoco.mjNREF)),
solreffriction=jp.zeros((ncon, mujoco.mjNREF)), solreffriction=jp.zeros((ncon, mujoco.mjNREF)),
solimp=jp.zeros((ncon, mujoco.mjNIMP,)), solimp=jp.zeros((ncon, mujoco.mjNIMP,)),
geom1=jp.zeros(ncon, dtype=jp.int32), geom1=jp.zeros(ncon, dtype=int),
geom2=jp.zeros(ncon, dtype=jp.int32), geom2=jp.zeros(ncon, dtype=int),
) )
mjx/mujoco/mjx/test_data/constraints.xml
+1 -1
View File
@@ -5,7 +5,7 @@
* solref, solimp * solref, solimp
--> -->
<mujoco> <mujoco>
<option timestep="0.02"/> <option timestep="0.015" impratio="1.5"/>
<default> <default>
<default class="box"> <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 ## Changelog
* 20-02-2024: Sorted actuators in the same order as the joints.
* 02-01-2024: Add more keyframes. * 02-01-2024: Add more keyframes.
* 27-11-2023: Move humanoid geoms to group 1. * 27-11-2023: Move humanoid geoms to group 1.
* 05-04-2023: Fix typo in texture size. * 05-04-2023: Fix typo in texture size.
model/humanoid/humanoid.xml
+3 -3
View File
@@ -200,21 +200,21 @@
</tendon> </tendon>
<actuator> <actuator>
<motor name="abdomen_y" gear="40" joint="abdomen_y"/>
<motor name="abdomen_z" gear="40" joint="abdomen_z"/> <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="abdomen_x" gear="40" joint="abdomen_x"/>
<motor name="hip_x_right" gear="40" joint="hip_x_right"/> <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_z_right" gear="40" joint="hip_z_right"/>
<motor name="hip_y_right" gear="120" joint="hip_y_right"/> <motor name="hip_y_right" gear="120" joint="hip_y_right"/>
<motor name="knee_right" gear="80" joint="knee_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_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_x_left" gear="40" joint="hip_x_left"/>
<motor name="hip_z_left" gear="40" joint="hip_z_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="hip_y_left" gear="120" joint="hip_y_left"/>
<motor name="knee_left" gear="80" joint="knee_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_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="shoulder1_right" gear="20" joint="shoulder1_right"/>
<motor name="shoulder2_right" gear="20" joint="shoulder2_right"/> <motor name="shoulder2_right" gear="20" joint="shoulder2_right"/>
<motor name="elbow_right" gear="40" joint="elbow_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 expected_ncon = 4
mujoco.mj_forward(self.model, self.data) mujoco.mj_forward(self.model, self.data)
self.assertLen(self.data.contact, expected_ncon) self.assertLen(self.data.contact, expected_ncon)
np.testing.assert_array_equal(self.data.contact.geom, [[0, 1]] * 4)
expected_pos = [] expected_pos = []
for contact in self.data.contact: for contact in self.data.contact:
@@ -551,6 +552,13 @@ class MuJoCoBindingsTest(parameterized.TestCase):
self.assertLen(expected_H, expected_ncon) self.assertLen(expected_H, expected_ncon)
np.testing.assert_array_equal(self.data.contact.H, expected_H) 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): def test_realloc_con_efc(self):
self.assertEmpty(self.data.contact) 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 <array>
#include <cstdint> #include <cstdint>
#include <optional>
#include <type_traits>
#include <Eigen/Core> #include <Eigen/Core>
#include <mujoco/mjrender.h> #include <mujoco/mjrender.h>
@@ -40,7 +42,10 @@ class MjWrapper<raw::MjrContext> : public WrapperBase<raw::MjrContext> {
void Free(); 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(fogRGBA);
X(auxWidth); X(auxWidth);
X(auxHeight); X(auxHeight);
python/mujoco/structs.h
+52 -12
View File
@@ -17,12 +17,17 @@
#include <array> #include <array>
#include <cstddef> #include <cstddef>
#include <functional>
#include <istream> #include <istream>
#include <memory> #include <memory>
#include <optional> #include <optional>
#include <ostream> #include <ostream>
#include <sstream>
#include <string_view>
#include <unordered_map> #include <unordered_map>
#include <string> #include <string>
#include <type_traits>
#include <utility>
#include <vector> #include <vector>
#include <absl/types/span.h> #include <absl/types/span.h>
@@ -36,6 +41,7 @@
namespace mujoco::python { namespace mujoco::python {
namespace _impl { namespace _impl {
template <typename T> template <typename T>
class WrapperBase { class WrapperBase {
public: public:
@@ -44,7 +50,7 @@ class WrapperBase {
T* get() { return ptr_; } T* get() { return ptr_; }
const T* get() const { return ptr_; } const T* get() const { return ptr_; }
const pybind11::handle owner() const { return owner_; } pybind11::handle owner() const { return owner_; }
protected: protected:
static void DefaultCapsuleDestructor(PyObject* pyobj) { 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(raw::MjOption* ptr, pybind11::handle owner);
~MjWrapper() = default; ~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 MJOPTION_VECTORS
#undef X #undef X
}; };
@@ -188,7 +197,10 @@ class MjWrapper<raw::MjVisualHeadlight>
MjWrapper(raw::MjVisualHeadlight* ptr, pybind11::handle owner); MjWrapper(raw::MjVisualHeadlight* ptr, pybind11::handle owner);
~MjWrapper() = default; ~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(ambient);
X(diffuse); X(diffuse);
X(specular); X(specular);
@@ -209,7 +221,10 @@ class MjWrapper<raw::MjVisualRgba> : public WrapperBase<raw::MjVisualRgba> {
MjWrapper(raw::MjVisualRgba* ptr, pybind11::handle owner); MjWrapper(raw::MjVisualRgba* ptr, pybind11::handle owner);
~MjWrapper() = default; ~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(fog);
X(haze); X(haze);
X(force); X(force);
@@ -271,7 +286,10 @@ class MjWrapper<raw::MjStatistic> : public WrapperBase<raw::MjStatistic> {
MjWrapper(raw::MjStatistic* ptr, pybind11::handle owner); MjWrapper(raw::MjStatistic* ptr, pybind11::handle owner);
~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::MjStatistic::var)>> \
var
X(center); X(center);
#undef X #undef X
}; };
@@ -504,7 +522,10 @@ class MjWrapper<raw::MjContact> : public WrapperBase<raw::MjContact> {
MjWrapper(raw::MjContact* ptr, pybind11::handle owner); MjWrapper(raw::MjContact* ptr, pybind11::handle owner);
~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::MjContact::var)>> \
var
X(pos); X(pos);
X(frame); X(frame);
X(friction); X(friction);
@@ -628,7 +649,10 @@ class MjWrapper<raw::MjvPerturb> : public WrapperBase<raw::MjvPerturb> {
MjWrapper(MjWrapper&&) = default; MjWrapper(MjWrapper&&) = default;
~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(refpos);
X(refquat); X(refquat);
X(refselpos); X(refselpos);
@@ -650,7 +674,10 @@ class MjWrapper<raw::MjvCamera> : public WrapperBase<raw::MjvCamera> {
MjWrapper(MjWrapper&&) = default; MjWrapper(MjWrapper&&) = default;
~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); X(lookat);
#undef X #undef X
}; };
@@ -671,7 +698,10 @@ class MjWrapper<raw::MjvGLCamera> : public WrapperBase<raw::MjvGLCamera> {
explicit MjWrapper(raw::MjvGLCamera&& other); explicit MjWrapper(raw::MjvGLCamera&& other);
~MjWrapper() = default; ~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(pos);
X(forward); X(forward);
X(up); X(up);
@@ -693,7 +723,10 @@ class MjWrapper<raw::MjvGeom> : public WrapperBase<raw::MjvGeom> {
MjWrapper(raw::MjvGeom* ptr, pybind11::handle owner); MjWrapper(raw::MjvGeom* ptr, pybind11::handle owner);
~MjWrapper() = default; ~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(texrepeat);
X(size); X(size);
X(pos); X(pos);
@@ -717,7 +750,10 @@ class MjWrapper<raw::MjvLight> : public WrapperBase<raw::MjvLight> {
MjWrapper(raw::MjvLight* ptr, pybind11::handle owner); MjWrapper(raw::MjvLight* ptr, pybind11::handle owner);
~MjWrapper() = default; ~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(pos);
X(dir); X(dir);
X(attenuation); X(attenuation);
@@ -741,7 +777,10 @@ class MjWrapper<raw::MjvOption> : public WrapperBase<raw::MjvOption> {
MjWrapper(MjWrapper&&) = default; MjWrapper(MjWrapper&&) = default;
~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(geomgroup);
X(sitegroup); X(sitegroup);
X(jointgroup); X(jointgroup);
@@ -862,6 +901,7 @@ class ScopedMsanDisabler {
void* shadow_; void* shadow_;
}; };
#endif #endif
} // namespace _impl } // namespace _impl
template <typename T> template <typename T>
simulate/simulate.cc
+1 -8
View File
@@ -1969,14 +1969,7 @@ void Simulate::Sync() {
} }
if (pending_.load_key) { if (pending_.load_key) {
int i = this->key; mj_resetDataKeyframe(m_, d_, 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_forward(m_, d_); mj_forward(m_, d_);
update_profiler = true; update_profiler = true;
update_sensor = 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; mjResource* r = NULL;
// first try vfs, otherwise try a provider or OS filesystem // first try vfs, otherwise try a provider or OS filesystem
if ((r = mju_openVfsResource(filename, vfs)) == NULL) { if (!(r = mju_openVfsResource(filename, vfs))) {
if ((r = mju_openResource(filename)) == NULL) { char error[1024];
if (!(r = mju_openResource(filename, error, 1024))) {
mju_warning("%s", error);
return NULL; 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) { void mj_resetDataKeyframe(const mjModel* m, mjData* d, int key) {
_resetData(m, d, 0); _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 // set data to defaults, fill everything else with debug_value
MJAPI void mj_resetDataDebug(const mjModel* m, mjData* d, unsigned char 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); MJAPI void mj_resetDataKeyframe(const mjModel* m, mjData* d, int key);
// mjData arena allocate // 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 // open the given resource; if the name doesn't have a prefix matching with a
// resource provider, then the OS filesystem is used // 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)); mjResource* resource = (mjResource*) mju_malloc(sizeof(mjResource));
const mjpResourceProvider* provider = NULL; const mjpResourceProvider* provider = NULL;
if (resource == NULL) { if (resource == NULL) {
@@ -72,9 +77,12 @@ mjResource* mju_openResource(const char* name) {
return resource; return resource;
} }
mju_warning("mju_openResource: could not open resource '%s' " if (error) {
"using a resource provider matching prefix '%s'", snprintf(error, error_sz, "could not open '%s'"
name, provider->prefix); "using a resource provider matching prefix '%s'",
name, provider->prefix);
}
mju_closeResource(resource); mju_closeResource(resource);
return NULL; return NULL;
} }
@@ -85,7 +93,10 @@ mjResource* mju_openResource(const char* name) {
file_buffer* fb = (file_buffer*) resource->data; file_buffer* fb = (file_buffer*) resource->data;
fb->buffer = mju_fileToMemory(name, &(fb->nbuffer)); fb->buffer = mju_fileToMemory(name, &(fb->nbuffer));
if (fb->buffer == NULL) { 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); mju_closeResource(resource);
return NULL; return NULL;
} }
@@ -94,8 +105,9 @@ mjResource* mju_openResource(const char* name) {
memcpy(&fb->mtime, &file_stat.st_mtime, sizeof(time_t)); memcpy(&fb->mtime, &file_stat.st_mtime, sizeof(time_t));
} else { } else {
memset(&fb->mtime, 0, sizeof(time_t)); 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; return resource;
} }
@@ -175,24 +187,20 @@ static int mju_isModifiedFile(const char* name, const file_buffer* fb) {
if (stat(name, &file_stat) == 0) { if (stat(name, &file_stat) == 0) {
return difftime(fb->mtime, file_stat.st_mtime) < 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 // return 0 if the resource's timestamp matches the provided timestamp
// if inconclusive // return > 0 if the the resource is younger than the given timestamp
int mju_isModifiedResource(const mjResource* resource) { // return < 0 if the resource is older than the given timestamp
if (resource == NULL) { int mju_isModifiedResource(const mjResource* resource, const char* timestamp) {
return -2;
}
// provider is not OS filesystem // provider is not OS filesystem
if (resource->provider) { if (resource->provider) {
if (resource->provider->modified) { if (resource->provider->modified) {
return resource->provider->modified(resource); return resource->provider->modified(resource, timestamp);
} }
return 1; // default (modified) return 1; // default (modified)
} }
@@ -209,7 +217,7 @@ int mju_dirnamelen(const char* path) {
} }
int pos = -1; int pos = -1;
for (int i = 0; path[i] && i >= 0; ++i) { for (int i = 0; path[i]; ++i) {
if (path[i] == '/' || path[i] == '\\') { if (path[i] == '/' || path[i] == '\\') {
pos = i; pos = i;
} }
src/engine/engine_resource.h
+7 -6
View File
@@ -18,7 +18,7 @@
#include <stddef.h> #include <stddef.h>
#include <mujoco/mjexport.h> #include <mujoco/mjexport.h>
#include "engine/engine_plugin.h" #include <mujoco/mujoco.h>
#ifdef __cplusplus #ifdef __cplusplus
extern "C" { extern "C" {
@@ -26,7 +26,7 @@ extern "C" {
// open the given resource; if the name doesn't have a prefix matching with a // open the given resource; if the name doesn't have a prefix matching with a
// resource provider, then the OS filesystem is used // 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 // close the given resource; no-op if resource is NULL
MJAPI void mju_closeResource(mjResource* resource); MJAPI void mju_closeResource(mjResource* resource);
@@ -35,12 +35,13 @@ MJAPI void mju_closeResource(mjResource* resource);
// return negative value if error // return negative value if error
MJAPI int mju_readResource(mjResource* resource, const void** buffer); 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); 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 // return 0 if the resource's timestamp matches the provided timestamp
// if inconclusive // return > 0 if the the resource is younger than the given timestamp
MJAPI int mju_isModifiedResource(const mjResource* resource); // 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 // get the length of the dirname portion of a given path
int mju_dirnamelen(const char* 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 "engine/engine_vfs.h"
#include <stddef.h> #include <stddef.h>
#include <stdint.h>
#include <string.h> #include <string.h>
#include <stdio.h>
#include "engine/engine_array_safety.h" #include "engine/engine_array_safety.h"
#include "engine/engine_resource.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) // initialize to empty (no deallocation)
void mj_defaultVFS(mjVFS* vfs) { void mj_defaultVFS(mjVFS* vfs) {
memset(vfs, 0, sizeof(mjVFS)); 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]) { if (!vfs->filedata[vfs->nfile]) {
return -1; 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->filesize[vfs->nfile] = filesize;
vfs->nfile++; vfs->nfile++;
@@ -136,6 +172,7 @@ int mj_makeEmptyFileVFS(mjVFS* vfs, const char* filename, int filesize) {
mjERROR("could not allocate memory"); mjERROR("could not allocate memory");
} }
memset(vfs->filedata[vfs->nfile], 0, filesize); memset(vfs->filedata[vfs->nfile], 0, filesize);
vfs->filestamp[vfs->nfile] = 0;
// assign size and count // assign size and count
vfs->filesize[vfs->nfile] = filesize; 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 // return file index in VFS, or -1 if not found in VFS
int mj_findFileVFS(const mjVFS* vfs, const char* filename) { int mj_findFileVFS(const mjVFS* vfs, const char* filename) {
// strip path // strip path
@@ -216,7 +298,13 @@ static int vfs_open_callback(mjResource* resource) {
} }
const mjVFS* vfs = (const mjVFS*) resource->data; 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_ #ifndef MUJOCO_SRC_ENGINE_ENGINE_VFS_H_
#define MUJOCO_SRC_ENGINE_ENGINE_VFS_H_ #define MUJOCO_SRC_ENGINE_ENGINE_VFS_H_
#include <stddef.h>
#include <mujoco/mjexport.h> #include <mujoco/mjexport.h>
#include <mujoco/mjmodel.h> #include <mujoco/mjmodel.h>
#include <mujoco/mjplugin.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 // 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); 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); 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 // return file index in VFS, or -1 if not found in VFS
MJAPI int mj_findFileVFS(const mjVFS* vfs, const char* filename); 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. // limitations under the License.
#include "user/user_api.h" #include "user/user_api.h"
#include <functional>
#include <map>
#include <string> #include <string>
#include <utility>
#include <vector> #include <vector>
#include <mujoco/mujoco.h> #include <mujoco/mujoco.h>
#include "user/user_model.h" #include "user/user_model.h"
#include "user/user_objects.h" #include "user/user_objects.h"
#include "xml/xml_util.h"
// create model // create model
void* mjm_createModel() { mjmModel* mjm_createModel() {
mjCModel* modelC = new mjCModel(); mjCModel* modelC = new mjCModel();
return modelC; return &modelC->spec;
} }
// delete model // delete model
void mjm_deleteModel(void* modelspec) { void mjm_deleteModel(mjmModel* modelspec) {
mjCModel* model = static_cast<mjCModel*>(modelspec); mjCModel* model = reinterpret_cast<mjCModel*>(modelspec->element);
delete model; delete model;
} }
@@ -47,8 +51,8 @@ MJAPI void mjm_finalize(mjElement object) {
// add child body to body, return child spec // add child body to body, return child spec
mjmBody* mjm_addBody(mjmBody* bodyspec, void* defspec) { mjmBody* mjm_addBody(mjmBody* bodyspec, mjmDefault* defspec) {
mjCDef* def = static_cast<mjCDef*>(defspec); mjCDef* def = defspec ? reinterpret_cast<mjCDef*>(defspec->element) : 0;
mjCBody* body = reinterpret_cast<mjCBody*>(bodyspec->element)->AddBody(def); mjCBody* body = reinterpret_cast<mjCBody*>(bodyspec->element)->AddBody(def);
return &body->spec; return &body->spec;
} }
@@ -56,8 +60,8 @@ mjmBody* mjm_addBody(mjmBody* bodyspec, void* defspec) {
// add site to body, return site spec // add site to body, return site spec
mjmSite* mjm_addSite(mjmBody* bodyspec, void* defspec) { mjmSite* mjm_addSite(mjmBody* bodyspec, mjmDefault* defspec) {
mjCDef* def = static_cast<mjCDef*>(defspec); mjCDef* def = defspec ? reinterpret_cast<mjCDef*>(defspec->element) : 0;
mjCBody* body = reinterpret_cast<mjCBody*>(bodyspec->element); mjCBody* body = reinterpret_cast<mjCBody*>(bodyspec->element);
mjCSite* site = body->AddSite(def); mjCSite* site = body->AddSite(def);
return &site->spec; return &site->spec;
@@ -66,8 +70,8 @@ mjmSite* mjm_addSite(mjmBody* bodyspec, void* defspec) {
// add joint to body // add joint to body
mjmJoint* mjm_addJoint(mjmBody* bodyspec, void* defspec) { mjmJoint* mjm_addJoint(mjmBody* bodyspec, mjmDefault* defspec) {
mjCDef* def = static_cast<mjCDef*>(defspec); mjCDef* def = defspec ? reinterpret_cast<mjCDef*>(defspec->element) : 0;
mjCBody* body = reinterpret_cast<mjCBody*>(bodyspec->element); mjCBody* body = reinterpret_cast<mjCBody*>(bodyspec->element);
mjCJoint* joint = body->AddJoint(def); mjCJoint* joint = body->AddJoint(def);
return &joint->spec; return &joint->spec;
@@ -85,8 +89,8 @@ mjmJoint* mjm_addFreeJoint(mjmBody* bodyspec) {
// add geom to body // add geom to body
mjmGeom* mjm_addGeom(mjmBody* bodyspec, void* defspec) { mjmGeom* mjm_addGeom(mjmBody* bodyspec, mjmDefault* defspec) {
mjCDef* def = static_cast<mjCDef*>(defspec); mjCDef* def = defspec ? reinterpret_cast<mjCDef*>(defspec->element) : 0;
mjCBody* body = reinterpret_cast<mjCBody*>(bodyspec->element); mjCBody* body = reinterpret_cast<mjCBody*>(bodyspec->element);
mjCGeom* geom = body->AddGeom(def); mjCGeom* geom = body->AddGeom(def);
return &geom->spec; return &geom->spec;
@@ -95,8 +99,8 @@ mjmGeom* mjm_addGeom(mjmBody* bodyspec, void* defspec) {
// add camera to body // add camera to body
mjmCamera* mjm_addCamera(mjmBody* bodyspec, void* defspec) { mjmCamera* mjm_addCamera(mjmBody* bodyspec, mjmDefault* defspec) {
mjCDef* def = static_cast<mjCDef*>(defspec); mjCDef* def = defspec ? reinterpret_cast<mjCDef*>(defspec->element) : 0;
mjCBody* body = reinterpret_cast<mjCBody*>(bodyspec->element); mjCBody* body = reinterpret_cast<mjCBody*>(bodyspec->element);
mjCCamera* camera = body->AddCamera(def); mjCCamera* camera = body->AddCamera(def);
return &camera->spec; return &camera->spec;
@@ -105,8 +109,8 @@ mjmCamera* mjm_addCamera(mjmBody* bodyspec, void* defspec) {
// add light to body // add light to body
mjmLight* mjm_addLight(mjmBody* bodyspec, void* defspec) { mjmLight* mjm_addLight(mjmBody* bodyspec, mjmDefault* defspec) {
mjCDef* def = static_cast<mjCDef*>(defspec); mjCDef* def = defspec ? reinterpret_cast<mjCDef*>(defspec->element) : 0;
mjCBody* body = reinterpret_cast<mjCBody*>(bodyspec->element); mjCBody* body = reinterpret_cast<mjCBody*>(bodyspec->element);
mjCLight* light = body->AddLight(def); mjCLight* light = body->AddLight(def);
return &light->spec; 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 // add frame to body
mjmFrame* mjm_addFrame(mjmBody* bodyspec, mjmFrame* parentframe) { mjmFrame* mjm_addFrame(mjmBody* bodyspec, mjmFrame* parentframe) {
mjCFrame* parentframeC = 0; mjCFrame* parentframeC = 0;
@@ -127,20 +140,76 @@ mjmFrame* mjm_addFrame(mjmBody* bodyspec, mjmFrame* parentframe) {
// Add material to model. // add mesh to model
mjmMaterial* mjm_addMaterial(void* model, void* defspec) { mjmMesh* mjm_addMesh(mjmModel* model, mjmDefault* defspec) {
mjCModel* modelC = static_cast<mjCModel*>(model); mjCDef* def = defspec ? reinterpret_cast<mjCDef*>(defspec->element) : 0;
mjCDef* def = static_cast<mjCDef*>(defspec); 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); mjCMaterial* material = modelC->AddMaterial(def);
return &material->spec; 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 // add equality to model
mjmEquality* mjm_addEquality(void* model, void* defspec) { mjmEquality* mjm_addEquality(mjmModel* model, mjmDefault* defspec) {
mjCModel* modelC = static_cast<mjCModel*>(model); mjCModel* modelC = reinterpret_cast<mjCModel*>(model->element);
mjCDef* def = static_cast<mjCDef*>(defspec); mjCDef* def = defspec ? reinterpret_cast<mjCDef*>(defspec->element) : 0;
mjCEquality* equality = modelC->AddEquality(def); mjCEquality* equality = modelC->AddEquality(def);
return &equality->spec; return &equality->spec;
} }
@@ -148,9 +217,9 @@ mjmEquality* mjm_addEquality(void* model, void* defspec) {
// add tendon to model // add tendon to model
mjmTendon* mjm_addTendon(void* model, void* defspec) { mjmTendon* mjm_addTendon(mjmModel* model, mjmDefault* defspec) {
mjCModel* modelC = static_cast<mjCModel*>(model); mjCModel* modelC = reinterpret_cast<mjCModel*>(model->element);
mjCDef* def = static_cast<mjCDef*>(defspec); mjCDef* def = defspec ? reinterpret_cast<mjCDef*>(defspec->element) : 0;
mjCTendon* tendon = modelC->AddTendon(def); mjCTendon* tendon = modelC->AddTendon(def);
return &tendon->spec; return &tendon->spec;
} }
@@ -194,9 +263,9 @@ mjmWrap* mjm_wrapPulley(mjmTendon* tendonspec, double divisor) {
// add actuator to model // add actuator to model
mjmActuator* mjm_addActuator(void* model, void* defspec) { mjmActuator* mjm_addActuator(mjmModel* model, mjmDefault* defspec) {
mjCModel* modelC = static_cast<mjCModel*>(model); mjCModel* modelC = reinterpret_cast<mjCModel*>(model->element);
mjCDef* def = static_cast<mjCDef*>(defspec); mjCDef* def = defspec ? reinterpret_cast<mjCDef*>(defspec->element) : 0;
mjCActuator* actuator = modelC->AddActuator(def); mjCActuator* actuator = modelC->AddActuator(def);
return &actuator->spec; return &actuator->spec;
} }
@@ -204,40 +273,90 @@ mjmActuator* mjm_addActuator(void* model, void* defspec) {
// add sensor to model // add sensor to model
mjmSensor* mjm_addSensor(void* model) { mjmSensor* mjm_addSensor(mjmModel* model) {
mjCModel* modelC = static_cast<mjCModel*>(model); mjCModel* modelC = reinterpret_cast<mjCModel*>(model->element);
mjCSensor* sensor = modelC->AddSensor(); mjCSensor* sensor = modelC->AddSensor();
return &sensor->spec; return &sensor->spec;
} }
// Add plugin to model. // add numeric to model
mjElement mjm_addPlugin(void* model) { mjmNumeric* mjm_addNumeric(mjmModel* model) {
mjCModel* modelC = static_cast<mjCModel*>(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(); 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 // get objects
void* mjm_getModel(mjmBody* bodyspec) { mjmModel* mjm_getModel(mjmBody* bodyspec) {
return reinterpret_cast<mjCBody*>(bodyspec->element)->model; return &(reinterpret_cast<mjCBody*>(bodyspec->element)->model->spec);
} }
// get default // get default
void* mjm_getDefault(mjElement element) { mjmDefault* mjm_getDefault(mjElement element) {
return reinterpret_cast<mjCBase*>(element)->def; return &(reinterpret_cast<mjCBase*>(element)->def->spec);
} }
// find body in model by name // find body in model by name
mjmBody* mjm_findBody(void* modelspec, const char* name) { mjmBody* mjm_findBody(mjmModel* modelspec, const char* name) {
mjCModel* model = static_cast<mjCModel*>(modelspec); mjCModel* model = reinterpret_cast<mjCModel*>(modelspec->element);
mjCBase* body = model->FindObject(mjOBJ_BODY, std::string(name)); mjCBase* body = model->FindObject(mjOBJ_BODY, std::string(name));
if (!body) { if (!body) {
return 0; return 0;
@@ -277,13 +396,16 @@ int mjm_getId(mjElement element) {
} }
// set default // set default
void mjm_setDefault(mjElement element, void* defspec) { void mjm_setDefault(mjElement element, mjmDefault* defspec) {
mjCBase* baseC = reinterpret_cast<mjCBase*>(element); mjCBase* baseC = reinterpret_cast<mjCBase*>(element);
baseC->def = static_cast<mjCDef*>(defspec); baseC->def = reinterpret_cast<mjCDef*>(defspec->element);
baseC->def->PointToLocal();
} }
// set string // set string
void mjm_setString(mjString dest, const char* text) { void mjm_setString(mjString dest, const char* text) {
std::string* str = reinterpret_cast<std::string*>(dest); std::string* str = reinterpret_cast<std::string*>(dest);
@@ -292,8 +414,75 @@ void mjm_setString(mjString dest, const char* text) {
// set double array // Set specific entry in destination string vector.
void mjm_setDouble(mjDouble dest, const double* array, int size) { 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); std::vector<double>* v = reinterpret_cast<std::vector<double>*>(dest);
v->assign(size, 0.0); v->assign(size, 0.0);
for (int i = 0; i < size; ++i) { for (int i = 0; i < size; ++i) {
@@ -312,7 +501,7 @@ const char* mjm_getString(const mjString source) {
// get double array // 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); std::vector<double>* v = reinterpret_cast<std::vector<double>*>(source);
if (size) { if (size) {
*size = v->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 // compute full inertia
const char* mjm_setFullInertia(mjmBody* bodyspec, double quat[4], double inertia[3]) { const char* mjm_setFullInertia(mjmBody* bodyspec, double quat[4], double inertia[3]) {
mjCBody* body = reinterpret_cast<mjCBody*>(bodyspec->element); 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_ #ifndef MUJOCO_SRC_USER_USER_API_H_
#define MUJOCO_SRC_USER_USER_API_H_ #define MUJOCO_SRC_USER_USER_API_H_
#include <stddef.h>
#include <mujoco/mjexport.h> #include <mujoco/mjexport.h>
#include <mujoco/mjmodel.h> #include <mujoco/mjmodel.h>
#include <mujoco/mjtnum.h> #include <mujoco/mjtnum.h>
@@ -30,19 +31,103 @@ extern "C" {
typedef struct _mjElement* mjElement; typedef struct _mjElement* mjElement;
typedef struct _mjString* mjString; 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) ---------------------------------------------- //---------------------------------- enum types (mjt) ----------------------------------------------
typedef enum _mjtGeomInertia { // type of inertia inference typedef enum _mjtGeomInertia { // type of inertia inference
mjINERTIA_VOLUME, // mass distributed in the volume mjINERTIA_VOLUME, // mass distributed in the volume
mjINERTIA_SHELL, // mass distributed on the surface mjINERTIA_SHELL, // mass distributed on the surface
} mjtGeomInertia; } 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) --------------------------------------- //---------------------------------- 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 typedef struct _mjmOrientation { // alternative orientation specifiers
double axisangle[4]; // rotation axis and angle double axisangle[4]; // rotation axis and angle
double xyaxes[6]; // x and y axes double xyaxes[6]; // x and y axes
@@ -55,7 +140,9 @@ typedef struct _mjmPlugin { // plugin specification
mjElement instance; // internal, do not modify mjElement instance; // internal, do not modify
mjString name; // name mjString name; // name
mjString instance_name; // instance 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; } mjmPlugin;
@@ -80,7 +167,7 @@ typedef struct _mjmBody { // body specification
// other // other
mjtByte mocap; // is this a mocap body mjtByte mocap; // is this a mocap body
double gravcomp; // gravity compensation double gravcomp; // gravity compensation
mjDouble userdata; // user data mjDoubleVec userdata; // user data
mjtByte explicitinertial; // whether to save the body with explicit inertial clause mjtByte explicitinertial; // whether to save the body with explicit inertial clause
mjmPlugin plugin; // passive force plugin mjmPlugin plugin; // passive force plugin
mjString info; // message appended to compiler errors mjString info; // message appended to compiler errors
@@ -113,12 +200,12 @@ typedef struct _mjmJoint { // joint specification
double springdamper[2]; // timeconst, dampratio double springdamper[2]; // timeconst, dampratio
// limits // limits
int limited; // does joint have limits int limited; // does joint have limits (mjtLimited)
double range[2]; // joint limits double range[2]; // joint limits
double margin; // margin value for joint limit detection double margin; // margin value for joint limit detection
mjtNum solref_limit[mjNREF]; // solver reference: joint limits mjtNum solref_limit[mjNREF]; // solver reference: joint limits
mjtNum solimp_limit[mjNIMP]; // solver impedance: 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 double actfrcrange[2]; // actuator force limits
// dof properties // dof properties
@@ -131,7 +218,7 @@ typedef struct _mjmJoint { // joint specification
// other // other
int group; // group int group; // group
double urdfeffort; // effort (urdf) double urdfeffort; // effort (urdf)
mjDouble userdata; // user data mjDoubleVec userdata; // user data
mjString info; // message appended to compiler errors mjString info; // message appended to compiler errors
} mjmJoint; } mjmJoint;
@@ -179,7 +266,7 @@ typedef struct _mjmGeom { // geom specification
mjString hfieldname; // heightfield attached to geom mjString hfieldname; // heightfield attached to geom
mjString meshname; // mesh attached to geom mjString meshname; // mesh attached to geom
double fitscale; // scale mesh uniformly double fitscale; // scale mesh uniformly
mjDouble userdata; // user data mjDoubleVec userdata; // user data
mjmPlugin plugin; // sdf plugin mjmPlugin plugin; // sdf plugin
mjString info; // message appended to compiler errors mjString info; // message appended to compiler errors
} mjmGeom; } mjmGeom;
@@ -204,7 +291,7 @@ typedef struct _mjmSite { // site specification
float rgba[4]; // rgba when material is omitted float rgba[4]; // rgba when material is omitted
// other // other
mjDouble userdata; // user data mjDoubleVec userdata; // user data
mjString info; // message appended to compiler errors mjString info; // message appended to compiler errors
} mjmSite; } mjmSite;
@@ -233,7 +320,7 @@ typedef struct _mjmCamera { // camera specification
float principal_pixel[2]; // principal point (pixel) float principal_pixel[2]; // principal point (pixel)
// other // other
mjDouble userdata; // user data mjDoubleVec userdata; // user data
mjString info; // message appended to compiler errors mjString info; // message appended to compiler errors
} mjmCamera; } mjmCamera;
@@ -265,12 +352,149 @@ typedef struct _mjmLight { // light specification
} mjmLight; } 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 typedef struct _mjmMaterial { // material specification
mjElement element; // internal, do not modify mjElement element; // internal, do not modify
mjString name; // name mjString name; // name
mjString classname; // class name mjString classname; // class name
mjString texture; // name of texture (empty: none) 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 texrepeat[2]; // texture repetition for 2D mapping
float emission; // emission float emission; // emission
float specular; // specular float specular; // specular
@@ -281,6 +505,34 @@ typedef struct _mjmMaterial { // material specification
} mjmMaterial; } 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 typedef struct _mjmEquality { // equality specification
mjElement element; // internal, do not modify mjElement element; // internal, do not modify
mjString name; // name mjString name; // name
@@ -310,7 +562,7 @@ typedef struct _mjmTendon { // tendon specification
mjtNum solimp_friction[mjNIMP]; // solver impedance: tendon friction mjtNum solimp_friction[mjNIMP]; // solver impedance: tendon friction
// length range // 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 range[2]; // length limits
double margin; // margin value for tendon limit detection double margin; // margin value for tendon limit detection
mjtNum solref_limit[mjNREF]; // solver reference: tendon limits mjtNum solref_limit[mjNREF]; // solver reference: tendon limits
@@ -323,7 +575,7 @@ typedef struct _mjmTendon { // tendon specification
int group; // group int group; // group
// other // other
mjDouble userdata; // user data mjDoubleVec userdata; // user data
mjString info; // message appended to errors mjString info; // message appended to errors
} mjmTendon; } mjmTendon;
@@ -362,18 +614,19 @@ typedef struct _mjmActuator { // actuator specification
mjString slidersite; // site defining cylinder, for slider-crank mjString slidersite; // site defining cylinder, for slider-crank
double cranklength; // crank length, for slider-crank double cranklength; // crank length, for slider-crank
double lengthrange[2]; // transmission length range double lengthrange[2]; // transmission length range
double inheritrange; // automatic range setting for position and intvelocity
// input/output clamping // 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 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 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 double actrange[2]; // activation range
// other // other
int group; // group int group; // group
mjDouble userdata; // user data mjDoubleVec userdata; // user data
mjmPlugin plugin; // actuator plugin mjmPlugin plugin; // actuator plugin
mjString info; // message appended to compiler errors mjString info; // message appended to compiler errors
} mjmActuator; } mjmActuator;
@@ -401,55 +654,135 @@ typedef struct _mjmSensor { // sensor specfication
double noise; // noise stdev double noise; // noise stdev
// other // other
mjDouble userdata; // user data mjDoubleVec userdata; // user data
mjmPlugin plugin; // sensor plugin mjmPlugin plugin; // sensor plugin
mjString info; // message appended to compiler errors mjString info; // message appended to compiler errors
} mjmSensor; } 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 ------------------------------------------------- //---------------------------------- API functions -------------------------------------------------
// Create model. // Create model.
MJAPI void* mjm_createModel(); MJAPI mjmModel* mjm_createModel();
// Delete model. // Delete model.
MJAPI void mjm_deleteModel(void* modelspec); MJAPI void mjm_deleteModel(mjmModel* modelspec);
// Copy spec into private attributes. // Copy spec into private attributes.
MJAPI void mjm_finalize(mjElement object); MJAPI void mjm_finalize(mjElement object);
// Add child body to body, return child spec. // 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. // 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. // Add joint to body.
MJAPI mjmJoint* mjm_addJoint(mjmBody* body, void* defspec); MJAPI mjmJoint* mjm_addJoint(mjmBody* body, mjmDefault* def);
// Add freejoint to body. // Add freejoint to body.
MJAPI mjmJoint* mjm_addFreeJoint(mjmBody* body); MJAPI mjmJoint* mjm_addFreeJoint(mjmBody* body);
// Add geom to 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. // Add camera to body.
MJAPI mjmCamera* mjm_addCamera(mjmBody* body, void* defspec); MJAPI mjmCamera* mjm_addCamera(mjmBody* body, mjmDefault* def);
// Add light to body. // Add light to body.
MJAPI mjmLight* mjm_addLight(mjmBody* body, void* defspec); MJAPI mjmLight* mjm_addLight(mjmBody* body, mjmDefault* def);
// Add frame to body. // Add frame to body.
MJAPI mjmFrame* mjm_addFrame(mjmBody* body, mjmFrame* parentframe); 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. // 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. // Add equality to model.
MJAPI mjmEquality* mjm_addEquality(void* model, void* defspec); MJAPI mjmEquality* mjm_addEquality(mjmModel* model, mjmDefault* def);
// Add tendon to model. // Add tendon to model.
MJAPI mjmTendon* mjm_addTendon(void* model, void* defspec); MJAPI mjmTendon* mjm_addTendon(mjmModel* model, mjmDefault* def);
// Wrap site using tendon. // Wrap site using tendon.
MJAPI mjmWrap* mjm_wrapSite(mjmTendon* tendon, const char* name); 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); MJAPI mjmWrap* mjm_wrapPulley(mjmTendon* tendon, double divisor);
// Add actuator to model. // Add actuator to model.
MJAPI mjmActuator* mjm_addActuator(void* model, void* defspec); MJAPI mjmActuator* mjm_addActuator(mjmModel* model, mjmDefault* def);
// Add sensor to model. // 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. // 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. // Get model from body.
MJAPI void* mjm_getModel(mjmBody* body); MJAPI mjmModel* mjm_getModel(mjmBody* body);
// Get default corresponding to an mjElement. // Get default corresponding to an mjElement.
MJAPI void* mjm_getDefault(mjElement element); MJAPI mjmDefault* mjm_getDefault(mjElement element);
// Find body in model by name. // 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. // Find child body by name.
MJAPI mjmBody* mjm_findChild(mjmBody* body, const char* 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. // Get element id.
MJAPI int mjm_getId(mjElement element); MJAPI int mjm_getId(mjElement element);
// Copy text to destination string. // Copy text to string.
MJAPI void mjm_setString(mjString dest, const char* text); MJAPI void mjm_setString(mjString dest, const char* text);
// Copy double array to destination vector. // Split text to entries and copy to string vector.
MJAPI void mjm_setDouble(mjDouble dest, const double* array, int size); 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. // Get string contents.
MJAPI const char* mjm_getString(mjString source); MJAPI const char* mjm_getString(mjString source);
// Get double array contents and optionally its size. // 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. // Set default.
MJAPI void mjm_setDefault(mjElement element, void* defspec); MJAPI void mjm_setDefault(mjElement element, mjmDefault* def);
// Set frame. // Set frame.
MJAPI void mjm_setFrame(mjElement dest, mjmFrame* 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 -------------------------------------- //---------------------------------- Initialization functions --------------------------------------
// Default model attributes.
MJAPI void mjm_defaultModel(mjmModel& model);
// Default body attributes. // Default body attributes.
MJAPI void mjm_defaultBody(mjmBody& body); MJAPI void mjm_defaultBody(mjmBody& body);
@@ -532,9 +907,27 @@ MJAPI void mjm_defaultCamera(mjmCamera& camera);
// Default light attributes. // Default light attributes.
MJAPI void mjm_defaultLight(mjmLight& light); 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. // Default material attributes.
MJAPI void mjm_defaultMaterial(mjmMaterial& material); MJAPI void mjm_defaultMaterial(mjmMaterial& material);
// Default pair attributes.
MJAPI void mjm_defaultPair(mjmPair& pair);
// Default equality attributes. // Default equality attributes.
MJAPI void mjm_defaultEquality(mjmEquality& equality); MJAPI void mjm_defaultEquality(mjmEquality& equality);
@@ -547,6 +940,21 @@ MJAPI void mjm_defaultActuator(mjmActuator& actuator);
// Default sensor attributes. // Default sensor attributes.
MJAPI void mjm_defaultSensor(mjmSensor& sensor); 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 #ifdef __cplusplus
} }
#endif #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 <vector>
#include <mujoco/mjmodel.h> #include <mujoco/mjmodel.h>
#include "user/user_api.h"
#include "user/user_model.h" #include "user/user_model.h"
#include "user/user_objects.h" #include "user/user_objects.h"
@@ -79,14 +80,14 @@ class mjCComposite {
void MakeSkin2(mjCModel* model, mjtNum inflate); void MakeSkin2(mjCModel* model, mjtNum inflate);
void MakeSkin2Subgrid(mjCModel* model, mjtNum inflate); void MakeSkin2Subgrid(mjCModel* model, mjtNum inflate);
void MakeClothBones(mjCModel* model, mjCSkin* skin); void MakeClothBones(mjCModel* model, mjmSkin* skin);
void MakeClothBonesSubgrid(mjCModel* model, mjCSkin* skin); void MakeClothBonesSubgrid(mjCModel* model, mjmSkin* skin);
void MakeCableBones(mjCModel* model, mjCSkin* skin); void MakeCableBones(mjCModel* model, mjmSkin* skin);
void MakeCableBonesSubgrid(mjCModel* model, mjCSkin* skin); void MakeCableBonesSubgrid(mjCModel* model, mjmSkin* skin);
void MakeSkin3(mjCModel* model); void MakeSkin3(mjCModel* model);
void MakeSkin3Box(mjCSkin* skin, int c0, int c1, int side, int& vcnt, const char* format); void MakeSkin3Box(mjmSkin* skin, int c0, int c1, int side, int& vcnt, const char* format);
void MakeSkin3Smooth(mjCSkin* skin, int c0, int c1, int side, void MakeSkin3Smooth(mjmSkin* skin, int c0, int c1, int side,
const std::map<std::string, int>& vmap, const char* format); const std::map<std::string, int>& vmap, const char* format);
void BoxProject(double* pos); void BoxProject(double* pos);
@@ -113,10 +114,9 @@ class mjCComposite {
std::vector<std::string> username; std::vector<std::string> username;
// plugin support // plugin support
bool is_plugin;
std::string plugin_name; std::string plugin_name;
std::string plugin_instance_name; std::string plugin_instance_name;
mjCPlugin* plugin_instance; mjmPlugin plugin;
// skin // skin
bool skin; // generate skin bool skin; // generate skin
@@ -138,6 +138,16 @@ class mjCComposite {
private: private:
mjmBody* AddRopeBody(mjCModel* model, mjmBody* body, int ix, int ix1); 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]); 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_ #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); mjuu_setvec(quat, 1, 0, 0, 0);
rigid = false; rigid = false;
centered = 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 // make flexcomp object
bool mjCFlexcomp::Make(mjCModel* model, mjmBody* body, char* error, int error_sz) { bool mjCFlexcomp::Make(mjCModel* model, mjmBody* body, char* error, int error_sz) {
mjmFlex* dflex = def.spec.flex;
bool radial = (type==mjFCOMPTYPE_BOX || bool radial = (type==mjFCOMPTYPE_BOX ||
type==mjFCOMPTYPE_CYLINDER || type==mjFCOMPTYPE_CYLINDER ||
type==mjFCOMPTYPE_ELLIPSOID); type==mjFCOMPTYPE_ELLIPSOID);
@@ -98,7 +104,7 @@ bool mjCFlexcomp::Make(mjCModel* model, mjmBody* body, char* error, int error_sz
} }
// check spacing // check spacing
double minspace = 2*def.flex.radius + def.flex.margin; double minspace = 2*dflex->radius + dflex->margin;
if (!direct) { if (!direct) {
if (spacing[0]<minspace || if (spacing[0]<minspace ||
spacing[1]<minspace || spacing[1]<minspace ||
@@ -118,7 +124,7 @@ bool mjCFlexcomp::Make(mjCModel* model, mjmBody* body, char* error, int error_sz
} }
// compute orientation // 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) { if (alterr) {
return comperr(error, alterr, error_sz); 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 // get dim and check
int dim = def.flex.dim; int dim = dflex->dim;
if (dim<1 || dim>3) { if (dim<1 || dim>3) {
return comperr(error, "Invalid dim, must be between 1 and 3", error_sz); 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 // force flatskin shading for box, cylinder and 3D grid
if (type==mjFCOMPTYPE_BOX || type==mjFCOMPTYPE_CYLINDER || if (type==mjFCOMPTYPE_BOX || type==mjFCOMPTYPE_CYLINDER ||
(type==mjFCOMPTYPE_GRID && dim==3)) { (type==mjFCOMPTYPE_GRID && dim==3)) {
def.flex.flatskin = true; dflex->flatskin = true;
} }
// check pin sizes // check pin sizes
@@ -368,21 +374,22 @@ bool mjCFlexcomp::Make(mjCModel* model, mjmBody* body, char* error, int error_sz
} }
// create flex, copy parameters // create flex, copy parameters
mjCFlex* pf = model->AddFlex(); mjCFlex* flex = model->AddFlex();
int id = pf->id; mjmFlex* pf = &flex->spec;
*pf = def.flex; int id = flex->id;
pf->model = model;
pf->id = id; *flex = def.flex;
pf->name = name; flex->PointToLocal();
pf->elem = element;
if (!centered) { flex->model = model;
pf->vert = point; flex->id = id;
} mjm_setString(pf->name, name.c_str());
pf->texcoord = texcoord; mjm_setInt(pf->elem, element.data(), element.size());
mjm_setFloat(pf->texcoord, texcoord.data(), texcoord.size());
// rigid: set parent name, nothing else to do // rigid: set parent name, nothing else to do
if (rigid) { if (rigid) {
pf->vertbody.push_back(std::string(mjm_getString(body->name)).c_str()); mjm_appendString(pf->vertbody, mjm_getString(body->name));
return true; return true;
} }
@@ -390,6 +397,12 @@ bool mjCFlexcomp::Make(mjCModel* model, mjmBody* body, char* error, int error_sz
double bodymass = mass/npnt; double bodymass = mass/npnt;
double bodyinertia = bodymass*(2.0*inertiabox*inertiabox)/3.0; 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 // create bodies, construct flex vert and vertbody
for (int i=0; i<npnt; i++) { for (int i=0; i<npnt; i++) {
// not used: skip // not used: skip
@@ -399,15 +412,15 @@ bool mjCFlexcomp::Make(mjCModel* model, mjmBody* body, char* error, int error_sz
// pinned: parent body // pinned: parent body
if (pinned[i]) { 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 // add plugin
if (plugin_instance) { if (plugin.active) {
mjmPlugin* plugin = &body->plugin; mjmPlugin* pplugin = &body->plugin;
plugin->active = true; pplugin->active = true;
plugin->instance = (mjElement)plugin_instance; pplugin->instance = (mjElement)plugin.instance;
mjm_setString(plugin->name, plugin_name.c_str()); mjm_setString(pplugin->name, mjm_getString(plugin.name));
mjm_setString(plugin->instance_name, plugin_instance_name.c_str()); 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]; char txt[100];
mju::sprintf_arr(txt, "%s_%d", name.c_str(), i); mju::sprintf_arr(txt, "%s_%d", name.c_str(), i);
mjm_setString(pb->name, txt); 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 // clear flex vertex coordinates if allocated
if (!centered) { if (!centered) {
pf->vert[3*i] = 0; point[3*i] = 0;
pf->vert[3*i+1] = 0; point[3*i+1] = 0;
pf->vert[3*i+2] = 0; point[3*i+2] = 0;
} }
// add plugin // add plugin
if (plugin_instance) { if (plugin.active) {
mjmPlugin* plugin = &pb->plugin; mjmPlugin* pplugin = &pb->plugin;
plugin->active = true; pplugin->active = true;
plugin->instance = (mjElement)plugin_instance; pplugin->instance = (mjElement)plugin.instance;
mjm_setString(plugin->name, plugin_name.c_str()); mjm_setString(pplugin->name, mjm_getString(plugin.name));
mjm_setString(plugin->instance_name, plugin_instance_name.c_str()); mjm_setString(pplugin->instance_name, plugin_instance_name.c_str());
} }
} }
} }
if (!centered) {
mjm_setDouble(pf->vert, point.data(), point.size());
}
// create edge equality constraint // create edge equality constraint
if (equality) { if (equality) {
mjmEquality* pe = mjm_addEquality(model, &def); mjmEquality* pe = mjm_addEquality(&model->spec, &def.spec);
mjm_setDefault(pe->element, model->defaults[0]); mjm_setDefault(pe->element, &model->defaults[0]->spec);
pe->type = mjEQ_FLEX; pe->type = mjEQ_FLEX;
pe->active = true; pe->active = true;
mjm_setString(pe->name1, name.c_str()); mjm_setString(pe->name1, name.c_str());
@@ -502,7 +519,7 @@ int mjCFlexcomp::GridID(int ix, int iy, int iz) {
// make grid // make grid
bool mjCFlexcomp::MakeGrid(char* error, int error_sz) { bool mjCFlexcomp::MakeGrid(char* error, int error_sz) {
int dim = def.flex.dim; int dim = def.flex.spec.dim;
bool hastex = texcoord.empty(); bool hastex = texcoord.empty();
// 1D // 1D
@@ -683,7 +700,7 @@ bool mjCFlexcomp::MakeBox(char* error, int error_sz) {
double pos[3]; double pos[3];
// set 3D // set 3D
def.flex.dim = 3; def.spec.flex->dim = 3;
// add center point // add center point
point.push_back(0); point.push_back(0);
@@ -797,7 +814,7 @@ template <typename T> static T* VecToArray(std::vector<T>& vector, bool clear =
// make mesh // make mesh
bool mjCFlexcomp::MakeMesh(mjCModel* model, char* error, int error_sz) { bool mjCFlexcomp::MakeMesh(mjCModel* model, char* error, int error_sz) {
// strip path // strip path
if (!file.empty() && model->strippath) { if (!file.empty() && model->spec.strippath) {
file = mjuu_strippath(file); file = mjuu_strippath(file);
} }
@@ -815,12 +832,12 @@ bool mjCFlexcomp::MakeMesh(mjCModel* model, char* error, int error_sz) {
} }
// check dim // 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); return comperr(error, "Flex dim must be 2 in for mesh", error_sz);
} }
// load resource // 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; mjResource* resource = nullptr;
try { try {
@@ -850,13 +867,13 @@ bool mjCFlexcomp::MakeMesh(mjCModel* model, char* error, int error_sz) {
// LoadOBJ uses userXXX, extra processing needed // LoadOBJ uses userXXX, extra processing needed
if (isobj) { if (isobj) {
// check sizes // 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); 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); 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); 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 // load points and elements from GMSH file
bool mjCFlexcomp::MakeGMSH(mjCModel* model, char* error, int error_sz) { bool mjCFlexcomp::MakeGMSH(mjCModel* model, char* error, int error_sz) {
// strip path // strip path
if (!file.empty() && model->strippath) { if (!file.empty() && model->spec.strippath) {
file = mjuu_strippath(file); file = mjuu_strippath(file);
} }
@@ -926,7 +943,7 @@ bool mjCFlexcomp::MakeGMSH(mjCModel* model, char* error, int error_sz) {
} }
// open resource // 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; mjResource* resource = nullptr;
try { try {
@@ -1027,7 +1044,7 @@ void mjCFlexcomp::LoadGMSH(mjCModel* model, mjResource* resource) {
if (entityDim<1 || entityDim>3) { if (entityDim<1 || entityDim>3) {
throw mjCError(NULL, "Entity must be 1D, 2D or 3D"); 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 // read and discard node tags; require range from minNodeTag to maxNodeTag
for (size_t i=0; i<numNodes; i++) { for (size_t i=0; i<numNodes; i++) {
@@ -1079,7 +1096,7 @@ void mjCFlexcomp::LoadGMSH(mjCModel* model, mjResource* resource) {
if (entityDim<1 || entityDim>3) { if (entityDim<1 || entityDim>3) {
throw mjCError(NULL, "Entity must be 1D, 2D or 3D"); throw mjCError(NULL, "Entity must be 1D, 2D or 3D");
} }
def.flex.dim = entityDim; def.spec.flex->dim = entityDim;
// check section byte size // check section byte size
if (nodeend-nodebegin < 52+numNodes*4*8) { if (nodeend-nodebegin < 52+numNodes*4*8) {
@@ -1127,7 +1144,7 @@ void mjCFlexcomp::LoadGMSH(mjCModel* model, mjResource* resource) {
} }
// dimensionality must be same as nodes // dimensionality must be same as nodes
if (entityDim!=def.flex.dim) { if (entityDim!=def.spec.flex->dim) {
throw mjCError(NULL, "Inconsistent dimensionality in Elements"); throw mjCError(NULL, "Inconsistent dimensionality in Elements");
} }
@@ -1176,7 +1193,7 @@ void mjCFlexcomp::LoadGMSH(mjCModel* model, mjResource* resource) {
} }
// dimensionality must be same as nodes // dimensionality must be same as nodes
if (entityDim!=def.flex.dim) { if (entityDim!=def.spec.flex->dim) {
throw mjCError(NULL, "Inconsistent dimensionality in Elements"); throw mjCError(NULL, "Inconsistent dimensionality in Elements");
} }
src/user/user_flexcomp.h
+2 -2
View File
@@ -19,6 +19,7 @@
#include <vector> #include <vector>
#include <mujoco/mujoco.h> #include <mujoco/mujoco.h>
#include "user/user_api.h"
#include "user/user_model.h" #include "user/user_model.h"
#include "user/user_objects.h" #include "user/user_objects.h"
@@ -87,10 +88,9 @@ class mjCFlexcomp {
std::vector<float> texcoord; // vertex texture coordinates std::vector<float> texcoord; // vertex texture coordinates
// plugin support // plugin support
bool is_plugin;
std::string plugin_name; std::string plugin_name;
std::string plugin_instance_name; std::string plugin_instance_name;
mjCPlugin* plugin_instance; mjmPlugin plugin;
}; };
#endif // MUJOCO_SRC_USER_USER_FLEXCOMP_H_ #endif // MUJOCO_SRC_USER_USER_FLEXCOMP_H_
src/user/user_init.cc
+176 -7
View File
@@ -15,11 +15,66 @@
#include <cstring> #include <cstring>
#include <mujoco/mjmodel.h> #include <mujoco/mjmodel.h>
#include <mujoco/mujoco.h> #include <mujoco/mujoco.h>
#include "cc/array_safety.h"
#include "user/user_api.h" #include "user/user_api.h"
#include "user/user_util.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 // default body attributes
void mjm_defaultBody(mjmBody& body) { void mjm_defaultBody(mjmBody& body) {
memset(&body, 0, sizeof(mjmBody)); memset(&body, 0, sizeof(mjmBody));
@@ -54,8 +109,8 @@ void mjm_defaultJoint(mjmJoint& joint) {
joint.type = mjJNT_HINGE; joint.type = mjJNT_HINGE;
joint.axis[2] = 1; joint.axis[2] = 1;
joint.limited = 2; joint.limited = mjLIMITED_AUTO;
joint.actfrclimited = 2; joint.actfrclimited = mjLIMITED_AUTO;
mj_defaultSolRefImp(joint.solref_limit, joint.solimp_limit); mj_defaultSolRefImp(joint.solref_limit, joint.solimp_limit);
mj_defaultSolRefImp(joint.solref_friction, joint.solimp_friction); mj_defaultSolRefImp(joint.solref_friction, joint.solimp_friction);
joint.urdfeffort = -1; 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) { void mjm_defaultMaterial(mjmMaterial& material) {
memset(&material, 0, sizeof(mjmMaterial)); memset(&material, 0, sizeof(mjmMaterial));
material.texuniform = false; 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 // default equality attributes
void mjm_defaultEquality(mjmEquality& equality) { void mjm_defaultEquality(mjmEquality& equality) {
memset(&equality, 0, sizeof(mjmEquality)); memset(&equality, 0, sizeof(mjmEquality));
@@ -196,7 +332,7 @@ void mjm_defaultEquality(mjmEquality& equality) {
// default tendon attributes // default tendon attributes
void mjm_defaultTendon(mjmTendon& tendon) { void mjm_defaultTendon(mjmTendon& tendon) {
memset(&tendon, 0, sizeof(mjmTendon)); memset(&tendon, 0, sizeof(mjmTendon));
tendon.limited = 2; tendon.limited = mjLIMITED_AUTO;
tendon.springlength[0] = tendon.springlength[1] = -1; tendon.springlength[0] = tendon.springlength[1] = -1;
mj_defaultSolRefImp(tendon.solref_limit, tendon.solimp_limit); mj_defaultSolRefImp(tendon.solref_limit, tendon.solimp_limit);
mj_defaultSolRefImp(tendon.solref_friction, tendon.solimp_friction); mj_defaultSolRefImp(tendon.solref_friction, tendon.solimp_friction);
@@ -226,9 +362,9 @@ void mjm_defaultActuator(mjmActuator& actuator) {
actuator.gear[0] = 1; actuator.gear[0] = 1;
// input/output clamping // input/output clamping
actuator.ctrllimited = 2; actuator.ctrllimited = mjLIMITED_AUTO;
actuator.forcelimited = 2; actuator.forcelimited = mjLIMITED_AUTO;
actuator.actlimited = 2; 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 -------------------------------------------------- //------------------ class mjCMesh implementation --------------------------------------------------
mjCMesh::mjCMesh(mjCModel* _model, mjCDef* _def) { mjCMesh::mjCMesh(mjCModel* _model, mjCDef* _def) {
// set defaults mjm_defaultMesh(spec);
mjuu_setvec(refpos_, 0, 0, 0);
mjuu_setvec(refquat_, 1, 0, 0, 0);
mjuu_setvec(scale_, 1, 1, 1);
smoothnormal_ = false;
// clear internal variables // clear internal variables
mjuu_setvec(pos_surface_, 0, 0, 0); mjuu_setvec(pos_surface_, 0, 0, 0);
@@ -179,9 +175,50 @@ mjCMesh::mjCMesh(mjCModel* _model, mjCDef* _def) {
model = _model; model = _model;
def = (_def ? _def : (_model ? _model->defaults[0] : 0)); def = (_def ? _def : (_model ? _model->defaults[0] : 0));
// in case this body is not compiled
CopyFromSpec();
// point to local (needs to be after defaults) // point to local (needs to be after defaults)
plugin.name = (mjString)&plugin_name; PointToLocal();
plugin.instance_name = (mjString)&plugin_instance_name; }
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) { void mjCMesh::set_needhull(bool needhull) {
needhull_ = needhull; needhull_ = needhull;
} }
@@ -303,14 +250,14 @@ void mjCMesh::LoadSDF() {
name.c_str(), id); 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)", throw mjCError(this, "attribute scale is not compatible with SDFs in mesh '%s', (id = %d)",
name.c_str(), id); name.c_str(), id);
} }
mjCPlugin* plugin_instance = (mjCPlugin*)plugin.instance; mjCPlugin* plugin_instance = (mjCPlugin*)plugin.instance;
model->ResolvePlugin(this, plugin_name, plugin_instance_name, &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)) { if (!(pplugin->capabilityflags & mjPLUGIN_SDF)) {
throw mjCError(this, "plugin '%s' does not support signed distance fields", pplugin->name); 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)); userface.push_back(mesh.indices.at(i));
} }
set_uservert(uservert); uservert_ = std::move(uservert);
set_usernormal(usernormal); usernormal_ = std::move(usernormal);
set_userface(userface); userface_ = std::move(userface);
delete[] field; delete[] field;
} }
@@ -381,6 +328,8 @@ void mjCMesh::LoadSDF() {
// compiler // compiler
void mjCMesh::Compile(const mjVFS* vfs) { void mjCMesh::Compile(const mjVFS* vfs) {
CopyFromSpec();
// load file // load file
if (!file_.empty()) { if (!file_.empty()) {
// remove path from file if necessary // 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()); 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); mjResource* resource = LoadResource(filename, vfs);
try { try {
@@ -957,7 +906,7 @@ void mjCMesh::LoadOBJ(mjResource* resource) {
if (!objReader.GetShapes().empty()) { if (!objReader.GetShapes().empty()) {
const auto& mesh = objReader.GetShapes()[0].mesh; 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 // iterate over mesh faces
std::vector<tinyobj::index_t> face_indices; std::vector<tinyobj::index_t> face_indices;
@@ -1005,7 +954,7 @@ void mjCMesh::LoadOBJ(mjResource* resource) {
// load STL binary mesh // load STL binary mesh
void mjCMesh::LoadSTL(mjResource* resource) { 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 // get file data in buffer
char* buffer = 0; char* buffer = 0;
@@ -1085,7 +1034,7 @@ void mjCMesh::LoadSTL(mjResource* resource) {
// load MSH binary mesh // load MSH binary mesh
void mjCMesh::LoadMSH(mjResource* resource) { 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 // get file data in buffer
char* buffer = 0; char* buffer = 0;
@@ -1193,9 +1142,9 @@ void mjCMesh::ComputeVolume(double CoM[3], mjtGeomInertia type,
// apply transformations // apply transformations
void mjCMesh::ApplyTransformations() { void mjCMesh::ApplyTransformations() {
// translate // translate
if (refpos_[0]!=0 || refpos_[1]!=0 || refpos_[2]!=0) { if (refpos[0]!=0 || refpos[1]!=0 || refpos[2]!=0) {
// prepare translation // 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 // process vertices
for (int i=0; i<nvert_; i++) { for (int i=0; i<nvert_; i++) {
@@ -1206,9 +1155,9 @@ void mjCMesh::ApplyTransformations() {
} }
// rotate // 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 // 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]; mjtNum mat[9];
mju_normalize4(quat); mju_normalize4(quat);
mju_quat2Mat(mat, quat); mju_quat2Mat(mat, quat);
@@ -1233,17 +1182,17 @@ void mjCMesh::ApplyTransformations() {
} }
// scale // 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++) { for (int i=0; i<nvert_; i++) {
vert_[3*i] *= scale_[0]; vert_[3*i] *= scale[0];
vert_[3*i+1] *= scale_[1]; vert_[3*i+1] *= scale[1];
vert_[3*i+2] *= scale_[2]; vert_[3*i+2] *= scale[2];
} }
for (int i=0; i<nnormal_; i++) { for (int i=0; i<nnormal_; i++) {
normal_[3*i] *= scale_[0]; normal_[3*i] *= scale[0];
normal_[3*i+1] *= scale_[1]; normal_[3*i+1] *= scale[1];
normal_[3*i+2] *= scale_[2]; normal_[3*i+2] *= scale[2];
} }
} }
@@ -1764,7 +1713,7 @@ void mjCMesh::MakeNormal(void) {
} }
// remove large-angle faces // remove large-angle faces
if (!smoothnormal_) { if (!smoothnormal) {
// allocate removal and clear // allocate removal and clear
float* nremove = (float*) mju_malloc(3*nnormal_*sizeof(float)); float* nremove = (float*) mju_malloc(3*nnormal_*sizeof(float));
memset(nremove, 0, 3*nnormal_*sizeof(float)); memset(nremove, 0, 3*nnormal_*sizeof(float));
@@ -1880,45 +1829,92 @@ void mjCMesh::MakeCenter(void) {
// constructor // constructor
mjCSkin::mjCSkin(mjCModel* _model) { mjCSkin::mjCSkin(mjCModel* _model) {
mjm_defaultSkin(spec);
// set model pointer // set model pointer
model = _model; model = _model;
// clear data // clear data
file.clear(); spec_file_.clear();
material_.clear(); spec_material_.clear();
rgba[0] = rgba[1] = rgba[2] = 0.5f; spec_vert_.clear();
rgba[3] = 1.0f; spec_texcoord_.clear();
inflate = 0; spec_face_.clear();
group = 0; 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(); bodyid.clear();
matid = -1; 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 // destructor
mjCSkin::~mjCSkin() { mjCSkin::~mjCSkin() {
file.clear(); spec_file_.clear();
material_.clear(); spec_material_.clear();
vert.clear(); spec_vert_.clear();
texcoord.clear(); spec_texcoord_.clear();
face.clear(); spec_face_.clear();
bodyname.clear(); spec_bodyname_.clear();
bindpos.clear(); spec_bindpos_.clear();
bindquat.clear(); spec_bindquat_.clear();
vertid.clear(); spec_vertid_.clear();
vertweight.clear(); spec_vertweight_.clear();
bodyid.clear(); bodyid.clear();
} }
@@ -1926,33 +1922,35 @@ mjCSkin::~mjCSkin() {
// compiler // compiler
void mjCSkin::Compile(const mjVFS* vfs) { void mjCSkin::Compile(const mjVFS* vfs) {
CopyFromSpec();
// load file // load file
if (!file.empty()) { if (!file_.empty()) {
// make sure data is not present // make sure data is not present
if (!vert.empty() || if (!vert_.empty() ||
!texcoord.empty() || !texcoord_.empty() ||
!face.empty() || !face_.empty() ||
!bodyname.empty() || !bodyname_.empty() ||
!bindpos.empty() || !bindpos_.empty() ||
!bindquat.empty() || !bindquat_.empty() ||
!vertid.empty() || !vertid_.empty() ||
!vertweight.empty() || !vertweight_.empty() ||
!bodyid.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 // remove path from file if necessary
if (model->strippath) { if (model->strippath) {
file = mjuu_strippath(file); file_ = mjuu_strippath(file_);
} }
// load SKN // load SKN
string ext = mjuu_getext(file); string ext = mjuu_getext(file_);
if (strcasecmp(ext.c_str(), ".skn")) { 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); mjResource* resource = LoadResource(filename, vfs);
try { try {
@@ -1965,48 +1963,48 @@ void mjCSkin::Compile(const mjVFS* vfs) {
} }
// make sure all data is present // make sure all data is present
if (vert.empty() || if (vert_.empty() ||
face.empty() || face_.empty() ||
bodyname.empty() || bodyname_.empty() ||
bindpos.empty() || bindpos_.empty() ||
bindquat.empty() || bindquat_.empty() ||
vertid.empty() || vertid_.empty() ||
vertweight.empty()) { vertweight_.empty()) {
throw mjCError(this, "Missing data in skin"); throw mjCError(this, "Missing data in skin");
} }
// check mesh sizes // check mesh sizes
if (vert.size()%3) { if (vert_.size()%3) {
throw mjCError(this, "Vertex data must be multiple of 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"); 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"); throw mjCError(this, "Face data must be multiple of 3");
} }
// check bone sizes // check bone sizes
size_t nbone = bodyname.size(); size_t nbone = bodyname_.size();
if (bindpos.size()!=3*nbone) { if (bindpos_.size()!=3*nbone) {
throw mjCError(this, "Unexpected bindpos size in skin"); 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"); throw mjCError(this, "Unexpected bindquat size in skin");
} }
if (vertid.size()!=nbone) { if (vertid_.size()!=nbone) {
throw mjCError(this, "Unexpected vertid size in skin"); throw mjCError(this, "Unexpected vertid size in skin");
} }
if (vertweight.size()!=nbone) { if (vertweight_.size()!=nbone) {
throw mjCError(this, "Unexpected vertweight size in skin"); throw mjCError(this, "Unexpected vertweight size in skin");
} }
// resolve body names // resolve body names
bodyid.resize(nbone); bodyid.resize(nbone);
for (int i=0; i<nbone; i++) { 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) { 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; bodyid[i] = pbody->id;
} }
@@ -2021,28 +2019,28 @@ void mjCSkin::Compile(const mjVFS* vfs) {
// set total vertex weights to 0 // set total vertex weights to 0
vector<float> vw; vector<float> vw;
size_t nvert = vert.size()/3; size_t nvert = vert_.size()/3;
vw.resize(nvert); vw.resize(nvert);
fill(vw.begin(), vw.end(), 0.0f); fill(vw.begin(), vw.end(), 0.0f);
// accumulate vertex weights from all bones // accumulate vertex weights from all bones
for (int i=0; i<nbone; i++) { for (int i=0; i<nbone; i++) {
// make sure bone has vertices and sizes match // make sure bone has vertices and sizes match
size_t nbv = vertid[i].size(); size_t nbv = vertid_[i].size();
if (vertweight[i].size()!=nbv || nbv==0) { if (vertweight_[i].size()!=nbv || nbv==0) {
throw mjCError(this, "vertid and vertweight must have same non-zero size in skin"); throw mjCError(this, "vertid and vertweight must have same non-zero size in skin");
} }
// accumulate weights in global array // accumulate weights in global array
for (int j=0; j<nbv; j++) { for (int j=0; j<nbv; j++) {
// get index and check range // get index and check range
int jj = vertid[i][j]; int jj = vertid_[i][j];
if (jj<0 || jj>=nvert) { if (jj<0 || jj>=nvert) {
throw mjCError(this, "vertid %d out of range in skin", NULL, jj); throw mjCError(this, "vertid %d out of range in skin", NULL, jj);
} }
// accumulate // accumulate
vw[jj] += vertweight[i][j]; vw[jj] += vertweight_[i][j];
} }
} }
@@ -2055,25 +2053,25 @@ void mjCSkin::Compile(const mjVFS* vfs) {
// normalize vertex weights // normalize vertex weights
for (int i=0; i<nbone; i++) { for (int i=0; i<nbone; i++) {
for (int j=0; j<vertid[i].size(); j++) { for (int j=0; j<vertid_[i].size(); j++) {
vertweight[i][j] /= vw[vertid[i][j]]; vertweight_[i][j] /= vw[vertid_[i][j]];
} }
} }
// normalize bindquat // normalize bindquat
for (int i=0; i<nbone; i++) { for (int i=0; i<nbone; i++) {
mjtNum quat[4] = { mjtNum quat[4] = {
(mjtNum)bindquat[4*i], (mjtNum)bindquat_[4*i],
(mjtNum)bindquat[4*i+1], (mjtNum)bindquat_[4*i+1],
(mjtNum)bindquat[4*i+2], (mjtNum)bindquat_[4*i+2],
(mjtNum)bindquat[4*i+3] (mjtNum)bindquat_[4*i+3]
}; };
mju_normalize4(quat); mju_normalize4(quat);
bindquat[4*i] = (float) quat[0]; bindquat_[4*i] = (float) quat[0];
bindquat[4*i+1] = (float) quat[1]; bindquat_[4*i+1] = (float) quat[1];
bindquat[4*i+2] = (float) quat[2]; bindquat_[4*i+2] = (float) quat[2];
bindquat[4*i+3] = (float) quat[3]; bindquat_[4*i+3] = (float) quat[3];
} }
} }
@@ -2117,31 +2115,31 @@ void mjCSkin::LoadSKN(mjResource* resource) {
// copy vert // copy vert
if (nvert) { if (nvert) {
vert.resize(3*nvert); vert_.resize(3*nvert);
memcpy(vert.data(), pdata+cnt, 3*nvert*sizeof(float)); memcpy(vert_.data(), pdata+cnt, 3*nvert*sizeof(float));
cnt += 3*nvert; cnt += 3*nvert;
} }
// copy texcoord // copy texcoord
if (ntexcoord) { if (ntexcoord) {
texcoord.resize(2*ntexcoord); texcoord_.resize(2*ntexcoord);
memcpy(texcoord.data(), pdata+cnt, 2*ntexcoord*sizeof(float)); memcpy(texcoord_.data(), pdata+cnt, 2*ntexcoord*sizeof(float));
cnt += 2*ntexcoord; cnt += 2*ntexcoord;
} }
// copy face // copy face
if (nface) { if (nface) {
face.resize(3*nface); face_.resize(3*nface);
memcpy(face.data(), pdata+cnt, 3*nface*sizeof(int)); memcpy(face_.data(), pdata+cnt, 3*nface*sizeof(int));
cnt += 3*nface; cnt += 3*nface;
} }
// allocate bone arrays // allocate bone arrays
bodyname.clear(); bodyname_.clear();
bindpos.resize(3*nbone); bindpos_.resize(3*nbone);
bindquat.resize(4*nbone); bindquat_.resize(4*nbone);
vertid.resize(nbone); vertid_.resize(nbone);
vertweight.resize(nbone); vertweight_.resize(nbone);
// read bones // read bones
for (int i=0; i<nbone; i++) { for (int i=0; i<nbone; i++) {
@@ -2155,14 +2153,14 @@ void mjCSkin::LoadSKN(mjResource* resource) {
strncpy(txt, (char*)(pdata+cnt), 39); strncpy(txt, (char*)(pdata+cnt), 39);
txt[39] = '\0'; txt[39] = '\0';
cnt += 10; cnt += 10;
bodyname.push_back(txt); bodyname_.push_back(txt);
// read bindpos // read bindpos
memcpy(bindpos.data()+3*i, pdata+cnt, 3*sizeof(float)); memcpy(bindpos_.data()+3*i, pdata+cnt, 3*sizeof(float));
cnt += 3; cnt += 3;
// read bind quat // 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; cnt += 4;
// read vertex count // read vertex count
@@ -2182,13 +2180,13 @@ void mjCSkin::LoadSKN(mjResource* resource) {
} }
// read vertid // read vertid
vertid[i].resize(vcount); vertid_[i].resize(vcount);
memcpy(vertid[i].data(), (int*)(pdata+cnt), vcount*sizeof(int)); memcpy(vertid_[i].data(), (int*)(pdata+cnt), vcount*sizeof(int));
cnt += vcount; cnt += vcount;
// read vertweight // read vertweight
vertweight[i].resize(vcount); vertweight_[i].resize(vcount);
memcpy(vertweight[i].data(), (int*)(pdata+cnt), vcount*sizeof(int)); memcpy(vertweight_[i].data(), (int*)(pdata+cnt), vcount*sizeof(int));
cnt += vcount; cnt += vcount;
} }
@@ -2222,34 +2220,11 @@ constexpr int eledge[3][6][2] = {{{ 0, 1}, {-1, -1}, {-1, -1},
// constructor // constructor
mjCFlex::mjCFlex(mjCModel* _model) { mjCFlex::mjCFlex(mjCModel* _model) {
mjm_defaultFlex(spec);
// set model // set model
model = _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 // clear internal variables
nvert = 0; nvert = 0;
nedge = 0; nedge = 0;
@@ -2257,50 +2232,84 @@ mjCFlex::mjCFlex(mjCModel* _model) {
matid = -1; matid = -1;
rigid = false; rigid = false;
centered = 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 { bool mjCFlex::HasTexcoord() const {
return !texcoord.empty(); return !texcoord_.empty();
} }
void mjCFlex::DelTexcoord() { void mjCFlex::DelTexcoord() {
texcoord.clear(); texcoord_.clear();
} }
// compiler // compiler
void mjCFlex::Compile(const mjVFS* vfs) { void mjCFlex::Compile(const mjVFS* vfs) {
CopyFromSpec();
// set nelem; check sizes // set nelem; check sizes
if (dim<1 || dim>3) { if (dim<1 || dim>3) {
throw mjCError(this, "dim must be 1, 2 or 3"); throw mjCError(this, "dim must be 1, 2 or 3");
} }
if (elem.empty()) { if (elem_.empty()) {
throw mjCError(this, "elem is 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)"); throw mjCError(this, "elem size must be multiple of (dim+1)");
} }
if (vertbody.empty()) { if (vertbody_.empty()) {
throw mjCError(this, "vertbody is 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"); throw mjCError(this, "vert size must be a multiple of 3");
} }
if (edgestiffness>0 && dim>1) { if (edgestiffness>0 && dim>1) {
throw mjCError(this, "edge stiffness only available for dim=1, please use elasticity plugins"); 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 // set nvert, rigid, centered; check size
if (vert.empty()) { if (vert_.empty()) {
centered = true; centered = true;
nvert = (int)vertbody.size(); nvert = (int)vertbody_.size();
} }
else { else {
nvert = (int)vert.size()/3; nvert = (int)vert_.size()/3;
if (vertbody.size()==1) { if (vertbody_.size()==1) {
rigid = true; rigid = true;
} }
} }
@@ -2309,14 +2318,14 @@ void mjCFlex::Compile(const mjVFS* vfs) {
} }
// check elem vertex ids // check elem vertex ids
for (int i=0; i<(int)elem.size(); i++) { for (int i=0; i<(int)elem_.size(); i++) {
if (elem[i]<0 || elem[i]>=nvert) { if (elem_[i]<0 || elem_[i]>=nvert) {
throw mjCError(this, "elem vertex id out of range"); throw mjCError(this, "elem vertex id out of range");
} }
} }
// check texcoord // 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"); throw mjCError(this, "two texture coordinates per vertex expected");
} }
@@ -2329,20 +2338,20 @@ void mjCFlex::Compile(const mjVFS* vfs) {
} }
// resolve body ids // resolve body ids
for (int i=0; i<(int)vertbody.size(); i++) { for (int i=0; i<(int)vertbody_.size(); i++) {
mjCBase* pbody = model->FindObject(mjOBJ_BODY, vertbody[i]); mjCBase* pbody = model->FindObject(mjOBJ_BODY, vertbody_[i]);
if (pbody) { if (pbody) {
vertbodyid.push_back(pbody->id); vertbodyid.push_back(pbody->id);
} else { } 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 // 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 // make sorted copy of element
vector<int> el; 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()); std::sort(el.begin(), el.end());
// check for repeated vertices // check for repeated vertices
@@ -2367,8 +2376,8 @@ void mjCFlex::Compile(const mjVFS* vfs) {
// determine centered if not already set // determine centered if not already set
if (!centered) { if (!centered) {
centered = true; centered = true;
for (int i=0; i<(int)vert.size(); i++) { for (int i=0; i<(int)vert_.size(); i++) {
if (vert[i]!=0) { if (vert_[i]!=0) {
centered = false; centered = false;
break; break;
} }
@@ -2385,7 +2394,7 @@ void mjCFlex::Compile(const mjVFS* vfs) {
// add vertex offset within body if not centered // add vertex offset within body if not centered
if (!centered) { if (!centered) {
mjtNum offset[3]; 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); 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) // faces are (0,1,2); (0,2,3); (0,3,1); (1,3,2)
if (dim==3) { if (dim==3) {
for (int e=0; e<nelem; e++) { 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* v0 = vertxpos.data() + 3*edata[0];
mjtNum* v1 = vertxpos.data() + 3*edata[1]; mjtNum* v1 = vertxpos.data() + 3*edata[1];
mjtNum* v2 = vertxpos.data() + 3*edata[2]; mjtNum* v2 = vertxpos.data() + 3*edata[2];
@@ -2408,22 +2417,22 @@ void mjCFlex::Compile(const mjVFS* vfs) {
mju_cross(nrm, v01, v02); mju_cross(nrm, v01, v02);
if (mju_dot3(nrm, v03)>0) { if (mju_dot3(nrm, v03)>0) {
// flip orientation // flip orientation
int tmp = elem[e*(dim+1)+1]; int tmp = elem_[e*(dim+1)+1];
elem[e*(dim+1)+1] = elem[e*(dim+1)+2]; elem_[e*(dim+1)+1] = elem_[e*(dim+1)+2];
elem[e*(dim+1)+2] = tmp; elem_[e*(dim+1)+2] = tmp;
} }
} }
} }
// create edges // 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 // map from edge vertices to their index in `edges` vector
std::unordered_map<std::pair<int, int>, int, PairHash> edge_indices; std::unordered_map<std::pair<int, int>, int, PairHash> edge_indices;
// insert local edges into global vector // insert local edges into global vector
for (int f = 0; f < (int)elem.size()/(dim+1); f++) { for (int f = 0; f < (int)elem_.size()/(dim+1); f++) {
int* v = elem.data() + f*(dim+1); int* v = elem_.data() + f*(dim+1);
for (int e = 0; e < kNumEdges[dim-1]; e++) { for (int e = 0; e < kNumEdges[dim-1]; e++) {
auto pair = std::pair( auto pair = std::pair(
std::min(v[eledge[dim-1][e][0]], v[eledge[dim-1][e][1]]), 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 // add plugins
std::string userface, useredge; std::string userface, useredge;
mjXUtil::Vector2String(userface, elem); mjXUtil::Vector2String(userface, elem_);
mjXUtil::Vector2String(useredge, edgeidx); mjXUtil::Vector2String(useredge, edgeidx);
for (int i=0; i<(int)vertbodyid.size(); i++) { for (int i=0; i<(int)vertbodyid.size(); i++) {
@@ -2472,12 +2481,12 @@ void mjCFlex::CreateBVH(void) {
int nbvh = 0; int nbvh = 0;
// allocate element bounding boxes // allocate element bounding boxes
elemaabb.resize(6*nelem); elemaabb_.resize(6*nelem);
tree.AllocateBoundingVolumes(nelem); tree.AllocateBoundingVolumes(nelem);
// construct element bounding boxes, add to hierarchy // construct element bounding boxes, add to hierarchy
for (int e=0; e<nelem; e++) { 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 // skip inactive in 3D
if (dim==3 && elemlayer[e]>=activelayers) { if (dim==3 && elemlayer[e]>=activelayers) {
@@ -2496,12 +2505,12 @@ void mjCFlex::CreateBVH(void) {
} }
// compute aabb (center, size) // compute aabb (center, size)
elemaabb[6*e+0] = 0.5*(xmax[0]+xmin[0]); elemaabb_[6*e+0] = 0.5*(xmax[0]+xmin[0]);
elemaabb[6*e+1] = 0.5*(xmax[1]+xmin[1]); elemaabb_[6*e+1] = 0.5*(xmax[1]+xmin[1]);
elemaabb[6*e+2] = 0.5*(xmax[2]+xmin[2]); elemaabb_[6*e+2] = 0.5*(xmax[2]+xmin[2]);
elemaabb[6*e+3] = 0.5*(xmax[0]-xmin[0]) + radius; 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+4] = 0.5*(xmax[1]-xmin[1]) + radius;
elemaabb[6*e+5] = 0.5*(xmax[2]-xmin[2]) + radius; elemaabb_[6*e+5] = 0.5*(xmax[2]-xmin[2]) + radius;
// add bounding volume for this element // add bounding volume for this element
mjCBoundingVolume* bv = tree.GetBoundingVolume(nbvh++); mjCBoundingVolume* bv = tree.GetBoundingVolume(nbvh++);
@@ -2509,7 +2518,7 @@ void mjCFlex::CreateBVH(void) {
bv->conaffinity = conaffinity; bv->conaffinity = conaffinity;
bv->quat = NULL; bv->quat = NULL;
bv->SetId(e); bv->SetId(e);
bv->aabb = elemaabb.data() + 6*e; bv->aabb = elemaabb_.data() + 6*e;
bv->pos = bv->aabb; bv->pos = bv->aabb;
} }
@@ -2533,7 +2542,7 @@ void mjCFlex::CreateShellPair(void) {
// element vertices in original (unsorted) order // element vertices in original (unsorted) order
vector<int> el; 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 // line: 2 vertex fragments
if (dim==1) { if (dim==1) {
@@ -2702,7 +2711,7 @@ void mjCFlex::CreateShellPair(void) {
// process both elements // process both elements
for (int ei=0; ei<2; ei++) { 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 // find element vertex that is not in the common fragment
for (int i=0; i<=dim; i++) { 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_blas.h"
#include "engine/engine_util_errmem.h" #include "engine/engine_util_errmem.h"
#include "engine/engine_util_misc.h" #include "engine/engine_util_misc.h"
#include "user/user_api.h"
#include "user/user_objects.h" #include "user/user_objects.h"
#include "user/user_util.h" #include "user/user_util.h"
@@ -82,40 +83,12 @@ static void copyvec(T1* dest, T2* src, int n) {
// constructor // constructor
mjCModel::mjCModel() { mjCModel::mjCModel() {
mjm_defaultModel(spec);
comment.clear(); comment.clear();
modelfiledir.clear(); modelfiledir.clear();
spec_meshdir_.clear();
//------------------------ compiler settings spec_texturedir_.clear();
autolimits = true; spec_modelname_ = "MuJoCo Model";
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;
//------------------------ auto-computed statistics //------------------------ auto-computed statistics
#ifndef MEMORY_SANITIZER #ifndef MEMORY_SANITIZER
@@ -127,28 +100,8 @@ mjCModel::mjCModel() {
center_auto[0] = center_auto[1] = center_auto[2] = 0; center_auto[0] = center_auto[1] = center_auto[2] = 0;
#endif #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; nmocap = 0;
nplugin = 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 //------------------------ private variables
cameras.clear(); cameras.clear();
lights.clear(); lights.clear();
@@ -214,6 +167,31 @@ mjCModel::mjCModel() {
object_lists[mjOBJ_TUPLE] = (std::vector<mjCBase*>*) &tuples; object_lists[mjOBJ_TUPLE] = (std::vector<mjCBase*>*) &tuples;
object_lists[mjOBJ_KEY] = (std::vector<mjCBase*>*) &keys; object_lists[mjOBJ_KEY] = (std::vector<mjCBase*>*) &keys;
object_lists[mjOBJ_PLUGIN] = (std::vector<mjCBase*>*) &plugins; 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 // use filename if name is missing
for (int i=0; i<assets.size(); i++) { for (int i=0; i<assets.size(); i++) {
assets[i]->CopyFromSpec();
if (assets[i]->name.empty()) { if (assets[i]->name.empty()) {
stripped = mjuu_strippath(assets[i]->get_file()); stripped = mjuu_strippath(assets[i]->get_file());
assets[i]->name = mjuu_stripext(stripped); assets[i]->name = mjuu_stripext(stripped);
@@ -1043,12 +1022,12 @@ void mjCModel::SetSizes(void) {
// skin counts // skin counts
for (int i=0; i<nskin; i++) { for (int i=0; i<nskin; i++) {
nskinvert += skins[i]->vert.size()/3; nskinvert += skins[i]->get_vert().size()/3;
nskintexvert += skins[i]->texcoord.size()/2; nskintexvert += skins[i]->get_texcoord().size()/2;
nskinface += skins[i]->face.size()/3; nskinface += skins[i]->get_face().size()/3;
nskinbone += skins[i]->bodyid.size(); nskinbone += skins[i]->bodyid.size();
for (int j=0; j<skins[i]->bodyid.size(); j++) { 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; for (int i=0; i<nnumeric; i++) nnumericdata += numerics[i]->size;
// ntextdata // 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 // 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 // npluginattr
for (int i=0; i<nplugin; i++) npluginattr += (int)plugins[i]->flattened_attributes.size(); for (int i=0; i<nplugin; i++) npluginattr += (int)plugins[i]->flattened_attributes.size();
// nnames // 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<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<njnt; i++) nnames += (int)joints[i]->name.length() + 1;
for (int i=0; i<ngeom; i++) nnames += (int)geoms[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 // copy names, compute name addresses
void mjCModel::CopyNames(mjModel* m) { void mjCModel::CopyNames(mjModel* m) {
// start with model name // start with model name
int adr = (int)modelname.size()+1; int adr = (int)modelname_.size()+1;
int* map_adr = m->names_map; 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); memset(m->names_map, -1, sizeof(int) * m->nnames_map);
// process all lists // process all lists
@@ -1995,15 +1974,15 @@ void mjCModel::CopyObjects(mjModel* m) {
m->flex_evpairnum[i] = (int)pfl->evpair.size()/2; m->flex_evpairnum[i] = (int)pfl->evpair.size()/2;
memcpy(m->flex_evpair + 2*evpair_adr, pfl->evpair.data(), pfl->evpair.size()*sizeof(int)); 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; m->flex_texcoordadr[i] = -1;
} else { } else {
m->flex_texcoordadr[i] = texcoord_adr; m->flex_texcoordadr[i] = texcoord_adr;
memcpy(m->flex_texcoord + 2*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; 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)); memcpy(m->flex_elemlayer + elem_adr, pfl->elemlayer.data(), pfl->nelem*sizeof(int));
if (m->flex_shellnum[i]) { if (m->flex_shellnum[i]) {
memcpy(m->flex_shell + shelldata_adr, pfl->shell.data(), pfl->shell.size()*sizeof(int)); 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); mju_zero(m->flex_vert + 3*vert_adr, 3*pfl->nvert);
} }
else { 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 // copy or set vertbodyid
@@ -2073,7 +2052,7 @@ void mjCModel::CopyObjects(mjModel* m) {
elemdata_adr += (pfl->dim+1) * pfl->nelem; elemdata_adr += (pfl->dim+1) * pfl->nelem;
shelldata_adr += (int)pfl->shell.size(); shelldata_adr += (int)pfl->shell.size();
evpair_adr += (int)pfl->evpair.size()/2; 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; bvh_adr += pfl->tree.nbvh;
} }
@@ -2093,25 +2072,25 @@ void mjCModel::CopyObjects(mjModel* m) {
copyvec(m->skin_rgba+4*i, psk->rgba, 4); copyvec(m->skin_rgba+4*i, psk->rgba, 4);
m->skin_inflate[i] = psk->inflate; m->skin_inflate[i] = psk->inflate;
m->skin_vertadr[i] = vert_adr; m->skin_vertadr[i] = vert_adr;
m->skin_vertnum[i] = psk->vert.size()/3; m->skin_vertnum[i] = psk->get_vert().size()/3;
m->skin_texcoordadr[i] = (!psk->texcoord.empty() ? texcoord_adr : -1); m->skin_texcoordadr[i] = (!psk->get_texcoord().empty() ? texcoord_adr : -1);
m->skin_faceadr[i] = face_adr; 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_boneadr[i] = bone_adr;
m->skin_bonenum[i] = psk->bodyid.size(); m->skin_bonenum[i] = psk->bodyid.size();
// copy mesh data // copy mesh data
memcpy(m->skin_vert + 3*vert_adr, psk->vert.data(), psk->vert.size()*sizeof(float)); memcpy(m->skin_vert + 3*vert_adr, psk->get_vert().data(), psk->get_vert().size()*sizeof(float));
if (!psk->texcoord.empty()) if (!psk->get_texcoord().empty())
memcpy(m->skin_texcoord + 2*texcoord_adr, psk->texcoord.data(), memcpy(m->skin_texcoord + 2*texcoord_adr, psk->get_texcoord().data(),
psk->texcoord.size()*sizeof(float)); psk->get_texcoord().size()*sizeof(float));
memcpy(m->skin_face + 3*face_adr, psk->face.data(), psk->face.size()*sizeof(int)); memcpy(m->skin_face + 3*face_adr, psk->get_face().data(), psk->get_face().size()*sizeof(int));
// copy bind poses and body ids // copy bind poses and body ids
memcpy(m->skin_bonebindpos+3*bone_adr, psk->bindpos.data(), memcpy(m->skin_bonebindpos+3*bone_adr, psk->get_bindpos().data(),
psk->bindpos.size()*sizeof(float)); psk->get_bindpos().size()*sizeof(float));
memcpy(m->skin_bonebindquat+4*bone_adr, psk->bindquat.data(), memcpy(m->skin_bonebindquat+4*bone_adr, psk->get_bindquat().data(),
psk->bindquat.size()*sizeof(float)); psk->get_bindquat().size()*sizeof(float));
memcpy(m->skin_bonebodyid+bone_adr, psk->bodyid.data(), memcpy(m->skin_bonebodyid+bone_adr, psk->bodyid.data(),
psk->bodyid.size()*sizeof(int)); psk->bodyid.size()*sizeof(int));
@@ -2119,13 +2098,13 @@ void mjCModel::CopyObjects(mjModel* m) {
for (int j=0; j<m->skin_bonenum[i]; j++) { for (int j=0; j<m->skin_bonenum[i]; j++) {
// set fields // set fields
m->skin_bonevertadr[bone_adr+j] = bonevert_adr; 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 // copy data
memcpy(m->skin_bonevertid+bonevert_adr, psk->vertid[j].data(), memcpy(m->skin_bonevertid+bonevert_adr, psk->get_vertid()[j].data(),
psk->vertid[j].size()*sizeof(int)); psk->get_vertid()[j].size()*sizeof(int));
memcpy(m->skin_bonevertweight+bonevert_adr, psk->vertweight[j].data(), memcpy(m->skin_bonevertweight+bonevert_adr, psk->get_vertweight()[j].data(),
psk->vertid[j].size()*sizeof(float)); psk->get_vertid()[j].size()*sizeof(float));
// advance counter // advance counter
bonevert_adr += m->skin_bonevertnum[bone_adr+j]; bonevert_adr += m->skin_bonevertnum[bone_adr+j];
@@ -2133,7 +2112,7 @@ void mjCModel::CopyObjects(mjModel* m) {
// advance mesh and bone counters // advance mesh and bone counters
vert_adr += m->skin_vertnum[i]; 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]; face_adr += m->skin_facenum[i];
bone_adr += m->skin_bonenum[i]; bone_adr += m->skin_bonenum[i];
} }
@@ -2334,10 +2313,10 @@ void mjCModel::CopyObjects(mjModel* m) {
// set fields // set fields
m->numeric_adr[i] = adr; m->numeric_adr[i] = adr;
m->numeric_size[i] = pcu->size; m->numeric_size[i] = pcu->size;
for (int j=0; j<(int)pcu->data.size(); j++) { for (int j=0; j<(int)pcu->data_.size(); j++) {
m->numeric_data[adr+j] = (mjtNum)pcu->data[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; m->numeric_data[adr+j] = 0;
} }
@@ -2353,8 +2332,8 @@ void mjCModel::CopyObjects(mjModel* m) {
// set fields // set fields
m->text_adr[i] = adr; m->text_adr[i] = adr;
m->text_size[i] = (int)pte->data.size()+1; m->text_size[i] = (int)pte->data_.size()+1;
mju_strncpy(m->text_data + adr, pte->data.c_str(), m->ntextdata - adr); mju_strncpy(m->text_data + adr, pte->data_.c_str(), m->ntextdata - adr);
// advance address counter // advance address counter
adr += m->text_size[i]; adr += m->text_size[i];
@@ -2368,11 +2347,11 @@ void mjCModel::CopyObjects(mjModel* m) {
// set fields // set fields
m->tuple_adr[i] = adr; 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++) { 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_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 // advance address counter
@@ -2383,14 +2362,14 @@ void mjCModel::CopyObjects(mjModel* m) {
for (int i=0; i<nkey; i++) { for (int i=0; i<nkey; i++) {
// copy data // copy data
m->key_time[i] = (mjtNum)keys[i]->time; m->key_time[i] = (mjtNum)keys[i]->time;
copyvec(m->key_qpos+i*nq, keys[i]->qpos.data(), nq); 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_qvel+i*nv, keys[i]->qvel_.data(), nv);
if (na) { 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) { if (nmocap) {
copyvec(m->key_mpos + i*3*nmocap, keys[i]->mpos.data(), 3*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_mquat + i*4*nmocap, keys[i]->mquat_.data(), 4*nmocap);
} }
// normalize quaternions in m->key_qpos // 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); 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) // save qpos0 in user model (to recognize changed key_qpos in write)
@@ -2742,6 +2721,8 @@ static void warninghandler(const char* msg) {
// compiler // compiler
mjModel* mjCModel::Compile(const mjVFS* vfs) { mjModel* mjCModel::Compile(const mjVFS* vfs) {
CopyFromSpec();
// The volatile keyword is necessary to prevent a possible memory leak due to // The volatile keyword is necessary to prevent a possible memory leak due to
// an interaction between longjmp and compiler optimization. Specifically, at // an interaction between longjmp and compiler optimization. Specifically, at
// the point where the setjmp takes places, these pointers have never been // 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 // append directory separator
if (!meshdir.empty()) { if (!meshdir_.empty()) {
int n = meshdir.length(); int n = meshdir_.length();
if (meshdir[n-1]!='/' && meshdir[n-1]!='\\') { if (meshdir_[n-1]!='/' && meshdir_[n-1]!='\\') {
meshdir += '/'; meshdir_ += '/';
} }
} }
if (!texturedir.empty()) { if (!texturedir_.empty()) {
int n = texturedir.length(); int n = texturedir_.length();
if (texturedir[n-1]!='/' && texturedir[n-1]!='\\') { if (texturedir_[n-1]!='/' && texturedir_[n-1]!='\\') {
texturedir += '/'; texturedir_ += '/';
} }
} }
@@ -3037,7 +3018,7 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) {
{ {
int adr = 0; int adr = 0;
for (int i = 0; i < nplugin; ++i) { 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(); const int size = plugins[i]->flattened_attributes.size();
std::memcpy(m->plugin_attr + adr, std::memcpy(m->plugin_attr + adr,
plugins[i]->flattened_attributes.data(), size); 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); copyvec(center_auto, m->stat.center, 3);
// override model statistics if defined by user // override model statistics if defined by user
if (mjuu_defined(extent)) m->stat.extent = (mjtNum)extent; if (mjuu_defined(stat.extent)) m->stat.extent = (mjtNum)stat.extent;
if (mjuu_defined(meaninertia)) m->stat.meaninertia = (mjtNum)meaninertia; if (mjuu_defined(stat.meaninertia)) m->stat.meaninertia = (mjtNum)stat.meaninertia;
if (mjuu_defined(meanmass)) m->stat.meanmass = (mjtNum)meanmass; if (mjuu_defined(stat.meanmass)) m->stat.meanmass = (mjtNum)stat.meanmass;
if (mjuu_defined(meansize)) m->stat.meansize = (mjtNum)meansize; if (mjuu_defined(stat.meansize)) m->stat.meansize = (mjtNum)stat.meansize;
if (mjuu_defined(center[0])) copyvec(m->stat.center, center, 3); if (mjuu_defined(stat.center[0])) copyvec(m->stat.center, stat.center, 3);
// assert that model has valid references // assert that model has valid references
const char* validationerr = mj_validateReferences(m); const char* validationerr = mj_validateReferences(m);
@@ -3278,14 +3259,14 @@ bool mjCModel::CopyBack(const mjModel* m) {
visual = m->vis; visual = m->vis;
// runtime-modifiable members of mjStatistic, if different from computed values // runtime-modifiable members of mjStatistic, if different from computed values
if (m->stat.meaninertia != meaninertia_auto) meaninertia = m->stat.meaninertia; if (m->stat.meaninertia != meaninertia_auto) stat.meaninertia = m->stat.meaninertia;
if (m->stat.meanmass != meanmass_auto) meanmass = m->stat.meanmass; if (m->stat.meanmass != meanmass_auto) stat.meanmass = m->stat.meanmass;
if (m->stat.meansize != meansize_auto) meansize = m->stat.meansize; if (m->stat.meansize != meansize_auto) stat.meansize = m->stat.meansize;
if (m->stat.extent != extent_auto) extent = m->stat.extent; if (m->stat.extent != extent_auto) stat.extent = m->stat.extent;
if (m->stat.center[0] != center_auto[0] || if (m->stat.center[0] != center_auto[0] ||
m->stat.center[1] != center_auto[1] || m->stat.center[1] != center_auto[1] ||
m->stat.center[2] != center_auto[2]) { m->stat.center[2] != center_auto[2]) {
mju_copy3(center, m->stat.center); mju_copy3(stat.center, m->stat.center);
} }
// qpos0, qpos_spring // qpos0, qpos_spring
@@ -3386,11 +3367,11 @@ bool mjCModel::CopyBack(const mjModel* m) {
mjCHField* phf; mjCHField* phf;
for (int i=0; i<nhfield; i++) { for (int i=0; i<nhfield; i++) {
phf = hfields[i]; phf = hfields[i];
int size = phf->userdata().size(); int size = phf->get_userdata().size();
if (size) { if (size) {
int nrow = m->hfield_nrow[i]; int nrow = m->hfield_nrow[i];
int ncol = m->hfield_ncol[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]; float* modeldata = m->hfield_data + m->hfield_adr[i];
// copy back in reverse row order // copy back in reverse row order
for (int j=0; j<nrow; j++) { for (int j=0; j<nrow; j++) {
@@ -3517,14 +3498,14 @@ bool mjCModel::CopyBack(const mjModel* m) {
// numeric data // numeric data
for (int i=0; i<nnumeric; i++) { for (int i=0; i<nnumeric; i++) {
for (int j=0; j<m->numeric_size[i]; j++) { 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 // tuple data
for (int i=0; i<ntuple; i++) { for (int i=0; i<ntuple; i++) {
for (int j=0; j<m->tuple_size[i]; j++) { 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]; mjCKey* pk = keys[i];
pk->time = (double)m->key_time[i]; pk->time = (double)m->key_time[i];
copyvec(pk->qpos.data(), m->key_qpos + i*nq, nq); copyvec(pk->qpos_.data(), m->key_qpos + i*nq, nq);
copyvec(pk->qvel.data(), m->key_qvel + i*nv, nv); copyvec(pk->qvel_.data(), m->key_qvel + i*nv, nv);
if (na) { if (na) {
copyvec(pk->act.data(), m->key_act + i*na, na); copyvec(pk->act_.data(), m->key_act + i*na, na);
} }
if (nmocap) { if (nmocap) {
copyvec(pk->mpos.data(), m->key_mpos + i*3*nmocap, 3*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->mquat_.data(), m->key_mquat + i*4*nmocap, 4*nmocap);
} }
if (nu) { 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 // implicit plugin instance
if (*plugin_instance && (*plugin_instance)->plugin_slot == -1) { if (*plugin_instance && (*plugin_instance)->spec.plugin_slot == -1) {
(*plugin_instance)->plugin_slot = plugin_slot; (*plugin_instance)->spec.plugin_slot = plugin_slot;
(*plugin_instance)->parent = obj; (*plugin_instance)->parent = obj;
} }
@@ -3581,10 +3562,10 @@ void mjCModel::ResolvePlugin(mjCBase* obj, const std::string& plugin_name,
throw mjCError( throw mjCError(
obj, "unrecognized name '%s' for plugin instance", plugin_instance_name.c_str()); 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( throw mjCError(
obj, "'plugin' attribute does not match that of the instance"); 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/mjdata.h>
#include <mujoco/mjmodel.h> #include <mujoco/mjmodel.h>
#include <mujoco/mjplugin.h> #include <mujoco/mjplugin.h>
#include <mujoco/mjtnum.h>
#include "user/user_api.h"
#include "user/user_objects.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::map<std::string, int, std::less<> > mjKeyMap;
typedef std::array<mjKeyMap, mjNOBJECT> mjListKeyMap; 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 // constructed, 'Compile' can be called to generate the corresponding mjModel object
// (which is the low-level model). The mjCModel object can then be deleted. // (which is the low-level model). The mjCModel object can then be deleted.
class mjCModel { class mjCModel : private mjmModel {
friend class mjCBody; friend class mjCBody;
friend class mjCJoint; friend class mjCCamera;
friend class mjCGeom; friend class mjCGeom;
friend class mjCFlex; friend class mjCFlex;
friend class mjCHField;
friend class mjCFrame;
friend class mjCJoint;
friend class mjCEquality;
friend class mjCMesh; friend class mjCMesh;
friend class mjCSkin; friend class mjCSkin;
friend class mjCHField;
friend class mjCPair;
friend class mjCBodyPair;
friend class mjCSite; friend class mjCSite;
friend class mjCEquality;
friend class mjCTendon; friend class mjCTendon;
friend class mjCWrap; friend class mjCTexture;
friend class mjCActuator; friend class mjCActuator;
friend class mjCSensor; friend class mjCSensor;
friend class mjCNumeric; friend class mjCDef;
friend class mjCTuple;
friend class mjCKey;
friend class mjXReader; friend class mjXReader;
friend class mjXWriter; friend class mjXWriter;
public: public:
mjCModel(); // constructor mjCModel(); // constructor
~mjCModel(); // destructor ~mjCModel(); // destructor
void CopyFromSpec(); // copy spec to private attributes
void PointToLocal();
mjmModel spec;
mjModel* Compile(const mjVFS* vfs = 0); // COMPILER: construct mjModel mjModel* Compile(const mjVFS* vfs = 0); // COMPILER: construct mjModel
bool CopyBack(const mjModel*); // DECOMPILER: copy numeric back 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 mjCBase* FindObject(mjtObj type, std::string name); // find object given type and name
bool IsNullPose(const mjtNum* pos, const mjtNum* quat); // detect null pose 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 //------------------------ API for plugins
void ResolvePlugin(mjCBase* obj, // resolve plugin instance, create a new one if needed void ResolvePlugin(mjCBase* obj, // resolve plugin instance, create a new one if needed
const std::string& plugin_name, const std::string& plugin_name,
@@ -131,53 +133,6 @@ class mjCModel {
std::string modelfiledir; // path to model file std::string modelfiledir; // path to model file
std::vector<mjCDef*> defaults; // settings for each defaults class 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: private:
void TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs); void TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs);
mjModel* _Compile(const mjVFS* vfs); mjModel* _Compile(const mjVFS* vfs);
@@ -230,7 +185,6 @@ class mjCModel {
int nnumeric; // number of numeric fields int nnumeric; // number of numeric fields
int ntext; // number of text fields int ntext; // number of text fields
int ntuple; // number of tuple fields int ntuple; // number of tuple fields
int nkey; // number of keyframes
int nmocap; // number of mocap bodies int nmocap; // number of mocap bodies
int nplugin; // number of plugin instances int nplugin; // number of plugin instances
@@ -323,5 +277,13 @@ class mjCModel {
mjCError errInfo; // last error info mjCError errInfo; // last error info
int fixCount; // how many bodies have been fixed int fixCount; // how many bodies have been fixed
std::vector<mjtNum> qpos0; // save qpos0, to recognize changed key_qpos in write 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_ #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}; 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 // error information
class [[nodiscard]] mjCError { class [[nodiscard]] mjCError {
public: public:
@@ -191,10 +173,6 @@ class mjCBase {
mjCModel* model; // pointer to model that created object mjCModel* model; // pointer to model that created object
mjCFrame* frame; // pointer to frame transformation mjCFrame* frame; // pointer to frame transformation
// plugin support
mjmPlugin plugin;
std::string plugin_name;
std::string plugin_instance_name;
protected: protected:
mjCBase(); // constructor mjCBase(); // constructor
virtual ~mjCBase() = default; // destructor virtual ~mjCBase() = default; // destructor
@@ -251,7 +229,6 @@ class mjCBody : public mjCBase, private mjmBody {
using mjCBase::name; using mjCBase::name;
using mjCBase::classname; using mjCBase::classname;
using mjCBase::info; using mjCBase::info;
using mjCBase::plugin;
// used by mjXWriter and mjCModel // used by mjXWriter and mjCModel
const std::vector<double>& get_userdata() { return userdata_; } const std::vector<double>& get_userdata() { return userdata_; }
@@ -296,6 +273,8 @@ class mjCBody : public mjCBase, private mjmBody {
void PointToLocal(void); void PointToLocal(void);
// variable-size data // variable-size data
std::string plugin_name;
std::string plugin_instance_name;
std::vector<double> userdata_; std::vector<double> userdata_;
std::vector<double> spec_userdata_; std::vector<double> spec_userdata_;
}; };
@@ -354,10 +333,10 @@ class mjCJoint : public mjCBase, private mjmJoint {
// used by mjXWriter and mjCModel // used by mjXWriter and mjCModel
const std::vector<double>& get_userdata() { return userdata_; } const std::vector<double>& get_userdata() { return userdata_; }
const double* get_range() { return range; }
// public getters bool is_limited() const;
bool is_limited() const { return limited_; } bool is_actfrclimited() const;
bool is_actfrclimited() const { return actfrclimited_; }
private: private:
@@ -367,8 +346,7 @@ class mjCJoint : public mjCBase, private mjmJoint {
void PointToLocal(void); void PointToLocal(void);
mjCBody* body; // joint's body mjCBody* body; // joint's body
bool limited_; // actual (inferred) value of limited
bool actfrclimited_; // actual (inferred) value of actfrclimited
// variable-size data // variable-size data
std::vector<double> userdata_; std::vector<double> userdata_;
std::vector<double> spec_userdata_; std::vector<double> spec_userdata_;
@@ -434,6 +412,8 @@ class mjCGeom : public mjCBase, private mjmGeom {
mjtNum fluid[mjNFLUID]; // compile-time fluid-interaction parameters mjtNum fluid[mjNFLUID]; // compile-time fluid-interaction parameters
// variable-size data // variable-size data
std::string plugin_name;
std::string plugin_instance_name;
std::string hfieldname_; std::string hfieldname_;
std::string meshname_; std::string meshname_;
std::string material_; std::string material_;
@@ -446,7 +426,6 @@ class mjCGeom : public mjCBase, private mjmGeom {
// inherited // inherited
using mjCBase::classname; using mjCBase::classname;
using mjCBase::info; using mjCBase::info;
using mjCBase::plugin;
}; };
@@ -554,6 +533,7 @@ class mjCLight : public mjCBase, private mjmLight {
mjCLight(mjCModel* = 0, mjCDef* = 0); // constructor mjCLight(mjCModel* = 0, mjCDef* = 0); // constructor
void Compile(void); // compiler void Compile(void); // compiler
void CopyFromSpec(void); void CopyFromSpec(void);
void PointToLocal(void);
mjCBody* body; // light's body mjCBody* body; // light's body
int targetbodyid; // id of target body; -1: none int targetbodyid; // id of target body; -1: none
@@ -566,7 +546,7 @@ class mjCLight : public mjCBase, private mjmLight {
//------------------------- class mjCFlex ---------------------------------------------------------- //------------------------- class mjCFlex ----------------------------------------------------------
// Describes a flex // Describes a flex
class mjCFlex: public mjCBase { class mjCFlex: public mjCBase, private mjmFlex {
friend class mjCDef; friend class mjCDef;
friend class mjCModel; friend class mjCModel;
friend class mjCFlexcomp; friend class mjCFlexcomp;
@@ -574,41 +554,24 @@ class mjCFlex: public mjCBase {
friend class mjXWriter; friend class mjXWriter;
public: public:
void set_material(std::string _material) { material_ = _material; } mjmFlex spec;
std::string& get_material() { return material_; } using mjCBase::name;
using mjCBase::classname;
using mjCBase::info;
// contact properties void CopyFromSpec(void);
int contype; // contact type void PointToLocal(void);
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 // used by mjXWriter and mjCModel
int dim; // element dimensionality const std::string& get_material() { return material_; }
double radius; // radius around primitive element const std::vector<std::string>& get_vertbody() { return vertbody_; }
bool internal; // enable internal collisions const std::vector<double>& get_vert() { return vert_; }
bool flatskin; // render flex skin with flat shading const std::vector<double>& get_elemaabb() { return elemaabb_; }
int selfcollide; // mode for flex self colllision const std::vector<int>& get_elem() { return elem_; }
int activelayers; // number of active element layers in 3D const std::vector<float>& get_texcoord() { return texcoord_; }
int group; // group for visualizatioh
double edgestiffness; // edge stiffness
double edgedamping; // edge damping
float rgba[4]; // rgba when material is omitted
std::vector<std::string> vertbody; // vertex body names bool HasTexcoord() const; // texcoord not null
std::vector<mjtNum> vert; // vertex positions void DelTexcoord(); // delete texcoord
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
private: private:
mjCFlex(mjCModel* = 0); mjCFlex(mjCModel* = 0);
@@ -629,7 +592,20 @@ class mjCFlex: public mjCBase {
std::vector<int> evpair; // element-vertex pairs std::vector<int> evpair; // element-vertex pairs
std::vector<mjtNum> vertxpos; // global vertex positions std::vector<mjtNum> vertxpos; // global vertex positions
mjCBoundingVolumeHierarchy tree; // bounding volume hierarchy 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 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 ---------------------------------------------------------- //------------------------- class mjCMesh ----------------------------------------------------------
// Describes a mesh // Describes a mesh
class mjCMesh: public mjCBase { class mjCMesh: public mjCBase, private mjmMesh {
friend class mjCFlexcomp; friend class mjCFlexcomp;
friend class mjXWriter;
public: public:
mjCMesh(mjCModel* = 0, mjCDef* = 0); mjCMesh(mjCModel* = 0, mjCDef* = 0);
~mjCMesh(); ~mjCMesh();
// public getters mjmMesh spec;
const std::string& content_type() const { return content_type_; } using mjCBase::name;
const std::string& file() const { return file_; } 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 std::string& get_file() const { return file_; }
const double* refpos() const { return refpos_; } const double* get_refpos() const { return refpos; }
const double* refquat() const { return refquat_; } const double* get_refquat() const { return refquat; }
const double* scale() const { return scale_; } const double* get_scale() const { return scale; }
bool smoothnormal() const { return smoothnormal_; } bool get_smoothnormal() const { return smoothnormal; }
void set_needhull(bool needhull);
// public getters for user data // public getters for user data
const std::vector<float>& uservert() const { return uservert_; } const std::vector<float>& get_uservert() const { return uservert_; }
const std::vector<float>& usernormal() const { return usernormal_; } const std::vector<float>& get_usernormal() const { return usernormal_; }
const std::vector<float>& usertexcoord() const { return usertexcoord_; } const std::vector<float>& get_usertexcoord() const { return usertexcoord_; }
const std::vector<int>& userface() const { return userface_; } const std::vector<int>& get_userface() const { return userface_; }
// mesh properties computed by Compile // mesh properties computed by Compile
const double* aamm() const { return aamm_; } const double* aamm() const { return aamm_; }
@@ -673,24 +658,6 @@ class mjCMesh: public mjCBase {
// bounding volume hierarchy tree // bounding volume hierarchy tree
const mjCBoundingVolumeHierarchy& tree() { return 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 void Compile(const mjVFS* vfs); // compiler
double* GetPosPtr(mjtGeomInertia type); // get position double* GetPosPtr(mjtGeomInertia type); // get position
double* GetQuatPtr(mjtGeomInertia type); // get orientation double* GetQuatPtr(mjtGeomInertia type); // get orientation
@@ -716,19 +683,28 @@ class mjCMesh: public mjCBase {
void SetBoundingVolume(int faceid); void SetBoundingVolume(int faceid);
private: private:
bool visual_; // true: the mesh is only visual // variable size attributes
std::string content_type_; // content type of file std::string plugin_name;
std::string file_; // mesh file std::string plugin_instance_name;
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
std::string content_type_; // content type of file
std::string file_; // mesh file
std::vector<float> uservert_; // user vertex data std::vector<float> uservert_; // user vertex data
std::vector<float> usernormal_; // user normal data std::vector<float> usernormal_; // user normal data
std::vector<float> usertexcoord_; // user texcoord data std::vector<float> usertexcoord_; // user texcoord data
std::vector<int> userface_; // user vertex indices std::vector<int> userface_; // user vertex indices
std::vector<int> userfacenormal_; // user normal 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<int> userfacetexcoord_; // user texcoord indices
std::vector< std::pair<int, int> > useredge_; // user half-edge data std::vector< std::pair<int, int> > useredge_; // user half-edge data
@@ -797,32 +773,30 @@ class mjCMesh: public mjCBase {
//------------------------- class mjCSkin ---------------------------------------------------------- //------------------------- class mjCSkin ----------------------------------------------------------
// Describes a skin // Describes a skin
class mjCSkin: public mjCBase { class mjCSkin: public mjCBase, private mjmSkin {
friend class mjCModel; friend class mjCModel;
friend class mjXWriter; friend class mjXWriter;
public: public:
std::string get_file() const { return file; } mjmSkin spec;
void set_material(std::string _material) { material_ = _material; } using mjCBase::name;
using mjCBase::classname;
using mjCBase::info;
std::string get_file() const { return file_; }
std::string& get_material() { return material_; } 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(); } void del_material() { material_.clear(); }
std::string file; // skin file void CopyFromSpec();
float rgba[4]; // rgba when material is omitted void PointToLocal();
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
private: private:
mjCSkin(mjCModel* = 0); // constructor mjCSkin(mjCModel* = 0); // constructor
@@ -830,7 +804,29 @@ class mjCSkin: public mjCBase {
void Compile(const mjVFS* vfs); // compiler void Compile(const mjVFS* vfs); // compiler
void LoadSKN(mjResource* resource); // load skin in SKN BIN format 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 int matid; // material id
std::vector<int> bodyid; // body ids std::vector<int> bodyid; // body ids
}; };
@@ -840,34 +836,39 @@ class mjCSkin: public mjCBase {
//------------------------- class mjCHField -------------------------------------------------------- //------------------------- class mjCHField --------------------------------------------------------
// Describes a height field // Describes a height field
class mjCHField : public mjCBase { class mjCHField : public mjCBase, private mjmHField {
friend class mjCGeom;
friend class mjCModel; friend class mjCModel;
friend class mjXWriter; friend class mjXWriter;
public: public:
std::string get_file() const { return file; } mjmHField spec;
using mjCBase::name;
using mjCBase::info;
std::string content_type; // content type of file void CopyFromSpec(void);
std::string file; // file: (nrow, ncol, [elevation data]) void PointToLocal(void);
double size[4]; // hfield size (ignore referencing geom size)
int nrow; // number of rows std::string get_file() const { return file_; }
int ncol; // number of columns
float* data; // elevation data, row-major format
// getter for user data // getter for user data
std::vector<float>& userdata() { return userdata_; } std::vector<float>& get_userdata() { return userdata_; }
// setter for user data
void set_userdata(std::optional<std::vector<float>>&& userdata);
private: private:
mjCHField(mjCModel* model); // constructor mjCHField(mjCModel* model); // constructor
~mjCHField(); // destructor ~mjCHField(); // destructor
std::vector<float> userdata_; // user-provided elevation data float* data; // elevation data, row-major format
void Compile(const mjVFS* vfs); // compiler void Compile(const mjVFS* vfs); // compiler
void LoadCustom(mjResource* resource); // load from custom format void LoadCustom(mjResource* resource); // load from custom format
void LoadPNG(mjResource* resource); // load from PNG 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 ------------------------------------------------------- //------------------------- class mjCTexture -------------------------------------------------------
// Describes a texture // Describes a texture
class mjCTexture : public mjCBase { class mjCTexture : public mjCBase, private mjmTexture {
friend class mjCModel; friend class mjCModel;
friend class mjXReader; friend class mjXReader;
friend class mjXWriter; friend class mjXWriter;
@@ -883,32 +884,17 @@ class mjCTexture : public mjCBase {
public: public:
~mjCTexture(); // destructor ~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 std::string get_file() const { return file_; }
mjtBuiltin builtin; // builtin type std::string get_content_type() const { return content_type_; }
mjtMark mark; // mark type std::vector<std::string> get_cubefiles() const { return cubefiles_; }
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
private: private:
mjCTexture(mjCModel*); // constructor mjCTexture(mjCModel*); // constructor
@@ -932,6 +918,13 @@ class mjCTexture : public mjCBase {
unsigned int& w, unsigned int& h); unsigned int& w, unsigned int& h);
mjtByte* rgb; // rgb data 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 ---------------------------------------------------------- //------------------------- class mjCPair ----------------------------------------------------------
// Predefined geom pair for collision detection // Predefined geom pair for collision detection
class mjCPair : public mjCBase { class mjCPair : public mjCBase, private mjmPair {
friend class mjCDef; friend class mjCDef;
friend class mjCBody; friend class mjCBody;
friend class mjCModel; friend class mjCModel;
friend class mjXWriter;
public: public:
// parameters set by user mjmPair spec;
std::string geomname1; // name of geom 1 using mjCBase::name;
std::string geomname2; // name of geom 2 using mjCBase::classname;
using mjCBase::info;
// optional parameters: computed from geoms if not set by user void CopyFromSpec();
int condim; // contact dimensionality void PointToLocal();
mjtNum solref[mjNREF]; // solver reference, normal direction
mjtNum solreffriction[mjNREF]; // solver reference, frictional directions std::string get_geomname1() { return geomname1_; }
mjtNum solimp[mjNIMP]; // solver impedance std::string get_geomname2() { return geomname2_; }
double margin; // margin for contact detection
double gap; // include in solver if dist<margin-gap
double friction[5]; // full contact friction
int GetSignature(void) { int GetSignature(void) {
return signature; return signature;
@@ -1000,6 +992,11 @@ class mjCPair : public mjCBase {
mjCGeom* geom1; // geom1 mjCGeom* geom1; // geom1
mjCGeom* geom2; // geom2 mjCGeom* geom2; // geom2
int signature; // body1<<16 + body2 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 ------------------------------------------------------ //------------------------- class mjCBodyPair ------------------------------------------------------
// Body pair specification, use to exclude pairs // Body pair specification, use to exclude pairs
class mjCBodyPair : public mjCBase { class mjCBodyPair : public mjCBase, private mjmExclude {
friend class mjCBody; friend class mjCBody;
friend class mjCModel; friend class mjCModel;
public: public:
// parameters set by user mjmExclude spec;
std::string bodyname1; // name of geom 1 using mjCBase::name;
std::string bodyname2; // name of geom 2 using mjCBase::info;
void CopyFromSpec();
void PointToLocal();
std::string get_bodyname1() const { return bodyname1_; }
std::string get_bodyname2() const { return bodyname2_; }
int GetSignature(void) { int GetSignature(void) {
return signature; return signature;
} }
private: private:
mjCBodyPair(mjCModel*); // constructor mjCBodyPair(mjCModel*); // constructor
void Compile(void); // compiler void Compile(void); // compiler
int body1; // id of body1 int body1; // id of body1
int body2; // id of body2 int body2; // id of body2
int signature; // body1<<16 + 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 // used by mjXWriter and mjCModel
const std::vector<double>& get_userdata() { return userdata_; } const std::vector<double>& get_userdata() { return userdata_; }
const double* get_range() { return range; }
void CopyFromSpec(); void CopyFromSpec();
void PointToLocal(); void PointToLocal();
// public getters bool is_limited() const;
bool is_limited() const { return limited_; }
private: private:
mjCTendon(mjCModel* = 0, mjCDef* = 0); // constructor mjCTendon(mjCModel* = 0, mjCDef* = 0); // constructor
@@ -1108,7 +1116,6 @@ class mjCTendon : public mjCBase, private mjmTendon {
void Compile(void); // compiler void Compile(void); // compiler
int matid; // material id for rendering int matid; // material id for rendering
bool limited_; // actual (inferred) value of limited
// variable-size data // variable-size data
std::string material_; std::string material_;
@@ -1157,7 +1164,7 @@ class mjCPlugin : public mjCBase {
friend class mjXWriter; friend class mjXWriter;
public: public:
int plugin_slot; // global registered slot number of the plugin mjmPlugin spec;
int nstate; // state size for the plugin instance int nstate; // state size for the plugin instance
mjCBase* parent; // parent object (only used when generating error message) 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 std::map<std::string, std::string, std::less<>> config_attribs; // raw config attributes from XML
@@ -1166,6 +1173,7 @@ class mjCPlugin : public mjCBase {
private: private:
mjCPlugin(mjCModel*); // constructor mjCPlugin(mjCModel*); // constructor
void Compile(void); // compiler void Compile(void); // compiler
std::string instance_name;
}; };
@@ -1183,7 +1191,6 @@ class mjCActuator : public mjCBase, private mjmActuator {
using mjCBase::name; using mjCBase::name;
using mjCBase::classname; using mjCBase::classname;
using mjCBase::info; using mjCBase::info;
using mjCBase::plugin;
// used by mjXWriter and mjCModel // used by mjXWriter and mjCModel
const std::vector<double>& get_userdata() { return userdata_; } 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_slidersite() { return spec_slidersite_; }
const std::string& get_refsite() { return spec_refsite_; } const std::string& get_refsite() { return spec_refsite_; }
// public getters bool is_ctrllimited() const;
bool is_ctrllimited() const { return ctrllimited_; } bool is_forcelimited() const;
bool is_forcelimited() const { return forcelimited_; } bool is_actlimited() const;
bool is_actlimited() const { return actlimited_; }
private: private:
mjCActuator(mjCModel* = 0, mjCDef* = 0); // constructor mjCActuator(mjCModel* = 0, mjCDef* = 0); // constructor
void Compile(void); // compiler void Compile(void); // compiler
void CopyFromSpec(); void CopyFromSpec();
void MakePointerLocal(); void PointToLocal();
int trnid[2]; // id of transmission target 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 // variable-size data
std::string plugin_name;
std::string plugin_instance_name;
std::string target_; std::string target_;
std::string slidersite_; std::string slidersite_;
std::string refsite_; std::string refsite_;
@@ -1233,7 +1238,6 @@ class mjCSensor : public mjCBase, private mjmSensor {
using mjCBase::name; using mjCBase::name;
using mjCBase::classname; using mjCBase::classname;
using mjCBase::info; using mjCBase::info;
using mjCBase::plugin;
// used by mjXWriter and mjCModel // used by mjXWriter and mjCModel
const std::vector<double>& get_userdata() { return userdata_; } const std::vector<double>& get_userdata() { return userdata_; }
@@ -1250,6 +1254,8 @@ class mjCSensor : public mjCBase, private mjmSensor {
int refid; // id of reference frame int refid; // id of reference frame
// variable-size data // variable-size data
std::string plugin_name;
std::string plugin_instance_name;
std::string objname_; std::string objname_;
std::string refname_; std::string refname_;
std::vector<double> userdata_; std::vector<double> userdata_;
@@ -1263,18 +1269,25 @@ class mjCSensor : public mjCBase, private mjmSensor {
//------------------------- class mjCNumeric ------------------------------------------------------- //------------------------- class mjCNumeric -------------------------------------------------------
// Describes a custom data field // Describes a custom data field
class mjCNumeric : public mjCBase { class mjCNumeric : public mjCBase, private mjmNumeric {
friend class mjCModel; friend class mjCModel;
friend class mjXWriter;
public: public:
// variables set by user mjmNumeric spec;
std::vector<double> data; // initialization data using mjCBase::name;
int size; // array size, can be bigger than data.size() using mjCBase::info;
void PointToLocal();
void CopyFromSpec();
private: private:
mjCNumeric(mjCModel*); // constructor mjCNumeric(mjCModel*); // constructor
~mjCNumeric(); // destructor ~mjCNumeric(); // destructor
void Compile(void); // compiler void Compile(void); // compiler
std::vector<double> data_;
std::vector<double> spec_data_;
}; };
@@ -1282,17 +1295,25 @@ class mjCNumeric : public mjCBase {
//------------------------- class mjCText ---------------------------------------------------------- //------------------------- class mjCText ----------------------------------------------------------
// Describes a custom text field // Describes a custom text field
class mjCText : public mjCBase { class mjCText : public mjCBase, private mjmText {
friend class mjCModel; friend class mjCModel;
friend class mjXWriter;
public: public:
// variables set by user mjmText spec;
std::string data; // string using mjCBase::name;
using mjCBase::info;
void PointToLocal();
void CopyFromSpec();
private: private:
mjCText(mjCModel*); // constructor mjCText(mjCModel*); // constructor
~mjCText(); // destructor ~mjCText(); // destructor
void Compile(void); // compiler void Compile(void); // compiler
std::string data_;
std::string spec_data_;
}; };
@@ -1300,14 +1321,17 @@ class mjCText : public mjCBase {
//------------------------- class mjCTuple --------------------------------------------------------- //------------------------- class mjCTuple ---------------------------------------------------------
// Describes a custom tuple field // Describes a custom tuple field
class mjCTuple : public mjCBase { class mjCTuple : public mjCBase, private mjmTuple {
friend class mjCModel; friend class mjCModel;
friend class mjXWriter;
public: public:
// variables set by user mjmTuple spec;
std::vector<mjtObj> objtype; // object types using mjCBase::name;
std::vector<std::string> objname; // object names using mjCBase::info;
std::vector<double> objprm; // object parameters
void PointToLocal();
void CopyFromSpec();
private: private:
mjCTuple(mjCModel*); // constructor mjCTuple(mjCModel*); // constructor
@@ -1315,6 +1339,14 @@ class mjCTuple : public mjCBase {
void Compile(void); // compiler void Compile(void); // compiler
std::vector<mjCBase*> obj; // object pointers 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 ----------------------------------------------------------- //------------------------- class mjCKey -----------------------------------------------------------
// Describes a keyframe // Describes a keyframe
class mjCKey : public mjCBase { class mjCKey : public mjCBase, private mjmKey {
friend class mjCModel; friend class mjCModel;
friend class mjXWriter; friend class mjXWriter;
public: public:
double time; // time mjmKey spec;
std::vector<double> qpos; // qpos using mjCBase::name;
std::vector<double> qvel; // qvel using mjCBase::info;
std::vector<double> act; // act
std::vector<double> mpos; // mocap pos void PointToLocal();
std::vector<double> mquat; // mocap quat void CopyFromSpec();
std::vector<double> ctrl; // ctrl
private: private:
mjCKey(mjCModel*); // constructor mjCKey(mjCModel*); // constructor
~mjCKey(); // destructor ~mjCKey(); // destructor
void Compile(const mjModel* m); // compiler 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 // Describes one set of defaults
class mjCDef { class mjCDef {
friend class mjXWriter;
public: public:
mjCDef(void); // constructor mjCDef(void); // constructor
void Compile(const mjCModel* model); // compiler void Compile(const mjCModel* model); // compiler
@@ -1357,7 +1403,9 @@ class mjCDef {
int parentid; // id of parent class int parentid; // id of parent class
std::vector<int> childid; // ids of child classes std::vector<int> childid; // ids of child classes
// default objects mjmDefault spec;
// default objects (TODO: they should become private)
mjCJoint joint; mjCJoint joint;
mjCGeom geom; mjCGeom geom;
mjCSite site; mjCSite site;
src/xml/xml.cc
+89 -28
View File
@@ -20,8 +20,10 @@
#include <xlocale.h> #include <xlocale.h>
#endif #endif
#include <array>
#include <cstdio> #include <cstdio>
#include <string> #include <string>
#include <string_view>
#include <unordered_set> #include <unordered_set>
#include "tinyxml2.h" #include "tinyxml2.h"
@@ -33,6 +35,7 @@
#include "engine/engine_resource.h" #include "engine/engine_resource.h"
#include "engine/engine_vfs.h" #include "engine/engine_vfs.h"
#include "user/user_model.h" #include "user/user_model.h"
#include "user/user_util.h"
#include "xml/xml_native_reader.h" #include "xml/xml_native_reader.h"
#include "xml/xml_native_writer.h" #include "xml/xml_native_writer.h"
#include "xml/xml_urdf.h" #include "xml/xml_urdf.h"
@@ -40,7 +43,6 @@
namespace { namespace {
using std::string;
using tinyxml2::XMLDocument; using tinyxml2::XMLDocument;
using tinyxml2::XMLElement; using tinyxml2::XMLElement;
using tinyxml2::XMLNode; using tinyxml2::XMLNode;
@@ -96,7 +98,7 @@ class LocaleOverride {
} // namespace } // namespace
// Main writer function - calls mjXWrite // 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; LocaleOverride locale_override;
// check for empty model // 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 // find include elements recursively, replace them with subtree from xml file
static void mjIncludeXML(XMLElement* elem, string dir, const mjVFS* vfs, static void mjIncludeXML(mjXReader& reader, XMLElement* elem,
std::unordered_set<string>& included) { 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 // not an include, recursively go through all children
if (strcasecmp(elem->Value(), "include")) { if (strcasecmp(elem->Value(), "include")) {
XMLElement* child = elem->FirstChildElement(); XMLElement* child = elem->FirstChildElement();
for (; child; child = child->NextSiblingElement()) { for (; child; child = child->NextSiblingElement()) {
mjIncludeXML(child, dir, vfs, included); mjIncludeXML(reader, child, dir, vfs, included);
} }
return; return;
} }
@@ -130,24 +151,60 @@ static void mjIncludeXML(XMLElement* elem, string dir, const mjVFS* vfs,
} }
// get filename // get filename
string filename; auto file_attr = mjXUtil::ReadAttrStr(elem, "file", true);
mjXUtil::ReadAttrTxt(elem, "file", filename, true); if (!file_attr.has_value()) {
filename = dir + filename; throw mjXError(elem, "Include element missing file attribute");
}
std::string filename = file_attr.value();
// block repeated include files // block repeated include files
if (included.find(filename) != included.end()) { if (included.find(filename) != included.end()) {
throw mjXError(elem, "File '%s' already included", filename.c_str()); throw mjXError(elem, "File '%s' already included", filename.c_str());
} }
// get data source // TODO: b/325905702 - We have a messy wrapper here to remain backwards
mjResource *resource = nullptr; // compatible, which will be removed in the near future.
if ((resource = mju_openVfsResource(filename.c_str(), vfs)) == nullptr) { 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 // load from provider or OS filesystem
if ((resource = mju_openResource(filename.c_str())) == nullptr) { std::array<char, 1024> error;
throw mjXError(elem, "Could not open file '%s'", filename.c_str()); 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; const char* xmlstring = nullptr;
int buffer_size = mju_readResource(resource, (const void**) &xmlstring); int buffer_size = mju_readResource(resource, (const void**) &xmlstring);
if (buffer_size < 0) { if (buffer_size < 0) {
@@ -207,14 +264,15 @@ static void mjIncludeXML(XMLElement* elem, string dir, const mjVFS* vfs,
// recursively run include // recursively run include
child = include->FirstChildElement(); child = include->FirstChildElement();
for (; child; child = child->NextSiblingElement()) { for (; child; child = child->NextSiblingElement()) {
mjIncludeXML(child, dir, vfs, included); mjIncludeXML(reader, child, next_dir, vfs, included);
} }
} }
// Main parser function // 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; LocaleOverride locale_override;
// check arguments // check arguments
@@ -232,14 +290,15 @@ mjCModel* mjParseXML(const char* filename, const mjVFS* vfs, char* error, int er
} }
// get data source // get data source
mjResource* resource = nullptr;
const char* xmlstring = 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 // load from provider or fallback to OS filesystem
if ((resource = mju_openResource(filename)) == nullptr) { std::array<char, 1024> rerror;
if (error) { resource = mju_openResource(filename, rerror.data(), rerror.size());
std::snprintf(error, error_sz, "mjParseXML: could not open file '%s'", filename); if (!resource) {
} std::snprintf(error, error_sz, "mjParseXML: %s", rerror.data());
return nullptr; 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); int buffer_size = mju_readResource(resource, (const void**) &xmlstring);
if (buffer_size < 0) { if (buffer_size < 0) {
if (error) { 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); mju_closeResource(resource);
return nullptr; return nullptr;
@@ -300,11 +360,12 @@ mjCModel* mjParseXML(const char* filename, const mjVFS* vfs, char* error, int er
try { try {
if (!strcasecmp(root->Value(), "mujoco")) { if (!strcasecmp(root->Value(), "mujoco")) {
// find include elements, replace them with subtree from xml file // find include elements, replace them with subtree from xml file
std::unordered_set<string> included = {filename}; std::unordered_set<std::string> included = {filename};
mjIncludeXML(root, model->modelfiledir, vfs, included); mjXReader parser;
parser.SetModelFileDir(model->modelfiledir);
mjIncludeXML(parser, root, model->modelfiledir, vfs, included);
// parse MuJoCo model // parse MuJoCo model
mjXReader parser;
parser.SetModel(model); parser.SetModel(model);
parser.Parse(root); 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 // set reasonable default for parsing a URDF
// this is separate from the Parser to allow multiple URDFs to be loaded. // this is separate from the Parser to allow multiple URDFs to be loaded.
model->strippath = true; model->spec.strippath = true;
model->fusestatic = true; model->spec.fusestatic = true;
model->discardvisual = true; model->spec.discardvisual = true;
parser.SetModel(model); parser.SetModel(model);
parser.Parse(root); 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_ #define MUJOCO_SRC_XML_XML_NATIVE_READER_H_
#include <sstream> #include <sstream>
#include <string>
#include "user/user_model.h"
#include "xml/xml_base.h"
#include "tinyxml2.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 { class mjXReader : public mjXBase {
public: public:
mjXReader(); // constructor mjXReader(); // constructor
@@ -29,10 +33,18 @@ class mjXReader : public mjXBase {
void Parse(tinyxml2::XMLElement* root); // parse XML document void Parse(tinyxml2::XMLElement* root); // parse XML document
void PrintSchema(std::stringstream& str, bool html, bool pad); // print text or HTML schema 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 // 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 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: private:
// XML section specific to MJCF // XML section specific to MJCF
@@ -53,28 +65,41 @@ class mjXReader : public mjXBase {
void Keyframe(tinyxml2::XMLElement* section); // keyframe section void Keyframe(tinyxml2::XMLElement* section); // keyframe section
// single element parsers, used in defaults and main body // single element parsers, used in defaults and main body
void OneFlex(tinyxml2::XMLElement* elem, mjCFlex* pflex); void OneFlex(tinyxml2::XMLElement* elem, mjmFlex* pflex);
void OneMesh(tinyxml2::XMLElement* elem, mjCMesh* pmesh); void OneMesh(tinyxml2::XMLElement* elem, mjmMesh* pmesh);
void OneSkin(tinyxml2::XMLElement* elem, mjCSkin* pskin); void OneSkin(tinyxml2::XMLElement* elem, mjmSkin* pskin);
void OneMaterial(tinyxml2::XMLElement* elem, mjmMaterial* pmaterial); void OneMaterial(tinyxml2::XMLElement* elem, mjmMaterial* pmaterial);
void OneJoint(tinyxml2::XMLElement* elem, mjmJoint* pjoint); void OneJoint(tinyxml2::XMLElement* elem, mjmJoint* pjoint);
void OneGeom(tinyxml2::XMLElement* elem, mjmGeom* pgeom); 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 OneCamera(tinyxml2::XMLElement* elem, mjmCamera* pcamera);
void OneLight(tinyxml2::XMLElement* elem, mjmLight* plight); 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 OneEquality(tinyxml2::XMLElement* elem, mjmEquality* pequality);
void OneTendon(tinyxml2::XMLElement* elem, mjmTendon* ptendon); void OneTendon(tinyxml2::XMLElement* elem, mjmTendon* ptendon);
void OneActuator(tinyxml2::XMLElement* elem, mjmActuator* pactuator); 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 OneFlexcomp(tinyxml2::XMLElement* elem, mjmBody* pbody);
void OnePlugin(tinyxml2::XMLElement* elem, mjmPlugin* plugin); void OnePlugin(tinyxml2::XMLElement* elem, mjmPlugin* plugin);
mjXSchema schema; // schema used for validation mjXSchema schema; // schema used for validation
mjCDef* GetClass(tinyxml2::XMLElement* section); // get default class name mjmDefault* GetClass(tinyxml2::XMLElement* section); // get default class name
static void GetXMLPos(tinyxml2::XMLElement* elem, mjCBase* obj); // get xml position
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_ #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); WriteAttrInt(elem, "group", pflex->group, defflex.group);
// data vectors // data vectors
if (!pflex->vertbody.empty()) { if (!pflex->get_vertbody().empty()) {
Vector2String(text, pflex->vertbody); Vector2String(text, pflex->get_vertbody());
WriteAttrTxt(elem, "body", text); WriteAttrTxt(elem, "body", text);
} }
if (!pflex->vert.empty()) { if (!pflex->get_vert().empty()) {
Vector2String(text, pflex->vert); Vector2String(text, pflex->get_vert());
WriteAttrTxt(elem, "vertex", text); WriteAttrTxt(elem, "vertex", text);
} }
if (!pflex->elem.empty()) { if (!pflex->get_elem().empty()) {
Vector2String(text, pflex->elem); Vector2String(text, pflex->get_elem());
WriteAttrTxt(elem, "element", text); WriteAttrTxt(elem, "element", text);
} }
if (!pflex->texcoord.empty()) { if (!pflex->get_texcoord().empty()) {
Vector2String(text, pflex->texcoord); Vector2String(text, pflex->get_texcoord());
WriteAttrTxt(elem, "texcoord", text); WriteAttrTxt(elem, "texcoord", text);
} }
@@ -159,39 +159,40 @@ void mjXWriter::OneMesh(XMLElement* elem, mjCMesh* pmesh, mjCDef* def) {
if (!writingdefaults) { if (!writingdefaults) {
WriteAttrTxt(elem, "name", pmesh->name); WriteAttrTxt(elem, "name", pmesh->name);
WriteAttrTxt(elem, "class", pmesh->classname); WriteAttrTxt(elem, "class", pmesh->classname);
WriteAttrTxt(elem, "content_type", pmesh->content_type()); WriteAttrTxt(elem, "content_type", pmesh->get_content_type());
WriteAttrTxt(elem, "file", pmesh->file()); WriteAttrTxt(elem, "file", pmesh->get_file());
// write vertex data // write vertex data
if (!pmesh->uservert().empty()) { if (!pmesh->get_uservert().empty()) {
Vector2String(text, pmesh->uservert()); Vector2String(text, pmesh->get_uservert());
WriteAttrTxt(elem, "vertex", text); WriteAttrTxt(elem, "vertex", text);
} }
// write normal data // write normal data
if (!pmesh->usernormal().empty()) { if (!pmesh->get_usernormal().empty()) {
Vector2String(text, pmesh->usernormal()); Vector2String(text, pmesh->get_usernormal());
WriteAttrTxt(elem, "normal", text); WriteAttrTxt(elem, "normal", text);
} }
// write texcoord data // write texcoord data
if (!pmesh->usertexcoord().empty()) { if (!pmesh->get_usertexcoord().empty()) {
Vector2String(text, pmesh->usertexcoord()); Vector2String(text, pmesh->get_usertexcoord());
WriteAttrTxt(elem, "texcoord", text); WriteAttrTxt(elem, "texcoord", text);
} }
// write face data // write face data
if (!pmesh->userface().empty()) { if (!pmesh->get_userface().empty()) {
Vector2String(text, pmesh->userface()); Vector2String(text, pmesh->get_userface());
WriteAttrTxt(elem, "face", text); WriteAttrTxt(elem, "face", text);
} }
} }
// defaults and regular // defaults and regular
WriteAttr(elem, "refpos", 3, pmesh->refpos(), def->mesh.refpos()); WriteAttr(elem, "refpos", 3, pmesh->refpos, def->mesh.refpos);
WriteAttr(elem, "refquat", 4, pmesh->refquat(), def->mesh.refquat()); WriteAttr(elem, "refquat", 4, pmesh->refquat, def->mesh.refquat);
WriteAttr(elem, "scale", 3, pmesh->scale(), def->mesh.scale()); WriteAttr(elem, "scale", 3, pmesh->scale, def->mesh.scale);
WriteAttrKey(elem, "smoothnormal", bool_map, 2, pmesh->smoothnormal(), def->mesh.smoothnormal()); 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 // write attributes
WriteAttrTxt(elem, "name", pskin->name); WriteAttrTxt(elem, "name", pskin->name);
WriteAttrTxt(elem, "file", pskin->file); WriteAttrTxt(elem, "file", pskin->get_file());
WriteAttrTxt(elem, "material", pskin->get_material()); WriteAttrTxt(elem, "material", pskin->get_material());
WriteAttrInt(elem, "group", pskin->group, 0); WriteAttrInt(elem, "group", pskin->group, 0);
WriteAttr(elem, "rgba", 4, pskin->rgba, mydef.geom.rgba); WriteAttr(elem, "rgba", 4, pskin->rgba, mydef.geom.rgba);
WriteAttr(elem, "inflate", 1, &pskin->inflate, &zero); WriteAttr(elem, "inflate", 1, &pskin->inflate, &zero);
// write data if no file // write data if no file
if (pskin->file.empty()) { if (pskin->get_file().empty()) {
// mesh vert // mesh vert
Vector2String(text, pskin->vert); Vector2String(text, pskin->get_vert());
WriteAttrTxt(elem, "vertex", text); WriteAttrTxt(elem, "vertex", text);
// mesh texcoord // mesh texcoord
if (!pskin->texcoord.empty()) { if (!pskin->get_texcoord().empty()) {
Vector2String(text, pskin->texcoord); Vector2String(text, pskin->get_texcoord());
WriteAttrTxt(elem, "texcoord", text); WriteAttrTxt(elem, "texcoord", text);
} }
// mesh face // mesh face
Vector2String(text, pskin->face); Vector2String(text, pskin->get_face());
WriteAttrTxt(elem, "face", text); WriteAttrTxt(elem, "face", text);
// bones // bones
for (size_t i=0; i<pskin->bodyname.size(); i++) { for (size_t i=0; i<pskin->get_bodyname().size(); i++) {
// make bone // make bone
XMLElement* bone = InsertEnd(elem, "bone"); XMLElement* bone = InsertEnd(elem, "bone");
// write attributes // write attributes
WriteAttrTxt(bone, "body", pskin->bodyname[i]); WriteAttrTxt(bone, "body", pskin->get_bodyname()[i]);
WriteAttr(bone, "bindpos", 3, pskin->bindpos.data()+3*i); WriteAttr(bone, "bindpos", 3, pskin->get_bindpos().data()+3*i);
WriteAttr(bone, "bindquat", 4, pskin->bindquat.data()+4*i); WriteAttr(bone, "bindquat", 4, pskin->get_bindquat().data()+4*i);
// write vertid // write vertid
Vector2String(text, pskin->vertid[i]); Vector2String(text, pskin->get_vertid()[i]);
WriteAttrTxt(bone, "vertid", text); WriteAttrTxt(bone, "vertid", text);
// write vertweight // write vertweight
Vector2String(text, pskin->vertweight[i]); Vector2String(text, pskin->get_vertweight()[i]);
WriteAttrTxt(bone, "vertweight", text); 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); WriteAttrInt(elem, "group", pjoint->group, def->joint.group);
WriteAttr(elem, "ref", 1, &pjoint->ref, &zero); WriteAttr(elem, "ref", 1, &pjoint->ref, &zero);
WriteAttr(elem, "springref", 1, &pjoint->springref, &zero); WriteAttr(elem, "springref", 1, &pjoint->springref, &zero);
WriteAttr(elem, "solreflimit", mjNREF, pjoint->solref_limit, def->joint.solref_limit); WriteAttr(elem, "solreflimit", mjNREF, pjoint->solref_limit, def->joint.solref_limit, true);
WriteAttr(elem, "solimplimit", mjNIMP, pjoint->solimp_limit, def->joint.solimp_limit); WriteAttr(elem, "solimplimit", mjNIMP, pjoint->solimp_limit, def->joint.solimp_limit, true);
WriteAttr(elem, "solreffriction", mjNREF, pjoint->solref_friction, def->joint.solref_friction); WriteAttr(elem, "solreffriction", mjNREF, pjoint->solref_friction, def->joint.solref_friction,
WriteAttr(elem, "solimpfriction", mjNIMP, pjoint->solimp_friction, def->joint.solimp_friction); true);
WriteAttr(elem, "solimpfriction", mjNIMP, pjoint->solimp_friction, def->joint.solimp_friction,
true);
WriteAttr(elem, "stiffness", 1, &pjoint->stiffness, &def->joint.stiffness); WriteAttr(elem, "stiffness", 1, &pjoint->stiffness, &def->joint.stiffness);
WriteAttrKey(elem, "limited", TFAuto_map, 3, pjoint->limited, def->joint.limited); WriteAttrKey(elem, "limited", TFAuto_map, 3, pjoint->limited, def->joint.limited);
WriteAttr(elem, "range", 2, pjoint->range, def->joint.range); 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, "condim", pgeom->condim, def->geom.condim);
WriteAttrInt(elem, "group", pgeom->group, def->geom.group); WriteAttrInt(elem, "group", pgeom->group, def->geom.group);
WriteAttrInt(elem, "priority", pgeom->priority, def->geom.priority); 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, "solmix", 1, &pgeom->solmix, &def->geom.solmix);
WriteAttr(elem, "solref", mjNREF, pgeom->solref, def->geom.solref); WriteAttr(elem, "solref", mjNREF, pgeom->solref, def->geom.solref, true);
WriteAttr(elem, "solimp", mjNIMP, pgeom->solimp, def->geom.solimp); WriteAttr(elem, "solimp", mjNIMP, pgeom->solimp, def->geom.solimp, true);
WriteAttr(elem, "margin", 1, &pgeom->margin, &def->geom.margin); 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);
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 // write plugin
if (pgeom->plugin.active) { 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 // regular
if (!writingdefaults) { if (!writingdefaults) {
WriteAttrTxt(elem, "class", ppair->classname); WriteAttrTxt(elem, "class", ppair->classname);
WriteAttrTxt(elem, "geom1", ppair->geomname1); WriteAttrTxt(elem, "geom1", ppair->get_geomname1());
WriteAttrTxt(elem, "geom2", ppair->geomname2); WriteAttrTxt(elem, "geom2", ppair->get_geomname2());
} }
// defaults and regular // defaults and regular
WriteAttrTxt(elem, "name", ppair->name); WriteAttrTxt(elem, "name", ppair->name);
WriteAttrInt(elem, "condim", ppair->condim, def->pair.condim); WriteAttrInt(elem, "condim", ppair->condim, def->pair.spec.condim);
WriteAttr(elem, "margin", 1, &ppair->margin, &def->pair.margin); WriteAttr(elem, "margin", 1, &ppair->margin, &def->pair.spec.margin);
WriteAttr(elem, "gap", 1, &ppair->gap, &def->pair.gap); WriteAttr(elem, "gap", 1, &ppair->gap, &def->pair.spec.gap);
WriteAttr(elem, "solref", mjNREF, ppair->solref, def->pair.solref); WriteAttr(elem, "solref", mjNREF, ppair->solref, def->pair.spec.solref, true);
WriteAttr(elem, "solreffriction", mjNREF, ppair->solreffriction, def->pair.solreffriction); WriteAttr(elem, "solreffriction", mjNREF, ppair->solreffriction, def->pair.spec.solreffriction,
WriteAttr(elem, "solimp", mjNIMP, ppair->solimp, def->pair.solimp); true);
WriteAttr(elem, "friction", 5, ppair->friction, def->pair.friction); 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 // defaults and regular
WriteAttrKey(elem, "active", bool_map, 2, peq->active, def->equality.active); WriteAttrKey(elem, "active", bool_map, 2, peq->active, def->equality.active);
WriteAttr(elem, "solref", mjNREF, peq->solref, def->equality.solref); WriteAttr(elem, "solref", mjNREF, peq->solref, def->equality.solref, true);
WriteAttr(elem, "solimp", mjNIMP, peq->solimp, def->equality.solimp); 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 // defaults and regular
WriteAttrInt(elem, "group", pten->group, def->tendon.group); WriteAttrInt(elem, "group", pten->group, def->tendon.group);
WriteAttr(elem, "solreflimit", mjNREF, pten->solref_limit, def->tendon.solref_limit); WriteAttr(elem, "solreflimit", mjNREF, pten->solref_limit, def->tendon.solref_limit, true);
WriteAttr(elem, "solimplimit", mjNIMP, pten->solimp_limit, def->tendon.solimp_limit); WriteAttr(elem, "solimplimit", mjNIMP, pten->solimp_limit, def->tendon.solimp_limit, true);
WriteAttr(elem, "solreffriction", mjNREF, pten->solref_friction, def->tendon.solref_friction); WriteAttr(elem, "solreffriction", mjNREF, pten->solref_friction, def->tendon.solref_friction,
WriteAttr(elem, "solimpfriction", mjNIMP, pten->solimp_friction, def->tendon.solimp_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); WriteAttrKey(elem, "limited", TFAuto_map, 3, pten->limited, def->tendon.limited);
WriteAttr(elem, "range", 2, pten->range, def->tendon.range); WriteAttr(elem, "range", 2, pten->range, def->tendon.range);
WriteAttr(elem, "margin", 1, &pten->margin, &def->tendon.margin); 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 // plugins: write config attributes
if (pact->plugin.active) { if (pact->plugin.active) {
OnePlugin(elem, pact); OnePlugin(elem, &pact->plugin);
} }
// non-plugins: write actuator parameters // 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, "gaintype", gain_map, gain_sz, pact->gaintype, def->actuator.gaintype);
WriteAttrKey(elem, "biastype", bias_map, bias_sz, pact->biastype, def->actuator.biastype); WriteAttrKey(elem, "biastype", bias_map, bias_sz, pact->biastype, def->actuator.biastype);
WriteAttr(elem, "gainprm", mjNGAIN, pact->gainprm, def->actuator.gainprm); WriteAttr(elem, "gainprm", mjNGAIN, pact->gainprm, def->actuator.gainprm, true);
WriteAttr(elem, "biasprm", mjNBIAS, pact->biasprm, def->actuator.biasprm); WriteAttr(elem, "biasprm", mjNBIAS, pact->biasprm, def->actuator.biasprm, true);
} }
// userdata // userdata
@@ -725,21 +731,21 @@ void mjXWriter::OneActuator(XMLElement* elem, mjCActuator* pact, mjCDef* def) {
// write plugin // write plugin
void mjXWriter::OnePlugin(XMLElement* elem, mjCBase* object) { void mjXWriter::OnePlugin(XMLElement* elem, mjmPlugin* plugin) {
const std::string instance_name = std::string(mjm_getString(object->plugin.instance_name)); const std::string instance_name = std::string(mjm_getString(plugin->instance_name));
const std::string plugin_name = std::string(mjm_getString(object->plugin.name)); const std::string plugin_name = std::string(mjm_getString(plugin->name));
if (!instance_name.empty()) { if (!instance_name.empty()) {
WriteAttrTxt(elem, "instance", instance_name); WriteAttrTxt(elem, "instance", instance_name);
} else { } else {
WriteAttrTxt(elem, "plugin", plugin_name); WriteAttrTxt(elem, "plugin", plugin_name);
const mjpPlugin* plugin = mjp_getPluginAtSlot( const mjpPlugin* pplugin = mjp_getPluginAtSlot(
((mjCPlugin*)object->plugin.instance)->plugin_slot); ((mjCPlugin*)plugin->instance)->spec.plugin_slot);
const char* c = &((mjCPlugin*)object->plugin.instance)->flattened_attributes[0]; const char* c = &((mjCPlugin*)plugin->instance)->flattened_attributes[0];
for (int i = 0; i < plugin->nattribute; ++i) { for (int i = 0; i < pplugin->nattribute; ++i) {
std::string value(c); std::string value(c);
if (!value.empty()) { if (!value.empty()) {
XMLElement* config_elem = InsertEnd(elem, "config"); 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); WriteAttrTxt(config_elem, "value", value);
c += value.size(); c += value.size();
} }
@@ -769,7 +775,7 @@ string mjXWriter::Write(char *error, size_t error_sz) {
// create document and root // create document and root
XMLDocument doc; XMLDocument doc;
XMLElement* root = doc.NewElement("mujoco"); XMLElement* root = doc.NewElement("mujoco");
root->SetAttribute("model", model->modelname.c_str()); root->SetAttribute("model", mjm_getString(model->modelname));
// insert root // insert root
doc.InsertFirstChild(root); doc.InsertFirstChild(root);
@@ -815,11 +821,11 @@ void mjXWriter::Compiler(XMLElement* root) {
WriteAttrTxt(section, "convexhull", FindValue(bool_map, 2, model->convexhull)); WriteAttrTxt(section, "convexhull", FindValue(bool_map, 2, model->convexhull));
} }
WriteAttrTxt(section, "angle", "radian"); WriteAttrTxt(section, "angle", "radian");
if (!model->meshdir.empty()) { if (!model->get_meshdir().empty()) {
WriteAttrTxt(section, "meshdir", model->meshdir); WriteAttrTxt(section, "meshdir", model->get_meshdir());
} }
if (!model->texturedir.empty()) { if (!model->get_texturedir().empty()) {
WriteAttrTxt(section, "texturedir", model->texturedir); WriteAttrTxt(section, "texturedir", model->get_texturedir());
} }
if (!model->usethread) { if (!model->usethread) {
WriteAttrTxt(section, "usethread", "false"); WriteAttrTxt(section, "usethread", "false");
@@ -969,12 +975,13 @@ void mjXWriter::Size(XMLElement* root) {
// statistic section // statistic section
void mjXWriter::Statistic(XMLElement* root) { void mjXWriter::Statistic(XMLElement* root) {
XMLElement* section = InsertEnd(root, "statistic"); XMLElement* section = InsertEnd(root, "statistic");
mjStatistic* s = &model->stat;
if (mjuu_defined(model->meaninertia)) WriteAttr(section, "meaninertia", 1, &model->meaninertia); if (mjuu_defined(s->meaninertia)) WriteAttr(section, "meaninertia", 1, &s->meaninertia);
if (mjuu_defined(model->meanmass)) WriteAttr(section, "meanmass", 1, &model->meanmass); if (mjuu_defined(s->meanmass)) WriteAttr(section, "meanmass", 1, &s->meanmass);
if (mjuu_defined(model->meansize)) WriteAttr(section, "meansize", 1, &model->meansize); if (mjuu_defined(s->meansize)) WriteAttr(section, "meansize", 1, &s->meansize);
if (mjuu_defined(model->extent)) WriteAttr(section, "extent", 1, &model->extent); if (mjuu_defined(s->extent)) WriteAttr(section, "extent", 1, &s->extent);
if (mjuu_defined(model->center[0])) WriteAttr(section, "center", 3, model->center); if (mjuu_defined(s->center[0])) WriteAttr(section, "center", 3, s->center);
// remove entire section if no attributes // remove entire section if no attributes
if (!section->FirstAttribute()) root->DeleteChild(section); 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 // 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) { if (plugin != last_plugin) {
plugin_elem = InsertEnd(section, "plugin"); plugin_elem = InsertEnd(section, "plugin");
WriteAttrTxt(plugin_elem, "plugin", plugin->name); WriteAttrTxt(plugin_elem, "plugin", plugin->name);
@@ -1282,7 +1289,7 @@ void mjXWriter::Custom(XMLElement* root) {
elem = InsertEnd(section, "numeric"); elem = InsertEnd(section, "numeric");
WriteAttrTxt(elem, "name", ptr->name); WriteAttrTxt(elem, "name", ptr->name);
WriteAttrInt(elem, "size", ptr->size); WriteAttrInt(elem, "size", ptr->size);
WriteAttr(elem, "data", ptr->size, ptr->data.data()); WriteAttr(elem, "data", ptr->size, ptr->data_.data());
} }
// write all texts // write all texts
@@ -1290,7 +1297,7 @@ void mjXWriter::Custom(XMLElement* root) {
mjCText* ptr = (mjCText*)model->GetObject(mjOBJ_TEXT, i); mjCText* ptr = (mjCText*)model->GetObject(mjOBJ_TEXT, i);
elem = InsertEnd(section, "text"); elem = InsertEnd(section, "text");
WriteAttrTxt(elem, "name", ptr->name); WriteAttrTxt(elem, "name", ptr->name);
WriteAttrTxt(elem, "data", ptr->data.c_str()); WriteAttrTxt(elem, "data", ptr->data_.c_str());
} }
// write all tuples // write all tuples
@@ -1300,11 +1307,11 @@ void mjXWriter::Custom(XMLElement* root) {
WriteAttrTxt(elem, "name", ptr->name); WriteAttrTxt(elem, "name", ptr->name);
// write objects in tuple // 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"); XMLElement* obj = InsertEnd(elem, "element");
WriteAttrTxt(obj, "objtype", mju_type2Str((int)ptr->objtype[j])); WriteAttrTxt(obj, "objtype", mju_type2Str((int)ptr->objtype_[j]));
WriteAttrTxt(obj, "objname", ptr->objname[j].c_str()); WriteAttrTxt(obj, "objname", ptr->objname_[j].c_str());
double oprm = ptr->objprm[j]; double oprm = ptr->objprm_[j];
if (oprm!=0) { if (oprm!=0) {
WriteAttr(obj, "prm", 1, &oprm); WriteAttr(obj, "prm", 1, &oprm);
} }
@@ -1358,16 +1365,16 @@ void mjXWriter::Asset(XMLElement* root) {
// write textures loaded from files // write textures loaded from files
else { else {
// write single file // write single file
WriteAttrTxt(elem, "content_type", ptex->content_type); WriteAttrTxt(elem, "content_type", ptex->get_content_type());
WriteAttrTxt(elem, "file", ptex->file); WriteAttrTxt(elem, "file", ptex->get_file());
// write separate files // write separate files
WriteAttrTxt(elem, "fileright", ptex->cubefiles[0]); WriteAttrTxt(elem, "fileright", ptex->get_cubefiles()[0]);
WriteAttrTxt(elem, "fileleft", ptex->cubefiles[1]); WriteAttrTxt(elem, "fileleft", ptex->get_cubefiles()[1]);
WriteAttrTxt(elem, "fileup", ptex->cubefiles[2]); WriteAttrTxt(elem, "fileup", ptex->get_cubefiles()[2]);
WriteAttrTxt(elem, "filedown", ptex->cubefiles[3]); WriteAttrTxt(elem, "filedown", ptex->get_cubefiles()[3]);
WriteAttrTxt(elem, "filefront", ptex->cubefiles[4]); WriteAttrTxt(elem, "filefront", ptex->get_cubefiles()[4]);
WriteAttrTxt(elem, "fileback", ptex->cubefiles[5]); WriteAttrTxt(elem, "fileback", ptex->get_cubefiles()[5]);
if (ptex->hflip) { if (ptex->hflip) {
WriteAttrKey(elem, "hflip", bool_map, 2, 1); WriteAttrKey(elem, "hflip", bool_map, 2, 1);
} }
@@ -1399,7 +1406,7 @@ void mjXWriter::Asset(XMLElement* root) {
if (pmesh->plugin.active) { if (pmesh->plugin.active) {
elem = InsertEnd(section, "mesh"); elem = InsertEnd(section, "mesh");
WriteAttrTxt(elem, "name", pmesh->name); WriteAttrTxt(elem, "name", pmesh->name);
OnePlugin(InsertEnd(elem, "plugin"), pmesh); OnePlugin(InsertEnd(elem, "plugin"), &pmesh->plugin);
} else{ } else{
elem = InsertEnd(section, "mesh"); elem = InsertEnd(section, "mesh");
OneMesh(elem, pmesh, pmesh->def); OneMesh(elem, pmesh, pmesh->def);
@@ -1415,15 +1422,15 @@ void mjXWriter::Asset(XMLElement* root) {
// write attributes // write attributes
WriteAttrTxt(elem, "name", phf->name); WriteAttrTxt(elem, "name", phf->name);
WriteAttr(elem, "size", 4, phf->size); WriteAttr(elem, "size", 4, phf->size);
if (!phf->file.empty()) { if (!phf->file_.empty()) {
WriteAttrTxt(elem, "content_type", phf->content_type); WriteAttrTxt(elem, "content_type", phf->content_type_);
WriteAttrTxt(elem, "file", phf->file); WriteAttrTxt(elem, "file", phf->file_);
} else { } else {
WriteAttrInt(elem, "nrow", phf->nrow); WriteAttrInt(elem, "nrow", phf->nrow);
WriteAttrInt(elem, "ncol", phf->ncol); WriteAttrInt(elem, "ncol", phf->ncol);
if (!phf->userdata().empty()) { if (!phf->get_userdata().empty()) {
string text; string text;
Vector2String(text, phf->userdata(), phf->ncol); Vector2String(text, phf->get_userdata(), phf->ncol);
WriteAttrTxt(elem, "elevation", text); WriteAttrTxt(elem, "elevation", text);
} }
} }
@@ -1499,7 +1506,7 @@ void mjXWriter::Body(XMLElement* elem, mjCBody* body) {
// write plugin // write plugin
if (body->plugin.active) { if (body->plugin.active) {
OnePlugin(InsertEnd(elem, "plugin"), body); OnePlugin(InsertEnd(elem, "plugin"), &body->plugin);
} }
// write child bodies recursively // write child bodies recursively
@@ -1542,8 +1549,8 @@ void mjXWriter::Contact(XMLElement* root) {
// write attributes // write attributes
WriteAttrTxt(elem, "name", pexclude->name); WriteAttrTxt(elem, "name", pexclude->name);
WriteAttrTxt(elem, "body1", pexclude->bodyname1); WriteAttrTxt(elem, "body1", pexclude->get_bodyname1());
WriteAttrTxt(elem, "body2", pexclude->bodyname2); WriteAttrTxt(elem, "body2", pexclude->get_bodyname2());
} }
} }
@@ -1897,7 +1904,7 @@ void mjXWriter::Sensor(XMLElement* root) {
WriteAttrTxt(elem, "objtype", mju_type2Str(psen->objtype)); WriteAttrTxt(elem, "objtype", mju_type2Str(psen->objtype));
WriteAttrTxt(elem, "objname", psen->get_objname()); WriteAttrTxt(elem, "objname", psen->get_objname());
} }
OnePlugin(elem, psen); OnePlugin(elem, &psen->plugin);
break; break;
// user-defined sensor // user-defined sensor
@@ -1963,8 +1970,8 @@ void mjXWriter::Keyframe(XMLElement* root) {
// check qpos and write // check qpos and write
for (int j=0; j<model->nq; j++) { for (int j=0; j<model->nq; j++) {
if (pk->qpos[j]!=model->qpos0[j]) { if (pk->qpos_[j]!=model->qpos0[j]) {
WriteAttr(elem, "qpos", model->nq, pk->qpos.data()); WriteAttr(elem, "qpos", model->nq, pk->qpos_.data());
change = true; change = true;
break; break;
} }
@@ -1972,8 +1979,8 @@ void mjXWriter::Keyframe(XMLElement* root) {
// check qvel and write // check qvel and write
for (int j=0; j<model->nv; j++) { for (int j=0; j<model->nv; j++) {
if (pk->qvel[j]!=0) { if (pk->qvel_[j]!=0) {
WriteAttr(elem, "qvel", model->nv, pk->qvel.data()); WriteAttr(elem, "qvel", model->nv, pk->qvel_.data());
change = true; change = true;
break; break;
} }
@@ -1981,8 +1988,8 @@ void mjXWriter::Keyframe(XMLElement* root) {
// check act and write // check act and write
for (int j=0; j<model->na; j++) { for (int j=0; j<model->na; j++) {
if (pk->act[j]!=0) { if (pk->act_[j]!=0) {
WriteAttr(elem, "act", model->na, pk->act.data()); WriteAttr(elem, "act", model->na, pk->act_.data());
change = true; change = true;
break; break;
} }
@@ -1994,10 +2001,10 @@ void mjXWriter::Keyframe(XMLElement* root) {
if (model->bodies[j]->mocap) { if (model->bodies[j]->mocap) {
mjCBody* pb = model->bodies[j]; mjCBody* pb = model->bodies[j];
int id = pb->mocapid; int id = pb->mocapid;
if (pb->pos[0] != pk->mpos[3*id] || if (pb->pos[0] != pk->mpos_[3*id] ||
pb->pos[1] != pk->mpos[3*id+1] || pb->pos[1] != pk->mpos_[3*id+1] ||
pb->pos[2] != pk->mpos[3*id+2]) { pb->pos[2] != pk->mpos_[3*id+2]) {
WriteAttr(elem, "mpos", 3*model->nmocap, pk->mpos.data()); WriteAttr(elem, "mpos", 3*model->nmocap, pk->mpos_.data());
change = true; change = true;
break; break;
} }
@@ -2011,11 +2018,11 @@ void mjXWriter::Keyframe(XMLElement* root) {
if (model->bodies[j]->mocap) { if (model->bodies[j]->mocap) {
mjCBody* pb = model->bodies[j]; mjCBody* pb = model->bodies[j];
int id = pb->mocapid; int id = pb->mocapid;
if (pb->quat[0] != pk->mquat[4*id] || if (pb->quat[0] != pk->mquat_[4*id] ||
pb->quat[1] != pk->mquat[4*id+1] || pb->quat[1] != pk->mquat_[4*id+1] ||
pb->quat[2] != pk->mquat[4*id+2] || pb->quat[2] != pk->mquat_[4*id+2] ||
pb->quat[3] != pk->mquat[4*id+3]) { pb->quat[3] != pk->mquat_[4*id+3]) {
WriteAttr(elem, "mquat", 4*model->nmocap, pk->mquat.data()); WriteAttr(elem, "mquat", 4*model->nmocap, pk->mquat_.data());
change = true; change = true;
break; break;
} }
@@ -2025,8 +2032,8 @@ void mjXWriter::Keyframe(XMLElement* root) {
// check ctrl and write // check ctrl and write
for (int j=0; j<model->nu; j++) { for (int j=0; j<model->nu; j++) {
if (pk->ctrl[j]!=0) { if (pk->ctrl_[j]!=0) {
WriteAttr(elem, "ctrl", model->nu, pk->ctrl.data()); WriteAttr(elem, "ctrl", model->nu, pk->ctrl_.data());
change = true; change = true;
break; 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 OneEquality(tinyxml2::XMLElement* elem, mjCEquality* pequality, mjCDef* def);
void OneTendon(tinyxml2::XMLElement* elem, mjCTendon* ptendon, mjCDef* def); void OneTendon(tinyxml2::XMLElement* elem, mjCTendon* ptendon, mjCDef* def);
void OneActuator(tinyxml2::XMLElement* elem, mjCActuator* pactuator, 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 bool writingdefaults; // true during defaults write
}; };
src/xml/xml_urdf.cc
+38 -26
View File
@@ -93,23 +93,26 @@ void mjXURDF::Parse(
if (mjc) { if (mjc) {
XMLElement *section; XMLElement *section;
if ((section = FindSubElem(mjc, "compiler"))) { if ((section = FindSubElem(mjc, "compiler"))) {
mjXReader::Compiler(section, model); mjXReader::Compiler(section, &model->spec);
} }
if ((section = FindSubElem(mjc, "option"))) { if ((section = FindSubElem(mjc, "option"))) {
mjXReader::Option(section, &model->option); mjXReader::Option(section, &model->spec.option);
} }
if ((section = FindSubElem(mjc, "size"))) { if ((section = FindSubElem(mjc, "size"))) {
mjXReader::Size(section, model); mjXReader::Size(section, &model->spec);
} }
} }
// enforce required compiler defaults for URDF // enforce required compiler defaults for URDF
model->degree = false; model->spec.degree = false;
// get model name // 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 // find and register all materials
MakeMaterials(root); MakeMaterials(root);
@@ -202,7 +205,7 @@ void mjXURDF::Parse(
// override the pose for the base link and add a free joint // override the pose for the base link and add a free joint
for (int i = 0; i < (int)urName.size(); i++) { for (int i = 0; i < (int)urName.size(); i++) {
if (urParent[i] < 0) { 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()); mjmBody* pbody = mjm_findChild(world, urName[i].c_str());
mjuu_copyvec(pbody->pos, pos, 3); mjuu_copyvec(pbody->pos, pos, 3);
mjuu_copyvec(pbody->quat, quat, 4); mjuu_copyvec(pbody->quat, quat, 4);
@@ -228,7 +231,7 @@ void mjXURDF::Body(XMLElement* body_elem) {
// get body name and pointer to mjmBody // get body name and pointer to mjmBody
ReadAttrTxt(body_elem, "name", name, true); ReadAttrTxt(body_elem, "name", name, true);
name = GetPrefixedName(name); name = GetPrefixedName(name);
world = mjm_findBody(model, "world"); world = mjm_findBody(&model->spec, "world");
pbody = mjm_findChild(world, name.c_str()); pbody = mjm_findChild(world, name.c_str());
if (!pbody) { if (!pbody) {
throw mjXError(body_elem, "URDF body not found"); // SHOULD NOT OCCUR 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 // create geom if not discarded
if (!model->discardvisual) { if (!model->spec.discardvisual) {
pgeom = Geom(elem, pbody, false); pgeom = Geom(elem, pbody, false);
// save color // save color
@@ -386,7 +389,7 @@ void mjXURDF::Joint(XMLElement* joint_elem) {
elem = FindSubElem(joint_elem, "parent", true); elem = FindSubElem(joint_elem, "parent", true);
ReadAttrTxt(elem, "link", name, true); ReadAttrTxt(elem, "link", name, true);
name = GetPrefixedName(name); name = GetPrefixedName(name);
world = mjm_findBody(model, "world"); world = mjm_findBody(&model->spec, "world");
parent = mjm_findChild(world, name.c_str()); parent = mjm_findChild(world, name.c_str());
if (!parent) { // SHOULD NOT OCCUR if (!parent) { // SHOULD NOT OCCUR
throw mjXError(elem, "invalid parent name in URDF joint definition"); 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); elem = FindSubElem(joint_elem, "child", true);
ReadAttrTxt(elem, "link", name, true); ReadAttrTxt(elem, "link", name, true);
name = GetPrefixedName(name); name = GetPrefixedName(name);
world = mjm_findBody(model, "world"); world = mjm_findBody(&model->spec, "world");
pbody = mjm_findChild(world, name.c_str()); pbody = mjm_findChild(world, name.c_str());
if (!pbody) { // SHOULD NOT OCCUR if (!pbody) { // SHOULD NOT OCCUR
throw mjXError(elem, "invalid child name in URDF joint definition"); 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"))) { if ((elem = FindSubElem(joint_elem, "limit"))) {
ReadAttr(elem, "lower", 1, pjoint->range, text); ReadAttr(elem, "lower", 1, pjoint->range, text);
ReadAttr(elem, "upper", 1, pjoint->range+1, text); ReadAttr(elem, "upper", 1, pjoint->range+1, text);
pjoint->limited = (mjuu_defined(pjoint->range[0]) && bool is_limited = mjuu_defined(pjoint->range[0]) &&
mjuu_defined(pjoint->range[1]) && mjuu_defined(pjoint->range[1]) &&
pjoint->range[0] < 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, "velocity", 1, &pjoint->maxvel, text); // no maxvel in MuJoCo
ReadAttr(elem, "effort", 1, &pjoint->urdfeffort, text); ReadAttr(elem, "effort", 1, &pjoint->urdfeffort, text);
} else { } 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); .value_or(default_meshscale);
// strip file name if necessary // strip file name if necessary
if (model->strippath) { if (model->spec.strippath) {
meshfile = mjuu_strippath(meshfile); meshfile = mjuu_strippath(meshfile);
} }
@@ -571,26 +575,34 @@ mjmGeom* mjXURDF::Geom(XMLElement* geom_elem, mjmBody* pbody, bool collision) {
meshname = mjuu_stripext(meshname); meshname = mjuu_stripext(meshname);
// look for existing mesh // 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 // does not exist: create
if (!pmesh) { if (!mesh) {
pmesh = model->AddMesh(); pmesh = mjm_addMesh(&model->spec, 0);
} }
// exists with different scale: append name with '1', create // exists with different scale: append name with '1', create
else if (pmesh->scale()[0]!=meshscale[0] || else if (mesh->spec.scale[0]!=meshscale[0] ||
pmesh->scale()[1]!=meshscale[1] || mesh->spec.scale[1]!=meshscale[1] ||
pmesh->scale()[2]!=meshscale[2]) { mesh->spec.scale[2]!=meshscale[2]) {
pmesh = model->AddMesh(); pmesh = mjm_addMesh(&model->spec, 0);
meshname = meshname + "1"; meshname = meshname + "1";
} }
// point to already existing spec
else {
pmesh = &mesh->spec;
}
// set fields // set fields
pmesh->set_file(meshfile); mjm_setString(pmesh->file, meshfile.c_str());
pmesh->name = meshname; mjm_setString(pmesh->name, meshname.c_str());
mjm_setString(pgeom->meshname, 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 { else {
@@ -672,7 +684,7 @@ void mjXURDF::AddToTree(int n) {
// get pointer to parent in mjCModel tree // get pointer to parent in mjCModel tree
mjmBody *parent = 0, *child = 0, *world = 0; mjmBody *parent = 0, *child = 0, *world = 0;
if (urParent[n]>=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()); parent = mjm_findChild(world, urName[urParent[n]].c_str());
if (!parent) if (!parent)
src/xml/xml_util.cc
+59 -69
View File
@@ -32,8 +32,11 @@
#include "tinyxml2.h" #include "tinyxml2.h"
#include <mujoco/mujoco.h>
#include "cc/array_safety.h" #include "cc/array_safety.h"
#include "engine/engine_resource.h"
#include "engine/engine_util_errmem.h" #include "engine/engine_util_errmem.h"
#include "user/user_util.h"
#include "xml/xml_util.h" #include "xml/xml_util.h"
#include "xml/xml_numeric_format.h" #include "xml/xml_numeric_format.h"
@@ -156,57 +159,37 @@ XMLElement* NextSiblingElement(XMLElement* e, const char* name) {
return nullptr; return nullptr;
} }
// constructor static std::string ResolveFilePath(XMLElement* e, std::string filename,
mjXSchema::mjXSchema(const char* schema[][mjXATTRNUM], unsigned nrow, bool checkptr) { const std::string& dir) {
if (schema[0][0][0] == '<' || schema[0][0][0] == '>') { std::string path = "";
throw "expected element, found bracket"; if (mjuu_isabspath(filename)) {
return filename;
} }
// check entire schema for null pointers // TODO(kylebayes): We first look in the base model directory for files to
if (checkptr) { // remain backwards compatible.
char msg[100]; 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++) { XMLElement* parent = e->Parent()->ToElement();
// base pointers for (; parent; parent = parent->Parent()->ToElement()) {
if (!schema[i][0]) { if (!std::strcmp(parent->Value(), "include")) {
mju::sprintf_arr(msg, "null pointer found in row %d", i); auto file_attr = mjXUtil::ReadAttrStr(parent, "dir", false);
throw msg; if (file_attr.has_value()) {
} path = file_attr.value();
// 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;
}
}
} }
break;
} }
} }
return path + filename;
}
// constructor
mjXSchema::mjXSchema(const char* schema[][mjXATTRNUM], unsigned nrow) {
// set name and type // set name and type
name_ = schema[0][0]; name_ = schema[0][0];
type_ = schema[0][1][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 // process sub-elements of complex element
if (nrow>1) { if (nrow > 1) {
// check for bracketed block
if (schema[1][0][0]!='<' || schema[nrow-1][0][0]!='>') {
throw "expected brackets after complex element";
}
// parse block into simple and complex elements, create children // parse block into simple and complex elements, create children
int start = 2; int start = 2;
while (start < nrow-1) { while (start < nrow-1) {
int end = start; int end = start;
// look for bracketed block at start+1 // look for bracketed block at start+1
if (schema[start+1][0][0]=='<') { if (schema[start+1][0][0] == '<') {
// look for corresponding closing bracket // look for corresponding closing bracket
int cnt = 0; int cnt = 0;
while (end <= nrow-1) { while (end <= nrow-1) {
if (schema[end][0][0]=='<') { if (schema[end][0][0] == '<') {
cnt++; cnt++;
} else if (schema[end][0][0]=='>') { } else if (schema[end][0][0] == '>') {
cnt--; cnt--;
if (cnt==0) { if (cnt == 0) {
break; break;
} }
} }
end++; end++;
} }
// closing bracket not found
if (end > nrow-1) {
throw "matching closing bracket not found";
}
} }
// add element, check for error // add child element
subschema_.emplace_back(schema+start, end-start+1, false); subschema_.emplace_back(schema+start, end-start+1);
// proceed with next subelement // proceed with next subelement
start = end+1; 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 // if attribute is present, return attribute as a string
std::optional<std::string> mjXUtil::ReadAttrStr(XMLElement* elem, const char* attr, std::optional<std::string>
bool required) { mjXUtil::ReadAttrStr(XMLElement* elem, const char* attr, bool required) {
const char* pstr = elem->Attribute(attr); const char* pstr = elem->Attribute(attr);
// check if attribute exists // check if attribute exists
@@ -637,7 +610,16 @@ std::optional<std::string> mjXUtil::ReadAttrStr(XMLElement* elem, const char* at
return std::string(pstr); 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 // if attribute is present, return numerical value of attribute
template<typename T> template<typename T>
@@ -963,7 +945,8 @@ static int Round(double x) {
// write attribute // write attribute
template<typename T> 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 // make sure all are defined
if constexpr (std::is_floating_point_v<T>) { if constexpr (std::is_floating_point_v<T>) {
for (int i=0; i<n; i++) { 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; return;
} }
// trim identical trailing default values
if (trim) {
while (n > 0 && data[n-1] == def[n-1]) {
n--;
}
}
// increase precision for testing // increase precision for testing
stringstream stream; stringstream stream;
stream.precision(mujoco::_mjPRIVATE__get_xml_precision()); 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, 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, 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, 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, template void mjXUtil::WriteAttr(XMLElement* elem, string name, int n,
const unsigned char* data, const unsigned char* data,
const unsigned char* def); const unsigned char* def, bool trim);
// write vector<double> attribute, default = zero array // 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* FirstChildElement(XMLElement* e, const char* name = nullptr);
XMLElement* NextSiblingElement(XMLElement* e, const char* name = nullptr); XMLElement* NextSiblingElement(XMLElement* e, const char* name = nullptr);
// XML Error info // XML Error info
class [[nodiscard]] mjXError { class [[nodiscard]] mjXError {
public: public:
@@ -56,7 +55,7 @@ class [[nodiscard]] mjXError {
// Custom XML file validation // Custom XML file validation
class mjXSchema { class mjXSchema {
public: public:
mjXSchema(const char* schema[][mjXATTRNUM], unsigned nrow, bool checkptr = true); mjXSchema(const char* schema[][mjXATTRNUM], unsigned nrow);
std::string GetError(); // return error std::string GetError(); // return error
void Print(std::stringstream& str, int level) const; // print schema 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, static std::optional<std::string> ReadAttrStr(tinyxml2::XMLElement* elem, const char* attr,
bool required = false); 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 // if attribute is present, return numerical value of attribute
template<typename T> template<typename T>
static std::optional<T> ReadAttrNum(tinyxml2::XMLElement* elem, const char* attr, static std::optional<T> ReadAttrNum(tinyxml2::XMLElement* elem, const char* attr,
@@ -195,7 +198,7 @@ class mjXUtil {
// write attribute- any type // write attribute- any type
template<typename T> template<typename T>
static void WriteAttr(tinyxml2::XMLElement* elem, std::string name, int n, const T* data, 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 // write vector<double> attribute, with and without default
static void WriteVector(tinyxml2::XMLElement* elem, std::string name, static void WriteVector(tinyxml2::XMLElement* elem, std::string name,
test/CMakeLists.txt
+2
View File
@@ -58,6 +58,8 @@ target_link_libraries(
fixture fixture
PUBLIC absl::core_headers PUBLIC absl::core_headers
absl::synchronization absl::synchronization
absl::flat_hash_map
absl::flat_hash_set
gtest gtest
gmock gmock
mujoco::mujoco mujoco::mujoco
test/engine/engine_resource_test.cc
+10 -14
View File
@@ -14,6 +14,7 @@
// Tests for engine/engine_resource.c // Tests for engine/engine_resource.c
#include <array>
#include <cstring> #include <cstring>
#include <gmock/gmock.h> #include <gmock/gmock.h>
@@ -228,7 +229,7 @@ TEST_F(ResourceTest, GeneralTest) {
EXPECT_GT(i, 0); EXPECT_GT(i, 0);
// open resource // open resource
mjResource* resource = mju_openResource("str:file"); mjResource* resource = mju_openResource("str:file", nullptr, 0);
ASSERT_THAT(resource, NotNull()); ASSERT_THAT(resource, NotNull());
const char* buffer = NULL; const char* buffer = NULL;
@@ -239,7 +240,7 @@ TEST_F(ResourceTest, GeneralTest) {
mju_closeResource(resource); mju_closeResource(resource);
} }
TEST_F(ResourceTest, GeneralTestFailure) { TEST_F(ResourceTest, GeneralFailureTest) {
mjpResourceProvider provider = { mjpResourceProvider provider = {
"str", open_str, read_str, close_str "str", open_str, read_str, close_str
}; };
@@ -248,19 +249,14 @@ TEST_F(ResourceTest, GeneralTestFailure) {
int i = mjp_registerResourceProvider(&provider); int i = mjp_registerResourceProvider(&provider);
EXPECT_GT(i, 0); EXPECT_GT(i, 0);
static std::array<char, 1024> error;
// install warning handler
static char warning[1024];
warning[0] = '\0';
mju_user_warning = [](const char* msg) {
util::strcpy_arr(warning, msg);
};
// open resource // open resource
mjResource* resource = mju_openResource("str:notfound"); mjResource* resource = mju_openResource("str:notfound",
error.data(), error.size());
ASSERT_THAT(resource, IsNull()); ASSERT_THAT(resource, IsNull());
EXPECT_THAT(warning, HasSubstr("could not open")); EXPECT_THAT(error.data(), HasSubstr("could not open"));
} }
TEST_F(ResourceTest, NameWithValidPrefix) { TEST_F(ResourceTest, NameWithValidPrefix) {
@@ -281,7 +277,7 @@ TEST_F(ResourceTest, NameWithValidPrefix) {
}; };
// open resource // open resource
mjResource* resource = mju_openResource("nop:found"); mjResource* resource = mju_openResource("nop:found", nullptr, 0);
ASSERT_THAT(resource, NotNull()); ASSERT_THAT(resource, NotNull());
mju_closeResource(resource); mju_closeResource(resource);
} }
@@ -304,7 +300,7 @@ TEST_F(ResourceTest, NameWithUpperCasePrefix) {
}; };
// open resource // open resource
mjResource* resource = mju_openResource("NOP:found"); mjResource* resource = mju_openResource("NOP:found", nullptr, 0);
ASSERT_THAT(resource, NotNull()); ASSERT_THAT(resource, NotNull());
mju_closeResource(resource); mju_closeResource(resource);
} }
@@ -327,7 +323,7 @@ TEST_F(ResourceTest, NameWithInvalidPrefix) {
}; };
// open resource // open resource
mjResource* resource = mju_openResource("nopfound"); mjResource* resource = mju_openResource("nopfound", nullptr, 0);
ASSERT_THAT(resource, IsNull()); 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(buffer.data(), StrEq("D4a+//A="));
EXPECT_THAT(n, std::strlen(buffer.data()) + 1); EXPECT_THAT(n, std::strlen(buffer.data()) + 1);
EXPECT_THAT(n, buffer.size()); EXPECT_THAT(n, buffer.size());
} }
TEST_F(Base64Test, mju_encodeBase64_align0) { 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 // See the License for the specific language governing permissions and
// limitations under the License. // limitations under the License.
#include <array>
#include <cstdio>
#include <memory>
#include <string> #include <string>
#include <gmock/gmock.h> #include <gmock/gmock.h>
@@ -25,7 +28,7 @@ namespace {
using ::testing::NotNull; using ::testing::NotNull;
using EngineVfsTest = MujocoTest; using EngineVfsTest = MujocoTest;
TEST_F(EngineVfsTest, AddFileVFS) { TEST_F(EngineVfsTest, AddFileTest) {
constexpr char path[] = "engine/testdata/actuation/"; constexpr char path[] = "engine/testdata/actuation/";
const std::string dir = GetTestDataFilePath(path); const std::string dir = GetTestDataFilePath(path);
std::string file1 = "activation.xml"; std::string file1 = "activation.xml";
@@ -44,38 +47,51 @@ TEST_F(EngineVfsTest, AddFileVFS) {
ASSERT_THAT(fp3, NotNull()) << "Input file3 missing."; ASSERT_THAT(fp3, NotNull()) << "Input file3 missing.";
std::fclose(fp3); std::fclose(fp3);
auto mj_vfs = std::make_unique<mjVFS>(); auto vfs = std::make_unique<mjVFS>();
mj_defaultVFS(mj_vfs.get()); mj_defaultVFS(vfs.get());
EXPECT_THAT(mj_vfs->nfile, 0); EXPECT_THAT(vfs->nfile, 0);
EXPECT_THAT(mj_addFileVFS(mj_vfs.get(), dir.c_str(), file1.c_str()), 0); EXPECT_THAT(mj_addFileVFS(vfs.get(), dir.c_str(), file1.c_str()), 0);
EXPECT_THAT(mj_vfs->nfile, 1); EXPECT_THAT(vfs->nfile, 1);
EXPECT_THAT(mj_vfs->filename[0], file1); EXPECT_THAT(vfs->filename[0], file1);
EXPECT_THAT(mj_addFileVFS(mj_vfs.get(), dir.c_str(), file2.c_str()), 0); EXPECT_THAT(mj_addFileVFS(vfs.get(), dir.c_str(), file2.c_str()), 0);
EXPECT_THAT(mj_vfs->nfile, 2); EXPECT_THAT(vfs->nfile, 2);
EXPECT_THAT(mj_vfs->filename[0], file1); EXPECT_THAT(vfs->filename[0], file1);
EXPECT_THAT(mj_vfs->filename[1], file2); EXPECT_THAT(vfs->filename[1], file2);
EXPECT_THAT(mj_addFileVFS(mj_vfs.get(), dir.c_str(), file3.c_str()), 0); EXPECT_THAT(mj_addFileVFS(vfs.get(), dir.c_str(), file3.c_str()), 0);
EXPECT_THAT(mj_vfs->nfile, 3); EXPECT_THAT(vfs->nfile, 3);
EXPECT_THAT(mj_vfs->filename[0], file1); EXPECT_THAT(vfs->filename[0], file1);
EXPECT_THAT(mj_vfs->filename[1], file2); EXPECT_THAT(vfs->filename[1], file2);
EXPECT_THAT(mj_vfs->filename[2], file3); EXPECT_THAT(vfs->filename[2], file3);
mj_deleteFileVFS(mj_vfs.get(), file1.c_str()); mj_deleteFileVFS(vfs.get(), file1.c_str());
EXPECT_THAT(mj_vfs->nfile, 2); EXPECT_THAT(vfs->nfile, 2);
EXPECT_THAT(mj_vfs->filename[0], file2); EXPECT_THAT(vfs->filename[0], file2);
EXPECT_THAT(mj_vfs->filename[1], file3); EXPECT_THAT(vfs->filename[1], file3);
mj_deleteFileVFS(mj_vfs.get(), file3.c_str()); mj_deleteFileVFS(vfs.get(), file3.c_str());
EXPECT_THAT(mj_vfs->nfile, 1); EXPECT_THAT(vfs->nfile, 1);
EXPECT_THAT(mj_vfs->filename[0], file2); EXPECT_THAT(vfs->filename[0], file2);
mj_deleteFileVFS(mj_vfs.get(), file2.c_str()); mj_deleteFileVFS(vfs.get(), file2.c_str());
EXPECT_THAT(mj_vfs->nfile, 0); 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 } // namespace
test/fixture.cc
+121
View File
@@ -14,6 +14,8 @@
#include "test/fixture.h" #include "test/fixture.h"
#include <array>
#include <cerrno>
#include <cstdio> #include <cstdio>
#include <cstring> #include <cstring>
#include <filesystem> #include <filesystem>
@@ -21,12 +23,15 @@
#include <sstream> #include <sstream>
#include <string> #include <string>
#include <string_view> #include <string_view>
#include <utility>
#include <vector> #include <vector>
#include <gmock/gmock.h> #include <gmock/gmock.h>
#include <gtest/gtest.h> #include <gtest/gtest.h>
#include <absl/base/const_init.h> #include <absl/base/const_init.h>
#include <absl/strings/str_cat.h> #include <absl/strings/str_cat.h>
#include <absl/strings/str_join.h>
#include <absl/synchronization/mutex.h> #include <absl/synchronization/mutex.h>
#include <mujoco/mjmodel.h> #include <mujoco/mjmodel.h>
#include <mujoco/mujoco.h> #include <mujoco/mujoco.h>
@@ -167,4 +172,120 @@ std::vector<mjtNum> GetCtrlNoise(const mjModel* m, int nsteps,
return ctrl; 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 } // namespace mujoco
test/fixture.h
+47
View File
@@ -22,6 +22,8 @@
#include <vector> #include <vector>
#include <gtest/gtest.h> #include <gtest/gtest.h>
#include <absl/container/flat_hash_map.h>
#include <absl/container/flat_hash_set.h>
#include <mujoco/mjmodel.h> #include <mujoco/mjmodel.h>
#include <mujoco/mujoco.h> #include <mujoco/mujoco.h>
@@ -99,6 +101,51 @@ std::string SaveAndReadXml(const mjModel* model);
std::vector<mjtNum> GetCtrlNoise(const mjModel* m, int nsteps, std::vector<mjtNum> GetCtrlNoise(const mjModel* m, int nsteps,
mjtNum ctrlnoise = 0.01); 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 // Installs all plugins
class PluginTest : public MujocoTest { class PluginTest : public MujocoTest {
public: public:
test/fixture_test.cc
+19
View File
@@ -14,6 +14,8 @@
#include "test/fixture.h" #include "test/fixture.h"
#include <array>
#include <gmock/gmock.h> #include <gmock/gmock.h>
#include <gtest/gtest-spi.h> #include <gtest/gtest-spi.h>
#include <gtest/gtest.h> #include <gtest/gtest.h>
@@ -49,5 +51,22 @@ TEST_F(MujocoErrorTestGuardTest, NestedErrorGuards) {
EXPECT_THAT(mju_user_warning, IsNull()); 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
} // namespace mujoco } // namespace mujoco
test/user/user_api_test.cc
+1 -1
View File
@@ -29,7 +29,7 @@ using ::testing::HasSubstr;
// ----------------------------- test set/get -------------------------------- // ----------------------------- test set/get --------------------------------
TEST_F(MujocoTest, ReadWriteData) { TEST_F(MujocoTest, ReadWriteData) {
void* model = mjm_createModel(); mjmModel* model = mjm_createModel();
mjmBody* world = mjm_findBody(model, "world"); mjmBody* world = mjm_findBody(model, "world");
mjmBody* body = mjm_addBody(world, 0); mjmBody* body = mjm_addBody(world, 0);
mjmSite* site = mjm_addSite(body, 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); 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) { TEST_F(KeyframeTest, BadSize) {
static constexpr char xml[] = R"( static constexpr char xml[] = R"(
<mujoco> <mujoco>
@@ -909,6 +931,55 @@ TEST_F(ActuatorTest, ActuatorOrderDoesntMatter) {
mj_deleteModel(model1); 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 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 ------------------------------------- // ------------------------ 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) { TEST_F(XMLReaderTest, IncludeTest) {
static constexpr char xml[] = R"( static constexpr char xml[] = R"(
<mujoco> <mujoco>
@@ -517,7 +557,6 @@ TEST_F(XMLReaderTest, IncludeSameFileTest) {
<geom name="box" type="box" size="1 1 1"/> <geom name="box" type="box" size="1 1 1"/>
</mujoco>)"; </mujoco>)";
auto vfs = std::make_unique<mjVFS>(); auto vfs = std::make_unique<mjVFS>();
mj_defaultVFS(vfs.get()); mj_defaultVFS(vfs.get());
@@ -533,7 +572,229 @@ TEST_F(XMLReaderTest, IncludeSameFileTest) {
mj_deleteVFS(vfs.get()); 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 frame parsing ---------------------------------
TEST_F(XMLReaderTest, ParseFrame) { TEST_F(XMLReaderTest, ParseFrame) {
static constexpr char xml[] = R"( static constexpr char xml[] = R"(
<mujoco> <mujoco>
@@ -1040,10 +1301,10 @@ TEST_F(ActuatorParseTest, PositionIntvelocityVelocityDefaultsPropagate) {
<mujoco> <mujoco>
<default> <default>
<default class="position"> <default class="position">
<position kp="3" kv="4"/> <position kp="3" kv="4" inheritrange="2"/>
</default> </default>
<default class="intvelocity"> <default class="intvelocity">
<intvelocity kp="5" kv="6" actrange="-1 1"/> <intvelocity kp="5" kv="6" inheritrange="0.5"/>
</default> </default>
<default class="velocity"> <default class="velocity">
<velocity kv="7"/> <velocity kv="7"/>
@@ -1052,7 +1313,7 @@ TEST_F(ActuatorParseTest, PositionIntvelocityVelocityDefaultsPropagate) {
<worldbody> <worldbody>
<body> <body>
<geom size="1"/> <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> </body>
</worldbody> </worldbody>
<actuator> <actuator>
@@ -1082,6 +1343,10 @@ TEST_F(ActuatorParseTest, PositionIntvelocityVelocityDefaultsPropagate) {
EXPECT_EQ(model->actuator_gainprm[i*mjNGAIN + j], 0.0); 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); mj_deleteModel(model);
} }
test/xml/xml_native_writer_test.cc
+118
View File
@@ -694,6 +694,38 @@ TEST_F(XMLWriterTest, WritesDefaults) {
mj_deleteModel(model); 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) { TEST_F(XMLWriterTest, WritesDensity) {
static constexpr char xml[] = R"( static constexpr char xml[] = R"(
<mujoco> <mujoco>
@@ -967,6 +999,92 @@ TEST_F(XMLWriterTest, ActdimDefaults) {
mj_deleteModel(model); 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 // check that no precision is lost when saving XMLs with FullFloatPrecision
TEST_F(XMLWriterTest, SetPrecision) { TEST_F(XMLWriterTest, SetPrecision) {
static constexpr char xml[] = R"( 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 * 1000)] public char[] filename;
[MarshalAs(UnmanagedType.ByValArray, SizeConst = 2000)] public UIntPtr[] filesize; [MarshalAs(UnmanagedType.ByValArray, SizeConst = 2000)] public UIntPtr[] filesize;
[MarshalAs(UnmanagedType.ByValArray, SizeConst = 2000)] public IntPtr[] filedata; [MarshalAs(UnmanagedType.ByValArray, SizeConst = 2000)] public IntPtr[] filedata;
[MarshalAs(UnmanagedType.ByValArray, SizeConst = 2000)] public UInt64[] filestamp;
} }
[StructLayout(LayoutKind.Sequential)] [StructLayout(LayoutKind.Sequential)]
@@ -5365,8 +5366,6 @@ public unsafe struct mjModel_ {
public int* mesh_texcoordadr; public int* mesh_texcoordadr;
public int* mesh_texcoordnum; public int* mesh_texcoordnum;
public int* mesh_graphadr; public int* mesh_graphadr;
public double* mesh_pos;
public double* mesh_quat;
public float* mesh_vert; public float* mesh_vert;
public float* mesh_normal; public float* mesh_normal;
public float* mesh_texcoord; public float* mesh_texcoord;
@@ -5374,6 +5373,8 @@ public unsafe struct mjModel_ {
public int* mesh_facenormal; public int* mesh_facenormal;
public int* mesh_facetexcoord; public int* mesh_facetexcoord;
public int* mesh_graph; public int* mesh_graph;
public double* mesh_pos;
public double* mesh_quat;
public int* mesh_pathadr; public int* mesh_pathadr;
public int* skin_matid; public int* skin_matid;
public int* skin_group; 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); public static unsafe extern int mj_addFileVFS(void* vfs, [MarshalAs(UnmanagedType.LPStr)]string directory, [MarshalAs(UnmanagedType.LPStr)]string filename);
[DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] [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)] [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)]
public static unsafe extern int mj_findFileVFS(void* vfs, [MarshalAs(UnmanagedType.LPStr)]string filename); 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)] [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)]
public static unsafe extern void mj_deleteVFS(void* vfs); 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)] [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)]
public static unsafe extern mjModel_* mj_loadXML([MarshalAs(UnmanagedType.LPStr)]string filename, void* vfs, StringBuilder error, int error_sz); public static unsafe extern mjModel_* mj_loadXML([MarshalAs(UnmanagedType.LPStr)]string filename, void* vfs, StringBuilder error, int error_sz);