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

Merge branch 'google-deepmind:main' into tendons

Browse Source
This commit is contained in:
Abhishek Joshi
2024-06-03 11:14:21 -05:00
committed by GitHub
parent ee82810ad3 2830a4071f
commit 0cfdc89c5b
173 changed files with 9083 additions and 40007 deletions
Download Patch File Download Diff File Expand all files Collapse all files
CMakeLists.txt
+1 -1
View File
@@ -28,7 +28,7 @@ set(MSVC_INCREMENTAL_DEFAULT ON)
project(
mujoco
VERSION 3.1.5
VERSION 3.1.6
DESCRIPTION "MuJoCo Physics Simulator"
HOMEPAGE_URL "https://mujoco.org"
)
README.md
+4 -3
View File
@@ -50,16 +50,17 @@ your machine.
If you are a Python user, you might want to start with our tutorial notebooks
running on Google Colab:
- The **introductory tutorial** teaches MuJoCo basics:
- The **introductory** tutorial teaches MuJoCo basics:
[![Open In Colab](https://colab.research.google.com/assets/colab-badge.svg)](https://colab.research.google.com/github/google-deepmind/mujoco/blob/main/python/tutorial.ipynb)
- The **LQR** tutorial synthesizes a linear-quadratic controller, balancing a humanoid on one leg:
[![Open In Colab](https://colab.research.google.com/assets/colab-badge.svg)](https://colab.research.google.com/github/google-deepmind/mujoco/blob/main/python/LQR.ipynb)
- The **least-squares** tutorial explains how to use the Python-based nonlinear least-squares solver:
[![Open In Colab](https://colab.research.google.com/assets/colab-badge.svg)](https://colab.research.google.com/github/google-deepmind/mujoco/blob/main/python/least_squares.ipynb)
- The **MJX** tutorial provides usage examples of
[MuJoCo XLA](https://mujoco.readthedocs.io/en/stable/mjx.html), a branch of MuJoCo written in
JAX:
[MuJoCo XLA](https://mujoco.readthedocs.io/en/stable/mjx.html), a branch of MuJoCo written in JAX:
[![Open In Colab](https://colab.research.google.com/assets/colab-badge.svg)](https://colab.research.google.com/github/google-deepmind/mujoco/blob/main/mjx/tutorial.ipynb)
- The **differentiable physics** tutorial trains locomotion policies with analytical gradients automatically derived from MuJoCo's physics step:
[![Open In Colab](https://colab.research.google.com/assets/colab-badge.svg)](https://colab.research.google.com/github/google-deepmind/mujoco/blob/main/mjx/training_apg.ipynb)
## Installation
cmake/MujocoDependencies.cmake
+2 -2
View File
@@ -27,7 +27,7 @@ set(MUJOCO_DEP_VERSION_tinyobjloader
CACHE STRING "Version of `tinyobjloader` to be fetched."
)
set(MUJOCO_DEP_VERSION_MarchingCubeCpp
5b79e5d6bded086a0abe276a4b5a69fc17ae9bf1
f03a1b3ec29b1d7d865691ca8aea4f1eb2c2873d
CACHE STRING "Version of `MarchingCubeCpp` to be fetched."
)
set(MUJOCO_DEP_VERSION_ccd
@@ -44,7 +44,7 @@ set(MUJOCO_DEP_VERSION_Eigen3
)
set(MUJOCO_DEP_VERSION_abseil
2f9e432cce407ce0ae50676696666f33a77d42ac # LTS 20240116.1
d7aaad83b488fd62bd51c81ecf16cd938532cc0a # LTS 20240116.2
CACHE STRING "Version of `abseil` to be fetched."
)
dist/mujoco.rc
Vendored
+4 -4
View File
@@ -1,6 +1,6 @@
1 VERSIONINFO
FILEVERSION 3,1,5,0
PRODUCTVERSION 3,1,5,0
FILEVERSION 3,1,6,0
PRODUCTVERSION 3,1,6,0
FILEOS 0x4
FILETYPE 0x1
{
@@ -9,9 +9,9 @@ FILETYPE 0x1
BLOCK "040904b0"
{
VALUE "ProductName", "MuJoCo"
VALUE "ProductVersion", "3.1.5"
VALUE "ProductVersion", "3.1.6"
VALUE "FileDescription", "MuJoCo"
VALUE "FileVersion", "3.1.5"
VALUE "FileVersion", "3.1.6"
VALUE "InternalName", "mujoco.dll"
VALUE "OriginalFilename", "mujoco.dll"
VALUE "CompanyName", "Google DeepMind"
dist/simulate.rc
Vendored
+4 -4
View File
@@ -1,8 +1,8 @@
MUJOCO ICON "mujoco.ico"
1 VERSIONINFO
FILEVERSION 3,1,5,0
PRODUCTVERSION 3,1,5,0
FILEVERSION 3,1,6,0
PRODUCTVERSION 3,1,6,0
FILEOS 0x4
FILETYPE 0x1
{
@@ -11,9 +11,9 @@ FILETYPE 0x1
BLOCK "040904b0"
{
VALUE "ProductName", "MuJoCo"
VALUE "ProductVersion", "3.1.5"
VALUE "ProductVersion", "3.1.6"
VALUE "FileDescription", "MuJoCo"
VALUE "FileVersion", "3.1.5"
VALUE "FileVersion", "3.1.6"
VALUE "InternalName", "simulate.exe"
VALUE "OriginalFilename", "simulate.exe"
VALUE "CompanyName", "Google DeepMind"
doc/APIreference/APIglobals.rst
+1 -1
View File
@@ -522,7 +522,7 @@ shown in the table below. Their names are in the format ``mjKEY_XXX``. They corr
- Maximum number of UI rectangles.
Defined in `mjui.h <https://github.com/google-deepmind/mujoco/blob/main/include/mujoco/mjui.h>`_.
* - ``mjVERSION_HEADER``
- 315
- 316
- The version of the MuJoCo headers; changes with every release. This is an integer equal to 100x the software
version, so 210 corresponds to version 2.1. Defined in mujoco.h. The API function :ref:`mj_version` returns a
number with the same meaning but for the compiled library.
doc/APIreference/functions.rst
+32 -10
View File
@@ -57,27 +57,27 @@ Main simulation
These are the main entry points to the simulator. Most users will only need to call :ref:`mj_step`, which computes
everything and advanced the simulation state by one time step. Controls and applied forces must either be set in advance
(in mjData.{ctrl, qfrc_applied, xfrc_applied}), or a control callback :ref:`mjcb_control` must be installed which will be
called just before the controls and applied forces are needed. Alternatively, one can use :ref:`mj_step1` and
(in ``mjData.{ctrl, qfrc_applied, xfrc_applied}``), or a control callback :ref:`mjcb_control` must be installed which
will be called just before the controls and applied forces are needed. Alternatively, one can use :ref:`mj_step1` and
:ref:`mj_step2` which break down the simulation pipeline into computations that are executed before and after the
controls are needed; in this way one can set controls that depend on the results from :ref:`mj_step1`. Keep in mind
though that the RK4 solver does not work with mj_step1/2.
though that the RK4 solver does not work with mj_step1/2. See :ref:`Pipeline` for a more detailed description.
mj_forward performs the same computations as :ref:`mj_step` but without the integration. It is useful after loading or
resetting a model (to put the entire mjData in a valid state), and also for out-of-order computations that involve
sampling or finite-difference approximations.
mj_inverse runs the inverse dynamics, and writes its output in ``mjData.qfrc_inverse``. Note that ``mjData.qacc`` must
be set before calling this function. Given the state (qpos, qvel, act), mj_forward maps from force to acceleration,
:ref:`mj_inverse` runs the inverse dynamics, and writes its output in ``mjData.qfrc_inverse``. Note that ``mjData.qacc``
must be set before calling this function. Given the state (qpos, qvel, act), mj_forward maps from force to acceleration,
while mj_inverse maps from acceleration to force. Mathematically these functions are inverse of each other, but
numerically this may not always be the case because the forward dynamics rely on a constraint optimization algorithm
which is usually terminated early. The difference between the results of forward and inverse dynamics can be computed
with the function :ref:`mj_compareFwdInv`, which can be thought of as another solver accuracy check (as well as a general
sanity check).
with the function :ref:`mj_compareFwdInv`, which can be thought of as another solver accuracy check (as well as a
general sanity check).
The skip version of :ref:`mj_forward` and :ref:`mj_inverse` are useful for example when qpos was unchanged but qvel was
changed (usually in the context of finite differencing). Then there is no point repeating the computations that only
depend on qpos. Calling the dynamics with skipstage = mjSTAGE_POS will achieve these savings.
depend on qpos. Calling the dynamics with skipstage = :ref:`mjSTAGE_POS<mjtStage>` will achieve these savings.
.. _mj_step:
@@ -249,8 +249,8 @@ degrees-of-freedom and a given point. Given a body specified by its integer id (
frame (``point``) treated as attached to the body, the Jacobian has both translational (``jacp``) and rotational
(``jacr``) components. Passing ``NULL`` for either pointer will skip that part of the computation. Each component is a
3-by-nv matrix. Each row of this matrix is the gradient of the corresponding coordinate of the specified point with
respect to the degrees-of-freedom. The ability to compute end-effector Jacobians efficiently and analytically is one of
the advantages of working in minimal coordinates.
respect to the degrees-of-freedom. The :ref:`pipeline stages<piStages>` required for Jacobian computations to be
consistenst with the current generalized positions ``mjData.qpos`` are :ref:`mj_kinematics` and :ref:`mj_comPos`.
.. _mj_jacBody:
@@ -408,6 +408,28 @@ Compute object 6D acceleration (rot:lin) in object-centered frame, world/local o
sensors are not present in the model, :ref:`mj_rnePostConstraint` must be manually called in order to calculate
mjData.cacc -- the total body acceleration, including contributions from the constraint solver.
.. _mj_geomDistance:
mj_geomDistance
~~~~~~~~~~~~~~~
.. mujoco-include:: mj_geomDistance
Returns the smallest signed distance between two geoms and optionally the segment from ``geom1`` to ``geom2``.
Returned distances are bounded from above by ``distmax``. |br| If no collision of distance smaller than ``distmax`` is
found, the function will return ``distmax`` and ``fromto``, if given, will be set to (0, 0, 0, 0, 0, 0).
.. admonition:: Positive ``distmax`` values
:class: note
.. TODO: b/339596989 - Improve mjc_Convex.
For some colliders, a large, positive ``distmax`` will result in an accurate measurement. However, for collision
pairs which use the general ``mjc_Convex`` collider, the result will be approximate and likely innacurate.
This is considered a bug to be fixed in a future release.
In order to determine whether a geom pair uses ``mjc_Convex``, inspect the table at the top of
`engine_collision_driver.c <https://github.com/google-deepmind/mujoco/blob/main/src/engine/engine_collision_driver.c>`__.
.. _mj_contactForce:
mj_contactForce
doc/APIreference/functions_override.rst
+27 -10
View File
@@ -33,27 +33,27 @@ The model and all files referenced in it can be loaded from disk or from a VFS w
These are the main entry points to the simulator. Most users will only need to call :ref:`mj_step`, which computes
everything and advanced the simulation state by one time step. Controls and applied forces must either be set in advance
(in mjData.{ctrl, qfrc_applied, xfrc_applied}), or a control callback :ref:`mjcb_control` must be installed which will be
called just before the controls and applied forces are needed. Alternatively, one can use :ref:`mj_step1` and
(in ``mjData.{ctrl, qfrc_applied, xfrc_applied}``), or a control callback :ref:`mjcb_control` must be installed which
will be called just before the controls and applied forces are needed. Alternatively, one can use :ref:`mj_step1` and
:ref:`mj_step2` which break down the simulation pipeline into computations that are executed before and after the
controls are needed; in this way one can set controls that depend on the results from :ref:`mj_step1`. Keep in mind
though that the RK4 solver does not work with mj_step1/2.
though that the RK4 solver does not work with mj_step1/2. See :ref:`Pipeline` for a more detailed description.
mj_forward performs the same computations as :ref:`mj_step` but without the integration. It is useful after loading or
resetting a model (to put the entire mjData in a valid state), and also for out-of-order computations that involve
sampling or finite-difference approximations.
mj_inverse runs the inverse dynamics, and writes its output in ``mjData.qfrc_inverse``. Note that ``mjData.qacc`` must
be set before calling this function. Given the state (qpos, qvel, act), mj_forward maps from force to acceleration,
:ref:`mj_inverse` runs the inverse dynamics, and writes its output in ``mjData.qfrc_inverse``. Note that ``mjData.qacc``
must be set before calling this function. Given the state (qpos, qvel, act), mj_forward maps from force to acceleration,
while mj_inverse maps from acceleration to force. Mathematically these functions are inverse of each other, but
numerically this may not always be the case because the forward dynamics rely on a constraint optimization algorithm
which is usually terminated early. The difference between the results of forward and inverse dynamics can be computed
with the function :ref:`mj_compareFwdInv`, which can be thought of as another solver accuracy check (as well as a general
sanity check).
with the function :ref:`mj_compareFwdInv`, which can be thought of as another solver accuracy check (as well as a
general sanity check).
The skip version of :ref:`mj_forward` and :ref:`mj_inverse` are useful for example when qpos was unchanged but qvel was
changed (usually in the context of finite differencing). Then there is no point repeating the computations that only
depend on qpos. Calling the dynamics with skipstage = mjSTAGE_POS will achieve these savings.
depend on qpos. Calling the dynamics with skipstage = :ref:`mjSTAGE_POS<mjtStage>` will achieve these savings.
.. _Initialization:
@@ -165,8 +165,8 @@ degrees-of-freedom and a given point. Given a body specified by its integer id (
frame (``point``) treated as attached to the body, the Jacobian has both translational (``jacp``) and rotational
(``jacr``) components. Passing ``NULL`` for either pointer will skip that part of the computation. Each component is a
3-by-nv matrix. Each row of this matrix is the gradient of the corresponding coordinate of the specified point with
respect to the degrees-of-freedom. The ability to compute end-effector Jacobians efficiently and analytically is one of
the advantages of working in minimal coordinates.
respect to the degrees-of-freedom. The :ref:`pipeline stages<piStages>` required for Jacobian computations to be
consistenst with the current generalized positions ``mjData.qpos`` are :ref:`mj_kinematics` and :ref:`mj_comPos`.
.. _mj_jacBody:
@@ -180,6 +180,23 @@ generalized velocities to subtree angular momentum. More precisely if :math:`h`
body index ``body`` in ``mjData.subtree_angmom`` (reported by the :ref:`subtreeangmom<sensor-subtreeangmom>` sensor)
and :math:`\dot q` is the generalized velocity ``mjData.qvel``, then :math:`h = H \dot q`.
.. _mj_geomDistance:
Returns the smallest signed distance between two geoms and optionally the segment from ``geom1`` to ``geom2``.
Returned distances are bounded from above by ``distmax``. |br| If no collision of distance smaller than ``distmax`` is
found, the function will return ``distmax`` and ``fromto``, if given, will be set to (0, 0, 0, 0, 0, 0).
.. admonition:: Positive ``distmax`` values
:class: note
.. TODO: b/339596989 - Improve mjc_Convex.
For some colliders, a large, positive ``distmax`` will result in an accurate measurement. However, for collision
pairs which use the general ``mjc_Convex`` collider, the result will be approximate and likely innacurate.
This is considered a bug to be fixed in a future release.
In order to determine whether a geom pair uses ``mjc_Convex``, inspect the table at the top of
`engine_collision_driver.c <https://github.com/google-deepmind/mujoco/blob/main/src/engine/engine_collision_driver.c>`__.
.. _mj_mulM:
This function multiplies the joint-space inertia matrix stored in mjData.qM by a vector. qM has a custom sparse format
doc/XMLreference.rst
+249 -16
View File
@@ -214,6 +214,10 @@ and namespaced appropriately. Detailed examples of models using replicate can be
</replicate>
</replicate>
</worldbody>
<sensor>
<accelerometer name="Bob" site="Alice"/>
</sensor>
</mujoco>
Results in this model:
@@ -227,6 +231,13 @@ and namespaced appropriately. Detailed examples of models using replicate can be
<geom name="Alice-01" size="0.1" pos="0 1 0" quat="1 1 0 0"/>
<geom name="Alice-11" size="0.1" pos="1 1 0" quat="0.5 0.5 0.5 0.5"/>
</worldbody>
<sensor>
<accelerometer name="Bob-00" site="Alice-00"/>
<accelerometer name="Bob-10" site="Alice-10"/>
<accelerometer name="Bob-01" site="Alice-01"/>
<accelerometer name="Bob-11" site="Alice-11"/>
</sensor>
</mujoco>
.. _include:
@@ -301,8 +312,8 @@ adjust it properly through the XML.
setting of solimp determines a single impedance value for all contact dimensions, which is then modulated by this
attribute. Settings larger than 1 cause friction forces to be "harder" than normal forces, having the general effect
of preventing slip, without increasing the actual friction coefficient. For pyramidal friction cones the situation is
more complex because the pyramidal approximation mixes normal and frictional dimensions within each basis vector; but
the overall effect of this attribute is qualitatively similar.
more complex because the pyramidal approximation mixes normal and frictional dimensions within each basis vector; it
is not recommended to use high impratio values with pyramidal cones.
.. _option-gravity:
@@ -1088,6 +1099,8 @@ specified with OBJ files and MSH files, as well as explicitly in the XML with th
STL files. These mechanism cannot be mixed. So if you have an STL mesh, the only way to add texture coordinates to it is
to convert to one of the other supported formats.
.. _legacy-msh-docs:
MSH file format
The binary MSH file starts with 4 integers specifying the number of vertex positions (nvertex), vertex normals
(nnormal), vertex texture coordinates (ntexcoord), and vertex indices making up the faces (nface), followed by the
@@ -2274,7 +2287,8 @@ helps clarify the role of bodies and geoms in MuJoCo.
along both axes of the tangent plane. The second number is the torsional friction, acting around the contact normal.
The third number is the rolling friction, acting around both axes of the tangent plane. The friction parameters for
the contact pair are combined depending on the solmix and priority attributes, as explained in :ref:`Contact
parameters <CContact>`.
parameters <CContact>`. See the general :ref:`Contact<coContact>` section for descriptions of the semantics of this
attribute.
.. _body-geom-mass:
@@ -4261,11 +4275,15 @@ joint types (slide and hinge) can be used.
.. _equality-joint-polycoef:
:at:`polycoef`: :at-val:`real(5), "0 1 0 0 0"`
Coefficients a0 ... a4 of the quartic polynomial. If the two joint values are y and x, and their reference positions
(corresponding to the joint values in the initial model configuration) are y0 and x0, the constraint is:
y-y0 = a0 + a1*(x-x0) + a2*(x-x0)^2 + a3*(x-x0)^3 + a4*(x-x0)^4.
Omitting the second joint is equivalent to setting x = x0, in which case the constraint is y = y0 + a0.
Coefficients :math:`a_0 \ldots a_4` of the quartic polynomial. If the joint values of :at:`joint1` and :at:`joint2`
are respectively :math:`y` and :math:`x`, and their reference positions (corresponding to the joint values in the
initial model configuration) are :math:`y_0` and :math:`x_0`, the constraint is:
.. math::
y-y_0 = a_0 + a_1(x-x_0) + a_2(x-x_0)^2 + a_3(x-x_0)^3 + a_4(x-x_0)^4
Omitting :at:`joint2` is equivalent to setting :math:`x = x_0`, in which case the constraint is
:math:`y = y_0 + a_0`.
.. _equality-tendon:
@@ -4301,7 +4319,7 @@ This element constrains the length of one tendon to be a quartic polynomial of a
.. _equality-tendon-polycoef:
:at:`polycoef`: :at-val:`real(5), "0 1 0 0 0"`
Same as in the equality/ :ref:`joint <equality-joint>` element above, but applied to tendon lengths instead of joint
Same as in the :ref:`equality/joint <equality-joint>` element above, but applied to tendon lengths instead of joint
positions.
@@ -4958,15 +4976,16 @@ This element does not have custom attributes. It only has common attributes, whi
:el-prefix:`actuator/` |-| **position** (*)
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
This element creates a position servo. The underlying :el:`general` attributes are set as follows:
This element creates a position servo with an optional first-order filter. The underlying :el:`general` attributes are
set as follows:
========= ======= ========= =========
Attribute Setting Attribute Setting
========= ======= ========= =========
dyntype none dynprm 1 0 0
gaintype fixed gainprm kp 0 0
biastype affine biasprm 0 -kp -kv
========= ======= ========= =========
========= =================== ========= =============
Attribute Setting Attribute Setting
========= =================== ========= =============
dyntype none or filterexact dynprm timeconst 0 0
gaintype fixed gainprm kp 0 0
biastype affine biasprm 0 -kp -kv
========= =================== ========= =============
This element has one custom attribute in addition to the common attributes:
@@ -5026,6 +5045,29 @@ This element has one custom attribute in addition to the common attributes:
Damping applied by the actuator.
When using this attribute, it is recommended to use the implicitfast or implicit :ref:`integrators<geIntegration>`.
.. _actuator-position-dampratio:
:at:`dampratio`: :at-val:`real, "0"`
Damping applied by the actuator, using damping ratio units.
This attribute is exclusive with :at:`kv` and has similar meaning, but instead of units of force/velocity, the units
are :math:`2 \sqrt{k_p \cdot m}`, corresponding to a harmonic oscillator's
`damping ratio <https://en.wikipedia.org/wiki/Damping#Damping_ratio_definition>`__.
A value of 1 corresponds to a *critically damped* oscillator, which often produces desirable behavior.
Values smaller or larger than 1 correspond to underdamped and overdamped oscillations, respectively.
The mass :math:`m` is computed at the reference configuration ``mjModel.qpos0``, taking into account joint
:ref:`armature <body-joint-armature>`.
However, passive :ref:`damping <body-joint-damping>` or :ref:`frictionloss <body-joint-frictionloss>` in the affected
joints are not taken into account; if they are non-negligible, :at:`dampratio` values smaller than 1 might be
required to achieve desirable motion.
When using this attribute, it is recommended to use the implicitfast or implicit :ref:`integrators<geIntegration>`.
.. _actuator-position-timeconst:
:at:`timeconst`: :at-val:`real, "0"`
Time-constant of optional first-order filter. If larger than zero, the actuator uses the :at:`filterexact`
:ref:`dynamics type<actuator-general-dyntype>`, if zero (the default) no filter is used.
.. _actuator-position-inheritrange:
:at:`inheritrange`: :at-val:`real, "0"`
@@ -5188,6 +5230,11 @@ This element has one custom attribute in addition to the common attributes:
Damping applied by the actuator.
When using this attribute, it is recommended to use the implicitfast or implicit :ref:`integrators<geIntegration>`.
.. _actuator-intvelocity-dampratio:
:at:`dampratio`: :at-val:`real, "0"`
See :ref:`position/dampratio<actuator-position-dampratio>`.
.. _actuator-intvelocity-inheritrange:
:at:`inheritrange`: :at-val:`real, "0"`
@@ -6720,6 +6767,186 @@ The presence of this sensor in a model triggers a call to :ref:`mj_subtreeVel` d
:at:`body`: :at-val:`string, required`
Name of the body where the kinematic subtree is rooted.
.. _collision-sensors:
collision sensors
^^^^^^^^^^^^^^^^^
The following 3 sensor types, :ref:`sensor/distance<sensor-distance>`, :ref:`sensor/normal<sensor-normal>` and
:ref:`sensor/fromto<sensor-fromto>`, respectively measure the distance, normal direction and line segment of the
smallest signed distance between the surfaces of two geoms using the narrow-phase geom-geom colliders. The collision
computation is always performed, independently of the standard collision :ref:`selection and filtering<coSelection>`
pipeline. These 3 sensors share some common properties:
.. _collision-sensors-cutoff:
:at:`cutoff`
For most sensors, the :at:`cutoff` attribute simply defines a clipping operation on sensor values. For collision
sensors, it defines the maximum distance at which collisions will be detected, corresponding to the ``dismax``
argument of :ref:`mj_geomDistance`. For example, at the default value of 0, only negative distances (corresponding
to geom-geom penetration) will be reported by :ref:`sensor/distance<sensor-distance>`.
In order to determine collision properties of non-penetrating geom pairs, a positive :at:`cutoff` is required.
.. admonition:: Positive cutoff values
:class: note
.. TODO: b/339596989 - Improve mjc_Convex.
For some colliders, a positive :at:`cutoff` will result in an accurate measurement. However, for collision
pairs which use the general ``mjc_Convex`` collider, the result will be approximate and likely innacurate.
This is considered a bug to be fixed in a future release.
In order to determine whether a geom pair uses ``mjc_Convex``, inspect the table at the top of
`engine_collision_driver.c <https://github.com/google-deepmind/mujoco/blob/main/src/engine/engine_collision_driver.c>`__.
:at:`geom1`, :at:`geom2`, :at:`body1`, :at:`body2`
For all 3 collision sensor types, the two colliding geoms can be specified explicitly using the :at:`geom1` and
:at:`geom2` attributes or implicitly, using :at:`body1`, :at:`body2`. In the latter case the sensor will iterate over
all geoms of the specified body or bodies (mixed specification like :at:`geom1`, :at:`body2` are allowed), and
select the collision with the smallest signed distance.
sequential sensors
When multiple collision sensors are defined sequentially and have identical attributes (:at:`geom1`, :at:`body1`,
:at:`geom2`, :at:`body2`, :at:`cutoff`), for example when both distance and normal are queried for the same geom
pair, the collision functions will be called once for the whole sensor block, avoiding repeated computation.
.. _sensor-distance:
:el-prefix:`sensor/` |-| **distance** (*)
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
This element creates a sensor that returns the smallest signed distance between the surfaces of two geoms.
See :ref:`collision-sensors` for more details about sensors of this type.
.. _sensor-distance-cutoff:
:at:`cutoff`
See :ref:`collision-sensors` for the sematics of this attribute, which is different than for other sensor categories.
If no collision is detected, the distance sensor returns the :at:`cutoff` value, so in this case
:at:`cutoff` acts as a maximum clipping value, in addition to the special semantics.
.. _sensor-distance-geom1:
:at:`geom1`: :at-val:`string, optional`
Name of the first geom. Exactly one of (:at:`geom1`, :at:`body1`) must be specified.
.. _sensor-distance-geom2:
:at:`geom2`: :at-val:`string, optional`
Name of the second geom. Exactly one of (:at:`geom2`, :at:`body2`) must be specified.
.. _sensor-distance-body1:
:at:`body1`: :at-val:`string, optional`
Name of the first body. Exactly one of (:at:`geom1`, :at:`body1`) must be specified.
.. _sensor-distance-body2:
:at:`body2`: :at-val:`string, optional`
Name of the second body. Exactly one of (:at:`geom2`, :at:`body2`) must be specified.
.. _sensor-distance-name:
.. _sensor-distance-noise:
.. _sensor-distance-user:
:at:`name`, :at:`noise`, :at:`user`
See :ref:`CSensor`.
.. _sensor-normal:
:el-prefix:`sensor/` |-| **normal** (*)
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
This element creates a sensor that returns the normal direction of the smallest signed distance between the surfaces of
two geoms. It is guaranteed to point from the surface of geom1 to the surface of geom2, though note that in the case of
penetration, this direction is generally in the opposite direction to that of the centroids.
See :ref:`collision-sensors` for more details about sensors of this type.
.. _sensor-normal-cutoff:
:at:`cutoff`
See :ref:`collision-sensors` for the sematics of this attribute, which is different than for other sensor categories.
If no collision is detected, the :ref:`normal<sensor-normal>` sensor returns (0, 0, 0), otherwise it returns a
normalized direction vector. For this sensor, :at:`cutoff` does not lead to any clamping.
.. _sensor-normal-geom1:
:at:`geom1`: :at-val:`string, optional`
Name of the first geom. Exactly one of (:at:`geom1`, :at:`body1`) must be specified.
.. _sensor-normal-geom2:
:at:`geom2`: :at-val:`string, optional`
Name of the second geom. Exactly one of (:at:`geom2`, :at:`body2`) must be specified.
.. _sensor-normal-body1:
:at:`body1`: :at-val:`string, optional`
Name of the first body. Exactly one of (:at:`geom1`, :at:`body1`) must be specified.
.. _sensor-normal-body2:
:at:`body2`: :at-val:`string, optional`
Name of the second body. Exactly one of (:at:`geom2`, :at:`body2`) must be specified.
.. _sensor-normal-name:
.. _sensor-normal-noise:
.. _sensor-normal-user:
:at:`name`, :at:`noise`, :at:`user`
See :ref:`CSensor`.
.. _sensor-fromto:
:el-prefix:`sensor/` |-| **fromto** (*)
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
This element creates a sensor that returns the segment defining the smallest signed distance between the surfaces of two
geoms. The segment is defined by 6 numbers (x1, y1, z1, x2, y2, z2) corresponding to two points in the world frame.
(x1, y1, z1) is on the surface of geom1, (x2, y2, z2) is on the surface of geom2. When this sensor is present and the
:ref:`mjVIS_RANGEFINDER<mjtVisFlag>` visualization flag is set, segments will be visualized as rangefinder rays.
See :ref:`collision-sensors` for more details about sensors of this type.
.. _sensor-fromto-cutoff:
:at:`cutoff`
See :ref:`collision-sensors` for the sematics of this attribute, which is different than for other sensor categories.
If no collision is detected, the :ref:`fromto<sensor-fromto>` sensor returns 6 zeros.
For this sensor, :at:`cutoff` does not lead to any clamping.
.. _sensor-fromto-geom1:
:at:`geom1`: :at-val:`string, optional`
Name of the first geom. Exactly one of (:at:`geom1`, :at:`body1`) must be specified.
.. _sensor-fromto-geom2:
:at:`geom2`: :at-val:`string, optional`
Name of the second geom. Exactly one of (:at:`geom2`, :at:`body2`) must be specified.
.. _sensor-fromto-body1:
:at:`body1`: :at-val:`string, optional`
Name of the first body. Exactly one of (:at:`geom1`, :at:`body1`) must be specified.
.. _sensor-fromto-body2:
:at:`body2`: :at-val:`string, optional`
Name of the second body. Exactly one of (:at:`geom2`, :at:`body2`) must be specified.
.. _sensor-fromto-name:
.. _sensor-fromto-noise:
.. _sensor-fromto-user:
:at:`name`, :at:`noise`, :at:`user`
See :ref:`CSensor`.
.. _sensor-clock:
@@ -7891,6 +8118,10 @@ tendon, slidersite, cranksite.
.. _default-position-kv:
.. _default-position-dampratio:
.. _default-position-timeconst:
:el-prefix:`default/` |-| **position** (?)
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
@@ -7951,6 +8182,8 @@ refsite, tendon, slidersite, cranksite.
.. _default-intvelocity-kv:
.. _default-intvelocity-dampratio:
:el-prefix:`default/` |-| **intvelocity** (?)
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
doc/XMLschema.rst
+34 -3
View File
@@ -705,7 +705,7 @@
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :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>` | | | | |
| | | | :ref:`kv<actuator-position-kv>` | :ref:`dampratio<actuator-position-dampratio>` | :ref:`timeconst<actuator-position-timeconst>` | | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
| |_| actuator |br| |_| |L| | | .. table:: |
@@ -737,7 +737,7 @@
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`slidersite<actuator-intvelocity-slidersite>` | :ref:`cranksite<actuator-intvelocity-cranksite>` | :ref:`site<actuator-intvelocity-site>` | :ref:`refsite<actuator-intvelocity-refsite>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`kp<actuator-intvelocity-kp>` | :ref:`kv<actuator-intvelocity-kv>` | | | |
| | | | :ref:`kp<actuator-intvelocity-kp>` | :ref:`kv<actuator-intvelocity-kv>` | :ref:`dampratio<actuator-intvelocity-dampratio>` | | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
| |_| actuator |br| |_| |L| | | .. table:: |
@@ -1163,6 +1163,33 @@
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
| |_| sensor |br| |_| |L| | | .. table:: |
| :ref:`distance | \* | :class: mjcf-attributes |
| <sensor-distance>` | | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`name<sensor-distance-name>` | :ref:`geom1<sensor-distance-geom1>` | :ref:`geom2<sensor-distance-geom2>` | :ref:`body1<sensor-distance-body1>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`body2<sensor-distance-body2>` | :ref:`cutoff<sensor-distance-cutoff>` | :ref:`noise<sensor-distance-noise>` | :ref:`user<sensor-distance-user>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
| |_| sensor |br| |_| |L| | | .. table:: |
| :ref:`normal | \* | :class: mjcf-attributes |
| <sensor-normal>` | | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`name<sensor-normal-name>` | :ref:`geom1<sensor-normal-geom1>` | :ref:`geom2<sensor-normal-geom2>` | :ref:`body1<sensor-normal-body1>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`body2<sensor-normal-body2>` | :ref:`cutoff<sensor-normal-cutoff>` | :ref:`noise<sensor-normal-noise>` | :ref:`user<sensor-normal-user>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
| |_| sensor |br| |_| |L| | | .. table:: |
| :ref:`fromto | \* | :class: mjcf-attributes |
| <sensor-fromto>` | | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`name<sensor-fromto-name>` | :ref:`geom1<sensor-fromto-geom1>` | :ref:`geom2<sensor-fromto-geom2>` | :ref:`body1<sensor-fromto-body1>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`body2<sensor-fromto-body2>` | :ref:`cutoff<sensor-fromto-cutoff>` | :ref:`noise<sensor-fromto-noise>` | :ref:`user<sensor-fromto-user>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
| |_| sensor |br| |_| |L| | | .. table:: |
| :ref:`clock | \* | :class: mjcf-attributes |
| <sensor-clock>` | | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
@@ -1450,7 +1477,9 @@
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`forcerange<default-position-forcerange>` | :ref:`gear<default-position-gear>` | :ref:`cranklength<default-position-cranklength>` | :ref:`user<default-position-user>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`group<default-position-group>` | :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>` | :ref:`dampratio<default-position-dampratio>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`timeconst<default-position-timeconst>` | | | | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
| |_| default |br| |_| |L| | | .. table:: |
@@ -1474,6 +1503,8 @@
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`user<default-intvelocity-user>` | :ref:`group<default-intvelocity-group>` | :ref:`kp<default-intvelocity-kp>` | :ref:`kv<default-intvelocity-kv>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`dampratio<default-intvelocity-dampratio>` | | | | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
| |_| default |br| |_| |L| | | .. table:: |
| :ref:`damper | ? | :class: mjcf-attributes |
doc/changelog.rst
+60 -22
View File
@@ -8,56 +8,94 @@ Upcoming version (not yet released)
General
^^^^^^^
1. Added :ref:`mj_geomDistance` for computing the shortest signed distance between two geoms and optionally a segment
connecting them. Relatedly, added the 3 sensors: :ref:`distance<sensor-distance>`, :ref:`normal<sensor-normal>`,
:ref:`fromto<sensor-fromto>`. See the function and sensor documentation for details. Fixes :github:issue:`51`.
2. Improvements to position actuators:
- Added :ref:`timeconst<actuator-position-timeconst>` attribute to the :ref:`position actuator<actuator-position>`.
When set to a positive value, the actuator is made stateful with :at:`filterexact` dynamics.
- Added :ref:`dampratio<actuator-position-dampratio>` to both :el:`position` and :el:`intvelocity` actuators. An
alternative to the :at:`kv` attribute, it provides a convenient way to set actuator damping using natural units.
See attribute documentation for details.
MJX
^^^
3. Add height-field collision support. Fixes :github:issue:`1491`.
4. Add a pre-compiled field ``mesh_convex`` to ``mjx.Model`` so that mesh properties can be vmapped over.
Fixes :github:issue:`1655`.
5. Fix a bug in convex mesh collisions, where erroneous edge contacts were being created even though face
separating axes were found. Fixes :github:issue:`1695`.
Bug fixes
^^^^^^^^^
6. Fixed a bug the could cause collisions to be missed when :ref:`fusestatic<compiler-fusestatic>` is enabled, as is
often the case for URDF imports. Fixes :github:issue:`1069`, :github:issue:`1577`.
7. Fixed a bug that was causing the visualization of SDF iterations to write outside the size of the vector storing
them. Fixes :github:issue:`1539`.
Version 3.1.5 (May 7, 2024)
---------------------------
General
^^^^^^^
.. youtube:: 5k0_wsIRAFc
:align: right
:width: 240px
1. Added the :ref:`replicate<replicate>` to MJCF, a :ref:`meta-element<meta-element>` which permits to repeat a subtree
with incremental translational and rotational offsets.
2. Added ``mjModel.mesh_scale``: the scaling applied to asset vertices, as specified in the
2. Enabled an internal cache in the MuJoCo compiler resulting in recompilation speedup. Currently, processed
textures, hfields, and OBJ meshes are cached. Support for Unity environments is not yet available.
3. Added ``mjModel.mesh_scale``: the scaling applied to asset vertices, as specified in the
:ref:`scale<asset-mesh-scale>` attribute.
3. Added visual properties which are ignored by the native renderer, but can be used by external renderers:
4. Added visual properties which are ignored by the native renderer, but can be used by external renderers:
- :ref:`light/bulbradius<body-light-bulbradius>` attribute and corresponding ``mjModel.light_bulbradius`` field.
- :ref:`material/metallic<asset-material-metallic>` attribute and corresponding ``mjModel.material_metallic`` field.
- :ref:`material/roughness<asset-material-roughness>` attribute and corresponding ``mjModel.material_roughness``
field.
4. The type of the ``size`` argument of :ref:`mj_stackAllocNum` and :ref:`mj_stackAllocInt` was changed from ``int``
5. The type of the ``size`` argument of :ref:`mj_stackAllocNum` and :ref:`mj_stackAllocInt` was changed from ``int``
to ``size_t``.
5. Added support for gmsh format version 2.2 surface meshes in :ref:`flexcomp<body-flexcomp-file>`.
6. Added support for gmsh format version 2.2 surface meshes in :ref:`flexcomp<body-flexcomp-file>`.
MJX
^^^
.. admonition:: Breaking API changes
:class: attention
6. Removed deprecated ``mjx.device_get_into`` and ``mjx.device_put`` functions as they lack critical new
7. Removed deprecated ``mjx.device_get_into`` and ``mjx.device_put`` functions as they lack critical new
functionality.
**Migration:** Use ``mjx.get_data_into`` instead of ``mjx.device_get_into``, and ``mjx.put_data`` instead of
``mjx.device_put``.
7. Added cylinder plane collisions.
8. Added ``efc_type`` to ``mjx.Data`` and ``dim``, ``efc_address`` to ``mjx.Contact``.
9. Added ``geom`` to ``mjx.Contact`` and marked ``geom1``, ``geom2`` deprecated.
10. Added ``ne``, ``nf``, ``nl``, ``nefc``, and ``ncon`` to ``mjx.Data`` to match ``mujoco.MjData``.
11. Given the above added fields, removed ``mjx.get_params``, ``mjx.ncon``, and ``mjx.count_constraints``.
12. Changed the way meshes are organized on device to speed up collision detection when a mesh is replicated for many
8. Added cylinder plane collisions.
9. Added ``efc_type`` to ``mjx.Data`` and ``dim``, ``efc_address`` to ``mjx.Contact``.
10. Added ``geom`` to ``mjx.Contact`` and marked ``geom1``, ``geom2`` deprecated.
11. Added ``ne``, ``nf``, ``nl``, ``nefc``, and ``ncon`` to ``mjx.Data`` to match ``mujoco.MjData``.
12. Given the above added fields, removed ``mjx.get_params``, ``mjx.ncon``, and ``mjx.count_constraints``.
13. Changed the way meshes are organized on device to speed up collision detection when a mesh is replicated for many
geoms.
13. Fixed a bug where capsules might be ignored in broadphase colliision checking.
14. Added cylinder collisions using SDFs.
15. Added support for all :ref:`condim <coContact>`: 1, 3, 4, 6.
16. Add support functions for ``id2name`` and ``name2id``, MJX versions of :ref:`mj_id2name` and :ref:`mj_name2id`.
17. Added support for :ref:`gravcomp<body-gravcomp>` and :ref:`actuatorgravcomp<body-joint-actuatorgravcomp>`.
18. Fixed a bug in ``mjx.ray`` for sometimes allowed negative distances for ray-mesh tests.
14. Fixed a bug where capsules might be ignored in broadphase colliision checking.
15. Added cylinder collisions using SDFs.
16. Added support for all :ref:`condim <coContact>`: 1, 3, 4, 6.
17. Add support functions for ``id2name`` and ``name2id``, MJX versions of :ref:`mj_id2name` and :ref:`mj_name2id`.
18. Added support for :ref:`gravcomp<body-gravcomp>` and :ref:`actuatorgravcomp<body-joint-actuatorgravcomp>`.
19. Fixed a bug in ``mjx.ray`` for sometimes allowed negative distances for ray-mesh tests.
20. Added a new `differentiable physics tutorial <https://colab.research.google.com/github/google-deepmind/mujoco/blob/main/mjx/training_apg.ipynb>`__ that demonstrates training locomotion policies with analytical gradients
automatically derived from the MJX physics step. Contribution by :github:user:`Andrew-Luo1`.
Bug fixes
^^^^^^^^^
19. Defaults of lights were not being saved, now fixed.
20. Prevent overwriting of frame names by body names when saving an XML. Bug introduced in 3.1.4.
21. Fixed bug in Python binding of :ref:`mj_saveModel`: ``buffer`` argument was documented as optional but was actually
21. Defaults of lights were not being saved, now fixed.
22. Prevent overwriting of frame names by body names when saving an XML. Bug introduced in 3.1.4.
23. Fixed bug in Python binding of :ref:`mj_saveModel`: ``buffer`` argument was documented as optional but was actually
not optional.
22. Fixed bug that prevented memory allocations larger than 2.15 GB.
24. Fixed bug that prevented memory allocations larger than 2.15 GB. Fixes :github:issue:`1606`.
Version 3.1.4 (April 10th, 2024)
@@ -193,7 +231,7 @@ MJX
Python bindings
^^^^^^^^^^^^^^^
12. Improved the implementation of the :ref:`rollout<PySample>` module. Note the changes below are breaking, dependent
12. Improved the implementation of the :ref:`rollout<PyRollout>` module. Note the changes below are breaking, dependent
code will require modification.
- Uses :ref:`mjSTATE_FULLPHYSICS<geFullPhysics>` as state spec, enabling divergence detection by inspecting time.
doc/computation/index.rst
+91 -42
View File
@@ -29,7 +29,7 @@ Robots as well as humans interact with their environment primarily through physi
importance of physics modeling in robotics, machine learning, animation, virtual reality, biomechanics and other fields,
there is need for simulation models of contact dynamics that are both physically accurate and computationally efficient.
One application of simulation models is to assess candidate estimation and control strategies before deploying them on
physical systems. Another application is to automate the design of those strategies - usually through numerical
physical systems. Another application is to automate the design of those strategies -- usually through numerical
optimization that uses simulation in an inner loop. The latter application imposes an additional constraint: the
objective function defined with respect to the contact dynamics should be amenable to numerical optimization. The
contact model underlying MuJoCo has benefits along these and other relevant dimensions. In the following sections we
@@ -50,7 +50,7 @@ for frictional contacts there are differences.
If one sees convex models as approximations to LCP, the logical question to ask is how good that approximation is.
However we do not see it that way. Instead, we see both LCP models and convex models as different approximations to
physical reality, each with its strengths and weaknesses. The immediate consequence of dropping strict complementarity
and replacing it with a cost is that complementarity can be violated - meaning that force and velocity in the contact
and replacing it with a cost is that complementarity can be violated -- meaning that force and velocity in the contact
normal direction can be simultaneously positive, and frictional forces may not be maximally dissipative. A related
phenomenon is that the only way to initiate slip is to generate some motion in the normal direction. These effects are
numerically small yet undesirable. This shortcoming however has little practical relevance, because it is premised on
@@ -117,7 +117,7 @@ Inverse dynamics and optimization
The objective of inverse dynamics is to recover the applied force and contact force given the position, velocity and
acceleration of the multi-joint system. With hard contacts this computation is impossible. Consider pushing against a
wall without moving. The contact force cannot be recovered from the kinematics, unless of course we consider the
material deformations - in which case we need a soft contact model. Inverse dynamics are trivial to compute with
material deformations -- in which case we need a soft contact model. Inverse dynamics are trivial to compute with
spring-damper models of contact, because in that case the contact force is only a function of position and velocity and
does not depend on applied force. But this is also the reason why spring-damper models are undesirable: ignoring the
applied force means that an error is introduced at each time step, and so the simulator is perpetually in
@@ -125,7 +125,7 @@ error-correction mode, in turn causing instabilities. In contrast, modern contac
well as all internal forces) into account when computing the contact force/impulse. But this complicates inversion. The
present contact model has a uniquely-defined inverse. The inverse dynamics are in fact easier to compute than the
forward dynamics, because the optimization problem becomes diagonal and decomposes into independent optimization
problems over individual contacts - which can be solved analytically.
problems over individual contacts -- which can be solved analytically.
Inverse dynamics play a key role in optimization algorithms arising in system identification, estimation and control.
They make it possible to treat the sequence of positions (or a parametric representation thereof) as the object being
@@ -269,7 +269,7 @@ activation state :math:`w_i` with its own dynamics. The control inputs for all a
the force outputs are stored in ``mjData.actuator_force``, and the activation states (if any) are stored in
``mjData.act``.
These three components of an actuator - transmission, activation dynamics, and force generation - determine how the
These three components of an actuator -- transmission, activation dynamics, and force generation -- determine how the
actuator works. The user can set them independently for maximum flexibility, or use :ref:`Actuator shortcuts
<CActShortcuts>` which instantiate common actuator types.
@@ -736,8 +736,8 @@ properties of quaternions, differentiation with respect to :math:`q` produces ve
Among other applications, equality constraints can be used to create "loop joints", i.e., joints that cannot be modeled
via the kinematic tree. Gaming engines represent all joints in this way. The same can be done in MuJoCo but is not
recommended - because it leads to both slower and less accurate simulation, effectively turning MuJoCo into a gaming
engine. The only reason to represent joints with equality constraints would be to model soft joints - which can be done
recommended -- because it leads to both slower and less accurate simulation, effectively turning MuJoCo into a gaming
engine. The only reason to represent joints with equality constraints would be to model soft joints -- which can be done
via the constraint solver but not via the kinematic tree.
There are five types of equality constraints described next. The numbers in the headings correspond to the
@@ -846,7 +846,7 @@ at the limit, and negative if the limit is violated. The constraint becomes acti
constraint force depends on distance through the solver :ref:`parameters <soParameters>` described later.
It is possible that both the lower and the upper limits for a given joint or tendon become active. In that case they are
both included in the list of scalar constraints, however this situation should be avoided - by increasing the range or
both included in the list of scalar constraints, however this situation should be avoided -- by increasing the range or
decreasing the margin. In particular, avoid using narrow ranges to approximate an equality constraint. Instead use an
explicit equality constraint, and if some slack is desired make the constraint soft by adjusting the solver parameters.
This is more efficient computationally, not only because it involves one scalar constraint instead of two, but also
@@ -900,7 +900,8 @@ model definition.
* - ``condim``
- Dimensionality of the contact force/torque in the contact frame. |br| It can be 1, 3, 4 or 6.
* - ``friction``
- Vector of friction coefficients with dimensionality ``condim-1``.
- Vector of friction coefficients with dimensionality ``condim-1``. See below for semantics of the specific
coefficients.
* - ``margin``
- The distance margin used to determine if the contact should be included in the global contact array
``mjData.contact``.
@@ -921,19 +922,23 @@ as defined later. The ``condim`` parameter determines the contact type, and has
similar to a joint or tendon limit, but is applied to the distance between two geoms.
``condim = 3`` : 3 for elliptic, 4 for pyramidal
This is a regular frictional contact, which can generate normal force as well as tangential friction force opposing
slip.
This is a regular frictional contact, which can generate normal force as well as a tangential friction force opposing
slip. An interpertation of this number is the slope of a surface above which a flat object will begin to slip
under gravity.
``condim = 4`` : 4 for elliptic, 6 for pyramidal
In addition to normal and tangential force, this contact can generate torsional friction torque opposing rotation
around the contact normal. This is useful for modeling soft fingers, and can substantially improve the stability of
simulated grasping. Keep in mind that the torsional (as well as rolling) friction coefficients have different units
from the tangential friction coefficients.
around the contact normal, corresponding to a torque generated by a contacting surface patch. This is useful for
modeling soft fingers, and can substantially improve the stability of simulated grasping. Torsional friction
coefficients have **units of length** which can be interperted as the diameter of the surface contact patch.
``condim = 6`` : 6 for elliptic, 10 for pyramidal
This contact can oppose motion in all relative degrees of freedom between the two geoms. In particular it adds
rolling friction, which can be used for example to stop a ball from rolling indefinitely on a plane. It can also be
used to model rolling friction between tires and a road, and in general to stabilize contacts.
rolling friction, which can be used for example to stop a ball from rolling indefinitely on a plane. Rolling friction
in the real world results from energy dissipated by local deformations near the contact point. It can be
used to model rolling friction between tires and a road, and in general to stabilize contacts. Rolling friction
coefficients also have **units of length** which can be interperted as the depth of the local deformation within
which energy is dissipated.
Note that condim cannot be 2 or 5. This is because the two tangential directions and the two rolling directions are
treated as pairs. The friction coefficients within a pair can be different though, which can be used to model skating
@@ -1109,7 +1114,7 @@ and the dual of the dual of a cone is the cone itself. The pyramidal friction co
self-dual, but the elliptic one is not.
The Huber "norm" is based on the Huber function from robust statistics: it is a quadratic around zero, and transitions
smoothly to a linear function when the absolute value of the argument crosses a threshold - in this case given by the
smoothly to a linear function when the absolute value of the argument crosses a threshold -- in this case given by the
friction loss parameters. Setting :math:`\eta = \infty` recovers the quadratic norm; we use this convention for all
constraint forces that are not due to friction loss. This is another instance of reverse engineering: we want to obtain
interval constraints on the friction loss forces, which is non-trivial because Lagrange duality usually yields
@@ -1190,7 +1195,7 @@ the constraints. We are then left with an unconstrained optimization problem ove
more efficient algorithms.
The reduction is based on the fact that minimization over :math:`y` in :eq:`eq:primal` comes down to finding the nearest
point on the constraint set - which is either a plane or a cone, and can be done analytically. Substituting the result,
point on the constraint set -- which is either a plane or a cone, and can be done analytically. Substituting the result,
we obtain the unconstrained problem
.. math::
@@ -1296,7 +1301,7 @@ because the forward dynamics need all the quantities that enter into the inverse
the analytical formula. This makes it possible to implement an automatic correctness check in MuJoCo. When the flag
``fwdinv`` in ``mjModel.opt.enableflags`` is on, the forward and inverse dynamics are automatically compared at the end
of each time step, and the difference is recorded in ``mjData.solver_fwdinv``. Discrepancies indicate that the forward
solver - which is numerical and is usually terminated early - is not converging well. Of course the inverse dynamics are
solver---which is numerical and is usually terminated early---is not converging well. Of course the inverse dynamics are
also useful on their own, without computing the forward dynamics first.
.. _soAlgorithms:
@@ -1348,7 +1353,7 @@ representations of the constraint Jacobian and related matrices.
can be up to 5-dimensional given our contact model. Now we optimize the quadratic cost within this ellipsoid. This is
an instance of quadratically constrained quadratic programming (QCQP). Since there is only one scalar constraint
(however nonlinear it may be), the dual is a scalar optimization problem over the unknown Lagrange multiplier. We
solve this problem with Newton's method applied until convergence - which in practice takes less than 10 iterations,
solve this problem with Newton's method applied until convergence -- which in practice takes less than 10 iterations,
and involves small matrices. Overall this algorithm has similar behavior to PGS for pyramidal cones, but it can
handle elliptic cones without approximating them. It does more work per contact, however the contact dimensionality
is smaller, and these two factors roughly balance each other.
@@ -1570,11 +1575,23 @@ Top level
pipeline does not support the Runge Kutta integrator.
- :ref:`mj_fwdPosition` invokes stages **2-11**, the position-dependent part of the pipeline.
.. _piStages:
Stages
^^^^^^
Below we describe the pipeline stages and API functions corresponding to each stage. All functions write their outputs
into attributes of :ref:`mjData`. It is informative to compare the list below with the :ref:`mjData` struct definition,
wherein comments above blocks of attributes specify the function that computes them. Note that each stage depends on
the values computed in some or all of the previous stages.
1. Check the positions and velocities for invalid or unacceptably large real values indicating divergence. If divergence
is detected, the state is automatically reset and the corresponding warning is raised:
:ref:`mj_checkPos`, :ref:`mj_checkVel`
Position
''''''''
The stages below compute quantities that depend on the generalized positions ``mjData.qpos``.
2. Compute the forward kinematics. This yields the global positions and orientations of all bodies, geoms, sites,
cameras and lights. It also normalizes all quaternions: :ref:`mj_kinematics`, :ref:`mj_camLight`
3. Compute the body inertias and joint axes, in global frames centered at the centers of mass of the corresponding
@@ -1591,6 +1608,12 @@ Stages
11. Compute the matrices and vectors needed by the constraint solvers: :ref:`mj_projectConstraint`
12. Compute sensor data that only depends on position, and the potential energy if enabled: :ref:`mj_sensorPos`,
:ref:`mj_energyPos`
Velocity
''''''''
The stages below compute quantities that depend on the generalized velocity ``mjData.qvel``. Due to the sequential
dependence structure of the pipeline, the actual dependence is on both ``qpos`` and ``qvel``.
13. Compute the tendon, flex edge and actuator velocities: :ref:`mj_fwdVelocity`
14. Compute the body velocities and rates of change of the joint axes, again in the global coordinate frames centered at
the subtree centers of mass: :ref:`mj_comVel`
@@ -1599,6 +1622,12 @@ Stages
(if required by sensors, call :ref:`mj_subtreeVel`): :ref:`mj_sensorVel`
17. Compute the reference constraint acceleration: :ref:`mj_referenceConstraint`
18. Compute the vector of Coriolis, centrifugal and gravitational forces: :ref:`mj_rne`
Force/acceleration
''''''''''''''''''
The stages below compute quantities that depend on :ref:`user inputs<geInput>`. Due to the sequential nature
of the pipeline, the actual dependence is on the entire :ref:`integration state<geIntegrationState>`.
19. Compute the actuator forces and activation dynamics if defined: :ref:`mj_fwdActuation`
20. Compute the joint acceleration resulting from all forces except for the (still unknown) constraint forces:
:ref:`mj_fwdAcceleration`
@@ -1615,12 +1644,53 @@ Stages
repeats the above sequence three more times, except for the optional computations which are performed only once:
one of :ref:`mj_Euler`, :ref:`mj_RungeKutta`, :ref:`mj_implicit`
.. _piConsistency:
Consistency in ``mjData``
~~~~~~~~~~~~~~~~~~~~~~~~~
The MuJoCo computation pipeline is entirely imperative, nothing happens automatically. This leads to behavior which
can seem unexpected to users more familiar with other paradigms. Here are two examples of intended behaviors which can
sometimes be surprising:
- After setting :ref:`the state<geState>`, state-derived quantities do not automatically correspond to the new state.
The required stage or stages must be manually invoked. For example after setting the generalized positions
``mjData.qpos``, Cartesian positions and orientations will not be consistent with ``qpos`` without first calling
:ref:`mj_kinematics`.
- After an :ref:`mj_step`, which terminates immediately after updating the state, quantities in ``mjData`` correspond
to the *previous* state (or more precisely, the *transition* between the previous and current state).
In particular, all position-dependent sensor values and position-dependent computations like kinematic
:ref:`Jacobians<mj_jac>`, will be with respect to the *previous positions*.
.. _piReproducibility:
Reproducibility
~~~~~~~~~~~~~~~
MuJoCo's simulation pipeline is entirely deterministic and reproducible -- if a :ref:`state<geState>` in a trajectory is
saved and reloaded and :ref:`mj_step` called again, the resulting next state will be identical. However, there are some
important caveats:
- Save all the required :ref:`integration state<geIntegrationState>` components. In particular :ref:`warmstart
accelerations<geWarmstart>` have only a very small effect on the next state, but should be saved if bit-wise equality
is required.
- Any numerical difference between states, no matter how small, will become significant upon integration, especially for
systems with contact. Contact events have high `Lyapunov exponents
<https://en.wikipedia.org/wiki/Lyapunov_exponent>`__; this is a property of any rigid-body simulator (and indeed of
`real-world physics <https://en.wikipedia.org/wiki/Roulette>`__) and is not MuJoCo-specific.
- Exact reproducibility is only guaranteed within a **single version**, on the **same architecture**. Small numerical
differences are quite common between versioned releases, for example due to code optimizations. This means that when
saving an initial state and an open-loop control sequence, the resulting rolled-out trajectory will be identical
within the same version, but will likely be different between MuJoCo versions or different operating systems.
.. _piInverse:
Inverse dynamics
~~~~~~~~~~~~~~~~
The top-level function :ref:`mj_inverse` invokes the following sequence of computations.
The top-level function :ref:`mj_inverse` invokes the following sequence of computations. The notes above regarding
:ref:`consistency<piConsistency>` and :ref:`reproducibility<piReproducibility>` apply here as well.
#. Compute the forward kinematics.
#. Compute the body inertias and joint axes.
@@ -1647,27 +1717,6 @@ The top-level function :ref:`mj_inverse` invokes the following sequence of compu
equals the sum of external and actuation forces.
.. _piReproducibility:
Reproducibility
~~~~~~~~~~~~~~~
MuJoCo's simulation pipeline is entirely deterministic and reproducible -- if a :ref:`state<geState>` in a trajectory is
saved and reloaded and :ref:`mj_step` called again, the resulting next state will be identical. However, there are some
important caveats:
- Save all the required :ref:`integration state<geIntegrationState>` components. In particular :ref:`warmstart
accelerations<geWarmstart>` have only a very small effect on the next state, but should be saved if bit-wise equality
is required.
- Any numerical difference between states, no matter how small, will become significant upon integration, especially for
systems with contact. Contact events have high `Lyapunov exponents
<https://en.wikipedia.org/wiki/Lyapunov_exponent>`__; this is a property of any rigid-body simulator (and indeed of
`real-world physics <https://en.wikipedia.org/wiki/Roulette>`__) and is not MuJoCo-specific.
- Exact reproducibility is only guaranteed within a **single version**, on the **same architecture**. Small numerical
differences are quite common between versioned releases, for example due to code optimizations. This means that when
saving an initial state and an open-loop control sequence, the resulting rolled-out trajectory will be identical
within the same version, but will likely be different between MuJoCo versions or different operating systems.
.. _derivatives:
Derivatives
doc/conf.py
+1 -1
View File
@@ -111,7 +111,7 @@ html_logo = 'images/banner.svg'
SHARED_CSS_VARIABLES = {
'admonition-font-size': '1rem',
'admonition-title-font-size': '1rem',
'sidebar-item-font-size': '115%',
'sidebar-item-font-size': '130%',
}
# font-stack--monospace used in code blocks, Inconsolata fits in 100 chars.
doc/css/theme_overrides.css
+6
View File
@@ -260,6 +260,12 @@ dt .at {
margin-left: 0.5em;
}
/* Reduce top and bottom margins around displayed KaTeX equations */
.katex-display {
margin-top: 0.3em;
margin-bottom: 0.3em;
}
details summary {
font-weight: 600;
}
doc/includes/references.h
+11 -4
View File
@@ -94,7 +94,7 @@ struct mjContact_ { // result of collision detection functions
// internal storage used by solver
mjtNum mu; // friction of regularized cone, set by mj_makeConstraint
mjtNum H[36]; // cone Hessian, set by mj_updateConstraint
mjtNum H[36]; // cone Hessian, set by mj_constraintUpdate
// contact descriptors set by mj_collideXXX
int dim; // contact space dimensionality: 1, 3, 4 or 6
@@ -643,6 +643,11 @@ typedef enum mjtSensor_ { // type of sensor
mjSENS_SUBTREELINVEL, // 3D linear velocity of subtree
mjSENS_SUBTREEANGMOM, // 3D angular momentum of subtree
// sensors for geometric distance; attached to geoms or bodies
mjSENS_GEOMDIST, // signed distance between two geoms
mjSENS_GEOMNORMAL, // normal direction between two geoms
mjSENS_GEOMFROMTO, // segment between two geoms
// global sensors
mjSENS_CLOCK, // simulation time
@@ -2542,6 +2547,8 @@ void mj_objectVelocity(const mjModel* m, const mjData* d,
int objtype, int objid, mjtNum res[6], int flg_local);
void mj_objectAcceleration(const mjModel* m, const mjData* d,
int objtype, int objid, mjtNum res[6], int flg_local);
mjtNum mj_geomDistance(const mjModel* m, const mjData* d, int geom1, int geom2,
mjtNum distmax, mjtNum fromto[6]);
void mj_contactForce(const mjModel* m, const mjData* d, int id, mjtNum result[6]);
void mj_differentiatePos(const mjModel* m, mjtNum* qvel, mjtNum dt,
const mjtNum* qpos1, const mjtNum* qpos2);
@@ -2632,9 +2639,9 @@ void mjv_updateSceneState(const mjModel* m, mjData* d, const mjvOption* opt,
mjvSceneState* scnstate);
void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* opt,
const mjvPerturb* pert, int catmask, mjvScene* scn);
void mjv_makeLights(const mjModel* m, mjData* d, mjvScene* scn);
void mjv_updateCamera(const mjModel* m, mjData* d, mjvCamera* cam, mjvScene* scn);
void mjv_updateSkin(const mjModel* m, mjData* d, mjvScene* scn);
void mjv_makeLights(const mjModel* m, const mjData* d, mjvScene* scn);
void mjv_updateCamera(const mjModel* m, const mjData* d, mjvCamera* cam, mjvScene* scn);
void mjv_updateSkin(const mjModel* m, const mjData* d, mjvScene* scn);
void mjr_defaultContext(mjrContext* con);
void mjr_makeContext(const mjModel* m, mjrContext* con, int fontscale);
void mjr_changeFont(int fontscale, mjrContext* con);
doc/mjx.rst
+5 -5
View File
@@ -75,7 +75,7 @@ an ``mjx.Data``:
model = mujoco.MjModel.from_xml_string("...")
data = mujoco.MjData(model)
mjx_model = mjx.put_model(model)
mjx_data = mjx.put_data(data)
mjx_data = mjx.put_data(model, data)
These MJX variants mirror their MuJoCo counterparts but have a few key differences:
@@ -100,7 +100,7 @@ Neither ``mjx.Model`` nor ``mjx.Data`` are meant to be constructed manually. An
.. code-block:: python
model = mujoco.MjModel.from_xml_string("...")
mjx_model = mjx.device_put(model)
mjx_model = mjx.put_model(model)
mjx_data = mjx.make_data(model)
Using ``mjx.make_data`` may be preferable when constructing batched ``mjx.Data`` structures inside of a ``vmap``.
@@ -151,7 +151,7 @@ Minimal example
"""
model = mujoco.MjModel.from_xml_string(XML)
mjx_model = mjx.device_put(model)
mjx_model = mjx.put_model(model)
@jax.vmap
def batched_step(vel):
@@ -196,7 +196,7 @@ The following features are **fully supported** in MJX:
* - :ref:`Actuator Bias <mjtBias>`
- ``NONE``, ``AFFINE``
* - :ref:`Geom <mjtGeom>`
- ``PLANE``, ``SPHERE``, ``CAPSULE``, ``BOX``, ``MESH``
- ``PLANE``, ``HFIELD``, ``SPHERE``, ``CAPSULE``, ``BOX``, ``MESH`` are fully implemented. ``ELLIPSOID`` and ``CYLINDER`` are implemented but only collide with other primitives.
* - :ref:`Constraint <mjtConstraint>`
- ``EQUALITY``, ``LIMIT_JOINT``, ``CONTACT_FRICTIONLESS``, ``CONTACT_PYRAMIDAL``
* - :ref:`Equality <mjtEq>`
@@ -223,7 +223,7 @@ The following features are **in development** and coming soon:
* - Category
- Feature
* - :ref:`Geom <mjtGeom>`
- ``SDF``, ``HFIELD``, ``ELLIPSOID``, ``CYLINDER``
- ``SDF``. Collisions between (``SPHERE``, ``BOX``, ``MESH``, ``HFIELD``) and ``CYLINDER``. Collisions between (``BOX``, ``MESH``, ``HFIELD``) and ``ELLIPSOID``.
* - :ref:`Constraint <mjtConstraint>`
- :ref:`Frictionloss <coFriction>`, ``CONTACT_ELLIPTIC``, ``FRICTION_DOF``
* - :ref:`Integrator <mjtIntegrator>`
doc/modeling.rst
+86 -26
View File
@@ -31,7 +31,7 @@ Loading models
As explained in :ref:`Model instances <Instance>` in the Overview chapter, MuJoCo models can be loaded from plain-text
XML files in the MJCF or URDF formats, and then compiled into a low-level mjModel. Alternatively a previously saved
mjModel can be loaded directly from a binary MJB file - whose format is not documented but is essentially a copy of the
mjModel can be loaded directly from a binary MJB file -- whose format is not documented but is essentially a copy of the
mjModel memory buffer. MJCF and URDF files are loaded with :ref:`mj_loadXML` while MJB files are loaded with
:ref:`mj_loadModel`.
@@ -45,8 +45,8 @@ XML file has a unique top-level element. This element must be :el:`mujoco` for M
Compiling models
~~~~~~~~~~~~~~~~
Once a high-level mjCModel is created - by loading an MJCF file or a URDF file, or programmatically when such
functionality becomes available - it is compiled into mjModel. Even though loading and compilation are presently
Once a high-level mjCModel is created---by loading an MJCF file or a URDF file, or programmatically when such
functionality becomes available---it is compiled into mjModel. Even though loading and compilation are presently
combined in one step, compilation is independent of loading, meaning that the compiler works in the same way
regardless of how mjCModel was created. Both the parser and the compiler perform extensive error checking, and abort
when the first error is encountered. The resulting error messages contain the row and column number in the XML file,
@@ -56,7 +56,7 @@ simulation step of the compiled model is performed and any runtime errors are in
(temporarily) setting :ref:`mju_user_error` to point to a function that throws C++
exceptions; the user can implement similar error-interception functionality at runtime if desired.
The entire process of parsing and compilation is very fast - less than a second if the model does not contain large
The entire process of parsing and compilation is very fast -- less than a second if the model does not contain large
meshes or actuator lengthranges that need to be computed via simulation. This makes it possible to design models
interactively, by re-loading often and visualizing the changes. Note that the :ref:`simulate.cc <saSimulate>` code
sample has a keyboard shortcut for re-loading the current model (Ctrl+L).
@@ -166,7 +166,7 @@ which overrides the default settings. The cylinder specifies defaults class "mai
"sub", even though the latter was specified in the childclass attribute of the body containing the geom.
Now we describe the general rules. MuJoCo supports unlimited number of defaults classes, created by possibly nested
:ref:`default <default>` elements in the XML. Each class has a unique name - which is a required
:ref:`default <default>` elements in the XML. Each class has a unique name -- which is a required
attribute except for the top-level class whose name is "main" if left undefined. Each class also has a complete
collection of dummy model elements, with their attributes set as follows. When a defaults class is defined within
another defaults class, the child automatically inherits all attribute values from the parent. It can then override
@@ -248,7 +248,7 @@ specified by the user, the frame is not rotated.
:at:`zaxis`: :at-val:`real(3), optional`
The Z axis of the frame. The compiler finds the minimal rotation that maps the vector :math:`(0, 0, 1)` into the
vector specified here. This determines the X and Y axes of the frame implicitly. This is useful for geoms with
rotational symmetry around the Z axis, as well as lights - which are oriented along the Z axis of their frame.
rotational symmetry around the Z axis, as well as lights -- which are oriented along the Z axis of their frame.
.. _CSolver:
@@ -448,33 +448,36 @@ there is nothing to do, but what if their parameters are different? In that case
:at:`solmix` and :at:`priority` to decide how to combine them. The combination rules for each contact parameter
are as follows:
condim
**condim**
If one of the two geoms has higher priority, its condim is used. If both geoms have the same priority, the maximum of
the two condims is used. In this way a frictionless geom and a frictional geom form a frictional contact, unless the
frictionless geom has higher priority. The latter is desirable in particle systems for example, where we may not want
the particles to stick to any objects.
friction
**friction**
Recall that contacts can have up to 5 friction coefficients: two tangential, one torsional, two rolling. Each contact
in mjData.contact actually has all 5 of them, even if condim is less than 6 and not all coefficients are used. In
contrast, geoms have only 3 friction coefficients: tangential (same for both axes), torsional, rolling (same for both
axes). Each of these 3D vectors of friction coefficients is expanded into a 5D vector of friction coefficients by
replicating the tangetial and rolling components. The contact friction coefficients are then computed according to
the following rule: if one of the two geoms has higher priority, its friction coefficients are used. Otherwise the
element-wise maximum of each friction coefficient over the two geoms is used. The rationale is similar to taking the
maximum over condim: we want the more frictional geom to win.
replicating the tangetial and rolling components. See the :ref:`Contact<coContact>` section in the Computation
chapter for an intuitive description of the semantics of tangential, torsional and rolling coefficients.
The contact friction coefficients are then computed according to the following rule: if one of the two geoms has
higher priority, its friction coefficients are used. Otherwise the **element-wise maximum** of each friction
coefficient over the two geoms is used.
The reason for having 5 coefficients per contact and only 3 per geom is as follows. For a contact pair, we want to
allow the most flexible model our solver can handle. As mentioned earlier, anisotropic friction can be exploited to
model effects such as skating. This however requires knowing how the two axes of the contact tangent plane are
oriented. For a predefined contact pair we know the two geom types in advance, and the corresponding collision
function always generates contact frames oriented in the same way - which we do not describe here but it can be seen
function always generates contact frames oriented in the same way -- which we do not describe here but it can be seen
in the visualizer. For individual geoms however, we do not know which other geoms they might collide with and what
their geom types might be, so there is no way to know how the contact tangent plane will be oriented when specifying
an individual geom. This is why MuJoCo does not allow anisotropic friction in the individual geom specifications, but
only in the explicit contact pair specifications.
margin, gap
**margin**, **gap**
The maximum of the two geom margins (or gaps respectively) is used. The geom priority is ignored here, because the
margin and gap are distance properties and a one-sided specification makes little sense.
solref, solimp
**solref**, **solimp**
If one of the two geoms has higher priority, its solref and solimp parameters are used. If both geoms have the same
priority, the weighted average is used. The weights are proportional to the solmix attributes, i.e., weight1 =
solmix1 / (solmix1 + solmix2) and similarly for weight2. There is one important exception to this weighted averaging
@@ -493,7 +496,7 @@ facilitate this process, we provide a mechanism to override some of the solver p
the actual model. Once the override is disabled, the simulation reverts to the parameters specified in the model. This
mechanism can also be used to implement continuation methods in the context of numerical optimization (such as optimal
control or state estimation). This is done by allowing contacts to act from a distance in the early phases of
optimization - so as to help the optimizer find a gradient and get close to a good solution - and reducing this effect
optimization---so as to help the optimizer find a gradient and get close to a good solution---and reducing this effect
later to make the final solution physically realistic.
The relevant settings here are the :at:`override` attribute of :ref:`flag <option-flag>` which
@@ -556,12 +559,12 @@ general guidelines and observations:
threshold is better defined in terms of number of active constraints, which is model and behavior dependent.
- The choice between pyramidal and elliptic friction cones is a modeling choice rather than an algorithmic choice,
i.e., it leads to a different optimization problem solved with the same algorithms. Elliptic cones correspond more
closely to physical reality. However pyramidal cones can improve the performance of the algorithms - but not
closely to physical reality. However pyramidal cones can improve the performance of the algorithms -- but not
necessarily. While the default is pyramidal, we recommend trying the elliptic cones. When contact slip is a problem,
the best way to suppress it is to use elliptic cones, large impratio, and the Newton algorithm with very small
tolerance. If that is not sufficient, enable the Noslip solver.
- The Newton algorithm is the best choice for most models. It has quadratic convergence near the global minimum and
gets there in surprisingly few iterations - usually around 5, and rarely more than 20. It should be used with
gets there in surprisingly few iterations -- usually around 5, and rarely more than 20. It should be used with
aggressive tolerance values, say 1e-10, because it is capable of achieving high accuracy without added delay (due to
quadratic convergence at the end). The only situation where we have seen it slow down are large models with elliptic
cones and many slipping contacts. In that regime the Hessian factorization needs a lot of updates. It may also slow
@@ -1314,7 +1317,7 @@ probe away (which is possible because the probe is a mocap body which can move i
we can see the indentation made by the probe, and the resulting deformation in the rest of the body. By changing the
solref and solimp attributes of the equality constraints that hold the soft object together, one can adjust the behavior
of the system making it softer or harder, damped or springy, etc. Note that box, cylinder and ellipsoid objects do not
involve long kinematic chains, and can be simulated at large timesteps - similar to particle and grid, and unlike rope
involve long kinematic chains, and can be simulated at large timesteps -- similar to particle and grid, and unlike rope
and cloth.
.. _CDeformable:
@@ -1403,7 +1406,7 @@ self-collisions; see XML reference.
In case of 3D flexes made of tetrahedra, it may be useful to examine how the flex is "triangulated" internally. We have
a special visualization mode that peels off the outer layers. Below is an example with the Stanford Bunny. Note how it
has smaller tetrahedra on the outside and larger ones on the inside. This mesh design makes sense, because we want the
collision surface to be accurate, but on the inside we just need soft material properties - which require less spatial
collision surface to be accurate, but on the inside we just need soft material properties -- which require less spatial
resolution. In order to convert a surface mesh to a tetrahedral mesh, we recommend open tools like the
`fTetWild library <https://github.com/wildmeshing/fTetWild>`__.
@@ -1436,7 +1439,7 @@ The flexibility of repeated MCJF sections comes at a price: global settings that
the :at:`angle` attribute of :ref:`compiler <compiler>` for example, can be defined multiple times.
MuJoCo allows this, and uses the last definition encountered in the composite model, after all include elements have
been processed. So if model A is defined in degrees and model B is defined in radians, and A is included in B after
the :el:`compiler` element in B, the entire composite model will be treated as if it was defined in degrees - leading
the :el:`compiler` element in B, the entire composite model will be treated as if it was defined in degrees -- leading
to undesirable consequences in this case. The user has to make sure that models included in each other are compatible
in this sense; local vs. global coordinates is another compatibility requirement.
@@ -1469,7 +1472,7 @@ unless there is a specific reason to define it. There can be several such reason
is useful for custom computations involving a model element that is identified by its name in the XML (as opposed to
relying on a fixed index which can change when the model is edited).
- The model file could in principle become more readable by naming certain elements. Keep in mind however that XML
itself has a commenting mechanism, and that mechanism is more suitable for achieving readability - especially since
itself has a commenting mechanism, and that mechanism is more suitable for achieving readability -- especially since
most text editors provide syntax highlighting which detects XML comments.
.. _CURDF:
@@ -1486,7 +1489,9 @@ sub-elements :ref:`compiler <compiler>`, :ref:`option <option>`,
are modified so as to accomodate the URDF modeling convention. The :ref:`compiler <compiler>` extension
in particular has proven very useful, and indeed several of its attributes were introduced because a number of
existing URDF models have non-physical dynamics parameters which MuJoCo's built-in compiler will reject if left
unmodified. This extension is also needed to specify mesh directories.
unmodified. This extension is also needed to specify mesh directories. Also note that the compiler attributes
:ref:`strippath<compiler-strippath>`, :ref:`angle<compiler-angle>`, :ref:`fusestatic<compiler-fusestatic>` and
:ref:`discardvisual<compiler-discardvisual>` have different default values for URDF and MJCF.
Note that the while MJCF models are checked against a custom XML schema by the parser, URDF models are not. Even the
MuJoCo-specific elements embedded in the URDF file are not checked. As a result, mis-typed attribute names are
@@ -1620,8 +1625,8 @@ is not always obvious, so it may be useful to have it spelled out.
Performance tuning
~~~~~~~~~~~~~~~~~~
Here is a list of steps one can take in order to maximize simulation throughput. All of the recommendations below
involve some tweaking. It is recommended that these be carried out in interactive fashion while looking at the
Below is a list of steps one can take in order to maximize simulation throughput. All of the recommendations
involve parameter tweaking. It is recommended that these be carried out in interactive fashion while looking at the
:ref:`simulate<saSimulate>` utility's built-in profiler. A detailed and sometimes more useful profile is also reported
by the :ref:`testspeed<saTestspeed>` utility. When embarking on the more elaborate steps below, target the most
expensive pipeline component reported by the profiler. Note that some of these are subtly different for MJX, see
@@ -1663,6 +1668,61 @@ dedicated section :ref:`therein<MjxPerformance>`.
- If replacing collision meshes with primitives is not feasible, decimate the meshes as much as possible. Open source
tools like trimesh, Blender, MeshLab and CoACD are very useful in this regard.
.. _CSlippage:
Preventing slip
~~~~~~~~~~~~~~~
Below is a list of steps one can take in order to diagnose and solve contact slippage, which is especially problematic
in manipulation tasks. In order to diagnose slippage, it is recommended to use the :ref:`simulate<saSimulate>` utility's
built in visualization options to inspect contacts and contact forces. It is often helpful to tweak the visual size of
contacts and forces (using the global :ref:`meansize<statistic-meansize>` or the specific
:ref:`contactwidth<visual-scale-contactwidth>`, :ref:`contactheight<visual-scale-contactheight>` and
:ref:`forcewidth<visual-scale-forcewidth>` attributes) and the :ref:`force scaling<visual-map-force>` attribute, to
better visualize and understand the contact configuration and resulting forces.
**Slip-preventing contact forces are outside the friction cone**
This implies that the physics cannot prevent slip, even in principle. This occurs when:
a. *The normal force is too small.* Ensure that the maximum force that can be applied by the gripper mutiplied by
the sliding friction coefficient is significantly greater than the weight of the object.
b. *The sliding friction coefficient is too low.* Increase the sliding :ref:`friction<body-geom-friction>`
coefficient.
c. *Torsional friction is insufficient to apply the required torques.* Increase :ref:`condim<body-geom-condim>` to
4 or 6 and choose appropriate friction coefficients.
**condim 4** enables torsional friction, preventing rotation around the normal.
**condim 6** also enables rolling friction, preventing rotation around the tangential directions.
See the :ref:`Contact<coContact>` section for details and the specifc semantics of these coefficients.
**The geometry does not support the required forces or torques**
This is a common real-world problem, solved by improved design of grippers and handles.
a. Improve the geometry of the contacting geoms in order to add more contact points, possibly with non-flat
geometry (e.g., bumps), so slippage is prevented by the normal force and not only frictional components.
b. If contacts are between flat surfaces, try enabling the :ref:`multiccd<option-flag-multiccd>` flag, which allows
the detector to find more contacts than the single contact returned by the convex-convex collider.
**High-frequency vibration**
High-frequency, low-amplitude vibrations are also a real-world problem in many industrial settings, but unlike in
simulation, in the real world they are audible. Such vibration is often caused by controllers with very
high gains and sometimes by stick-slip feedback from contacts or joints, resonating with the eigen-modes of the
mechanism. The easist way to diagnose such vibration is to visualize contact forces in
:ref:`simulate<saSimulate>`. The solution is usually to reduce the :ref:`timestep<option-timestep>` and/or add
some :ref:`armature<body-joint-armature>` to the relevant joints. Another reason for vibration is feedback from
explicit damping. Use the implicit or implicitfast integrators, as documented in the
:ref:`Numerical Integration<geIntegration>` section.
**Slow slippage**
Unlike the above problems which lead to fast slippage, slow, gradual slippage is a property of MuJoCo's contact
model by design, since without it the inverse dynamics are not defined. This is discussed in detail in the
:ref:`softness and slip<Soft>` clarification. This type of slippage can be addressed in two ways.
a. Increase the :ref:`impration<option-impratio>` parameter. This will reduce (but not entirely prevent) slow
slippage. Note that high impratio values work well only with :ref:`elliptic cones<option-cone>`.
b. Enable the noslip solver by increasing :ref:`noslip_iterations<option-noslip_iterations>` to a positive integer.
A small number (1, 2 or 3) is usually sufficient. The noslip post-processing solver will entirely prevent slip,
at the cost of making inverse dynamics ill-defined and additional computational cost.
.. _CBacklash:
Backlash
@@ -1710,7 +1770,7 @@ destabilize the system because of integration errors. This was already discussed
The standard approach to reducing integration errors is to reduce the timestep or use the Runge-Kutta integrator, both
of which are effective but slow down the simulation. An alternative approach is to put all damping in the joints and
use the Euler integrator. In that case damping forces are integrated implicitly - meaning that the inertia matrix is
use the Euler integrator. In that case damping forces are integrated implicitly -- meaning that the inertia matrix is
adjusted and re-factorized internally as part of the velocity update, in a way transparent to the user. Implicit
integration is much more stable than explicit integration, allowing substantially larger time steps. Note that the
Runge-Kutta integrator is explicit, and so is Euler except for the way it treats damping forces. Ideally we would have
doc/models.rst
+6
View File
@@ -34,6 +34,8 @@ Humanoids
* - Model
- Preview
* - `Unitree G1 <https://github.com/google-deepmind/mujoco_menagerie/tree/main/unitree_g1>`_
- .. image:: https://raw.githubusercontent.com/google-deepmind/mujoco_menagerie/main/unitree_g1/g1.png
* - `Unitree H1 <https://github.com/google-deepmind/mujoco_menagerie/tree/main/unitree_h1>`_
- .. image:: https://raw.githubusercontent.com/google-deepmind/mujoco_menagerie/main/unitree_h1/h1.png
* - `Robotis OP3 <https://github.com/google-deepmind/mujoco_menagerie/tree/main/robotis_op3>`_
@@ -74,6 +76,8 @@ Quadrupeds
- .. image:: https://raw.githubusercontent.com/google-deepmind/mujoco_menagerie/main/google_barkour_v0/barkour_v0.png
* - `Google Barkour vB <https://github.com/google-deepmind/mujoco_menagerie/tree/main/google_barkour_vb>`_
- .. image:: https://raw.githubusercontent.com/google-deepmind/mujoco_menagerie/main/google_barkour_vb/barkour_vb.png
* - `Boston Dynamics Spot <https://github.com/google-deepmind/mujoco_menagerie/tree/main/boston_dynamics_spot>`_
- .. image:: https://raw.githubusercontent.com/google-deepmind/mujoco_menagerie/main/boston_dynamics_spot/spot.png
Grippers & Hands
^^^^^^^^^^^^^^^^
@@ -100,6 +104,8 @@ Arms
- Preview
* - `Franka Emika Panda <https://github.com/google-deepmind/mujoco_menagerie/tree/main/franka_emika_panda>`_
- .. image:: https://raw.githubusercontent.com/google-deepmind/mujoco_menagerie/main/franka_emika_panda/panda.png
* - `Franka FR3 <https://github.com/google-deepmind/mujoco_menagerie/tree/main/franka_fr3>`_
- .. image:: https://raw.githubusercontent.com/google-deepmind/mujoco_menagerie/main/franka_fr3/fr3.png
* - `Universal Robots UR5e <https://github.com/google-deepmind/mujoco_menagerie/tree/main/universal_robots_ur5e>`_
- .. image:: https://raw.githubusercontent.com/google-deepmind/mujoco_menagerie/main/universal_robots_ur5e/ur5e.png
* - `Universal Robots UR10e <https://github.com/google-deepmind/mujoco_menagerie/tree/main/universal_robots_ur10e>`_
doc/overview.rst
+5 -4
View File
@@ -56,7 +56,7 @@ Soft, convex and analytically-invertible contact dynamics
equality constraints.
Tendon geometry
MuJoCo can model the 3D geometry of tendons - which are minimum-path-length strings obeying wrapping and via-point
MuJoCo can model the 3D geometry of tendons -- which are minimum-path-length strings obeying wrapping and via-point
constraints. The mechanism is similar to the one in OpenSim but implements a more restricted, closed-form set of
wrapping options to speed up computation. It also offers robotics-specific structures such as pulleys and coupled
degrees of freedom. Tendons can be used for actuation as well as to impose inequality or equality constraints on the
@@ -635,7 +635,7 @@ store results from custom computations there; recall that everything that change
Custom text
^^^^^^^^^^^
Custom text fields can be saved in the model. They can be used in custom computations - either to specify keyword
Custom text fields can be saved in the model. They can be used in custom computations -- either to specify keyword
commands, or to provide some other textual information. Do not use them for comments though; there is no benefit to
saving comments in a compiled model. XML has its own commenting mechanism (ignored by MuJoCo's parser and compiler)
which is more suitable.
@@ -788,7 +788,8 @@ For situations where it is desirable to suppress slip completely, there is a sec
the main solver. It updates the contact forces in friction dimensions by disregarding constraint softness. When this
option is used however, MuJoCo is no longer solving the convex optimization problem it was designed to solve, and the
simulation may become less robust. Thus using the Newton solver with elliptic friction cones and large value of
``impratio`` is the recommended way of reducing slip.
``impratio`` is the recommended way of reducing slip. For more detailed recommendations, see
:ref:`preventing slip<CSlippage>` in the Modeling chapter.
.. _TypeNameId:
@@ -847,7 +848,7 @@ Now the differences. Bodies are used to construct the kinematic tree and are con
geoms and sites. Bodies have a spatial frame, inertial properties, but no properties related to appearance or collision
geometry. This is because such properties do not affect the physics (except for contacts of course, but these are
handled separately). If you have seen diagrams of kinematic trees in robotics textbooks, the bodies are usually drawn as
amorphous shapes - to make the point that their actual shape is irrelevant to the physics.
amorphous shapes -- to make the point that their actual shape is irrelevant to the physics.
Geoms (short for geometric primitive) are used to specify appearance and collision geometry. Each geom belongs to a body
and is rigidly attached to that body. Multiple geoms can be attached to the same body. This is particularly useful in
doc/programming/simulation.rst
+9 -9
View File
@@ -150,10 +150,10 @@ continuous time, and then integrate over a time period specified by ``mjModel.op
the acceleration ``mjData.qacc`` at time ``mjData.time``, given the :ref:`state and control <siStateControl>` at time
``mjData.time``. The numerical integrator then advances the state and time to ``mjData.time + mjModel.opt.timestep``.
Now, the control is required to be a function of the state at time ``mjData.time``. However a general feedback
controller can be a very complex function, depending on various features of the state - in particular all the features
controller can be a very complex function, depending on various features of the state -- in particular all the features
computed by MuJoCo as intermediate results of the simulation. These may include contacts, Jacobians, passive forces.
None of these quantities are available before ``mj_step`` is called (or rather, they are available but outdated by one
time step). In contrast, when ``mj_step`` calls our control callback, it does so as late in the computation as possible
None of these quantities are available before ``mj_step`` is called (or rather, they are available but *outdated by one
time step*). In contrast, when ``mj_step`` calls our control callback, it does so as late in the computation as possible
- namely after all the intermediate results dependent on the state but not on the control have been computed.
The same effect can be achieved without using a control callback. This is done by breaking ``mj_step`` in two parts:
@@ -253,7 +253,7 @@ notion of state of a dynamical system. Dynamical systems are usually described i
where ``t`` is the time, ``x`` is the state vector, ``u`` is the control vector, and ``f`` is the function that
computes the time-derivative of the state. This is a continuous-time formulation, and indeed the physics model
simulated by MuJoCo is defined in continuous time. Even though the numerical integrator operates in discrete time, the
main part of the computation - namely the function :ref:`mj_forward` - corresponds to the
main part of the computation---namely the function :ref:`mj_forward`---corresponds to the
continuous-time dynamics function ``f(t,x,u)`` above. Here we explain this correspondence.
The state vector in MuJoCo is:
@@ -426,7 +426,7 @@ the first and second sections (position and velocity dependent) have already bee
When can we use the above machinery and skip some of the computations? In a regular simulation this is not possible.
However, MuJoCo is designed not only for simulation but also for more advanced applications such as model-based
optimization, machine learning etc. In such settings one often needs to sample the dynamics at a cloud of nearby
states, or approximate derivatives via finite differences - which is another form of sampling. If the samples are
states, or approximate derivatives via finite differences -- which is another form of sampling. If the samples are
arranged on a grid, where only the position or only the velocity or only the control is different from the center
point, then the above mechanism can improve performance by about a factor of 2.
@@ -591,7 +591,7 @@ The only way to guarantee that all changes are saved is to save the model as a b
changes need to be made programmatically, as in system identification for example, and this can only be done with the
compiled model. So in summary, we have reasonable but not perfect mechanisms for saving model changes. The reason for
this lack of perfection is that we are working with a compiled model, so this is like changing a binary executable and
asking a "decompiler" to make corresponding changes to the C code - it is just not possible in general.
asking a "decompiler" to make corresponding changes to the C code -- it is just not possible in general.
.. _siLayout:
@@ -642,7 +642,7 @@ spread out the data so that we do not have to perform many memory moves when ins
layout "uncompressed". It is still a valid layout, but instead of A_rowadr[r] = A_rowadr[r-1] + A_rownnz[r] which is
the standard convention, we set A_rowadr[r] = r*n. MuJoCo uses sparse matrices internally
To represent 3D orientations and rotations, MuJoCo uses unit quaternions - namely 4D unit vectors arranged as q = (w,
To represent 3D orientations and rotations, MuJoCo uses unit quaternions -- namely 4D unit vectors arranged as q = (w,
x, y, z). Here (x, y, z) is the rotation axis unit vector scaled by sin(a/2), where a is the rotation angle in
radians, and w = cos(a/2). Thus the quaternion corresponding to a null rotation is (1, 0, 0, 0). This is the default
setting of all quaternions in MJCF.
@@ -841,7 +841,7 @@ Jacobians
The derivative of any vector function with respect to its vector argument is called Jacobian. When this term is used
in multi-joint kinematics and dynamics, it refers to the derivative of some spatial quantity as a function of the
system configuration. In that case the Jacobian is also a linear map that operates on vectors in the (co)tangent space
to the configuration manifold - such as velocities, momenta, accelerations, forces. One caveat here is that the system
to the configuration manifold -- such as velocities, momenta, accelerations, forces. One caveat here is that the system
configuration encoded in ``mjData.qpos`` has dimensionality ``mjModel.nq``, while the tangent space has dimensionality
``mjModel.nv``, and the latter is smaller when quaternion joints are present. So the size of the Jacobian matrix is
N-by-``mjModel.nv`` where N is the dimensionality of the spatial quantity being differentiated.
@@ -861,7 +861,7 @@ Jacobians, which tell us how a spatial frame anchored at the given point will tr
change to the kinematic configuration. More precisely, the Jacobian maps joint velocities to end-effector velocities,
while the transpose of the Jacobian maps end-effector forces to joint forces. There are also several other
``mj_jacXXX`` functions; these are convenience functions that call the main ``mj_jac`` function with different points
of interest - such as a body center of mass, geom center etc.
of interest -- such as a body center of mass, geom center etc.
The ability to compute end-effector Jacobians exactly and efficiently is a key advantage of working in joint
coordinates. Such Jacobians are the foundation of many control schemes that map end-effector errors to actuator
doc/python.rst
+145 -100
View File
@@ -1,8 +1,8 @@
===============
Python Bindings
===============
======
Python
======
Starting with version 2.1.2, MuJoCo comes with native Python bindings that are developed in C++ using
MuJoCo comes with native Python bindings that are developed in C++ using
`pybind11 <https://pybind11.readthedocs.io/>`__. The Python API is consistent with the underlying C API. This leads to
some non-Pythonic code structure (e.g. order of function arguments), but it has the benefit that the
:doc:`API documentation<APIreference/index>` is applicable to both languages.
@@ -12,23 +12,22 @@ low-level bindings that are meant to give as close to a direct access to the MuJ
order to provide an API and semantics that developers would expect in a typical Python library, the bindings
deliberately diverge from the raw MuJoCo API in a number of places, which are documented throughout this page.
Google DeepMind’s `dm_control <https://github.com/google-deepmind/dm_control>`__ reinforcement learning library (which
prior to version 1.0.0 implemented its own MuJoCo bindings based on ``ctypes``) has been updated to depend on the
``mujoco`` package and continues to be supported by Google DeepMind. Changes in dm_control should be largely transparent
to users of previous versions, however code that depended directly on its low-level API may need to be updated. Consult
the `migration guide <https://github.com/google-deepmind/dm_control/blob/main/migration_guide_1.0.md>`__ for detail.
Google DeepMind’s `dm_control <https://github.com/google-deepmind/dm_control>`__ reinforcement learning library depends
on the ``mujoco`` package and continues to be supported by Google DeepMind. For code that depends on dm_control versions
prior to 1.0.0, consult the
`migration guide <https://github.com/google-deepmind/dm_control/blob/main/migration_guide_1.0.md>`__.
For mujoco-py users, we include :ref:`notes <PyMjpy_migration>` below to aid migration.
For mujoco-py users, we include :ref:`migration notes <PyMjpy_migration>` below.
.. _PyNotebook:
Tutorial notebook
=================
A MuJoCo tutorial using the Python bindings is available here: |colab|
A MuJoCo tutorial using the Python bindings is available here: |mjcolab|
.. |colab| image:: https://colab.research.google.com/assets/colab-badge.svg
:target: https://colab.research.google.com/github/google-deepmind/mujoco/blob/main/python/tutorial.ipynb
.. |mjcolab| image:: https://colab.research.google.com/assets/colab-badge.svg
:target: https://colab.research.google.com/github/google-deepmind/mujoco/blob/main/python/tutorial.ipynb
.. _PyInstallation:
@@ -55,8 +54,8 @@ use cases are supported:
.. _PyViewerApp:
Standalone application
----------------------
Standalone app
--------------
- ``python -m mujoco.viewer`` launches an empty visualization session, where a model can be loaded by drag-and-drop.
- ``python -m mujoco.viewer --mjcf=/path/to/some/mjcf.xml`` launches a visualization session for the specified
@@ -466,90 +465,6 @@ Alternatively, if a callback is implemented in a native dynamic library, users c
it to ``mujoco.set_mjcb_foo``. The bindings will then retrieve the underlying function pointer and assign it directly to
the raw callback pointer, and the GIL will **not** be acquired each time the callback is entered.
.. _PySample:
Open-loop rollouts
==================
We include a code sample showing how to add additional C/C++ functionality, exposed as a Python module via pybind11. The
sample, implemented in `rollout.cc <https://github.com/google-deepmind/mujoco/blob/main/python/mujoco/rollout.cc>`__
and wrapped in `rollout.py <https://github.com/google-deepmind/mujoco/blob/main/python/mujoco/rollout.py>`__,
implements a common use case where tight loops implemented outside of Python are beneficial: rolling out a trajectory
(i.e., calling ``mj_step()`` in a loop), given an intial state and sequence of controls, and returning subsequent states
and sensor values. The basic usage form is
.. code-block:: python
state, sensordata = rollout.rollout(model, data, initial_state, control)
``initial_state`` is a ``nroll x nstate`` array, with ``nroll`` initial states of size ``nstate``, where
``nstate = mj_stateSize(model, mjtState.mjSTATE_FULLPHYSICS)`` is the size of the
:ref:`full physics state<geFullPhysics>`. ``control`` is a ``nroll x nstep x ncontrol`` array of controls. Controls are
by default the ``mjModel.nu`` standard actuators, but any combination of :ref:`user input<geInput>` arrays can be
specified by passing an optional ``control_spec`` bitflag.
If a rollout diverges, the current state and sensor values are used to fill the remainder of the trajectory.
Therefore, non-increasing time values can be used to detect diverged rollouts.
The ``rollout`` function is designed to be completely stateless, so all inputs of the stepping pipeline are set and any
values already present in the given ``MjData`` instance will have no effect on the output.
Since the Global Interpreter Lock can be released, this function can be efficiently threaded using Python threads. See
the ``test_threading`` function in
`rollout_test.py <https://github.com/google-deepmind/mujoco/blob/main/python/mujoco/rollout_test.py>`__ for an example
of threaded operation (and more generally for usage examples).
.. _PyMjpy_migration:
Migration from mujoco-py
========================
In mujoco-py, the main entry point is the `MjSim <https://github.com/openai/mujoco-py/blob/master/mujoco_py/mjsim.pyx>`_
class. Users construct a stateful ``MjSim`` instance from an MJCF model (similar to ``dm_control.Physics``), and this
instance holds references to an ``mjModel`` instance and its associated ``mjData``. In contrast, the MuJoCo Python
bindings (``mujoco``) take a more low-level approach, as explained above: following the design principle of the C
library, the ``mujoco`` module itself is stateless, and merely wraps the underlying native structs and functions.
While a complete survey of mujoco-py is beyond the scope of this document, we offer below implementation notes for a
non-exhaustive list of specific mujoco-py features:
``mujoco_py.load_model_from_xml(bstring)``
This factory function constructs a stateful ``MjSim`` instance. When using ``mujoco``, the user should call the
factory function ``mujoco.MjModel.from_xml_*`` as described :ref:`above <PyStructs>`. The user is then responsible
for holding the resulting ``MjModel`` struct instance and explicitly generating the corresponding ``MjData`` by
calling ``mujoco.MjData(model)``.
``sim.reset()``, ``sim.forward()``, ``sim.step()``
Here as above, ``mujoco`` users needs to call the underlying library functions, passing instances of ``MjModel`` and
``MjData``: :ref:`mujoco.mj_resetData(model, data) <mj_resetData>`, :ref:`mujoco.mj_forward(model, data)
<mj_forward>`, and :ref:`mujoco.mj_step(model, data) <mj_step>`.
``sim.get_state()``, ``sim.set_state(state)``, ``sim.get_flattened_state()``, ``sim.set_state_from_flattened(state)``
The MuJoCo library’s computation is deterministic given a specific input, as explained in the :ref:`Programming
section <Simulation>`. mujoco-py implements methods for getting and setting some of the relevant fields (and
similarly ``dm_control.Physics`` offers methods that correspond to the flattened case). ``mujoco`` do not offer such
abstraction, and the user is expected to get/set the values of the relevant fields explicitly.
``sim.model.get_joint_qvel_addr(joint_name)``
This is a convenience method in mujoco-py that returns a list of contiguous indices corresponding to this joint. The
list starts from ``model.jnt_qposadr[joint_index]``, and its length depends on the joint type. ``mujoco`` doesn't
offer this functionality, but this list can be easily constructed using ``model.jnt_qposadr[joint_index]`` and
``xrange``.
``sim.model.*_name2id(name)``
mujoco-py creates dicts in ``MjSim`` that allow for efficient lookup of indices for objects of different types:
``site_name2id``, ``body_name2id`` etc. These functions replace the function :ref:`mujoco.mj_name2id(model,
type_enum, name) <mj_name2id>`. ``mujoco`` offers a different approach for using entity names – :ref:`named access
<PyNamed>`, as well as access to the native :ref:`mj_name2id`.
``sim.save(fstream, format_name)``
This is the one context in which the MuJoCo library (and therefore also ``mujoco``) is stateful: it holds a copy in
memory of the last XML that was compiled, which is used in :ref:`mujoco.mj_saveLastXML(fname) <mj_saveLastXML>`. Note
that mujoco-py’s implementation has a convenient extra feature, whereby the pose (as determined by ``sim.data``’s
state) is transformed to a keyframe that’s added to the model before saving. This extra feature is not currently
available in ``mujoco``.
.. _PyBuild:
Building from source
@@ -609,7 +524,9 @@ Building from source
.. code-block:: shell
cd dist
MUJOCO_PATH=/PATH/TO/MUJOCO MUJOCO_PLUGIN_PATH=/PATH/TO/MUJOCO_PLUGIN pip install mujoco-x.y.z.tar.gz
MUJOCO_PATH=/PATH/TO/MUJOCO
MUJOCO_PLUGIN_PATH=/PATH/TO/MUJOCO_PLUGIN
pip install mujoco-x.y.z.tar.gz
The Python bindings should now be installed! To check that they've been
successfully installed, ``cd`` outside of the ``mujoco`` directory and run
@@ -619,3 +536,131 @@ successfully installed, ``cd`` outside of the ``mujoco`` directory and run
As a reference, a working build configuration can be found in MuJoCo's
`continuous integration setup <https://github.com/google-deepmind/mujoco/blob/main/.github/workflows/build.yml>`_ on
GitHub.
.. _PyModule:
Modules
=======
The ``mujoco`` package contains two sub-modules: ``mujoco.rollout`` and ``mujoco.minimize``
.. _PyRollout:
rollout
-------
``mujoco.rollout`` shows how to add additional C/C++ functionality, exposed as a Python module via pybind11. It is
implemented in `rollout.cc <https://github.com/google-deepmind/mujoco/blob/main/python/mujoco/rollout.cc>`__
and wrapped in `rollout.py <https://github.com/google-deepmind/mujoco/blob/main/python/mujoco/rollout.py>`__. The module
performs a common functionality where tight loops implemented outside of Python are beneficial: rolling out a trajectory
(i.e., calling :ref:`mj_step` in a loop), given an intial state and sequence of controls, and returning subsequent
states and sensor values. The basic usage form is
.. code-block:: python
state, sensordata = rollout.rollout(model, data, initial_state, control)
``initial_state`` is an ``nroll x nstate`` array, with ``nroll`` initial states of size ``nstate``, where
``nstate = mj_stateSize(model, mjtState.mjSTATE_FULLPHYSICS)`` is the size of the
:ref:`full physics state<geFullPhysics>`. ``control`` is a ``nroll x nstep x ncontrol`` array of controls. Controls are
by default the ``mjModel.nu`` standard actuators, but any combination of :ref:`user input<geInput>` arrays can be
specified by passing an optional ``control_spec`` bitflag.
If a rollout diverges, the current state and sensor values are used to fill the remainder of the trajectory.
Therefore, non-increasing time values can be used to detect diverged rollouts.
The ``rollout`` function is designed to be completely stateless, so all inputs of the stepping pipeline are set and any
values already present in the given ``MjData`` instance will have no effect on the output.
Since the Global Interpreter Lock can be released, this function can be efficiently threaded using Python threads. See
the ``test_threading`` function in
`rollout_test.py <https://github.com/google-deepmind/mujoco/blob/main/python/mujoco/rollout_test.py>`__ for an example
of threaded operation (and more generally for usage examples).
.. _PyMinimize:
minimize
--------
This module contains optimization-related utilities.
The ``minimize.least_squares()`` function implements a nonlinear Least Squares optimizer solving sequential
Quadratic Programs with :ref:`mju_boxQP`. It is documented in the associated notebook: |lscolab|
.. |lscolab| image:: https://colab.research.google.com/assets/colab-badge.svg
:target: https://colab.research.google.com/github/google-deepmind/mujoco/blob/main/python/least_squares.ipynb
.. _PyUtility:
Utilities
=========
The `python/mujoco <https://github.com/google-deepmind/mujoco/tree/main/python/mujoco>`__ directory also contains
utility scripts.
.. _PyMsh2obj:
msh2obj.py
----------
The `msh2obj.py <https://github.com/google-deepmind/mujoco/blob/main/python/mujoco/msh2obj.py>`__ script converts the
:ref:`legacy .msh format<legacy-msh-docs>` for surface meshes (different from the possibly-volumetric
:ref:`gmsh format<gmsh-file-docs>` also using .msh), to OBJ files. The legacy format is depricated and will be removed
in a future release. Please convert all legacy files to OBJ.
.. _PyMjpy_migration:
mujoco-py migration
===================
In mujoco-py, the main entry point is the `MjSim <https://github.com/openai/mujoco-py/blob/master/mujoco_py/mjsim.pyx>`_
class. Users construct a stateful ``MjSim`` instance from an MJCF model (similar to ``dm_control.Physics``), and this
instance holds references to an ``mjModel`` instance and its associated ``mjData``. In contrast, the MuJoCo Python
bindings (``mujoco``) take a more low-level approach, as explained above: following the design principle of the C
library, the ``mujoco`` module itself is stateless, and merely wraps the underlying native structs and functions.
While a complete survey of mujoco-py is beyond the scope of this document, we offer below implementation notes for a
non-exhaustive list of specific mujoco-py features:
``mujoco_py.load_model_from_xml(bstring)``
This factory function constructs a stateful ``MjSim`` instance. When using ``mujoco``, the user should call the
factory function ``mujoco.MjModel.from_xml_*`` as described :ref:`above <PyStructs>`. The user is then responsible
for holding the resulting ``MjModel`` struct instance and explicitly generating the corresponding ``MjData`` by
calling ``mujoco.MjData(model)``.
``sim.reset()``, ``sim.forward()``, ``sim.step()``
Here as above, ``mujoco`` users needs to call the underlying library functions, passing instances of ``MjModel`` and
``MjData``: :ref:`mujoco.mj_resetData(model, data) <mj_resetData>`, :ref:`mujoco.mj_forward(model, data)
<mj_forward>`, and :ref:`mujoco.mj_step(model, data) <mj_step>`.
``sim.get_state()``, ``sim.set_state(state)``, ``sim.get_flattened_state()``, ``sim.set_state_from_flattened(state)``
The MuJoCo library’s computation is deterministic given a specific input, as explained in the :ref:`Programming
section <Simulation>`. mujoco-py implements methods for getting and setting some of the relevant fields (and
similarly ``dm_control.Physics`` offers methods that correspond to the flattened case). ``mujoco`` do not offer such
abstraction, and the user is expected to get/set the values of the relevant fields explicitly.
``sim.model.get_joint_qvel_addr(joint_name)``
This is a convenience method in mujoco-py that returns a list of contiguous indices corresponding to this joint. The
list starts from ``model.jnt_qposadr[joint_index]``, and its length depends on the joint type. ``mujoco`` doesn't
offer this functionality, but this list can be easily constructed using ``model.jnt_qposadr[joint_index]`` and
``xrange``.
``sim.model.*_name2id(name)``
mujoco-py creates dicts in ``MjSim`` that allow for efficient lookup of indices for objects of different types:
``site_name2id``, ``body_name2id`` etc. These functions replace the function :ref:`mujoco.mj_name2id(model,
type_enum, name) <mj_name2id>`. ``mujoco`` offers a different approach for using entity names – :ref:`named access
<PyNamed>`, as well as access to the native :ref:`mj_name2id`.
``sim.save(fstream, format_name)``
This is the one context in which the MuJoCo library (and therefore also ``mujoco``) is stateful: it holds a copy in
memory of the last XML that was compiled, which is used in :ref:`mujoco.mj_saveLastXML(fname) <mj_saveLastXML>`. Note
that mujoco-py’s implementation has a convenient extra feature, whereby the pose (as determined by ``sim.data``’s
state) is transformed to a keyframe that’s added to the model before saving. This extra feature is not currently
available in ``mujoco``.
doc/unity.rst
+2 -2
View File
@@ -30,14 +30,14 @@ _____
The MuJoCo app needs to be run at least once before the native library can be used, in order to register the library as
a trusted binary. Then, copy the dynamic library file from
``/Applications/MuJoCo.app/Contents/Frameworks/mujoco.framework/Versions/Current/libmujoco.3.1.5.dylib`` (it can be
``/Applications/MuJoCo.app/Contents/Frameworks/mujoco.framework/Versions/Current/libmujoco.3.1.6.dylib`` (it can be
found by browsing the contents of ``MuJoCo.app``) and rename it as ``mujoco.dylib``.
Linux
_____
Expand the ``tar.gz`` archive to ``~/.mujoco``. Then copy the dynamic library from
``~/.mujoco/mujoco-3.1.5/lib/libmujoco.so.3.1.5`` and rename it as ``libmujoco.so``.
``~/.mujoco/mujoco-3.1.6/lib/libmujoco.so.3.1.6`` and rename it as ``libmujoco.so``.
Windows
_______
include/mujoco/mjdata.h
+1 -1
View File
@@ -110,7 +110,7 @@ struct mjContact_ { // result of collision detection functions
// internal storage used by solver
mjtNum mu; // friction of regularized cone, set by mj_makeConstraint
mjtNum H[36]; // cone Hessian, set by mj_updateConstraint
mjtNum H[36]; // cone Hessian, set by mj_constraintUpdate
// contact descriptors set by mj_collideXXX
int dim; // contact space dimensionality: 1, 3, 4 or 6
include/mujoco/mjmodel.h
+5
View File
@@ -327,6 +327,11 @@ typedef enum mjtSensor_ { // type of sensor
mjSENS_SUBTREELINVEL, // 3D linear velocity of subtree
mjSENS_SUBTREEANGMOM, // 3D angular momentum of subtree
// sensors for geometric distance; attached to geoms or bodies
mjSENS_GEOMDIST, // signed distance between two geoms
mjSENS_GEOMNORMAL, // normal direction between two geoms
mjSENS_GEOMFROMTO, // segment between two geoms
// global sensors
mjSENS_CLOCK, // simulation time
include/mujoco/mujoco.h
+8 -4
View File
@@ -24,7 +24,7 @@ extern "C" {
#endif
// header version; should match the library version as returned by mj_version()
#define mjVERSION_HEADER 315
#define mjVERSION_HEADER 316
// needed to define size_t, fabs and log10
#include <stdlib.h>
@@ -464,6 +464,10 @@ MJAPI void mj_objectVelocity(const mjModel* m, const mjData* d,
MJAPI void mj_objectAcceleration(const mjModel* m, const mjData* d,
int objtype, int objid, mjtNum res[6], int flg_local);
// Returns smallest signed distance between two geoms and optionally segment from geom1 to geom2.
MJAPI mjtNum mj_geomDistance(const mjModel* m, const mjData* d, int geom1, int geom2,
mjtNum distmax, mjtNum fromto[6]);
// Extract 6D force:torque given contact id, in the contact frame.
MJAPI void mj_contactForce(const mjModel* m, const mjData* d, int id, mjtNum result[6]);
@@ -684,13 +688,13 @@ MJAPI void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* opt,
const mjvPerturb* pert, int catmask, mjvScene* scn);
// Make list of lights.
MJAPI void mjv_makeLights(const mjModel* m, mjData* d, mjvScene* scn);
MJAPI void mjv_makeLights(const mjModel* m, const mjData* d, mjvScene* scn);
// Update camera.
MJAPI void mjv_updateCamera(const mjModel* m, mjData* d, mjvCamera* cam, mjvScene* scn);
MJAPI void mjv_updateCamera(const mjModel* m, const mjData* d, mjvCamera* cam, mjvScene* scn);
// Update skins.
MJAPI void mjv_updateSkin(const mjModel* m, mjData* d, mjvScene* scn);
MJAPI void mjv_updateSkin(const mjModel* m, const mjData* d, mjvScene* scn);
//---------------------------------- OpenGL rendering ----------------------------------------------
introspect/enums.py
+6 -3
View File
@@ -338,9 +338,12 @@ ENUMS: Mapping[str, EnumDecl] = dict([
('mjSENS_SUBTREECOM', 34),
('mjSENS_SUBTREELINVEL', 35),
('mjSENS_SUBTREEANGMOM', 36),
('mjSENS_CLOCK', 37),
('mjSENS_PLUGIN', 38),
('mjSENS_USER', 39),
('mjSENS_GEOMDIST', 37),
('mjSENS_GEOMNORMAL', 38),
('mjSENS_GEOMFROMTO', 39),
('mjSENS_CLOCK', 40),
('mjSENS_PLUGIN', 41),
('mjSENS_USER', 42),
]),
)),
('mjtStage',
introspect/functions.py
+42 -3
View File
@@ -2743,6 +2743,45 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([
),
doc='Compute object 6D acceleration (rot:lin) in object-centered frame, world/local orientation.', # pylint: disable=line-too-long
)),
('mj_geomDistance',
FunctionDecl(
name='mj_geomDistance',
return_type=ValueType(name='mjtNum'),
parameters=(
FunctionParameterDecl(
name='m',
type=PointerType(
inner_type=ValueType(name='mjModel', is_const=True),
),
),
FunctionParameterDecl(
name='d',
type=PointerType(
inner_type=ValueType(name='mjData', is_const=True),
),
),
FunctionParameterDecl(
name='geom1',
type=ValueType(name='int'),
),
FunctionParameterDecl(
name='geom2',
type=ValueType(name='int'),
),
FunctionParameterDecl(
name='distmax',
type=ValueType(name='mjtNum'),
),
FunctionParameterDecl(
name='fromto',
type=ArrayType(
inner_type=ValueType(name='mjtNum'),
extents=(6,),
),
),
),
doc='Returns smallest signed distance between two geoms and optionally segment from geom1 to geom2.', # pylint: disable=line-too-long
)),
('mj_contactForce',
FunctionDecl(
name='mj_contactForce',
@@ -4439,7 +4478,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([
FunctionParameterDecl(
name='d',
type=PointerType(
inner_type=ValueType(name='mjData'),
inner_type=ValueType(name='mjData', is_const=True),
),
),
FunctionParameterDecl(
@@ -4465,7 +4504,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([
FunctionParameterDecl(
name='d',
type=PointerType(
inner_type=ValueType(name='mjData'),
inner_type=ValueType(name='mjData', is_const=True),
),
),
FunctionParameterDecl(
@@ -4497,7 +4536,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([
FunctionParameterDecl(
name='d',
type=PointerType(
inner_type=ValueType(name='mjData'),
inner_type=ValueType(name='mjData', is_const=True),
),
),
FunctionParameterDecl(
introspect/structs.py
+1 -1
View File
@@ -4011,7 +4011,7 @@ STRUCTS: Mapping[str, StructDecl] = dict([
inner_type=ValueType(name='mjtNum'),
extents=(36,),
),
doc='cone Hessian, set by mj_updateConstraint',
doc='cone Hessian, set by mj_constraintUpdate',
),
StructFieldDecl(
name='dim',
mjx/mujoco/mjx/_src/collision_convex.py
+275 -52
View File
@@ -14,8 +14,9 @@
# ==============================================================================
"""Convex collisions."""
from collections.abc import Callable
import functools
from typing import Tuple
from typing import Tuple, Union
import jax
from jax import numpy as jp
@@ -26,16 +27,19 @@ from mujoco.mjx._src.collision_types import Collision
from mujoco.mjx._src.collision_types import ConvexInfo
from mujoco.mjx._src.collision_types import FunctionKey
from mujoco.mjx._src.collision_types import GeomInfo
from mujoco.mjx._src.collision_types import HFieldInfo
from mujoco.mjx._src.types import Data
from mujoco.mjx._src.types import GeomType
from mujoco.mjx._src.types import Model
# pylint: enable=g-importing-member
_GeomInfo = Union[GeomInfo, ConvexInfo]
def collider(ncon: int):
"""Wraps collision functions for use by collision_driver."""
def wrapper(func):
def wrapper(collision_fn):
def collide(
m: Model, d: Data, key: FunctionKey, geom: jax.Array
) -> Collision:
@@ -45,18 +49,25 @@ def collider(ncon: int):
GeomInfo(d.geom_xpos[g2], d.geom_xmat[g2], m.geom_size[g2]),
]
in_axes = [0, 0]
fn = collision_fn
for i in [0, 1]:
if key.types[i] == GeomType.BOX:
infos[i] = mesh.box(infos[i])
in_axes[i] = jax.tree_util.tree_map(lambda x: None, infos[i]).replace(
pos=0, mat=0, face=0, vert=0
pos=0, mat=0, size=0, face=0, vert=0
)
elif key.types[i] == GeomType.MESH:
infos[i] = mesh.convex(m, key.data_ids[i], infos[i])
c, cm = infos[i], m.mesh_convex[key.data_ids[i]]
infos[i] = ConvexInfo(**vars(c), **vars(cm))
in_axes[i] = jax.tree_util.tree_map(lambda x: None, infos[i]).replace(
pos=0, mat=0
pos=0, mat=0, size=0
)
dist, pos, frame = jax.vmap(func, in_axes=in_axes)(*infos)
elif key.types[i] == GeomType.HFIELD:
hfield_info = mesh.hfield(m, key.data_ids[i])
infos[i] = hfield_info.replace(pos=infos[i].pos, mat=infos[i].mat)
in_axes[i] = hfield_info.replace(pos=0, mat=0, data=None)
fn = functools.partial(fn, subgrid_size=key.subgrid_size)
dist, pos, frame = jax.vmap(fn, in_axes=in_axes)(*infos)
if ncon > 1:
return jax.tree_util.tree_map(jp.concatenate, (dist, pos, frame))
return dist, pos, frame
@@ -242,9 +253,8 @@ def plane_convex(plane: GeomInfo, convex: ConvexInfo) -> Collision:
return dist, pos, frame
@collider(ncon=1)
def sphere_convex(sphere: GeomInfo, convex: ConvexInfo) -> Collision:
"""Calculates contact between a sphere and a convex object."""
def _sphere_convex(sphere: GeomInfo, convex: ConvexInfo) -> Collision:
"""Calculates contact between a sphere and a convex mesh."""
faces = convex.face
normals = convex.face_normal
@@ -276,7 +286,7 @@ def sphere_convex(sphere: GeomInfo, convex: ConvexInfo) -> Collision:
face_normal,
)
edge_dist = jax.vmap(
lambda plane_pt, plane_norm: (pt - plane_pt).dot(plane_norm)
lambda plane_pt, plane_norm, pt=pt: (pt - plane_pt).dot(plane_norm)
)(edge_p0, side_normals)
pt_on_face = jp.all(edge_dist <= 0) # lte to handle degenerate edges
@@ -291,7 +301,8 @@ def sphere_convex(sphere: GeomInfo, convex: ConvexInfo) -> Collision:
# Get the normal, dist, and contact position.
pt_normal, d = math.normalize_with_norm(pt - sphere_pos)
# Ensure normal points towards convex centroid.
# Ensure normal points towards convex centroid. Assume convex centroid is at
# the origin.
inside = jp.dot(pt, pt_normal) > 0
sign = jp.where(inside, -1, 1)
n = jp.where(pt_on_face | (d < 1e-6), -face_normal, sign * pt_normal)
@@ -305,11 +316,17 @@ def sphere_convex(sphere: GeomInfo, convex: ConvexInfo) -> Collision:
n = convex.mat @ n
pos = convex.mat @ pos + convex.pos
return dist, pos, n
@collider(ncon=1)
def sphere_convex(sphere: GeomInfo, convex: ConvexInfo) -> Collision:
"""Calculates contact between a sphere and a convex mesh."""
dist, pos, n = _sphere_convex(sphere, convex)
return dist, pos, math.make_frame(n)
@collider(ncon=2)
def capsule_convex(cap: GeomInfo, convex: ConvexInfo) -> Collision:
def _capsule_convex(cap: GeomInfo, convex: ConvexInfo) -> Collision:
"""Calculates contacts between a capsule and a convex object."""
# Get convex transformed normals, faces, and vertices.
faces = convex.face
@@ -431,6 +448,13 @@ def capsule_convex(cap: GeomInfo, convex: ConvexInfo) -> Collision:
dist = -jp.where(
has_edge_contact, jp.array([edge_penetration, -1]), face_penetration
)
return dist, pos, n
@collider(ncon=2)
def capsule_convex(cap: GeomInfo, convex: ConvexInfo) -> Collision:
"""Calculates contacts between a capsule and a convex object."""
dist, pos, n = _capsule_convex(cap, convex)
frame = jax.vmap(math.make_frame)(n)
return dist, pos, frame
@@ -851,6 +875,7 @@ def _sat_gaussmap(
incident_face_norm,
-best_axis,
)
dist = jp.where(is_face_separating, 1.0, dist)
# Handle edge separating axes by checking all edge pairs.
a_idx = jp.tile(jp.arange(edges_a.shape[0]), reps=edges_b.shape[0])
@@ -891,8 +916,9 @@ def _sat_gaussmap(
best_edge_idx = edge_dist.argmax()
best_edge_dist = edge_dist[best_edge_idx]
is_edge_contact = jp.where(
dist.max() < 0, best_edge_dist > dist.max() - 1e-6,
(best_edge_dist < 0) & ~jp.isinf(best_edge_dist)
dist.max() < 0.0,
best_edge_dist > dist.max() - 1e-6,
(best_edge_dist < 0) & ~jp.isinf(best_edge_dist),
)
is_edge_contact = is_edge_contact & ~is_face_separating
normal = jp.where(is_edge_contact, edge_axes[best_edge_idx], normal)
@@ -911,9 +937,45 @@ def _sat_gaussmap(
return dist, pos, normal
@collider(ncon=4)
def convex_convex(c1: ConvexInfo, c2: ConvexInfo) -> Collision:
"""Calculates contacts between two convex objects."""
def _box_box(b1: ConvexInfo, b2: ConvexInfo) -> Collision:
"""Calculates contacts between two boxes."""
faces1 = b1.face
faces2 = b2.face
to_local_pos = b2.mat.T @ (b1.pos - b2.pos)
to_local_mat = b2.mat.T @ b1.mat
faces1 = to_local_pos + faces1 @ to_local_mat.T
normals1 = b1.face_normal @ to_local_mat.T
normals2 = b2.face_normal
vertices1 = to_local_pos + b1.vert @ to_local_mat.T
vertices2 = b2.vert
unique_edges1 = jp.take(vertices1, b1.edge_dir, axis=0)
unique_edges2 = jp.take(vertices2, b2.edge_dir, axis=0)
# brute-force SAT is more performant for box-box
dist, pos, normal = _sat_bruteforce(
faces1,
faces2,
vertices1,
vertices2,
normals1,
normals2,
unique_edges1,
unique_edges2,
)
# Go back to world frame.
pos = b2.pos + pos @ b2.mat.T
n = normal @ b2.mat.T
return dist, pos, n
def _convex_convex(c1: ConvexInfo, c2: ConvexInfo) -> Collision:
"""Calculates contacts between two convex meshes."""
# pad face vertices so that we can broadcast between geom1 and geom2
# face has shape (n_face, n_vert, 3)
nvert1, nvert2 = c1.face.shape[1], c2.face.shape[1]
@@ -932,6 +994,7 @@ def convex_convex(c1: ConvexInfo, c2: ConvexInfo) -> Collision:
faces1 = c1.face
faces2 = c2.face
# convert to c2 frame
to_local_pos = c2.mat.T @ (c1.pos - c2.pos)
to_local_mat = c2.mat.T @ c1.mat
@@ -942,49 +1005,209 @@ def convex_convex(c1: ConvexInfo, c2: ConvexInfo) -> Collision:
vertices1 = to_local_pos + c1.vert @ to_local_mat.T
vertices2 = c2.vert
unique_edges1 = jp.take(vertices1, c1.edge_dir, axis=0)
unique_edges2 = jp.take(vertices2, c2.edge_dir, axis=0)
edges1 = jp.take(vertices1, c1.edge, axis=0)
edges2 = jp.take(vertices2, c2.edge, axis=0)
edge_face_normals1 = c1.edge_face_normal @ to_local_mat.T
edge_face_normals2 = c2.edge_face_normal
enable_bruteforce = (
unique_edges1.shape[0] * unique_edges2.shape[0]
< edges1[0].shape[0] * edges2[0].shape[0]
dist, pos, normal = _sat_gaussmap(
to_local_pos,
faces1,
faces2,
vertices1,
vertices2,
normals1,
normals2,
edges1,
edges2,
edge_face_normals1,
edge_face_normals2,
)
if enable_bruteforce:
dist, pos, normal = _sat_bruteforce(
faces1,
faces2,
vertices1,
vertices2,
normals1,
normals2,
unique_edges1,
unique_edges2,
)
else:
dist, pos, normal = _sat_gaussmap(
to_local_pos,
faces1,
faces2,
vertices1,
vertices2,
normals1,
normals2,
edges1,
edges2,
edge_face_normals1,
edge_face_normals2,
)
# Go back to world frame.
pos = c2.pos + pos @ c2.mat.T
normal = normal @ c2.mat.T
normal = -normal if swapped else normal
frame = jax.vmap(math.make_frame)(normal)
n = normal @ c2.mat.T
n = -n if swapped else n
return dist, pos, n
@collider(ncon=4)
def box_box(b1: ConvexInfo, b2: ConvexInfo) -> Collision:
"""Calculates contacts between two boxes."""
dist, pos, n = _box_box(b1, b2)
frame = jax.vmap(math.make_frame)(n)
return dist, pos, frame
@collider(ncon=4)
def convex_convex(c1: ConvexInfo, c2: ConvexInfo) -> Collision:
"""Calculates contacts between two convex objects."""
dist, pos, n = _convex_convex(c1, c2)
frame = jax.vmap(math.make_frame)(n)
return dist, pos, frame
def _hfield_collision(
collider_fn: Callable[[_GeomInfo, _GeomInfo], Collision],
h: HFieldInfo,
obj: _GeomInfo,
obj_rbound: jax.Array,
subgrid_size: Tuple[int, int],
) -> Collision:
"""Collides an object with prisms in a height field."""
# put obj in hfield frame
obj_pos = h.mat.T @ (obj.pos - h.pos)
obj_mat = h.mat.T @ obj.mat
xmin = obj_pos[0] - obj_rbound
ymin = obj_pos[1] - obj_rbound
cmin = jp.floor((xmin + h.size[0]) / (2 * h.size[0]) * (h.ncol - 1))
cmin = cmin.astype(int)
rmin = jp.floor((ymin + h.size[1]) / (2 * h.size[1]) * (h.nrow - 1))
rmin = rmin.astype(int)
# compute real-valued grid step
dx = 2.0 * h.size[0] / (h.ncol - 1)
dy = 2.0 * h.size[1] / (h.nrow - 1)
# set zbottom value using base size
bvert = jp.array([0.0, 0.0, -h.size[3]])
bmask = jp.array([True, True, False])
# process all prisms in sub-grid
prisms = []
for r in range(subgrid_size[1]):
for c in range(subgrid_size[0]):
ri, ci = rmin + r, cmin + c
# ensure ri, ci are in the bounds of the hfield
ri = jp.clip(ri, 0, h.nrow - 2)
ci = jp.clip(ci, 0, h.ncol - 2)
p1 = [
dx * ci - h.size[0],
dy * ri - h.size[1],
h.data[ci, ri] * h.size[2],
]
p2 = [
dx * (ci + 1) - h.size[0],
dy * (ri + 1) - h.size[1],
h.data[ci + 1, ri + 1] * h.size[2],
]
p3 = [
dx * ci - h.size[0],
dy * (ri + 1) - h.size[1],
h.data[ci, ri + 1] * h.size[2],
]
top = jp.array([p1, p2, p3])
bottom = jp.array([p1, p3, p2]) * bmask + bvert
vert = jp.concatenate([bottom, top])
prisms.append(mesh.hfield_prism(vert))
p3 = p2
p2 = [
dx * (ci + 1) - h.size[0],
dy * ri - h.size[1],
h.data[ci + 1, ri] * h.size[2],
]
top = jp.array([p1, p2, p3])
bottom = jp.array([p1, p3, p2]) * bmask + bvert
vert = jp.concatenate([bottom, top])
# NB: If the order of verts is updated above, the corresponding
# hfield_prism function must be updated to ensure that all faces have the
# correct winding order.
prisms.append(mesh.hfield_prism(vert))
n_prisms = len(prisms)
prisms = jax.tree_util.tree_map(lambda *x: jp.stack(x), *prisms)
dist, pos, n = jax.vmap(collider_fn, in_axes=[None, 0])(
obj.replace(pos=obj_pos, mat=obj_mat), prisms
)
dist = dist.flatten()
pos = pos.reshape((-1, 3))
n = n.reshape((-1, 3))
n *= -1 # flip the normal since we flipped args in the call to collider_fn
# Check that we're in the half-space of the hfield norm. If not, pick the top
# face norm. This resolves issues with cracks of doom.
n_repeats = dist.shape[0] // n_prisms
top_norm = jp.repeat(prisms.face_normal[:, 1], n_repeats, axis=0)
cond = jax.vmap(jp.dot, in_axes=[0, None])(n, h.mat[2]) < 1e-6
n = jp.where(cond[:, None], top_norm, n)
return dist, pos, n
@collider(ncon=4)
def hfield_sphere(
h: HFieldInfo, s: GeomInfo, subgrid_size: Tuple[int, int]
) -> Collision:
"""Calculates contacts between a hfield and a sphere."""
rbound = jp.max(s.size)
dist, pos, n = _hfield_collision(_sphere_convex, h, s, rbound, subgrid_size)
n_mean = jp.mean(n, axis=0)
mask = dist < jp.minimum(0, dist.min() + 1e-3)
idx = _manifold_points(pos, mask, n_mean)
dist, pos, n = dist[idx], pos[idx], n[idx]
# zero out non-unique contacts
unique = jp.tril(idx == idx[:, None]).sum(axis=1) == 1
dist = jp.where(unique, dist, 1)
# back to world frame, _hfield_collision returns collision in hfield frame
pos = jax.vmap(lambda p: h.mat @ p + h.pos)(pos)
n = jax.vmap(lambda n: h.mat @ n)(n)
return dist, pos, jax.vmap(math.make_frame)(n)
@collider(ncon=4)
def hfield_capsule(
h: HFieldInfo, c: GeomInfo, subgrid_size: Tuple[int, int]
) -> Collision:
"""Calculates contacts between a hfield and a capsule."""
rbound = c.size[0] + c.size[1]
dist, pos, n = _hfield_collision(_capsule_convex, h, c, rbound, subgrid_size)
n_mean = jp.mean(n, axis=0)
mask = dist < jp.minimum(0, dist.min() + 1e-3)
idx = _manifold_points(pos, mask, n_mean)
dist, pos, n = dist[idx], pos[idx], n[idx]
# zero out non-unique contacts
unique = jp.tril(idx == idx[:, None]).sum(axis=1) == 1
dist = jp.where(unique, dist, 1)
# back to world frame, _hfield_collision returns collision in hfield frame
pos = jax.vmap(lambda p: h.mat @ p + h.pos)(pos)
n = jax.vmap(lambda n: h.mat @ n)(n)
return dist, pos, jax.vmap(math.make_frame)(n)
@collider(ncon=4)
def hfield_convex(
h: HFieldInfo, c: ConvexInfo, subgrid_size: Tuple[int, int]
) -> Collision:
"""Calculates contacts between a hfield and a capsule."""
rbound = jp.max(c.size)
dist, pos, n = _hfield_collision(_convex_convex, h, c, rbound, subgrid_size)
n_mean = jp.mean(n, axis=0)
mask = dist < jp.minimum(0, dist.min() + 1e-3)
idx = _manifold_points(pos, mask, n_mean)
dist, pos, n = dist[idx], pos[idx], n[idx]
# zero out non-unique contacts
unique = jp.tril(idx == idx[:, None]).sum(axis=1) == 1
dist = jp.where(unique, dist, 1)
# back to world frame, _hfield_collision returns collision in hfield frame
pos = jax.vmap(lambda p: h.mat @ p + h.pos)(pos)
n = jax.vmap(lambda n: h.mat @ n)(n)
return dist, pos, jax.vmap(math.make_frame)(n)
mjx/mujoco/mjx/_src/collision_driver.py
+24 -1
View File
@@ -45,8 +45,12 @@ from jax import numpy as jp
import mujoco
from mujoco.mjx._src import support
# pylint: disable=g-importing-member
from mujoco.mjx._src.collision_convex import box_box
from mujoco.mjx._src.collision_convex import capsule_convex
from mujoco.mjx._src.collision_convex import convex_convex
from mujoco.mjx._src.collision_convex import hfield_capsule
from mujoco.mjx._src.collision_convex import hfield_convex
from mujoco.mjx._src.collision_convex import hfield_sphere
from mujoco.mjx._src.collision_convex import plane_convex
from mujoco.mjx._src.collision_convex import sphere_convex
from mujoco.mjx._src.collision_primitive import capsule_capsule
@@ -78,6 +82,10 @@ _COLLISION_FUNC = {
(GeomType.PLANE, GeomType.ELLIPSOID): plane_ellipsoid,
(GeomType.PLANE, GeomType.CYLINDER): plane_cylinder,
(GeomType.PLANE, GeomType.MESH): plane_convex,
(GeomType.HFIELD, GeomType.SPHERE): hfield_sphere,
(GeomType.HFIELD, GeomType.CAPSULE): hfield_capsule,
(GeomType.HFIELD, GeomType.BOX): hfield_convex,
(GeomType.HFIELD, GeomType.MESH): hfield_convex,
(GeomType.SPHERE, GeomType.SPHERE): sphere_sphere,
(GeomType.SPHERE, GeomType.CAPSULE): sphere_capsule,
(GeomType.SPHERE, GeomType.BOX): sphere_convex,
@@ -90,7 +98,7 @@ _COLLISION_FUNC = {
(GeomType.ELLIPSOID, GeomType.ELLIPSOID): ellipsoid_ellipsoid,
(GeomType.ELLIPSOID, GeomType.CYLINDER): ellipsoid_cylinder,
(GeomType.CYLINDER, GeomType.CYLINDER): cylinder_cylinder,
(GeomType.BOX, GeomType.BOX): convex_convex,
(GeomType.BOX, GeomType.BOX): box_box,
(GeomType.BOX, GeomType.MESH): convex_convex,
(GeomType.MESH, GeomType.MESH): convex_convex,
}
@@ -210,6 +218,21 @@ def _geom_groups(
condim = max(m.geom_condim[g1], m.geom_condim[g2])
key = FunctionKey(types, data_ids, condim)
if types[0] == mujoco.mjtGeom.mjGEOM_HFIELD:
# add static grid bounds to the grouping key for hfield collisions
geom_rbound_hfield = (
m.geom_rbound_hfield if isinstance(m, Model) else m.geom_rbound
)
nrow, ncol = m.hfield_nrow[data_ids[0]], m.hfield_ncol[data_ids[0]]
xsize, ysize = m.hfield_size[data_ids[0]][:2]
xtick, ytick = (2 * xsize) / (ncol - 1), (2 * ysize) / (nrow - 1)
xbound = int(np.ceil(2 * geom_rbound_hfield[g2] / xtick)) + 1
xbound = min(xbound, ncol)
ybound = int(np.ceil(2 * geom_rbound_hfield[g2] / ytick)) + 1
ybound = min(ybound, nrow)
key = FunctionKey(types, data_ids, condim, (xbound, ybound))
groups.setdefault(key, []).append((g1, g2, ip))
return groups
mjx/mujoco/mjx/_src/collision_driver_test.py
+89
View File
@@ -721,6 +721,95 @@ class ConvexTest(absltest.TestCase):
self.assertTrue((c.dist > 0).all())
class HFieldTest(absltest.TestCase):
_HFIELD = """
<mujoco>
<asset>
<hfield name="J" size="0.9 1.1 .2 .1" nrow="8" ncol="10" elevation="
0 0 0 0 0 0 1 1 0 0
0 0 0 0 0 0 1 1 0 0
0 0 0 0 1 1 1 1 0 0
0 0 1 1 0 0 1 1 0 0
0 0 1 1 1 1 1 1 0 0
0 0 1 1 1 1 1 0 0 0
0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0"/>
</asset>
<worldbody>
<light pos="0 0 0.32"/>
<geom type="hfield" hfield="J"/>
<body pos="0 0 1">
<freejoint/>
<geom size="0.1" contype="0"/>
</body>
<body pos="0 0 0.2">
<freejoint/>
<geom type="capsule" size="0.01 0.1" contype="0"/>
</body>
<body pos="0 0 0.55">
<freejoint/>
<geom type="box" size="0.05 0.05 0.025" contype="0"/>
</body>
</worldbody>
<keyframe>
<key name="qpos1" qpos='-0.0127496 0.156995 0.118336 0.336325 -0.810241 0.442853 -0.185139 -0.19614 -0.000912274 0.112334 -0.455846 0.852431 -0.0871208 0.24078 0.124334 0.23346 0.100627 0.293376 -0.27087 0.855587 0.32944'/>
<key name="qpos2" qpos='0.0815885 -3.18397 -9.4802 0.57036 0.695092 -0.403995 -0.168298 -0.0156545 0.157173 -0.00734406 0.606091 -0.185052 0.759332 -0.147728 0.100088 -0.234066 0.224884 0.999906 0.000384 -9.62645e-05 -0.013677'/>
</keyframe>
</mujoco>
"""
def test_sphere_hfield(self):
m = mujoco.MjModel.from_xml_string(self._HFIELD)
mx = mjx.put_model(m)
d = mujoco.MjData(m)
d.qpos[:] = m.keyframe('qpos1').qpos
dx = mjx.put_data(m, d)
collision_jit_fn = jax.jit(mjx.collision)
kinematics_jit_fn = jax.jit(mjx.kinematics)
dx = kinematics_jit_fn(mx, dx)
dx = collision_jit_fn(mx, dx)
# check that all geoms are colliding with the hfield
for geom_id in [1, 2, 3]:
mask = (dx.contact.geom == np.array([0, geom_id])).all(axis=1)
c = jax.tree_util.tree_map(lambda x, m=mask: x[m], dx.contact)
self.assertTrue((c.dist < 0).any())
self.assertTrue((c.dist > -1e-3).any())
# all contact normals are roughly pointing in the right direction
self.assertTrue((c.frame[:, 0].dot(np.array([0, 0, 1])) > 0.7).all())
def test_hfield_outside(self):
"""Tests that objects outside of the hfield do not collide."""
positions = ['2.0 0', '-2.0 0', '0 -2.0', '0 2.0']
for p in positions:
xml = self._HFIELD.replace('<body pos="0 0', f'<body pos="{p}')
_, dx = _collide(xml)
self.assertTrue((dx.contact.dist >= 0).all())
def test_hfield_deep(self):
"""Tests that objects with deep penetration do not get stuck."""
m = mujoco.MjModel.from_xml_string(self._HFIELD)
mx = mjx.put_model(m)
d = mujoco.MjData(m)
d.qpos[:] = m.keyframe('qpos2').qpos
dx = mjx.put_data(m, d)
collision_jit_fn = jax.jit(mjx.collision)
kinematics_jit_fn = jax.jit(mjx.kinematics)
dx = kinematics_jit_fn(mx, dx)
dx = collision_jit_fn(mx, dx)
# check that all geoms are colliding with the hfield
for geom_id in [1, 2, 3]:
mask = (dx.contact.geom == np.array([0, geom_id])).all(axis=1)
c = jax.tree_util.tree_map(lambda x, m=mask: x[m], dx.contact)
# all contact normals are in the top half-face of the hfield
self.assertTrue((c.frame[:, 0].dot(np.array([0, 0, 1])) > 0.7).all())
class BodyPairFilterTest(absltest.TestCase):
"""Tests that certain body pairs get filtered."""
mjx/mujoco/mjx/_src/collision_sdf.py
+15 -5
View File
@@ -122,8 +122,16 @@ def cylinder_jvp(primals, tangents):
return primal_out, tangent_out
def _to_local(f: SDFFn, pos: jax.Array, mat: jax.Array)-> SDFFn:
return lambda p: f(mat.T @ (p - pos))
def _from_to(
f: SDFFn,
from_pos: jax.Array,
from_mat: jax.Array,
to_pos: jax.Array,
to_mat: jax.Array,
) -> SDFFn:
relmat = math.matmul_unroll(to_mat.T, from_mat)
relpos = to_mat.T @ (from_pos - to_pos)
return lambda p: f(relmat @ p + relpos)
def _intersect(d1: SDFFn, d2: SDFFn) -> SDFFn:
@@ -175,13 +183,16 @@ def _optim(
) -> Collision:
"""Optimizes the clearance function."""
d1 = functools.partial(d1, size=info1.size)
d1 = _to_local(d1, info1.pos, info1.mat)
# evaluate d1 in d2 frame
d1 = _from_to(d1, info2.pos, info2.mat, info1.pos, info1.mat)
d2 = functools.partial(d2, size=info2.size)
d2 = _to_local(d2, info2.pos, info2.mat)
x0 = info2.mat.T @ (x0 - info2.pos)
fn = _clearance(d1, d2)
_, pos = _gradient_descent(fn, x0, 10)
dist = d1(pos) + d2(pos)
n = jax.grad(d1)(pos) - jax.grad(d2)(pos)
pos = info2.mat @ pos + info2.pos # d2 to global frame
n = info2.mat @ n
return dist, pos, math.make_frame(n)
@@ -240,4 +251,3 @@ def cylinder_cylinder(c1: GeomInfo, c2: GeomInfo) -> Collision:
])
optim_ = functools.partial(_optim, _cylinder, _cylinder, c1, c2)
return jax.vmap(optim_)(x0)
mjx/mujoco/mjx/_src/collision_types.py
+42 -28
View File
@@ -15,31 +15,10 @@
"""Collision base types."""
import dataclasses
from typing import Tuple
from typing import Optional, Tuple
import jax
# pylint: disable=g-importing-member
from mujoco.mjx._src.dataclasses import PyTreeNode
# pylint: enable=g-importing-member
class GeomInfo(PyTreeNode):
"""Geom propertes of primitive and SDF shapes."""
pos: jax.Array
mat: jax.Array
size: jax.Array
class ConvexInfo(PyTreeNode):
"""Geom propertes of convex meshes."""
pos: jax.Array
mat: jax.Array
vert: jax.Array
face: jax.Array
face_normal: jax.Array
edge: jax.Array
edge_face_normal: jax.Array
edge_dir: jax.Array
from mujoco.mjx._src.dataclasses import PyTreeNode # pylint: disable=g-importing-member
import numpy as np
# Collision returned by collision functions:
# - distance distance between nearest points; neg: penetration
@@ -48,18 +27,53 @@ class ConvexInfo(PyTreeNode):
Collision = Tuple[jax.Array, jax.Array, jax.Array]
class GeomInfo(PyTreeNode):
"""Geom properties for primitive shapes."""
pos: jax.Array
mat: jax.Array
size: jax.Array
class ConvexInfo(PyTreeNode):
"""Geom properties for convex meshes."""
pos: jax.Array
mat: jax.Array
size: jax.Array
vert: jax.Array
face: jax.Array
face_normal: jax.Array
edge: jax.Array
edge_face_normal: jax.Array
edge_dir: Optional[jax.Array] = None
class HFieldInfo(PyTreeNode):
"""Geom properties for height fields."""
pos: jax.Array
mat: jax.Array
size: np.ndarray
nrow: int
ncol: int
data: jax.Array
@dataclasses.dataclass(frozen=True)
class FunctionKey:
"""Specifies how geom pairs group into collision_driver's function table.
Attributes:
types: geom type pair, which determines the collision function
data_ids: geom data id pair: mesh id for mesh geoms, otherwise -1.
Meshes have distinct face/vertex counts, so must occupy distinct
entries in the collision function table.
data_ids: geom data id pair: mesh id for mesh geoms, otherwise -1. Meshes
have distinct face/vertex counts, so must occupy distinct entries in the
collision function table.
condim: grouping by condim of the colliision ensures that the size of the
resulting constraint jacobian is determined at compile time.
resulting constraint jacobian is determined at compile time.
subgrid_size: the size determines the hfield subgrid to collide with
"""
types: Tuple[int, int]
data_ids: Tuple[int, int]
condim: int
subgrid_size: Tuple[int, int] = (-1, -1)
mjx/mujoco/mjx/_src/constraint.py
+2 -2
View File
@@ -73,8 +73,8 @@ def _kbi(
k = 1 / (dmax * dmax * timeconst * timeconst * dampratio * dampratio)
b = 2 / (dmax * timeconst)
# TODO(robotics-simulation): check various solparam settings in model gen test
k = jp.where(dampratio <= 0, -dampratio / (dmax * dmax), k)
b = jp.where(timeconst <= 0, -timeconst / dmax, b)
k = jp.where(dampratio <= 0, -solref[0] / (dmax * dmax), k)
b = jp.where(timeconst <= 0, -solref[1] / dmax, b)
imp_x = jp.abs(pos) / width
imp_a = (1.0 / jp.power(mid, power - 1)) * jp.power(imp_x, power)
mjx/mujoco/mjx/_src/constraint_test.py
+4 -4
View File
@@ -104,10 +104,10 @@ class ConstraintTest(absltest.TestCase):
ne, nf, nl, nc = constraint.counts(constraint.make_efc_type(m))
self.assertEqual(ne, 0)
self.assertEqual(nf, 0)
self.assertEqual(nl, 2)
self.assertEqual(nl, 3)
self.assertEqual(nc, 148)
dx = constraint.make_constraint(mjx.put_model(m), mjx.make_data(m))
self.assertEqual(dx.efc_J.shape[0], 150) # only joint range, contact
self.assertEqual(dx.efc_J.shape[0], 151) # only joint range, contact
def test_disable_contact(self):
m = test_util.load_test_file('constraints.xml')
@@ -115,10 +115,10 @@ class ConstraintTest(absltest.TestCase):
ne, nf, nl, nc = constraint.counts(constraint.make_efc_type(m))
self.assertEqual(ne, 10)
self.assertEqual(nf, 0)
self.assertEqual(nl, 2)
self.assertEqual(nl, 3)
self.assertEqual(nc, 0)
dx = constraint.make_constraint(mjx.put_model(m), mjx.make_data(m))
self.assertEqual(dx.efc_J.shape[0], 12) # only joint range, limit
self.assertEqual(dx.efc_J.shape[0], 13) # only joint range, limit
if __name__ == '__main__':
mjx/mujoco/mjx/_src/dataclasses.py
+1 -1
View File
@@ -30,7 +30,7 @@ def _jax_in_args(typ) -> bool:
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):
if typing.get_origin(typ) in (tuple, list, dict, Union, set):
return any(_jax_in_args(t) for t in typing.get_args(typ))
return False
mjx/mujoco/mjx/_src/io.py
+21 -3
View File
@@ -22,6 +22,7 @@ from jax import numpy as jp
import mujoco
from mujoco.mjx._src import collision_driver
from mujoco.mjx._src import constraint
from mujoco.mjx._src import mesh
from mujoco.mjx._src import support
from mujoco.mjx._src import types
import numpy as np
@@ -68,14 +69,16 @@ def put_model(m: mujoco.MjModel, device=None) -> types.Model:
if m.ntendon:
raise NotImplementedError('tendons are not supported')
mesh_geomid = set()
for g1, g2, ip in collision_driver.geom_pairs(m):
t1, t2 = m.geom_type[[g1, g2]]
# check collision function exists for type pair
if not collision_driver.has_collision_fn(t1, t2):
t1, t2 = mujoco.mjtGeom(t1), mujoco.mjtGeom(t2)
raise NotImplementedError(f'({t1}, {t2}) collisions not implemented.')
# margin/gap not supported for geoms
if mujoco.mjtGeom.mjGEOM_MESH in (t1, t2):
# margin/gap not supported for meshes and height fields
no_margin = {mujoco.mjtGeom.mjGEOM_MESH, mujoco.mjtGeom.mjGEOM_HFIELD}
if no_margin.intersection({t1, t2}):
if ip != -1:
margin = m.pair_margin[ip]
else:
@@ -83,6 +86,9 @@ def put_model(m: mujoco.MjModel, device=None) -> types.Model:
if margin.any():
t1, t2 = mujoco.mjtGeom(t1), mujoco.mjtGeom(t2)
raise NotImplementedError(f'({t1}, {t2}) margin/gap not implemented.')
for t, g in [(t1, g1), (t2, g2)]:
if t == mujoco.mjtGeom.mjGEOM_MESH:
mesh_geomid.add(g)
for enum_field, enum_type, mj_type in (
(m.actuator_biastype, types.BiasType, mujoco.mjtBias),
@@ -100,12 +106,24 @@ def put_model(m: mujoco.MjModel, device=None) -> types.Model:
if not np.allclose(m.dof_frictionloss, 0):
raise NotImplementedError('dof_frictionloss is not implemented.')
fields = {f.name: getattr(m, f.name) for f in types.Model.fields()}
mjx_only = {'mesh_convex', 'geom_rbound_hfield'}
mj_field_names = {f.name for f in types.Model.fields()} - mjx_only
fields = {f: getattr(m, f) for f in mj_field_names}
fields['geom_rbound_hfield'] = fields['geom_rbound']
fields['geom_rgba'] = fields['geom_rgba'].reshape((-1, 4))
fields['mat_rgba'] = fields['mat_rgba'].reshape((-1, 4))
fields['cam_mat0'] = fields['cam_mat0'].reshape((-1, 3, 3))
fields['opt'] = _make_option(m.opt)
fields['stat'] = _make_statistic(m.stat)
# Pre-compile meshes for MJX collisions.
fields['mesh_convex'] = [None] * m.nmesh
for i in mesh_geomid:
dataid = m.geom_dataid[i]
if fields['mesh_convex'][dataid] is None:
fields['mesh_convex'][dataid] = mesh.convex(m, dataid) # pytype: disable=unsupported-operands
fields['mesh_convex'] = tuple(fields['mesh_convex'])
model = types.Model(**{k: copy.copy(v) for k, v in fields.items()})
return jax.device_put(model, device=device)
mjx/mujoco/mjx/_src/mesh.py
+98 -15
View File
@@ -16,14 +16,18 @@
import collections
import itertools
from typing import Tuple
from typing import Tuple, Union
import warnings
import jax
from jax import numpy as jp
import mujoco
from mujoco.mjx._src import math
# pylint: disable=g-importing-member
from mujoco.mjx._src.collision_types import ConvexInfo
from mujoco.mjx._src.collision_types import GeomInfo
from mujoco.mjx._src.collision_types import HFieldInfo
from mujoco.mjx._src.types import ConvexMesh
from mujoco.mjx._src.types import Model
# pylint: enable=g-importing-member
import numpy as np
@@ -141,7 +145,9 @@ def _convex_hull_2d(points: np.ndarray, normal: np.ndarray) -> np.ndarray:
return hull_point_idx
def _merge_coplanar(m: Model, tm: trimesh.Trimesh, meshid: int) -> np.ndarray:
def _merge_coplanar(
m: Union[mujoco.MjModel, Model], tm: trimesh.Trimesh, meshid: int
) -> np.ndarray:
"""Merges coplanar facets."""
if not tm.facets:
return tm.faces.copy() # no facets
@@ -221,6 +227,7 @@ def box(info: GeomInfo) -> ConvexInfo:
c = ConvexInfo(
info.pos,
info.mat,
info.size,
vert,
face,
face_normal,
@@ -236,22 +243,21 @@ def box(info: GeomInfo) -> ConvexInfo:
return c
def convex(m: Model, mesh_id: int, info: GeomInfo) -> ConvexInfo:
def convex(m: Union[mujoco.MjModel, Model], data_id: int) -> ConvexMesh:
"""Processes a mesh for use in convex collision algorithms.
Args:
m: an MJX model
mesh_id: the mesh id to process
info: pos, mat, size of this geom
data_id: the mesh id to process
Returns:
a convex mesh info
a convex mesh
"""
vert_beg = m.mesh_vertadr[mesh_id]
vert_end = m.mesh_vertadr[mesh_id + 1] if mesh_id < m.nmesh - 1 else None
vert_beg = m.mesh_vertadr[data_id]
vert_end = m.mesh_vertadr[data_id + 1] if data_id < m.nmesh - 1 else None
vert = m.mesh_vert[vert_beg:vert_end]
graphadr = m.mesh_graphadr[mesh_id]
graphadr = m.mesh_graphadr[data_id]
graph = m.mesh_graph[graphadr:]
graph_idx = 0
@@ -273,21 +279,98 @@ def convex(m: Model, mesh_id: int, info: GeomInfo) -> ConvexInfo:
tm_convex = trimesh.Trimesh(vertices=vert, faces=face)
vert = np.array(tm_convex.vertices)
face = _merge_coplanar(m, tm_convex, mesh_id)
face = _merge_coplanar(m, tm_convex, data_id)
face_normal = _get_face_norm(vert, face)
edge, edge_face_normal = _get_edge_normals(face, face_normal)
edge_dir = _get_unique_edge_dir(vert, face)
face = vert[face] # materialize full nface x nvert matrix
c = ConvexInfo(
info.pos,
info.mat,
c = ConvexMesh(
vert,
face,
face_normal,
edge,
edge_face_normal,
edge_dir,
)
return jax.tree_util.tree_map(jp.array, c)
def hfield_prism(vert: jax.Array) -> ConvexInfo:
"""Builds a hfield prism."""
# The first 3 vertices define the bottom triangle, and the next 3 vertices
# define the top triangle. The remaining triangles define the side of the
# prism.
face = np.array([
[0, 1, 2, 0], # bottom
[3, 4, 5, 3], # top
[0, 3, 5, 1],
[0, 2, 4, 3],
[2, 1, 5, 4],
])
edges = np.array([
# bottom
[0, 1],
[1, 2],
[0, 2],
# top
[3, 4],
[3, 5],
[4, 5],
# sides
[0, 3],
[1, 5],
[2, 4],
])
edge_face_norm = np.array([
# bottom
[0, 2],
[0, 4],
[0, 3],
# top
[1, 3],
[1, 2],
[1, 4],
# sides
[2, 3],
[2, 4],
[3, 4],
])
def get_face_norm(face):
# use ccw winding order convention, and avoid using the last vertex
edge0 = face[2, :] - face[1, :]
edge1 = face[0, :] - face[1, :]
return math.normalize(jp.cross(edge0, edge1))
centroid = jp.mean(vert, axis=0)
vert = vert - centroid
face = vert[face]
face_norm = jax.vmap(get_face_norm)(face)
c = ConvexInfo(
centroid,
jp.eye(3, dtype=float),
jp.ones(3),
vert,
face,
face_norm,
edges,
face_norm[edge_face_norm],
None,
)
return jax.tree_util.tree_map(jp.array, c)
def hfield(m: Union[mujoco.MjModel, Model], data_id: int) -> HFieldInfo:
adr = m.hfield_adr[data_id]
nrow, ncol = m.hfield_nrow[data_id], m.hfield_ncol[data_id]
h = HFieldInfo(
jp.zeros(3, dtype=float),
jp.eye(3, dtype=float),
m.hfield_size[data_id],
nrow,
ncol,
m.hfield_data[adr : adr + nrow * ncol].reshape((ncol, nrow), order='F'),
)
return h
mjx/mujoco/mjx/_src/support.py
+42
View File
@@ -197,6 +197,7 @@ def _getnum(m: Union[Model, mujoco.MjModel], obj: mujoco._enums.mjtObj) -> int:
mujoco.mjtObj.mjOBJ_SITE: m.nsite,
mujoco.mjtObj.mjOBJ_CAMERA: m.ncam,
mujoco.mjtObj.mjOBJ_MESH: m.nmesh,
mujoco.mjtObj.mjOBJ_HFIELD: m.nhfield,
mujoco.mjtObj.mjOBJ_PAIR: m.npair,
mujoco.mjtObj.mjOBJ_EQUALITY: m.neq,
mujoco.mjtObj.mjOBJ_ACTUATOR: m.nu,
@@ -218,6 +219,7 @@ def _getadr(
mujoco.mjtObj.mjOBJ_SITE: m.name_siteadr,
mujoco.mjtObj.mjOBJ_CAMERA: m.name_camadr,
mujoco.mjtObj.mjOBJ_MESH: m.name_meshadr,
mujoco.mjtObj.mjOBJ_HFIELD: m.name_hfieldadr,
mujoco.mjtObj.mjOBJ_PAIR: m.name_pairadr,
mujoco.mjtObj.mjOBJ_EQUALITY: m.name_eqadr,
mujoco.mjtObj.mjOBJ_ACTUATOR: m.name_actuatoradr,
@@ -277,3 +279,43 @@ def name2id(
}
return names_map.get(name, -1)
def _decode_pyramid(
pyramid: jax.Array, mu: jax.Array, condim: int
) -> jax.Array:
"""Converts pyramid representation to contact force."""
force = jp.zeros(6, dtype=float)
if condim == 1:
return force.at[0].set(pyramid[0])
# force_normal = sum(pyramid0_i + pyramid1_i)
force = force.at[0].set(pyramid[0 : 2 * (condim - 1)].sum())
# force_tangent_i = (pyramid0_i - pyramid1_i) * mu_i
i = np.arange(0, condim)
force = force.at[i + 1].set((pyramid[2 * i] - pyramid[2 * i + 1]) * mu[i])
return force
def contact_force(
m: Model, d: Data, contact_id: int, to_world_frame: bool = False
) -> jax.Array:
"""Extract 6D force:torque for one contact, in contact frame by default."""
efc_address = d.contact.efc_address[contact_id]
condim = d.contact.dim[contact_id]
if m.opt.cone == mujoco.mjtCone.mjCONE_PYRAMIDAL:
force = _decode_pyramid(
d.efc_force[efc_address:], d.contact.friction[contact_id], condim
)
elif m.opt.cone == mujoco.mjtCone.mjCONE_ELLIPTIC:
raise NotImplementedError('Elliptic cone force is not implemented yet.')
else:
raise ValueError(f'Unknown cone type: {m.opt.cone}')
if to_world_frame:
force = force.reshape((-1, 3)) @ d.contact.frame[contact_id]
force = force.reshape(-1)
return force * (efc_address >= 0)
mjx/mujoco/mjx/_src/support_test.py
+59
View File
@@ -157,6 +157,65 @@ class SupportTest(parameterized.TestCase):
i = i if n is not None else -1
self.assertEqual(support.name2id(mx, obj, n), i)
_CONTACTS = """
<mujoco>
<worldbody>
<body pos="0 0 0.55" euler="1 0 0">
<joint axis="1 0 0" type="free"/>
<geom fromto="-0.4 0 0 0.4 0 0" size="0.05" type="capsule" condim="6"/>
</body>
<body pos="0 0 0.5" euler="0 1 0">
<joint axis="1 0 0" type="free"/>
<geom fromto="-0.4 0 0 0.4 0 0" size="0.05" type="capsule" condim="3"/>
</body>
<body pos="0 0 0.445" euler="0 90 0">
<joint axis="1 0 0" type="free"/>
<geom fromto="-0.4 0 0 0.4 0 0" size="0.05" type="capsule" condim="1"/>
</body>
</worldbody>
</mujoco>
"""
def test_contact_force(self):
m = mujoco.MjModel.from_xml_string(self._CONTACTS)
d = mujoco.MjData(m)
mujoco.mj_step(m, d)
assert (
np.unique(d.contact.geom).shape[0] == 3
), 'This test assumes all capsule are in contact.'
mx = mjx.put_model(m)
dx = mjx.put_data(m, d)
mujoco.mj_step(m, d)
dx = mjx.step(mx, dx)
# map MJX contacts to MJ ones
def _find(g):
val = (g == dx.contact.geom).sum(axis=1)
return np.where(val == 2)[0][0]
contact_id_map = {i: _find(d.contact.geom[i]) for i in range(d.ncon)}
for i in range(d.ncon):
result = np.zeros(6, dtype=float)
mujoco.mj_contactForce(m, d, i, result)
j = contact_id_map[i]
force = jax.jit(support.contact_force, static_argnums=(2,))(mx, dx, j)
np.testing.assert_allclose(result, force, rtol=1e-5, atol=2)
# test world conversion
force = jax.jit(
support.contact_force,
static_argnums=(
2,
3,
),
)(mx, dx, j, True)
# back to contact frame
force = force.at[:3].set(dx.contact.frame[j] @ force[:3])
force = force.at[3:].set(dx.contact.frame[j] @ force[3:])
np.testing.assert_allclose(result, force, rtol=1e-5, atol=2)
if __name__ == '__main__':
absltest.main()
mjx/mujoco/mjx/_src/types.py
+36 -1
View File
@@ -15,7 +15,7 @@
"""Base types used in MJX."""
import enum
from typing import Tuple
import jax
import mujoco
from mujoco.mjx._src.dataclasses import PyTreeNode # pylint: disable=g-importing-member
@@ -112,6 +112,24 @@ class GeomType(enum.IntEnum):
# unsupported: NGEOMTYPES, ARROW*, LINE, SKIN, LABEL, NONE
class ConvexMesh(PyTreeNode):
"""Geom properties for convex meshes.
Attributes:
vert: vertices of the convex mesh
face: faces of the convex mesh
face_normal: normal vectors for the faces
edge: edge indexes for all edges in the convex mesh
edge_face_normal: indexes for face normals adjacent to edges in `edge`
"""
vert: jax.Array
face: jax.Array
face_normal: jax.Array
edge: jax.Array
edge_face_normal: jax.Array
class ConeType(enum.IntEnum):
"""Type of friction cone.
@@ -322,6 +340,7 @@ class Model(PyTreeNode):
nmesh: number of meshes
nmeshvert: number of vertices in all meshes
nmeshface: number of triangular faces in all meshes
nhfield: number of heightfields
nmat: number of materials
npair: number of predefined geom pairs
nexclude: number of excluded geom pairs
@@ -396,6 +415,7 @@ class Model(PyTreeNode):
geom_solimp: constraint solver impedance: contact (ngeom, mjNIMP)
geom_size: geom-specific size parameters (ngeom, 3)
geom_rbound: radius of bounding sphere (ngeom,)
geom_rbound_hfield: static rbound for hfield grid bounds (ngeom,)
geom_pos: local position offset rel. to body (ngeom, 3)
geom_quat: local orientation offset rel. to body (ngeom, 4)
geom_friction: friction for (slide, spin, roll) (ngeom, 3)
@@ -419,6 +439,12 @@ class Model(PyTreeNode):
mesh_vert: vertex positions for all meshes (nmeshvert, 3)
mesh_face: vertex face data (nmeshface, 3)
mesh_graph: convex graph data (nmeshgraph,)
mesh_convex: pre-compiled convex mesh info for MJX (nmesh,)
hfield_size: (x, y, z_top, z_bottom) (nhfield,)
hfield_nrow: number of rows in grid (nhfield,)
hfield_ncol: number of columns in grid (nhfield,)
hfield_adr: address in hfield_data (nhfield,)
hfield_data: elevation data (nhfielddata,)
mat_rgba: rgba (nmat, 4)
pair_dim: contact dimensionality (npair,)
pair_geom1: id of geom1 (npair,)
@@ -488,6 +514,7 @@ class Model(PyTreeNode):
nmesh: int
nmeshvert: int
nmeshface: int
nhfield: int
nmat: int
npair: int
nexclude: int
@@ -561,6 +588,7 @@ class Model(PyTreeNode):
geom_solimp: jax.Array
geom_size: jax.Array
geom_rbound: jax.Array
geom_rbound_hfield: np.ndarray
geom_pos: jax.Array
geom_quat: jax.Array
geom_friction: jax.Array
@@ -584,6 +612,12 @@ class Model(PyTreeNode):
mesh_vert: np.ndarray
mesh_face: np.ndarray
mesh_graph: np.ndarray
mesh_convex: Tuple[ConvexMesh, ...]
hfield_size: np.ndarray
hfield_nrow: np.ndarray
hfield_ncol: np.ndarray
hfield_adr: np.ndarray
hfield_data: jax.Array
mat_rgba: np.ndarray
pair_dim: np.ndarray
pair_geom1: np.ndarray
@@ -632,6 +666,7 @@ class Model(PyTreeNode):
name_siteadr: np.ndarray
name_camadr: np.ndarray
name_meshadr: np.ndarray
name_hfieldadr: np.ndarray
name_pairadr: np.ndarray
name_eqadr: np.ndarray
name_actuatoradr: np.ndarray
mjx/mujoco/mjx/test_data/barkour_v0/assets/barkour_v0_mjx.xml
+3 -1
View File
@@ -77,6 +77,7 @@
</custom>
<asset>
<hfield name="rand_uniform" file="hfield_240_280.png" size="20.1 20.0 .05 .1"/>
<mesh file="head.stl"/>
<mesh file="powercable.stl"/>
<mesh file="handle.stl"/>
@@ -101,7 +102,8 @@
<site name="origin"/>
<light pos="0 0 1.5" dir="0 0 -1" directional="true"/>
<camera name="default" pos="0.846 -1.465 0.916" xyaxes="0.866 0.500 0.000 -0.171 0.296 0.940"/>
<geom name="floor" size="0 0 0.05" type="plane" conaffinity="1" material="groundplane"/>
<geom name="hfield_floor" type="hfield" hfield="rand_uniform" conaffinity="1"/>
<!-- <geom name="floor" pos="0 0 -0.05" size="0 0 0.05" type="plane" conaffinity="1" material="groundplane"/> -->
<body name="chassis">
<camera name="track" pos="0.846 -1.465 0.916" xyaxes="0.866 0.500 0.000 -0.171 0.296 0.940" mode="trackcom"/>
<freejoint/>
mjx/mujoco/mjx/test_data/barkour_v0/assets/hfield_240_280.png
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 170 KiB

mjx/mujoco/mjx/test_data/constraints.xml
+5
View File
@@ -38,6 +38,11 @@
<geom class="box"/>
</body>
<body name="beam5" pos="-2 0 0" euler="45 45 0">
<joint name="joint5" type="ball" range="0 45" solreflimit="-100 -1"/> <!-- test negative solref -->
<geom class="box" contype="0" conaffinity="0"/>
</body>
<body name="box_condim1" pos="4 0 0">
<freejoint/>
<geom class="box" condim="1"/>
mjx/mujoco/mjx/test_data/convex.xml
-4
View File
@@ -1,8 +1,4 @@
<mujoco model="convex_convex">
<custom>
<numeric data="0.2" name="baumgarte_erp"/>
<numeric data="0.5" name="elasticity"/>
</custom>
<default>
<geom friction="0.5 0.0 0.0"/>
</default>
mjx/pyproject.toml
+4 -4
View File
@@ -4,7 +4,7 @@ build-backend = "setuptools.build_meta"
[project]
name="mujoco-mjx"
version = "3.1.5"
version = "3.1.6"
authors = [
{name = "Google DeepMind", email = "mujoco@deepmind.com"},
]
@@ -31,7 +31,7 @@ dependencies = [
"etils[epath]",
"jax",
"jaxlib",
"mujoco>=3.1.5.dev0",
"mujoco>=3.1.6.dev0",
"scipy",
"trimesh",
]
@@ -42,6 +42,6 @@ mjx-viewer = "mujoco.mjx.viewer:main"
[project.urls]
Homepage = "https://github.com/google-deepmind/mujoco/tree/main/mjx"
Documentation = "https://mujoco.readthedocs.io/en/3.1.5"
Documentation = "https://mujoco.readthedocs.io/en/3.1.6"
Repository = "https://github.com/google-deepmind/mujoco/tree/main/mjx"
Changelog = "https://mujoco.readthedocs.io/en/3.1.5/changelog.html"
Changelog = "https://mujoco.readthedocs.io/en/3.1.6/changelog.html"
mjx/requirements.txt
+6 -3
View File
@@ -32,7 +32,8 @@ jaxlib==0.4.18; python_version >= '3.9' \
--hash=sha256:f7787a5531d226d6cc9ec2baa7141260bb713435e1cfc053cb9f5cefa9756ac3 \
--hash=sha256:6cb20bbbdafd90e71ad0deb9295519a0175c108c8c557b84fb9fe94f751daee4 \
--hash=sha256:116a0d6aedd3e856b52493d7e392fb1b40952b84fb72448fde1c1ab5687db667 \
--hash=sha256:9593ff69f424947567e206f3e356b2a2df55ca68e6d815d5adc6cae308e8f652
--hash=sha256:9593ff69f424947567e206f3e356b2a2df55ca68e6d815d5adc6cae308e8f652 \
--hash=sha256:2b17b3f05b3bbf8e0ddb85fba339525ac03bac21c9f26d0f83dcea1b1654353e
pip==23.3.1 \
--hash=sha256:55eb67bb6171d37447e82213be585b75fe2b12b359e993773aca4de9247a052b
pytest==7.4.2 \
@@ -66,7 +67,8 @@ scipy==1.11.3; python_version >= '3.9' \
--hash=sha256:9ea7f579182d83d00fed0e5c11a4aa5ffe01460444219dedc448a36adf0c3917 \
--hash=sha256:5305792c7110e32ff155aed0df46aa60a60fc6e52cd4ee02cdeb67eaccd5356e \
--hash=sha256:a63d1ec9cadecce838467ce0631c17c15c7197ae61e49429434ba01d618caa83 \
--hash=sha256:715c9966eb8906bc67e450e962bd07a5254420077178f98258904da4004a172f
--hash=sha256:715c9966eb8906bc67e450e962bd07a5254420077178f98258904da4004a172f \
--hash=sha256:d4d88a6fc091614b842a739b3db6ae15f95c77b308113bd6daefd4b05539b103
setuptools==68.2.2 \
--hash=sha256:b454a35605876da60632df1a60f736524eb73cc47bbc9f3f1ef1b644de74fd2a
trimesh==4.0.0 \
@@ -135,7 +137,8 @@ numpy==1.26.0; python_version >= '3.9' \
--hash=sha256:914b28d3215e0c721dc75db3ad6d62f51f630cb0c277e6b3bcb39519bed10bd8 \
--hash=sha256:c78a22e95182fb2e7874712433eaa610478a3caf86f28c621708d35fa4fd6e7f \
--hash=sha256:86f737708b366c36b76e953c46ba5827d8c27b7a8c9d0f471810728e5a2fe57c \
--hash=sha256:020cdbee66ed46b671429c7265cf00d8ac91c046901c55684954c3958525dab2
--hash=sha256:020cdbee66ed46b671429c7265cf00d8ac91c046901c55684954c3958525dab2 \
--hash=sha256:d6fa6d17727169ff1385ad3cb8f290bbcc3f2097322d90507c1956a4f9f870fc
opt-einsum==3.3.0 \
--hash=sha256:2455e59e3947d3c275477df7f5205b30635e266fe6dc300e3d9f9646bfcea147
mjx/training_apg.ipynb
+33
View File
@@ -1,6 +1,39 @@
{
"cells": [
{
"cell_type": "markdown",
"metadata": {
"id": "MpkYHwCqk7W-"
},
"source": [
"![MuJoCo banner](https://raw.githubusercontent.com/google-deepmind/mujoco/main/banner.png)\n",
"\n",
"# <h1><center>Tutorial <a href=\"https://colab.research.google.com/github/google-deepmind/mujoco/blob/main/mjx/training_apg.ipynb\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" width=\"140\" align=\"center\"/></a></center></h1>\n",
"\n",
"This notebook provides a tutorial for differentiable physics for policy learning in [**MuJoCo XLA (MJX)**](https://github.com/google-deepmind/mujoco/blob/main/mjx), a JAX-based implementation of MuJoCo.\n",
"\n",
"**A Colab runtime with GPU acceleration is required.** If you're using a CPU-only runtime, you can switch using the menu \"Runtime > Change runtime type\".\n",
"\n",
"\n",
"This notebook was written by [Jing Yuan Luo](https://github.com/Andrew-Luo1).\n",
"\n",
"\u003c!-- Copyright 2021 DeepMind Technologies Limited\n",
"\n",
" Licensed under the Apache License, Version 2.0 (the \"License\");\n",
" you may not use this file except in compliance with the License.\n",
" You may obtain a copy of the License at\n",
"\n",
" http://www.apache.org/licenses/LICENSE-2.0\n",
"\n",
" Unless required by applicable law or agreed to in writing, software\n",
" distributed under the License is distributed on an \"AS IS\" BASIS,\n",
" WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n",
" See the License for the specific language governing permissions and\n",
" limitations under the License.\n",
"--\u003e"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
mjx/tutorial.ipynb
+2 -2
View File
@@ -977,7 +977,7 @@
" path = epath.Path('mujoco_menagerie/google_barkour_vb/scene_mjx.xml')\n",
" sys = mjcf.load(path.as_posix())\n",
" self._dt = 0.02 # this environment is 50 fps\n",
" sys = sys.tree_replace({'opt.timestep': 0.004, 'dt': 0.004})\n",
" sys = sys.tree_replace({'opt.timestep': 0.004})\n",
"\n",
" # override menagerie params for smoother policy\n",
" sys = sys.replace(\n",
@@ -1448,7 +1448,7 @@
},
"outputs": [],
"source": [
"HTML(html.render(eval_env.sys.replace(dt=eval_env.dt), rollout))"
"HTML(html.render(eval_env.sys.tree_replace({'opt.timestep': eval_env.dt}), rollout))"
]
}
],
plugin/elasticity/solid.cc
+3
View File
@@ -123,6 +123,9 @@ Solid::Solid(const mjModel* m, mjData* d, int instance, mjtNum nu, mjtNum E,
if (m->flex_vertbodyid[m->flex_vertadr[i]+j] == i0) {
f0 = i;
nv = m->flex_vertnum[f0];
if (m->flex_dim[i] != 3) { // SHOULD NOT OCCUR
mju_error("mujoco.elasticity.solid requires a 3D mesh");
}
}
}
}
plugin/sdf/sdf.cc
+1 -1
View File
@@ -38,7 +38,7 @@ SdfVisualizer::SdfVisualizer() {
}
void SdfVisualizer::AddPoint(const mjtNum point[3]) {
if (!npoints_.empty()) {
if (!npoints_.empty() && npoints_.back() < points_.size()/3) {
points_[3*npoints_.back()+0] = point[0];
points_[3*npoints_.back()+1] = point[1];
points_[3*npoints_.back()+2] = point[2];
python/LQR.ipynb
+16 -22
View File
@@ -10,27 +10,22 @@
"\n",
"# <h1><center>LQR tutorial <a href=\"https://colab.research.google.com/github/google-deepmind/mujoco/blob/main/python/LQR.ipynb\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" width=\"140\" align=\"center\"/></a></center></h1>\n",
"\n",
"This notebook provides an example of an LQR controller using [**MuJoCo** physics](https://github.com/google-deepmind/mujoco#readme)."
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "LBAvTJ0xHKy7"
},
"source": [
"### Copyright notice"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "_UbO9uhtBSX5"
},
"source": [
"> <p><small><small>Copyright 2022 DeepMind Technologies Limited</small></p>\n",
"> <p><small><small>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 <a href=\"http://www.apache.org/licenses/LICENSE-2.0\">http://www.apache.org/licenses/LICENSE-2.0</a>.</small></small></p>\n",
"> <p><small><small>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.</small></small></p>"
"This notebook provides an example of an LQR controller using [**MuJoCo** physics](https://github.com/google-deepmind/mujoco#readme).\n",
"\n",
"<!-- Copyright 2021 DeepMind Technologies Limited\n",
"\n",
" Licensed under the Apache License, Version 2.0 (the \"License\");\n",
" you may not use this file except in compliance with the License.\n",
" You may obtain a copy of the License at\n",
"\n",
" http://www.apache.org/licenses/LICENSE-2.0\n",
"\n",
" Unless required by applicable law or agreed to in writing, software\n",
" distributed under the License is distributed on an \"AS IS\" BASIS,\n",
" WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n",
" See the License for the specific language governing permissions and\n",
" limitations under the License.\n",
"-->"
]
},
{
@@ -935,7 +930,6 @@
"accelerator": "GPU",
"colab": {
"collapsed_sections": [
"LBAvTJ0xHKy7",
"QPdJNe3k62mx"
],
"private_outputs": true
python/build_requirements.txt
+3 -1
View File
@@ -40,7 +40,9 @@ numpy==1.26.0; python_version >= '3.9' \
--hash=sha256:914b28d3215e0c721dc75db3ad6d62f51f630cb0c277e6b3bcb39519bed10bd8 \
--hash=sha256:c78a22e95182fb2e7874712433eaa610478a3caf86f28c621708d35fa4fd6e7f \
--hash=sha256:86f737708b366c36b76e953c46ba5827d8c27b7a8c9d0f471810728e5a2fe57c \
--hash=sha256:020cdbee66ed46b671429c7265cf00d8ac91c046901c55684954c3958525dab2
--hash=sha256:020cdbee66ed46b671429c7265cf00d8ac91c046901c55684954c3958525dab2 \
--hash=sha256:d6fa6d17727169ff1385ad3cb8f290bbcc3f2097322d90507c1956a4f9f870fc \
--hash=sha256:79d9c3c363cbd919d879dd38aeb19d96be8699eda2af5f3f3c97bf774e1e6438
pip==23.3.1 \
--hash=sha256:55eb67bb6171d37447e82213be585b75fe2b12b359e993773aca4de9247a052b
PyOpenGL==3.1.7 \
python/least_squares.ipynb
+827 -635
View File
File diff suppressed because it is too large Load Diff
python/mujoco/CMakeLists.txt
+3 -3
View File
@@ -84,7 +84,7 @@ if(NOT TARGET mujoco)
if(MUJOCO_FRAMEWORK)
message("MuJoCo framework is at ${MUJOCO_FRAMEWORK}/mujoco.framework")
set(MUJOCO_LIBRARY
${MUJOCO_FRAMEWORK}/mujoco.framework/Versions/A/libmujoco.3.1.5.dylib
${MUJOCO_FRAMEWORK}/mujoco.framework/Versions/A/libmujoco.3.1.6.dylib
)
target_compile_options(mujoco INTERFACE -F${MUJOCO_FRAMEWORK})
endif()
@@ -92,7 +92,7 @@ if(NOT TARGET mujoco)
if(NOT MUJOCO_FRAMEWORK)
find_library(
MUJOCO_LIBRARY mujoco mujoco.3.1.5 HINTS ${MUJOCO_LIBRARY_DIR} REQUIRED
MUJOCO_LIBRARY mujoco mujoco.3.1.6 HINTS ${MUJOCO_LIBRARY_DIR} REQUIRED
)
find_path(MUJOCO_INCLUDE mujoco/mujoco.h HINTS ${MUJOCO_INCLUDE_DIR} REQUIRED)
message("MuJoCo is at ${MUJOCO_LIBRARY}")
@@ -140,7 +140,7 @@ findorfetch(
GIT_REPO
https://github.com/abseil/abseil-cpp
GIT_TAG
2f9e432cce407ce0ae50676696666f33a77d42ac # LTS 20240116.1
d7aaad83b488fd62bd51c81ecf16cd938532cc0a # LTS 20240116.2
TARGETS
${MUJOCO_PYTHON_ABSL_TARGETS}
EXCLUDE_FROM_ALL
python/mujoco/bindings_test.py
+8
View File
@@ -1194,6 +1194,14 @@ Euler integrator, semi-implicit in velocity.
mujoco.mjd_inverseFD(self.model, self.data, eps, flg_centered,
None, None, None, None, None, None, None)
def test_geom_distance(self):
mujoco.mj_forward(self.model, self.data)
fromto = np.empty(6, np.float64)
dist = mujoco.mj_geomDistance(self.model, self.data, 0, 2, 200, fromto)
self.assertEqual(dist, 41.9)
np.testing.assert_array_equal(fromto,
np.array((42., 0., 0., 42., 0., 41.9)))
def test_inverse_fd(self):
eps = 1e-6
flg_centered = 0
python/mujoco/functions.cc
+12
View File
@@ -531,6 +531,18 @@ PYBIND11_MODULE(_functions, pymodule) {
Def<traits::mj_objectVelocity>(pymodule);
Def<traits::mj_objectAcceleration>(pymodule);
Def<traits::mj_contactForce>(pymodule);
Def<traits::mj_geomDistance>(
pymodule,
[](const raw::MjModel* m, const raw::MjData* d,
int geom1, int geom2, mjtNum distmax,
std::optional<Eigen::Ref<EigenArrayXX>> fromto) {
if (fromto.has_value() && fromto->size() != 6) {
throw py::type_error("fromto should be of size 6");
}
return InterceptMjErrors(::mj_geomDistance)(
m, d, geom1, geom2, distmax,
fromto.has_value() ? fromto->data() : nullptr);
});
Def<traits::mj_differentiatePos>(
pymodule,
[](const raw::MjModel* m, Eigen::Ref<EigenVectorX> qvel,
python/mujoco/minimize.py
+240 -118
View File
@@ -14,10 +14,11 @@
# ==============================================================================
"""Nonlinear Least Squares minimization with box bounds."""
import abc
import dataclasses
import enum
import time
from typing import Callable, List, Optional, TextIO, Tuple, Union
from typing import Callable, List, Optional, Sequence, TextIO, Tuple, Union
import mujoco
import numpy as np
@@ -32,14 +33,14 @@ class Verbosity(enum.Enum):
class Status(enum.Enum):
FACTORIZATION_FAILED = enum.auto()
NO_IMPORVEMENT = enum.auto()
NO_IMPROVEMENT = enum.auto()
MAX_ITER = enum.auto()
DX_TOL = enum.auto()
_STATUS_MESSAGE = {
Status.FACTORIZATION_FAILED: 'factorization failed.',
Status.NO_IMPORVEMENT: 'insufficient reduction.',
Status.NO_IMPROVEMENT: 'insufficient reduction.',
Status.MAX_ITER: 'maximum iterations reached.',
Status.DX_TOL: 'norm(dx) < tol.',
}
@@ -68,124 +69,109 @@ class IterLog:
step: Optional[np.ndarray] = None
def jacobian_fd(
residual: Callable[[np.ndarray], np.ndarray],
x: np.ndarray,
r: np.ndarray,
eps: np.float64,
central: bool,
n_res: int,
bounds: Optional[List[np.ndarray]] = None,
):
"""Finite-difference Jacobian of a residual function.
class Norm(abc.ABC):
"""Abstract interface for norm functions, measuring the magnitude of vectors.
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.
central: whether to use central differences.
n_res: number or residual evaluations so far.
bounds: optional pair of lower and upper bounds.
Key Concepts:
Returns:
jac: Jacobian of the residual at x.
n_res: updated number of residual evaluations.
* Norm Value: The value of the norm for a given input vector.
* Gradient and Hessian: The gradient (first derivative) and Hessian (second
derivative) of the norm function with respect to the input vector.
Subclasses Must Implement:
* `value(self, r: np.ndarray)`: Computes and returns the norm value for the
input vector `r`.
* `grad_hess(self, r: np.ndarray, proj: np.ndarray)`: Computes and returns
both the gradient and Hessian of the norm at `r`, projected onto `proj`.
The reason we ask the user to perform the projection themselves is that
norm Hessians are often large and sparse, and the "sandwich" projection
operator `proj.T @ hess @ proj` can be computed efficiently by taking the
specific norm structure into account.
"""
nx = x.size
nr = r.size
jac = np.zeros((nr, nx))
xh = x.copy()
if bounds is None:
# No bounds, simple forward or central differencing.
for i in range(nx):
xh[i] = x[i] + eps
rp = residual(xh)
if central:
xh[i] = x[i] - eps
rm = residual(xh)
jac[:, i] = (rp - rm) / (2*eps)
else:
jac[:, i] = (rp - r) / eps
xh[i] = x[i]
n_res += 2*nx if central else nx
else:
lower, upper = bounds
midpoint = 0.5 * (upper - lower)
for i in range(nx):
# Scale eps, don't cross bounds.
eps_i = eps * (upper[i] - lower[i])
if central:
# Use central differencing if away from bounds.
if x[i] - eps_i < lower[i]:
# Near lower bound, use forward.
xh[i] = x[i] + eps_i
rp = residual(xh)
jac[:, i] = (rp - r) / eps_i
n_res += 1
elif x[i] + eps_i > upper[i]:
# Near upper bound, use backward.
xh[i] = x[i] - eps_i
rm = residual(xh)
jac[:, i] = (r - rm) / eps_i
n_res += 1
else:
# Use central.
xh[i] = x[i] + eps_i
rp = residual(xh)
xh[i] = x[i] - eps_i
rm = residual(xh)
jac[:, i] = (rp - rm) / (2*eps_i)
n_res += 2
else:
# Below midpoint use forward differencing, otherwise backward.
if x[i] < midpoint[i]:
xh[i] = x[i] + eps_i
rp = residual(xh)
jac[:, i] = (rp - r) / eps_i
else:
xh[i] = x[i] - eps_i
rm = residual(xh)
jac[:, i] = (r - rm) / eps_i
n_res += 1
# Reset.
xh[i] = x[i]
return jac, n_res
@abc.abstractmethod
def value(self, r: np.ndarray) -> np.float64:
"""Returns the value of the norm at the input vector `y = norm(r)`."""
pass
@abc.abstractmethod
def grad_hess(self, r: np.ndarray, proj: np.ndarray):
"""Computes the projected gradient and Hessian of the norm at `r`.
Args:
r: A NumPy column vector (nr x 1).
proj: A pre-computed projection matrix (nr x nx).
Returns:
A tuple containing:
* Projected gradient: proj.T @ (d_norm/d_r).
* Projected Hessian: proj.T @ (d^2_norm/d_r^2) @ proj.
"""
pass
class Quadratic(Norm):
"""Implementation of the quadratic norm."""
def value(self, r: np.ndarray):
"""Returns the quadratic norm of `r`."""
return 0.5 * (r.T @ r).item()
def grad_hess(self, r: np.ndarray, proj: np.ndarray):
"""Computes the projected gradient and Hessian of the quadratic norm at `r`.
Args:
r: A NumPy column vector (nr x 1).
proj: A pre-computed projection matrix (nr x nx).
Returns:
A tuple containing:
* Projected gradient: `proj.T @ r`.
* Projected Hessian: `proj.T @ proj`.
"""
grad = proj.T @ r
hess = proj.T @ proj # Notionally proj.T @ np.eye(r.size) @ proj
return grad, hess
def least_squares(
x0: np.ndarray,
residual: Callable[[np.ndarray], np.ndarray],
bounds: Optional[List[np.ndarray]] = None,
bounds: Optional[Sequence[np.ndarray]] = None,
jacobian: Optional[Callable[[np.ndarray, np.ndarray], np.ndarray]] = None,
norm: Norm = Quadratic(),
eps: float = 1e-6,
central: bool = False,
mu_min: float = 1e-6,
mu_max: float = 1e8,
mu_factor: float = 10.0**0.1,
tol: float = 1e-7,
tol: float = 1e-6,
max_iter: int = 100,
verbose: Union[Verbosity, int] = Verbosity.ITER,
output: Optional[TextIO] = None,
iter_callback: Optional[Callable[[List[IterLog]], None]] = None,
check_derivatives: bool = False,
) -> 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
x0: Initial guess
residual: Vectorized function returning the residual for 1 or more points.
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: perurbation used for automatic finite-differencing.
central: whether to use central differences.
mu_min: minimum value of the regularizer.
mu_max: maximum value of the regularizer.
mu_factor: 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.
norm: Norm object returning norm scalar or its projected gradient and
Hessian. See Norm class for detailed documentation.
eps: Perurbation used for automatic finite-differencing.
mu_min: Minimum value of the regularizer.
mu_max: Maximum value of the regularizer.
mu_factor: Factor for 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.
iter_callback: Optional iteration callback, takes trace argument.
check_derivatives: Compare user-defined Jacobian and norm against fin-diff.
Returns:
x: best solution found
@@ -202,10 +188,10 @@ def least_squares(
# Initialize locals.
status = Status.MAX_ITER
i = 0
x = x0.astype(np.float64)
n = x.size
xnew = np.zeros((n,))
dx = np.zeros((n,))
n = x0.size
x = x0.astype(np.float64).reshape((n, 1))
xnew = np.zeros((n, 1))
dx = np.zeros((n, 1))
scratch = np.zeros((n, n + 7))
eps = np.float64(eps)
mu = np.float64(0.0) # Optimistically start with no regularization.
@@ -234,6 +220,8 @@ def least_squares(
n_reduc = 0 # Reset n_reduc.
return mu, n_reduc
# Make local copy of bounds to avoid reshaping user input.
bounds = None if bounds is None else list(bounds)
if bounds is not None:
# Checks bounds.
if len(bounds) != 2:
@@ -244,7 +232,10 @@ def least_squares(
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.
# Reshape and clip.
bounds[0] = bounds[0].reshape(n, 1)
bounds[1] = bounds[1].reshape(n, 1)
np.clip(x, bounds[0], bounds[1], out=x)
# Check for NaNs.
@@ -267,21 +258,28 @@ def least_squares(
break
# Get objective y.
y = 0.5 * r.dot(r)
y = norm.value(r)
# Get Jacobian jac.
t_start = time.time()
if jacobian is None:
jac, n_res = jacobian_fd(residual, x, r, eps, central, n_res, bounds)
jac, n_res = jacobian_fd(residual, x, r, eps, n_res, bounds)
t_res += time.time() - t_start
else:
jac = jacobian(x, r)
t_jac += time.time() - t_start
n_jac += 1
# Check user-provided Jacobian
if i == 0 and check_derivatives:
n_res = check_jacobian(residual, x, r, jac, eps, n_res, bounds, output)
# Check user-provided norm
if i == 0 and check_derivatives and not isinstance(norm, Quadratic):
check_norm(r, norm, eps, output)
# Get gradient, Gauss-Newton Hessian.
grad = jac.T @ r
hess = jac.T @ jac
grad, hess = norm.grad_hess(r, jac)
# Bounds relative to x
dlower = None if bounds is None else bounds[0] - x
@@ -316,13 +314,13 @@ def least_squares(
n_res += 1
# New objective, evaluate reduction.
ynew = 0.5 * rnew.dot(rnew)
ynew = norm.value(rnew)
reduction = y - ynew
armijo = reduction + armijo_c1*grad.dot(dx)
armijo = reduction + armijo_c1 * (grad.T @ dx).item()
if armijo < 0:
if mu >= mu_max:
status = Status.NO_IMPORVEMENT
status = Status.NO_IMPROVEMENT
break
mu, n_reduc = increase_mu(mu)
@@ -330,7 +328,7 @@ def least_squares(
break
# Compute reduction ratio.
expected_reduction = -(grad.dot(dx) + 0.5 * dx.T @ hess @ dx)
expected_reduction = -(grad.T @ dx + 0.5 * dx.T @ hess @ dx).item()
reduction_ratio = 0.0
if expected_reduction <= 0:
if verbose > Verbosity.SILENT.value:
@@ -352,11 +350,13 @@ def least_squares(
)
print(message, file=output)
# Append log to trace.
# Append log to trace, call iter_callback.
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)
if iter_callback is not None:
iter_callback(trace)
# Check for success.
if dx_norm < tol:
@@ -373,12 +373,14 @@ def least_squares(
x = xnew
r = rnew
# Append final log to trace.
# Note: unlike other iter logs, this is at the end point.
yfinal = 0.5 * r.dot(r)
red = np.float64(0.0)
# Append final log to trace, call iter_callback.
# Note: unlike other iter logs, values are computed at the end point.
yfinal = norm.value(r)
red = np.float64(0.0) # No reduction sice we didn't take a step.
log = IterLog(candidate=x, objective=yfinal, reduction=red, regularizer=mu)
trace.append(log)
if iter_callback is not None:
iter_callback(trace)
# Print final diagnostics.
if verbose > Verbosity.SILENT.value:
@@ -401,4 +403,124 @@ def least_squares(
message += f' Jacobian {jac_percent:<.1f}%'
print(message, file=output)
return x, trace
return x.reshape(x0.shape), trace
def jacobian_fd(
residual: Callable[[np.ndarray], np.ndarray],
x: np.ndarray,
r: np.ndarray,
eps: np.float64,
n_res: int,
bounds: Optional[List[np.ndarray]] = None,
) -> Tuple[np.ndarray, int]:
"""Finite-difference Jacobian of a residual function.
Args:
residual: vectorized function that returns the residual of a vector array.
x: point at which to evaluate the Jacobian.
r: residual at x.
eps: finite-difference step size.
n_res: number or residual evaluations so far.
bounds: optional pair of lower and upper bounds.
Returns:
jac: Jacobian of the residual at x.
n_res: updated number of residual evaluations (add x.size).
"""
n = x.size
if bounds is None:
eps_vec = eps * np.ones(n)
else:
mid = 0.5 * (bounds[1] - bounds[0])
eps_vec = np.where(x > mid, -eps, eps).flatten()
xh = x + np.diag(eps_vec)
rh = residual(xh)
jac = (rh - r) / eps_vec
return jac, n_res+n
def check_jacobian(
residual: Callable[[np.ndarray], np.ndarray],
x: np.ndarray,
r: np.ndarray,
jac: np.ndarray,
eps: np.float64,
n_res: int,
bounds: Optional[List[np.ndarray]] = None,
output: Optional[TextIO] = None,
name: Optional[str] = 'Jacobian',
) -> int:
"""Check user-provided Jacobian against internal finite-differencing.
Args:
residual: vectorized function that returns the residual of a vector array.
x: point at which the r and jac were evaluated.
r: residual at x.
jac: Jacobian at x.
eps: finite-difference step size.
n_res: number or residual evaluations so far.
bounds: optional pair of lower and upper bounds.
output: Optional file or StringIO to which to print messages.
name: Optional name of the function being tested.
Returns:
n_res: updated number of residual evaluations.
"""
jac_fd, n_res = jacobian_fd(residual, x, r, eps, n_res, bounds)
denom = np.abs(jac).sum() + np.abs(jac_fd).sum() + 1e-8
rel_diff = np.abs(jac - jac_fd) / denom
if np.any(rel_diff > 1e-5):
raise ValueError(f'User-provided {name} does not match finite-differences '
'to a relative tolerance of 1e-5.')
print(f'User-provided {name} matches finite-differences.', file=output)
return n_res
def check_norm(
r: np.ndarray,
norm: Norm,
eps: np.float64,
output: Optional[TextIO] = None,
):
"""Check user-provided norm against internal finite-differencing.
Args:
r: residual vector.
norm: Norm function returning either the norm scalar or its gradient
and Gauss-Newton Hessian.
eps: finite-difference step size.
output: Optional file or StringIO to which to print messages.
"""
# Get norm(r) value and 1st, 2nd derivatives.
n = np.atleast_2d(norm.value(r)) # norm value as 1x1 array.
eye = np.eye(r.size) # Identity projection.
n_g, n_h = norm.grad_hess(r, eye) # Gradient and Hessian.
# Check that Hessian is symmetric.
if not np.allclose(n_h, n_h.T):
raise ValueError('User-provided norm Hessian is not symmetric.')
# Check that Hessian is positive-definite.
if np.any(np.linalg.eigvals(n_h) < 0):
h_min = np.min(np.linalg.eigvals(n_h))
raise ValueError('User-provided norm Hessian is not positive definite. '
f'Minimum eigenvalue is {h_min:<.4g}')
# Local function returning norm values (vectorized).
def norm_vec(v):
norms = [np.atleast_2d(norm.value(v[:, i:i+1])) for i in range(v.shape[1])]
return np.hstack(norms)
# Check the norm gradient.
check_jacobian(norm_vec, r, n, n_g.T, eps, 0, None, output, 'norm gradient')
# Local function returning norm gradients (vectorized).
def grad_vec(v):
gradients = [norm.grad_hess(v[:, i:i+1], eye)[0] for i in range(v.shape[1])]
return np.hstack(gradients)
# Check the norm Hessian.
check_jacobian(grad_vec, r, n_g, n_h, eps, 0, None, output, 'norm Hessian')
python/mujoco/minimize_test.py
+158 -52
View File
@@ -15,7 +15,6 @@
"""Tests for minimize.py."""
import io
from typing import Tuple
from absl.testing import absltest
from mujoco import minimize
@@ -25,20 +24,19 @@ import numpy as np
class MinimizeTest(absltest.TestCase):
def test_basic(self) -> None:
def residual(x: np.ndarray) -> np.ndarray:
return np.array([1 - x[0], 10 * (x[1] - x[0] ** 2)], dtype=np.float64)
def residual(x):
return np.stack([1 - x[0, :], 10 * (x[1, :] - x[0, :] ** 2)])
for central in [False, True]:
out = io.StringIO()
x0 = np.array((0.0, 0.0))
x, _ = minimize.least_squares(x0, residual, output=out, central=central)
expected_x = np.array((1.0, 1.0))
np.testing.assert_array_almost_equal(x, expected_x)
self.assertContainsSubsequence(out.getvalue(), 'norm(dx) < tol')
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(dx) < tol')
def test_start_at_minimum(self) -> None:
def residual(x: np.ndarray) -> np.ndarray:
return np.array([1 - x[0], 10 * (x[1] - x[0] ** 2)])
def residual(x):
return np.stack([1 - x[0, :], 10 * (x[1, :] - x[0, :] ** 2)])
out = io.StringIO()
x0 = np.array((1.0, 1.0))
@@ -49,33 +47,36 @@ class MinimizeTest(absltest.TestCase):
self.assertContainsSubsequence(out.getvalue(), 'exact minimum found')
def test_jac_callback(self) -> None:
def residual(x: np.ndarray) -> np.ndarray:
return np.array([1 - x[0], 10 * (x[1] - x[0] ** 2)])
def residual(x):
return np.stack([1 - x[0, :], 10 * (x[1, :] - x[0, :] ** 2)])
def jacobian(x: np.ndarray, r: np.ndarray) -> Tuple[float, np.ndarray]:
def jacobian(x, r):
del r # Unused.
return np.array([[-1, 0], [-20 * x[0], 10]])
return np.array([[-1, 0], [-20 * x[0, 0], 10]])
x0 = np.array((0.0, 0.0))
out = io.StringIO()
x, _ = minimize.least_squares(x0, residual, jacobian=jacobian, output=out)
x, _ = minimize.least_squares(x0, residual, jacobian=jacobian, output=out,
check_derivatives=True)
expected_x = np.array((1.0, 1.0))
np.testing.assert_array_almost_equal(x, expected_x)
self.assertContainsSubsequence(out.getvalue(), 'norm(dx) < tol')
self.assertContainsSubsequence(out.getvalue(), 'Jacobian matches')
# 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(), 'insufficient reduction')
# Try with bad Jacobian, ask least_squares to check it.
def bad_jacobian(x, r):
del r # Unused.
return np.array([[-1, 0], [-20 * x[0, 0], 15]])
with self.assertRaisesRegex(ValueError, r'\bJacobian does not match\b'):
minimize.least_squares(x0, residual, jacobian=bad_jacobian, output=out,
check_derivatives=True)
def test_max_iter(self) -> None:
dim = 20 # High-D Rosenbrock
def residual(x: np.ndarray) -> np.ndarray:
res0 = [1 - x[i] for i in range(dim - 1)]
res1 = [10 * (x[i] - x[i + 1] ** 2) for i in range(dim - 1)]
def residual(x):
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.
@@ -90,8 +91,8 @@ class MinimizeTest(absltest.TestCase):
np.testing.assert_array_almost_equal(x, expected_x)
def test_bounds(self) -> None:
def residual(x: np.ndarray) -> np.ndarray:
return np.array([1 - x[0], 10 * (x[1] - x[0] ** 2)])
def residual(x):
return np.stack([1 - x[0, :], 10 * (x[1, :] - x[0, :] ** 2)])
out = io.StringIO()
x0 = np.array((0.0, 0.0))
@@ -108,32 +109,28 @@ class MinimizeTest(absltest.TestCase):
self.assertContainsSubsequence(out.getvalue(), 'norm(dx) < tol')
# Test different bounds conditions.
verbose = minimize.Verbosity.FULLITER
for central in [False, True]:
for bounds in bounds_types.values():
out = io.StringIO()
x, trace = minimize.least_squares(
x0,
residual,
bounds=bounds,
output=out,
central=central,
verbose=verbose,
)
self.assertContainsSubsequence(out.getvalue(), ' < tol')
grad = trace[-2].jacobian.T @ trace[-2].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)
for bounds in bounds_types.values():
out = io.StringIO()
x, trace = minimize.least_squares(
x0,
residual,
bounds=bounds,
output=out,
verbose=minimize.Verbosity.FULLITER,
)
self.assertContainsSubsequence(out.getvalue(), ' < tol')
grad = trace[-2].jacobian.T @ trace[-2].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].item(), 0, places=4)
def test_bad_bounds(self) -> None:
def residual(x: np.ndarray) -> np.ndarray:
def residual(x):
return np.array([1 - x[0], 10 * (x[1] - x[0] ** 2)])
out = io.StringIO()
@@ -150,5 +147,114 @@ class MinimizeTest(absltest.TestCase):
with self.assertRaises(ValueError):
minimize.least_squares(x0, residual, bounds=bounds, output=out)
def test_iter_callback(self) -> None:
def residual(x):
return np.stack([1 - x[0, :], 10 * (x[1, :] - x[0, :] ** 2)])
out = io.StringIO()
def iter_callback(trace):
print(f'Hello iteration {len(trace)}!', file=out)
x0 = np.array((0.0, 0.0))
x, _ = minimize.least_squares(x0, residual, output=out,
iter_callback=iter_callback)
expected_x = np.array((1.0, 1.0))
np.testing.assert_array_almost_equal(x, expected_x)
self.assertContainsSubsequence(out.getvalue(), 'Hello iteration 3!')
def test_norm(self) -> None:
def residual(x):
return np.stack([1 - x[0, :], 10 * (x[1, :] - x[0, :] ** 2)])
p = 0.01 # Smoothing radius for smooth-L2 norm.
class SmoothL2(minimize.Norm):
def value(self, r):
return np.sqrt((r.T @ r).item() + p*p) - p
def grad_hess(self, r, proj):
s = np.sqrt((r.T @ r).item() + p*p)
y_r = r / s
grad = proj.T @ y_r
y_rr = (np.eye(r.size) - y_r @ y_r.T) / s
hess = proj.T @ y_rr @ proj
return grad, hess
out = io.StringIO()
x0 = np.array((0.0, 0.0))
x, _ = minimize.least_squares(x0, residual, norm=SmoothL2(), output=out,
check_derivatives=True)
expected_x = np.array((1.0, 1.0))
np.testing.assert_array_almost_equal(x, expected_x)
self.assertContainsSubsequence(out.getvalue(), 'norm(dx) < tol')
self.assertContainsSubsequence(out.getvalue(),
'User-provided norm gradient matches')
self.assertContainsSubsequence(out.getvalue(),
'User-provided norm Hessian matches')
class SmoothL2BadGrad(minimize.Norm):
def value(self, r):
return np.sqrt((r.T @ r).item() + p*p) - p
def grad_hess(self, r, proj):
s = np.sqrt((r.T @ r).item() + p*p)
y_r = r / s
grad = proj.T @ (y_r + 0.001) # 0.001 is erronous.
y_rr = (np.eye(r.size) - y_r @ y_r.T) / s
hess = proj.T @ y_rr @ proj
return grad, hess
with self.assertRaisesRegex(ValueError, r'\bgradient does not match\b'):
minimize.least_squares(x0, residual, norm=SmoothL2BadGrad(), output=out,
check_derivatives=True)
class SmoothL2BadHess(minimize.Norm):
def value(self, r):
return np.sqrt((r.T @ r).item() + p*p) - p
def grad_hess(self, r, proj):
s = np.sqrt((r.T @ r).item() + p*p)
y_r = r / s
grad = proj.T @ y_r
y_rr = (1.001 * np.eye(r.size) - y_r @ y_r.T) / s # 1.001 is erronous.
hess = proj.T @ y_rr @ proj
return grad, hess
with self.assertRaisesRegex(ValueError, r'\bHessian does not match\b'):
minimize.least_squares(x0, residual, norm=SmoothL2BadHess(), output=out,
check_derivatives=True)
class SmoothL2AsymHess(minimize.Norm):
def value(self, r):
return np.sqrt((r.T @ r).item() + p*p) - p
def grad_hess(self, r, proj):
s = np.sqrt((r.T @ r).item() + p*p)
y_r = r / s
grad = proj.T @ y_r
y_rr = (np.eye(r.size) - (y_r + 0.0001) @ y_r.T) / s
hess = proj.T @ y_rr @ proj
return grad, hess
with self.assertRaisesRegex(ValueError, r'\bnot symmetric\b'):
minimize.least_squares(x0, residual, norm=SmoothL2AsymHess(), output=out,
check_derivatives=True)
class SmoothL2NegHess(minimize.Norm):
def value(self, r):
return np.sqrt((r.T @ r).item() + p*p) - p
def grad_hess(self, r, proj):
s = np.sqrt((r.T @ r).item() + p*p)
y_r = r / s
grad = proj.T @ y_r
y_rr = -(np.eye(r.size) - y_r @ y_r.T) / s # Negative-definite.
hess = proj.T @ y_rr @ proj
return grad, hess
with self.assertRaisesRegex(ValueError, r'\bnot positive definite\b'):
minimize.least_squares(x0, residual, norm=SmoothL2NegHess(), output=out,
check_derivatives=True)
if __name__ == '__main__':
absltest.main()
python/mujoco/mjpython/Info.plist
+4 -4
View File
@@ -7,13 +7,13 @@
<key>CFBundleIdentifier</key>
<string>org.mujoco.mjpython</string>
<key>CFBundleVersion</key>
<string>3.1.5</string>
<string>3.1.6</string>
<key>CFBundleGetInfoString</key>
<string>3.1.5</string>
<string>3.1.6</string>
<key>CFBundleLongVersionString</key>
<string>3.1.5</string>
<string>3.1.6</string>
<key>CFBundleShortVersionString</key>
<string>3.1.5</string>
<string>3.1.6</string>
<key>CFBundleExecutable</key>
<string>mjpython</string>
<key>CFBundleIconFile</key>
python/pyproject.toml
+3 -3
View File
@@ -4,7 +4,7 @@ build-backend = "setuptools.build_meta"
[project]
name = "mujoco"
version = "3.1.5"
version = "3.1.6"
authors = [
{name = "Google DeepMind", email = "mujoco@deepmind.com"},
]
@@ -36,9 +36,9 @@ dynamic = ["readme", "scripts"]
[project.urls]
Homepage = "https://github.com/google-deepmind/mujoco"
Documentation = "https://mujoco.readthedocs.io/en/3.1.5"
Documentation = "https://mujoco.readthedocs.io/en/3.1.6"
Repository = "https://github.com/google-deepmind/mujoco"
Changelog = "https://mujoco.readthedocs.io/en/3.1.5/changelog.html"
Changelog = "https://mujoco.readthedocs.io/en/3.1.6/changelog.html"
[tool.setuptools]
include-package-data = false
python/tutorial.ipynb
+16 -22
View File
@@ -10,27 +10,22 @@
"\n",
"# <h1><center>Tutorial <a href=\"https://colab.research.google.com/github/google-deepmind/mujoco/blob/main/python/tutorial.ipynb\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" width=\"140\" align=\"center\"/></a></center></h1>\n",
"\n",
"This notebook provides an introductory tutorial for [**MuJoCo** physics](https://github.com/google-deepmind/mujoco#readme), using the native Python bindings."
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "xBSdkbmGN2K-"
},
"source": [
"### Copyright notice"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "_UbO9uhtBSX5"
},
"source": [
"> <p><small><small>Copyright 2022 DeepMind Technologies Limited.</small></p>\n",
"> <p><small><small>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 <a href=\"http://www.apache.org/licenses/LICENSE-2.0\">http://www.apache.org/licenses/LICENSE-2.0</a>.</small></small></p>\n",
"> <p><small><small>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.</small></small></p>"
"This notebook provides an introductory tutorial for [**MuJoCo** physics](https://github.com/google-deepmind/mujoco#readme), using the native Python bindings.\n",
"\n",
"<!-- Copyright 2021 DeepMind Technologies Limited\n",
"\n",
" Licensed under the Apache License, Version 2.0 (the \"License\");\n",
" you may not use this file except in compliance with the License.\n",
" You may obtain a copy of the License at\n",
"\n",
" http://www.apache.org/licenses/LICENSE-2.0\n",
"\n",
" Unless required by applicable law or agreed to in writing, software\n",
" distributed under the License is distributed on an \"AS IS\" BASIS,\n",
" WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n",
" See the License for the specific language governing permissions and\n",
" limitations under the License.\n",
"-->"
]
},
{
@@ -2122,7 +2117,6 @@
"accelerator": "GPU",
"colab": {
"collapsed_sections": [
"xBSdkbmGN2K-",
"YvyGCsgSCxHQ"
],
"gpuClass": "premium",
sample/CMakeLists.txt
+1 -1
View File
@@ -24,7 +24,7 @@ set(MSVC_INCREMENTAL_DEFAULT ON)
project(
mujoco_samples
VERSION 3.1.5
VERSION 3.1.6
DESCRIPTION "MuJoCo samples binaries"
HOMEPAGE_URL "https://mujoco.org"
)
simulate/CMakeLists.txt
+1 -1
View File
@@ -29,7 +29,7 @@ set(MUJOCO_DEP_VERSION_lodepng
project(
mujoco_simulate
VERSION 3.1.5
VERSION 3.1.6
DESCRIPTION "MuJoCo simulate binaries"
HOMEPAGE_URL "https://mujoco.org"
)
src/engine/CMakeLists.txt
-2
View File
@@ -76,8 +76,6 @@ set(MUJOCO_ENGINE_SRCS
engine_util_sparse_avx.h
engine_util_spatial.c
engine_util_spatial.h
engine_vfs.c
engine_vfs.h
engine_vis_init.c
engine_vis_init.h
engine_vis_interact.c
src/engine/engine_collision_convex.c
+21 -15
View File
@@ -29,6 +29,8 @@
#include "engine/engine_util_misc.h"
#include "engine/engine_util_spatial.h"
// the LibCCD penetration function we use (ccdMPRPenetration or ccdGJKPenetration)
#define _mjCCDPENETRATION ccdMPRPenetration
// ccd center function
void mjccd_center(const void *obj, ccd_vec3_t *center) {
@@ -267,13 +269,23 @@ void mjccd_support(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *vec) {
// initialize CCD structure
static void mjc_initCCD(ccd_t* ccd, const mjModel* m) {
CCD_INIT(ccd);
ccd->mpr_tolerance = m->opt.mpr_tolerance;
ccd->epa_tolerance = m->opt.mpr_tolerance; // use MPR tolerance for EPA
ccd->max_iterations = m->opt.mpr_iterations;
}
// find single convex-convex collision, using libccd
static int mjc_MPRIteration(mjtCCD* obj1, mjtCCD* obj2, const ccd_t* ccd,
const mjModel* m, const mjData* d,
mjContact* con, mjtNum margin) {
ccd_vec3_t dir, pos;
ccd_real_t depth;
if (ccdMPRPenetration(obj1, obj2, ccd, &depth, &dir, &pos) == 0) {
if (_mjCCDPENETRATION(obj1, obj2, ccd, &depth, &dir, &pos) == 0) {
// contact is found but normal is undefined
if (ccdVec3Eq(&dir, ccd_vec3_origin)) {
return 0;
@@ -343,16 +355,13 @@ int mjc_Convex(const mjModel* m, const mjData* d,
mjtCCD obj2 = {m, d, g2, -1, -1, -1, -1, margin, {1, 0, 0, 0}};
// init ccd structure
mjc_initCCD(&ccd, m);
ccd.first_dir = ccdFirstDirDefault;
ccd.center1 = mjccd_center;
ccd.center2 = mjccd_center;
ccd.support1 = mjccd_support;
ccd.support2 = mjccd_support;
// set ccd parameters
ccd.max_iterations = m->opt.mpr_iterations;
ccd.mpr_tolerance = m->opt.mpr_tolerance;
// find initial contact
int ncon = mjc_MPRIteration(&obj1, &obj2, &ccd, m, d, con, margin);
@@ -747,16 +756,13 @@ int mjc_ConvexHField(const mjModel* m, const mjData* d,
//------------------------------------- collision testing
// init ccd structure
mjc_initCCD(&ccd, m);
ccd.first_dir = prism_firstdir;
ccd.center1 = prism_center;
ccd.center2 = mjccd_center;
ccd.support1 = prism_support;
ccd.support2 = mjccd_support;
// set ccd parameters
ccd.max_iterations = m->opt.mpr_iterations;
ccd.mpr_tolerance = m->opt.mpr_tolerance;
// geom margin needed for actual collision test
obj.margin = margin;
@@ -787,7 +793,7 @@ int mjc_ConvexHField(const mjModel* m, const mjData* d,
}
// run MPR, save contact
if (ccdMPRPenetration(&prism, &obj, &ccd, &depth, &dirccd, &vecccd) == 0 &&
if (_mjCCDPENETRATION(&prism, &obj, &ccd, &depth, &dirccd, &vecccd) == 0 &&
!ccdVec3Eq(&dirccd, ccd_vec3_origin)) {
// fill in contact data, transform to global coordinates
con[cnt].dist = -depth;
@@ -1092,16 +1098,13 @@ int mjc_ConvexElem(const mjModel* m, const mjData* d, mjContact* con,
mjtCCD obj2 = {m, d, -1, -1, f2, e2, -1, margin, {1, 0, 0, 0}};
// init ccd structure
mjc_initCCD(&ccd, m);
ccd.first_dir = ccdFirstDirDefault;
ccd.center1 = mjccd_center;
ccd.center2 = mjccd_center;
ccd.support1 = mjccd_support;
ccd.support2 = mjccd_support;
// set ccd parameters
ccd.max_iterations = m->opt.mpr_iterations;
ccd.mpr_tolerance = m->opt.mpr_tolerance;
// find contacts
int ncon = mjc_MPRIteration(&obj1, &obj2, &ccd, m, d, con, margin);
@@ -1197,6 +1200,7 @@ int mjc_HFieldElem(const mjModel* m, const mjData* d, mjContact* con,
//------------------------------------- collision testing
// init ccd structure
CCD_INIT(&ccd);
ccd.first_dir = prism_firstdir;
ccd.center1 = prism_center;
ccd.center2 = mjccd_center;
@@ -1234,7 +1238,7 @@ int mjc_HFieldElem(const mjModel* m, const mjData* d, mjContact* con,
}
// run MPR, save contact
if (ccdMPRPenetration(&prism, &obj, &ccd, &depth, &dirccd, &vecccd) == 0) {
if (_mjCCDPENETRATION(&prism, &obj, &ccd, &depth, &dirccd, &vecccd) == 0) {
if (!ccdVec3Eq(&dirccd, ccd_vec3_origin)) {
// fill in contact data, transform to global coordinates
con[cnt].dist = -depth;
@@ -1266,3 +1270,5 @@ int mjc_HFieldElem(const mjModel* m, const mjData* d, mjContact* con,
return cnt;
}
#undef _mjCCDPENETRATION
src/engine/engine_io.c
+107 -97
View File
@@ -27,13 +27,11 @@
#include <mujoco/mjplugin.h>
#include <mujoco/mjxmacro.h>
#include "engine/engine_crossplatform.h"
#include "engine/engine_resource.h"
#include "engine/engine_macro.h"
#include "engine/engine_plugin.h"
#include "engine/engine_util_blas.h"
#include "engine/engine_util_errmem.h"
#include "engine/engine_util_misc.h"
#include "engine/engine_vfs.h"
#include "thread/thread_pool.h"
#ifdef ADDRESS_SANITIZER
@@ -449,22 +447,40 @@ static int safeAddToBufferSize(intptr_t* offset, size_t* nbuffer,
// free model memory without destroying the struct
static void freeModelBuffers(mjModel* m) {
mju_free(m->buffer);
}
// allocate and initialize mjModel structure
mjModel* mj_makeModel(
int nq, int nv, int nu, int na, int nbody, int nbvh, int nbvhstatic, int nbvhdynamic, int njnt,
int ngeom, int nsite, int ncam, int nlight, int nflex, int nflexvert, int nflexedge,
int nflexelem, int nflexelemdata, int nflexshelldata, int nflexevpair, int nflextexcoord,
int nmesh, int nmeshvert, int nmeshnormal, int nmeshtexcoord, int nmeshface, int nmeshgraph,
int nskin, int nskinvert, int nskintexvert, int nskinface, int nskinbone, int nskinbonevert,
int nhfield, int nhfielddata, int ntex, int ntexdata, int nmat, int npair, int nexclude, int neq,
int ntendon, int nwrap, int nsensor, int nnumeric, int nnumericdata, int ntext, int ntextdata,
int ntuple, int ntupledata, int nkey, int nmocap, int nplugin, int npluginattr, int nuser_body,
int nuser_jnt, int nuser_geom, int nuser_site, int nuser_cam, int nuser_tendon,
int nuser_actuator, int nuser_sensor, int nnames, int npaths) {
void mj_makeModel(mjModel** dest,
int nq, int nv, int nu, int na, int nbody, int nbvh,
int nbvhstatic, int nbvhdynamic, int njnt, int ngeom, int nsite, int ncam,
int nlight, int nflex, int nflexvert, int nflexedge, int nflexelem,
int nflexelemdata, int nflexshelldata, int nflexevpair, int nflextexcoord,
int nmesh, int nmeshvert, int nmeshnormal, int nmeshtexcoord, int nmeshface,
int nmeshgraph, int nskin, int nskinvert, int nskintexvert, int nskinface,
int nskinbone, int nskinbonevert, int nhfield, int nhfielddata, int ntex,
int ntexdata, int nmat, int npair, int nexclude, int neq, int ntendon,
int nwrap, int nsensor, int nnumeric, int nnumericdata, int ntext,
int ntextdata, int ntuple, int ntupledata, int nkey, int nmocap,
int nplugin, int npluginattr, int nuser_body, int nuser_jnt, int nuser_geom,
int nuser_site, int nuser_cam, int nuser_tendon, int nuser_actuator,
int nuser_sensor, int nnames, int npaths) {
intptr_t offset = 0;
int allocate = *dest ? 0 : 1;
mjModel* m = NULL;
// allocate mjModel
mjModel* m = (mjModel*)mju_malloc(sizeof(mjModel));
if (!allocate) {
m = *dest;
freeModelBuffers(m);
} else {
m = (mjModel*)mju_malloc(sizeof(mjModel));
}
if (!m) {
mjERROR("could not allocate mjModel");
}
@@ -538,43 +554,43 @@ mjModel* mj_makeModel(
nhfield + ntex + nmat + npair + nexclude + neq + ntendon + nu + nsensor +
nnumeric + ntext + ntuple + nkey + nplugin;
if (nnames_map >= INT_MAX / mjLOAD_MULTIPLE) {
mju_free(m);
if (allocate) mju_free(m);
mju_warning("Invalid model: size of nnames_map is larger than INT_MAX");
return 0;
return;
}
m->nnames_map = mjLOAD_MULTIPLE * nnames_map;
m->npaths = npaths;
#define X(name) \
if ((m->name) < 0) { \
mju_free(m); \
if (allocate) mju_free(m); \
mju_warning("Invalid model: negative " #name); \
return 0; \
return; \
}
MJMODEL_INTS;
#undef X
// nbody should always be positive
if (m->nbody == 0) {
mju_free(m);
if (allocate) mju_free(m);
mju_warning("Invalid model: nbody == 0");
return 0;
return;
}
// nmocap is going to get multiplied by 4, and shouldn't overflow
if (m->nmocap >= MAX_ARRAY_SIZE) {
mju_free(m);
if (allocate) mju_free(m);
mju_warning("Invalid model: nmocap too large");
return 0;
return;
}
// compute buffer size
m->nbuffer = 0;
#define X(type, name, nr, nc) \
if (!safeAddToBufferSize(&offset, &m->nbuffer, sizeof(type), m->nr, nc)) { \
mju_free(m); \
if (allocate) mju_free(m); \
mju_warning("Invalid model: " #name " too large."); \
return 0; \
return; \
}
MJMODEL_POINTERS
@@ -583,7 +599,7 @@ mjModel* mj_makeModel(
// allocate buffer
m->buffer = mju_malloc(m->nbuffer);
if (!m->buffer) {
mju_free(m);
if (allocate) mju_free(m);
mjERROR("could not allocate mjModel buffer");
}
@@ -600,18 +616,19 @@ mjModel* mj_makeModel(
mj_defaultVisual(&m->vis);
mj_defaultStatistic(&m->stat);
return m;
// copy pointer if allocated here
if (allocate) {
*dest = m;
}
}
// copy mjModel, if dest==NULL create new model
mjModel* mj_copyModel(mjModel* dest, const mjModel* src) {
void* save_bufptr;
// allocate new model if needed
if (!dest) {
dest = mj_makeModel(
mj_makeModel(&dest,
src->nq, src->nv, src->nu, src->na, src->nbody, src->nbvh,
src->nbvhstatic, src->nbvhdynamic, src->njnt, src->ngeom, src->nsite,
src->ncam, src->nlight, src->nflex, src->nflexvert, src->nflexedge,
@@ -714,36 +731,17 @@ void mj_saveModel(const mjModel* m, const char* filename, void* buffer, int buff
}
// load model from binary MJB resource
mjModel* mj_loadModel(const char* filename, const mjVFS* vfs) {
// load binary MJB model
mjModel* mj_loadModelBuffer(const void* buffer, int buffer_sz) {
int header[NHEADER] = {0};
int expected_header[NHEADER] = {ID, sizeof(mjtNum), getnint(), getnsize(), getnptr()};
int ints[256];
size_t sizes[8];
int ptrbuf = 0;
mjModel *m = 0;
mjResource* r = NULL;
// first try vfs, otherwise try a provider or OS filesystem
if (!(r = mju_openVfsResource(filename, vfs))) {
char error[1024];
if (!(r = mju_openResource(filename, error, 1024))) {
mju_warning("%s", error);
return NULL;
}
}
const void* buffer = NULL;
int buffer_sz = mju_readResource(r, &buffer);
if (buffer_sz <= 0) {
mju_closeResource(r);
return NULL;
}
if (buffer_sz < NHEADER*sizeof(int)) {
mju_warning("Model file has an incomplete header");
mju_closeResource(r);
return NULL;
}
@@ -755,27 +753,22 @@ mjModel* mj_loadModel(const char* filename, const mjVFS* vfs) {
switch (i) {
case 0:
mju_warning("Model missing header ID");
mju_closeResource(r);
return NULL;
case 1:
mju_warning("Model and executable have different floating point precision");
mju_closeResource(r);
return NULL;
case 2:
mju_warning("Model and executable have different number of ints in mjModel");
mju_closeResource(r);
return NULL;
case 3:
mju_warning("Model and executable have different number of size_t members in mjModel");
mju_closeResource(r);
return NULL;
default:
mju_warning("Model and executable have different number of pointers in mjModel");
mju_closeResource(r);
return NULL;
}
}
@@ -783,7 +776,6 @@ mjModel* mj_loadModel(const char* filename, const mjVFS* vfs) {
// read mjModel structure: info only
if (ptrbuf + sizeof(int)*getnint() + sizeof(size_t)*getnsize() > buffer_sz) {
mju_closeResource(r);
mju_warning("Truncated model file - ran out of data while reading sizes");
return NULL;
}
@@ -791,18 +783,18 @@ mjModel* mj_loadModel(const char* filename, const mjVFS* vfs) {
bufread(sizes, sizeof(size_t)*getnsize(), buffer_sz, buffer, &ptrbuf);
// allocate new mjModel, check sizes
m = mj_makeModel(ints[0], ints[1], ints[2], ints[3], ints[4], ints[5], ints[6],
ints[7], ints[8], ints[9], ints[10], ints[11], ints[12], ints[13],
ints[14], ints[15], ints[16], ints[17], ints[18], ints[19], ints[20],
ints[21], ints[22], ints[23], ints[24], ints[25], ints[26], ints[27],
ints[28], ints[29], ints[30], ints[31], ints[32], ints[33], ints[34],
ints[35], ints[36], ints[37], ints[38], ints[39], ints[40], ints[41],
ints[42], ints[43], ints[44], ints[45], ints[46], ints[47], ints[48],
ints[49], ints[50], ints[51], ints[52], ints[53], ints[54], ints[55],
ints[56], ints[57], ints[58], ints[59], ints[60], ints[61], ints[62],
ints[63]);
mj_makeModel(&m,
ints[0], ints[1], ints[2], ints[3], ints[4], ints[5], ints[6],
ints[7], ints[8], ints[9], ints[10], ints[11], ints[12], ints[13],
ints[14], ints[15], ints[16], ints[17], ints[18], ints[19], ints[20],
ints[21], ints[22], ints[23], ints[24], ints[25], ints[26], ints[27],
ints[28], ints[29], ints[30], ints[31], ints[32], ints[33], ints[34],
ints[35], ints[36], ints[37], ints[38], ints[39], ints[40], ints[41],
ints[42], ints[43], ints[44], ints[45], ints[46], ints[47], ints[48],
ints[49], ints[50], ints[51], ints[52], ints[53], ints[54], ints[55],
ints[56], ints[57], ints[58], ints[59], ints[60], ints[61], ints[62],
ints[63]);
if (!m || m->nbuffer != sizes[getnsize()-1]) {
mju_closeResource(r);
mju_warning("Corrupted model, wrong size parameters");
mj_deleteModel(m);
return NULL;
@@ -820,7 +812,6 @@ mjModel* mj_loadModel(const char* filename, const mjVFS* vfs) {
// read options and buffer
if (ptrbuf + sizeof(mjOption) + sizeof(mjVisual) + sizeof(mjStatistic) > buffer_sz) {
mju_closeResource(r);
mju_warning("Truncated model file - ran out of data while reading structs");
return NULL;
}
@@ -831,7 +822,6 @@ mjModel* mj_loadModel(const char* filename, const mjVFS* vfs) {
MJMODEL_POINTERS_PREAMBLE(m)
#define X(type, name, nr, nc) \
if (ptrbuf + sizeof(type) * (m->nr) * (nc) > buffer_sz) { \
mju_closeResource(r); \
mju_warning( \
"Truncated model file - ran out of data while reading " #name); \
mj_deleteModel(m); \
@@ -845,7 +835,6 @@ mjModel* mj_loadModel(const char* filename, const mjVFS* vfs) {
// make sure buffer is the correct size
if (ptrbuf != buffer_sz) {
mju_closeResource(r);
mju_warning("Model file is too large");
mj_deleteModel(m);
return NULL;
@@ -853,22 +842,19 @@ mjModel* mj_loadModel(const char* filename, const mjVFS* vfs) {
const char* validationError = mj_validateReferences(m);
if (validationError) {
mju_closeResource(r);
mju_warning("%s", validationError);
mj_deleteModel(m);
return NULL;
}
mju_closeResource(r);
return m;
}
// de-allocate mjModel
void mj_deleteModel(mjModel* m) {
if (m) {
mju_free(m->buffer);
freeModelBuffers(m);
mju_free(m);
}
}
@@ -1123,12 +1109,40 @@ static void _initPlugin(const mjModel* m, mjData* d) {
// free mjData memory without destroying the struct
static void freeDataBuffers(mjData* d) {
#ifdef ADDRESS_SANITIZER
// raise an error if there's a dangling stack frame
mj_freeStack(d);
#endif
// destroy plugin instances
for (int i = 0; i < d->nplugin; ++i) {
const mjpPlugin* plugin = mjp_getPluginAtSlot(d->plugin[i]);
if (plugin->destroy) {
plugin->destroy(d, i);
}
}
mju_free(d->buffer);
mju_free(d->arena);
}
// allocate and initialize raw mjData structure
mjData* mj_makeRawData(const mjModel* m) {
void mj_makeRawData(mjData** dest, const mjModel* m) {
intptr_t offset = 0;
int allocate = *dest ? 0 : 1;
mjData* d = NULL;
// allocate mjData
mjData* d = (mjData*) mju_malloc(sizeof(mjData));
if (!allocate) {
d = *dest;
freeDataBuffers(d);
} else {
d = (mjData*) mju_malloc(sizeof(mjData));
}
if (!d) {
mjERROR("could not allocate mjData");
}
@@ -1141,9 +1155,9 @@ mjData* mj_makeRawData(const mjModel* m) {
d->buffer = d->arena = NULL;
#define X(type, name, nr, nc) \
if (!safeAddToBufferSize(&offset, &d->nbuffer, sizeof(type), m->nr, nc)) { \
mju_free(d); \
if (allocate) mju_free(d); \
mju_warning("Invalid data: " #name " too large."); \
return 0; \
return; \
}
MJDATA_POINTERS
@@ -1155,7 +1169,7 @@ mjData* mj_makeRawData(const mjModel* m) {
// allocate buffer
d->buffer = mju_malloc(d->nbuffer);
if (!d->buffer) {
mju_free(d);
if (allocate) mju_free(d);
mjERROR("could not allocate mjData buffer");
}
@@ -1163,7 +1177,7 @@ mjData* mj_makeRawData(const mjModel* m) {
d->arena = mju_malloc(d->narena);
if (!d->arena) {
mju_free(d->buffer);
mju_free(d);
if (allocate) mju_free(d);
mjERROR("could not allocate mjData arena");
}
@@ -1176,14 +1190,18 @@ mjData* mj_makeRawData(const mjModel* m) {
// clear nplugin (overwritten by _initPlugin)
d->nplugin = 0;
return d;
// copy pointer if allocated here
if (allocate) {
*dest = d;
}
}
// allocate and initialize mjData structure
mjData* mj_makeData(const mjModel* m) {
mjData* d = mj_makeRawData(m);
mjData* d = NULL;
mj_makeRawData(&d, m);
if (d) {
_initPlugin(m, d);
mj_resetData(m, d);
@@ -1200,7 +1218,7 @@ mjData* mj_copyData(mjData* dest, const mjModel* m, const mjData* src) {
// allocate new data if needed
if (!dest) {
dest = mj_makeRawData(m);
mj_makeRawData(&dest, m);
_initPlugin(m, dest);
}
@@ -1731,20 +1749,7 @@ void mj_resetDataKeyframe(const mjModel* m, mjData* d, int key) {
// de-allocate mjData
void mj_deleteData(mjData* d) {
if (d) {
#ifdef ADDRESS_SANITIZER
// raise an error if there's a dangling stack frame
mj_freeStack(d);
#endif
// destroy plugin instances
for (int i = 0; i < d->nplugin; ++i) {
const mjpPlugin* plugin = mjp_getPluginAtSlot(d->plugin[i]);
if (plugin->destroy) {
plugin->destroy(d, i);
}
}
mju_free(d->buffer);
mju_free(d->arena);
freeDataBuffers(d);
mju_free(d);
}
}
@@ -1773,6 +1778,7 @@ static int sensorSize(mjtSensor sensor_type, int sensor_dim) {
case mjSENS_TENDONLIMITPOS:
case mjSENS_TENDONLIMITVEL:
case mjSENS_TENDONLIMITFRC:
case mjSENS_GEOMDIST:
case mjSENS_CLOCK:
return 1;
@@ -1797,8 +1803,12 @@ static int sensorSize(mjtSensor sensor_type, int sensor_dim) {
case mjSENS_SUBTREECOM:
case mjSENS_SUBTREELINVEL:
case mjSENS_SUBTREEANGMOM:
case mjSENS_GEOMNORMAL:
return 3;
case mjSENS_GEOMFROMTO:
return 6;
case mjSENS_BALLQUAT:
case mjSENS_FRAMEQUAT:
return 4;
src/engine/engine_io.h
+17 -17
View File
@@ -51,19 +51,20 @@ void mj_defaultStatistic(mjStatistic* stat);
//------------------------------- mjModel ----------------------------------------------------------
// allocate mjModel
mjModel* mj_makeModel(int nq, int nv, int nu, int na, int nbody, int nbvh, int nbvhstatic, int nbvhdynamic,
int njnt, int ngeom, int nsite, int ncam, int nlight, int nflex, int nflexvert,
int nflexedge, int nflexelem, int nflexelemdata, int nflexshelldata, int nflexevpair,
int nflextexcoord, int nmesh, int nmeshvert, int nmeshnormal, int nmeshtexcoord, int nmeshface,
int nmeshgraph, int nskin, int nskinvert, int nskintexvert, int nskinface,
int nskinbone, int nskinbonevert, int nhfield, int nhfielddata,
int ntex, int ntexdata, int nmat, int npair, int nexclude,
int neq, int ntendon, int nwrap, int nsensor,
int nnumeric, int nnumericdata, int ntext, int ntextdata,
int ntuple, int ntupledata, int nkey, int nmocap, int nplugin,
int npluginattr, int nuser_body, int nuser_jnt, int nuser_geom,
int nuser_site, int nuser_cam, int nuser_tendon, int nuser_actuator,
int nuser_sensor, int nnames, int npaths);
void mj_makeModel(mjModel** dest,
int nq, int nv, int nu, int na, int nbody, int nbvh, int nbvhstatic, int nbvhdynamic,
int njnt, int ngeom, int nsite, int ncam, int nlight, int nflex, int nflexvert,
int nflexedge, int nflexelem, int nflexelemdata, int nflexshelldata, int nflexevpair,
int nflextexcoord, int nmesh, int nmeshvert, int nmeshnormal, int nmeshtexcoord, int nmeshface,
int nmeshgraph, int nskin, int nskinvert, int nskintexvert, int nskinface,
int nskinbone, int nskinbonevert, int nhfield, int nhfielddata,
int ntex, int ntexdata, int nmat, int npair, int nexclude,
int neq, int ntendon, int nwrap, int nsensor,
int nnumeric, int nnumericdata, int ntext, int ntextdata,
int ntuple, int ntupledata, int nkey, int nmocap, int nplugin,
int npluginattr, int nuser_body, int nuser_jnt, int nuser_geom,
int nuser_site, int nuser_cam, int nuser_tendon, int nuser_actuator,
int nuser_sensor, int nnames, int npaths);
// copy mjModel; allocate new if dest is NULL
MJAPI mjModel* mj_copyModel(mjModel* dest, const mjModel* src);
@@ -71,9 +72,8 @@ MJAPI mjModel* mj_copyModel(mjModel* dest, const mjModel* src);
// save model to binary file
MJAPI void mj_saveModel(const mjModel* m, const char* filename, void* buffer, int buffer_sz);
// load model from binary MJB file
// if vfs is not NULL, look up file in vfs before reading from disk
MJAPI mjModel* mj_loadModel(const char* filename, const mjVFS* vfs);
// load binary MJB
mjModel* mj_loadModelBuffer(const void* buffer, int buffer_sz);
// de-allocate model
MJAPI void mj_deleteModel(mjModel* m);
@@ -92,7 +92,7 @@ MJAPI const char* mj_validateReferences(const mjModel* m);
MJAPI mjData* mj_makeData(const mjModel* m);
// allocate mjData corresponding to given model, used internally
MJAPI mjData* mj_makeRawData(const mjModel* m);
MJAPI void mj_makeRawData(mjData** dest, const mjModel* m);
// Copy mjData.
// m is only required to contain the size fields from MJMODEL_INTS.
src/engine/engine_ray.c
+4 -5
View File
@@ -1137,14 +1137,13 @@ static int point_in_box(const mjtNum aabb[6], const mjtNum xpos[3],
//---------------------------- main entry point ---------------------------------------------------
//---------------------------- main entry point ----------------------------------------------------
// intersect ray (pnt+x*vec, x>=0) with visible geoms, except geoms on bodyexclude
// return geomid and distance (x) to nearest surface, or -1 if no intersection
// geomgroup, flg_static are as in mjvOption; geomgroup==NULL skips group exclusion
mjtNum mj_ray(const mjModel* m, const mjData* d, const mjtNum* pnt, const mjtNum* vec,
const mjtByte* geomgroup, mjtByte flg_static, int bodyexclude,
int geomid[1]) {
const mjtByte* geomgroup, mjtByte flg_static, int bodyexclude, int geomid[1]) {
mjtNum dist, newdist;
// check vector length
@@ -1154,7 +1153,7 @@ mjtNum mj_ray(const mjModel* m, const mjData* d, const mjtNum* pnt, const mjtNum
// clear result
dist = -1;
*geomid = -1;
if (geomid) *geomid = -1;
// loop over geoms not eliminated by mask and bodyexclude
for (int i=0; i < m->ngeom; i++) {
@@ -1177,7 +1176,7 @@ mjtNum mj_ray(const mjModel* m, const mjData* d, const mjtNum* pnt, const mjtNum
// update if closer intersection found
if (newdist >= 0 && (newdist < dist || dist < 0)) {
dist = newdist;
*geomid = i;
if (geomid) *geomid = i;
}
}
}
src/engine/engine_sensor.c
+118 -16
View File
@@ -40,6 +40,11 @@ static void apply_cutoff(const mjModel* m, mjData* d, mjtStage stage) {
// process sensors matching stage and having positive cutoff
for (int i=0; i < m->nsensor; i++) {
if (m->sensor_needstage[i] == stage && m->sensor_cutoff[i] > 0) {
// skip fromto sensors
if (m->sensor_type[i] == mjSENS_GEOMFROMTO) {
continue;
}
// get sensor info
int adr = m->sensor_adr[i];
int dim = m->sensor_dim[i];
@@ -214,9 +219,8 @@ static void cam_project(mjtNum sensordata[2], const mjtNum target_xpos[3],
// position-dependent sensors
void mj_sensorPos(const mjModel* m, mjData* d) {
int rgeomid, objtype, objid, reftype, refid, adr, offset, nusersensor = 0;
int ne = d->ne, nf = d->nf, nefc = d->nefc;
mjtNum rvec[3], *xpos, *xmat, *xpos_ref, *xmat_ref;
int ne = d->ne, nf = d->nf, nefc = d->nefc, nsensor = m->nsensor;
int nusersensor = 0;
// disabled sensors: return
if (mjDISABLED(mjDSBL_SENSOR)) {
@@ -224,22 +228,26 @@ void mj_sensorPos(const mjModel* m, mjData* d) {
}
// process sensors matching stage
for (int i=0; i < m->nsensor; i++) {
for (int i=0; i < nsensor; i++) {
mjtSensor type = (mjtSensor) m->sensor_type[i];
// skip sensor plugins -- these are handled after builtin sensor types
if (m->sensor_type[i] == mjSENS_PLUGIN) {
if (type == mjSENS_PLUGIN) {
continue;
}
if (m->sensor_needstage[i] == mjSTAGE_POS) {
// get sensor info
objtype = m->sensor_objtype[i];
objid = m->sensor_objid[i];
refid = m->sensor_refid[i];
reftype = m->sensor_reftype[i];
adr = m->sensor_adr[i];
int objtype = m->sensor_objtype[i];
int objid = m->sensor_objid[i];
int refid = m->sensor_refid[i];
int reftype = m->sensor_reftype[i];
int adr = m->sensor_adr[i];
mjtNum rvec[3], *xpos, *xmat, *xpos_ref, *xmat_ref;
// process according to type
switch ((mjtSensor) m->sensor_type[i]) {
switch (type) {
case mjSENS_MAGNETOMETER: // magnetometer
mju_mulMatTVec(d->sensordata+adr, d->site_xmat+9*objid, m->opt.magnetic, 3, 3);
break;
@@ -255,7 +263,8 @@ void mj_sensorPos(const mjModel* m, mjData* d) {
rvec[1] = d->site_xmat[9*objid+5];
rvec[2] = d->site_xmat[9*objid+8];
d->sensordata[adr] = mj_ray(m, d, d->site_xpos+3*objid, rvec, NULL, 1,
m->site_bodyid[objid], &rgeomid);
m->site_bodyid[objid], NULL);
break;
case mjSENS_JOINTPOS: // jointpos
@@ -303,11 +312,11 @@ void mj_sensorPos(const mjModel* m, mjData* d) {
// reference frame unspecified: global frame
if (refid == -1) {
if (m->sensor_type[i] == mjSENS_FRAMEPOS) {
if (type == mjSENS_FRAMEPOS) {
mju_copy3(d->sensordata+adr, xpos);
} else {
// offset = (0 or 1 or 2) for (x or y or z)-axis sensors, respectively
offset = m->sensor_type[i] - mjSENS_FRAMEXAXIS;
int offset = type - mjSENS_FRAMEXAXIS;
d->sensordata[adr] = xmat[offset];
d->sensordata[adr+1] = xmat[offset+3];
d->sensordata[adr+2] = xmat[offset+6];
@@ -317,12 +326,12 @@ void mj_sensorPos(const mjModel* m, mjData* d) {
// reference frame specified
else {
get_xpos_xmat(d, reftype, refid, i, &xpos_ref, &xmat_ref);
if (m->sensor_type[i] == mjSENS_FRAMEPOS) {
if (type == mjSENS_FRAMEPOS) {
mju_sub3(rvec, xpos, xpos_ref);
mju_rotVecMatT(d->sensordata+adr, rvec, xmat_ref);
} else {
// offset = (0 or 1 or 2) for (x or y or z)-axis sensors, respectively
offset = m->sensor_type[i] - mjSENS_FRAMEXAXIS;
int offset = type - mjSENS_FRAMEXAXIS;
mjtNum axis[3] = {xmat[offset], xmat[offset+3], xmat[offset+6]};
mju_rotVecMatT(d->sensordata+adr, axis, xmat_ref);
}
@@ -354,6 +363,99 @@ void mj_sensorPos(const mjModel* m, mjData* d) {
mju_copy3(d->sensordata+adr, d->subtree_com+3*objid);
break;
case mjSENS_GEOMDIST: // signed distance between two geoms
case mjSENS_GEOMNORMAL: // normal direction between two geoms
case mjSENS_GEOMFROMTO: // segment between two geoms
{
// use cutoff for collision margin
mjtNum margin = m->sensor_cutoff[i];
// initialize outputs
mjtNum dist = margin; // collision distance
mjtNum fromto[6] = {0}; // segment between geoms
// get lists of geoms to collide
int n1, id1;
if (objtype == mjOBJ_BODY) {
n1 = m->body_geomnum[objid];
id1 = m->body_geomadr[objid];
} else {
n1 = 1;
id1 = objid;
}
int n2, id2;
if (reftype == mjOBJ_BODY) {
n2 = m->body_geomnum[refid];
id2 = m->body_geomadr[refid];
} else {
n2 = 1;
id2 = refid;
}
// collide all pairs
for (int geom1=id1; geom1 < id1+n1; geom1++) {
for (int geom2=id2; geom2 < id2+n2; geom2++) {
mjtNum fromto_new[6] = {0};
mjtNum dist_new = mj_geomDistance(m, d, geom1, geom2, margin, fromto_new);
if (dist_new < dist) {
dist = dist_new;
mju_copy(fromto, fromto_new, 6);
}
}
}
// write sensordata for this sensor and all subsequent sensors with identical signature
int write_sensor = 1;
while (write_sensor) {
// write geom distance
if (type == mjSENS_GEOMDIST) {
d->sensordata[adr] = dist;
}
// write distance normal
else if (type == mjSENS_GEOMNORMAL) {
mjtNum normal[3] = {fromto[3]-fromto[0], fromto[4]-fromto[1], fromto[5]-fromto[2]};
if (normal[0] || normal[1] || normal[2]) {
mju_normalize3(normal);
}
mju_copy3(d->sensordata + adr, normal);
}
// write distance fromto
else {
mju_copy(d->sensordata + adr, fromto, 6);
}
// if this is the last sensor, break
if (i+1 == nsensor) {
break;
}
// type of the next sensor
mjtSensor type_next = m->sensor_type[i+1];
// check if signature of next sensor matches this sensor
write_sensor = (type_next == mjSENS_GEOMDIST ||
type_next == mjSENS_GEOMNORMAL ||
type_next == mjSENS_GEOMFROMTO) &&
m->sensor_objtype[i+1] == objtype &&
m->sensor_objid[i+1] == objid &&
m->sensor_reftype[i+1] == reftype &&
m->sensor_refid[i+1] == refid &&
m->sensor_cutoff[i+1] == margin;
// if signature matches, increment external loop variable i
if (write_sensor) {
i++;
// update adr and type, everything else is the same
adr = m->sensor_adr[i];
type = type_next;
}
}
}
break;
case mjSENS_CLOCK: // clock
d->sensordata[adr] = d->time;
break;
src/engine/engine_setconst.c
+37 -1
View File
@@ -341,12 +341,48 @@ static void set0(mjModel* m, mjData* d) {
// compute positional offsets
mju_sub3(m->light_pos0+3*i, d->light_xpos+3*i, d->xpos+3*id);
mju_sub3(m->light_poscom0+3*i, d->light_xpos+3*i, d->subtree_com+ (id1 >= 0 ? 3*id1 : 3*id));
mju_sub3(m->light_poscom0+3*i, d->light_xpos+3*i, d->subtree_com + (id1 >= 0 ? 3*id1 : 3*id));
// copy dir
mju_copy3(m->light_dir0+3*i, d->light_xdir+3*i);
}
// compute actuator damping from dampratio
for (int i=0; i < m->nu; i++) {
// get bias, gain parameters
mjtNum* biasprm = m->actuator_biasprm + i*mjNBIAS;
mjtNum* gainprm = m->actuator_gainprm + i*mjNGAIN;
// not a position-like actuator: skip
if (gainprm[0] != -biasprm[1]) {
continue;
}
// damping is 0 or negative (interpreted as regular "kv"): skip
if (biasprm[2] <= 0) {
continue;
}
// === interpret biasprm[2] > 0 as dampratio for position-like actuators
// "reflected" inertia (inversely scaled by transmission squared)
mjtNum* transmission = d->actuator_moment + i*nv;
mjtNum mass = 0;
for (int j=0; j < nv; j++) {
mjtNum trn = mju_abs(transmission[j]);
mjtNum trn2 = trn*trn; // transmission squared
if (trn2 > mjMINVAL) {
mass += m->dof_M0[j] / trn2;
}
}
// damping = dampratio * 2 * sqrt(kp * mass)
mjtNum damping = biasprm[2] * 2 * mju_sqrt(gainprm[0] * mass);
// set biasprm[2] to negative damping
biasprm[2] = -damping;
}
mj_freeStack(d);
}
src/engine/engine_support.c
+45 -2
View File
@@ -20,6 +20,7 @@
#include <mujoco/mjdata.h>
#include <mujoco/mjmodel.h>
#include "engine/engine_collision_driver.h"
#include "engine/engine_core_constraint.h"
#include "engine/engine_crossplatform.h"
#include "engine/engine_io.h"
@@ -38,8 +39,8 @@
//-------------------------- Constants -------------------------------------------------------------
#define mjVERSION 315
#define mjVERSIONSTRING "3.1.5"
#define mjVERSION 316
#define mjVERSIONSTRING "3.1.6"
// names of disable flags
const char* mjDISABLESTRING[mjNDISABLE] = {
@@ -1703,6 +1704,48 @@ void mj_objectAcceleration(const mjModel* m, const mjData* d,
//-------------------------- miscellaneous ---------------------------------------------------------
// returns the smallest distance between two geoms
mjtNum mj_geomDistance(const mjModel* m, const mjData* d, int geom1, int geom2, mjtNum distmax,
mjtNum fromto[6]) {
mjContact con[mjMAXCONPAIR];
mjtNum dist = distmax;
if (fromto) mju_zero(fromto, 6);
// flip geom order if required
int flip = m->geom_type[geom1] > m->geom_type[geom2];
int g1 = flip ? geom2 : geom1;
int g2 = flip ? geom1 : geom2;
int type1 = m->geom_type[g1];
int type2 = m->geom_type[g2];
// call collision function if it exists
if (!mjCOLLISIONFUNC[type1][type2]) {
return dist;
}
int num = mjCOLLISIONFUNC[type1][type2](m, d, con, g1, g2, distmax);
// find smallest distance
int smallest = -1;
for (int i=0; i < num; i++) {
mjtNum dist_i = con[i].dist;
if (dist_i < dist) {
dist = dist_i;
smallest = i;
}
}
// write fromto if given and a collision has been found
if (fromto && smallest >= 0) {
mjtNum sign = flip ? -1 : 1;
mju_addScl3(fromto+0, con[smallest].pos, con[smallest].frame, -0.5*sign*dist);
mju_addScl3(fromto+3, con[smallest].pos, con[smallest].frame, 0.5*sign*dist);
}
return dist;
}
// extract 6D force:torque for one contact, in contact frame
void mj_contactForce(const mjModel* m, const mjData* d, int id, mjtNum result[6]) {
mjContact* con;
src/engine/engine_support.h
+4
View File
@@ -189,6 +189,10 @@ MJAPI void mj_objectAcceleration(const mjModel* m, const mjData* d,
//-------------------------- miscellaneous ---------------------------------------------------------
// returns the smallest distance between two geoms
MJAPI mjtNum mj_geomDistance(const mjModel* m, const mjData* d, int geom1, int geom2,
mjtNum distmax, mjtNum fromto[6]);
// extract 6D force:torque for one contact, in contact frame
MJAPI void mj_contactForce(const mjModel* m, const mjData* d, int id, mjtNum result[6]);
src/engine/engine_vis_visualize.c
+18 -4
View File
@@ -1974,6 +1974,20 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
mjv_connector(thisgeom, mjGEOM_LINE, 3, from, to);
f2f(thisgeom->rgba, m->vis.rgba.rangefinder, 4);
FINISH
} else if (m->sensor_type[i] == mjSENS_GEOMFROMTO) {
// sensor data
mjtNum* fromto = d->sensordata + m->sensor_adr[i];
// null output: nothing to render
if (mju_isZero(fromto, 6)) {
continue;
}
// make ray
START
mjv_connector(thisgeom, mjGEOM_LINE, 3, fromto, fromto+3);
f2f(thisgeom->rgba, m->vis.rgba.rangefinder, 4);
FINISH
}
}
}
@@ -2049,7 +2063,7 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
// make list of lights only
void mjv_makeLights(const mjModel* m, mjData* d, mjvScene* scn) {
void mjv_makeLights(const mjModel* m, const mjData* d, mjvScene* scn) {
mjvLight* thislight;
// clear counter
@@ -2116,7 +2130,7 @@ void mjv_makeLights(const mjModel* m, mjData* d, mjvScene* scn) {
// update camera only
void mjv_updateCamera(const mjModel* m, mjData* d, mjvCamera* cam, mjvScene* scn) {
void mjv_updateCamera(const mjModel* m, const mjData* d, mjvCamera* cam, mjvScene* scn) {
mjtNum ca, sa, ce, se, move[3], *mat;
mjtNum headpos[3], forward[3], up[3], right[3], ipd;
@@ -2546,7 +2560,7 @@ void mjv_updateActiveFlex(const mjModel* m, mjData* d, mjvScene* scn, const mjvO
// update all skins, here for backward API compatibility
void mjv_updateSkin(const mjModel* m, mjData* d, mjvScene* scn) {
void mjv_updateSkin(const mjModel* m, const mjData* d, mjvScene* scn) {
mjvOption opt;
mjv_defaultOption(&opt);
mjv_updateActiveSkin(m, d, scn, &opt);
@@ -2556,7 +2570,7 @@ void mjv_updateSkin(const mjModel* m, mjData* d, mjvScene* scn) {
// update visible skins only
void mjv_updateActiveSkin(const mjModel* m, mjData* d, mjvScene* scn, const mjvOption* opt) {
void mjv_updateActiveSkin(const mjModel* m, const mjData* d, mjvScene* scn, const mjvOption* opt) {
// process skins
for (int i=0; i < m->nskin; i++) {
// get info
src/engine/engine_vis_visualize.h
+4 -4
View File
@@ -48,19 +48,19 @@ MJAPI void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* opt,
const mjvPerturb* pert, int catmask, mjvScene* scn);
// make list of lights only
MJAPI void mjv_makeLights(const mjModel* m, mjData* d, mjvScene* scn);
MJAPI void mjv_makeLights(const mjModel* m, const mjData* d, mjvScene* scn);
// update camera only
MJAPI void mjv_updateCamera(const mjModel* m, mjData* d, mjvCamera* cam, mjvScene* scn);
MJAPI void mjv_updateCamera(const mjModel* m, const mjData* d, mjvCamera* cam, mjvScene* scn);
// update visible flexes only
MJAPI void mjv_updateActiveFlex(const mjModel* m, mjData* d, mjvScene* scn, const mjvOption* opt);
// update skins only
MJAPI void mjv_updateSkin(const mjModel* m, mjData* d, mjvScene* scn);
MJAPI void mjv_updateSkin(const mjModel* m, const mjData* d, mjvScene* scn);
// update visible skins only
MJAPI void mjv_updateActiveSkin(const mjModel* m, mjData* d, mjvScene* scn, const mjvOption* opt);
MJAPI void mjv_updateActiveSkin(const mjModel* m, const mjData* d, mjvScene* scn, const mjvOption* opt);
#define mjNCATENARY 31
src/user/CMakeLists.txt
+2
View File
@@ -29,6 +29,8 @@ set(MUJOCO_USER_SRCS
user_objects.h
user_util.cc
user_util.h
user_vfs.c
user_vfs.h
)
target_sources(mujoco PRIVATE ${MUJOCO_USER_SRCS})
src/user/user_api.cc
+37 -11
View File
@@ -29,6 +29,9 @@
#include "user/user_cache.h"
#include "xml/xml_util.h"
// global cache size in bytes (default 500MB)
static constexpr std::size_t kGlobalCacheSize = 500 * (1 << 20);
// prepend prefix
template <typename T>
@@ -115,7 +118,7 @@ int mjs_detachBody(mjSpec* s, mjsBody* b) {
mjCModel* model = static_cast<mjCModel*>(s->element);
mjCBody* body = static_cast<mjCBody*>(b->element);
*model -= *body;
mjs_deleteBody(b);
mjs_delete(b->element);
return 0;
}
@@ -137,10 +140,10 @@ void mjs_deleteSpec(mjSpec* s) {
// delete body
void mjs_deleteBody(mjsBody* b) {
mjCBody* body = static_cast<mjCBody*>(b->element);
delete body;
// delete object, it will call the appropriate destructor since ~mjCBase is virtual
void mjs_delete(mjElement* element) {
mjCBase* object = static_cast<mjCBase*>(element);
delete object;
}
@@ -426,8 +429,8 @@ mjsPlugin* mjs_addPlugin(mjSpec* s) {
// add default to model
mjsDefault* mjs_addDefault(mjSpec* s, const char* classname, int parentid, int* id) {
mjCModel* modelC = static_cast<mjCModel*>(s->element);
*id = (int)modelC->defaults.size();
mjCDef* def = modelC->AddDef(classname, parentid);
*id = (int)modelC->NumDefaults();
mjCDef* def = modelC->AddDefault(classname, parentid);
if (def) {
return &def->spec;
} else {
@@ -454,7 +457,7 @@ mjsDefault* mjs_getDefault(mjElement* element) {
// Find default with given name in model.
mjsDefault* mjs_findDefault(mjSpec* s, const char* classname) {
mjCModel* modelC = static_cast<mjCModel*>(s->element);
mjCDef* cdef = modelC->FindDef(classname);
mjCDef* cdef = modelC->FindDefault(classname);
if (!cdef) {
return nullptr;
}
@@ -466,7 +469,7 @@ mjsDefault* mjs_findDefault(mjSpec* s, const char* classname) {
// get default[0] from model
mjsDefault* mjs_getSpecDefault(mjSpec* s) {
mjCModel* modelC = static_cast<mjCModel*>(s->element);
mjCDef* def = modelC->defaults[0];
mjCDef* def = modelC->Defaults(0);
if (!def) {
return nullptr;
}
@@ -551,6 +554,22 @@ void mjs_setDefault(mjElement* element, mjsDefault* defspec) {
// return first child of selected type
mjElement* mjs_firstChild(mjsBody* body, mjtObj type) {
mjCBody* bodyC = static_cast<mjCBody*>(body->element);
return bodyC->NextChild(NULL, type);
}
// return body's next child; return NULL if child is last
mjElement* mjs_nextChild(mjsBody* body, mjElement* child) {
mjCBody* bodyC = static_cast<mjCBody*>(body->element);
return bodyC->NextChild(child);
}
// set string
void mjs_setString(mjString dest, const char* text) {
std::string* str = reinterpret_cast<std::string*>(dest);
@@ -674,7 +693,7 @@ void mjs_setActivePlugins(mjSpec* s, void* activeplugins) {
mjCModel* modelC = static_cast<mjCModel*>(s->element);
std::vector<std::pair<const mjpPlugin*, int>>* active_plugins =
reinterpret_cast<std::vector<std::pair<const mjpPlugin*, int>>*>(activeplugins);
modelC->active_plugins = std::move(*active_plugins);
modelC->SetActivePlugins(std::move(*active_plugins));
}
@@ -698,5 +717,12 @@ void mj_setCacheSize(mjCache cache, std::size_t size) {
mjCache mj_globalCache() {
return NULL; // currently disabled
// mjCCache is not trivially destructible and so the global cache needs to
// allocated on the heap
if constexpr (kGlobalCacheSize != 0) {
static mjCCache* cache = new(std::nothrow) mjCCache(kGlobalCacheSize);
return (mjCache) cache;
} else {
return NULL;
}
}
src/user/user_api.h
+11 -3
View File
@@ -244,7 +244,6 @@ typedef struct _mjsJoint { // joint specification
// other
int group; // group
mjtByte actgravcomp; // is gravcomp force applied via actuators
double urdfeffort; // effort (urdf)
mjDoubleVec userdata; // user data
mjString info; // message appended to compiler errors
} mjsJoint;
@@ -811,8 +810,8 @@ MJAPI mjsLight* mjs_addLight(mjsBody* body, mjsDefault* def);
// Add frame to body.
MJAPI mjsFrame* mjs_addFrame(mjsBody* body, mjsFrame* parentframe);
// Delete body. TODO: make this a general mjs_deleteElement function
MJAPI void mjs_deleteBody(mjsBody* b);
// Delete object corresponding to the given element.
MJAPI void mjs_delete(mjElement* element);
//---------------------------------- Add non-tree elements -----------------------------------------
@@ -917,6 +916,15 @@ MJAPI mjsDefault* mjs_getSpecDefault(mjSpec* s);
MJAPI int mjs_getId(mjElement* element);
//---------------------------------- Tree traversal ------------------------------------------------
// Return body's first child of given type.
MJAPI mjElement* mjs_firstChild(mjsBody* body, mjtObj type);
// Return body's next child of the same type; return NULL if child is last.
MJAPI mjElement* mjs_nextChild(mjsBody* body, mjElement* child);
//---------------------------------- Attribute setters ---------------------------------------------
// Copy text to string.
src/user/user_cache.cc
+20 -13
View File
@@ -187,16 +187,23 @@ const std::string* mjCCache::HasAsset(const std::string& id) {
// is updated only if the timestamps disagree
bool mjCCache::Insert(const mjCAsset& asset) {
std::lock_guard<std::mutex> lock(mutex_);
// check if asset is too large to fit in the cache
std::size_t nbytes = asset.BytesCount();
const std::string& id = asset.Id();
if ((size_ + nbytes > max_size_) && lookup_.find(id) == lookup_.end()) {
return false;
}
if (asset.References().size() != 1) {
return false;
}
const std::string& filename = *(asset.References().begin());
auto [it, inserted] = lookup_.insert({id, asset});
mjCAsset* asset_ptr = &(it->second);
if (!inserted) {
mjCAsset* asset_ptr = &(it->second);
if (size_ - asset_ptr->BytesCount() + asset.BytesCount() > max_size_) {
if (size_ - asset_ptr->BytesCount() + nbytes > max_size_) {
return false;
}
models_[filename].insert(asset_ptr); // add it for the model
@@ -205,19 +212,16 @@ bool mjCCache::Insert(const mjCAsset& asset) {
return true;
}
asset_ptr->SetTimestamp(asset.Timestamp());
size_ = size_ - asset_ptr->BytesCount() + asset.BytesCount();
asset_ptr->ReplaceBlocks(asset.Blocks(), asset.BytesCount());
size_ = size_ - asset_ptr->BytesCount() + nbytes;
asset_ptr->ReplaceBlocks(asset.Blocks(), nbytes);
return true;
} else if (size_ + asset.BytesCount() > max_size_) {
return false;
}
// new asset
mjCAsset* asset_ptr = &(it->second);
asset_ptr->SetInsertNum(insert_num_++);
entries_.insert(asset_ptr);
models_[filename].insert(asset_ptr);
size_ += asset.BytesCount();
size_ += nbytes;
return true;
}
@@ -225,16 +229,22 @@ bool mjCCache::Insert(const mjCAsset& asset) {
bool mjCCache::Insert(mjCAsset&& asset) {
std::lock_guard<std::mutex> lock(mutex_);
// check if asset is too large to fit in the cache
std::size_t nbytes = asset.BytesCount();
const std::string& id = asset.Id();
if ((size_ + nbytes > max_size_) && lookup_.find(id) == lookup_.end()) {
return false;
}
if (asset.References().size() != 1) {
return false;
}
const std::string& filename = *(asset.References().begin());
std::size_t nbytes = asset.BytesCount();
auto [it, inserted] = lookup_.try_emplace(id, std::move(asset));
mjCAsset* asset_ptr = &(it->second);
if (!inserted) {
mjCAsset* asset_ptr = &(it->second);
if (size_ - asset_ptr->BytesCount() + nbytes > max_size_) {
return false;
}
@@ -248,12 +258,9 @@ bool mjCCache::Insert(mjCAsset&& asset) {
size_ = size_ - asset_ptr->BytesCount() + nbytes;
asset_ptr->ReplaceBlocks(std::move(asset.blocks_), asset.nbytes_);
return true;
} else if (size_ + nbytes > max_size_) {
return false;
}
// new asset
mjCAsset* asset_ptr = &(it->second);
asset_ptr->SetInsertNum(insert_num_++);
entries_.insert(asset_ptr);
models_[filename].insert(asset_ptr);
src/user/user_composite.cc
+13 -13
View File
@@ -561,7 +561,7 @@ bool mjCComposite::MakeParticle(mjCModel* model, mjsBody* body, char* error, int
// create tendon
mjsTendon* ten = mjs_addTendon(&model->spec, &def[mjCOMPKIND_TENDON].spec);
mjs_setDefault(ten->element, &model->defaults[0]->spec);
mjs_setDefault(ten->element, &model->Defaults(0)->spec);
mjs_setString(ten->name, txt0);
ten->group = 4;
mjs_wrapSite(ten, txt1);
@@ -569,7 +569,7 @@ bool mjCComposite::MakeParticle(mjCModel* model, mjsBody* body, char* error, int
// add equality constraint
mjsEquality* eq = mjs_addEquality(&model->spec, &def[mjCOMPKIND_TENDON].spec);
mjs_setDefault(eq->element, &model->defaults[0]->spec);
mjs_setDefault(eq->element, &model->Defaults(0)->spec);
eq->type = mjEQ_TENDON;
mjs_setString(eq->name1, mjs_getString(ten->name));
}
@@ -675,7 +675,7 @@ bool mjCComposite::MakeGrid(mjCModel* model, mjsBody* body, char* error, int err
// create tendon
mjCTendon* ten = model->AddTendon(def + mjCOMPKIND_TENDON);
ten->def = model->defaults[0];
ten->def = model->Defaults(0);
mju::sprintf_arr(txt, "%sT%d_%d_%d", prefix.c_str(), i, ix, iy);
ten->name = txt;
ten->WrapSite(txt1);
@@ -683,7 +683,7 @@ bool mjCComposite::MakeGrid(mjCModel* model, mjsBody* body, char* error, int err
// add equality constraint
mjsEquality* eq = mjs_addEquality(&model->spec, &def[mjCOMPKIND_TENDON].spec);
mjs_setDefault(eq->element, &model->defaults[0]->spec);
mjs_setDefault(eq->element, &model->Defaults(0)->spec);
eq->type = mjEQ_TENDON;
mjs_setString(eq->name1, ten->name.c_str());
}
@@ -1045,7 +1045,7 @@ mjsBody* mjCComposite::AddRopeBody(mjCModel* model, mjsBody* body, int ix, int i
// add constraint
mjsEquality* eq = mjs_addEquality(&model->spec, &def[mjCOMPKIND_TWIST].spec);
mjs_setDefault(eq->element, &model->defaults[0]->spec);
mjs_setDefault(eq->element, &model->Defaults(0)->spec);
eq->type = mjEQ_JOINT;
mjs_setString(eq->name1, mjs_getString(jnt->name));
}
@@ -1063,7 +1063,7 @@ mjsBody* mjCComposite::AddRopeBody(mjCModel* model, mjsBody* body, int ix, int i
// add constraint
mjsEquality* eq = mjs_addEquality(&model->spec, &def[mjCOMPKIND_STRETCH].spec);
mjs_setDefault(eq->element, &model->defaults[0]->spec);
mjs_setDefault(eq->element, &model->Defaults(0)->spec);
eq->type = mjEQ_JOINT;
mjs_setString(eq->name1, mjs_getString(jnt->name));
}
@@ -1131,7 +1131,7 @@ bool mjCComposite::MakeBox(mjCModel* model, mjsBody* body, char* error, int erro
// fixed tendon for all joints
mjCTendon* ten = model->AddTendon(def + mjCOMPKIND_TENDON);
ten->def = model->defaults[0];
ten->def = model->Defaults(0);
mju::sprintf_arr(txt, "%sT", prefix.c_str());
ten->name = txt;
@@ -1184,7 +1184,7 @@ bool mjCComposite::MakeBox(mjCModel* model, mjsBody* body, char* error, int erro
// add fix constraint
mjsEquality* eq = mjs_addEquality(&model->spec, &def[mjCOMPKIND_JOINT].spec);
mjs_setDefault(eq->element, &model->defaults[0]->spec);
mjs_setDefault(eq->element, &model->Defaults(0)->spec);
eq->type = mjEQ_JOINT;
mjs_setString(eq->name1, mjs_getString(jnt->name));
@@ -1218,7 +1218,7 @@ bool mjCComposite::MakeBox(mjCModel* model, mjsBody* body, char* error, int erro
// finalize fixed tendon
mjsEquality* eqt = mjs_addEquality(&model->spec, &def[mjCOMPKIND_TENDON].spec);
mjs_setDefault(eqt->element, &model->defaults[0]->spec);
mjs_setDefault(eqt->element, &model->Defaults(0)->spec);
eqt->type = mjEQ_TENDON;
mjs_setString(eqt->name1, ten->name.c_str());
@@ -1244,7 +1244,7 @@ void mjCComposite::MakeShear(mjCModel* model) {
// create tendon
mjCTendon* ten = model->AddTendon(def + mjCOMPKIND_SHEAR);
ten->def = model->defaults[0];
ten->def = model->Defaults(0);
ten->WrapSite(txt1);
ten->WrapSite(txt2);
@@ -1254,7 +1254,7 @@ void mjCComposite::MakeShear(mjCModel* model) {
// equality constraint
mjsEquality* eq = mjs_addEquality(&model->spec, &def[mjCOMPKIND_SHEAR].spec);
mjs_setDefault(eq->element, &model->defaults[0]->spec);
mjs_setDefault(eq->element, &model->Defaults(0)->spec);
eq->type = mjEQ_TENDON;
mjs_setString(eq->name1, txt);
}
@@ -1552,11 +1552,11 @@ void mjCComposite::MakeCableBonesSubgrid(mjCModel* model, mjsSkin* skin) {
// bind pose
if (iy==0) {
bindpos.push_back((ix==count[0]-1) ? -2*def[0].spec.geom->size[0] : 0);
bindpos.push_back(-def[0].geom.spec.size[1]);
bindpos.push_back(-def[0].Geom().spec.size[1]);
bindpos.push_back(0);
} else if (iy==2) {
bindpos.push_back((ix==count[0]-1) ? -2*def[0].spec.geom->size[0] : 0);
bindpos.push_back(def[0].geom.spec.size[1]);
bindpos.push_back(def[0].Geom().spec.size[1]);
bindpos.push_back(0);
} else {
bindpos.push_back((ix==count[0]-1) ? -2*def[0].spec.geom->size[0] : 0);
src/user/user_flexcomp.cc
+3 -3
View File
@@ -400,7 +400,7 @@ bool mjCFlexcomp::Make(mjSpec* spec, mjsBody* body, char* error, int error_sz) {
mjsFlex* pf = &flex->spec;
int id = flex->id;
*flex = def.flex;
*flex = def.Flex();
flex->PointToLocal();
flex->model = model;
@@ -521,7 +521,7 @@ bool mjCFlexcomp::Make(mjSpec* spec, mjsBody* body, char* error, int error_sz) {
// create edge equality constraint
if (equality) {
mjsEquality* pe = mjs_addEquality(&model->spec, &def.spec);
mjs_setDefault(pe->element, &model->defaults[0]->spec);
mjs_setDefault(pe->element, &model->Defaults(0)->spec);
pe->type = mjEQ_FLEX;
pe->active = true;
mjs_setString(pe->name1, name.c_str());
@@ -544,7 +544,7 @@ int mjCFlexcomp::GridID(int ix, int iy, int iz) {
// make grid
bool mjCFlexcomp::MakeGrid(char* error, int error_sz) {
int dim = def.flex.spec.dim;
int dim = def.Flex().spec.dim;
bool hastex = texcoord.empty();
// 1D
src/user/user_init.c
-1
View File
@@ -105,7 +105,6 @@ void mjs_defaultJoint(mjsJoint* joint) {
joint->actfrclimited = mjLIMITED_AUTO;
mj_defaultSolRefImp(joint->solref_limit, joint->solimp_limit);
mj_defaultSolRefImp(joint->solref_friction, joint->solimp_friction);
joint->urdfeffort = -1;
}
src/user/user_mesh.cc
+8 -8
View File
@@ -146,12 +146,12 @@ mjCMesh::mjCMesh(mjCModel* _model, mjCDef* _def) {
// reset to default if given
if (_def) {
*this = _def->mesh;
*this = _def->Mesh();
}
// set model, def
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();
@@ -1426,7 +1426,7 @@ void mjCMesh::Process() {
// apply formula, accumulate
GetVolumeRef(type) += vol;
for (int j=0; j<6; j++) {
P[j] += def->geom.density*vol /
P[j] += def->Geom().density*vol /
(type==mjINERTIA_SHELL ? 12 : 20) * (
2*(D[k[j][0]] * D[k[j][1]] +
E[k[j][0]] * E[k[j][1]] +
@@ -1467,7 +1467,7 @@ void mjCMesh::Process() {
}
// compute sizes of equivalent inertia box
double mass = GetVolumeRef(type) * def->geom.density;
double mass = GetVolumeRef(type) * def->Geom().density;
double* boxsz = GetInertiaBoxPtr(type);
boxsz[0] = sqrt(6*(eigval[1]+eigval[2]-eigval[0])/mass)/2;
boxsz[1] = sqrt(6*(eigval[0]+eigval[2]-eigval[1])/mass)/2;
@@ -2560,12 +2560,12 @@ void mjCFlex::Compile(const mjVFS* vfs) {
for (int i=0; i < nvert; i++) {
// get body id, set vertxpos = body.xpos0
int b = rigid ? vertbodyid[0] : vertbodyid[i];
mju_copy3(vertxpos.data()+3*i, model->bodies[b]->xpos0);
mju_copy3(vertxpos.data()+3*i, model->Bodies()[b]->xpos0);
// add vertex offset within body if not centered
if (!centered) {
mjtNum offset[3];
mju_rotVecQuat(offset, vert_.data()+3*i, model->bodies[b]->xquat0);
mju_rotVecQuat(offset, vert_.data()+3*i, model->Bodies()[b]->xquat0);
mju_addTo3(vertxpos.data()+3*i, offset);
}
}
@@ -2631,9 +2631,9 @@ void mjCFlex::Compile(const mjVFS* vfs) {
mjXUtil::Vector2String(useredge, edgeidx);
for (const auto& vbodyid : vertbodyid) {
if (model->bodies[vbodyid]->plugin.instance) {
if (model->Bodies()[vbodyid]->plugin.instance) {
mjCPlugin* plugin_instance =
static_cast<mjCPlugin*>(model->bodies[vbodyid]->plugin.instance);
static_cast<mjCPlugin*>(model->Bodies()[vbodyid]->plugin.instance);
plugin_instance->config_attribs["face"] = userface;
plugin_instance->config_attribs["edge"] = useredge;
}
src/user/user_model.cc
+626 -636
View File
File diff suppressed because it is too large Load Diff
src/user/user_model.h
+86 -55
View File
@@ -192,14 +192,6 @@ class mjCModel : public mjCModel_, private mjSpec {
mjCKey* AddKey();
mjCPlugin* AddPlugin();
// copy vector of elements to this model
template <class T> void CopyList(std::vector<T*>& dest,
const std::vector<T*>& source,
std::map<mjCDef*, int>& def_map,
const std::vector<mjCDef*>& defaults);
// delete from list the elements that are compatible with other but not this model
template <class T> void RemoveFromList(std::vector<T*>& list, const mjCModel& other);
// delete elements marked as discard=true
template <class T> void Delete(std::vector<T*>& elements,
@@ -214,43 +206,59 @@ class mjCModel : public mjCModel_, private mjSpec {
// API for access to other variables
bool IsCompiled() const; // is model already compiled
const mjCError& GetError(void) const; // get reference of error object
mjCBody* GetWorld() const; // pointer to world body
mjCDef* FindDef(std::string name) const; // find default class name
mjCDef* AddDef(std::string name, int parentid); // add default class to array
const mjCError& GetError() const; // get reference of error object
mjCBody* GetWorld(); // pointer to world body
mjCDef* FindDefault(std::string name); // find defaults class name
mjCDef* AddDefault(std::string name, int parentid); // add defaults class to array
mjCBase* FindObject(mjtObj type, std::string name) const; // find object given type and name
mjCBody* FindBody(mjCBody* body, std::string name) const; // find body given name
mjCBody* FindBody(mjCBody* body, std::string name); // find body given name
mjCFrame* FindFrame(mjCBody* body, std::string name) const; // find frame given name
bool IsNullPose(const mjtNum* pos, const mjtNum* quat) const; // detect null pose
void SetActivePlugins(const std::vector<std::pair<const mjpPlugin*, int>>&& active_plugins) {
active_plugins_ = std::move(active_plugins);
}
// accessors
std::string get_meshdir(void) const { return meshdir_; }
std::string get_texturedir(void) const { return texturedir_; }
std::string get_meshdir() const { return meshdir_; }
std::string get_texturedir() const { return texturedir_; }
mjCDef* Defaults(int i) const { return defaults_[i]; }
int NumDefaults() const { return defaults_.size(); }
const std::vector<std::pair<const mjpPlugin*, int>>& ActivePlugins() const {
return active_plugins_;
};
const std::vector<mjCFlex*>& Flexes() const { return flexes_; }
const std::vector<mjCMesh*>& Meshes() const {return meshes_; }
const std::vector<mjCSkin*>& Skins() const { return skins_; }
const std::vector<mjCHField*>& HFields() const { return hfields_; }
const std::vector<mjCTexture*>& Textures() const { return textures_; }
const std::vector<mjCMaterial*>& Materials() const { return materials_; }
const std::vector<mjCPair*>& Pairs() const { return pairs_; }
const std::vector<mjCBodyPair*>& Excludes() const { return excludes_; }
const std::vector<mjCEquality*>& Equalities() const { return equalities_; }
const std::vector<mjCTendon*>& Tendons() const { return tendons_; }
const std::vector<mjCActuator*>& Actuators() const { return actuators_; }
const std::vector<mjCSensor*>& Sensors() const { return sensors_; }
const std::vector<mjCNumeric*>& Numerics() const { return numerics_; }
const std::vector<mjCText*>& Texts() const { return texts_; }
const std::vector<mjCTuple*>& Tuples() const { return tuples_; }
const std::vector<mjCKey*>& Keys() const { return keys_; }
const std::vector<mjCPlugin*>& Plugins() const { return plugins_; }
const std::vector<mjCBody*>& Bodies() const { return bodies_; }
const std::vector<mjCGeom*>& Geoms() const { return geoms_; }
// resolve plugin instance, create a new one if needed
void ResolvePlugin(mjCBase* obj, const std::string& plugin_name,
const std::string& plugin_instance_name,
mjCPlugin** plugin_instance);
// settings for each defaults class
std::vector<mjCDef*> defaults;
// list of active plugins
std::vector<std::pair<const mjpPlugin*, int>> active_plugins;
private:
void TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs);
mjModel* _Compile(const mjVFS* vfs);
// clear objects allocated by Compile
void Clear(void);
// add object of any type
template <class T> T* AddObject(std::vector<T*>& list, std::string type);
// add object of any type, with def parameter
template <class T> T* AddObjectDef(std::vector<T*>& list, std::string type,
mjCDef* def);
void Clear();
// if asset name is missing, set to filename
template<class T> void SetDefaultNames(std::vector<T*>& assets);
@@ -259,6 +267,13 @@ class mjCModel : public mjCModel_, private mjSpec {
template <class T> void DeleteMaterial(std::vector<T*>& list,
std::string_view name = "");
private:
// settings for each defaults class
std::vector<mjCDef*> defaults_;
// list of active plugins
std::vector<std::pair<const mjpPlugin*, int>> active_plugins_;
// compile phases
void MakeLists(mjCBody* body); // make lists of bodies, geoms, joints, sites
void SetNuser(); // set nuser fields
@@ -273,35 +288,51 @@ class mjCModel : public mjCModel_, private mjSpec {
void CopyTree(mjModel*); // copy objects inside kinematic tree
// objects created here
std::vector<mjCFlex*> flexes; // list of flexes
std::vector<mjCMesh*> meshes; // list of meshes
std::vector<mjCSkin*> skins; // list of skins
std::vector<mjCHField*> hfields; // list of height fields
std::vector<mjCTexture*> textures; // list of textures
std::vector<mjCMaterial*> materials; // list of materials
std::vector<mjCPair*> pairs; // list of geom pairs to include
std::vector<mjCBodyPair*> excludes; // list of body pairs to exclude
std::vector<mjCEquality*> equalities; // list of equality constraints
std::vector<mjCTendon*> tendons; // list of tendons
std::vector<mjCActuator*> actuators; // list of actuators
std::vector<mjCSensor*> sensors; // list of sensors
std::vector<mjCNumeric*> numerics; // list of numeric fields
std::vector<mjCText*> texts; // list of text fields
std::vector<mjCTuple*> tuples; // list of tuple fields
std::vector<mjCKey*> keys; // list of keyframe fields
std::vector<mjCPlugin*> plugins; // list of plugin instances
std::vector<mjCFlex*> flexes_; // list of flexes
std::vector<mjCMesh*> meshes_; // list of meshes
std::vector<mjCSkin*> skins_; // list of skins
std::vector<mjCHField*> hfields_; // list of height fields
std::vector<mjCTexture*> textures_; // list of textures
std::vector<mjCMaterial*> materials_; // list of materials
std::vector<mjCPair*> pairs_; // list of geom pairs to include
std::vector<mjCBodyPair*> excludes_; // list of body pairs to exclude
std::vector<mjCEquality*> equalities_; // list of equality constraints
std::vector<mjCTendon*> tendons_; // list of tendons
std::vector<mjCActuator*> actuators_; // list of actuators
std::vector<mjCSensor*> sensors_; // list of sensors
std::vector<mjCNumeric*> numerics_; // list of numeric fields
std::vector<mjCText*> texts_; // list of text fields
std::vector<mjCTuple*> tuples_; // list of tuple fields
std::vector<mjCKey*> keys_; // list of keyframe fields
std::vector<mjCPlugin*> plugins_; // list of plugin instances
// pointers to objects created inside kinematic tree
std::vector<mjCBody*> bodies; // list of bodies
std::vector<mjCJoint*> joints; // list of joints allowing motion relative to parent
std::vector<mjCGeom*> geoms; // list of geoms attached to this body
std::vector<mjCSite*> sites; // list of sites attached to this body
std::vector<mjCCamera*> cameras; // list of cameras
std::vector<mjCLight*> lights; // list of lights
std::vector<mjCFrame*> frames; // list of frames
std::vector<mjCBody*> bodies_; // list of bodies
std::vector<mjCJoint*> joints_; // list of joints allowing motion relative to parent
std::vector<mjCGeom*> geoms_; // list of geoms attached to this body
std::vector<mjCSite*> sites_; // list of sites attached to this body
std::vector<mjCCamera*> cameras_; // list of cameras
std::vector<mjCLight*> lights_; // list of lights
std::vector<mjCFrame*> frames_; // list of frames
// array of pointers to each object list (enumerated by type)
std::array<std::vector<mjCBase*>*, mjNOBJECT> object_lists;
std::array<std::vector<mjCBase*>*, mjNOBJECT> object_lists_;
// add object of any type
template <class T> T* AddObject(std::vector<T*>& list, std::string type);
// add object of any type, with defaults parameter
template <class T> T* AddObjectDefault(std::vector<T*>& list, std::string type,
mjCDef* def);
// copy vector of elements to this model
template <class T> void CopyList(std::vector<T*>& dest,
const std::vector<T*>& sources,
std::map<mjCDef*, int>& def_map,
const std::vector<mjCDef*>& defaults);
// delete from list the elements that are compatible with other but not this model
template <class T> void RemoveFromList(std::vector<T*>& list, const mjCModel& other);
// create mjCBase lists from children lists
void CreateObjectLists();
src/user/user_objects.cc
+208 -133
View File
@@ -45,11 +45,11 @@
#include "engine/engine_util_misc.h"
#include "engine/engine_util_solve.h"
#include "engine/engine_util_spatial.h"
#include "engine/engine_vfs.h"
#include "user/user_api.h"
#include "user/user_cache.h"
#include "user/user_model.h"
#include "user/user_util.h"
#include "user/user_vfs.h"
namespace {
namespace mju = ::mujoco::util;
@@ -70,7 +70,7 @@ static void checksize(double* size, mjtGeom type, mjCBase* object, const char* n
else {
for (int i=0; i<mjGEOMINFO[type]; i++) {
if (size[i]<=0) {
throw mjCError(object, "size %d must be positive in geom", "", i);
throw mjCError(object, "size %d must be positive in geom", nullptr, i);
}
}
}
@@ -399,6 +399,7 @@ void mjCBoundingVolumeHierarchy::Set(mjtNum ipos_element[3], mjtNum iquat_elemen
void mjCBoundingVolumeHierarchy::AllocateBoundingVolumes(int nleaf) {
nbvh = 0;
bvh.clear();
child.clear();
nodeid.clear();
level.clear();
@@ -556,22 +557,22 @@ int mjCBoundingVolumeHierarchy::MakeBVH(
//------------------------- class mjCDef implementation --------------------------------------------
// constructor
mjCDef::mjCDef(void) {
mjCDef::mjCDef() {
name.clear();
parentid = -1;
childid.clear();
mjs_defaultJoint(&joint.spec);
mjs_defaultGeom(&geom.spec);
mjs_defaultSite(&site.spec);
mjs_defaultCamera(&camera.spec);
mjs_defaultLight(&light.spec);
mjs_defaultFlex(&flex.spec);
mjs_defaultMesh(&mesh.spec);
mjs_defaultMaterial(&material.spec);
mjs_defaultPair(&pair.spec);
mjs_defaultEquality(&equality.spec);
mjs_defaultTendon(&tendon.spec);
mjs_defaultActuator(&actuator.spec);
mjs_defaultJoint(&joint_.spec);
mjs_defaultGeom(&geom_.spec);
mjs_defaultSite(&site_.spec);
mjs_defaultCamera(&camera_.spec);
mjs_defaultLight(&light_.spec);
mjs_defaultFlex(&flex_.spec);
mjs_defaultMesh(&mesh_.spec);
mjs_defaultMaterial(&material_.spec);
mjs_defaultPair(&pair_.spec);
mjs_defaultEquality(&equality_.spec);
mjs_defaultTendon(&tendon_.spec);
mjs_defaultActuator(&actuator_.spec);
// make sure all the pointers are local
PointToLocal();
@@ -591,12 +592,12 @@ void mjCDef::Compile(const mjCModel* model) {
CopyFromSpec();
// enforce length of all default userdata arrays
joint.userdata_.resize(model->nuser_jnt);
geom.userdata_.resize(model->nuser_geom);
site.userdata_.resize(model->nuser_site);
camera.userdata_.resize(model->nuser_cam);
tendon.userdata_.resize(model->nuser_tendon);
actuator.userdata_.resize(model->nuser_actuator);
joint_.userdata_.resize(model->nuser_jnt);
geom_.userdata_.resize(model->nuser_geom);
site_.userdata_.resize(model->nuser_site);
camera_.userdata_.resize(model->nuser_cam);
tendon_.userdata_.resize(model->nuser_tendon);
actuator_.userdata_.resize(model->nuser_actuator);
}
@@ -607,18 +608,18 @@ mjCDef& mjCDef::operator=(const mjCDef& other) {
name = other.name;
parentid = other.parentid;
childid = other.childid;
joint = other.joint;
geom = other.geom;
site = other.site;
camera = other.camera;
light = other.light;
flex = other.flex;
mesh = other.mesh;
material = other.material;
pair = other.pair;
equality = other.equality;
tendon = other.tendon;
actuator = other.actuator;
joint_ = other.joint_;
geom_ = other.geom_;
site_ = other.site_;
camera_ = other.camera_;
light_ = other.light_;
flex_ = other.flex_;
mesh_ = other.mesh_;
material_ = other.material_;
pair_ = other.pair_;
equality_ = other.equality_;
tendon_ = other.tendon_;
actuator_ = other.actuator_;
}
PointToLocal();
return *this;
@@ -627,49 +628,49 @@ mjCDef& mjCDef::operator=(const mjCDef& other) {
void mjCDef::PointToLocal() {
joint.PointToLocal();
geom.PointToLocal();
site.PointToLocal();
camera.PointToLocal();
light.PointToLocal();
flex.PointToLocal();
mesh.PointToLocal();
material.PointToLocal();
pair.PointToLocal();
equality.PointToLocal();
tendon.PointToLocal();
actuator.PointToLocal();
joint_.PointToLocal();
geom_.PointToLocal();
site_.PointToLocal();
camera_.PointToLocal();
light_.PointToLocal();
flex_.PointToLocal();
mesh_.PointToLocal();
material_.PointToLocal();
pair_.PointToLocal();
equality_.PointToLocal();
tendon_.PointToLocal();
actuator_.PointToLocal();
spec.element = static_cast<mjElement*>(this);
spec.name = (mjString)&name;
spec.joint = &joint.spec;
spec.geom = &geom.spec;
spec.site = &site.spec;
spec.camera = &camera.spec;
spec.light = &light.spec;
spec.flex = &flex.spec;
spec.mesh = &mesh.spec;
spec.material = &material.spec;
spec.pair = &pair.spec;
spec.equality = &equality.spec;
spec.tendon = &tendon.spec;
spec.actuator = &actuator.spec;
spec.joint = &joint_.spec;
spec.geom = &geom_.spec;
spec.site = &site_.spec;
spec.camera = &camera_.spec;
spec.light = &light_.spec;
spec.flex = &flex_.spec;
spec.mesh = &mesh_.spec;
spec.material = &material_.spec;
spec.pair = &pair_.spec;
spec.equality = &equality_.spec;
spec.tendon = &tendon_.spec;
spec.actuator = &actuator_.spec;
}
void mjCDef::CopyFromSpec() {
joint.CopyFromSpec();
geom.CopyFromSpec();
site.CopyFromSpec();
camera.CopyFromSpec();
light.CopyFromSpec();
flex.CopyFromSpec();
mesh.CopyFromSpec();
material.CopyFromSpec();
pair.CopyFromSpec();
equality.CopyFromSpec();
tendon.CopyFromSpec();
actuator.CopyFromSpec();
joint_.CopyFromSpec();
geom_.CopyFromSpec();
site_.CopyFromSpec();
camera_.CopyFromSpec();
light_.CopyFromSpec();
flex_.CopyFromSpec();
mesh_.CopyFromSpec();
material_.CopyFromSpec();
pair_.CopyFromSpec();
equality_.CopyFromSpec();
tendon_.CopyFromSpec();
actuator_.CopyFromSpec();
}
@@ -1254,6 +1255,53 @@ mjCBase* mjCBody::FindObject(mjtObj type, string _name, bool recursive) {
template <class T>
static mjElement* GetNext(std::vector<T*>& list, mjElement* child) {
for (unsigned int i = 0; i < list.size()-1; i++) {
if (list[i]->spec.element == child) {
return list[i+1]->spec.element;
}
}
return nullptr;
}
// get next child of given type
mjElement* mjCBody::NextChild(mjElement* child, mjtObj type) {
if (type == mjOBJ_UNKNOWN) {
if (!child) {
throw mjCError(this, "child type must be specified if no child element is given");
} else {
type = child->elemtype;
}
} else if (child && child->elemtype != type) {
throw mjCError(this, "child element is not of requested type");
}
switch (type) {
case mjOBJ_BODY:
case mjOBJ_XBODY:
return child ? GetNext(bodies, child) : bodies[0];
case mjOBJ_JOINT:
return child ? GetNext(joints, child) : joints[0];
case mjOBJ_GEOM:
return child ? GetNext(geoms, child) : geoms[0];
case mjOBJ_SITE:
return child ? GetNext(sites, child) : sites[0];
case mjOBJ_CAMERA:
return child ? GetNext(cameras, child) : cameras[0];
case mjOBJ_LIGHT:
return child ? GetNext(lights, child) : lights[0];
case mjOBJ_FRAME:
return child ? GetNext(frames, child) : frames[0];
default:
return nullptr;
}
}
// compute geom inertial frame: ipos, iquat, mass, inertia
void mjCBody::GeomFrame(void) {
int sz;
@@ -1333,6 +1381,23 @@ void mjCBody::MakeInertialExplicit() {
}
// compute bounding volume hierarchy
void mjCBody::ComputeBVH() {
if (geoms.empty()) {
return;
}
tree.Set(ipos, iquat);
tree.AllocateBoundingVolumes(geoms.size());
for (int i=0; i<geoms.size(); i++) {
geoms[i]->SetBoundingVolume(tree.GetBoundingVolume(i));
}
tree.CreateBVH();
}
// compiler
void mjCBody::Compile(void) {
CopyFromSpec();
@@ -1447,14 +1512,7 @@ void mjCBody::Compile(void) {
}
// compute bounding volume hierarchy
if (!geoms.empty()) {
tree.Set(ipos, iquat);
tree.AllocateBoundingVolumes(geoms.size());
for (int i=0; i<geoms.size(); i++) {
geoms[i]->SetBoundingVolume(tree.GetBoundingVolume(i));
}
tree.CreateBVH();
}
ComputeBVH();
// compile all joints, count dofs
dofnum = 0;
@@ -1486,7 +1544,7 @@ void mjCBody::Compile(void) {
// compute body global pose (no joint transformations in qpos0)
if (id>0) {
mjCBody* par = model->bodies[parentid];
mjCBody* par = model->Bodies()[parentid];
mju_rotVecQuat(xpos0, pos, par->xquat0);
mju_addTo3(xpos0, par->xpos0);
mju_mulQuat(xquat0, par->xquat0, quat);
@@ -1663,12 +1721,12 @@ mjCJoint::mjCJoint(mjCModel* _model, mjCDef* _def) {
// reset to default if given
if (_def) {
*this = _def->joint;
*this = _def->Joint();
}
// set model, def
model = _model;
def = (_def ? _def : (_model ? _model->defaults[0] : 0));
def = (_def ? _def : (_model ? _model->Defaults(0) : 0));
// point to local
PointToLocal();
@@ -1867,12 +1925,12 @@ mjCGeom::mjCGeom(mjCModel* _model, mjCDef* _def) {
// reset to default if given
if (_def) {
*this = _def->geom;
*this = _def->Geom();
}
// set model, def
model = _model;
def = (_def ? _def : (_model ? _model->defaults[0] : 0));
def = (_def ? _def : (_model ? _model->Defaults(0) : 0));
// point to local
PointToLocal();
@@ -1953,12 +2011,12 @@ void mjCGeom::NameSpace(const mjCModel* m) {
// compute geom volume
double mjCGeom::GetVolume(void) {
double mjCGeom::GetVolume() const {
double height;
// get from mesh
if (type==mjGEOM_MESH || type==mjGEOM_SDF) {
if (mesh->id<0 || !((std::size_t) mesh->id <= model->meshes.size())) {
if (mesh->id<0 || !((std::size_t) mesh->id <= model->Meshes().size())) {
throw mjCError(this, "invalid mesh id in mesh geom");
}
@@ -2011,7 +2069,7 @@ void mjCGeom::SetInertia(void) {
// get from mesh
if (type==mjGEOM_MESH || type==mjGEOM_SDF) {
if (mesh->id<0 || !((std::size_t) mesh->id <= model->meshes.size())) {
if (mesh->id<0 || !((std::size_t) mesh->id <= model->Meshes().size())) {
throw mjCError(this, "invalid mesh id in mesh geom");
}
@@ -2411,8 +2469,7 @@ void mjCGeom::Compile(void) {
if (type==mjGEOM_HFIELD) {
size[0] = hfield->size[0];
size[1] = hfield->size[1];
size[2] = 0.5*(0.5*hfield->size[2] +
hfield->size[3]);
size[2] = 0.25 * hfield->size[2] + 0.5 * hfield->size[3];
} else if (type==mjGEOM_MESH || type==mjGEOM_SDF) {
const double* aamm = mesh->aamm();
size[0] = mju_max(fabs(aamm[0]), fabs(aamm[3]));
@@ -2502,7 +2559,7 @@ mjCSite::mjCSite(mjCModel* _model, mjCDef* _def) {
// reset to default if given
if (_def) {
*this = _def->site;
*this = _def->Site();
}
// point to local
@@ -2513,7 +2570,7 @@ mjCSite::mjCSite(mjCModel* _model, mjCDef* _def) {
// set model, def
model = _model;
def = (_def ? _def : (_model ? _model->defaults[0] : 0));
def = (_def ? _def : (_model ? _model->Defaults(0) : 0));
}
@@ -2654,12 +2711,12 @@ mjCCamera::mjCCamera(mjCModel* _model, mjCDef* _def) {
// reset to default if given
if (_def) {
*this = _def->camera;
*this = _def->Camera();
}
// set model, def
model = _model;
def = (_def ? _def : (_model ? _model->defaults[0] : 0));
def = (_def ? _def : (_model ? _model->Defaults(0) : 0));
// point to local
PointToLocal();
@@ -2808,12 +2865,12 @@ mjCLight::mjCLight(mjCModel* _model, mjCDef* _def) {
// reset to default if given
if (_def) {
*this = _def->light;
*this = _def->Light();
}
// set model, def
model = _model;
def = (_def ? _def : (_model ? _model->defaults[0] : 0));
def = (_def ? _def : (_model ? _model->Defaults(0) : 0));
PointToLocal();
CopyFromSpec();
@@ -3845,12 +3902,12 @@ mjCMaterial::mjCMaterial(mjCModel* _model, mjCDef* _def) {
// reset to default if given
if (_def) {
*this = _def->material;
*this = _def->Material();
}
// set model, def
model = _model;
def = (_def ? _def : (_model ? _model->defaults[0] : 0));
def = (_def ? _def : (_model ? _model->Defaults(0) : 0));
// point to local
PointToLocal();
@@ -3933,12 +3990,12 @@ mjCPair::mjCPair(mjCModel* _model, mjCDef* _def) {
// reset to default if given
if (_def) {
*this = _def->pair;
*this = _def->Pair();
}
// set model, def
model = _model;
def = (_def ? _def : (_model ? _model->defaults[0] : 0));
def = (_def ? _def : (_model ? _model->Defaults(0) : 0));
// point to local
PointToLocal();
@@ -4294,12 +4351,12 @@ mjCEquality::mjCEquality(mjCModel* _model, mjCDef* _def) {
// reset to default if given
if (_def) {
*this = _def->equality;
*this = _def->Equality();
}
// set model, def
model = _model;
def = (_def ? _def : (_model ? _model->defaults[0] : 0));
def = (_def ? _def : (_model ? _model->Defaults(0) : 0));
// point to local
PointToLocal();
@@ -4434,7 +4491,7 @@ void mjCEquality::Compile(void) {
ResolveReferences(model);
// make sure flex is not rigid
if (type==mjEQ_FLEX && model->flexes[obj1id]->rigid) {
if (type==mjEQ_FLEX && model->Flexes()[obj1id]->rigid) {
throw mjCError(this, "rigid flex '%s' in equality constraint %d", name1_.c_str(), id);
}
}
@@ -4456,12 +4513,12 @@ mjCTendon::mjCTendon(mjCModel* _model, mjCDef* _def) {
// reset to default if given
if (_def) {
*this = _def->tendon;
*this = _def->Tendon();
}
// set model, def
model = _model;
def = (_def ? _def : (_model ? _model->defaults[0] : 0));
def = (_def ? _def : (_model ? _model->Defaults(0) : 0));
// point to local
PointToLocal();
@@ -4619,19 +4676,18 @@ void mjCTendon::WrapPulley(double divisor, std::string_view info) {
// get number of wraps
int mjCTendon::NumWraps(void) {
int mjCTendon::NumWraps() const {
return (int)path.size();
}
// get pointer to specified wrap
mjCWrap* mjCTendon::GetWrap(int id) {
if (id>=0 && id<(int)path.size()) {
return path[id];
} else {
return 0;
const mjCWrap* mjCTendon::GetWrap(int i) const {
if (i >= 0 && i < (int)path.size()) {
return path[i];
}
return nullptr;
}
@@ -4753,7 +4809,7 @@ void mjCTendon::Compile(void) {
}
// mark geoms as non visual
model->geoms[path[i]->obj->id]->SetNotVisual();
model->Geoms()[path[i]->obj->id]->SetNotVisual();
break;
case mjWRAP_JOINT:
@@ -4935,12 +4991,12 @@ mjCActuator::mjCActuator(mjCModel* _model, mjCDef* _def) {
// reset to default if given
if (_def) {
*this = _def->actuator;
*this = _def->Actuator();
}
// set model, def
model = _model;
def = (_def ? _def : (_model ? _model->defaults[0] : 0));
def = (_def ? _def : (_model ? _model->Defaults(0) : 0));
// in case this actuator is not compiled
CopyFromSpec();
@@ -5021,7 +5077,6 @@ void mjCActuator::CopyFromSpec() {
void mjCActuator::ResolveReferences(const mjCModel* m) {
mjCJoint* pjnt;
switch (trntype) {
case mjTRN_JOINT:
case mjTRN_JOINTINPARENT:
@@ -5031,14 +5086,6 @@ void mjCActuator::ResolveReferences(const mjCModel* m) {
throw mjCError(this,
"unknown transmission target '%s' for actuator id = %d", target_.c_str(), id);
}
pjnt = (mjCJoint*) ptarget;
// apply urdfeffort
if (pjnt->spec.urdfeffort>0) {
forcerange[0] = -pjnt->spec.urdfeffort;
forcerange[1] = pjnt->spec.urdfeffort;
forcelimited = mjLIMITED_TRUE;
}
break;
case mjTRN_SLIDERCRANK:
@@ -5125,7 +5172,7 @@ void mjCActuator::Compile(void) {
gainprm[0] == -biasprm[1] && inheritrange > 0) {
// semantic of actuator is the same as transmission, inheritrange is applicable
double* range;
if (dyntype == mjDYN_NONE) {
if (dyntype == mjDYN_NONE || dyntype == mjDYN_FILTEREXACT) {
// position actuator
range = ctrlrange;
} else if (dyntype == mjDYN_INTEGRATOR) {
@@ -5387,7 +5434,6 @@ void mjCSensor::ResolveReferences(const mjCModel* m) {
((mjCGeom*)obj)->SetNotVisual();
}
// get sensorized object id
} else if (type != mjSENS_CLOCK && type != mjSENS_PLUGIN && type != mjSENS_USER) {
throw mjCError(this, "invalid type in sensor");
}
@@ -5401,7 +5447,7 @@ void mjCSensor::ResolveReferences(const mjCModel* m) {
// find name
if (!ref) {
throw mjCError(this, "unrecognized name '%s' of reference frame object", refname_.c_str());
throw mjCError(this, "unrecognized name '%s' of object", refname_.c_str());
}
// must be attached to object with spatial frame
@@ -5459,7 +5505,7 @@ void mjCSensor::Compile(void) {
case mjSENS_CAMPROJECTION:
// must be attached to site
if (objtype!=mjOBJ_SITE) {
throw mjCError(this, "sensor must be attached to site: sensor");
throw mjCError(this, "sensor must be attached to site");
}
// set dim and datatype
@@ -5497,7 +5543,7 @@ void mjCSensor::Compile(void) {
case mjSENS_JOINTACTFRC:
// must be attached to joint
if (objtype!=mjOBJ_JOINT) {
throw mjCError(this, "sensor must be attached to joint: sensor");
throw mjCError(this, "sensor must be attached to joint");
}
// make sure joint is slide or hinge
@@ -5521,7 +5567,7 @@ void mjCSensor::Compile(void) {
case mjSENS_TENDONVEL:
// must be attached to tendon
if (objtype!=mjOBJ_TENDON) {
throw mjCError(this, "sensor must be attached to tendon: sensor");
throw mjCError(this, "sensor must be attached to tendon");
}
// set
@@ -5539,7 +5585,7 @@ void mjCSensor::Compile(void) {
case mjSENS_ACTUATORFRC:
// must be attached to actuator
if (objtype!=mjOBJ_ACTUATOR) {
throw mjCError(this, "sensor must be attached to actuator: sensor");
throw mjCError(this, "sensor must be attached to actuator");
}
// set
@@ -5558,7 +5604,7 @@ void mjCSensor::Compile(void) {
case mjSENS_BALLANGVEL:
// must be attached to joint
if (objtype!=mjOBJ_JOINT) {
throw mjCError(this, "sensor must be attached to joint: sensor");
throw mjCError(this, "sensor must be attached to joint");
}
// make sure joint is ball
@@ -5583,7 +5629,7 @@ void mjCSensor::Compile(void) {
case mjSENS_JOINTLIMITFRC:
// must be attached to joint
if (objtype!=mjOBJ_JOINT) {
throw mjCError(this, "sensor must be attached to joint: sensor");
throw mjCError(this, "sensor must be attached to joint");
}
// make sure joint has limit
@@ -5608,7 +5654,7 @@ void mjCSensor::Compile(void) {
case mjSENS_TENDONLIMITFRC:
// must be attached to tendon
if (objtype!=mjOBJ_TENDON) {
throw mjCError(this, "sensor must be attached to tendon: sensor");
throw mjCError(this, "sensor must be attached to tendon");
}
// make sure tendon has limit
@@ -5674,7 +5720,7 @@ void mjCSensor::Compile(void) {
case mjSENS_SUBTREEANGMOM:
// must be attached to body
if (objtype!=mjOBJ_BODY) {
throw mjCError(this, "sensor must be attached to body: sensor");
throw mjCError(this, "sensor must be attached to body");
}
// set
@@ -5687,6 +5733,34 @@ void mjCSensor::Compile(void) {
}
break;
case mjSENS_GEOMDIST:
case mjSENS_GEOMNORMAL:
case mjSENS_GEOMFROMTO:
// must be attached to body or geom
if ((objtype!=mjOBJ_BODY && objtype!=mjOBJ_GEOM) ||
(reftype!=mjOBJ_BODY && reftype!=mjOBJ_GEOM)) {
throw mjCError(this, "sensor must be attached to body or geom");
}
// objects must be different
if (objtype == reftype && obj == ref) {
throw mjCError(this, "1st body/geom must be different from 2nd body/geom");
}
// set
needstage = mjSTAGE_POS;
if (type==mjSENS_GEOMDIST) {
dim = 1;
datatype = mjDATATYPE_POSITIVE;
} else if (type==mjSENS_GEOMNORMAL) {
dim = 3;
datatype = mjDATATYPE_AXIS;
} else {
dim = 6;
datatype = mjDATATYPE_REAL;
}
break;
case mjSENS_CLOCK:
dim = 1;
needstage = mjSTAGE_POS;
@@ -5704,7 +5778,7 @@ void mjCSensor::Compile(void) {
throw mjCError(this,
"datatype AXIS requires dim=3 in sensor");
}
if (datatype==mjDATATYPE_QUATERNION && dim!=4) {
if (datatype==mjDATATYPE_QUATERNION && dim != 4) {
throw mjCError(this, "datatype QUATERNION requires dim=4 in sensor");
}
break;
@@ -5736,7 +5810,8 @@ void mjCSensor::Compile(void) {
}
// check cutoff for incompatible data types
if (cutoff>0 && (datatype==mjDATATYPE_AXIS || datatype==mjDATATYPE_QUATERNION)) {
if (cutoff > 0 && (datatype == mjDATATYPE_QUATERNION ||
(datatype == mjDATATYPE_AXIS && type != mjSENS_GEOMNORMAL))) {
throw mjCError(this, "cutoff applied to axis or quaternion datatype in sensor");
}
}
src/user/user_objects.h
+179 -156
View File
@@ -107,8 +107,8 @@ class mjCBoundingVolume {
void SetId(int val) { idval_ = val; }
private:
int idval_; // local id copy for nodes not storing their id's (e.g. faces)
const int* id_; // pointer to object id
int idval_; // local id copy for nodes not storing their id's (e.g. faces)
const int* id_; // pointer to object id
};
@@ -208,10 +208,11 @@ class mjCBase : public mjCBase_ {
mjCFrame* frame; // pointer to frame transformation
mjCModel* model; // pointer to model that created object
virtual ~mjCBase() = default; // destructor
protected:
mjCBase(); // constructor
mjCBase(const mjCBase& other); // copy constructor
virtual ~mjCBase() = default; // destructor
};
@@ -293,6 +294,9 @@ class mjCBody : public mjCBody_, private mjsBody {
// set explicitinertial to true
void MakeInertialExplicit();
// compute the bounding volume hierarchy of the body.
void ComputeBVH();
// variables set by user
mjsBody spec;
@@ -304,6 +308,9 @@ class mjCBody : public mjCBody_, private mjsBody {
// used by mjXWriter and mjCModel
const std::vector<double>& get_userdata() { return userdata_; }
// get next child of given type
mjElement* NextChild(mjElement* child, mjtObj type = mjOBJ_UNKNOWN);
private:
mjCBody(const mjCBody& other, mjCModel* _model); // copy constructor
mjCBody& operator=(const mjCBody& other); // copy assignment
@@ -351,6 +358,10 @@ class mjCFrame : public mjCFrame_, private mjsFrame {
friend class mjCModel;
public:
mjCFrame(mjCModel* = 0, mjCFrame* = 0);
mjCFrame(const mjCFrame& other);
mjCFrame& operator=(const mjCFrame& other);
mjsFrame spec;
using mjCBase::name;
using mjCBase::classname;
@@ -365,9 +376,6 @@ class mjCFrame : public mjCFrame_, private mjsFrame {
bool IsAncestor(const mjCFrame* child) const; // true if child is contained in this frame
private:
mjCFrame(mjCModel* = 0, mjCFrame* = 0); // constructor
mjCFrame(const mjCFrame& other); // copy constructor
mjCFrame& operator=(const mjCFrame& other); // copy assignment
void Compile(void); // compiler
mjCBody* body; // body that owns the frame
@@ -397,6 +405,10 @@ class mjCJoint : public mjCJoint_, private mjsJoint {
friend class mjXURDF;
public:
mjCJoint(mjCModel* = nullptr, mjCDef* = nullptr);
mjCJoint(const mjCJoint& other);
mjCJoint& operator=(const mjCJoint& other);
mjsJoint spec;
using mjCBase::name;
using mjCBase::classname;
@@ -405,18 +417,14 @@ class mjCJoint : public mjCJoint_, private mjsJoint {
void CopyFromSpec(void);
// used by mjXWriter and mjCModel
const std::vector<double>& get_userdata() { return userdata_; }
const double* get_range() { return range; }
const std::vector<double>& get_userdata() const { return userdata_; }
const double* get_range() const { return range; }
bool is_limited() const;
bool is_actfrclimited() const;
private:
mjCJoint(mjCModel* = 0, mjCDef* = 0);
mjCJoint(const mjCJoint& other); // copy constructor
mjCJoint& operator=(const mjCJoint& other); // copy assignment
int Compile(void); // compiler; return dofnum
void PointToLocal(void);
};
@@ -465,9 +473,13 @@ class mjCGeom : public mjCGeom_, private mjsGeom {
friend class mjXURDF;
public:
mjCGeom(mjCModel* = nullptr, mjCDef* = nullptr);
mjCGeom(const mjCGeom& other);
mjCGeom& operator=(const mjCGeom& other);
using mjCBase::name;
mjsGeom spec; // variables set by user
double GetVolume(void); // compute geom volume
double GetVolume() const; // compute geom volume
void SetInertia(void); // compute and set geom inertia
bool IsVisual(void) const { return visual_; }
void SetNotVisual(void) { visual_ = false; }
@@ -481,17 +493,13 @@ class mjCGeom : public mjCGeom_, private mjsGeom {
void SetBoundingVolume(mjCBoundingVolume* bv) const;
// used by mjXWriter and mjCModel
const std::vector<double>& get_userdata() { return userdata_; }
const std::string& get_hfieldname() { return spec_hfieldname_; }
const std::string& get_meshname() { return spec_meshname_; }
const std::string& get_material() { return spec_material_; }
const std::vector<double>& get_userdata() const { return userdata_; }
const std::string& get_hfieldname() const { return spec_hfieldname_; }
const std::string& get_meshname() const { return spec_meshname_; }
const std::string& get_material() const { return spec_material_; }
void del_material() { spec_material_.clear(); }
private:
mjCGeom(mjCModel* = 0, mjCDef* = 0);
mjCGeom(const mjCGeom& other); // copy constructor
mjCGeom& operator=(const mjCGeom& other); // copy assignment
void Compile(void); // compiler
double GetRBound(void); // compute bounding sphere radius
void ComputeAABB(void); // compute axis-aligned bounding box
@@ -530,6 +538,10 @@ class mjCSite : public mjCSite_, private mjsSite {
friend class mjXURDF;
public:
mjCSite(mjCModel* = nullptr, mjCDef* = nullptr);
mjCSite(const mjCSite& other);
mjCSite& operator=(const mjCSite& other);
mjsSite spec; // variables set by user
// use strings from mjCBase rather than mjStrings from mjsSite
@@ -538,15 +550,11 @@ class mjCSite : public mjCSite_, private mjsSite {
using mjCBase::info;
// used by mjXWriter and mjCModel
const std::vector<double>& get_userdata() { return userdata_; }
const std::string& get_material() { return material_; }
const std::vector<double>& get_userdata() const { return userdata_; }
const std::string& get_material() const { return material_; }
void del_material() { material_.clear(); }
private:
mjCSite(mjCModel* = 0, mjCDef* = 0); // constructor
mjCSite(const mjCSite& other); // copy constructor
mjCSite& operator=(const mjCSite& other); // copy assignment
void Compile(void); // compiler
void CopyFromSpec(); // copy spec into attributes
void PointToLocal(void);
@@ -575,20 +583,20 @@ class mjCCamera : public mjCCamera_, private mjsCamera {
friend class mjXWriter;
public:
mjCCamera(mjCModel* = nullptr, mjCDef* = nullptr);
mjCCamera(const mjCCamera& other);
mjCCamera& operator=(const mjCCamera& other);
mjsCamera spec;
using mjCBase::name;
using mjCBase::classname;
using mjCBase::info;
// used by mjXWriter and mjCModel
const std::string& get_targetbody() { return targetbody_; }
const std::vector<double>& get_userdata() { return userdata_; }
const std::string& get_targetbody() const { return targetbody_; }
const std::vector<double>& get_userdata() const { return userdata_; }
private:
mjCCamera(mjCModel* = 0, mjCDef* = 0); // constructor
mjCCamera(const mjCCamera& other); // copy constructor
mjCCamera& operator=(const mjCCamera& other); // copy assignment
void Compile(void); // compiler
void CopyFromSpec(void);
void PointToLocal(void);
@@ -615,19 +623,19 @@ class mjCLight : public mjCLight_, private mjsLight {
friend class mjXWriter;
public:
mjCLight(mjCModel* = nullptr, mjCDef* = nullptr);
mjCLight(const mjCLight& other);
mjCLight& operator=(const mjCLight& other);
mjsLight spec;
using mjCBase::name;
using mjCBase::classname;
using mjCBase::info;
// used by mjXWriter and mjCModel
const std::string& get_targetbody() { return targetbody_; }
const std::string& get_targetbody() const { return targetbody_; }
private:
mjCLight(mjCModel* = 0, mjCDef* = 0); // constructor
mjCLight(const mjCLight& other); // copy constructor
mjCLight& operator=(const mjCLight& other); // copy assignment
void Compile(void); // compiler
void CopyFromSpec(void);
void PointToLocal(void);
@@ -678,6 +686,10 @@ class mjCFlex: public mjCFlex_, private mjsFlex {
friend class mjXWriter;
public:
mjCFlex(mjCModel* = nullptr);
mjCFlex(const mjCFlex& other);
mjCFlex& operator=(const mjCFlex& other);
mjsFlex spec;
using mjCBase::name;
using mjCBase::classname;
@@ -689,21 +701,17 @@ class mjCFlex: public mjCFlex_, private mjsFlex {
void NameSpace(const mjCModel* m);
// used by mjXWriter and mjCModel
const std::string& get_material() { return material_; }
const std::vector<std::string>& get_vertbody() { return vertbody_; }
const std::vector<double>& get_vert() { return vert_; }
const std::vector<double>& get_elemaabb() { return elemaabb_; }
const std::vector<int>& get_elem() { return elem_; }
const std::vector<float>& get_texcoord() { return texcoord_; }
const std::string& get_material() const { return material_; }
const std::vector<std::string>& get_vertbody() const { return vertbody_; }
const std::vector<double>& get_vert() const { return vert_; }
const std::vector<double>& get_elemaabb() const { return elemaabb_; }
const std::vector<int>& get_elem() const { return elem_; }
const std::vector<float>& get_texcoord() const { return texcoord_; }
bool HasTexcoord() const; // texcoord not null
void DelTexcoord(); // delete texcoord
private:
mjCFlex(mjCModel* = 0);
mjCFlex(const mjCFlex& other); // copy constructor
mjCFlex& operator=(const mjCFlex& other); // copy assignment
void Compile(const mjVFS* vfs); // compiler
void CreateBVH(void); // create flex BVH
void CreateShellPair(void); // create shells and evpairs
@@ -776,9 +784,9 @@ class mjCMesh: public mjCMesh_, private mjsMesh {
friend class mjCFlexcomp;
friend class mjXWriter;
public:
mjCMesh(mjCModel* = 0, mjCDef* = 0);
mjCMesh(const mjCMesh& other); // copy constructor
mjCMesh& operator=(const mjCMesh& other); // copy assignment
mjCMesh(mjCModel* = nullptr, mjCDef* = nullptr);
mjCMesh(const mjCMesh& other);
mjCMesh& operator=(const mjCMesh& other);
~mjCMesh();
mjsMesh spec;
@@ -907,32 +915,32 @@ class mjCSkin: public mjCSkin_, private mjsSkin {
friend class mjXWriter;
public:
mjCSkin(mjCModel* = nullptr);
mjCSkin(const mjCSkin& other);
mjCSkin& operator=(const mjCSkin& other);
~mjCSkin();
mjsSkin spec;
using mjCBase::name;
using mjCBase::classname;
using mjCBase::info;
std::string get_file() const { return file_; }
std::string& get_material() { return material_; }
std::vector<float>& get_vert() { return vert_; }
std::vector<float>& get_texcoord() { return texcoord_; }
std::vector<int>& get_face() { return face_; }
std::vector<std::string>& get_bodyname() { return bodyname_; }
std::vector<float>& get_bindpos() { return bindpos_; }
std::vector<float>& get_bindquat() { return bindquat_; }
std::vector<std::vector<int>>& get_vertid() { return vertid_; }
std::vector<std::vector<float>>& get_vertweight() { return vertweight_; }
const std::string& get_file() const { return file_; }
const std::string& get_material() const { return material_; }
const std::vector<float>& get_vert() const { return vert_; }
const std::vector<float>& get_texcoord() const { return texcoord_; }
const std::vector<int>& get_face() const { return face_; }
const std::vector<std::string>& get_bodyname() const { return bodyname_; }
const std::vector<float>& get_bindpos() const { return bindpos_; }
const std::vector<float>& get_bindquat() const { return bindquat_; }
const std::vector<std::vector<int>>& get_vertid() const { return vertid_; }
const std::vector<std::vector<float>>& get_vertweight() const { return vertweight_; }
void del_material() { material_.clear(); }
void CopyFromSpec();
void PointToLocal();
private:
mjCSkin(mjCModel* = 0); // constructor
mjCSkin(const mjCSkin& other); // copy constructor
mjCSkin& operator=(const mjCSkin& other); // copy assignment
~mjCSkin(); // destructor
void ResolveReferences(const mjCModel* m);
void NameSpace(const mjCModel* m);
void Compile(const mjVFS* vfs); // compiler
@@ -962,6 +970,11 @@ class mjCHField : public mjCHField_, private mjsHField {
friend class mjXWriter;
public:
mjCHField(mjCModel* model);
mjCHField(const mjCHField& other);
mjCHField& operator=(const mjCHField& other);
~mjCHField();
mjsHField spec;
using mjCBase::name;
using mjCBase::info;
@@ -975,11 +988,6 @@ class mjCHField : public mjCHField_, private mjsHField {
std::vector<float>& get_userdata() { return userdata_; }
private:
mjCHField(mjCModel* model); // constructor
mjCHField(const mjCHField& other); // copy constructor
mjCHField& operator=(const mjCHField& other); // copy assignment
~mjCHField(); // destructor
void Compile(const mjVFS* vfs); // compiler
void LoadCustom(mjResource* resource); // load from custom format
@@ -1009,7 +1017,10 @@ class mjCTexture : public mjCTexture_, private mjsTexture {
friend class mjXWriter;
public:
~mjCTexture(); // destructor
mjCTexture(mjCModel*);
mjCTexture(const mjCTexture& other);
mjCTexture& operator=(const mjCTexture& other);
~mjCTexture();
mjsTexture spec;
using mjCBase::name;
@@ -1024,10 +1035,6 @@ class mjCTexture : public mjCTexture_, private mjsTexture {
std::vector<std::string> get_cubefiles() const { return cubefiles_; }
private:
mjCTexture(mjCModel*); // constructor
mjCTexture(const mjCTexture& other); // copy constructor
mjCTexture& operator=(const mjCTexture& other); // copy assignment
void Compile(const mjVFS* vfs); // compiler
void Builtin2D(void); // make builtin 2D
@@ -1066,6 +1073,10 @@ class mjCMaterial : public mjCMaterial_, private mjsMaterial {
friend class mjXWriter;
public:
mjCMaterial(mjCModel* = nullptr, mjCDef* = nullptr);
mjCMaterial(const mjCMaterial& other);
mjCMaterial& operator=(const mjCMaterial& other);
mjsMaterial spec;
using mjCBase::name;
using mjCBase::classname;
@@ -1075,14 +1086,10 @@ class mjCMaterial : public mjCMaterial_, private mjsMaterial {
void PointToLocal();
void NameSpace(const mjCModel* m);
std::string get_texture() { return texture_; }
const std::string& get_texture() const { return texture_; }
void del_texture() { texture_.clear(); }
private:
mjCMaterial(mjCModel* = 0, mjCDef* = 0); // constructor
mjCMaterial(const mjCMaterial& other); // copy constructor
mjCMaterial& operator=(const mjCMaterial& other); // copy assignment
void Compile(void); // compiler
};
@@ -1107,6 +1114,10 @@ class mjCPair : public mjCPair_, private mjsPair {
friend class mjXWriter;
public:
mjCPair(mjCModel* = nullptr, mjCDef* = nullptr);
mjCPair(const mjCPair& other);
mjCPair& operator=(const mjCPair& other);
mjsPair spec;
using mjCBase::name;
using mjCBase::classname;
@@ -1117,18 +1128,14 @@ class mjCPair : public mjCPair_, private mjsPair {
void ResolveReferences(const mjCModel* m);
void NameSpace(const mjCModel* m);
std::string get_geomname1() { return geomname1_; }
std::string get_geomname2() { return geomname2_; }
const std::string& get_geomname1() const { return geomname1_; }
const std::string& get_geomname2() const { return geomname2_; }
int GetSignature(void) {
return signature;
}
private:
mjCPair(mjCModel* = 0, mjCDef* = 0); // constructor
mjCPair(const mjCPair& other); // copy constructor
mjCPair& operator=(const mjCPair& other); // copy assignment
void Compile(void); // compiler
mjCGeom* geom1; // geom1
@@ -1157,6 +1164,10 @@ class mjCBodyPair : public mjCBodyPair_, private mjsExclude {
friend class mjCModel;
public:
mjCBodyPair(mjCModel*);
mjCBodyPair(const mjCBodyPair& other);
mjCBodyPair& operator=(const mjCBodyPair& other);
mjsExclude spec;
using mjCBase::name;
using mjCBase::info;
@@ -1174,9 +1185,6 @@ class mjCBodyPair : public mjCBodyPair_, private mjsExclude {
}
private:
mjCBodyPair(mjCModel*); // constructor
mjCBodyPair(const mjCBodyPair& other); // copy constructor
mjCBodyPair& operator=(const mjCBodyPair& other); // copy assignment
void Compile(void); // compiler
};
@@ -1203,6 +1211,10 @@ class mjCEquality : public mjCEquality_, private mjsEquality {
friend class mjXWriter;
public:
mjCEquality(mjCModel* = 0, mjCDef* = 0);
mjCEquality(const mjCEquality& other);
mjCEquality& operator=(const mjCEquality& other);
mjsEquality spec;
using mjCBase::name;
using mjCBase::classname;
@@ -1214,10 +1226,6 @@ class mjCEquality : public mjCEquality_, private mjsEquality {
void NameSpace(const mjCModel* m);
private:
mjCEquality(mjCModel* = 0, mjCDef* = 0); // constructor
mjCEquality(const mjCEquality& other); // copy constructor
mjCEquality& operator=(const mjCEquality& other); // copy assignment
void Compile(void); // compiler
};
@@ -1243,13 +1251,18 @@ class mjCTendon : public mjCTendon_, private mjsTendon {
friend class mjXWriter;
public:
mjCTendon(mjCModel* = nullptr, mjCDef* = nullptr);
mjCTendon(const mjCTendon& other);
mjCTendon& operator=(const mjCTendon& other);
~mjCTendon();
mjsTendon spec;
using mjCBase::name;
using mjCBase::classname;
using mjCBase::info;
void set_material(std::string _material) { material_ = _material; }
std::string& get_material() { return material_; }
const std::string& get_material() const { return material_; }
void del_material() { material_.clear(); }
// API for adding wrapping objects
@@ -1259,12 +1272,12 @@ class mjCTendon : public mjCTendon_, private mjsTendon {
void WrapPulley(double divisor, std::string_view info = ""); // pulley
// API for access to wrapping objects
int NumWraps(void); // number of wraps
mjCWrap* GetWrap(int); // pointer to wrap
int NumWraps() const; // number of wraps
const mjCWrap* GetWrap(int i) const; // pointer to wrap
std::vector<mjCWrap*> path; // wrapping objects
// used by mjXWriter and mjCModel
const std::vector<double>& get_userdata() { return userdata_; }
const std::vector<double>& get_userdata() const { return userdata_; }
const double* get_range() { return range; }
void CopyFromSpec();
@@ -1276,11 +1289,6 @@ class mjCTendon : public mjCTendon_, private mjsTendon {
bool is_limited() const;
private:
mjCTendon(mjCModel* = 0, mjCDef* = 0); // constructor
mjCTendon(const mjCTendon& other); // copy constructor
mjCTendon& operator=(const mjCTendon& other); // copy assignment
~mjCTendon(); // destructor
void Compile(void); // compiler
};
@@ -1339,14 +1347,13 @@ class mjCPlugin : public mjCPlugin_ {
friend class mjXWriter;
public:
mjCPlugin(mjCModel*);
mjCPlugin(const mjCPlugin& other);
mjCPlugin& operator=(const mjCPlugin& other);
mjsPlugin spec;
mjCBase* parent; // parent object (only used when generating error message)
mjCPlugin(const mjCPlugin& other); // copy constructor
private:
mjCPlugin(mjCModel*); // constructor
mjCPlugin& operator=(const mjCPlugin& other); // copy assignment
void Compile(void); // compiler
};
@@ -1378,26 +1385,26 @@ class mjCActuator : public mjCActuator_, private mjsActuator {
friend class mjXWriter;
public:
mjCActuator(mjCModel* = nullptr, mjCDef* = nullptr);
mjCActuator(const mjCActuator& other);
mjCActuator& operator=(const mjCActuator& other);
mjsActuator spec;
using mjCBase::name;
using mjCBase::classname;
using mjCBase::info;
// used by mjXWriter and mjCModel
const std::vector<double>& get_userdata() { return userdata_; }
const std::string& get_target() { return spec_target_; }
const std::string& get_slidersite() { return spec_slidersite_; }
const std::string& get_refsite() { return spec_refsite_; }
const std::vector<double>& get_userdata() const { return userdata_; }
const std::string& get_target() const { return spec_target_; }
const std::string& get_slidersite() const { return spec_slidersite_; }
const std::string& get_refsite() const { return spec_refsite_; }
bool is_ctrllimited() const;
bool is_forcelimited() const;
bool is_actlimited() const;
private:
mjCActuator(mjCModel* = 0, mjCDef* = 0); // constructor
mjCActuator(const mjCActuator& other); // copy constructor
mjCActuator& operator=(const mjCActuator& other); // copy assignment
void Compile(void); // compiler
void CopyFromSpec();
void PointToLocal();
@@ -1433,6 +1440,10 @@ class mjCSensor : public mjCSensor_, private mjsSensor {
friend class mjXWriter;
public:
mjCSensor(mjCModel*);
mjCSensor(const mjCSensor& other);
mjCSensor& operator=(const mjCSensor& other);
mjsSensor spec;
using mjCBase::name;
using mjCBase::classname;
@@ -1444,10 +1455,6 @@ class mjCSensor : public mjCSensor_, private mjsSensor {
const std::string& get_refname() { return spec_refname_; }
private:
mjCSensor(mjCModel*); // constructor
mjCSensor(const mjCSensor& other); // copy constructor
mjCSensor& operator=(const mjCSensor& other); // copy assignment
void Compile(void); // compiler
void CopyFromSpec();
void PointToLocal();
@@ -1474,6 +1481,11 @@ class mjCNumeric : public mjCNumeric_, private mjsNumeric {
friend class mjXWriter;
public:
mjCNumeric(mjCModel*);
mjCNumeric(const mjCNumeric& other);
mjCNumeric& operator=(const mjCNumeric& other);
~mjCNumeric();
mjsNumeric spec;
using mjCBase::name;
using mjCBase::info;
@@ -1482,11 +1494,6 @@ class mjCNumeric : public mjCNumeric_, private mjsNumeric {
void CopyFromSpec();
private:
mjCNumeric(mjCModel*); // constructor
mjCNumeric(const mjCNumeric& other); // copy constructor
mjCNumeric& operator=(const mjCNumeric& other); // copy assignment
~mjCNumeric(); // destructor
void Compile(void); // compiler
};
@@ -1506,6 +1513,11 @@ class mjCText : public mjCText_, private mjsText {
friend class mjXWriter;
public:
mjCText(mjCModel*);
mjCText(const mjCText& other);
mjCText& operator=(const mjCText& other);
~mjCText();
mjsText spec;
using mjCBase::name;
using mjCBase::info;
@@ -1514,11 +1526,6 @@ class mjCText : public mjCText_, private mjsText {
void CopyFromSpec();
private:
mjCText(mjCModel*); // constructor
mjCText(const mjCText& other); // copy constructor
mjCText& operator=(const mjCText& other); // copy assignment
~mjCText(); // destructor
void Compile(void); // compiler
};
@@ -1543,6 +1550,11 @@ class mjCTuple : public mjCTuple_, private mjsTuple {
friend class mjXWriter;
public:
mjCTuple(mjCModel*);
mjCTuple(const mjCTuple& other);
mjCTuple& operator=(const mjCTuple& other);
~mjCTuple();
mjsTuple spec;
using mjCBase::name;
using mjCBase::info;
@@ -1553,11 +1565,6 @@ class mjCTuple : public mjCTuple_, private mjsTuple {
void NameSpace(const mjCModel* m);
private:
mjCTuple(mjCModel*); // constructor
mjCTuple(const mjCTuple& other); // copy constructor
mjCTuple& operator=(const mjCTuple& other); // copy assignment
~mjCTuple(); // destructor
void Compile(void); // compiler
};
@@ -1587,6 +1594,11 @@ class mjCKey : public mjCKey_, private mjsKey {
friend class mjXWriter;
public:
mjCKey(mjCModel*);
mjCKey(const mjCKey& other);
mjCKey& operator=(const mjCKey& other);
~mjCKey();
mjsKey spec;
using mjCBase::name;
using mjCBase::info;
@@ -1595,11 +1607,6 @@ class mjCKey : public mjCKey_, private mjsKey {
void CopyFromSpec();
private:
mjCKey(mjCModel*); // constructor
mjCKey(const mjCKey& other); // copy constructor
mjCKey& operator=(const mjCKey& other); // copy assignment
~mjCKey(); // destructor
void Compile(const mjModel* m); // compiler
};
@@ -1612,13 +1619,29 @@ class mjCDef : public mjElement {
friend class mjXWriter;
public:
mjCDef(void); // constructor
mjCDef(const mjCDef& other); // copy constructor
void Compile(const mjCModel* model); // compiler
mjCDef& operator=(const mjCDef& other); // copy assignment
mjCDef();
mjCDef(const mjCDef& other);
mjCDef& operator=(const mjCDef& other);
void PointToLocal(void);
void CopyFromSpec(void);
void Compile(const mjCModel* model);
// accessors
mjCJoint& Joint() { return joint_; }
mjCGeom& Geom() { return geom_; }
mjCSite& Site() { return site_; }
mjCCamera& Camera() { return camera_; }
mjCLight& Light() { return light_; }
mjCFlex& Flex() { return flex_; }
mjCMesh& Mesh() { return mesh_; }
mjCMaterial& Material() { return material_; }
mjCPair& Pair() { return pair_; }
mjCEquality& Equality() { return equality_; }
mjCTendon& Tendon() { return tendon_; }
mjCActuator& Actuator() { return actuator_; }
// identifiers
std::string name; // class name
int parentid; // id of parent class
@@ -1626,19 +1649,19 @@ class mjCDef : public mjElement {
mjsDefault spec;
// default objects (TODO: they should become private)
mjCJoint joint;
mjCGeom geom;
mjCSite site;
mjCCamera camera;
mjCLight light;
mjCFlex flex;
mjCMesh mesh;
mjCMaterial material;
mjCPair pair;
mjCEquality equality;
mjCTendon tendon;
mjCActuator actuator;
private:
mjCJoint joint_;
mjCGeom geom_;
mjCSite site_;
mjCCamera camera_;
mjCLight light_;
mjCFlex flex_;
mjCMesh mesh_;
mjCMaterial material_;
mjCPair pair_;
mjCEquality equality_;
mjCTendon tendon_;
mjCActuator actuator_;
};
#endif // MUJOCO_SRC_USER_USER_OBJECTS_H_
src/engine/engine_vfs.c → src/user/user_vfs.c
+1 -1
View File
@@ -12,7 +12,7 @@
// See the License for the specific language governing permissions and
// limitations under the License.
#include "engine/engine_vfs.h"
#include "user/user_vfs.h"
#include <stddef.h>
#include <stdint.h>
src/engine/engine_vfs.h → src/user/user_vfs.h
+5 -3
View File
@@ -11,9 +11,11 @@
// 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.
// IWYU pragma: private, include "third_party/mujoco/include/mujoco.h"
// IWYU pragma: friend "third_party/mujoco/src/.*"
#ifndef MUJOCO_SRC_ENGINE_ENGINE_VFS_H_
#define MUJOCO_SRC_ENGINE_ENGINE_VFS_H_
#ifndef MUJOCO_SRC_USER_USER_VFS_H_
#define MUJOCO_SRC_USER_USER_VFS_H_
#include <stddef.h>
@@ -53,4 +55,4 @@ MJAPI mjResource* mju_openVfsResource(const char* name, const mjVFS* vfs);
}
#endif
#endif // MUJOCO_SRC_ENGINE_ENGINE_VFS_H_
#endif // MUJOCO_SRC_USER_USER_VFS_H_
src/xml/xml.cc
+17 -17
View File
@@ -35,9 +35,9 @@
#include "cc/array_safety.h"
#include "engine/engine_crossplatform.h"
#include "engine/engine_resource.h"
#include "engine/engine_vfs.h"
#include "user/user_api.h"
#include "user/user_util.h"
#include "user/user_vfs.h"
#include "xml/xml_native_reader.h"
#include "xml/xml_native_writer.h"
#include "xml/xml_urdf.h"
@@ -100,17 +100,17 @@ class LocaleOverride {
} // namespace
// Main writer function - calls mjXWrite
std::string mjWriteXML(mjSpec* model, char* error, int error_sz) {
std::string mjWriteXML(mjSpec* spec, char* error, int error_sz) {
LocaleOverride locale_override;
// check for empty model
if (!model) {
if (!spec) {
mjCopyError(error, "Cannot write empty model", error_sz);
return "";
}
mjXWriter writer;
writer.SetModel(model);
writer.SetModel(spec);
return writer.Write(error, error_sz);
}
@@ -286,7 +286,7 @@ mjSpec* mjParseXML(const char* filename, const mjVFS* vfs,
}
// clear
mjSpec* model = nullptr;
mjSpec* spec = nullptr;
if (error) {
error[0] = '\0';
}
@@ -345,14 +345,14 @@ mjSpec* mjParseXML(const char* filename, const mjVFS* vfs,
}
// create model, set filedir
model = mjs_createSpec();
spec = mjs_createSpec();
const char* dir;
int ndir = 0;
mju_getResourceDir(resource, &dir, &ndir);
if (dir != nullptr) {
mjs_setString(model->modelfiledir, std::string(dir, ndir).c_str());
mjs_setString(spec->modelfiledir, std::string(dir, ndir).c_str());
} else {
mjs_setString(model->modelfiledir, "");
mjs_setString(spec->modelfiledir, "");
}
// close resource
@@ -364,11 +364,11 @@ mjSpec* mjParseXML(const char* filename, const mjVFS* vfs,
// find include elements, replace them with subtree from xml file
std::unordered_set<std::string> included = {filename};
mjXReader parser;
parser.SetModelFileDir(mjs_getString(model->modelfiledir));
mjIncludeXML(parser, root, mjs_getString(model->modelfiledir), vfs, included);
parser.SetModelFileDir(mjs_getString(spec->modelfiledir));
mjIncludeXML(parser, root, mjs_getString(spec->modelfiledir), vfs, included);
// parse MuJoCo model
parser.SetModel(model);
parser.SetModel(spec);
parser.Parse(root);
}
@@ -378,11 +378,11 @@ mjSpec* mjParseXML(const char* filename, const mjVFS* vfs,
// set reasonable default for parsing a URDF
// this is separate from the Parser to allow multiple URDFs to be loaded.
model->strippath = true;
model->fusestatic = true;
model->discardvisual = true;
spec->strippath = true;
spec->fusestatic = true;
spec->discardvisual = true;
parser.SetModel(model);
parser.SetModel(spec);
parser.Parse(root);
}
@@ -394,11 +394,11 @@ mjSpec* mjParseXML(const char* filename, const mjVFS* vfs,
// catch known errors
catch (mjXError err) {
mjCopyError(error, err.message, error_sz);
mjs_deleteSpec(model);
mjs_deleteSpec(spec);
return nullptr;
}
return model;
return spec;
}
src/xml/xml.h
+1 -1
View File
@@ -24,7 +24,7 @@
// Top level API
// Main writer function
std::string mjWriteXML(mjSpec* model, char* error, int error_sz);
std::string mjWriteXML(mjSpec* spec, char* error, int error_sz);
// Main parser function
MJAPI mjSpec* mjParseXML(const char* filename, const mjVFS* vfs, char* error, int error_sz);
src/xml/xml_api.cc
+48 -17
View File
@@ -26,7 +26,10 @@
#include <type_traits>
#include <mujoco/mjmodel.h>
#include "engine/engine_io.h"
#include "engine/engine_resource.h"
#include "user/user_api.h"
#include "user/user_vfs.h"
#include "xml/xml.h"
#include "xml/xml_native_reader.h"
#include "xml/xml_util.h"
@@ -37,7 +40,7 @@
class GlobalModel {
public:
// deletes current model and takes ownership of model
void Set(mjSpec* model = nullptr);
void Set(mjSpec* spec = nullptr);
// writes XML to string
std::optional<std::string> ToXML(const mjModel* m, char* error,
@@ -46,30 +49,30 @@ class GlobalModel {
private:
// using raw pointers as GlobalModel needs to be trivially destructible
std::mutex* mutex_ = new std::mutex();
mjSpec* model_ = nullptr;
mjSpec* spec_ = nullptr;
};
std::optional<std::string> GlobalModel::ToXML(const mjModel* m, char* error,
int error_sz) {
std::lock_guard<std::mutex> lock(*mutex_);
if (!model_) {
if (!spec_) {
mjCopyError(error, "No XML model loaded", error_sz);
return std::nullopt;
}
mjs_copyBack(model_, m);
std::string result = mjWriteXML(model_, error, error_sz);
mjs_copyBack(spec_, m);
std::string result = mjWriteXML(spec_, error, error_sz);
if (result.empty()) {
return std::nullopt;
}
return result;
}
void GlobalModel::Set(mjSpec* model) {
void GlobalModel::Set(mjSpec* spec) {
std::lock_guard<std::mutex> lock(*mutex_);
if (model_ != nullptr) {
mjs_deleteSpec(model_);
if (spec_ != nullptr) {
mjs_deleteSpec(spec_);
}
model_ = model;
spec_ = spec;
}
@@ -91,29 +94,29 @@ mjModel* mj_loadXML(const char* filename, const mjVFS* vfs,
char* error, int error_sz) {
// parse new model
std::unique_ptr<mjSpec, std::function<void(mjSpec*)>> model(
std::unique_ptr<mjSpec, std::function<void(mjSpec*)>> spec(
mjParseXML(filename, vfs, error, error_sz),
[](mjSpec* m) { mjs_deleteSpec(m); });
if (!model) {
[](mjSpec* s) { mjs_deleteSpec(s); });
if (!spec) {
return nullptr;
}
// compile new model
mjModel* m = mjs_compile(model.get(), vfs);
mjModel* m = mjs_compile(spec.get(), vfs);
if (!m) {
mjCopyError(error, mjs_getError(model.get()), error_sz);
mjCopyError(error, mjs_getError(spec.get()), error_sz);
return nullptr;
}
// handle compile warning
if (mjs_isWarning(model.get())) {
mjCopyError(error, mjs_getError(model.get()), error_sz);
if (mjs_isWarning(spec.get())) {
mjCopyError(error, mjs_getError(spec.get()), error_sz);
} else if (error) {
error[0] = '\0';
}
// clear old and assign new
GetGlobalModel().Set(model.release());
GetGlobalModel().Set(spec.release());
return m;
}
@@ -178,3 +181,31 @@ int mj_printSchema(const char* filename, char* buffer, int buffer_sz, int flg_ht
// return string length
return str.str().size();
}
// load model from binary MJB resource
mjModel* mj_loadModel(const char* filename, const mjVFS* vfs) {
mjResource* resource = nullptr;
// first try vfs, otherwise try a provider or OS filesystem
if (!(resource = mju_openVfsResource(filename, vfs))) {
char error[1024];
if (!(resource = mju_openResource(filename, error, 1024))) {
mju_warning("%s", error);
return nullptr;
}
}
const void* buffer = NULL;
int buffer_sz = mju_readResource(resource, &buffer);
if (buffer_sz < 1) {
mju_closeResource(resource);
return nullptr;
}
mjModel* m = mj_loadModelBuffer(buffer, buffer_sz);
mju_closeResource(resource);
return m;
}
src/xml/xml_api.h
+3
View File
@@ -39,6 +39,9 @@ MJAPI void mj_freeLastXML(void);
MJAPI int mj_printSchema(const char* filename, char* buffer, int buffer_sz,
int flg_html, int flg_pad);
// load model from binary MJB file
// if vfs is not NULL, look up file in vfs before reading from disk
MJAPI mjModel* mj_loadModel(const char* filename, const mjVFS* vfs);
#ifdef __cplusplus
}
src/xml/xml_native_reader.cc
+113 -69
View File
@@ -177,19 +177,16 @@ const char* MJCF[nMJCF][mjXATTRNUM] = {
"dyntype", "gaintype", "biastype", "dynprm", "gainprm", "biasprm", "actearly"},
{"motor", "?", "8", "ctrllimited", "forcelimited", "ctrlrange", "forcerange",
"gear", "cranklength", "user", "group"},
{"position", "?", "11", "ctrllimited", "forcelimited", "ctrlrange", "inheritrange",
"forcerange", "gear", "cranklength", "user", "group",
"kp", "kv"},
{"position", "?", "13", "ctrllimited", "forcelimited", "ctrlrange", "inheritrange",
"forcerange", "gear", "cranklength", "user", "group", "kp", "kv", "dampratio", "timeconst"},
{"velocity", "?", "9", "ctrllimited", "forcelimited", "ctrlrange", "forcerange",
"gear", "cranklength", "user", "group",
"kv"},
{"intvelocity", "?", "12", "ctrllimited", "forcelimited",
"gear", "cranklength", "user", "group", "kv"},
{"intvelocity", "?", "13", "ctrllimited", "forcelimited",
"ctrlrange", "forcerange", "actrange", "inheritrange",
"gear", "cranklength", "user", "group",
"kp", "kv"},
"kp", "kv", "dampratio"},
{"damper", "?", "8", "forcelimited", "ctrlrange", "forcerange",
"gear", "cranklength", "user", "group",
"kv"},
"gear", "cranklength", "user", "group", "kv"},
{"cylinder", "?", "12", "ctrllimited", "forcelimited", "ctrlrange", "forcerange",
"gear", "cranklength", "user", "group",
"timeconst", "area", "diameter", "bias"},
@@ -390,22 +387,22 @@ const char* MJCF[nMJCF][mjXATTRNUM] = {
"ctrllimited", "forcelimited", "ctrlrange", "forcerange",
"lengthrange", "gear", "cranklength", "user",
"joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite"},
{"position", "*", "21", "name", "class", "group",
{"position", "*", "23", "name", "class", "group",
"ctrllimited", "forcelimited", "ctrlrange", "inheritrange", "forcerange",
"lengthrange", "gear", "cranklength", "user",
"joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite",
"kp", "kv"},
"kp", "kv", "dampratio", "timeconst"},
{"velocity", "*", "19", "name", "class", "group",
"ctrllimited", "forcelimited", "ctrlrange", "forcerange",
"lengthrange", "gear", "cranklength", "user",
"joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite",
"kv"},
{"intvelocity", "*", "22", "name", "class", "group",
{"intvelocity", "*", "23", "name", "class", "group",
"ctrllimited", "forcelimited",
"ctrlrange", "forcerange", "actrange", "inheritrange", "lengthrange",
"gear", "cranklength", "user",
"joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite",
"kp", "kv"},
"kp", "kv", "dampratio"},
{"damper", "*", "18", "name", "class", "group",
"forcelimited", "ctrlrange", "forcerange",
"lengthrange", "gear", "cranklength", "user",
@@ -472,6 +469,9 @@ const char* MJCF[nMJCF][mjXATTRNUM] = {
{"subtreecom", "*", "5", "name", "body", "cutoff", "noise", "user"},
{"subtreelinvel", "*", "5", "name", "body", "cutoff", "noise", "user"},
{"subtreeangmom", "*", "5", "name", "body", "cutoff", "noise", "user"},
{"distance", "*", "8", "name", "geom1", "geom2", "body1", "body2", "cutoff", "noise", "user"},
{"normal", "*", "8", "name", "geom1", "geom2", "body1", "body2", "cutoff", "noise", "user"},
{"fromto", "*", "8", "name", "geom1", "geom2", "body1", "body2", "cutoff", "noise", "user"},
{"clock", "*", "4", "name", "cutoff", "noise", "user"},
{"user", "*", "9", "name", "objtype", "objname", "datatype", "needstage",
"dim", "cutoff", "noise", "user"},
@@ -924,25 +924,25 @@ void mjXReader::Parse(XMLElement* root) {
// compiler section parser
void mjXReader::Compiler(XMLElement* section, mjSpec* mod) {
void mjXReader::Compiler(XMLElement* section, mjSpec* spec) {
string text;
int n;
// top-level attributes
if (MapValue(section, "autolimits", &n, bool_map, 2)) {
mod->autolimits = (n==1);
spec->autolimits = (n==1);
}
ReadAttr(section, "boundmass", 1, &mod->boundmass, text);
ReadAttr(section, "boundinertia", 1, &mod->boundinertia, text);
ReadAttr(section, "settotalmass", 1, &mod->settotalmass, text);
ReadAttr(section, "boundmass", 1, &spec->boundmass, text);
ReadAttr(section, "boundinertia", 1, &spec->boundinertia, text);
ReadAttr(section, "settotalmass", 1, &spec->settotalmass, text);
if (MapValue(section, "balanceinertia", &n, bool_map, 2)) {
mod->balanceinertia = (n==1);
spec->balanceinertia = (n==1);
}
if (MapValue(section, "strippath", &n, bool_map, 2)) {
mod->strippath = (n==1);
spec->strippath = (n==1);
}
if (MapValue(section, "fitaabb", &n, bool_map, 2)) {
mod->fitaabb = (n==1);
spec->fitaabb = (n==1);
}
if (MapValue(section, "coordinate", &n, coordinate_map, 2)) {
if (n==1) {
@@ -951,48 +951,48 @@ void mjXReader::Compiler(XMLElement* section, mjSpec* mod) {
}
}
if (MapValue(section, "angle", &n, angle_map, 2)) {
mod->degree = (n==1);
spec->degree = (n==1);
}
if (ReadAttrTxt(section, "eulerseq", text)) {
if (text.size()!=3) {
throw mjXError(section, "euler format must have length 3");
}
memcpy(mod->euler, text.c_str(), 3);
memcpy(spec->euler, text.c_str(), 3);
}
if (ReadAttrTxt(section, "assetdir", text)) {
mjs_setString(mod->meshdir, text.c_str());
mjs_setString(mod->texturedir, text.c_str());
mjs_setString(spec->meshdir, text.c_str());
mjs_setString(spec->texturedir, text.c_str());
}
// meshdir and texturedir take precedence over assetdir
std::string meshdir, texturedir;
if (ReadAttrTxt(section, "meshdir", meshdir)) {
mjs_setString(mod->meshdir, meshdir.c_str());
mjs_setString(spec->meshdir, meshdir.c_str());
};
if (ReadAttrTxt(section, "texturedir", texturedir)) {
mjs_setString(mod->texturedir, texturedir.c_str());
mjs_setString(spec->texturedir, texturedir.c_str());
}
if (MapValue(section, "discardvisual", &n, bool_map, 2)) {
mod->discardvisual = (n==1);
spec->discardvisual = (n==1);
}
if (MapValue(section, "convexhull", &n, bool_map, 2)) {
mod->convexhull = (n==1);
spec->convexhull = (n==1);
}
if (MapValue(section, "usethread", &n, bool_map, 2)) {
mod->usethread = (n==1);
spec->usethread = (n==1);
}
if (MapValue(section, "fusestatic", &n, bool_map, 2)) {
mod->fusestatic = (n==1);
spec->fusestatic = (n==1);
}
MapValue(section, "inertiafromgeom", &mod->inertiafromgeom, TFAuto_map, 3);
ReadAttr(section, "inertiagrouprange", 2, mod->inertiagrouprange, text);
MapValue(section, "inertiafromgeom", &spec->inertiafromgeom, TFAuto_map, 3);
ReadAttr(section, "inertiagrouprange", 2, spec->inertiagrouprange, text);
if (MapValue(section, "exactmeshinertia", &n, bool_map, 2)){
mod->exactmeshinertia = (n==1);
spec->exactmeshinertia = (n==1);
}
// lengthrange subelement
XMLElement* elem = FindSubElem(section, "lengthrange");
if (elem) {
mjLROpt* opt = &(mod->LRopt);
mjLROpt* opt = &(spec->LRopt);
// flags
MapValue(elem, "mode", &opt->mode, lrmode_map, lrmode_sz);
@@ -1110,7 +1110,7 @@ void mjXReader::Option(XMLElement* section, mjOption* opt) {
// size section parser
void mjXReader::Size(XMLElement* section, mjSpec* mod) {
void mjXReader::Size(XMLElement* section, mjSpec* spec) {
// read memory bytes
{
constexpr char err_msg[] =
@@ -1199,69 +1199,69 @@ void mjXReader::Size(XMLElement* section, mjSpec* mod) {
if (*memory / sizeof(mjtNum) > std::numeric_limits<std::size_t>::max()) {
throw mjXError(section, "%s", err_msg);
}
mod->memory = *memory;
spec->memory = *memory;
}
}
// read sizes
ReadAttrInt(section, "nuserdata", &mod->nuserdata);
ReadAttrInt(section, "nkey", &mod->nkey);
ReadAttrInt(section, "nuserdata", &spec->nuserdata);
ReadAttrInt(section, "nkey", &spec->nkey);
ReadAttrInt(section, "nconmax", &mod->nconmax);
if (mod->nconmax < -1) throw mjXError(section, "nconmax must be >= -1");
ReadAttrInt(section, "nconmax", &spec->nconmax);
if (spec->nconmax < -1) throw mjXError(section, "nconmax must be >= -1");
{
int nstack = -1;
const bool has_nstack = ReadAttrInt(section, "nstack", &nstack);
if (has_nstack) {
if (mod->nstack < -1) {
if (spec->nstack < -1) {
throw mjXError(section, "nstack must be >= -1");
}
if (mod->memory != -1 && nstack != -1) {
if (spec->memory != -1 && nstack != -1) {
throw mjXError(section,
"either 'memory' and 'nstack' attribute can be specified, not both");
}
mod->nstack = nstack;
spec->nstack = nstack;
}
}
{
int njmax = -1;
const bool has_njmax = ReadAttrInt(section, "njmax", &njmax);
if (has_njmax) {
if (mod->njmax < -1) {
if (spec->njmax < -1) {
throw mjXError(section, "njmax must be >= -1");
}
if (mod->memory != -1 && njmax != -1) {
if (spec->memory != -1 && njmax != -1) {
throw mjXError(section,
"either 'memory' and 'njmax' attribute can be specified, not both");
}
mod->njmax = njmax;
spec->njmax = njmax;
}
}
ReadAttrInt(section, "nuser_body", &mod->nuser_body);
if (mod->nuser_body < -1) throw mjXError(section, "nuser_body must be >= -1");
ReadAttrInt(section, "nuser_body", &spec->nuser_body);
if (spec->nuser_body < -1) throw mjXError(section, "nuser_body must be >= -1");
ReadAttrInt(section, "nuser_jnt", &mod->nuser_jnt);
if (mod->nuser_jnt < -1) throw mjXError(section, "nuser_jnt must be >= -1");
ReadAttrInt(section, "nuser_jnt", &spec->nuser_jnt);
if (spec->nuser_jnt < -1) throw mjXError(section, "nuser_jnt must be >= -1");
ReadAttrInt(section, "nuser_geom", &mod->nuser_geom);
if (mod->nuser_geom < -1) throw mjXError(section, "nuser_geom must be >= -1");
ReadAttrInt(section, "nuser_geom", &spec->nuser_geom);
if (spec->nuser_geom < -1) throw mjXError(section, "nuser_geom must be >= -1");
ReadAttrInt(section, "nuser_site", &mod->nuser_site);
if (mod->nuser_site < -1) throw mjXError(section, "nuser_site must be >= -1");
ReadAttrInt(section, "nuser_site", &spec->nuser_site);
if (spec->nuser_site < -1) throw mjXError(section, "nuser_site must be >= -1");
ReadAttrInt(section, "nuser_cam", &mod->nuser_cam);
if (mod->nuser_cam < -1) throw mjXError(section, "nuser_cam must be >= -1");
ReadAttrInt(section, "nuser_cam", &spec->nuser_cam);
if (spec->nuser_cam < -1) throw mjXError(section, "nuser_cam must be >= -1");
ReadAttrInt(section, "nuser_tendon", &mod->nuser_tendon);
if (mod->nuser_tendon < -1) throw mjXError(section, "nuser_tendon must be >= -1");
ReadAttrInt(section, "nuser_tendon", &spec->nuser_tendon);
if (spec->nuser_tendon < -1) throw mjXError(section, "nuser_tendon must be >= -1");
ReadAttrInt(section, "nuser_actuator", &mod->nuser_actuator);
if (mod->nuser_actuator < -1) throw mjXError(section, "nuser_actuator must be >= -1");
ReadAttrInt(section, "nuser_actuator", &spec->nuser_actuator);
if (spec->nuser_actuator < -1) throw mjXError(section, "nuser_actuator must be >= -1");
ReadAttrInt(section, "nuser_sensor", &mod->nuser_sensor);
if (mod->nuser_sensor < -1) throw mjXError(section, "nuser_sensor must be >= -1");
ReadAttrInt(section, "nuser_sensor", &spec->nuser_sensor);
if (spec->nuser_sensor < -1) throw mjXError(section, "nuser_sensor must be >= -1");
}
@@ -1882,13 +1882,13 @@ void mjXReader::OneEquality(XMLElement* elem, mjsEquality* pequality) {
case mjEQ_JOINT:
ReadAttrTxt(elem, "joint1", name1, true);
ReadAttrTxt(elem, "joint2", name2);
ReadAttr(elem, "polycoef", 5, pequality->data, text);
ReadAttr(elem, "polycoef", 5, pequality->data, text, false, false);
break;
case mjEQ_TENDON:
ReadAttrTxt(elem, "tendon1", name1, true);
ReadAttrTxt(elem, "tendon2", name2);
ReadAttr(elem, "polycoef", 5, pequality->data, text);
ReadAttr(elem, "polycoef", 5, pequality->data, text, false, false);
break;
case mjEQ_FLEX:
@@ -2085,12 +2085,33 @@ void mjXReader::OneActuator(XMLElement* elem, mjsActuator* pact) {
ReadAttr(elem, "kp", 1, pact->gainprm, text);
pact->biasprm[1] = -pact->gainprm[0];
if (ReadAttr(elem, "kv", 1, pact->biasprm + 2, text)) {
if (pact->biasprm[2] < 0)
throw mjXError(elem, "kv cannot be negative");
pact->biasprm[2] *= -1;
// read kv
double kv = -1; // -1: undefined
if (ReadAttr(elem, "kv", 1, &kv, text)) {
if (kv < 0) throw mjXError(elem, "kv cannot be negative");
}
// read dampratio
double dampratio = -1; // -1: undefined
if (ReadAttr(elem, "dampratio", 1, &dampratio, text)) {
if (dampratio < 0) throw mjXError(elem, "dampratio cannot be negative");
}
// set biasprm[2]; negative: regular damping, positive: dampratio
if (dampratio > 0 && kv > 0) {
throw mjXError(elem, "kv and dampratio cannot both be defined");
}
if (kv > 0) pact->biasprm[2] = -kv;
if (dampratio > 0) pact->biasprm[2] = dampratio;
// read timeconst, set dyntype
if (ReadAttr(elem, "timeconst", 1, pact->dynprm, text)) {
if (pact->dynprm[0] < 0)
throw mjXError(elem, "timeconst cannot be negative");
pact->dyntype = pact->dynprm[0] ? mjDYN_FILTEREXACT : mjDYN_NONE;
}
// handle inheritrange
ReadAttr(elem, "inheritrange", 1, &pact->inheritrange, text);
if (pact->inheritrange > 0) {
if (type == "position") {
@@ -3949,6 +3970,29 @@ void mjXReader::Sensor(XMLElement* section) {
ReadAttrTxt(elem, "body", objname, true);
}
// sensors for geometric distance; attached to geoms or bodies
else if (type=="distance" || type=="normal" || type=="fromto") {
bool has_body1 = ReadAttrTxt(elem, "body1", objname);
bool has_geom1 = ReadAttrTxt(elem, "geom1", objname);
if (has_body1 == has_geom1) {
throw mjXError(elem, "exactly one of (geom1, body1) must be specified");
}
psen->objtype = has_body1 ? mjOBJ_BODY : mjOBJ_GEOM;
bool has_body2 = ReadAttrTxt(elem, "body2", refname);
bool has_geom2 = ReadAttrTxt(elem, "geom2", refname);
if (has_body2 == has_geom2) {
throw mjXError(elem, "exactly one of (geom2, body2) must be specified");
}
psen->reftype = has_body2 ? mjOBJ_BODY : mjOBJ_GEOM;
if (type=="distance") {
psen->type = mjSENS_GEOMDIST;
} else if (type=="normal") {
psen->type = mjSENS_GEOMNORMAL;
} else {
psen->type = mjSENS_GEOMFROMTO;
}
}
// global sensors
else if (type=="clock") {
psen->type = mjSENS_CLOCK;
src/xml/xml_native_reader.h
+3 -3
View File
@@ -42,9 +42,9 @@ class mjXReader : public mjXBase {
void SetTextureDir(std::string texturedir);
// XML sections embedded in all formats
static void Compiler(tinyxml2::XMLElement* section, mjSpec* mod); // compiler section
static void Compiler(tinyxml2::XMLElement* section, mjSpec* spec); // compiler section
static void Option(tinyxml2::XMLElement* section, mjOption* opt); // option section
static void Size(tinyxml2::XMLElement* section, mjSpec* mod); // size section
static void Size(tinyxml2::XMLElement* section, mjSpec* spec); // size section
private:
// XML section specific to MJCF
@@ -99,7 +99,7 @@ class mjXReader : public mjXBase {
};
// MJCF schema
#define nMJCF 227
#define nMJCF 230
extern const char* MJCF[nMJCF][mjXATTRNUM];
#endif // MUJOCO_SRC_XML_XML_NATIVE_READER_H_
src/xml/xml_native_writer.cc
+222 -177
View File
@@ -84,7 +84,7 @@ XMLElement* mjXWriter::InsertEnd(XMLElement* parent, const char* name) {
//---------------------------------- class mjXWriter: one-element writers --------------------------
// write flex
void mjXWriter::OneFlex(XMLElement* elem, mjCFlex* pflex) {
void mjXWriter::OneFlex(XMLElement* elem, const mjCFlex* pflex) {
string text;
mjCFlex defflex;
@@ -152,7 +152,7 @@ void mjXWriter::OneFlex(XMLElement* elem, mjCFlex* pflex) {
// write mesh
void mjXWriter::OneMesh(XMLElement* elem, mjCMesh* pmesh, mjCDef* def) {
void mjXWriter::OneMesh(XMLElement* elem, const mjCMesh* pmesh, mjCDef* def) {
string text;
// regular
@@ -188,17 +188,17 @@ void mjXWriter::OneMesh(XMLElement* elem, mjCMesh* pmesh, mjCDef* def) {
}
// defaults and regular
WriteAttr(elem, "refpos", 3, pmesh->refpos, def->mesh.refpos);
WriteAttr(elem, "refquat", 4, pmesh->refquat, def->mesh.refquat);
WriteAttr(elem, "scale", 3, pmesh->scale, def->mesh.scale);
WriteAttr(elem, "refpos", 3, pmesh->refpos, def->Mesh().refpos);
WriteAttr(elem, "refquat", 4, pmesh->refquat, def->Mesh().refquat);
WriteAttr(elem, "scale", 3, pmesh->scale, def->Mesh().scale);
WriteAttrKey(elem, "smoothnormal", bool_map, 2, pmesh->get_smoothnormal(),
def->mesh.get_smoothnormal());
def->Mesh().get_smoothnormal());
}
// write skin
void mjXWriter::OneSkin(XMLElement* elem, mjCSkin* pskin) {
void mjXWriter::OneSkin(XMLElement* elem, const mjCSkin* pskin) {
string text;
mjCDef mydef;
float zero = 0;
@@ -208,7 +208,7 @@ void mjXWriter::OneSkin(XMLElement* elem, mjCSkin* pskin) {
WriteAttrTxt(elem, "file", pskin->get_file());
WriteAttrTxt(elem, "material", pskin->get_material());
WriteAttrInt(elem, "group", pskin->group, 0);
WriteAttr(elem, "rgba", 4, pskin->rgba, mydef.geom.rgba);
WriteAttr(elem, "rgba", 4, pskin->rgba, mydef.Geom().rgba);
WriteAttr(elem, "inflate", 1, &pskin->inflate, &zero);
// write data if no file
@@ -251,7 +251,7 @@ void mjXWriter::OneSkin(XMLElement* elem, mjCSkin* pskin) {
// write material
void mjXWriter::OneMaterial(XMLElement* elem, mjCMaterial* pmat, mjCDef* def) {
void mjXWriter::OneMaterial(XMLElement* elem, const mjCMaterial* pmat, mjCDef* def) {
// regular
if (!writingdefaults) {
WriteAttrTxt(elem, "name", pmat->name);
@@ -259,24 +259,24 @@ void mjXWriter::OneMaterial(XMLElement* elem, mjCMaterial* pmat, mjCDef* def) {
}
// defaults and regular
if (pmat->texture != def->material.texture) {
if (pmat->texture != def->Material().texture) {
WriteAttrTxt(elem, "texture", pmat->get_texture());
}
WriteAttrKey(elem, "texuniform", bool_map, 2, pmat->texuniform, def->material.texuniform);
WriteAttr(elem, "texrepeat", 2, pmat->texrepeat, def->material.texrepeat);
WriteAttr(elem, "emission", 1, &pmat->emission, &def->material.emission);
WriteAttr(elem, "specular", 1, &pmat->specular, &def->material.specular);
WriteAttr(elem, "shininess", 1, &pmat->shininess, &def->material.shininess);
WriteAttr(elem, "reflectance", 1, &pmat->reflectance, &def->material.reflectance);
WriteAttr(elem, "metallic", 1, &pmat->metallic, &def->material.metallic);
WriteAttr(elem, "roughness", 1, &pmat->roughness, &def->material.roughness);
WriteAttr(elem, "rgba", 4, pmat->rgba, def->material.rgba);
WriteAttrKey(elem, "texuniform", bool_map, 2, pmat->texuniform, def->Material().texuniform);
WriteAttr(elem, "texrepeat", 2, pmat->texrepeat, def->Material().texrepeat);
WriteAttr(elem, "emission", 1, &pmat->emission, &def->Material().emission);
WriteAttr(elem, "specular", 1, &pmat->specular, &def->Material().specular);
WriteAttr(elem, "shininess", 1, &pmat->shininess, &def->Material().shininess);
WriteAttr(elem, "reflectance", 1, &pmat->reflectance, &def->Material().reflectance);
WriteAttr(elem, "metallic", 1, &pmat->metallic, &def->Material().metallic);
WriteAttr(elem, "roughness", 1, &pmat->roughness, &def->Material().roughness);
WriteAttr(elem, "rgba", 4, pmat->rgba, def->Material().rgba);
}
// write joint
void mjXWriter::OneJoint(XMLElement* elem, mjCJoint* pjoint, mjCDef* def) {
void mjXWriter::OneJoint(XMLElement* elem, const mjCJoint* pjoint, mjCDef* def) {
double zero = 0;
// regular
@@ -292,42 +292,42 @@ void mjXWriter::OneJoint(XMLElement* elem, mjCJoint* pjoint, mjCDef* def) {
}
// defaults and regular
if (pjoint->type != def->joint.type) {
if (pjoint->type != def->Joint().type) {
WriteAttrTxt(elem, "type", FindValue(joint_map, joint_sz, pjoint->type));
}
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, "springref", 1, &pjoint->springref, &zero);
WriteAttr(elem, "solreflimit", mjNREF, pjoint->solref_limit, def->joint.solref_limit, true);
WriteAttr(elem, "solimplimit", mjNIMP, pjoint->solimp_limit, def->joint.solimp_limit, true);
WriteAttr(elem, "solreffriction", mjNREF, pjoint->solref_friction, def->joint.solref_friction,
WriteAttr(elem, "solreflimit", mjNREF, pjoint->solref_limit, def->Joint().solref_limit, true);
WriteAttr(elem, "solimplimit", mjNIMP, pjoint->solimp_limit, def->Joint().solimp_limit, true);
WriteAttr(elem, "solreffriction", mjNREF, pjoint->solref_friction, def->Joint().solref_friction,
true);
WriteAttr(elem, "solimpfriction", mjNIMP, pjoint->solimp_friction, def->joint.solimp_friction,
WriteAttr(elem, "solimpfriction", mjNIMP, pjoint->solimp_friction, def->Joint().solimp_friction,
true);
WriteAttr(elem, "stiffness", 1, &pjoint->stiffness, &def->joint.stiffness);
WriteAttrKey(elem, "limited", TFAuto_map, 3, pjoint->limited, def->joint.limited);
WriteAttr(elem, "range", 2, pjoint->range, def->joint.range);
WriteAttr(elem, "stiffness", 1, &pjoint->stiffness, &def->Joint().stiffness);
WriteAttrKey(elem, "limited", TFAuto_map, 3, pjoint->limited, def->Joint().limited);
WriteAttr(elem, "range", 2, pjoint->range, def->Joint().range);
WriteAttrKey(elem, "actuatorfrclimited", TFAuto_map, 3, pjoint->actfrclimited,
def->joint.actfrclimited);
WriteAttrKey(elem, "actuatorgravcomp", bool_map, 2, pjoint->actgravcomp, def->joint.actgravcomp);
WriteAttr(elem, "actuatorfrcrange", 2, pjoint->actfrcrange, def->joint.actfrcrange);
WriteAttr(elem, "margin", 1, &pjoint->margin, &def->joint.margin);
WriteAttr(elem, "armature", 1, &pjoint->armature, &def->joint.armature);
WriteAttr(elem, "damping", 1, &pjoint->damping, &def->joint.damping);
WriteAttr(elem, "frictionloss", 1, &pjoint->frictionloss, &def->joint.frictionloss);
def->Joint().actfrclimited);
WriteAttrKey(elem, "actuatorgravcomp", bool_map, 2, pjoint->actgravcomp, def->Joint().actgravcomp);
WriteAttr(elem, "actuatorfrcrange", 2, pjoint->actfrcrange, def->Joint().actfrcrange);
WriteAttr(elem, "margin", 1, &pjoint->margin, &def->Joint().margin);
WriteAttr(elem, "armature", 1, &pjoint->armature, &def->Joint().armature);
WriteAttr(elem, "damping", 1, &pjoint->damping, &def->Joint().damping);
WriteAttr(elem, "frictionloss", 1, &pjoint->frictionloss, &def->Joint().frictionloss);
// userdata
if (writingdefaults) {
WriteVector(elem, "user", pjoint->get_userdata());
} else {
WriteVector(elem, "user", pjoint->get_userdata(), def->joint.get_userdata());
WriteVector(elem, "user", pjoint->get_userdata(), def->Joint().get_userdata());
}
}
// write geom
void mjXWriter::OneGeom(XMLElement* elem, mjCGeom* pgeom, mjCDef* def) {
void mjXWriter::OneGeom(XMLElement* elem, const mjCGeom* pgeom, mjCDef* def) {
double unitq[4] = {1, 0, 0, 0};
double mass = 0;
@@ -336,10 +336,10 @@ void mjXWriter::OneGeom(XMLElement* elem, mjCGeom* pgeom, mjCDef* def) {
WriteAttrTxt(elem, "name", pgeom->name);
WriteAttrTxt(elem, "class", pgeom->classname);
if (mjGEOMINFO[pgeom->type]) {
WriteAttr(elem, "size", mjGEOMINFO[pgeom->type], pgeom->size, def->geom.size);
WriteAttr(elem, "size", mjGEOMINFO[pgeom->type], pgeom->size, def->Geom().size);
}
if (mjuu_defined(pgeom->mass)) {
mass = pgeom->GetVolume() * def->geom.density;
mass = pgeom->GetVolume() * def->Geom().density;
}
// mesh geom
@@ -368,35 +368,35 @@ void mjXWriter::OneGeom(XMLElement* elem, mjCGeom* pgeom, mjCDef* def) {
WriteAttr(elem, "quat", 4, pgeom->quat, unitq);
}
} else {
WriteAttr(elem, "size", 3, pgeom->size, def->geom.size);
WriteAttr(elem, "size", 3, pgeom->size, def->Geom().size);
}
// defaults and regular
WriteAttrKey(elem, "type", geom_map, mjNGEOMTYPES, pgeom->type, def->geom.type);
WriteAttrInt(elem, "contype", pgeom->contype, def->geom.contype);
WriteAttrInt(elem, "conaffinity", pgeom->conaffinity, def->geom.conaffinity);
WriteAttrInt(elem, "condim", pgeom->condim, def->geom.condim);
WriteAttrInt(elem, "group", pgeom->group, def->geom.group);
WriteAttrInt(elem, "priority", pgeom->priority, def->geom.priority);
WriteAttr(elem, "friction", 3, pgeom->friction, def->geom.friction, true);
WriteAttr(elem, "solmix", 1, &pgeom->solmix, &def->geom.solmix);
WriteAttr(elem, "solref", mjNREF, pgeom->solref, def->geom.solref, true);
WriteAttr(elem, "solimp", mjNIMP, pgeom->solimp, def->geom.solimp, true);
WriteAttr(elem, "margin", 1, &pgeom->margin, &def->geom.margin);
WriteAttr(elem, "gap", 1, &pgeom->gap, &def->geom.gap);
WriteAttr(elem, "gap", 1, &pgeom->gap, &def->geom.gap);
WriteAttrKey(elem, "fluidshape", fluid_map, 2, pgeom->fluid_ellipsoid, def->geom.fluid_ellipsoid);
WriteAttr(elem, "fluidcoef", 5, pgeom->fluid_coefs, def->geom.fluid_coefs);
WriteAttrKey(elem, "shellinertia", meshtype_map, 2, pgeom->typeinertia, def->geom.typeinertia);
WriteAttrKey(elem, "type", geom_map, mjNGEOMTYPES, pgeom->type, def->Geom().type);
WriteAttrInt(elem, "contype", pgeom->contype, def->Geom().contype);
WriteAttrInt(elem, "conaffinity", pgeom->conaffinity, def->Geom().conaffinity);
WriteAttrInt(elem, "condim", pgeom->condim, def->Geom().condim);
WriteAttrInt(elem, "group", pgeom->group, def->Geom().group);
WriteAttrInt(elem, "priority", pgeom->priority, def->Geom().priority);
WriteAttr(elem, "friction", 3, pgeom->friction, def->Geom().friction, true);
WriteAttr(elem, "solmix", 1, &pgeom->solmix, &def->Geom().solmix);
WriteAttr(elem, "solref", mjNREF, pgeom->solref, def->Geom().solref, true);
WriteAttr(elem, "solimp", mjNIMP, pgeom->solimp, def->Geom().solimp, true);
WriteAttr(elem, "margin", 1, &pgeom->margin, &def->Geom().margin);
WriteAttr(elem, "gap", 1, &pgeom->gap, &def->Geom().gap);
WriteAttr(elem, "gap", 1, &pgeom->gap, &def->Geom().gap);
WriteAttrKey(elem, "fluidshape", fluid_map, 2, pgeom->fluid_ellipsoid, def->Geom().fluid_ellipsoid);
WriteAttr(elem, "fluidcoef", 5, pgeom->fluid_coefs, def->Geom().fluid_coefs);
WriteAttrKey(elem, "shellinertia", meshtype_map, 2, pgeom->typeinertia, def->Geom().typeinertia);
if (mjuu_defined(pgeom->mass)) {
WriteAttr(elem, "mass", 1, &pgeom->mass_, &mass);
} else {
WriteAttr(elem, "density", 1, &pgeom->density, &def->geom.density);
WriteAttr(elem, "density", 1, &pgeom->density, &def->Geom().density);
}
if (pgeom->get_material() != def->geom.get_material()) {
if (pgeom->get_material() != def->Geom().get_material()) {
WriteAttrTxt(elem, "material", pgeom->get_material());
}
WriteAttr(elem, "rgba", 4, pgeom->rgba, def->geom.rgba);
WriteAttr(elem, "rgba", 4, pgeom->rgba, def->Geom().rgba);
// hfield and mesh attributes
if (pgeom->type==mjGEOM_HFIELD) {
@@ -410,7 +410,7 @@ void mjXWriter::OneGeom(XMLElement* elem, mjCGeom* pgeom, mjCDef* def) {
if (writingdefaults) {
WriteVector(elem, "user", pgeom->get_userdata());
} else {
WriteVector(elem, "user", pgeom->get_userdata(), def->geom.get_userdata());
WriteVector(elem, "user", pgeom->get_userdata(), def->Geom().get_userdata());
}
// write plugin
@@ -422,7 +422,7 @@ void mjXWriter::OneGeom(XMLElement* elem, mjCGeom* pgeom, mjCDef* def) {
// write site
void mjXWriter::OneSite(XMLElement* elem, mjCSite* psite, mjCDef* def) {
void mjXWriter::OneSite(XMLElement* elem, const mjCSite* psite, mjCDef* def) {
double unitq[4] = {1, 0, 0, 0};
// regular
@@ -432,32 +432,32 @@ void mjXWriter::OneSite(XMLElement* elem, mjCSite* psite, mjCDef* def) {
WriteAttr(elem, "pos", 3, psite->pos);
WriteAttr(elem, "quat", 4, psite->quat, unitq);
if (mjGEOMINFO[psite->type]) {
WriteAttr(elem, "size", mjGEOMINFO[psite->type], psite->size, def->site.size);
WriteAttr(elem, "size", mjGEOMINFO[psite->type], psite->size, def->Site().size);
}
} else {
WriteAttr(elem, "size", 3, psite->size, def->site.size);
WriteAttr(elem, "size", 3, psite->size, def->Site().size);
}
// defaults and regular
WriteAttrInt(elem, "group", psite->group, def->site.group);
WriteAttrKey(elem, "type", geom_map, mjNGEOMTYPES, psite->type, def->site.type);
if (psite->get_material() != def->site.get_material()) {
WriteAttrInt(elem, "group", psite->group, def->Site().group);
WriteAttrKey(elem, "type", geom_map, mjNGEOMTYPES, psite->type, def->Site().type);
if (psite->get_material() != def->Site().get_material()) {
WriteAttrTxt(elem, "material", psite->get_material());
}
WriteAttr(elem, "rgba", 4, psite->rgba, def->site.rgba);
WriteAttr(elem, "rgba", 4, psite->rgba, def->Site().rgba);
// userdata
if (writingdefaults) {
WriteVector(elem, "user", psite->get_userdata());
} else {
WriteVector(elem, "user", psite->get_userdata(), def->site.get_userdata());
WriteVector(elem, "user", psite->get_userdata(), def->Site().get_userdata());
}
}
// write camera
void mjXWriter::OneCamera(XMLElement* elem, mjCCamera* pcam, mjCDef* def) {
void mjXWriter::OneCamera(XMLElement* elem, const mjCCamera* pcam, mjCDef* def) {
double unitq[4] = {1, 0, 0, 0};
// regular
@@ -470,36 +470,36 @@ void mjXWriter::OneCamera(XMLElement* elem, mjCCamera* pcam, mjCDef* def) {
}
// defaults and regular
WriteAttr(elem, "ipd", 1, &pcam->ipd, &def->camera.ipd);
WriteAttrKey(elem, "mode", camlight_map, camlight_sz, pcam->mode, def->camera.mode);
WriteAttr(elem, "resolution", 2, pcam->resolution, def->camera.resolution);
WriteAttr(elem, "ipd", 1, &pcam->ipd, &def->Camera().ipd);
WriteAttrKey(elem, "mode", camlight_map, camlight_sz, pcam->mode, def->Camera().mode);
WriteAttr(elem, "resolution", 2, pcam->resolution, def->Camera().resolution);
// resolution if positive
WriteAttr(elem, "resolution", 2, pcam->resolution, def->camera.resolution);
WriteAttr(elem, "resolution", 2, pcam->resolution, def->Camera().resolution);
// camera intrinsics if specified
if (pcam->sensor_size[0]>0 && pcam->sensor_size[1]>0) {
WriteAttr(elem, "sensorsize", 2, pcam->sensor_size);
WriteAttr(elem, "focal", 2, pcam->focal_length, def->camera.focal_length);
WriteAttr(elem, "focalpixel", 2, pcam->focal_pixel, def->camera.focal_pixel);
WriteAttr(elem, "principal", 2, pcam->principal_length, def->camera.principal_length);
WriteAttr(elem, "principalpixel", 2, pcam->principal_pixel, def->camera.principal_pixel);
WriteAttr(elem, "focal", 2, pcam->focal_length, def->Camera().focal_length);
WriteAttr(elem, "focalpixel", 2, pcam->focal_pixel, def->Camera().focal_pixel);
WriteAttr(elem, "principal", 2, pcam->principal_length, def->Camera().principal_length);
WriteAttr(elem, "principalpixel", 2, pcam->principal_pixel, def->Camera().principal_pixel);
} else {
WriteAttr(elem, "fovy", 1, &pcam->fovy, &def->camera.fovy);
WriteAttr(elem, "fovy", 1, &pcam->fovy, &def->Camera().fovy);
}
// userdata
if (writingdefaults) {
WriteVector(elem, "user", pcam->get_userdata());
} else {
WriteVector(elem, "user", pcam->get_userdata(), def->camera.get_userdata());
WriteVector(elem, "user", pcam->get_userdata(), def->Camera().get_userdata());
}
}
// write light
void mjXWriter::OneLight(XMLElement* elem, mjCLight* plight, mjCDef* def) {
void mjXWriter::OneLight(XMLElement* elem, const mjCLight* plight, mjCDef* def) {
// regular
if (!writingdefaults) {
WriteAttrTxt(elem, "name", plight->name);
@@ -510,23 +510,23 @@ void mjXWriter::OneLight(XMLElement* elem, mjCLight* plight, mjCDef* def) {
}
// defaults and regular
WriteAttr(elem, "bulbradius", 1, &plight->bulbradius, &def->light.bulbradius);
WriteAttrKey(elem, "directional", bool_map, 2, plight->directional, def->light.directional);
WriteAttrKey(elem, "castshadow", bool_map, 2, plight->castshadow, def->light.castshadow);
WriteAttrKey(elem, "active", bool_map, 2, plight->active, def->light.active);
WriteAttr(elem, "attenuation", 3, plight->attenuation, def->light.attenuation);
WriteAttr(elem, "cutoff", 1, &plight->cutoff, &def->light.cutoff);
WriteAttr(elem, "exponent", 1, &plight->exponent, &def->light.exponent);
WriteAttr(elem, "ambient", 3, plight->ambient, def->light.ambient);
WriteAttr(elem, "diffuse", 3, plight->diffuse, def->light.diffuse);
WriteAttr(elem, "specular", 3, plight->specular, def->light.specular);
WriteAttrKey(elem, "mode", camlight_map, camlight_sz, plight->mode, def->light.mode);
WriteAttr(elem, "bulbradius", 1, &plight->bulbradius, &def->Light().bulbradius);
WriteAttrKey(elem, "directional", bool_map, 2, plight->directional, def->Light().directional);
WriteAttrKey(elem, "castshadow", bool_map, 2, plight->castshadow, def->Light().castshadow);
WriteAttrKey(elem, "active", bool_map, 2, plight->active, def->Light().active);
WriteAttr(elem, "attenuation", 3, plight->attenuation, def->Light().attenuation);
WriteAttr(elem, "cutoff", 1, &plight->cutoff, &def->Light().cutoff);
WriteAttr(elem, "exponent", 1, &plight->exponent, &def->Light().exponent);
WriteAttr(elem, "ambient", 3, plight->ambient, def->Light().ambient);
WriteAttr(elem, "diffuse", 3, plight->diffuse, def->Light().diffuse);
WriteAttr(elem, "specular", 3, plight->specular, def->Light().specular);
WriteAttrKey(elem, "mode", camlight_map, camlight_sz, plight->mode, def->Light().mode);
}
// write pair
void mjXWriter::OnePair(XMLElement* elem, mjCPair* ppair, mjCDef* def) {
void mjXWriter::OnePair(XMLElement* elem, const mjCPair* ppair, mjCDef* def) {
// regular
if (!writingdefaults) {
WriteAttrTxt(elem, "class", ppair->classname);
@@ -536,20 +536,20 @@ void mjXWriter::OnePair(XMLElement* elem, mjCPair* ppair, mjCDef* def) {
// defaults and regular
WriteAttrTxt(elem, "name", ppair->name);
WriteAttrInt(elem, "condim", ppair->condim, def->pair.spec.condim);
WriteAttr(elem, "margin", 1, &ppair->margin, &def->pair.spec.margin);
WriteAttr(elem, "gap", 1, &ppair->gap, &def->pair.spec.gap);
WriteAttr(elem, "solref", mjNREF, ppair->solref, def->pair.spec.solref, true);
WriteAttr(elem, "solreffriction", mjNREF, ppair->solreffriction, def->pair.spec.solreffriction,
WriteAttrInt(elem, "condim", ppair->condim, def->Pair().spec.condim);
WriteAttr(elem, "margin", 1, &ppair->margin, &def->Pair().spec.margin);
WriteAttr(elem, "gap", 1, &ppair->gap, &def->Pair().spec.gap);
WriteAttr(elem, "solref", mjNREF, ppair->solref, def->Pair().spec.solref, true);
WriteAttr(elem, "solreffriction", mjNREF, ppair->solreffriction, def->Pair().spec.solreffriction,
true);
WriteAttr(elem, "solimp", mjNIMP, ppair->solimp, def->pair.spec.solimp, true);
WriteAttr(elem, "friction", 5, ppair->friction, def->pair.spec.friction); // all 5 values
WriteAttr(elem, "solimp", mjNIMP, ppair->solimp, def->Pair().spec.solimp, true);
WriteAttr(elem, "friction", 5, ppair->friction, def->Pair().spec.friction); // all 5 values
}
// write equality
void mjXWriter::OneEquality(XMLElement* elem, mjCEquality* peq, mjCDef* def) {
void mjXWriter::OneEquality(XMLElement* elem, const mjCEquality* peq, mjCDef* def) {
// regular
if (!writingdefaults) {
WriteAttrTxt(elem, "name", peq->name);
@@ -592,15 +592,15 @@ void mjXWriter::OneEquality(XMLElement* elem, mjCEquality* peq, mjCDef* def) {
}
// defaults and regular
WriteAttrKey(elem, "active", bool_map, 2, peq->active, def->equality.active);
WriteAttr(elem, "solref", mjNREF, peq->solref, def->equality.solref, true);
WriteAttr(elem, "solimp", mjNIMP, peq->solimp, def->equality.solimp, true);
WriteAttrKey(elem, "active", bool_map, 2, peq->active, def->Equality().active);
WriteAttr(elem, "solref", mjNREF, peq->solref, def->Equality().solref, true);
WriteAttr(elem, "solimp", mjNIMP, peq->solimp, def->Equality().solimp, true);
}
// write tendon
void mjXWriter::OneTendon(XMLElement* elem, mjCTendon* pten, mjCDef* def) {
void mjXWriter::OneTendon(XMLElement* elem, const mjCTendon* pten, mjCDef* def) {
bool fixed = (pten->GetWrap(0) && pten->GetWrap(0)->type==mjWRAP_JOINT);
// regular
@@ -610,46 +610,46 @@ void mjXWriter::OneTendon(XMLElement* elem, mjCTendon* pten, mjCDef* def) {
}
// defaults and regular
WriteAttrInt(elem, "group", pten->group, def->tendon.group);
WriteAttr(elem, "solreflimit", mjNREF, pten->solref_limit, def->tendon.solref_limit, true);
WriteAttr(elem, "solimplimit", mjNIMP, pten->solimp_limit, def->tendon.solimp_limit, true);
WriteAttr(elem, "solreffriction", mjNREF, pten->solref_friction, def->tendon.solref_friction,
WriteAttrInt(elem, "group", pten->group, def->Tendon().group);
WriteAttr(elem, "solreflimit", mjNREF, pten->solref_limit, def->Tendon().solref_limit, true);
WriteAttr(elem, "solimplimit", mjNIMP, pten->solimp_limit, def->Tendon().solimp_limit, true);
WriteAttr(elem, "solreffriction", mjNREF, pten->solref_friction, def->Tendon().solref_friction,
true);
WriteAttr(elem, "solimpfriction", mjNIMP, pten->solimp_friction, def->tendon.solimp_friction,
WriteAttr(elem, "solimpfriction", mjNIMP, pten->solimp_friction, def->Tendon().solimp_friction,
true);
WriteAttrKey(elem, "limited", TFAuto_map, 3, pten->limited, def->tendon.limited);
WriteAttr(elem, "range", 2, pten->range, def->tendon.range);
WriteAttr(elem, "margin", 1, &pten->margin, &def->tendon.margin);
WriteAttr(elem, "stiffness", 1, &pten->stiffness, &def->tendon.stiffness);
WriteAttr(elem, "damping", 1, &pten->damping, &def->tendon.damping);
WriteAttr(elem, "frictionloss", 1, &pten->frictionloss, &def->tendon.frictionloss);
WriteAttrKey(elem, "limited", TFAuto_map, 3, pten->limited, def->Tendon().limited);
WriteAttr(elem, "range", 2, pten->range, def->Tendon().range);
WriteAttr(elem, "margin", 1, &pten->margin, &def->Tendon().margin);
WriteAttr(elem, "stiffness", 1, &pten->stiffness, &def->Tendon().stiffness);
WriteAttr(elem, "damping", 1, &pten->damping, &def->Tendon().damping);
WriteAttr(elem, "frictionloss", 1, &pten->frictionloss, &def->Tendon().frictionloss);
if (pten->springlength[0] != pten->springlength[1] ||
def->tendon.springlength[0] != def->tendon.springlength[1]) {
WriteAttr(elem, "springlength", 2, pten->springlength, def->tendon.springlength);
def->Tendon().springlength[0] != def->Tendon().springlength[1]) {
WriteAttr(elem, "springlength", 2, pten->springlength, def->Tendon().springlength);
} else {
WriteAttr(elem, "springlength", 1, pten->springlength, def->tendon.springlength);
WriteAttr(elem, "springlength", 1, pten->springlength, def->Tendon().springlength);
}
// spatial only
if (!fixed) {
if (pten->get_material()!=def->tendon.get_material()) {
if (pten->get_material()!=def->Tendon().get_material()) {
WriteAttrTxt(elem, "material", pten->get_material());
}
WriteAttr(elem, "width", 1, &pten->width, &def->tendon.width);
WriteAttr(elem, "rgba", 4, pten->rgba, def->tendon.rgba);
WriteAttr(elem, "width", 1, &pten->width, &def->Tendon().width);
WriteAttr(elem, "rgba", 4, pten->rgba, def->Tendon().rgba);
}
// userdata
if (writingdefaults) {
WriteVector(elem, "user", pten->get_userdata());
} else {
WriteVector(elem, "user", pten->get_userdata(), def->tendon.get_userdata());
WriteVector(elem, "user", pten->get_userdata(), def->Tendon().get_userdata());
}
}
// write actuator
void mjXWriter::OneActuator(XMLElement* elem, mjCActuator* pact, mjCDef* def) {
void mjXWriter::OneActuator(XMLElement* elem, const mjCActuator* pact, mjCDef* def) {
// regular
if (!writingdefaults) {
WriteAttrTxt(elem, "name", pact->name);
@@ -689,20 +689,20 @@ void mjXWriter::OneActuator(XMLElement* elem, mjCActuator* pact, mjCDef* def) {
}
// defaults and regular
WriteAttrInt(elem, "group", pact->group, def->actuator.group);
WriteAttrKey(elem, "ctrllimited", TFAuto_map, 3, pact->ctrllimited, def->actuator.ctrllimited);
WriteAttr(elem, "ctrlrange", 2, pact->ctrlrange, def->actuator.ctrlrange);
WriteAttrKey(elem, "forcelimited", TFAuto_map, 3, pact->forcelimited, def->actuator.forcelimited);
WriteAttr(elem, "forcerange", 2, pact->forcerange, def->actuator.forcerange);
WriteAttrKey(elem, "actlimited", TFAuto_map, 3, pact->actlimited, def->actuator.actlimited);
WriteAttr(elem, "actrange", 2, pact->actrange, def->actuator.actrange);
WriteAttr(elem, "lengthrange", 2, pact->lengthrange, def->actuator.lengthrange);
WriteAttr(elem, "gear", 6, pact->gear, def->actuator.gear);
WriteAttr(elem, "cranklength", 1, &pact->cranklength, &def->actuator.cranklength);
WriteAttrInt(elem, "group", pact->group, def->Actuator().group);
WriteAttrKey(elem, "ctrllimited", TFAuto_map, 3, pact->ctrllimited, def->Actuator().ctrllimited);
WriteAttr(elem, "ctrlrange", 2, pact->ctrlrange, def->Actuator().ctrlrange);
WriteAttrKey(elem, "forcelimited", TFAuto_map, 3, pact->forcelimited, def->Actuator().forcelimited);
WriteAttr(elem, "forcerange", 2, pact->forcerange, def->Actuator().forcerange);
WriteAttrKey(elem, "actlimited", TFAuto_map, 3, pact->actlimited, def->Actuator().actlimited);
WriteAttr(elem, "actrange", 2, pact->actrange, def->Actuator().actrange);
WriteAttr(elem, "lengthrange", 2, pact->lengthrange, def->Actuator().lengthrange);
WriteAttr(elem, "gear", 6, pact->gear, def->Actuator().gear);
WriteAttr(elem, "cranklength", 1, &pact->cranklength, &def->Actuator().cranklength);
WriteAttrKey(elem, "actearly", bool_map, 2, pact->actearly,
def->actuator.actearly);
WriteAttrKey(elem, "dyntype", dyn_map, dyn_sz, pact->dyntype, def->actuator.dyntype);
WriteAttr(elem, "dynprm", mjNDYN, pact->dynprm, def->actuator.dynprm);
def->Actuator().actearly);
WriteAttrKey(elem, "dyntype", dyn_map, dyn_sz, pact->dyntype, def->Actuator().dyntype);
WriteAttr(elem, "dynprm", mjNDYN, pact->dynprm, def->Actuator().dynprm);
// plugins: write config attributes
if (pact->plugin.active) {
@@ -713,29 +713,29 @@ void mjXWriter::OneActuator(XMLElement* elem, mjCActuator* pact, mjCDef* def) {
else {
// special handling of actdim which has default value of -1
if (writingdefaults) {
WriteAttrInt(elem, "actdim", pact->actdim, def->actuator.actdim);
WriteAttrInt(elem, "actdim", pact->actdim, def->Actuator().actdim);
} else {
int default_actdim = pact->dyntype == mjDYN_NONE ? 0 : 1;
WriteAttrInt(elem, "actdim", pact->actdim, default_actdim);
}
WriteAttrKey(elem, "gaintype", gain_map, gain_sz, pact->gaintype, def->actuator.gaintype);
WriteAttrKey(elem, "biastype", bias_map, bias_sz, pact->biastype, def->actuator.biastype);
WriteAttr(elem, "gainprm", mjNGAIN, pact->gainprm, def->actuator.gainprm, true);
WriteAttr(elem, "biasprm", mjNBIAS, pact->biasprm, def->actuator.biasprm, true);
WriteAttrKey(elem, "gaintype", gain_map, gain_sz, pact->gaintype, def->Actuator().gaintype);
WriteAttrKey(elem, "biastype", bias_map, bias_sz, pact->biastype, def->Actuator().biastype);
WriteAttr(elem, "gainprm", mjNGAIN, pact->gainprm, def->Actuator().gainprm, true);
WriteAttr(elem, "biasprm", mjNBIAS, pact->biasprm, def->Actuator().biasprm, true);
}
// userdata
if (writingdefaults) {
WriteVector(elem, "user", pact->get_userdata());
} else {
WriteVector(elem, "user", pact->get_userdata(), def->actuator.get_userdata());
WriteVector(elem, "user", pact->get_userdata(), def->Actuator().get_userdata());
}
}
// write plugin
void mjXWriter::OnePlugin(XMLElement* elem, mjsPlugin* plugin) {
void mjXWriter::OnePlugin(XMLElement* elem, const mjsPlugin* plugin) {
const std::string instance_name = std::string(mjs_getString(plugin->instance_name));
const std::string plugin_name = std::string(mjs_getString(plugin->name));
if (!instance_name.empty()) {
@@ -769,9 +769,9 @@ mjXWriter::mjXWriter(void) {
// cast model
void mjXWriter::SetModel(mjSpec* modelspec) {
if (modelspec) {
model = (mjCModel*)modelspec->element;
void mjXWriter::SetModel(mjSpec* spec) {
if (spec) {
model = (mjCModel*)spec->element;
}
}
@@ -806,7 +806,7 @@ string mjXWriter::Write(char *error, size_t error_sz) {
Visual(root);
Statistic(root);
writingdefaults = true;
Default(root, model->defaults[0]);
Default(root, model->Defaults(0));
writingdefaults = false;
Extension(root);
Custom(root);
@@ -1136,7 +1136,7 @@ void mjXWriter::Default(XMLElement* root, mjCDef* def) {
// pointer to parent defaults
mjCDef* par;
if (def->parentid>=0) {
par = model->defaults[def->parentid];
par = model->Defaults(def->parentid);
} else {
par = new mjCDef;
}
@@ -1147,57 +1147,57 @@ void mjXWriter::Default(XMLElement* root, mjCDef* def) {
// mesh
elem = InsertEnd(section, "mesh");
OneMesh(elem, &def->mesh, par);
OneMesh(elem, &def->Mesh(), par);
if (!elem->FirstAttribute()) section->DeleteChild(elem);
// material
elem = InsertEnd(section, "material");
OneMaterial(elem, &def->material, par);
OneMaterial(elem, &def->Material(), par);
if (!elem->FirstAttribute()) section->DeleteChild(elem);
// joint
elem = InsertEnd(section, "joint");
OneJoint(elem, &def->joint, par);
OneJoint(elem, &def->Joint(), par);
if (!elem->FirstAttribute()) section->DeleteChild(elem);
// geom
elem = InsertEnd(section, "geom");
OneGeom(elem, &def->geom, par);
OneGeom(elem, &def->Geom(), par);
if (!elem->FirstAttribute()) section->DeleteChild(elem);
// site
elem = InsertEnd(section, "site");
OneSite(elem, &def->site, par);
OneSite(elem, &def->Site(), par);
if (!elem->FirstAttribute()) section->DeleteChild(elem);
// camera
elem = InsertEnd(section, "camera");
OneCamera(elem, &def->camera, par);
OneCamera(elem, &def->Camera(), par);
if (!elem->FirstAttribute()) section->DeleteChild(elem);
// light
elem = InsertEnd(section, "light");
OneLight(elem, &def->light, par);
OneLight(elem, &def->Light(), par);
if (!elem->FirstAttribute()) section->DeleteChild(elem);
// pair
elem = InsertEnd(section, "pair");
OnePair(elem, &def->pair, par);
OnePair(elem, &def->Pair(), par);
if (!elem->FirstAttribute()) section->DeleteChild(elem);
// equality
elem = InsertEnd(section, "equality");
OneEquality(elem, &def->equality, par);
OneEquality(elem, &def->Equality(), par);
if (!elem->FirstAttribute()) section->DeleteChild(elem);
// tendon
elem = InsertEnd(section, "tendon");
OneTendon(elem, &def->tendon, par);
OneTendon(elem, &def->Tendon(), par);
if (!elem->FirstAttribute()) section->DeleteChild(elem);
// actuator
elem = InsertEnd(section, "general");
OneActuator(elem, &def->actuator, par);
OneActuator(elem, &def->Actuator(), par);
if (!elem->FirstAttribute()) section->DeleteChild(elem);
// if top-level class has no members or children, delete it and return
@@ -1209,7 +1209,7 @@ void mjXWriter::Default(XMLElement* root, mjCDef* def) {
// add children recursively
for (int i=0; i<(int)def->childid.size(); i++) {
Default(section, model->defaults[def->childid[i]]);
Default(section, model->Defaults(def->childid[i]));
}
// delete parent defaults if allocated here
@@ -1223,7 +1223,7 @@ void mjXWriter::Default(XMLElement* root, mjCDef* def) {
// extension section
void mjXWriter::Extension(XMLElement* root) {
// skip section if there is no required plugin
if (model->active_plugins.empty()) {
if (model->ActivePlugins().empty()) {
return;
}
@@ -1236,7 +1236,7 @@ void mjXWriter::Extension(XMLElement* root) {
// write all plugins
const mjpPlugin* last_plugin = nullptr;
XMLElement* plugin_elem = nullptr;
for (int i = 0; i < model->plugins.size(); ++i) {
for (int i = 0; i < model->Plugins().size(); ++i) {
mjCPlugin* pp = static_cast<mjCPlugin*>(model->GetObject(mjOBJ_PLUGIN, i));
if (pp->name.empty()) {
@@ -1273,7 +1273,7 @@ void mjXWriter::Extension(XMLElement* root) {
}
// write <plugin> elements for plugins without explicit instances
for (const auto& [plugin, slot] : model->active_plugins) {
for (const auto& [plugin, slot] : model->ActivePlugins()) {
if (seen_plugins.find(plugin) == seen_plugins.end()) {
plugin_elem = InsertEnd(section, "plugin");
WriteAttrTxt(plugin_elem, "plugin", plugin->name);
@@ -1677,7 +1677,7 @@ void mjXWriter::Tendon(XMLElement* root) {
// write wraps
XMLElement* wrap;
for (int j=0; j<pten->NumWraps(); j++) {
mjCWrap* pw = pten->GetWrap(j);
const mjCWrap* pw = pten->GetWrap(j);
switch (pw->type) {
case mjWRAP_JOINT:
wrap = InsertEnd(elem, "joint");
@@ -1755,7 +1755,7 @@ void mjXWriter::Sensor(XMLElement* root) {
// write all sensors
for (int i=0; i<num; i++) {
XMLElement* elem = 0;
mjCSensor* psen = model->sensors[i];
mjCSensor* psen = model->Sensors()[i];
std::string instance_name = "";
std::string plugin_name = "";
@@ -1875,46 +1875,82 @@ void mjXWriter::Sensor(XMLElement* root) {
elem = InsertEnd(section, "framepos");
WriteAttrTxt(elem, "objtype", mju_type2Str(psen->objtype));
WriteAttrTxt(elem, "objname", psen->get_objname());
if (psen->reftype != mjOBJ_UNKNOWN) {
WriteAttrTxt(elem, "reftype", mju_type2Str(psen->reftype));
WriteAttrTxt(elem, "refname", psen->get_refname());
}
break;
case mjSENS_FRAMEQUAT:
elem = InsertEnd(section, "framequat");
WriteAttrTxt(elem, "objtype", mju_type2Str(psen->objtype));
WriteAttrTxt(elem, "objname", psen->get_objname());
if (psen->reftype != mjOBJ_UNKNOWN) {
WriteAttrTxt(elem, "reftype", mju_type2Str(psen->reftype));
WriteAttrTxt(elem, "refname", psen->get_refname());
}
break;
case mjSENS_FRAMEXAXIS:
elem = InsertEnd(section, "framexaxis");
WriteAttrTxt(elem, "objtype", mju_type2Str(psen->objtype));
WriteAttrTxt(elem, "objname", psen->get_objname());
if (psen->reftype != mjOBJ_UNKNOWN) {
WriteAttrTxt(elem, "reftype", mju_type2Str(psen->reftype));
WriteAttrTxt(elem, "refname", psen->get_refname());
}
break;
case mjSENS_FRAMEYAXIS:
elem = InsertEnd(section, "frameyaxis");
WriteAttrTxt(elem, "objtype", mju_type2Str(psen->objtype));
WriteAttrTxt(elem, "objname", psen->get_objname());
if (psen->reftype != mjOBJ_UNKNOWN) {
WriteAttrTxt(elem, "reftype", mju_type2Str(psen->reftype));
WriteAttrTxt(elem, "refname", psen->get_refname());
}
break;
case mjSENS_FRAMEZAXIS:
elem = InsertEnd(section, "framezaxis");
WriteAttrTxt(elem, "objtype", mju_type2Str(psen->objtype));
WriteAttrTxt(elem, "objname", psen->get_objname());
if (psen->reftype != mjOBJ_UNKNOWN) {
WriteAttrTxt(elem, "reftype", mju_type2Str(psen->reftype));
WriteAttrTxt(elem, "refname", psen->get_refname());
}
break;
case mjSENS_FRAMELINVEL:
elem = InsertEnd(section, "framelinvel");
WriteAttrTxt(elem, "objtype", mju_type2Str(psen->objtype));
WriteAttrTxt(elem, "objname", psen->get_objname());
if (psen->reftype != mjOBJ_UNKNOWN) {
WriteAttrTxt(elem, "reftype", mju_type2Str(psen->reftype));
WriteAttrTxt(elem, "refname", psen->get_refname());
}
break;
case mjSENS_FRAMEANGVEL:
elem = InsertEnd(section, "frameangvel");
WriteAttrTxt(elem, "objtype", mju_type2Str(psen->objtype));
WriteAttrTxt(elem, "objname", psen->get_objname());
if (psen->reftype != mjOBJ_UNKNOWN) {
WriteAttrTxt(elem, "reftype", mju_type2Str(psen->reftype));
WriteAttrTxt(elem, "refname", psen->get_refname());
}
break;
case mjSENS_FRAMELINACC:
elem = InsertEnd(section, "framelinacc");
WriteAttrTxt(elem, "objtype", mju_type2Str(psen->objtype));
WriteAttrTxt(elem, "objname", psen->get_objname());
if (psen->reftype != mjOBJ_UNKNOWN) {
WriteAttrTxt(elem, "reftype", mju_type2Str(psen->reftype));
WriteAttrTxt(elem, "refname", psen->get_refname());
}
break;
case mjSENS_FRAMEANGACC:
elem = InsertEnd(section, "frameangacc");
WriteAttrTxt(elem, "objtype", mju_type2Str(psen->objtype));
WriteAttrTxt(elem, "objname", psen->get_objname());
if (psen->reftype != mjOBJ_UNKNOWN) {
WriteAttrTxt(elem, "reftype", mju_type2Str(psen->reftype));
WriteAttrTxt(elem, "refname", psen->get_refname());
}
break;
// sensors related to kinematic subtrees; attached to a body (which is the subtree root)
@@ -1930,6 +1966,21 @@ void mjXWriter::Sensor(XMLElement* root) {
elem = InsertEnd(section, "subtreeangmom");
WriteAttrTxt(elem, "body", psen->get_objname());
break;
case mjSENS_GEOMDIST:
elem = InsertEnd(section, "distance");
WriteAttrTxt(elem, psen->objtype == mjOBJ_BODY ? "body1" : "geom1", psen->get_objname());
WriteAttrTxt(elem, psen->reftype == mjOBJ_BODY ? "body2" : "geom2", psen->get_refname());
break;
case mjSENS_GEOMNORMAL:
elem = InsertEnd(section, "normal");
WriteAttrTxt(elem, psen->objtype == mjOBJ_BODY ? "body1" : "geom1", psen->get_objname());
WriteAttrTxt(elem, psen->reftype == mjOBJ_BODY ? "body2" : "geom2", psen->get_refname());
break;
case mjSENS_GEOMFROMTO:
elem = InsertEnd(section, "fromto");
WriteAttrTxt(elem, psen->objtype == mjOBJ_BODY ? "body1" : "geom1", psen->get_objname());
WriteAttrTxt(elem, psen->reftype == mjOBJ_BODY ? "body2" : "geom2", psen->get_refname());
break;
// global sensors
case mjSENS_CLOCK:
@@ -1968,12 +2019,6 @@ void mjXWriter::Sensor(XMLElement* root) {
WriteAttr(elem, "noise", 1, &psen->noise, &zero);
}
WriteVector(elem, "user", psen->get_userdata());
// add reference if present
if (psen->reftype != mjOBJ_UNKNOWN && psen->type != mjSENS_CAMPROJECTION) {
WriteAttrTxt(elem, "reftype", mju_type2Str(psen->reftype));
WriteAttrTxt(elem, "refname", psen->get_refname());
}
}
// remove section if empty
@@ -1994,7 +2039,7 @@ void mjXWriter::Keyframe(XMLElement* root) {
XMLElement* elem = InsertEnd(section, "key");
bool change = false;
mjCKey* pk = model->keys[i];
mjCKey* pk = model->Keys()[i];
// check name and write
if (!pk->name.empty()) {
@@ -2038,8 +2083,8 @@ void mjXWriter::Keyframe(XMLElement* root) {
// check mpos and write
if (model->nmocap) {
for (int j=0; j<model->nbody; j++) {
if (model->bodies[j]->mocap) {
mjCBody* pb = model->bodies[j];
if (model->Bodies()[j]->mocap) {
mjCBody* pb = model->Bodies()[j];
int id = pb->mocapid;
if (pb->pos[0] != pk->mpos_[3*id] ||
pb->pos[1] != pk->mpos_[3*id+1] ||
@@ -2055,8 +2100,8 @@ void mjXWriter::Keyframe(XMLElement* root) {
// check mquat and write
if (model->nmocap) {
for (int j=0; j<model->nbody; j++) {
if (model->bodies[j]->mocap) {
mjCBody* pb = model->bodies[j];
if (model->Bodies()[j]->mocap) {
mjCBody* pb = model->Bodies()[j];
int id = pb->mocapid;
if (pb->quat[0] != pk->mquat_[4*id] ||
pb->quat[1] != pk->mquat_[4*id+1] ||
src/xml/xml_native_writer.h
+15 -15
View File
@@ -27,7 +27,7 @@ class mjXWriter : public mjXBase {
public:
mjXWriter(); // constructor
virtual ~mjXWriter() = default; // destructor
void SetModel(mjSpec* modelspec);
void SetModel(mjSpec* spec);
// write XML document to string
std::string Write(char *error, std::size_t error_sz);
@@ -59,20 +59,20 @@ class mjXWriter : public mjXBase {
void Keyframe(tinyxml2::XMLElement* root); // keyframe section
// single element writers, used in defaults and main body
void OneFlex(tinyxml2::XMLElement* elem, mjCFlex* pflex);
void OneMesh(tinyxml2::XMLElement* elem, mjCMesh* pmesh, mjCDef* def);
void OneSkin(tinyxml2::XMLElement* elem, mjCSkin* pskin);
void OneMaterial(tinyxml2::XMLElement* elem, mjCMaterial* pmaterial, mjCDef* def);
void OneJoint(tinyxml2::XMLElement* elem, mjCJoint* pjoint, mjCDef* def);
void OneGeom(tinyxml2::XMLElement* elem, mjCGeom* pgeom, mjCDef* def);
void OneSite(tinyxml2::XMLElement* elem, mjCSite* psite, mjCDef* def);
void OneCamera(tinyxml2::XMLElement* elem, mjCCamera* pcamera, mjCDef* def);
void OneLight(tinyxml2::XMLElement* elem, mjCLight* plight, mjCDef* def);
void OnePair(tinyxml2::XMLElement* elem, mjCPair* ppair, mjCDef* def);
void OneEquality(tinyxml2::XMLElement* elem, mjCEquality* pequality, mjCDef* def);
void OneTendon(tinyxml2::XMLElement* elem, mjCTendon* ptendon, mjCDef* def);
void OneActuator(tinyxml2::XMLElement* elem, mjCActuator* pactuator, mjCDef* def);
void OnePlugin(tinyxml2::XMLElement* elem, mjsPlugin* plugin);
void OneFlex(tinyxml2::XMLElement* elem, const mjCFlex* pflex);
void OneMesh(tinyxml2::XMLElement* elem, const mjCMesh* pmesh, mjCDef* def);
void OneSkin(tinyxml2::XMLElement* elem, const mjCSkin* pskin);
void OneMaterial(tinyxml2::XMLElement* elem, const mjCMaterial* pmaterial, mjCDef* def);
void OneJoint(tinyxml2::XMLElement* elem, const mjCJoint* pjoint, mjCDef* def);
void OneGeom(tinyxml2::XMLElement* elem, const mjCGeom* pgeom, mjCDef* def);
void OneSite(tinyxml2::XMLElement* elem, const mjCSite* psite, mjCDef* def);
void OneCamera(tinyxml2::XMLElement* elem, const mjCCamera* pcamera, mjCDef* def);
void OneLight(tinyxml2::XMLElement* elem, const mjCLight* plight, mjCDef* def);
void OnePair(tinyxml2::XMLElement* elem, const mjCPair* ppair, mjCDef* def);
void OneEquality(tinyxml2::XMLElement* elem, const mjCEquality* pequality, mjCDef* def);
void OneTendon(tinyxml2::XMLElement* elem, const mjCTendon* ptendon, mjCDef* def);
void OneActuator(tinyxml2::XMLElement* elem, const mjCActuator* pactuator, mjCDef* def);
void OnePlugin(tinyxml2::XMLElement* elem, const mjsPlugin* plugin);
tinyxml2::XMLElement* OneFrame(tinyxml2::XMLElement* elem, mjCFrame* frame);
bool writingdefaults; // true during defaults write
src/xml/xml_urdf.cc
+16 -9
View File
@@ -13,6 +13,7 @@
// limitations under the License.
#include <array>
#include <cstdlib>
#include <cstring>
#include <iostream>
#include <string>
@@ -488,17 +489,23 @@ void mjXURDF::Joint(XMLElement* joint_elem) {
// limit element
if ((elem = FindSubElem(joint_elem, "limit"))) {
ReadAttr(elem, "lower", 1, pjoint->range, text);
ReadAttr(elem, "upper", 1, pjoint->range+1, text);
bool is_limited = mjuu_defined(pjoint->range[0]) &&
mjuu_defined(pjoint->range[1]) &&
pjoint->range[0] < pjoint->range[1];
pjoint->limited = is_limited ? mjLIMITED_TRUE : mjLIMITED_FALSE;
bool haslower = ReadAttr(elem, "lower", 1, pjoint->range, text);
bool hasupper = ReadAttr(elem, "upper", 1, pjoint->range+1, text);
// handle range mis-specification, otherwise the default mjLIMITED_AUTO will do the right thing
bool bad_range = (haslower != hasupper) || pjoint->range[0] > pjoint->range[1];
if (bad_range) {
pjoint->limited = mjLIMITED_FALSE;
}
// ReadAttr(elem, "velocity", 1, &pjoint->maxvel, text); // no maxvel in MuJoCo
ReadAttr(elem, "effort", 1, &pjoint->urdfeffort, text);
} else {
pjoint->limited = mjLIMITED_FALSE;
double effort = 0;
ReadAttr(elem, "effort", 1, &effort, text);
effort = std::abs(effort);
if (effort > 0) {
pjoint->actfrcrange[0] = -effort;
pjoint->actfrcrange[1] = effort;
}
}
}
src/xml/xml_util.cc
+10
View File
@@ -481,6 +481,16 @@ bool mjXUtil::ReadAttrValues(XMLElement* elem, const char* attr,
// read numbers
for (int i = 0; (max < 0 || i < max) && !strm.eof(); ++i) {
strm >> token;
// some C++ libraries do not allow .x instead of 0.x
if (!token.empty()) {
if (token[0] == std::string(".")[0]) {
token = "0" + token;
} else if (token[0] == std::string("-")[0] && token[1] == std::string(".")[0]) {
token.insert(1, "0");
}
}
std::istringstream token_strm(token);
token_strm >> item;
if (token_strm.fail() || !token_strm.eof()) {
test/engine/CMakeLists.txt
-3
View File
@@ -96,9 +96,6 @@ target_link_libraries(engine_util_solve_test fixture gmock)
mujoco_test(engine_util_spatial_test)
target_link_libraries(engine_util_spatial_test fixture gmock)
mujoco_test(engine_vfs_test)
target_link_libraries(engine_vfs_test fixture gmock)
mujoco_test(
engine_vis_state_test
PROPERTIES

Some files were not shown because too many files have changed in this diff Show More