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

merging with main mujoco repo

Browse Source
This commit is contained in:
Abhishek Joshi
2024-04-02 16:39:04 -04:00
parent 67a9f91c00 d7f131e086
commit a4b5355f46
136 changed files with 48702 additions and 2845 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/XMLreference.rst
+49 -37
View File
@@ -2129,12 +2129,6 @@ from its default.
When this flag is enabled, :ref:`mj_inverse` will interpret ``qacc`` as having been computed from the difference of
two sequential velocities, and undo the above modification.
.. _option-flag-sensornoise:
:at:`sensornoise`: :at-val:`[disable, enable], "disable"`
This flag enables the simulation of sensor noise. When disabled (which is the default) noise is not added to
sensordata, even if the sensors specify non-zero noise amplitudes. When enabled, zero-mean Gaussian noise is added to
the underlying deterministic sensor data. Its standard deviation is determined by the noise parameter of each sensor.
.. _option-flag-multiccd:
@@ -2354,24 +2348,6 @@ rotations as unit quaternions.
joint inertia in the model reference configuration. Note that the format is the same as the solref parameter of the
constraint solver.
.. _body-joint-limited:
:at:`limited`: :at-val:`[false, true, auto], "auto"`
This attribute specifies if the joint has limits. It interacts with the range attribute below. If this attribute
is "false", joint limits are disabled. If this attribute is "true", joint limits are enabled. If this
attribute is "auto", and :at:`autolimits` is set in :ref:`compiler <compiler>`, joint limits will be enabled
if range is defined.
.. _body-joint-actuatorfrclimited:
:at:`actuatorfrclimited`: :at-val:`[false, true, auto], "auto"`
This attribute specifies whether actuator forces acting on the joint should be clamped. See :ref:`CForceRange` for
details. It is available only for scalar joints (hinge and slider) and ignored for ball and free joints. |br| This
attribute interacts with the actuatorfrcrange attribute below. If this attribute is "false", actuator force
clamping is disabled. If it is "true", actuator force clamping is enabled. If this attribute is "auto", and
:at:`autolimits` is set in :ref:`compiler <compiler>`, actuator force clamping will be enabled if actuatorfrcrange
is defined.
.. _body-joint-solreflimit:
.. _body-joint-solimplimit:
@@ -2400,16 +2376,44 @@ rotations as unit quaternions.
joints, the limit is imposed on the angle of rotation (relative to the reference configuration) regardless of the
axis of rotation. Only the second range parameter is used for ball joints; the first range parameter should be set to
0. See the :ref:`Limit <coLimit>` section in the Computation chapter for more information.
|br| Setting this attribute without specifying :at:`limited` is an error, unless :at:`autolimits` is set in
|br| Setting this attribute without specifying :at:`limited` is an error if :at:`autolimits` is "false" in
:ref:`compiler <compiler>`.
.. _body-joint-limited:
:at:`limited`: :at-val:`[false, true, auto], "auto"`
This attribute specifies if the joint has limits. It interacts with the range attribute below. If this attribute
is "false", joint limits are disabled. If this attribute is "true", joint limits are enabled. If this
attribute is "auto", and :at:`autolimits` is set in :ref:`compiler <compiler>`, joint limits will be enabled
if range is defined.
.. _body-joint-actuatorfrcrange:
:at:`actuatorfrcrange`: :at-val:`real(2), "0 0"`
Range for clamping total actuator forces acting on this joint. See :ref:`CForceRange` for details. It is available
only for scalar joints (hinge and slider) and ignored for ball and free joints. |br| The compiler expects the first
value to be smaller than the second value. |br| Setting this attribute without specifying :at:`actuatorfrclimited`
is an error, unless :at:`compiler-autolimits` is set.
is an error if :at:`compiler-autolimits` is "false".
.. _body-joint-actuatorfrclimited:
:at:`actuatorfrclimited`: :at-val:`[false, true, auto], "auto"`
This attribute specifies whether actuator forces acting on the joint should be clamped. See :ref:`CForceRange` for
details. It is available only for scalar joints (hinge and slider) and ignored for ball and free joints. |br| This
attribute interacts with the actuatorfrcrange attribute below. If this attribute is "false", actuator force
clamping is disabled. If it is "true", actuator force clamping is enabled. If this attribute is "auto", and
:at:`autolimits` is set in :ref:`compiler <compiler>`, actuator force clamping will be enabled if
:at:`actuatorfrcrange` is defined.
.. _body-joint-actuatorgravcomp:
:at:`actuatorgravcomp`: :at-val:`[false, true], "false"`
If this flag is enabled, gravity compensation applied to this joint is added to actuator forces
(``mjData.qfrc_actuator``) rather than passive forces (``mjData.qfrc_passive``). Notionally, this means that gravity
compensation is the result of a control system rather than natural buoyancy. In practice, enabling this flag is
useful when joint-level actuator force clamping is used. In this case, the total actuation force applied on a joint,
including gravity compensation, is guaranteed to not exceeed the specified limits. See :ref:`CForceRange` and
:ref:`actuatorfrcrange<body-joint-actuatorfrcrange>` for more details on this type of force limit.
.. _body-joint-margin:
@@ -3808,8 +3812,12 @@ saving the XML:
in MuJoCo can be used as a rigid geom attached to a single body. In contrast, the flex generated here corresponds to
a soft mesh with the same initial shape, where each vertex is a separate moving body (unless pinned).
**gmsh** is similar to mesh, but it loads a `GMSH file <https://gmsh.info//doc/texinfo/gmsh.html#MSH-file-format>`__
in format 4.1 (ascii or binary). The file extension can be anything; the parser recognizes the format by examining
.. _gmsh-file-docs:
**gmsh** is similar to mesh, but it loads a GMSH file in
`format 4.1 <https://gmsh.info//doc/texinfo/gmsh.html#MSH-file-format>`__
and `format 2.2 <https://gmsh.info//doc/texinfo/gmsh.html#MSH-file-format-version-2-_0028Legacy_0029>`__
(ascii or binary). The file extension can be anything; the parser recognizes the format by examining
the file header. This is a very rich file format, allowing all kinds of elements with different dimensionality and
topology. MuJoCo only supports GMSH element types 1, 2, 4 which happen to correspond to our 1D, 2D and 3D flexes and
assumes that the nodes are specified in a single block. Only the Nodes and Elements sections of the GMHS file are
@@ -3817,7 +3825,7 @@ saving the XML:
GMSH file contains meshes that are not supported by MuJoCo. :at:`dim` is automatically set to the dimensionality
specified in the GMSH file. Presently this is the only mechanism to load a large tetrahedral mesh in MuJoCo and
generate a corresponding soft entity. If such a mesh is available in a different file format, use the freely
available `GMSH software <https://gmsh.info/>`__ to convert it to GMSH 4.1.
available `GMSH software <https://gmsh.info/>`__ to convert it to GMSH in one of the supported versions.
**direct** allows the user to specify the point and element data of the flexcomp directly in the XML. Note that
flexcomp will still generate moving bodies automatically, as well as automate other settings; so it still provides
@@ -3873,7 +3881,7 @@ saving the XML:
:at:`file`: :at-val:`string, optional`
The name of the file from which a **mesh** or a **gmsh** is loaded. For mesh, the file extentsion is used to
determine the file format. Supported formats are the same as in :ref:`mesh assets<asset-mesh>`. For gmsh, the file is
expected to be in GMSH format 4.1, ascii or binary.
expected to be in GMSH format 4.1 or 2.2, ascii or binary, see :ref:`here<gmsh-file-docs>`.
.. _body-flexcomp-rigid:
@@ -5092,14 +5100,14 @@ specify them independently.
:at:`ctrlrange`: :at-val:`real(2), "0 0"`
Range for clamping the control input. The first value must be smaller than the second value.
|br| Setting this attribute without specifying :at:`ctrllimited` is an error, unless :at:`autolimits` is set in
|br| Setting this attribute without specifying :at:`ctrllimited` is an error if :at:`autolimits` is "false" in
:ref:`compiler <compiler>`.
.. _actuator-general-forcerange:
:at:`forcerange`: :at-val:`real(2), "0 0"`
Range for clamping the force output. The first value must be no greater than the second value.
|br| Setting this attribute without specifying :at:`forcelimited` is an error, unless :at:`autolimits` is set in
|br| Setting this attribute without specifying :at:`forcelimited` is an error if :at:`autolimits` is "false" in
:ref:`compiler <compiler>`.
.. _actuator-general-actrange:
@@ -5107,7 +5115,7 @@ specify them independently.
:at:`actrange`: :at-val:`real(2), "0 0"`
Range for clamping the activation state. The first value must be no greater than the second value.
See the :ref:`Activation clamping <CActRange>` section for more details.
|br| Setting this attribute without specifying :at:`actlimited` is an error, unless :at:`autolimits` is set in
|br| Setting this attribute without specifying :at:`actlimited` is an error if :at:`autolimits` is "false" in
:ref:`compiler <compiler>`.
.. _actuator-general-lengthrange:
@@ -6485,10 +6493,12 @@ arms determined by the transmission). This sensor can be attached to any actuato
:el-prefix:`sensor/` |-| **jointactuatorfrc** (*)
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
This element creates an actuator force sensor, measured at a joint. The quantity being sensed is the
generalized force contributed by all actuators to a single scalar joint (hinge or slider). This type of sensor is
important when multiple actuators act on a single joint or when a single actuator act on multiple joints. See
:ref:`CForceRange` for details.
This element creates an actuator force sensor, measured at a joint. The quantity being sensed is the generalized force
contributed by all actuators to a single scalar joint (hinge or slider). If the joint's
:ref:`actuatorgravcomp<body-joint-actuatorgravcomp>` attribute is "true", this sensor will also measure contributions by
gravity compensation forces (which are added directly to the joint and would *not* register in the
:ref:`actuatorfrc<sensor-actuatorfrc>`) sensor. This type of sensor is important when multiple actuators act on a single
joint or when a single actuator act on multiple joints. See :ref:`CForceRange` for details.
.. _sensor-jointactuatorfrc-name:
@@ -7382,6 +7392,8 @@ if omitted.
.. _default-joint-actuatorfrclimited:
.. _default-joint-actuatorgravcomp:
.. _default-joint-solreflimit:
.. _default-joint-solimplimit:
doc/XMLschema.rst
+7 -7
View File
@@ -247,9 +247,9 @@
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`sensor<option-flag-sensor>` | :ref:`midphase<option-flag-midphase>` | :ref:`eulerdamp<option-flag-eulerdamp>` | :ref:`override<option-flag-override>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`energy<option-flag-energy>` | :ref:`fwdinv<option-flag-fwdinv>` | :ref:`invdiscrete<option-flag-invdiscrete>` | :ref:`sensornoise<option-flag-sensornoise>` | |
| | | | :ref:`energy<option-flag-energy>` | :ref:`fwdinv<option-flag-fwdinv>` | :ref:`invdiscrete<option-flag-invdiscrete>` | :ref:`multiccd<option-flag-multiccd>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`multiccd<option-flag-multiccd>` | :ref:`island<option-flag-island>` | | | |
| | | | :ref:`island<option-flag-island>` | | | | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
| mujoco |br| |L| | | .. table:: |
@@ -286,9 +286,9 @@
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`solimpfriction<body-joint-solimpfriction>` | :ref:`stiffness<body-joint-stiffness>` | :ref:`range<body-joint-range>` | :ref:`actuatorfrcrange<body-joint-actuatorfrcrange>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`margin<body-joint-margin>` | :ref:`ref<body-joint-ref>` | :ref:`springref<body-joint-springref>` | :ref:`armature<body-joint-armature>` | |
| | | | :ref:`actuatorgravcomp<body-joint-actuatorgravcomp>` | :ref:`margin<body-joint-margin>` | :ref:`ref<body-joint-ref>` | :ref:`springref<body-joint-springref>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`damping<body-joint-damping>` | :ref:`frictionloss<body-joint-frictionloss>` | :ref:`user<body-joint-user>` | | |
| | | | :ref:`armature<body-joint-armature>` | :ref:`damping<body-joint-damping>` | :ref:`frictionloss<body-joint-frictionloss>` | :ref:`user<body-joint-user>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
| |_| body |br| |_| |L| | | .. table:: |
@@ -1320,11 +1320,11 @@
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`solimplimit<default-joint-solimplimit>` | :ref:`solreffriction<default-joint-solreffriction>` | :ref:`solimpfriction<default-joint-solimpfriction>` | :ref:`stiffness<default-joint-stiffness>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`range<default-joint-range>` | :ref:`actuatorfrcrange<default-joint-actuatorfrcrange>` | :ref:`margin<default-joint-margin>` | :ref:`ref<default-joint-ref>` | |
| | | | :ref:`range<default-joint-range>` | :ref:`actuatorfrcrange<default-joint-actuatorfrcrange>` | :ref:`actuatorgravcomp<default-joint-actuatorgravcomp>` | :ref:`margin<default-joint-margin>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`springref<default-joint-springref>` | :ref:`armature<default-joint-armature>` | :ref:`damping<default-joint-damping>` | :ref:`frictionloss<default-joint-frictionloss>` | |
| | | | :ref:`ref<default-joint-ref>` | :ref:`springref<default-joint-springref>` | :ref:`armature<default-joint-armature>` | :ref:`damping<default-joint-damping>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`user<default-joint-user>` | | | | |
| | | | :ref:`frictionloss<default-joint-frictionloss>` | :ref:`user<default-joint-user>` | | | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
| |_| default |br| |_| |L| | | .. table:: |
doc/changelog.rst
+52 -12
View File
@@ -5,14 +5,62 @@ Changelog
Upcoming version (not yet released)
-----------------------------------
General
^^^^^^^
.. admonition:: Breaking API changes
:class: attention
1. Removed the ability to natively add noise to sensors. Note that the ``mjModel.sensor_noise`` field and
:ref:`corresponding attribute<CSensor>` are kept and now function as a convenient location for the user to save
standard-deviation information for their own use. This feature was removed because:
- There was no mechanism to seed the randon noise generator.
- It was not thread-safe, even if seeding would have been provided, sampling on multiple threads would lead to
non-reproducible results.
- This feature was seen as overreach by the engine. Adding noise should be the user's responsibility.
- We are not aware of anyone who was actually using the feature.
**Migration:** Add noise to sensor values yourself.
2. Added the :ref:`actuatorgravcomp<body-joint-actuatorgravcomp>` joint attribute. When enabled, gravity compensation
forces on the joint are treated as applied by actuators. See attribute documentation for more details. The example
model
`refsite.xml <https://github.com/google-deepmind/mujoco/blob/main/test/engine/testdata/actuation/refsite.xml>`__,
which demostrates Cartesian actuation of an arm, has been updated to use this attribute.
3. Added support for gmsh format 2.2, as generated by e.g. `fTetwild <https://github.com/wildmeshing/fTetWild>`__.
MJX
^^^
4. Improved performance of SAT for convex collisions.
5. Fixed bug for sphere/capsule-convex deep penetration.
6. Fixed bug where ``mjx.Data`` produced by ``mjx.put_data`` had different treedef than ``mjx.make_data``.
7. Throw an error for margin/gap for convex mesh collisions, since they are not supported.
Simulate
^^^^^^^^
8. Fixed bug in order of enable flag strings. Before this change, using the simulate UI to toggle the
:ref:`invdiscrete<option-flag-invdiscrete>` or the (now removed) ``sensornoise`` flags would actually toggle the
other flag.
Python bindings
^^^^^^^^^^^^^^^
.. youtube:: xHDS0n5DpqM
:align: right
:width: 240px
9. Added the ``mujoco.minimize`` Python module for nonlinear least-squares, designed for System Identification (sysID).
The sysID tutorial is work in progress, but a pedagogical colab notebook with examples, including Inverse
Kinematics, is available here: |ls_colab|
|br| The video on the right shows example clips from the tutorial.
.. |ls_colab| 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
1. Improved performance of SAT for convex collisions.
2. Fixed bug for sphere/capsule-convex deep penetration.
Version 3.1.3 (March 5th, 2024)
-----------------------------------
-------------------------------
General
^^^^^^^
@@ -31,7 +79,6 @@ General
MJX
^^^
4. Improved performance of getting and putting device data.
- Use ``tobytes()`` for numpy array serialization, which is orders of magnitude faster than converting to tuples.
@@ -50,16 +97,9 @@ Python bindings
^^^^^^^^^^^^^^^
11. Fixed incorrect data types in the bindings for the ``geom``, ``vert``, ``elem``, and ``flex`` array members
of the ``mjContact`` struct, and all array members of the ``mjrContext`` struct.
12. Added the ``mujoco.minimize`` Python module for nonlinear least-squares, designed for System Identification (sysID).
The sysID tutorial is work in progress, but a pedagogical colab notebook with examples, including Inverse
Kinematics, is available here: |ls_colab|
.. |ls_colab| 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
Version 3.1.2 (February 05, 2024)
-----------------------------------
---------------------------------
General
^^^^^^^
doc/computation/fluid.rst
+2 -4
View File
@@ -85,8 +85,7 @@ Ellipsoid model
:figwidth: 50%
:align: right
The flight-capable Drosophila Melanogaster model in this figure will be described in a
forthcoming publication.
The flight-capable Drosophila Melanogaster model in this figure is described in :cite:t:`Vaxenburg2024`.
In this section we describe and derive a stateless model of the forces exerted onto a moving rigid body by the
@@ -94,7 +93,6 @@ surrounding fluid, based on an ellipsoidal approximation of geom shape. This mod
different types of fluid forces than the inertia-based model of the previous section. The motivating use-case for this
model is insect flight, see figure on the right.
Summary
~~~~~~~
@@ -426,7 +424,7 @@ increasing Reynolds numbers, and a single reference area :math:`A_D` may not be
drag for highly irregular or slender bodies. For example, experimental fits are derived from problems ranging from
falling playing cards :cite:p:`wang2004,andersen2005a,andersen2005b` to particle transport :cite:p:`loth2008,
bagheri2016`. See screen capture of the
`cards.xml <https://github.com/deepmind/mujoco/blob/main/model/card/cards.xml>`__ model on the right.
`cards.xml <https://github.com/deepmind/mujoco/blob/main/model/cards/cards.xml>`__ model on the right.
We derive a formula for :math:`\mathbf{f}_\text{D}` based on two surfaces :math:`A^\text{proj}_\mathbf{v}` and
:math:`A_\text{max}`. The first, :math:`A^\text{proj}_\mathbf{v}`, is the cylindrical projection of the body onto a
doc/includes/references.h
+7 -7
View File
@@ -426,12 +426,11 @@ typedef enum mjtEnableBit_ { // enable optional feature bitflags
mjENBL_ENERGY = 1<<1, // energy computation
mjENBL_FWDINV = 1<<2, // record solver statistics
mjENBL_INVDISCRETE = 1<<3, // discrete-time inverse dynamics
mjENBL_SENSORNOISE = 1<<4, // add noise to sensor data
// experimental features:
mjENBL_MULTICCD = 1<<5, // multi-point convex collision detection
mjENBL_ISLAND = 1<<6, // constraint island discovery
mjENBL_MULTICCD = 1<<4, // multi-point convex collision detection
mjENBL_ISLAND = 1<<5, // constraint island discovery
mjNENABLE = 7 // number of enable flags
mjNENABLE = 6 // number of enable flags
} mjtEnableBit;
typedef enum mjtJoint_ { // type of degree of freedom
mjJNT_FREE = 0, // global position and orientation (quat) (7)
@@ -926,9 +925,9 @@ struct mjModel_ {
int nemax; // number of potential equality-constraint rows
int njmax; // number of available rows in constraint Jacobian
int nconmax; // number of potential contacts in contact list
int nuserdata; // number of extra fields in mjData
int nsensordata; // number of fields in sensor data vector
int npluginstate; // number of fields in plugin state vector
int nuserdata; // number of mjtNums reserved for the user
int nsensordata; // number of mjtNums in sensor data vector
int npluginstate; // number of mjtNums in plugin state vector
size_t narena; // number of bytes in the mjData arena (inclusive of stack)
size_t nbuffer; // number of bytes in buffer
@@ -993,6 +992,7 @@ struct mjModel_ {
int* jnt_group; // group for visibility (njnt x 1)
mjtByte* jnt_limited; // does joint have limits (njnt x 1)
mjtByte* jnt_actfrclimited; // does joint have actuator force limits (njnt x 1)
mjtByte* jnt_actgravcomp; // is gravcomp force applied via actuators (njnt x 1)
mjtNum* jnt_solref; // constraint solver reference: limit (njnt x mjNREF)
mjtNum* jnt_solimp; // constraint solver impedance: limit (njnt x mjNIMP)
mjtNum* jnt_pos; // local anchor position (njnt x 3)
doc/mjx.rst
+13 -12
View File
@@ -215,6 +215,14 @@ The following features are **in development** and coming soon:
* - Category
- Feature
* - :ref:`Geom <mjtGeom>`
- ``SDF``, ``HFIELD``, ``ELLIPSOID``, ``CYLINDER``
* - :ref:`Condim <coContact>`
- 1, 4, 6
* - :ref:`Constraint <mjtConstraint>`
- :ref:`Frictionloss <coFriction>`, ``CONTACT_FRICTIONLESS``, ``CONTACT_ELLIPTIC``, ``FRICTION_DOF``
* - :ref:`Integrator <mjtIntegrator>`
- ``IMPLICIT``, ``IMPLICITFAST``
* - Dynamics
- :ref:`Inverse <mj_inverse>`
* - :ref:`Transmission <mjtTrn>`
@@ -227,16 +235,8 @@ The following features are **in development** and coming soon:
- ``MUSCLE``
* - :ref:`Tendon Wrapping <mjtWrap>`
- ``NONE``, ``JOINT``, ``PULLEY``, ``SITE``, ``SPHERE``, ``CYLINDER``
* - :ref:`Geom <mjtGeom>`
- ``HFIELD``, ``ELLIPSOID``, ``CYLINDER``
* - :ref:`Constraint <mjtConstraint>`
- :ref:`Frictionloss <coFriction>`, ``CONTACT_FRICTIONLESS``, ``CONTACT_ELLIPTIC``, ``FRICTION_DOF``
* - :ref:`Integrator <mjtIntegrator>`
- ``IMPLICIT``, ``IMPLICITFAST``
* - :ref:`Cone <mjtCone>`
- ``ELLIPTIC``
* - :ref:`Condim <coContact>`
- 1, 4, 6
* - Fluid Model
- :ref:`flEllipsoid`
* - :ref:`Tendons <tendon>`
@@ -258,6 +258,8 @@ The following features are **unsupported**:
* - Category
- Feature
* - :ref:`margin<body-geom-margin>` and :ref:`gap<body-geom-gap>`
- Unimplemented for collisions with ``Mesh`` :ref:`Geom <mjtGeom>`.
* - :ref:`Transmission <mjtTrn>`
- ``TRN_JOINTINPARENT``, ``TRN_SLIDERCRANK``, ``TRN_BODY``
* - :ref:`Actuator Dynamics <mjtDyn>`
@@ -270,8 +272,6 @@ The following features are **unsupported**:
- ``PGS``
* - :ref:`Sensors <mjtSensor>`
- ``PLUGIN``, ``USER``
* - :ref:`Geom <mjtGeom>`
- ``SDF``
.. _MjxSharpBits:
@@ -299,8 +299,9 @@ Collisions between large meshes
SAT works well for smaller meshes but suffers in both runtime and memory for larger meshes.
For
collisions with convex meshes, the convex decompositon of the mesh should have
roughly **200 vertices or less** for reasonable performance.
collisions with convex meshes and primitives, the convex decompositon of the mesh should have
roughly **200 vertices or less** for reasonable performance. For convex-convex collisions,
the convex mesh should have roughly **fewer than 32 vertices**.
With careful
tuning, MJX can simulate scenes with mesh collisions -- see the MJX
`shadow hand <https://github.com/google-deepmind/mujoco/tree/main/mjx/mujoco/mjx/benchmark/model/shadow_hand>`__
doc/modeling.rst
+8 -4
View File
@@ -421,7 +421,7 @@ and the damping ratio is ignored. Equivalently, in the direct format, the :math:
impedance is constant, then the penetration depth at rest is
.. math::
r = \au \cdot (1 - d) \cdot \text{stiffness}
r = \frac{\au (1 - d)}{\text{stiffness}}
.. tip::
In the positive-value default format, the :math:`\text{timeconst}` parameter controls constraint **softness**.
@@ -431,6 +431,9 @@ and the damping ratio is ignored. Equivalently, in the direct format, the :math:
The negative-value "direct" format is more flexible, for example allowing for perfectly elastic collisions
(:math:`\text{damping} = 0`). It is the recommended format for system identification.
A :math:`\text{dampratio}` of 1 in the positive-value format is equivalent to
:math:`\text{damping} = 2 \sqrt{ \text{stiffness} }` in the direct format.
.. _CContact:
Contact parameters
@@ -1057,9 +1060,10 @@ Here we describe the XML attributes common to all sensor types, so as to avoid r
:at:`name`: :at-val:`string, optional`
Name of the sensor.
:at:`noise`: :at-val:`real, "0"`
The standard deviation of zero-mean Gaussian noise added to the sensor output, when the :at:`sensornoise`
attribute of :ref:`flag <option-flag>` is enabled. Sensor noise respects the sensor data type:
quaternions and unit vectors remain normalized, non-negative quantities remain non-negative.
The standard deviation of the noise model of this sensor. In versions prior to 3.1.4, this would lead to noise being
added to the sensors. In release 3.1.4 this feature was removed, see :doc:`3.1.4 changelog <changelog>` for a
detailed justification. As of subsequent versions, this attrbute serves as a convenient location for saving standard
deviation information for later use.
:at:`cutoff`: :at-val:`real, "0"`
When this value is positive, it limits the absolute value of the sensor output. It is also used to normalize the
sensor output in the sensor data plots in :ref:`simulate.cc <saSimulate>`.
doc/references.bib
+11
View File
@@ -88,3 +88,14 @@
year={2015},
publisher={Nature Publishing Group}
}
@article {Vaxenburg2024,
author = {Roman Vaxenburg and Igor Siwanowicz and Josh Merel and Alice A Robie and Carmen Morrow and Guido Novati and Zinovia Stefanidi and Gwyneth M Card and Michael B Reiser and Matthew M Botvinick and Kristin M Branson and Yuval Tassa and Srinivas C Turaga},
title = {Whole-body simulation of realistic fruit fly locomotion with deep reinforcement learning},
elocation-id = {2024.03.11.584515},
year = {2024},
doi = {10.1101/2024.03.11.584515},
publisher = {Cold Spring Harbor Laboratory},
journal = {bioRxiv}
}
include/mujoco/mjmodel.h
+7 -7
View File
@@ -74,12 +74,11 @@ typedef enum mjtEnableBit_ { // enable optional feature bitflags
mjENBL_ENERGY = 1<<1, // energy computation
mjENBL_FWDINV = 1<<2, // record solver statistics
mjENBL_INVDISCRETE = 1<<3, // discrete-time inverse dynamics
mjENBL_SENSORNOISE = 1<<4, // add noise to sensor data
// experimental features:
mjENBL_MULTICCD = 1<<5, // multi-point convex collision detection
mjENBL_ISLAND = 1<<6, // constraint island discovery
mjENBL_MULTICCD = 1<<4, // multi-point convex collision detection
mjENBL_ISLAND = 1<<5, // constraint island discovery
mjNENABLE = 7 // number of enable flags
mjNENABLE = 6 // number of enable flags
} mjtEnableBit;
@@ -641,9 +640,9 @@ struct mjModel_ {
int nemax; // number of potential equality-constraint rows
int njmax; // number of available rows in constraint Jacobian
int nconmax; // number of potential contacts in contact list
int nuserdata; // number of extra fields in mjData
int nsensordata; // number of fields in sensor data vector
int npluginstate; // number of fields in plugin state vector
int nuserdata; // number of mjtNums reserved for the user
int nsensordata; // number of mjtNums in sensor data vector
int npluginstate; // number of mjtNums in plugin state vector
size_t narena; // number of bytes in the mjData arena (inclusive of stack)
size_t nbuffer; // number of bytes in buffer
@@ -708,6 +707,7 @@ struct mjModel_ {
int* jnt_group; // group for visibility (njnt x 1)
mjtByte* jnt_limited; // does joint have limits (njnt x 1)
mjtByte* jnt_actfrclimited; // does joint have actuator force limits (njnt x 1)
mjtByte* jnt_actgravcomp; // is gravcomp force applied via actuators (njnt x 1)
mjtNum* jnt_solref; // constraint solver reference: limit (njnt x mjNREF)
mjtNum* jnt_solimp; // constraint solver impedance: limit (njnt x mjNIMP)
mjtNum* jnt_pos; // local anchor position (njnt x 3)
include/mujoco/mjxmacro.h
+1
View File
@@ -215,6 +215,7 @@
XMJV( int, jnt_group, njnt, 1 ) \
X ( mjtByte, jnt_limited, njnt, 1 ) \
X ( mjtByte, jnt_actfrclimited, njnt, 1 ) \
X ( mjtByte, jnt_actgravcomp, njnt, 1 ) \
X ( mjtNum, jnt_solref, njnt, mjNREF ) \
X ( mjtNum, jnt_solimp, njnt, mjNIMP ) \
X ( mjtNum, jnt_pos, njnt, 3 ) \
introspect/enums.py
+3 -4
View File
@@ -54,10 +54,9 @@ ENUMS: Mapping[str, EnumDecl] = dict([
('mjENBL_ENERGY', 2),
('mjENBL_FWDINV', 4),
('mjENBL_INVDISCRETE', 8),
('mjENBL_SENSORNOISE', 16),
('mjENBL_MULTICCD', 32),
('mjENBL_ISLAND', 64),
('mjNENABLE', 7),
('mjENBL_MULTICCD', 16),
('mjENBL_ISLAND', 32),
('mjNENABLE', 6),
]),
)),
('mjtJoint',
introspect/enums_test.py
+3 -4
View File
@@ -42,10 +42,9 @@ class EnumsTest(absltest.TestCase):
('mjENBL_ENERGY', 1<<1),
('mjENBL_FWDINV', 1<<2),
('mjENBL_INVDISCRETE', 1<<3),
('mjENBL_SENSORNOISE', 1<<4),
('mjENBL_MULTICCD', 1<<5),
('mjENBL_ISLAND', 1<<6),
('mjNENABLE', 7)))
('mjENBL_MULTICCD', 1<<4),
('mjENBL_ISLAND', 1<<5),
('mjNENABLE', 6)))
# values mostly increment by one with occasional overrides
def test_mjtGeom(self): # pylint: disable=invalid-name
introspect/structs.py
+10 -3
View File
@@ -1218,17 +1218,17 @@ STRUCTS: Mapping[str, StructDecl] = dict([
StructFieldDecl(
name='nuserdata',
type=ValueType(name='int'),
doc='number of extra fields in mjData',
doc='number of mjtNums reserved for the user',
),
StructFieldDecl(
name='nsensordata',
type=ValueType(name='int'),
doc='number of fields in sensor data vector',
doc='number of mjtNums in sensor data vector',
),
StructFieldDecl(
name='npluginstate',
type=ValueType(name='int'),
doc='number of fields in plugin state vector',
doc='number of mjtNums in plugin state vector',
),
StructFieldDecl(
name='narena',
@@ -1556,6 +1556,13 @@ STRUCTS: Mapping[str, StructDecl] = dict([
),
doc='does joint have actuator force limits (njnt x 1)',
),
StructFieldDecl(
name='jnt_actgravcomp',
type=PointerType(
inner_type=ValueType(name='mjtByte'),
),
doc='is gravcomp force applied via actuators (njnt x 1)',
),
StructFieldDecl(
name='jnt_solref',
type=PointerType(
mjx/mujoco/mjx/__init__.py
+2
View File
@@ -16,6 +16,7 @@
# pylint:disable=g-importing-member
from mujoco.mjx._src.collision_driver import collision
from mujoco.mjx._src.collision_driver import get_params
from mujoco.mjx._src.collision_driver import ncon
from mujoco.mjx._src.constraint import count_constraints
from mujoco.mjx._src.constraint import make_constraint
@@ -48,4 +49,5 @@ from mujoco.mjx._src.solver import solve
from mujoco.mjx._src.support import full_m
from mujoco.mjx._src.support import is_sparse
from mujoco.mjx._src.support import mul_m
from mujoco.mjx._src.test_util import benchmark
from mujoco.mjx._src.types import *
mjx/mujoco/mjx/_src/collision_base.py
+3 -3
View File
@@ -50,10 +50,10 @@ class GeomInfo(PyTreeNode):
size: jax.Array
face: Optional[jax.Array] = None
vert: Optional[jax.Array] = None
edge: Optional[jax.Array] = None
edge_dir: Optional[jax.Array] = None
facenorm: Optional[jax.Array] = None
face_edge: Optional[jax.Array] = None
face_edge_normal: Optional[jax.Array] = None
edge: Optional[jax.Array] = None
edge_face_normal: Optional[jax.Array] = None
class SolverParams(PyTreeNode):
mjx/mujoco/mjx/_src/collision_convex.py
+142 -130
View File
@@ -215,46 +215,47 @@ def sphere_convex(sphere: GeomInfo, convex: GeomInfo) -> Contact:
return jp.dot(pos - faces[0], normal)
support = get_support(faces, normals)
has_separating_axis = jp.any(support >= 0)
# Pick the face with minimal penetration as long as it has support.
support = jp.where(support >= 0, -1e12, support)
# Pick the face with the best separating axis.
best_idx = support.argmax()
face = faces[best_idx]
normal = normals[best_idx]
face_normal = normals[best_idx]
# Get closest point between the polygon face and the sphere center point.
# Project the sphere center point onto poly plane. If it's inside polygon
# edge normals, then we're done.
pt = _project_pt_onto_plane(sphere_pos, face[0], normal)
# side planes, then we're done.
pt = _project_pt_onto_plane(sphere_pos, face[0], face_normal)
edge_p0 = jp.roll(face, 1, axis=0)
edge_p1 = face
edge_normals = jax.vmap(jp.cross, in_axes=[0, None])(
side_normals = jax.vmap(jp.cross, in_axes=[0, None])(
edge_p1 - edge_p0,
normal,
face_normal,
)
edge_dist = jax.vmap(
lambda plane_pt, plane_norm: (pt - plane_pt).dot(plane_norm)
)(edge_p0, edge_normals)
inside = jp.all(edge_dist <= 0) # lte to handle degenerate edges
)(edge_p0, side_normals)
pt_on_face = jp.all(edge_dist <= 0) # lte to handle degenerate edges
# If the point is outside edge normals, project onto the closest edge plane
# If the point is outside side planes, project onto the closest side plane
# that the point is in front of.
degenerate_edge = jp.all(edge_normals == 0, axis=1)
degenerate_edge = jp.all(side_normals == 0, axis=1)
behind = edge_dist < 0.0
edge_dist = jp.where(degenerate_edge | behind, 1e12, edge_dist)
idx = edge_dist.argmin()
edge_pt = math.closest_segment_point(edge_p0[idx], edge_p1[idx], pt)
pt = jp.where(inside, pt, edge_pt)
pt = jp.where(pt_on_face, pt, edge_pt)
# Get the normal, dist, and contact position.
n, d = math.normalize_with_norm(pt - sphere_pos)
n = jp.where(inside | (d < 1e-6), normal, n)
pt_normal, d = math.normalize_with_norm(pt - sphere_pos)
# Ensure normal points towards convex centroid.
n *= jp.where(jp.dot(pt, n) < 0, 1, -1)
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)
d *= sign
spt = sphere_pos + n * sphere.size[0]
dist = d - sphere.size[0]
dist = jp.where(has_separating_axis, 1.0, d - sphere.size[0])
pos = (pt + spt) * 0.5
# Go back to world frame.
@@ -292,8 +293,7 @@ def capsule_convex(cap: GeomInfo, convex: GeomInfo) -> Contact:
support = get_support(faces, normals)
has_support = jp.all(support < 0)
# Pick the face with minimal penetration as long as it has support.
support = jp.where(support >= 0, -1e12, support)
# Pick the face with minimal penetration.
best_idx = support.argmax()
face = faces[best_idx]
normal = normals[best_idx]
@@ -302,12 +302,12 @@ def capsule_convex(cap: GeomInfo, convex: GeomInfo) -> Contact:
# the face.
edge_p0 = jp.roll(face, 1, axis=0)
edge_p1 = face
edge_normals = jax.vmap(jp.cross, in_axes=[0, None])(
side_planes = jax.vmap(jp.cross, in_axes=[0, None])(
edge_p1 - edge_p0,
normal,
)
cap_pts_clipped, mask = _clip_edge_to_planes(
cap_pts[0], cap_pts[1], edge_p0, edge_normals
cap_pts[0], cap_pts[1], edge_p0, side_planes
)
cap_pts_clipped = cap_pts_clipped - normal * cap.size[0]
face_pts = jax.vmap(_project_pt_onto_plane, in_axes=[0, None, None])(
@@ -316,33 +316,65 @@ def capsule_convex(cap: GeomInfo, convex: GeomInfo) -> Contact:
# Create variables for the face contact.
pos = (cap_pts_clipped + face_pts) * 0.5
contact_normal = -jp.stack([normal] * 2, 0)
penetration = jp.where(
face_penetration = jp.where(
mask & has_support, jp.dot(face_pts - cap_pts_clipped, normal), -1
)
# Get a potential edge contact.
edge_closest, cap_closest, t_a, t_b = jax.vmap(
math.closest_segment_to_segment_points_w_barycentric,
in_axes=[0, 0, None, None],
)(edge_p0, edge_p1, cap_pts[0], cap_pts[1])
e_idx = ((edge_closest - cap_closest) ** 2).sum(axis=1).argmin()
cap_closest_pt, edge_closest_pt = cap_closest[e_idx], edge_closest[e_idx]
edge_dist = math.norm(cap_closest_pt - edge_closest_pt)
t_a, t_b = t_a[e_idx], t_b[e_idx]
in_segment = ((t_a > 0) & (t_a < 1)) | ((t_b > 0) & (t_b < 1))
# Ensure edge_axis points towards the convex centroid.
edge_axis = jp.cross(edge_p0[e_idx] - edge_p1[e_idx], axis)
sign = jp.where(jp.dot(edge_closest_pt, edge_axis) < 0, 1, -1)
edge_axis *= sign
degenerate_edge_axis = (edge_axis**2).sum() < 1e-6
edge_axis = math.normalize(edge_axis)
# Pick a potential shallow edge contact.
def get_edge_axis(edge):
edge_closest_pt, cap_closest_pt = math.closest_segment_to_segment_points(
edge[0], edge[1], cap_pts[0], cap_pts[1]
)
edge_dir = edge_closest_pt - cap_closest_pt
degenerate_edge_dir = jp.sum(jp.square(edge_dir)) < 1e-6
edge_axis, edge_dist = math.normalize_with_norm(edge_dir)
return (
edge_dist,
edge_axis,
degenerate_edge_dir,
edge_closest_pt,
cap_closest_pt,
)
edge = jp.take(convex.vert, convex.edge, axis=0)
edge_face_normal = convex.edge_face_normal # pytype: disable=attribute-error
res = jax.vmap(get_edge_axis)(edge.reshape(-1, 2, 3))
e_idx = jp.abs(res[0]).argmin()
(
edge_dist,
edge_axis,
degenerate_edge_dir,
edge_closest_pt,
cap_closest_pt,
) = jax.tree_map(lambda x, i=e_idx: jp.take(x, i, axis=0), res)
edge_face_normals = edge_face_normal[e_idx]
edge_voronoi_front = ((edge_face_normals @ edge_axis) < 0).all()
shallow = ~degenerate_edge_dir & edge_voronoi_front
edge_penetration = jp.where(shallow, cap.size[0] - edge_dist, -1)
# Determine edge contact position.
edge_pos = (
edge_closest_pt + (cap_closest_pt + edge_axis * cap.size[0])
) * 0.5
edge_penetration = cap.size[0] - edge_dist
has_edge_contact = (edge_penetration > 0) & in_segment & ~degenerate_edge_axis
edge_dir_parallel_to_face = (
jp.abs(edge_axis.dot(normal)) > 0.99
) & ~degenerate_edge_dir
min_face_penetration = face_penetration.min()
has_edge_contact = (
(edge_penetration > 0)
# prefer edge contact if the edge is smaller than face penetration
& jp.where(
min_face_penetration > 0,
edge_penetration < min_face_penetration,
True,
)
# prefer face contact if the edge axis is parallel to the face normal
& ~edge_dir_parallel_to_face
# make sure we have a shallow contact
& edge_voronoi_front
)
# Get the contact info.
pos = jp.where(has_edge_contact, pos.at[0].set(edge_pos), pos)
@@ -355,7 +387,7 @@ def capsule_convex(cap: GeomInfo, convex: GeomInfo) -> Contact:
n = n @ convex.mat.T
dist = -jp.where(
has_edge_contact, penetration.at[0].set(edge_penetration), penetration
has_edge_contact, jp.array([edge_penetration, -1]), face_penetration
)
frame = jax.vmap(math.make_frame)(n)
return dist, pos, frame
@@ -598,8 +630,9 @@ def _sat_bruteforce(
We return both the edge and face contacts. Valid contacts can be checked with
dist < 0. Resulting edge contacts should be preferred over face contacts.
This method checks all unique edge-pairs and is thus costly to run over large
meshes, but is more performant for smaller meshes (boxes, tetrahedra, etc.).
This method checks all separating axes via a brute force support function, and
is thus costly to run over large meshes, but is more performant for smaller
meshes (boxes, tetrahedra, etc.).
Args:
faces_a: Faces for hull A.
@@ -699,7 +732,7 @@ def _arcs_intersect(
return (cba * dba < 0) & (adc * bdc < 0) & (cba * bdc > 0)
def _sat_approx(
def _sat_gaussmap(
centroid_a: jax.Array,
faces_a: jax.Array,
faces_b: jax.Array,
@@ -707,23 +740,16 @@ def _sat_approx(
vertices_b: jax.Array,
normals_a: jax.Array,
normals_b: jax.Array,
face_edges_a: jax.Array,
face_edges_b: jax.Array,
face_edge_normals_a: jax.Array,
face_edge_normals_b: jax.Array,
edges_a: jax.Array,
edges_b: jax.Array,
edge_face_normals_a: jax.Array,
edge_face_normals_b: jax.Array,
) -> Tuple[jax.Array, jax.Array, jax.Array]:
"""Runs the Separating Axis Test for a pair of hulls.
Runs the separating axis test for all faces. After obtaining a reference
and incident face, tests edge separating axes via edge intersections
on gauss maps.
Certain meshes can have nearly parallel coplanar faces that are not merged.
Thus reference/incident faces can be non-overlapping, and face contacts will
not get generated. Since we only check edge separating axes on
reference/incident faces, the correct edge separating axes may also be
missing. We mitigate this issue by checking nearly coplanar anti-parallel
reference/incident faces for support, hence why this method is "approximate".
Runs the separating axis test for all faces. Tests edge separating axes via
edge intersections on gauss maps for all edge pairs. h/t to Dirk Gregorius
for the implementation details and gauss map trick.
Args:
centroid_a: Centroid of hull A.
@@ -733,10 +759,10 @@ def _sat_approx(
vertices_b: Vertices for hull B.
normals_a: Normal vectors for hull A faces.
normals_b: Normal vectors for hull B faces.
face_edges_a: Edges for faces in hull A.
face_edges_b: Edges for faces in hull B.
face_edge_normals_a: Edge normals for faces in hull A.
face_edge_normals_b: Edge normals for faces in hull B.
edges_a: Edges for hull A.
edges_b: Edges for hull B.
edge_face_normals_a: Face normals for edges in hull A.
edge_face_normals_b: Face normals for edges in hull B.
Returns:
tuple of dist, pos, and normal
@@ -784,75 +810,61 @@ def _sat_approx(
-best_axis,
)
# Handle edge separating axes by checking edge pairs on the reference and
# incident faces. In principle, the correct edge-edge separating axis can be
# created from edge pairs on the reference and incident faces.
# first get the edge directions and face edge normals
face_edges_a = face_edges_a[face_a_idx]
v_norm = jax.vmap(math.normalize)
face_edges_dir_a = v_norm(face_edges_a[:, 0] - face_edges_a[:, 1])
face_edges_b = face_edges_b[face_b_idx]
face_edges_dir_b = v_norm(face_edges_b[:, 0] - face_edges_b[:, 1])
face_edge_normals_a = face_edge_normals_a[face_a_idx]
face_edge_normals_b = face_edge_normals_b[face_b_idx]
# scatter to all edge pairs
edge_idx_a = jp.repeat(
jp.arange(face_edges_a.shape[0]), face_edges_b.shape[0]
# Handle edge separating axes by checking all edge pairs.
a_idx = jp.tile(jp.arange(edges_a.shape[0]), reps=edges_b.shape[0])
b_idx = jp.repeat(
jp.arange(edges_b.shape[0]), repeats=edges_a.shape[0], axis=0
)
normal_a_1 = edge_face_normals_a[a_idx, 0]
normal_a_2 = edge_face_normals_a[a_idx, 1]
normal_b_1 = edge_face_normals_b[b_idx, 0]
normal_b_2 = edge_face_normals_b[b_idx, 1]
is_minkowski_face = jax.vmap(_arcs_intersect)(
normal_a_1, normal_a_2, -normal_b_1, -normal_b_2
)
edge_idx_b = jp.tile(jp.arange(face_edges_b.shape[0]), face_edges_a.shape[0])
face_edges_dir_a = face_edges_dir_a[edge_idx_a]
face_edges_dir_b = face_edges_dir_b[edge_idx_b]
face_edges_pt_a = face_edges_a[:, 0][edge_idx_a]
face_edges_pt_b = face_edges_b[:, 0][edge_idx_b]
face_edge_normals_a = face_edge_normals_a[edge_idx_a]
face_edge_normals_b = face_edge_normals_b[edge_idx_b]
@jax.vmap
# get distances
edge_a_dir = jax.vmap(math.normalize)(edges_a[:, 0] - edges_a[:, 1])[a_idx]
edge_b_dir = jax.vmap(math.normalize)(edges_b[:, 0] - edges_b[:, 1])[b_idx]
edges_a, edges_b = edges_a[a_idx], edges_b[b_idx]
edge_a_pt, edge_a_pt_2 = edges_a[:, 0], edges_a[:, 1]
edge_b_pt, edge_b_pt_2 = edges_b[:, 0], edges_b[:, 1]
def get_normals(a_dir, a_pt, b_dir):
edge_axis = math.normalize(jp.cross(a_dir, b_dir))
edge_axis = jp.cross(a_dir, b_dir)
degenerate_edge_axis = jp.sum(edge_axis**2) < 1e-6
edge_axis = math.normalize(edge_axis)
# correct normal to point from a to b, object b is at the origin
sign = jp.where(jp.dot(edge_axis, a_pt - centroid_a) > 0.0, 1.0, -1.0)
return edge_axis * sign
return edge_axis * sign, degenerate_edge_axis
edge_axes = get_normals(face_edges_dir_a, face_edges_pt_a, face_edges_dir_b)
edge_dist = jax.vmap(jp.dot)(edge_axes, face_edges_pt_b - face_edges_pt_a)
# handle degenerate axes
edge_dist = jp.where((edge_axes**2).sum(axis=1) < 1e-6, 1e6, edge_dist)
# ensure edges create a minkowski face by testing intersection on gauss maps
is_minkowski_face = jax.vmap(_arcs_intersect)(
face_edge_normals_a[:, 0],
face_edge_normals_a[:, 1],
-face_edge_normals_b[:, 0],
-face_edge_normals_b[:, 1],
)
edge_dist = jp.where(is_minkowski_face, edge_dist, 1e6)
edge_dist = jp.where(edge_dist > 0, -1e6, edge_dist)
edge_axes, degenerate_edge_axes = jax.vmap(get_normals)(
edge_a_dir, edge_a_pt, edge_b_dir)
edge_dist = jax.vmap(jp.dot)(edge_axes, edge_b_pt - edge_a_pt)
# handle degenerate axis
edge_dist = jp.where(degenerate_edge_axes, -jp.inf, edge_dist)
# ensure edges create minkowski face
edge_dist = jp.where(is_minkowski_face, edge_dist, -jp.inf)
best_edge_idx = edge_dist.argmax()
best_edge_dist = edge_dist[best_edge_idx]
# prefer edge over face contacts as long as we have a valid edge contact
is_edge_contact = (best_edge_dist > dist.min() + 1e-6) & (best_edge_dist < 0)
is_edge_contact = jp.where(
dist.max() < 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)
dist = jp.where(is_edge_contact, jp.array([best_edge_dist, 1, 1, 1]), dist)
# A failure mode occurs if faces are very narrow and nearly parallel
# (i.e. faces did not get merged properly as coplanar faces). The face
# contacts will be empty since the reference/incident faces may not overlap.
# An edge-edge separating axis will not be found, since the reference and
# incident faces will also not overlap or necessarily create a minkowski face.
# Thus, we approximate the contact for anti-parallel faces with support.
anti_parallel = incident_face_norm.dot(ref_face_norm) < -0.97
dist = dist.at[0].set(
jp.where(
anti_parallel
& ~is_face_separating
& (dist > 0).all()
& ~is_edge_contact,
-support[best_idx],
dist[0],
)
dist = jp.where(
is_edge_contact,
jp.array([best_edge_dist, 1, 1, 1]),
dist,
)
a_closest, b_closest = math.closest_segment_to_segment_points(
edge_a_pt[best_edge_idx], edge_a_pt_2[best_edge_idx],
edge_b_pt[best_edge_idx], edge_b_pt_2[best_edge_idx])
pos = jp.where(
is_edge_contact,
jp.tile(0.5 * (a_closest + b_closest), (4, 1)), pos)
return dist, pos, normal
@@ -892,15 +904,15 @@ def convex_convex(c1: GeomInfo, c2: GeomInfo) -> Contact:
unique_edges1 = jp.take(vertices1, c1.edge, axis=0)
unique_edges2 = jp.take(vertices2, c2.edge, axis=0)
face_edges1 = jp.take(vertices1, c1.face_edge, axis=0)
face_edges2 = jp.take(vertices2, c2.face_edge, axis=0)
edges1 = jp.take(vertices1, c1.edge, axis=0)
edges2 = jp.take(vertices2, c2.edge, axis=0)
face_edge_normals1 = c1.face_edge_normal @ to_local_mat.T
face_edge_normals2 = c2.face_edge_normal
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]
< face_edges1[0].shape[0] * face_edges2[0].shape[0]
< edges1[0].shape[0] * edges2[0].shape[0]
)
if enable_bruteforce:
dist, pos, normal = _sat_bruteforce(
@@ -914,7 +926,7 @@ def convex_convex(c1: GeomInfo, c2: GeomInfo) -> Contact:
unique_edges2,
)
else:
dist, pos, normal = _sat_approx(
dist, pos, normal = _sat_gaussmap(
to_local_pos,
faces1,
faces2,
@@ -922,10 +934,10 @@ def convex_convex(c1: GeomInfo, c2: GeomInfo) -> Contact:
vertices2,
normals1,
normals2,
face_edges1,
face_edges2,
face_edge_normals1,
face_edge_normals2,
edges1,
edges2,
edge_face_normals1,
edge_face_normals2,
)
# Go back to world frame.
mjx/mujoco/mjx/_src/collision_driver.py
+36 -26
View File
@@ -14,7 +14,7 @@
# ==============================================================================
"""Collide geometries."""
from typing import Callable, Dict, Optional, Sequence, Tuple, Union
from typing import Callable, Dict, List, Optional, Sequence, Tuple, Union
import jax
from jax import numpy as jp
@@ -33,6 +33,7 @@ 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
from mujoco.mjx._src.collision_primitive import plane_capsule
from mujoco.mjx._src.collision_primitive import plane_ellipsoid
from mujoco.mjx._src.collision_primitive import plane_sphere
from mujoco.mjx._src.collision_primitive import sphere_capsule
from mujoco.mjx._src.collision_primitive import sphere_sphere
@@ -48,6 +49,7 @@ _COLLISION_FUNC = {
(GeomType.PLANE, GeomType.SPHERE): plane_sphere,
(GeomType.PLANE, GeomType.CAPSULE): plane_capsule,
(GeomType.PLANE, GeomType.BOX): plane_convex,
(GeomType.PLANE, GeomType.ELLIPSOID): plane_ellipsoid,
(GeomType.PLANE, GeomType.MESH): plane_convex,
(GeomType.SPHERE, GeomType.SPHERE): sphere_sphere,
(GeomType.SPHERE, GeomType.CAPSULE): sphere_capsule,
@@ -187,6 +189,31 @@ def _dynamic_params(
return SolverParams(friction, solref, solreffriction, solimp, margin, gap)
def get_params(
m: Union[Model, mujoco.MjModel], candidates: Sequence[Candidate]
) -> Tuple[List[int], List[int], SolverParams]:
"""Gets solver params for a list of collision candidates."""
# group sol params by different candidate types
typ_cands = {}
for c in candidates:
typ = (c.ipair > -1, c.geomp > -1)
typ_cands.setdefault(typ, []).append(c)
geom1, geom2, params = [], [], []
for (pair, priority), candidates in typ_cands.items():
geom1.extend([c.geom1 for c in candidates])
geom2.extend([c.geom2 for c in candidates])
if pair:
params.append(_pair_params(m, candidates))
elif priority:
params.append(_priority_params(m, candidates))
else:
params.append(_dynamic_params(m, candidates))
params = jax.tree_map(lambda *x: jp.concatenate(x), *params)
return geom1, geom2, params
def _pair_info(
m: Model, d: Data, geom1: Sequence[int], geom2: Sequence[int]
) -> Tuple[GeomInfo, GeomInfo, Sequence[Dict[str, Optional[int]]]]:
@@ -205,20 +232,20 @@ def _pair_info(
info = info.replace(
face=jp.stack([m.geom_convex_face[i] for i in geom]),
vert=jp.stack([m.geom_convex_vert[i] for i in geom]),
edge=jp.stack([m.geom_convex_edge_dir[i] for i in geom]),
edge_dir=jp.stack([m.geom_convex_edge_dir[i] for i in geom]),
facenorm=jp.stack([m.geom_convex_facenormal[i] for i in geom]),
face_edge_normal=jp.stack(
[m.geom_convex_face_edge_normal[i] for i in geom]
edge=jp.stack([m.geom_convex_edge[i] for i in geom]),
edge_face_normal=jp.stack(
[m.geom_convex_edge_face_normal[i] for i in geom]
),
face_edge=jp.stack([m.geom_convex_face_edge[i] for i in geom]),
)
in_axes = in_axes.replace(
face=0,
vert=0,
edge=0,
edge_dir=0,
facenorm=0,
face_edge=0,
face_edge_normal=0,
edge=0,
edge_face_normal=0,
)
return info, in_axes
@@ -276,24 +303,7 @@ def _collide_geoms(
if not fn:
return Contact.zero()
# group sol params by different candidate types
typ_cands = {}
for c in candidates:
typ = (c.ipair > -1, c.geomp > -1)
typ_cands.setdefault(typ, []).append(c)
geom1, geom2, params = [], [], []
for (pair, priority), candidates in typ_cands.items():
geom1.extend([c.geom1 for c in candidates])
geom2.extend([c.geom2 for c in candidates])
if pair:
params.append(_pair_params(m, candidates))
elif priority:
params.append(_priority_params(m, candidates))
else:
params.append(_dynamic_params(m, candidates))
params = jax.tree_map(lambda *x: jp.concatenate(x), *params)
geom1, geom2, params = get_params(m, candidates)
g1, g2, in_axes = _pair_info(m, d, geom1, geom2)
# Run a crude version of broadphase.
mjx/mujoco/mjx/_src/collision_driver_test.py
+163 -23
View File
@@ -132,37 +132,93 @@ class SphereCollisionTest(parameterized.TestCase):
</mujoco>
"""
def test_sphere_convex(self):
d, dx = _collide(self._SPHERE_CONVEX)
def test_sphere_convex_face(self):
# no contact
xml = self._SPHERE_CONVEX.replace(
'<body pos="0.52 0 0.52">', '<body pos="0.55 0 0.5">'
)
d, dx = _collide(xml)
self.assertEmpty(d.contact.dist)
self.assertGreater(dx.contact.dist, 0)
# face contact
xml = self._SPHERE_CONVEX.replace(
'<body pos="0.52 0 0.52">', '<body pos="0.51 0 0.25">'
)
d, dx = _collide(xml)
for field in dataclasses.fields(Contact):
_assert_attr_eq(dx.contact, d.contact, field.name, 'sphere_convex', 1e-4)
_assert_attr_eq(dx.contact, d.contact, field.name, 'face', 1e-4)
# test deep penetration
# deep face contact
xml = self._SPHERE_CONVEX.replace(
'<body pos="0.52 0 0.52">', '<body pos="0.48 0 0.47">'
)
d, dx = _collide(xml)
self.assertTrue((dx.contact.dist < 0).all())
self.assertTrue((d.contact.dist < 0).all())
np.testing.assert_allclose(dx.contact.dist, [-0.07], atol=1e-5)
np.testing.assert_array_almost_equal(dx.contact.pos, d.contact.pos)
np.testing.assert_array_almost_equal(
dx.contact.frame, d.contact.frame.reshape((-1, 3, 3))
)
def test_sphere_convex_edge(self):
# edge contact
d, dx = _collide(self._SPHERE_CONVEX)
for field in dataclasses.fields(Contact):
_assert_attr_eq(dx.contact, d.contact, field.name, 'edge', 1e-4)
# deep edge penetration
xml = self._SPHERE_CONVEX.replace(
'<body pos="0.52 0 0.52">', '<body pos="0.49 0 0.49">'
)
d, dx = _collide(xml)
self.assertTrue((dx.contact.dist < 0).all())
self.assertTrue((d.contact.dist < 0).all())
np.testing.assert_allclose(dx.contact.dist, [-0.06], atol=1e-5)
np.testing.assert_array_almost_equal(dx.contact.pos, d.contact.pos)
np.testing.assert_array_almost_equal(
dx.contact.frame, d.contact.frame.reshape((-1, 3, 3))
)
# test sphere center on vertex
# vertex contact
xml = self._SPHERE_CONVEX.replace(
'<body pos="0.52 0 0.52">', '<body pos="0.5 0.52 0.51">'
)
d, dx = _collide(xml)
for field in dataclasses.fields(Contact):
_assert_attr_eq(dx.contact, d.contact, field.name, 'vertex', 1e-4)
# sphere center on vertex
xml = self._SPHERE_CONVEX.replace(
'<body pos="0.52 0 0.52">', '<body pos="0.5 0 0.5">'
)
d, dx = _collide(xml)
for field in dataclasses.fields(Contact):
_assert_attr_eq(dx.contact, d.contact, field.name, 'vertex_center', 1e-4)
self.assertTrue((dx.contact.dist < 0).all())
self.assertTrue((d.contact.dist < 0).all())
np.testing.assert_array_almost_equal(dx.contact.pos, d.contact.pos)
np.testing.assert_array_almost_equal(
dx.contact.frame, d.contact.frame.reshape((-1, 3, 3))
)
class EllipsoidCollisionTest(parameterized.TestCase):
_ELLIPSOID_PLANE = """
<mujoco>
<worldbody>
<geom name="floor" size="0 0 .05" type="plane"/>
<body pos="0 0 0.03" euler="45 0 0">
<freejoint/>
<geom size=".15 .03 .05" type="ellipsoid"/>
</body>
</worldbody>
</mujoco>
"""
def test_plane_ellipsoid(self):
"""Tests ellipsoid plane contact."""
d, dx = _collide(self._ELLIPSOID_PLANE)
self.assertLess(dx.contact.dist[0], 0)
for field in dataclasses.fields(Contact):
_assert_attr_eq(
dx.contact, d.contact, field.name, 'ellipsoid-plane', 1e-5)
class CapsuleCollisionTest(parameterized.TestCase):
@@ -248,8 +304,8 @@ class CapsuleCollisionTest(parameterized.TestCase):
</mujoco>
"""
def test_capsule_convex(self):
"""Tests a capsule-convex collision for a face contact."""
def test_capsule_convex_face(self):
"""Tests face contact."""
d, dx = _collide(self._CAP_BOX)
# sort positions for comparison
@@ -263,7 +319,8 @@ class CapsuleCollisionTest(parameterized.TestCase):
for field in dataclasses.fields(Contact):
_assert_attr_eq(dx.contact, d.contact, field.name, 'capsule_convex', 1e-4)
# test deep face penetration
def test_capsule_convex_face_deep(self):
"""Tests deep face penetration."""
xml = self._CAP_BOX.replace('<body pos="0 0 0.54">', '<body pos="0 0 0.4">')
_, dx = _collide(xml)
@@ -294,7 +351,7 @@ class CapsuleCollisionTest(parameterized.TestCase):
"""
def test_capsule_convex_edge(self):
"""Tests a capsule-convex collision for an edge contact."""
"""Tests edge contact."""
d, dx = _collide(self._CAP_EDGE_BOX)
c = dx.contact
@@ -305,17 +362,62 @@ class CapsuleCollisionTest(parameterized.TestCase):
for field in dataclasses.fields(Contact):
_assert_attr_eq(c, d.contact, field.name, 'capsule_convex_edge', 1e-4)
# test deep edge penetration
def test_capsule_convex_edge_deep(self):
"""Tests deep edge penetration."""
xml = self._CAP_EDGE_BOX.replace(
'<body pos="0.5 0 0.55"', '<body pos="0.5 0 0.48"'
'<body pos="0.5 0 0.55"', '<body pos="0.5 0 0.42"'
)
_, dx = _collide(xml)
np.testing.assert_array_equal(dx.contact.dist < 0, np.array([True, False]))
np.testing.assert_array_almost_equal(dx.contact.dist[0], np.array([-0.13]))
np.testing.assert_array_almost_equal(
dx.contact.pos[0], np.array([0.483, 0, 0.471]), decimal=3
dx.contact.pos[0], np.array([0.5, 0, 0.435]), decimal=3
)
np.testing.assert_array_almost_equal(
dx.contact.frame[0, 0], np.array([-0.5, 0, -0.866]), decimal=3
dx.contact.frame[0, 0], np.array([0, 0, -1]), decimal=3
)
def test_capsule_convex_edge_shallow_tip(self):
"""Tests shallow edge penetration on the tip of the capsule."""
# the capsule sphere is inside the edge voronoi region, so there is an
# edge contact
xml = self._CAP_EDGE_BOX.replace(
'<geom fromto="-0.6 0 0 0.6 0 0" size="0.05"',
'<geom fromto="0.6 0 0.6 -0.05 0 0" size="0.1"',
)
xml = xml.replace('<body pos="0.5 0 0.55"', '<body pos="0.58 0 0.55"')
d, dx = _collide(xml)
c = dx.contact
self.assertEqual(c.pos.shape[0], 2)
self.assertGreater(c.dist[1], 0)
# extract the contact point with penetration
c = jax.tree_map(lambda x: jp.take(x, 0, axis=0)[None], dx.contact)
for field in dataclasses.fields(Contact):
_assert_attr_eq(c, d.contact, field.name, 'edge_shallow_tip1', 1e-4)
np.testing.assert_array_almost_equal(
dx.contact.frame[0][0, :3], np.array([-0.43952, 0.0, -0.898233])
)
# the capsule sphere is outside the edge voronoi region, so there is a
# face contact
xml = self._CAP_EDGE_BOX.replace(
'<geom fromto="-0.6 0 0 0.6 0 0" size="0.05"',
'<geom fromto="-0.6 0 0.6 -0.05 0 0" size="0.1"',
)
xml = xml.replace('<body pos="0.5 0 0.55"', '<body pos="0.5 0 0.52"')
d, dx = _collide(xml)
c = dx.contact
self.assertEqual(c.pos.shape[0], 2)
self.assertGreater(c.dist[1], 0)
# extract the contact point with penetration
c = jax.tree_map(lambda x: jp.take(x, 0, axis=0)[None], dx.contact)
for field in dataclasses.fields(Contact):
_assert_attr_eq(c, d.contact, field.name, 'edge_shallow_tip2', 1e-4)
np.testing.assert_array_almost_equal(
dx.contact.frame[0][0, :3], np.array([0.0, 0.0, -1.0])
)
@@ -448,7 +550,7 @@ class ConvexTest(absltest.TestCase):
"""
def test_convex_convex(self):
"""Tests generic convex-convex collision via _sat_approx."""
"""Tests generic convex-convex collision via _sat_gaussmap."""
directory = epath.resource_path('mujoco.mjx')
assets = {
'meshes/dodecahedron.stl': (
@@ -463,6 +565,44 @@ class ConvexTest(absltest.TestCase):
np.testing.assert_array_less(0, c.dist[1:])
np.testing.assert_array_almost_equal(c.frame[0, 0], np.array([0, 0, 1]))
_CONVEX_CONVEX_THIN = """
<mujoco>
<asset>
<mesh name="poly"
vertex="0.3 0 0 0 0.5 0 -0.3 0 0 0 -0.5 0 0 -1 1 0 1 1"
face="0 1 5 0 5 4 0 4 3 3 4 2 2 4 5 1 2 5 0 2 1 0 3 2"/>
</asset>
<worldbody>
<body pos="0.0 2.0 0.35" euler="0 0 90">
<freejoint/>
<geom size="0.2 0.2 0.2" type="mesh" mesh="poly"/>
</body>
<body pos="0.0 2.0 2.281" euler="180 0 0">
<freejoint/>
<geom size="0.2 0.2 0.2" type="mesh" mesh="poly"/>
</body>
</worldbody>
</mujoco>
"""
def test_convex_convex_edge(self):
"""Tests convex-convex collisions with edge contact via _sat_gaussmap."""
_, dx = _collide(self._CONVEX_CONVEX_THIN)
c = dx.contact
# Only one contact point for an edge contact.
self.assertLess(c.dist[0], 0)
np.testing.assert_array_less(0, c.dist[1:])
np.testing.assert_array_almost_equal(c.frame[0, 0], np.array([0, 0, 1]))
np.testing.assert_array_almost_equal(
c.pos[0], np.array([0, 2, 1.3155]), decimal=5)
_, dx = _collide(
self._CONVEX_CONVEX_THIN.replace(
'pos="0.0 2.0 0.35"', 'pos="0.0 2.0 0"'))
c = dx.contact
self.assertTrue((c.dist > 0).all())
class BodyPairFilterTest(absltest.TestCase):
"""Tests that certain body pairs get filtered."""
@@ -568,11 +708,11 @@ class NconTest(parameterized.TestCase):
m.numeric_data[m.numeric_adr[i]] = -1
ncon = collision_driver.ncon(m)
self.assertEqual(ncon, 307)
self.assertEqual(ncon, 98)
mx = mjx.put_model(m)
ncon = collision_driver.ncon(mx)
self.assertEqual(ncon, 307)
self.assertEqual(ncon, 98)
class TopKContactTest(absltest.TestCase):
mjx/mujoco/mjx/_src/collision_primitive.py
+14
View File
@@ -65,6 +65,19 @@ def plane_capsule(plane: GeomInfo, cap: GeomInfo) -> Contact:
return jax.tree_map(lambda *x: jp.concatenate(x), *contacts)
def plane_ellipsoid(plane: GeomInfo, ellipsoid: GeomInfo) -> Contact:
"""Calculates one contact between an ellipsoid and a plane."""
n = plane.mat[:, 2]
size = ellipsoid.size
sphere_support = -math.normalize((ellipsoid.mat.T @ n) * size)
pos = ellipsoid.pos + ellipsoid.mat @ (sphere_support * size)
dist = jp.dot(n, pos - plane.pos)
pos = pos - n * dist * 0.5
return jax.tree_map(
lambda x: jp.expand_dims(x, axis=0), (dist, pos, math.make_frame(n))
)
def _sphere_sphere(
pos1: jax.Array, radius1: jax.Array, pos2: jax.Array, radius2: jax.Array
) -> Contact:
@@ -121,6 +134,7 @@ def capsule_capsule(cap1: GeomInfo, cap2: GeomInfo) -> Contact:
# store ncon as function attributes
plane_sphere.ncon = 1
plane_capsule.ncon = 2
plane_ellipsoid.ncon = 1
sphere_sphere.ncon = 1
sphere_capsule.ncon = 1
capsule_capsule.ncon = 1
mjx/mujoco/mjx/_src/device.py
+7 -2
View File
@@ -138,8 +138,7 @@ def _validate(m: mujoco.MjModel):
raise NotImplementedError('Tendons are not supported.')
# check collision geom types
candidate_set = collision_driver.collision_candidates(m)
for g1, g2, *_ in candidate_set:
for (g1, g2, *_), c in collision_driver.collision_candidates(m).items():
g1, g2 = mujoco.mjtGeom(g1), mujoco.mjtGeom(g2)
if g1 == mujoco.mjtGeom.mjGEOM_PLANE and g2 in (
mujoco.mjtGeom.mjGEOM_PLANE,
@@ -149,6 +148,12 @@ def _validate(m: mujoco.MjModel):
continue
if collision_driver.get_collision_fn((g1, g2)) is None:
raise NotImplementedError(f'({g1}, {g2}) collisions not implemented.')
*_, params = collision_driver.get_params(m, c)
margin_gap = not np.allclose(np.concatenate([params.margin, params.gap]), 0)
if mujoco.mjtGeom.mjGEOM_MESH in (g1, g2) and margin_gap:
raise NotImplementedError(
f'Margin and gap not implemented for ({g1}, {g2})'
)
# TODO(erikfrey): warn for high solver iterations, nefc, etc.
mjx/mujoco/mjx/_src/io.py
+13 -3
View File
@@ -91,10 +91,16 @@ def put_model(m: mujoco.MjModel, device=None) -> types.Model:
raise NotImplementedError('only condim=3 is supported')
# check collision geom types
for g1, g2, *_ in collision_driver.collision_candidates(m):
if collision_driver.get_collision_fn((g1, g2)) is None:
for (g1, g2, *_), c in collision_driver.collision_candidates(m).items():
g1, g2 = mujoco.mjtGeom(g1), mujoco.mjtGeom(g2)
if collision_driver.get_collision_fn((g1, g2)) is None:
raise NotImplementedError(f'({g1}, {g2}) has no collision function')
*_, params = collision_driver.get_params(m, c)
margin_gap = not np.allclose(np.concatenate([params.margin, params.gap]), 0)
if mujoco.mjtGeom.mjGEOM_MESH in (g1, g2) and margin_gap:
raise NotImplementedError(
f'Margin and gap not implemented for ({g1}, {g2})'
)
for enum_field, enum_type, mj_type in (
(m.actuator_biastype, types.BiasType, mujoco.mjtBias),
@@ -172,7 +178,7 @@ def make_data(m: Union[types.Model, mujoco.MjModel]) -> types.Data:
ctrl=zero_nu,
qfrc_applied=zero_nv,
xfrc_applied=zero_nbody_6,
eq_active=jp.zeros(m.neq, dtype=int),
eq_active=jp.zeros(m.neq, dtype=jp.uint8),
qacc=zero_nv,
act_dot=zero_na,
xpos=zero_nbody_3,
@@ -233,6 +239,7 @@ def _get_contact(
ncon = cx.dist.shape[0]
c.efc_address[:] = np.arange(efc_start, efc_start + ncon * 4, 4)[con_id]
c.dim[:] = 3
def get_data(
@@ -374,6 +381,9 @@ def put_data(m: mujoco.MjModel, d: mujoco.MjData, device=None) -> types.Data:
for fname in ('xmat', 'ximat', 'geom_xmat', 'site_xmat', 'cam_xmat'):
fields[fname] = fields[fname].reshape((-1, 3, 3))
# MJX does not support islanding, so only transfer the first solver_niter
fields['solver_niter'] = fields['solver_niter'][0]
# pad efc fields: MuJoCo efc arrays are sparse for inactive constraints.
# efc_J is also optionally column-sparse (typically for large nv). MJX is
# neither: it contains zeros for inactive constraints, and efc_J is always
mjx/mujoco/mjx/_src/io_test.py
+34
View File
@@ -209,6 +209,25 @@ class ModelIOTest(parameterized.TestCase):
</worldbody>
</mujoco>"""))
def test_margin_gap_mesh_not_implemented(self):
with self.assertRaises(NotImplementedError):
mjx.put_model(mujoco.MjModel.from_xml_string("""
<mujoco>
<asset>
<mesh name="box" vertex="-1 -1 -1 1 -1 -1 1 1 -1 1 1 1 1 -1 1 -1 1 -1 -1 1 1 -1 -1 1" scale="1 1 1"/>
</asset>
<worldbody>
<body>
<freejoint/>
<geom type="mesh" mesh="box" margin="0.3"/>
</body>
<body>
<freejoint/>
<geom size="0.05"/>
</body>
</worldbody>
</mujoco>"""))
class DataIOTest(parameterized.TestCase):
"""IO tests for mjx.Data."""
@@ -434,5 +453,20 @@ class DataIOTest(parameterized.TestCase):
self.assertEqual(d_2.contact.frame.shape, (1, 9))
np.testing.assert_allclose(d_2.contact.frame, d.contact.frame)
def test_make_matches_put(self):
"""Test that make_data produces a pytree that matches put_data."""
m = mujoco.MjModel.from_xml_string(_MULTIPLE_CONSTRAINTS)
d = mujoco.MjData(m)
mujoco.mj_step(m, d, 2)
dx = mjx.put_data(m, d)
step_fn = lambda d: d.replace(time=d.time + 1)
step_fn_jit = jax.jit(step_fn).lower(dx).compile()
# placing an MjData onto device should yield the same treedef mjx.Data as
# calling make_data. they should be interchangeable for jax functions:
step_fn_jit(mjx.make_data(m))
if __name__ == '__main__':
absltest.main()
mjx/mujoco/mjx/_src/math.py
+3 -15
View File
@@ -331,9 +331,9 @@ def closest_segment_point_and_dist(
return closest, dist
def closest_segment_to_segment_points_w_barycentric(
def closest_segment_to_segment_points(
a0: jax.Array, a1: jax.Array, b0: jax.Array, b1: jax.Array
) -> Tuple[jax.Array, jax.Array, jax.Array, jax.Array]:
) -> Tuple[jax.Array, jax.Array]:
"""Returns closest points between two line segments."""
# Gets the closest segment points by first finding the closest points
# between two lines. Points are then clipped to be on the line segments
@@ -364,10 +364,6 @@ def closest_segment_to_segment_points_w_barycentric(
t_a = jp.clip(orig_t_a, -half_len_a, half_len_a)
t_b = jp.clip(orig_t_b, -half_len_b, half_len_b)
# reparametrize t_a, t_b
t_a_01 = (orig_t_a + half_len_a) / jp.maximum(len_a, 1e-6)
t_b_01 = (orig_t_b + half_len_b) / jp.maximum(len_b, 1e-6)
best_a = a_mid + dir_a * t_a
best_b = b_mid + dir_b * t_b
@@ -381,12 +377,4 @@ def closest_segment_to_segment_points_w_barycentric(
best_a = jp.where(d1 < d2, new_a, best_a)
best_b = jp.where(d1 < d2, best_b, new_b)
return best_a, best_b, t_a_01, t_b_01
def closest_segment_to_segment_points(
a0: jax.Array, a1: jax.Array, b0: jax.Array, b1: jax.Array
) -> Tuple[jax.Array, jax.Array]:
"""Returns closest points between two line segments."""
a, b, *_ = closest_segment_to_segment_points_w_barycentric(a0, a1, b0, b1)
return a, b
return best_a, best_b
mjx/mujoco/mjx/_src/mesh.py
+10 -19
View File
@@ -49,8 +49,8 @@ _DERIVED_ARGS = [
'geom_convex_vert',
'geom_convex_edge_dir',
'geom_convex_facenormal',
'geom_convex_face_edge',
'geom_convex_face_edge_normal',
'geom_convex_edge',
'geom_convex_edge_face_normal',
]
DERIVED = {(Model, d) for d in _DERIVED_ARGS}
@@ -114,7 +114,7 @@ def _get_unique_edge_dir(vert: np.ndarray, face: np.ndarray) -> np.ndarray:
return edges[unique_edge_idx]
def _get_face_edge_normals(
def _get_edge_normals(
face: np.ndarray, face_norm: np.ndarray
) -> Tuple[np.ndarray, np.ndarray]:
"""Returns face edges and face edge normals."""
@@ -133,7 +133,7 @@ def _get_face_edge_normals(
continue
edge_map_list[tuple(face_edge_flat[i])].append(edge_face_norm[i])
edge_map = {}
edges, edge_face_normals = [], []
for k, v in edge_map_list.items():
v = np.array(v)
if len(v) > 2:
@@ -145,19 +145,10 @@ def _get_face_edge_normals(
# and face vertices were down sampled. In either case, we ignore these
# edges.
continue
edge_map[k] = v
edges.append(k)
edge_face_normals.append(v)
# for each face, list the edge normals
face_edge_normal = []
for face_idx in range(face_edge.shape[0]):
normals = []
for edge in face_edge[face_idx]:
k = tuple(edge)
normals.append(edge_map.get(k, np.zeros((2, 3))))
face_edge_normal.append(np.array(normals))
face_edge_normal = np.array(face_edge_normal)
return face_edge, face_edge_normal
return np.array(edges), np.array(edge_face_normals)
def _convex_hull_2d(points: np.ndarray, normal: np.ndarray) -> np.ndarray:
@@ -297,15 +288,15 @@ def _geom_mesh_kwargs(
vert = np.array(tm_convex.vertices)
face = _merge_coplanar(tm_convex, mesh_info)
facenormal = _get_face_norm(vert, face)
face_edge, face_edge_normal = _get_face_edge_normals(face, facenormal)
edge, edge_face_normal = _get_edge_normals(face, facenormal)
return {
'geom_convex_face': vert[face],
'geom_convex_face_vert_idx': face,
'geom_convex_vert': vert,
'geom_convex_edge_dir': _get_unique_edge_dir(vert, face),
'geom_convex_facenormal': facenormal,
'geom_convex_face_edge': face_edge,
'geom_convex_face_edge_normal': face_edge_normal,
'geom_convex_edge': edge,
'geom_convex_edge_face_normal': edge_face_normal,
}
mjx/mujoco/mjx/_src/mesh_test.py
+4 -13
View File
@@ -80,7 +80,7 @@ class GeomMeshKwargsTest(absltest.TestCase):
self.assertEqual(h['geom_convex_facenormal'].shape, (5, 3))
# face edges
edges = np.concatenate(h['geom_convex_face_edge'])
edges = h['geom_convex_edge']
edges = np.vectorize(map_.get)(edges)
mask = edges[:, 0] != edges[:, 1]
edges = edges[mask]
@@ -92,27 +92,18 @@ class GeomMeshKwargsTest(absltest.TestCase):
edges,
np.array([
[0, 2],
[0, 2],
[0, 3],
[0, 3],
[0, 4],
[0, 4],
[1, 2],
[1, 2],
[1, 3],
[1, 3],
[1, 4],
[1, 4],
[2, 4],
[2, 4],
[3, 4],
[3, 4],
]),
)
# face edge normals
edge_normal = h['geom_convex_face_edge_normal']
edge_normal = np.concatenate(edge_normal)
edge_normal = h['geom_convex_edge_face_normal']
edge_normal = edge_normal[mask]
edge_normal = np.take_along_axis(
edge_normal, sort_col_idx[..., None], axis=1
@@ -120,8 +111,8 @@ class GeomMeshKwargsTest(absltest.TestCase):
edge_normal = edge_normal[sort_row_idx]
edge_normal_02 = np.array([[0.4472136, -0.0, 0.89442719], [-1.0, 0.0, 0.0]])
np.testing.assert_array_almost_equal(
edge_normal[:2],
np.array([edge_normal_02, edge_normal_02]),
edge_normal[:1],
np.array([edge_normal_02]),
)
mjx/mujoco/mjx/_src/support.py
-1
View File
@@ -13,7 +13,6 @@
# limitations under the License.
# ==============================================================================
"""Engine support functions."""
from typing import Optional, Tuple, Union
import jax
mjx/mujoco/mjx/_src/test_util.py
+82
View File
@@ -14,14 +14,96 @@
# ==============================================================================
"""Utilities for testing."""
import os
import sys
import time
from typing import Dict, Tuple
from xml.etree import ElementTree as ET
from etils import epath
import jax
import mujoco
# pylint: disable=g-importing-member
from mujoco.mjx._src import forward
from mujoco.mjx._src import io
# pylint: enable=g-importing-member
import numpy as np
def _measure(fn, *args) -> Tuple[float, float]:
"""Reports jit time and op time for a function."""
beg = time.perf_counter()
compiled_fn = fn.lower(*args).compile()
end = time.perf_counter()
jit_time = end - beg
beg = time.perf_counter()
result = compiled_fn(*args)
jax.block_until_ready(result)
end = time.perf_counter()
run_time = end - beg
return jit_time, run_time
def benchmark(
m: mujoco.MjModel,
nstep: int = 1000,
batch_size: int = 1024,
unroll_steps: int = 1,
solver: str = 'cg',
iterations: int = 1,
ls_iterations: int = 4,
) -> Tuple[float, float, int]:
"""Benchmark a model."""
xla_flags = os.environ.get('XLA_FLAGS', '')
xla_flags += ' --xla_gpu_triton_gemm_any=True'
os.environ['XLA_FLAGS'] = xla_flags
m.opt.solver = {
'cg': mujoco.mjtSolver.mjSOL_CG,
'newton': mujoco.mjtSolver.mjSOL_NEWTON,
}[solver.lower()]
m.opt.iterations = iterations
m.opt.ls_iterations = ls_iterations
m = io.put_model(m)
@jax.pmap
def init(key):
key = jax.random.split(key, batch_size // jax.device_count())
@jax.vmap
def random_init(key):
d = io.make_data(m)
qvel = 0.01 * jax.random.normal(key, shape=(m.nv,))
d = d.replace(qvel=qvel)
return d
return random_init(key)
key = jax.random.split(jax.random.key(0), jax.device_count())
d = init(key)
jax.block_until_ready(d)
@jax.pmap
def unroll(d):
@jax.vmap
def step(d, _):
d = forward.step(m, d)
return d, None
d, _ = jax.lax.scan(step, d, None, length=nstep, unroll=unroll_steps)
return d
jit_time, run_time = _measure(unroll, d)
steps = nstep * batch_size
return jit_time, run_time, steps
_ACTUATOR_TYPES = ['motor', 'velocity', 'position', 'general', 'intvelocity']
_DYN_TYPES = ['none', 'integrator', 'filter', 'filterexact']
_DYN_PRMS = ['0.189', '2.1']
mjx/mujoco/mjx/_src/types.py
+2 -2
View File
@@ -543,8 +543,8 @@ class Model(PyTreeNode):
geom_convex_vert: List[Optional[jax.Array]]
geom_convex_edge_dir: List[Optional[jax.Array]]
geom_convex_facenormal: List[Optional[jax.Array]]
geom_convex_face_edge: List[Optional[jax.Array]]
geom_convex_face_edge_normal: List[Optional[jax.Array]]
geom_convex_edge: List[Optional[jax.Array]]
geom_convex_edge_face_normal: List[Optional[jax.Array]]
pair_solref: jax.Array
pair_solreffriction: jax.Array
pair_solimp: jax.Array
mjx/mujoco/mjx/test_data/humanoid/01_humanoids.xml
+150
View File
@@ -0,0 +1,150 @@
<mujoco model="01 Humanoids">
<option timestep="0.005" solver="Newton" iterations="1" ls_iterations="4">
<flag eulerdamp="disable"/>
</option>
<custom>
<numeric data="4" name="max_contact_points"/>
</custom>
<size memory="100M"/>
<asset>
<texture type="skybox" builtin="gradient" rgb1=".3 .5 .7" rgb2="0 0 0" width="512" height="512"/>
<texture name="body" type="cube" builtin="flat" mark="cross" width="128" height="128"
rgb1="0.8 0.6 0.4" rgb2="0.8 0.6 0.4" markrgb="1 1 1" random="0.01"/>
<material name="body" texture="body" texuniform="true" rgba="0.8 0.6 .4 1"/>
<texture name="grid" type="2d" builtin="checker" width="512" height="512" rgb1=".1 .2 .3" rgb2=".2 .3 .4"/>
<material name="grid" texture="grid" texrepeat="1 1" texuniform="true" reflectance=".2"/>
</asset>
<default>
<motor ctrlrange="-1 1" ctrllimited="true"/>
<default class="body">
<geom type="capsule" condim="3" friction=".7" solimp=".9 .99 .003" solref=".015 1" material="body" contype="0"/>
<joint type="hinge" damping=".2" stiffness="1" armature=".01" limited="true" solimplimit="0 .99 .01"/>
<default class="big_joint">
<joint damping="5" stiffness="10"/>
<default class="big_stiff_joint">
<joint stiffness="20"/>
</default>
</default>
</default>
</default>
<visual>
<map force="0.1" zfar="30"/>
<rgba haze="0.15 0.25 0.35 1"/>
<quality shadowsize="4096"/>
<global offwidth="800" offheight="800"/>
</visual>
<worldbody>
<geom size="10 10 .05" type="plane" material="grid" condim="3"/>
<light dir=".2 1 -.4" diffuse=".8 .8 .8" specular="0.3 0.3 0.3" pos="-2 -10 4" cutoff="35"/>
<light dir="-.2 1 -.4" diffuse=".8 .8 .8" specular="0.3 0.3 0.3" pos="2 -10 4" cutoff="35"/>
<body name="1a_torso" pos="-1 0 1.5" childclass="body">
<camera name="1a_back" pos="-3 0 1" xyaxes="0 -1 0 1 0 2" mode="trackcom"/>
<camera name="1a_side" pos="0 -3 1" xyaxes="1 0 0 0 1 2" mode="trackcom"/>
<freejoint name="1a_root"/>
<geom name="1a_torso" fromto="0 -.07 0 0 .07 0" size=".07"/>
<geom name="1a_upper_waist" fromto="-.01 -.06 -.12 -.01 .06 -.12" size=".06"/>
<body name="1a_head" pos="0 0 .19">
<geom name="1a_head" type="sphere" size=".09"/>
<camera name="1a_egocentric" pos=".09 0 0" xyaxes="0 -1 0 .1 0 1" fovy="80"/>
</body>
<body name="1a_lower_waist" pos="-.01 0 -.26">
<geom name="1a_lower_waist" fromto="0 -.06 0 0 .06 0" size=".06"/>
<joint name="1a_abdomen_z" pos="0 0 .065" axis="0 0 1" range="-45 45" class="big_stiff_joint"/>
<joint name="1a_abdomen_y" pos="0 0 .065" axis="0 1 0" range="-75 30" class="big_joint"/>
<body name="1a_pelvis" pos="0 0 -.165">
<joint name="1a_abdomen_x" pos="0 0 .1" axis="1 0 0" range="-35 35" class="big_joint"/>
<geom name="1a_butt" fromto="-.02 -.07 0 -.02 .07 0" size=".09"/>
<body name="1a_right_thigh" pos="0 -.1 -.04">
<joint name="1a_right_hip_x" axis="1 0 0" range="-25 5" class="big_joint"/>
<joint name="1a_right_hip_z" axis="0 0 1" range="-60 35" class="big_joint"/>
<joint name="1a_right_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
<geom name="1a_right_thigh" fromto="0 0 0 0 .01 -.34" size=".06"/>
<body name="1a_right_shin" pos="0 .01 -.403">
<joint name="1a_right_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
<geom name="1a_right_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
<body name="1a_right_foot" pos="0 0 -.39">
<joint name="1a_right_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
<joint name="1a_right_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
<geom name="1a_right_right_foot" fromto="-.07 -.02 0 .14 -.04 0" size=".027"/>
<geom name="1a_left_right_foot" fromto="-.07 0 0 .14 .02 0" size=".027"/>
</body>
</body>
</body>
<body name="1a_left_thigh" pos="0 .1 -.04">
<joint name="1a_left_hip_x" axis="-1 0 0" range="-25 5" class="big_joint"/>
<joint name="1a_left_hip_z" axis="0 0 -1" range="-60 35" class="big_joint"/>
<joint name="1a_left_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
<geom name="1a_left_thigh" fromto="0 0 0 0 -.01 -.34" size=".06"/>
<body name="1a_left_shin" pos="0 -.01 -.403">
<joint name="1a_left_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
<geom name="1a_left_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
<body name="1a_left_foot" pos="0 0 -.39">
<joint name="1a_left_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
<joint name="1a_left_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
<geom name="1a_left_left_foot" fromto="-.07 .02 0 .14 .04 0" size=".027"/>
<geom name="1a_right_left_foot" fromto="-.07 0 0 .14 -.02 0" size=".027"/>
</body>
</body>
</body>
</body>
</body>
<body name="1a_right_upper_arm" pos="0 -.17 .06">
<joint name="1a_right_shoulder1" axis="2 1 1" range="-85 60"/>
<joint name="1a_right_shoulder2" axis="0 -1 1" range="-85 60"/>
<geom name="1a_right_upper_arm" fromto="0 0 0 .16 -.16 -.16" size=".04 .16"/>
<body name="1a_right_lower_arm" pos=".18 -.18 -.18">
<joint name="1a_right_elbow" axis="0 -1 1" range="-90 50" stiffness="0"/>
<geom name="1a_right_lower_arm" fromto=".01 .01 .01 .17 .17 .17" size=".031"/>
<body name="1a_right_hand" pos=".18 .18 .18">
<geom name="1a_right_hand" type="sphere" size=".04" zaxis="1 1 1"/>
</body>
</body>
</body>
<body name="1a_left_upper_arm" pos="0 .17 .06">
<joint name="1a_left_shoulder1" axis="2 -1 1" range="-60 85"/>
<joint name="1a_left_shoulder2" axis="0 1 1" range="-60 85"/>
<geom name="1a_left_upper_arm" fromto="0 0 0 .16 .16 -.16" size=".04 .16"/>
<body name="1a_left_lower_arm" pos=".18 .18 -.18">
<joint name="1a_left_elbow" axis="0 -1 -1" range="-90 50" stiffness="0"/>
<geom name="1a_left_lower_arm" fromto=".01 -.01 .01 .17 -.17 .17" size=".031"/>
<body name="1a_left_hand" pos=".18 -.18 .18">
<geom name="1a_left_hand" type="sphere" size=".04" zaxis="1 -1 1"/>
</body>
</body>
</body>
</body>
</worldbody>
<actuator>
<motor gear="40" joint="1a_abdomen_y"/>
<motor gear="40" joint="1a_abdomen_z"/>
<motor gear="40" joint="1a_abdomen_x"/>
<motor gear="40" joint="1a_right_hip_x"/>
<motor gear="40" joint="1a_right_hip_z"/>
<motor gear="120" joint="1a_right_hip_y"/>
<motor gear="80" joint="1a_right_knee"/>
<motor gear="20" joint="1a_right_ankle_x"/>
<motor gear="20" joint="1a_right_ankle_y"/>
<motor gear="40" joint="1a_left_hip_x"/>
<motor gear="40" joint="1a_left_hip_z"/>
<motor gear="120" joint="1a_left_hip_y"/>
<motor gear="80" joint="1a_left_knee"/>
<motor gear="20" joint="1a_left_ankle_x"/>
<motor gear="20" joint="1a_left_ankle_y"/>
<motor gear="20" joint="1a_right_shoulder1"/>
<motor gear="20" joint="1a_right_shoulder2"/>
<motor gear="40" joint="1a_right_elbow"/>
<motor gear="20" joint="1a_left_shoulder1"/>
<motor gear="20" joint="1a_left_shoulder2"/>
<motor gear="40" joint="1a_left_elbow"/>
</actuator>
</mujoco>
mjx/mujoco/mjx/test_data/humanoid/02_humanoids.xml
+255
View File
@@ -0,0 +1,255 @@
<mujoco model="02 Humanoids">
<option timestep="0.005" solver="Newton" iterations="1" ls_iterations="4">
<flag eulerdamp="disable"/>
</option>
<custom>
<numeric data="8" name="max_contact_points"/>
<numeric data="8" name="max_geom_pairs"/>
</custom>
<size memory="100M"/>
<asset>
<texture type="skybox" builtin="gradient" rgb1=".3 .5 .7" rgb2="0 0 0" width="512" height="512"/>
<texture name="body" type="cube" builtin="flat" mark="cross" width="128" height="128"
rgb1="0.8 0.6 0.4" rgb2="0.8 0.6 0.4" markrgb="1 1 1" random="0.01"/>
<material name="body" texture="body" texuniform="true" rgba="0.8 0.6 .4 1"/>
<texture name="grid" type="2d" builtin="checker" width="512" height="512" rgb1=".1 .2 .3" rgb2=".2 .3 .4"/>
<material name="grid" texture="grid" texrepeat="1 1" texuniform="true" reflectance=".2"/>
</asset>
<default>
<motor ctrlrange="-1 1" ctrllimited="true"/>
<joint type="hinge" damping=".2" stiffness="1" armature=".01" limited="true" solimplimit="0 .99 .01"/>
<default class="big_joint">
<joint damping="5" stiffness="10"/>
<default class="big_stiff_joint">
<joint stiffness="20"/>
</default>
</default>
<default class="body1">
<geom type="capsule" condim="3" friction=".7" solimp=".9 .99 .003" solref=".015 1" material="body" contype="2" conaffinity="5"/>
</default>
<default class="body2">
<geom type="capsule" condim="3" friction=".7" solimp=".9 .99 .003" solref=".015 1" material="body" contype="4" conaffinity="3"/>
</default>
</default>
<visual>
<map force="0.1" zfar="30"/>
<rgba haze="0.15 0.25 0.35 1"/>
<quality shadowsize="4096"/>
<global offwidth="800" offheight="800"/>
</visual>
<worldbody>
<geom size="10 10 .05" type="plane" material="grid" condim="3"/>
<light dir=".2 1 -.4" diffuse=".8 .8 .8" specular="0.3 0.3 0.3" pos="-2 -10 4" cutoff="35"/>
<light dir="-.2 1 -.4" diffuse=".8 .8 .8" specular="0.3 0.3 0.3" pos="2 -10 4" cutoff="35"/>
<body name="1a_torso" pos="-1 0 1.5" childclass="body1">
<camera name="1a_back" pos="-3 0 1" xyaxes="0 -1 0 1 0 2" mode="trackcom"/>
<camera name="1a_side" pos="0 -3 1" xyaxes="1 0 0 0 1 2" mode="trackcom"/>
<freejoint name="1a_root"/>
<geom name="1a_torso" fromto="0 -.07 0 0 .07 0" size=".07"/>
<geom name="1a_upper_waist" fromto="-.01 -.06 -.12 -.01 .06 -.12" size=".06"/>
<body name="1a_head" pos="0 0 .19">
<geom name="1a_head" type="sphere" size=".09"/>
<camera name="1a_egocentric" pos=".09 0 0" xyaxes="0 -1 0 .1 0 1" fovy="80"/>
</body>
<body name="1a_lower_waist" pos="-.01 0 -.26">
<geom name="1a_lower_waist" fromto="0 -.06 0 0 .06 0" size=".06"/>
<joint name="1a_abdomen_z" pos="0 0 .065" axis="0 0 1" range="-45 45" class="big_stiff_joint"/>
<joint name="1a_abdomen_y" pos="0 0 .065" axis="0 1 0" range="-75 30" class="big_joint"/>
<body name="1a_pelvis" pos="0 0 -.165">
<joint name="1a_abdomen_x" pos="0 0 .1" axis="1 0 0" range="-35 35" class="big_joint"/>
<geom name="1a_butt" fromto="-.02 -.07 0 -.02 .07 0" size=".09"/>
<body name="1a_right_thigh" pos="0 -.1 -.04">
<joint name="1a_right_hip_x" axis="1 0 0" range="-25 5" class="big_joint"/>
<joint name="1a_right_hip_z" axis="0 0 1" range="-60 35" class="big_joint"/>
<joint name="1a_right_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
<geom name="1a_right_thigh" fromto="0 0 0 0 .01 -.34" size=".06"/>
<body name="1a_right_shin" pos="0 .01 -.403">
<joint name="1a_right_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
<geom name="1a_right_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
<body name="1a_right_foot" pos="0 0 -.39">
<joint name="1a_right_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
<joint name="1a_right_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
<geom name="1a_right_right_foot" fromto="-.07 -.02 0 .14 -.04 0" size=".027"/>
<geom name="1a_left_right_foot" fromto="-.07 0 0 .14 .02 0" size=".027"/>
</body>
</body>
</body>
<body name="1a_left_thigh" pos="0 .1 -.04">
<joint name="1a_left_hip_x" axis="-1 0 0" range="-25 5" class="big_joint"/>
<joint name="1a_left_hip_z" axis="0 0 -1" range="-60 35" class="big_joint"/>
<joint name="1a_left_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
<geom name="1a_left_thigh" fromto="0 0 0 0 -.01 -.34" size=".06"/>
<body name="1a_left_shin" pos="0 -.01 -.403">
<joint name="1a_left_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
<geom name="1a_left_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
<body name="1a_left_foot" pos="0 0 -.39">
<joint name="1a_left_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
<joint name="1a_left_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
<geom name="1a_left_left_foot" fromto="-.07 .02 0 .14 .04 0" size=".027"/>
<geom name="1a_right_left_foot" fromto="-.07 0 0 .14 -.02 0" size=".027"/>
</body>
</body>
</body>
</body>
</body>
<body name="1a_right_upper_arm" pos="0 -.17 .06">
<joint name="1a_right_shoulder1" axis="2 1 1" range="-85 60"/>
<joint name="1a_right_shoulder2" axis="0 -1 1" range="-85 60"/>
<geom name="1a_right_upper_arm" fromto="0 0 0 .16 -.16 -.16" size=".04 .16"/>
<body name="1a_right_lower_arm" pos=".18 -.18 -.18">
<joint name="1a_right_elbow" axis="0 -1 1" range="-90 50" stiffness="0"/>
<geom name="1a_right_lower_arm" fromto=".01 .01 .01 .17 .17 .17" size=".031"/>
<body name="1a_right_hand" pos=".18 .18 .18">
<geom name="1a_right_hand" type="sphere" size=".04" zaxis="1 1 1"/>
</body>
</body>
</body>
<body name="1a_left_upper_arm" pos="0 .17 .06">
<joint name="1a_left_shoulder1" axis="2 -1 1" range="-60 85"/>
<joint name="1a_left_shoulder2" axis="0 1 1" range="-60 85"/>
<geom name="1a_left_upper_arm" fromto="0 0 0 .16 .16 -.16" size=".04 .16"/>
<body name="1a_left_lower_arm" pos=".18 .18 -.18">
<joint name="1a_left_elbow" axis="0 -1 -1" range="-90 50" stiffness="0"/>
<geom name="1a_left_lower_arm" fromto=".01 -.01 .01 .17 -.17 .17" size=".031"/>
<body name="1a_left_hand" pos=".18 -.18 .18">
<geom name="1a_left_hand" type="sphere" size=".04" zaxis="1 -1 1"/>
</body>
</body>
</body>
</body>
<body name="2a_torso" pos="-1 1 1.5" childclass="body2">
<camera name="2a_back" pos="-3 0 1" xyaxes="0 -1 0 1 0 2" mode="trackcom"/>
<camera name="2a_side" pos="0 -3 1" xyaxes="1 0 0 0 1 2" mode="trackcom"/>
<freejoint name="2a_root"/>
<geom name="2a_torso" fromto="0 -.07 0 0 .07 0" size=".07"/>
<geom name="2a_upper_waist" fromto="-.01 -.06 -.12 -.01 .06 -.12" size=".06"/>
<body name="2a_head" pos="0 0 .19">
<geom name="2a_head" type="sphere" size=".09"/>
<camera name="2a_egocentric" pos=".09 0 0" xyaxes="0 -1 0 .1 0 1" fovy="80"/>
</body>
<body name="2a_lower_waist" pos="-.01 0 -.26">
<geom name="2a_lower_waist" fromto="0 -.06 0 0 .06 0" size=".06"/>
<joint name="2a_abdomen_z" pos="0 0 .065" axis="0 0 1" range="-45 45" class="big_stiff_joint"/>
<joint name="2a_abdomen_y" pos="0 0 .065" axis="0 1 0" range="-75 30" class="big_joint"/>
<body name="2a_pelvis" pos="0 0 -.165">
<joint name="2a_abdomen_x" pos="0 0 .1" axis="1 0 0" range="-35 35" class="big_joint"/>
<geom name="2a_butt" fromto="-.02 -.07 0 -.02 .07 0" size=".09"/>
<body name="2a_right_thigh" pos="0 -.1 -.04">
<joint name="2a_right_hip_x" axis="1 0 0" range="-25 5" class="big_joint"/>
<joint name="2a_right_hip_z" axis="0 0 1" range="-60 35" class="big_joint"/>
<joint name="2a_right_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
<geom name="2a_right_thigh" fromto="0 0 0 0 .01 -.34" size=".06"/>
<body name="2a_right_shin" pos="0 .01 -.403">
<joint name="2a_right_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
<geom name="2a_right_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
<body name="2a_right_foot" pos="0 0 -.39">
<joint name="2a_right_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
<joint name="2a_right_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
<geom name="2a_right_right_foot" fromto="-.07 -.02 0 .14 -.04 0" size=".027"/>
<geom name="2a_left_right_foot" fromto="-.07 0 0 .14 .02 0" size=".027"/>
</body>
</body>
</body>
<body name="2a_left_thigh" pos="0 .1 -.04">
<joint name="2a_left_hip_x" axis="-1 0 0" range="-25 5" class="big_joint"/>
<joint name="2a_left_hip_z" axis="0 0 -1" range="-60 35" class="big_joint"/>
<joint name="2a_left_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
<geom name="2a_left_thigh" fromto="0 0 0 0 -.01 -.34" size=".06"/>
<body name="2a_left_shin" pos="0 -.01 -.403">
<joint name="2a_left_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
<geom name="2a_left_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
<body name="2a_left_foot" pos="0 0 -.39">
<joint name="2a_left_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
<joint name="2a_left_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
<geom name="2a_left_left_foot" fromto="-.07 .02 0 .14 .04 0" size=".027"/>
<geom name="2a_right_left_foot" fromto="-.07 0 0 .14 -.02 0" size=".027"/>
</body>
</body>
</body>
</body>
</body>
<body name="2a_right_upper_arm" pos="0 -.17 .06">
<joint name="2a_right_shoulder1" axis="2 1 1" range="-85 60"/>
<joint name="2a_right_shoulder2" axis="0 -1 1" range="-85 60"/>
<geom name="2a_right_upper_arm" fromto="0 0 0 .16 -.16 -.16" size=".04 .16"/>
<body name="2a_right_lower_arm" pos=".18 -.18 -.18">
<joint name="2a_right_elbow" axis="0 -1 1" range="-90 50" stiffness="0"/>
<geom name="2a_right_lower_arm" fromto=".01 .01 .01 .17 .17 .17" size=".031"/>
<body name="2a_right_hand" pos=".18 .18 .18">
<geom name="2a_right_hand" type="sphere" size=".04" zaxis="1 1 1"/>
</body>
</body>
</body>
<body name="2a_left_upper_arm" pos="0 .17 .06">
<joint name="2a_left_shoulder1" axis="2 -1 1" range="-60 85"/>
<joint name="2a_left_shoulder2" axis="0 1 1" range="-60 85"/>
<geom name="2a_left_upper_arm" fromto="0 0 0 .16 .16 -.16" size=".04 .16"/>
<body name="2a_left_lower_arm" pos=".18 .18 -.18">
<joint name="2a_left_elbow" axis="0 -1 -1" range="-90 50" stiffness="0"/>
<geom name="2a_left_lower_arm" fromto=".01 -.01 .01 .17 -.17 .17" size=".031"/>
<body name="2a_left_hand" pos=".18 -.18 .18">
<geom name="2a_left_hand" type="sphere" size=".04" zaxis="1 -1 1"/>
</body>
</body>
</body>
</body>
</worldbody>
<actuator>
<motor gear="40" joint="1a_abdomen_y"/>
<motor gear="40" joint="1a_abdomen_z"/>
<motor gear="40" joint="1a_abdomen_x"/>
<motor gear="40" joint="1a_right_hip_x"/>
<motor gear="40" joint="1a_right_hip_z"/>
<motor gear="120" joint="1a_right_hip_y"/>
<motor gear="80" joint="1a_right_knee"/>
<motor gear="20" joint="1a_right_ankle_x"/>
<motor gear="20" joint="1a_right_ankle_y"/>
<motor gear="40" joint="1a_left_hip_x"/>
<motor gear="40" joint="1a_left_hip_z"/>
<motor gear="120" joint="1a_left_hip_y"/>
<motor gear="80" joint="1a_left_knee"/>
<motor gear="20" joint="1a_left_ankle_x"/>
<motor gear="20" joint="1a_left_ankle_y"/>
<motor gear="20" joint="1a_right_shoulder1"/>
<motor gear="20" joint="1a_right_shoulder2"/>
<motor gear="40" joint="1a_right_elbow"/>
<motor gear="20" joint="1a_left_shoulder1"/>
<motor gear="20" joint="1a_left_shoulder2"/>
<motor gear="40" joint="1a_left_elbow"/>
</actuator>
<actuator>
<motor gear="40" joint="2a_abdomen_y"/>
<motor gear="40" joint="2a_abdomen_z"/>
<motor gear="40" joint="2a_abdomen_x"/>
<motor gear="40" joint="2a_right_hip_x"/>
<motor gear="40" joint="2a_right_hip_z"/>
<motor gear="120" joint="2a_right_hip_y"/>
<motor gear="80" joint="2a_right_knee"/>
<motor gear="20" joint="2a_right_ankle_x"/>
<motor gear="20" joint="2a_right_ankle_y"/>
<motor gear="40" joint="2a_left_hip_x"/>
<motor gear="40" joint="2a_left_hip_z"/>
<motor gear="120" joint="2a_left_hip_y"/>
<motor gear="80" joint="2a_left_knee"/>
<motor gear="20" joint="2a_left_ankle_x"/>
<motor gear="20" joint="2a_left_ankle_y"/>
<motor gear="20" joint="2a_right_shoulder1"/>
<motor gear="20" joint="2a_right_shoulder2"/>
<motor gear="40" joint="2a_right_elbow"/>
<motor gear="20" joint="2a_left_shoulder1"/>
<motor gear="20" joint="2a_left_shoulder2"/>
<motor gear="40" joint="2a_left_elbow"/>
</actuator>
</mujoco>
mjx/mujoco/mjx/test_data/humanoid/03_humanoids.xml
+359
View File
@@ -0,0 +1,359 @@
<mujoco model="03 Humanoids">
<option timestep="0.005" solver="Newton" iterations="1" ls_iterations="4">
<flag eulerdamp="disable"/>
</option>
<custom>
<numeric data="12" name="max_contact_points"/>
<numeric data="12" name="max_geom_pairs"/>
</custom>
<size memory="100M"/>
<asset>
<texture type="skybox" builtin="gradient" rgb1=".3 .5 .7" rgb2="0 0 0" width="512" height="512"/>
<texture name="body" type="cube" builtin="flat" mark="cross" width="128" height="128"
rgb1="0.8 0.6 0.4" rgb2="0.8 0.6 0.4" markrgb="1 1 1" random="0.01"/>
<material name="body" texture="body" texuniform="true" rgba="0.8 0.6 .4 1"/>
<texture name="grid" type="2d" builtin="checker" width="512" height="512" rgb1=".1 .2 .3" rgb2=".2 .3 .4"/>
<material name="grid" texture="grid" texrepeat="1 1" texuniform="true" reflectance=".2"/>
</asset>
<default>
<motor ctrlrange="-1 1" ctrllimited="true"/>
<joint type="hinge" damping=".2" stiffness="1" armature=".01" limited="true" solimplimit="0 .99 .01"/>
<default class="big_joint">
<joint damping="5" stiffness="10"/>
<default class="big_stiff_joint">
<joint stiffness="20"/>
</default>
</default>
<default class="body1">
<geom type="capsule" condim="3" friction=".7" solimp=".9 .99 .003" solref=".015 1" material="body" contype="2" conaffinity="13"/>
</default>
<default class="body2">
<geom type="capsule" condim="3" friction=".7" solimp=".9 .99 .003" solref=".015 1" material="body" contype="4" conaffinity="11"/>
</default>
<default class="body3">
<geom type="capsule" condim="3" friction=".7" solimp=".9 .99 .003" solref=".015 1" material="body" contype="8" conaffinity="7"/>
</default>
</default>
<visual>
<map force="0.1" zfar="30"/>
<rgba haze="0.15 0.25 0.35 1"/>
<quality shadowsize="4096"/>
<global offwidth="800" offheight="800"/>
</visual>
<worldbody>
<geom size="10 10 .05" type="plane" material="grid" condim="3"/>
<light dir=".2 1 -.4" diffuse=".8 .8 .8" specular="0.3 0.3 0.3" pos="-2 -10 4" cutoff="35"/>
<light dir="-.2 1 -.4" diffuse=".8 .8 .8" specular="0.3 0.3 0.3" pos="2 -10 4" cutoff="35"/>
<body name="1a_torso" pos="-1 0 1.5" childclass="body1">
<camera name="1a_back" pos="-3 0 1" xyaxes="0 -1 0 1 0 2" mode="trackcom"/>
<camera name="1a_side" pos="0 -3 1" xyaxes="1 0 0 0 1 2" mode="trackcom"/>
<freejoint name="1a_root"/>
<geom name="1a_torso" fromto="0 -.07 0 0 .07 0" size=".07"/>
<geom name="1a_upper_waist" fromto="-.01 -.06 -.12 -.01 .06 -.12" size=".06"/>
<body name="1a_head" pos="0 0 .19">
<geom name="1a_head" type="sphere" size=".09"/>
<camera name="1a_egocentric" pos=".09 0 0" xyaxes="0 -1 0 .1 0 1" fovy="80"/>
</body>
<body name="1a_lower_waist" pos="-.01 0 -.26">
<geom name="1a_lower_waist" fromto="0 -.06 0 0 .06 0" size=".06"/>
<joint name="1a_abdomen_z" pos="0 0 .065" axis="0 0 1" range="-45 45" class="big_stiff_joint"/>
<joint name="1a_abdomen_y" pos="0 0 .065" axis="0 1 0" range="-75 30" class="big_joint"/>
<body name="1a_pelvis" pos="0 0 -.165">
<joint name="1a_abdomen_x" pos="0 0 .1" axis="1 0 0" range="-35 35" class="big_joint"/>
<geom name="1a_butt" fromto="-.02 -.07 0 -.02 .07 0" size=".09"/>
<body name="1a_right_thigh" pos="0 -.1 -.04">
<joint name="1a_right_hip_x" axis="1 0 0" range="-25 5" class="big_joint"/>
<joint name="1a_right_hip_z" axis="0 0 1" range="-60 35" class="big_joint"/>
<joint name="1a_right_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
<geom name="1a_right_thigh" fromto="0 0 0 0 .01 -.34" size=".06"/>
<body name="1a_right_shin" pos="0 .01 -.403">
<joint name="1a_right_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
<geom name="1a_right_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
<body name="1a_right_foot" pos="0 0 -.39">
<joint name="1a_right_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
<joint name="1a_right_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
<geom name="1a_right_right_foot" fromto="-.07 -.02 0 .14 -.04 0" size=".027"/>
<geom name="1a_left_right_foot" fromto="-.07 0 0 .14 .02 0" size=".027"/>
</body>
</body>
</body>
<body name="1a_left_thigh" pos="0 .1 -.04">
<joint name="1a_left_hip_x" axis="-1 0 0" range="-25 5" class="big_joint"/>
<joint name="1a_left_hip_z" axis="0 0 -1" range="-60 35" class="big_joint"/>
<joint name="1a_left_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
<geom name="1a_left_thigh" fromto="0 0 0 0 -.01 -.34" size=".06"/>
<body name="1a_left_shin" pos="0 -.01 -.403">
<joint name="1a_left_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
<geom name="1a_left_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
<body name="1a_left_foot" pos="0 0 -.39">
<joint name="1a_left_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
<joint name="1a_left_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
<geom name="1a_left_left_foot" fromto="-.07 .02 0 .14 .04 0" size=".027"/>
<geom name="1a_right_left_foot" fromto="-.07 0 0 .14 -.02 0" size=".027"/>
</body>
</body>
</body>
</body>
</body>
<body name="1a_right_upper_arm" pos="0 -.17 .06">
<joint name="1a_right_shoulder1" axis="2 1 1" range="-85 60"/>
<joint name="1a_right_shoulder2" axis="0 -1 1" range="-85 60"/>
<geom name="1a_right_upper_arm" fromto="0 0 0 .16 -.16 -.16" size=".04 .16"/>
<body name="1a_right_lower_arm" pos=".18 -.18 -.18">
<joint name="1a_right_elbow" axis="0 -1 1" range="-90 50" stiffness="0"/>
<geom name="1a_right_lower_arm" fromto=".01 .01 .01 .17 .17 .17" size=".031"/>
<body name="1a_right_hand" pos=".18 .18 .18">
<geom name="1a_right_hand" type="sphere" size=".04" zaxis="1 1 1"/>
</body>
</body>
</body>
<body name="1a_left_upper_arm" pos="0 .17 .06">
<joint name="1a_left_shoulder1" axis="2 -1 1" range="-60 85"/>
<joint name="1a_left_shoulder2" axis="0 1 1" range="-60 85"/>
<geom name="1a_left_upper_arm" fromto="0 0 0 .16 .16 -.16" size=".04 .16"/>
<body name="1a_left_lower_arm" pos=".18 .18 -.18">
<joint name="1a_left_elbow" axis="0 -1 -1" range="-90 50" stiffness="0"/>
<geom name="1a_left_lower_arm" fromto=".01 -.01 .01 .17 -.17 .17" size=".031"/>
<body name="1a_left_hand" pos=".18 -.18 .18">
<geom name="1a_left_hand" type="sphere" size=".04" zaxis="1 -1 1"/>
</body>
</body>
</body>
</body>
<body name="2a_torso" pos="-1 1 1.5" childclass="body2">
<camera name="2a_back" pos="-3 0 1" xyaxes="0 -1 0 1 0 2" mode="trackcom"/>
<camera name="2a_side" pos="0 -3 1" xyaxes="1 0 0 0 1 2" mode="trackcom"/>
<freejoint name="2a_root"/>
<geom name="2a_torso" fromto="0 -.07 0 0 .07 0" size=".07"/>
<geom name="2a_upper_waist" fromto="-.01 -.06 -.12 -.01 .06 -.12" size=".06"/>
<body name="2a_head" pos="0 0 .19">
<geom name="2a_head" type="sphere" size=".09"/>
<camera name="2a_egocentric" pos=".09 0 0" xyaxes="0 -1 0 .1 0 1" fovy="80"/>
</body>
<body name="2a_lower_waist" pos="-.01 0 -.26">
<geom name="2a_lower_waist" fromto="0 -.06 0 0 .06 0" size=".06"/>
<joint name="2a_abdomen_z" pos="0 0 .065" axis="0 0 1" range="-45 45" class="big_stiff_joint"/>
<joint name="2a_abdomen_y" pos="0 0 .065" axis="0 1 0" range="-75 30" class="big_joint"/>
<body name="2a_pelvis" pos="0 0 -.165">
<joint name="2a_abdomen_x" pos="0 0 .1" axis="1 0 0" range="-35 35" class="big_joint"/>
<geom name="2a_butt" fromto="-.02 -.07 0 -.02 .07 0" size=".09"/>
<body name="2a_right_thigh" pos="0 -.1 -.04">
<joint name="2a_right_hip_x" axis="1 0 0" range="-25 5" class="big_joint"/>
<joint name="2a_right_hip_z" axis="0 0 1" range="-60 35" class="big_joint"/>
<joint name="2a_right_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
<geom name="2a_right_thigh" fromto="0 0 0 0 .01 -.34" size=".06"/>
<body name="2a_right_shin" pos="0 .01 -.403">
<joint name="2a_right_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
<geom name="2a_right_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
<body name="2a_right_foot" pos="0 0 -.39">
<joint name="2a_right_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
<joint name="2a_right_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
<geom name="2a_right_right_foot" fromto="-.07 -.02 0 .14 -.04 0" size=".027"/>
<geom name="2a_left_right_foot" fromto="-.07 0 0 .14 .02 0" size=".027"/>
</body>
</body>
</body>
<body name="2a_left_thigh" pos="0 .1 -.04">
<joint name="2a_left_hip_x" axis="-1 0 0" range="-25 5" class="big_joint"/>
<joint name="2a_left_hip_z" axis="0 0 -1" range="-60 35" class="big_joint"/>
<joint name="2a_left_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
<geom name="2a_left_thigh" fromto="0 0 0 0 -.01 -.34" size=".06"/>
<body name="2a_left_shin" pos="0 -.01 -.403">
<joint name="2a_left_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
<geom name="2a_left_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
<body name="2a_left_foot" pos="0 0 -.39">
<joint name="2a_left_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
<joint name="2a_left_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
<geom name="2a_left_left_foot" fromto="-.07 .02 0 .14 .04 0" size=".027"/>
<geom name="2a_right_left_foot" fromto="-.07 0 0 .14 -.02 0" size=".027"/>
</body>
</body>
</body>
</body>
</body>
<body name="2a_right_upper_arm" pos="0 -.17 .06">
<joint name="2a_right_shoulder1" axis="2 1 1" range="-85 60"/>
<joint name="2a_right_shoulder2" axis="0 -1 1" range="-85 60"/>
<geom name="2a_right_upper_arm" fromto="0 0 0 .16 -.16 -.16" size=".04 .16"/>
<body name="2a_right_lower_arm" pos=".18 -.18 -.18">
<joint name="2a_right_elbow" axis="0 -1 1" range="-90 50" stiffness="0"/>
<geom name="2a_right_lower_arm" fromto=".01 .01 .01 .17 .17 .17" size=".031"/>
<body name="2a_right_hand" pos=".18 .18 .18">
<geom name="2a_right_hand" type="sphere" size=".04" zaxis="1 1 1"/>
</body>
</body>
</body>
<body name="2a_left_upper_arm" pos="0 .17 .06">
<joint name="2a_left_shoulder1" axis="2 -1 1" range="-60 85"/>
<joint name="2a_left_shoulder2" axis="0 1 1" range="-60 85"/>
<geom name="2a_left_upper_arm" fromto="0 0 0 .16 .16 -.16" size=".04 .16"/>
<body name="2a_left_lower_arm" pos=".18 .18 -.18">
<joint name="2a_left_elbow" axis="0 -1 -1" range="-90 50" stiffness="0"/>
<geom name="2a_left_lower_arm" fromto=".01 -.01 .01 .17 -.17 .17" size=".031"/>
<body name="2a_left_hand" pos=".18 -.18 .18">
<geom name="2a_left_hand" type="sphere" size=".04" zaxis="1 -1 1"/>
</body>
</body>
</body>
</body>
<body name="3a_torso" pos="-1 -1 1.5" childclass="body3">
<camera name="3a_back" pos="-3 0 1" xyaxes="0 -1 0 1 0 2" mode="trackcom"/>
<camera name="3a_side" pos="0 -3 1" xyaxes="1 0 0 0 1 2" mode="trackcom"/>
<freejoint name="3a_root"/>
<geom name="3a_torso" fromto="0 -.07 0 0 .07 0" size=".07"/>
<geom name="3a_upper_waist" fromto="-.01 -.06 -.12 -.01 .06 -.12" size=".06"/>
<body name="3a_head" pos="0 0 .19">
<geom name="3a_head" type="sphere" size=".09"/>
<camera name="3a_egocentric" pos=".09 0 0" xyaxes="0 -1 0 .1 0 1" fovy="80"/>
</body>
<body name="3a_lower_waist" pos="-.01 0 -.26">
<geom name="3a_lower_waist" fromto="0 -.06 0 0 .06 0" size=".06"/>
<joint name="3a_abdomen_z" pos="0 0 .065" axis="0 0 1" range="-45 45" class="big_stiff_joint"/>
<joint name="3a_abdomen_y" pos="0 0 .065" axis="0 1 0" range="-75 30" class="big_joint"/>
<body name="3a_pelvis" pos="0 0 -.165">
<joint name="3a_abdomen_x" pos="0 0 .1" axis="1 0 0" range="-35 35" class="big_joint"/>
<geom name="3a_butt" fromto="-.02 -.07 0 -.02 .07 0" size=".09"/>
<body name="3a_right_thigh" pos="0 -.1 -.04">
<joint name="3a_right_hip_x" axis="1 0 0" range="-25 5" class="big_joint"/>
<joint name="3a_right_hip_z" axis="0 0 1" range="-60 35" class="big_joint"/>
<joint name="3a_right_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
<geom name="3a_right_thigh" fromto="0 0 0 0 .01 -.34" size=".06"/>
<body name="3a_right_shin" pos="0 .01 -.403">
<joint name="3a_right_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
<geom name="3a_right_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
<body name="3a_right_foot" pos="0 0 -.39">
<joint name="3a_right_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
<joint name="3a_right_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
<geom name="3a_right_right_foot" fromto="-.07 -.02 0 .14 -.04 0" size=".027"/>
<geom name="3a_left_right_foot" fromto="-.07 0 0 .14 .02 0" size=".027"/>
</body>
</body>
</body>
<body name="3a_left_thigh" pos="0 .1 -.04">
<joint name="3a_left_hip_x" axis="-1 0 0" range="-25 5" class="big_joint"/>
<joint name="3a_left_hip_z" axis="0 0 -1" range="-60 35" class="big_joint"/>
<joint name="3a_left_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
<geom name="3a_left_thigh" fromto="0 0 0 0 -.01 -.34" size=".06"/>
<body name="3a_left_shin" pos="0 -.01 -.403">
<joint name="3a_left_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
<geom name="3a_left_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
<body name="3a_left_foot" pos="0 0 -.39">
<joint name="3a_left_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
<joint name="3a_left_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
<geom name="3a_left_left_foot" fromto="-.07 .02 0 .14 .04 0" size=".027"/>
<geom name="3a_right_left_foot" fromto="-.07 0 0 .14 -.02 0" size=".027"/>
</body>
</body>
</body>
</body>
</body>
<body name="3a_right_upper_arm" pos="0 -.17 .06">
<joint name="3a_right_shoulder1" axis="2 1 1" range="-85 60"/>
<joint name="3a_right_shoulder2" axis="0 -1 1" range="-85 60"/>
<geom name="3a_right_upper_arm" fromto="0 0 0 .16 -.16 -.16" size=".04 .16"/>
<body name="3a_right_lower_arm" pos=".18 -.18 -.18">
<joint name="3a_right_elbow" axis="0 -1 1" range="-90 50" stiffness="0"/>
<geom name="3a_right_lower_arm" fromto=".01 .01 .01 .17 .17 .17" size=".031"/>
<body name="3a_right_hand" pos=".18 .18 .18">
<geom name="3a_right_hand" type="sphere" size=".04" zaxis="1 1 1"/>
</body>
</body>
</body>
<body name="3a_left_upper_arm" pos="0 .17 .06">
<joint name="3a_left_shoulder1" axis="2 -1 1" range="-60 85"/>
<joint name="3a_left_shoulder2" axis="0 1 1" range="-60 85"/>
<geom name="3a_left_upper_arm" fromto="0 0 0 .16 .16 -.16" size=".04 .16"/>
<body name="3a_left_lower_arm" pos=".18 .18 -.18">
<joint name="3a_left_elbow" axis="0 -1 -1" range="-90 50" stiffness="0"/>
<geom name="3a_left_lower_arm" fromto=".01 -.01 .01 .17 -.17 .17" size=".031"/>
<body name="3a_left_hand" pos=".18 -.18 .18">
<geom name="3a_left_hand" type="sphere" size=".04" zaxis="1 -1 1"/>
</body>
</body>
</body>
</body>
</worldbody>
<actuator>
<motor gear="40" joint="1a_abdomen_y"/>
<motor gear="40" joint="1a_abdomen_z"/>
<motor gear="40" joint="1a_abdomen_x"/>
<motor gear="40" joint="1a_right_hip_x"/>
<motor gear="40" joint="1a_right_hip_z"/>
<motor gear="120" joint="1a_right_hip_y"/>
<motor gear="80" joint="1a_right_knee"/>
<motor gear="20" joint="1a_right_ankle_x"/>
<motor gear="20" joint="1a_right_ankle_y"/>
<motor gear="40" joint="1a_left_hip_x"/>
<motor gear="40" joint="1a_left_hip_z"/>
<motor gear="120" joint="1a_left_hip_y"/>
<motor gear="80" joint="1a_left_knee"/>
<motor gear="20" joint="1a_left_ankle_x"/>
<motor gear="20" joint="1a_left_ankle_y"/>
<motor gear="20" joint="1a_right_shoulder1"/>
<motor gear="20" joint="1a_right_shoulder2"/>
<motor gear="40" joint="1a_right_elbow"/>
<motor gear="20" joint="1a_left_shoulder1"/>
<motor gear="20" joint="1a_left_shoulder2"/>
<motor gear="40" joint="1a_left_elbow"/>
</actuator>
<actuator>
<motor gear="40" joint="2a_abdomen_y"/>
<motor gear="40" joint="2a_abdomen_z"/>
<motor gear="40" joint="2a_abdomen_x"/>
<motor gear="40" joint="2a_right_hip_x"/>
<motor gear="40" joint="2a_right_hip_z"/>
<motor gear="120" joint="2a_right_hip_y"/>
<motor gear="80" joint="2a_right_knee"/>
<motor gear="20" joint="2a_right_ankle_x"/>
<motor gear="20" joint="2a_right_ankle_y"/>
<motor gear="40" joint="2a_left_hip_x"/>
<motor gear="40" joint="2a_left_hip_z"/>
<motor gear="120" joint="2a_left_hip_y"/>
<motor gear="80" joint="2a_left_knee"/>
<motor gear="20" joint="2a_left_ankle_x"/>
<motor gear="20" joint="2a_left_ankle_y"/>
<motor gear="20" joint="2a_right_shoulder1"/>
<motor gear="20" joint="2a_right_shoulder2"/>
<motor gear="40" joint="2a_right_elbow"/>
<motor gear="20" joint="2a_left_shoulder1"/>
<motor gear="20" joint="2a_left_shoulder2"/>
<motor gear="40" joint="2a_left_elbow"/>
</actuator>
<actuator>
<motor gear="40" joint="3a_abdomen_y"/>
<motor gear="40" joint="3a_abdomen_z"/>
<motor gear="40" joint="3a_abdomen_x"/>
<motor gear="40" joint="3a_right_hip_x"/>
<motor gear="40" joint="3a_right_hip_z"/>
<motor gear="120" joint="3a_right_hip_y"/>
<motor gear="80" joint="3a_right_knee"/>
<motor gear="20" joint="3a_right_ankle_x"/>
<motor gear="20" joint="3a_right_ankle_y"/>
<motor gear="40" joint="3a_left_hip_x"/>
<motor gear="40" joint="3a_left_hip_z"/>
<motor gear="120" joint="3a_left_hip_y"/>
<motor gear="80" joint="3a_left_knee"/>
<motor gear="20" joint="3a_left_ankle_x"/>
<motor gear="20" joint="3a_left_ankle_y"/>
<motor gear="20" joint="3a_right_shoulder1"/>
<motor gear="20" joint="3a_right_shoulder2"/>
<motor gear="40" joint="3a_right_elbow"/>
<motor gear="20" joint="3a_left_shoulder1"/>
<motor gear="20" joint="3a_left_shoulder2"/>
<motor gear="40" joint="3a_left_elbow"/>
</actuator>
</mujoco>
mjx/mujoco/mjx/test_data/humanoid/04_humanoids.xml
+463
View File
@@ -0,0 +1,463 @@
<mujoco model="04 Humanoids">
<option timestep="0.005" solver="Newton" iterations="1" ls_iterations="4">
<flag eulerdamp="disable"/>
</option>
<custom>
<numeric data="16" name="max_contact_points"/>
<numeric data="16" name="max_geom_pairs"/>
</custom>
<size memory="100M"/>
<asset>
<texture type="skybox" builtin="gradient" rgb1=".3 .5 .7" rgb2="0 0 0" width="512" height="512"/>
<texture name="body" type="cube" builtin="flat" mark="cross" width="128" height="128"
rgb1="0.8 0.6 0.4" rgb2="0.8 0.6 0.4" markrgb="1 1 1" random="0.01"/>
<material name="body" texture="body" texuniform="true" rgba="0.8 0.6 .4 1"/>
<texture name="grid" type="2d" builtin="checker" width="512" height="512" rgb1=".1 .2 .3" rgb2=".2 .3 .4"/>
<material name="grid" texture="grid" texrepeat="1 1" texuniform="true" reflectance=".2"/>
</asset>
<default>
<motor ctrlrange="-1 1" ctrllimited="true"/>
<joint type="hinge" damping=".2" stiffness="1" armature=".01" limited="true" solimplimit="0 .99 .01"/>
<default class="big_joint">
<joint damping="5" stiffness="10"/>
<default class="big_stiff_joint">
<joint stiffness="20"/>
</default>
</default>
<default class="body1">
<geom type="capsule" condim="3" friction=".7" solimp=".9 .99 .003" solref=".015 1" material="body" contype="2" conaffinity="29"/>
</default>
<default class="body2">
<geom type="capsule" condim="3" friction=".7" solimp=".9 .99 .003" solref=".015 1" material="body" contype="4" conaffinity="27"/>
</default>
<default class="body3">
<geom type="capsule" condim="3" friction=".7" solimp=".9 .99 .003" solref=".015 1" material="body" contype="8" conaffinity="23"/>
</default>
<default class="body4">
<geom type="capsule" condim="3" friction=".7" solimp=".9 .99 .003" solref=".015 1" material="body" contype="16" conaffinity="15"/>
</default>
</default>
<visual>
<map force="0.1" zfar="30"/>
<rgba haze="0.15 0.25 0.35 1"/>
<quality shadowsize="4096"/>
<global offwidth="800" offheight="800"/>
</visual>
<worldbody>
<geom size="10 10 .05" type="plane" material="grid" condim="3"/>
<light dir=".2 1 -.4" diffuse=".8 .8 .8" specular="0.3 0.3 0.3" pos="-2 -10 4" cutoff="35"/>
<light dir="-.2 1 -.4" diffuse=".8 .8 .8" specular="0.3 0.3 0.3" pos="2 -10 4" cutoff="35"/>
<body name="1a_torso" pos="-1 0 1.5" childclass="body1">
<camera name="1a_back" pos="-3 0 1" xyaxes="0 -1 0 1 0 2" mode="trackcom"/>
<camera name="1a_side" pos="0 -3 1" xyaxes="1 0 0 0 1 2" mode="trackcom"/>
<freejoint name="1a_root"/>
<geom name="1a_torso" fromto="0 -.07 0 0 .07 0" size=".07"/>
<geom name="1a_upper_waist" fromto="-.01 -.06 -.12 -.01 .06 -.12" size=".06"/>
<body name="1a_head" pos="0 0 .19">
<geom name="1a_head" type="sphere" size=".09"/>
<camera name="1a_egocentric" pos=".09 0 0" xyaxes="0 -1 0 .1 0 1" fovy="80"/>
</body>
<body name="1a_lower_waist" pos="-.01 0 -.26">
<geom name="1a_lower_waist" fromto="0 -.06 0 0 .06 0" size=".06"/>
<joint name="1a_abdomen_z" pos="0 0 .065" axis="0 0 1" range="-45 45" class="big_stiff_joint"/>
<joint name="1a_abdomen_y" pos="0 0 .065" axis="0 1 0" range="-75 30" class="big_joint"/>
<body name="1a_pelvis" pos="0 0 -.165">
<joint name="1a_abdomen_x" pos="0 0 .1" axis="1 0 0" range="-35 35" class="big_joint"/>
<geom name="1a_butt" fromto="-.02 -.07 0 -.02 .07 0" size=".09"/>
<body name="1a_right_thigh" pos="0 -.1 -.04">
<joint name="1a_right_hip_x" axis="1 0 0" range="-25 5" class="big_joint"/>
<joint name="1a_right_hip_z" axis="0 0 1" range="-60 35" class="big_joint"/>
<joint name="1a_right_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
<geom name="1a_right_thigh" fromto="0 0 0 0 .01 -.34" size=".06"/>
<body name="1a_right_shin" pos="0 .01 -.403">
<joint name="1a_right_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
<geom name="1a_right_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
<body name="1a_right_foot" pos="0 0 -.39">
<joint name="1a_right_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
<joint name="1a_right_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
<geom name="1a_right_right_foot" fromto="-.07 -.02 0 .14 -.04 0" size=".027"/>
<geom name="1a_left_right_foot" fromto="-.07 0 0 .14 .02 0" size=".027"/>
</body>
</body>
</body>
<body name="1a_left_thigh" pos="0 .1 -.04">
<joint name="1a_left_hip_x" axis="-1 0 0" range="-25 5" class="big_joint"/>
<joint name="1a_left_hip_z" axis="0 0 -1" range="-60 35" class="big_joint"/>
<joint name="1a_left_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
<geom name="1a_left_thigh" fromto="0 0 0 0 -.01 -.34" size=".06"/>
<body name="1a_left_shin" pos="0 -.01 -.403">
<joint name="1a_left_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
<geom name="1a_left_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
<body name="1a_left_foot" pos="0 0 -.39">
<joint name="1a_left_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
<joint name="1a_left_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
<geom name="1a_left_left_foot" fromto="-.07 .02 0 .14 .04 0" size=".027"/>
<geom name="1a_right_left_foot" fromto="-.07 0 0 .14 -.02 0" size=".027"/>
</body>
</body>
</body>
</body>
</body>
<body name="1a_right_upper_arm" pos="0 -.17 .06">
<joint name="1a_right_shoulder1" axis="2 1 1" range="-85 60"/>
<joint name="1a_right_shoulder2" axis="0 -1 1" range="-85 60"/>
<geom name="1a_right_upper_arm" fromto="0 0 0 .16 -.16 -.16" size=".04 .16"/>
<body name="1a_right_lower_arm" pos=".18 -.18 -.18">
<joint name="1a_right_elbow" axis="0 -1 1" range="-90 50" stiffness="0"/>
<geom name="1a_right_lower_arm" fromto=".01 .01 .01 .17 .17 .17" size=".031"/>
<body name="1a_right_hand" pos=".18 .18 .18">
<geom name="1a_right_hand" type="sphere" size=".04" zaxis="1 1 1"/>
</body>
</body>
</body>
<body name="1a_left_upper_arm" pos="0 .17 .06">
<joint name="1a_left_shoulder1" axis="2 -1 1" range="-60 85"/>
<joint name="1a_left_shoulder2" axis="0 1 1" range="-60 85"/>
<geom name="1a_left_upper_arm" fromto="0 0 0 .16 .16 -.16" size=".04 .16"/>
<body name="1a_left_lower_arm" pos=".18 .18 -.18">
<joint name="1a_left_elbow" axis="0 -1 -1" range="-90 50" stiffness="0"/>
<geom name="1a_left_lower_arm" fromto=".01 -.01 .01 .17 -.17 .17" size=".031"/>
<body name="1a_left_hand" pos=".18 -.18 .18">
<geom name="1a_left_hand" type="sphere" size=".04" zaxis="1 -1 1"/>
</body>
</body>
</body>
</body>
<body name="2a_torso" pos="-1 1 1.5" childclass="body2">
<camera name="2a_back" pos="-3 0 1" xyaxes="0 -1 0 1 0 2" mode="trackcom"/>
<camera name="2a_side" pos="0 -3 1" xyaxes="1 0 0 0 1 2" mode="trackcom"/>
<freejoint name="2a_root"/>
<geom name="2a_torso" fromto="0 -.07 0 0 .07 0" size=".07"/>
<geom name="2a_upper_waist" fromto="-.01 -.06 -.12 -.01 .06 -.12" size=".06"/>
<body name="2a_head" pos="0 0 .19">
<geom name="2a_head" type="sphere" size=".09"/>
<camera name="2a_egocentric" pos=".09 0 0" xyaxes="0 -1 0 .1 0 1" fovy="80"/>
</body>
<body name="2a_lower_waist" pos="-.01 0 -.26">
<geom name="2a_lower_waist" fromto="0 -.06 0 0 .06 0" size=".06"/>
<joint name="2a_abdomen_z" pos="0 0 .065" axis="0 0 1" range="-45 45" class="big_stiff_joint"/>
<joint name="2a_abdomen_y" pos="0 0 .065" axis="0 1 0" range="-75 30" class="big_joint"/>
<body name="2a_pelvis" pos="0 0 -.165">
<joint name="2a_abdomen_x" pos="0 0 .1" axis="1 0 0" range="-35 35" class="big_joint"/>
<geom name="2a_butt" fromto="-.02 -.07 0 -.02 .07 0" size=".09"/>
<body name="2a_right_thigh" pos="0 -.1 -.04">
<joint name="2a_right_hip_x" axis="1 0 0" range="-25 5" class="big_joint"/>
<joint name="2a_right_hip_z" axis="0 0 1" range="-60 35" class="big_joint"/>
<joint name="2a_right_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
<geom name="2a_right_thigh" fromto="0 0 0 0 .01 -.34" size=".06"/>
<body name="2a_right_shin" pos="0 .01 -.403">
<joint name="2a_right_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
<geom name="2a_right_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
<body name="2a_right_foot" pos="0 0 -.39">
<joint name="2a_right_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
<joint name="2a_right_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
<geom name="2a_right_right_foot" fromto="-.07 -.02 0 .14 -.04 0" size=".027"/>
<geom name="2a_left_right_foot" fromto="-.07 0 0 .14 .02 0" size=".027"/>
</body>
</body>
</body>
<body name="2a_left_thigh" pos="0 .1 -.04">
<joint name="2a_left_hip_x" axis="-1 0 0" range="-25 5" class="big_joint"/>
<joint name="2a_left_hip_z" axis="0 0 -1" range="-60 35" class="big_joint"/>
<joint name="2a_left_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
<geom name="2a_left_thigh" fromto="0 0 0 0 -.01 -.34" size=".06"/>
<body name="2a_left_shin" pos="0 -.01 -.403">
<joint name="2a_left_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
<geom name="2a_left_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
<body name="2a_left_foot" pos="0 0 -.39">
<joint name="2a_left_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
<joint name="2a_left_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
<geom name="2a_left_left_foot" fromto="-.07 .02 0 .14 .04 0" size=".027"/>
<geom name="2a_right_left_foot" fromto="-.07 0 0 .14 -.02 0" size=".027"/>
</body>
</body>
</body>
</body>
</body>
<body name="2a_right_upper_arm" pos="0 -.17 .06">
<joint name="2a_right_shoulder1" axis="2 1 1" range="-85 60"/>
<joint name="2a_right_shoulder2" axis="0 -1 1" range="-85 60"/>
<geom name="2a_right_upper_arm" fromto="0 0 0 .16 -.16 -.16" size=".04 .16"/>
<body name="2a_right_lower_arm" pos=".18 -.18 -.18">
<joint name="2a_right_elbow" axis="0 -1 1" range="-90 50" stiffness="0"/>
<geom name="2a_right_lower_arm" fromto=".01 .01 .01 .17 .17 .17" size=".031"/>
<body name="2a_right_hand" pos=".18 .18 .18">
<geom name="2a_right_hand" type="sphere" size=".04" zaxis="1 1 1"/>
</body>
</body>
</body>
<body name="2a_left_upper_arm" pos="0 .17 .06">
<joint name="2a_left_shoulder1" axis="2 -1 1" range="-60 85"/>
<joint name="2a_left_shoulder2" axis="0 1 1" range="-60 85"/>
<geom name="2a_left_upper_arm" fromto="0 0 0 .16 .16 -.16" size=".04 .16"/>
<body name="2a_left_lower_arm" pos=".18 .18 -.18">
<joint name="2a_left_elbow" axis="0 -1 -1" range="-90 50" stiffness="0"/>
<geom name="2a_left_lower_arm" fromto=".01 -.01 .01 .17 -.17 .17" size=".031"/>
<body name="2a_left_hand" pos=".18 -.18 .18">
<geom name="2a_left_hand" type="sphere" size=".04" zaxis="1 -1 1"/>
</body>
</body>
</body>
</body>
<body name="3a_torso" pos="-1 -1 1.5" childclass="body3">
<camera name="3a_back" pos="-3 0 1" xyaxes="0 -1 0 1 0 2" mode="trackcom"/>
<camera name="3a_side" pos="0 -3 1" xyaxes="1 0 0 0 1 2" mode="trackcom"/>
<freejoint name="3a_root"/>
<geom name="3a_torso" fromto="0 -.07 0 0 .07 0" size=".07"/>
<geom name="3a_upper_waist" fromto="-.01 -.06 -.12 -.01 .06 -.12" size=".06"/>
<body name="3a_head" pos="0 0 .19">
<geom name="3a_head" type="sphere" size=".09"/>
<camera name="3a_egocentric" pos=".09 0 0" xyaxes="0 -1 0 .1 0 1" fovy="80"/>
</body>
<body name="3a_lower_waist" pos="-.01 0 -.26">
<geom name="3a_lower_waist" fromto="0 -.06 0 0 .06 0" size=".06"/>
<joint name="3a_abdomen_z" pos="0 0 .065" axis="0 0 1" range="-45 45" class="big_stiff_joint"/>
<joint name="3a_abdomen_y" pos="0 0 .065" axis="0 1 0" range="-75 30" class="big_joint"/>
<body name="3a_pelvis" pos="0 0 -.165">
<joint name="3a_abdomen_x" pos="0 0 .1" axis="1 0 0" range="-35 35" class="big_joint"/>
<geom name="3a_butt" fromto="-.02 -.07 0 -.02 .07 0" size=".09"/>
<body name="3a_right_thigh" pos="0 -.1 -.04">
<joint name="3a_right_hip_x" axis="1 0 0" range="-25 5" class="big_joint"/>
<joint name="3a_right_hip_z" axis="0 0 1" range="-60 35" class="big_joint"/>
<joint name="3a_right_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
<geom name="3a_right_thigh" fromto="0 0 0 0 .01 -.34" size=".06"/>
<body name="3a_right_shin" pos="0 .01 -.403">
<joint name="3a_right_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
<geom name="3a_right_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
<body name="3a_right_foot" pos="0 0 -.39">
<joint name="3a_right_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
<joint name="3a_right_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
<geom name="3a_right_right_foot" fromto="-.07 -.02 0 .14 -.04 0" size=".027"/>
<geom name="3a_left_right_foot" fromto="-.07 0 0 .14 .02 0" size=".027"/>
</body>
</body>
</body>
<body name="3a_left_thigh" pos="0 .1 -.04">
<joint name="3a_left_hip_x" axis="-1 0 0" range="-25 5" class="big_joint"/>
<joint name="3a_left_hip_z" axis="0 0 -1" range="-60 35" class="big_joint"/>
<joint name="3a_left_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
<geom name="3a_left_thigh" fromto="0 0 0 0 -.01 -.34" size=".06"/>
<body name="3a_left_shin" pos="0 -.01 -.403">
<joint name="3a_left_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
<geom name="3a_left_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
<body name="3a_left_foot" pos="0 0 -.39">
<joint name="3a_left_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
<joint name="3a_left_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
<geom name="3a_left_left_foot" fromto="-.07 .02 0 .14 .04 0" size=".027"/>
<geom name="3a_right_left_foot" fromto="-.07 0 0 .14 -.02 0" size=".027"/>
</body>
</body>
</body>
</body>
</body>
<body name="3a_right_upper_arm" pos="0 -.17 .06">
<joint name="3a_right_shoulder1" axis="2 1 1" range="-85 60"/>
<joint name="3a_right_shoulder2" axis="0 -1 1" range="-85 60"/>
<geom name="3a_right_upper_arm" fromto="0 0 0 .16 -.16 -.16" size=".04 .16"/>
<body name="3a_right_lower_arm" pos=".18 -.18 -.18">
<joint name="3a_right_elbow" axis="0 -1 1" range="-90 50" stiffness="0"/>
<geom name="3a_right_lower_arm" fromto=".01 .01 .01 .17 .17 .17" size=".031"/>
<body name="3a_right_hand" pos=".18 .18 .18">
<geom name="3a_right_hand" type="sphere" size=".04" zaxis="1 1 1"/>
</body>
</body>
</body>
<body name="3a_left_upper_arm" pos="0 .17 .06">
<joint name="3a_left_shoulder1" axis="2 -1 1" range="-60 85"/>
<joint name="3a_left_shoulder2" axis="0 1 1" range="-60 85"/>
<geom name="3a_left_upper_arm" fromto="0 0 0 .16 .16 -.16" size=".04 .16"/>
<body name="3a_left_lower_arm" pos=".18 .18 -.18">
<joint name="3a_left_elbow" axis="0 -1 -1" range="-90 50" stiffness="0"/>
<geom name="3a_left_lower_arm" fromto=".01 -.01 .01 .17 -.17 .17" size=".031"/>
<body name="3a_left_hand" pos=".18 -.18 .18">
<geom name="3a_left_hand" type="sphere" size=".04" zaxis="1 -1 1"/>
</body>
</body>
</body>
</body>
<body name="4a_torso" pos="-1 2 1.5" childclass="body4">
<camera name="4a_back" pos="-3 0 1" xyaxes="0 -1 0 1 0 2" mode="trackcom"/>
<camera name="4a_side" pos="0 -3 1" xyaxes="1 0 0 0 1 2" mode="trackcom"/>
<freejoint name="4a_root"/>
<geom name="4a_torso" fromto="0 -.07 0 0 .07 0" size=".07"/>
<geom name="4a_upper_waist" fromto="-.01 -.06 -.12 -.01 .06 -.12" size=".06"/>
<body name="4a_head" pos="0 0 .19">
<geom name="4a_head" type="sphere" size=".09"/>
<camera name="4a_egocentric" pos=".09 0 0" xyaxes="0 -1 0 .1 0 1" fovy="80"/>
</body>
<body name="4a_lower_waist" pos="-.01 0 -.26">
<geom name="4a_lower_waist" fromto="0 -.06 0 0 .06 0" size=".06"/>
<joint name="4a_abdomen_z" pos="0 0 .065" axis="0 0 1" range="-45 45" class="big_stiff_joint"/>
<joint name="4a_abdomen_y" pos="0 0 .065" axis="0 1 0" range="-75 30" class="big_joint"/>
<body name="4a_pelvis" pos="0 0 -.165">
<joint name="4a_abdomen_x" pos="0 0 .1" axis="1 0 0" range="-35 35" class="big_joint"/>
<geom name="4a_butt" fromto="-.02 -.07 0 -.02 .07 0" size=".09"/>
<body name="4a_right_thigh" pos="0 -.1 -.04">
<joint name="4a_right_hip_x" axis="1 0 0" range="-25 5" class="big_joint"/>
<joint name="4a_right_hip_z" axis="0 0 1" range="-60 35" class="big_joint"/>
<joint name="4a_right_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
<geom name="4a_right_thigh" fromto="0 0 0 0 .01 -.34" size=".06"/>
<body name="4a_right_shin" pos="0 .01 -.403">
<joint name="4a_right_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
<geom name="4a_right_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
<body name="4a_right_foot" pos="0 0 -.39">
<joint name="4a_right_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
<joint name="4a_right_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
<geom name="4a_right_right_foot" fromto="-.07 -.02 0 .14 -.04 0" size=".027"/>
<geom name="4a_left_right_foot" fromto="-.07 0 0 .14 .02 0" size=".027"/>
</body>
</body>
</body>
<body name="4a_left_thigh" pos="0 .1 -.04">
<joint name="4a_left_hip_x" axis="-1 0 0" range="-25 5" class="big_joint"/>
<joint name="4a_left_hip_z" axis="0 0 -1" range="-60 35" class="big_joint"/>
<joint name="4a_left_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
<geom name="4a_left_thigh" fromto="0 0 0 0 -.01 -.34" size=".06"/>
<body name="4a_left_shin" pos="0 -.01 -.403">
<joint name="4a_left_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
<geom name="4a_left_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
<body name="4a_left_foot" pos="0 0 -.39">
<joint name="4a_left_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
<joint name="4a_left_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
<geom name="4a_left_left_foot" fromto="-.07 .02 0 .14 .04 0" size=".027"/>
<geom name="4a_right_left_foot" fromto="-.07 0 0 .14 -.02 0" size=".027"/>
</body>
</body>
</body>
</body>
</body>
<body name="4a_right_upper_arm" pos="0 -.17 .06">
<joint name="4a_right_shoulder1" axis="2 1 1" range="-85 60"/>
<joint name="4a_right_shoulder2" axis="0 -1 1" range="-85 60"/>
<geom name="4a_right_upper_arm" fromto="0 0 0 .16 -.16 -.16" size=".04 .16"/>
<body name="4a_right_lower_arm" pos=".18 -.18 -.18">
<joint name="4a_right_elbow" axis="0 -1 1" range="-90 50" stiffness="0"/>
<geom name="4a_right_lower_arm" fromto=".01 .01 .01 .17 .17 .17" size=".031"/>
<body name="4a_right_hand" pos=".18 .18 .18">
<geom name="4a_right_hand" type="sphere" size=".04" zaxis="1 1 1"/>
</body>
</body>
</body>
<body name="4a_left_upper_arm" pos="0 .17 .06">
<joint name="4a_left_shoulder1" axis="2 -1 1" range="-60 85"/>
<joint name="4a_left_shoulder2" axis="0 1 1" range="-60 85"/>
<geom name="4a_left_upper_arm" fromto="0 0 0 .16 .16 -.16" size=".04 .16"/>
<body name="4a_left_lower_arm" pos=".18 .18 -.18">
<joint name="4a_left_elbow" axis="0 -1 -1" range="-90 50" stiffness="0"/>
<geom name="4a_left_lower_arm" fromto=".01 -.01 .01 .17 -.17 .17" size=".031"/>
<body name="4a_left_hand" pos=".18 -.18 .18">
<geom name="4a_left_hand" type="sphere" size=".04" zaxis="1 -1 1"/>
</body>
</body>
</body>
</body>
</worldbody>
<actuator>
<motor gear="40" joint="1a_abdomen_y"/>
<motor gear="40" joint="1a_abdomen_z"/>
<motor gear="40" joint="1a_abdomen_x"/>
<motor gear="40" joint="1a_right_hip_x"/>
<motor gear="40" joint="1a_right_hip_z"/>
<motor gear="120" joint="1a_right_hip_y"/>
<motor gear="80" joint="1a_right_knee"/>
<motor gear="20" joint="1a_right_ankle_x"/>
<motor gear="20" joint="1a_right_ankle_y"/>
<motor gear="40" joint="1a_left_hip_x"/>
<motor gear="40" joint="1a_left_hip_z"/>
<motor gear="120" joint="1a_left_hip_y"/>
<motor gear="80" joint="1a_left_knee"/>
<motor gear="20" joint="1a_left_ankle_x"/>
<motor gear="20" joint="1a_left_ankle_y"/>
<motor gear="20" joint="1a_right_shoulder1"/>
<motor gear="20" joint="1a_right_shoulder2"/>
<motor gear="40" joint="1a_right_elbow"/>
<motor gear="20" joint="1a_left_shoulder1"/>
<motor gear="20" joint="1a_left_shoulder2"/>
<motor gear="40" joint="1a_left_elbow"/>
</actuator>
<actuator>
<motor gear="40" joint="2a_abdomen_y"/>
<motor gear="40" joint="2a_abdomen_z"/>
<motor gear="40" joint="2a_abdomen_x"/>
<motor gear="40" joint="2a_right_hip_x"/>
<motor gear="40" joint="2a_right_hip_z"/>
<motor gear="120" joint="2a_right_hip_y"/>
<motor gear="80" joint="2a_right_knee"/>
<motor gear="20" joint="2a_right_ankle_x"/>
<motor gear="20" joint="2a_right_ankle_y"/>
<motor gear="40" joint="2a_left_hip_x"/>
<motor gear="40" joint="2a_left_hip_z"/>
<motor gear="120" joint="2a_left_hip_y"/>
<motor gear="80" joint="2a_left_knee"/>
<motor gear="20" joint="2a_left_ankle_x"/>
<motor gear="20" joint="2a_left_ankle_y"/>
<motor gear="20" joint="2a_right_shoulder1"/>
<motor gear="20" joint="2a_right_shoulder2"/>
<motor gear="40" joint="2a_right_elbow"/>
<motor gear="20" joint="2a_left_shoulder1"/>
<motor gear="20" joint="2a_left_shoulder2"/>
<motor gear="40" joint="2a_left_elbow"/>
</actuator>
<actuator>
<motor gear="40" joint="3a_abdomen_y"/>
<motor gear="40" joint="3a_abdomen_z"/>
<motor gear="40" joint="3a_abdomen_x"/>
<motor gear="40" joint="3a_right_hip_x"/>
<motor gear="40" joint="3a_right_hip_z"/>
<motor gear="120" joint="3a_right_hip_y"/>
<motor gear="80" joint="3a_right_knee"/>
<motor gear="20" joint="3a_right_ankle_x"/>
<motor gear="20" joint="3a_right_ankle_y"/>
<motor gear="40" joint="3a_left_hip_x"/>
<motor gear="40" joint="3a_left_hip_z"/>
<motor gear="120" joint="3a_left_hip_y"/>
<motor gear="80" joint="3a_left_knee"/>
<motor gear="20" joint="3a_left_ankle_x"/>
<motor gear="20" joint="3a_left_ankle_y"/>
<motor gear="20" joint="3a_right_shoulder1"/>
<motor gear="20" joint="3a_right_shoulder2"/>
<motor gear="40" joint="3a_right_elbow"/>
<motor gear="20" joint="3a_left_shoulder1"/>
<motor gear="20" joint="3a_left_shoulder2"/>
<motor gear="40" joint="3a_left_elbow"/>
</actuator>
<actuator>
<motor gear="40" joint="4a_abdomen_y"/>
<motor gear="40" joint="4a_abdomen_z"/>
<motor gear="40" joint="4a_abdomen_x"/>
<motor gear="40" joint="4a_right_hip_x"/>
<motor gear="40" joint="4a_right_hip_z"/>
<motor gear="120" joint="4a_right_hip_y"/>
<motor gear="80" joint="4a_right_knee"/>
<motor gear="20" joint="4a_right_ankle_x"/>
<motor gear="20" joint="4a_right_ankle_y"/>
<motor gear="40" joint="4a_left_hip_x"/>
<motor gear="40" joint="4a_left_hip_z"/>
<motor gear="120" joint="4a_left_hip_y"/>
<motor gear="80" joint="4a_left_knee"/>
<motor gear="20" joint="4a_left_ankle_x"/>
<motor gear="20" joint="4a_left_ankle_y"/>
<motor gear="20" joint="4a_right_shoulder1"/>
<motor gear="20" joint="4a_right_shoulder2"/>
<motor gear="40" joint="4a_right_elbow"/>
<motor gear="20" joint="4a_left_shoulder1"/>
<motor gear="20" joint="4a_left_shoulder2"/>
<motor gear="40" joint="4a_left_elbow"/>
</actuator>
</mujoco>
mjx/mujoco/mjx/test_data/humanoid/05_humanoids.xml
+567
View File
@@ -0,0 +1,567 @@
<mujoco model="05 Humanoids">
<option timestep="0.005" solver="Newton" iterations="1" ls_iterations="4">
<flag eulerdamp="disable"/>
</option>
<custom>
<numeric data="20" name="max_contact_points"/>
<numeric data="20" name="max_geom_pairs"/>
</custom>
<size memory="100M"/>
<asset>
<texture type="skybox" builtin="gradient" rgb1=".3 .5 .7" rgb2="0 0 0" width="512" height="512"/>
<texture name="body" type="cube" builtin="flat" mark="cross" width="128" height="128"
rgb1="0.8 0.6 0.4" rgb2="0.8 0.6 0.4" markrgb="1 1 1" random="0.01"/>
<material name="body" texture="body" texuniform="true" rgba="0.8 0.6 .4 1"/>
<texture name="grid" type="2d" builtin="checker" width="512" height="512" rgb1=".1 .2 .3" rgb2=".2 .3 .4"/>
<material name="grid" texture="grid" texrepeat="1 1" texuniform="true" reflectance=".2"/>
</asset>
<default>
<motor ctrlrange="-1 1" ctrllimited="true"/>
<joint type="hinge" damping=".2" stiffness="1" armature=".01" limited="true" solimplimit="0 .99 .01"/>
<default class="big_joint">
<joint damping="5" stiffness="10"/>
<default class="big_stiff_joint">
<joint stiffness="20"/>
</default>
</default>
<default class="body1">
<geom type="capsule" condim="3" friction=".7" solimp=".9 .99 .003" solref=".015 1" material="body" contype="2" conaffinity="61"/>
</default>
<default class="body2">
<geom type="capsule" condim="3" friction=".7" solimp=".9 .99 .003" solref=".015 1" material="body" contype="4" conaffinity="59"/>
</default>
<default class="body3">
<geom type="capsule" condim="3" friction=".7" solimp=".9 .99 .003" solref=".015 1" material="body" contype="8" conaffinity="55"/>
</default>
<default class="body4">
<geom type="capsule" condim="3" friction=".7" solimp=".9 .99 .003" solref=".015 1" material="body" contype="16" conaffinity="47"/>
</default>
<default class="body5">
<geom type="capsule" condim="3" friction=".7" solimp=".9 .99 .003" solref=".015 1" material="body" contype="32" conaffinity="31"/>
</default>
</default>
<visual>
<map force="0.1" zfar="30"/>
<rgba haze="0.15 0.25 0.35 1"/>
<quality shadowsize="4096"/>
<global offwidth="800" offheight="800"/>
</visual>
<worldbody>
<geom size="10 10 .05" type="plane" material="grid" condim="3"/>
<light dir=".2 1 -.4" diffuse=".8 .8 .8" specular="0.3 0.3 0.3" pos="-2 -10 4" cutoff="35"/>
<light dir="-.2 1 -.4" diffuse=".8 .8 .8" specular="0.3 0.3 0.3" pos="2 -10 4" cutoff="35"/>
<body name="1a_torso" pos="-1 0 1.5" childclass="body1">
<camera name="1a_back" pos="-3 0 1" xyaxes="0 -1 0 1 0 2" mode="trackcom"/>
<camera name="1a_side" pos="0 -3 1" xyaxes="1 0 0 0 1 2" mode="trackcom"/>
<freejoint name="1a_root"/>
<geom name="1a_torso" fromto="0 -.07 0 0 .07 0" size=".07"/>
<geom name="1a_upper_waist" fromto="-.01 -.06 -.12 -.01 .06 -.12" size=".06"/>
<body name="1a_head" pos="0 0 .19">
<geom name="1a_head" type="sphere" size=".09"/>
<camera name="1a_egocentric" pos=".09 0 0" xyaxes="0 -1 0 .1 0 1" fovy="80"/>
</body>
<body name="1a_lower_waist" pos="-.01 0 -.26">
<geom name="1a_lower_waist" fromto="0 -.06 0 0 .06 0" size=".06"/>
<joint name="1a_abdomen_z" pos="0 0 .065" axis="0 0 1" range="-45 45" class="big_stiff_joint"/>
<joint name="1a_abdomen_y" pos="0 0 .065" axis="0 1 0" range="-75 30" class="big_joint"/>
<body name="1a_pelvis" pos="0 0 -.165">
<joint name="1a_abdomen_x" pos="0 0 .1" axis="1 0 0" range="-35 35" class="big_joint"/>
<geom name="1a_butt" fromto="-.02 -.07 0 -.02 .07 0" size=".09"/>
<body name="1a_right_thigh" pos="0 -.1 -.04">
<joint name="1a_right_hip_x" axis="1 0 0" range="-25 5" class="big_joint"/>
<joint name="1a_right_hip_z" axis="0 0 1" range="-60 35" class="big_joint"/>
<joint name="1a_right_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
<geom name="1a_right_thigh" fromto="0 0 0 0 .01 -.34" size=".06"/>
<body name="1a_right_shin" pos="0 .01 -.403">
<joint name="1a_right_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
<geom name="1a_right_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
<body name="1a_right_foot" pos="0 0 -.39">
<joint name="1a_right_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
<joint name="1a_right_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
<geom name="1a_right_right_foot" fromto="-.07 -.02 0 .14 -.04 0" size=".027"/>
<geom name="1a_left_right_foot" fromto="-.07 0 0 .14 .02 0" size=".027"/>
</body>
</body>
</body>
<body name="1a_left_thigh" pos="0 .1 -.04">
<joint name="1a_left_hip_x" axis="-1 0 0" range="-25 5" class="big_joint"/>
<joint name="1a_left_hip_z" axis="0 0 -1" range="-60 35" class="big_joint"/>
<joint name="1a_left_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
<geom name="1a_left_thigh" fromto="0 0 0 0 -.01 -.34" size=".06"/>
<body name="1a_left_shin" pos="0 -.01 -.403">
<joint name="1a_left_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
<geom name="1a_left_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
<body name="1a_left_foot" pos="0 0 -.39">
<joint name="1a_left_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
<joint name="1a_left_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
<geom name="1a_left_left_foot" fromto="-.07 .02 0 .14 .04 0" size=".027"/>
<geom name="1a_right_left_foot" fromto="-.07 0 0 .14 -.02 0" size=".027"/>
</body>
</body>
</body>
</body>
</body>
<body name="1a_right_upper_arm" pos="0 -.17 .06">
<joint name="1a_right_shoulder1" axis="2 1 1" range="-85 60"/>
<joint name="1a_right_shoulder2" axis="0 -1 1" range="-85 60"/>
<geom name="1a_right_upper_arm" fromto="0 0 0 .16 -.16 -.16" size=".04 .16"/>
<body name="1a_right_lower_arm" pos=".18 -.18 -.18">
<joint name="1a_right_elbow" axis="0 -1 1" range="-90 50" stiffness="0"/>
<geom name="1a_right_lower_arm" fromto=".01 .01 .01 .17 .17 .17" size=".031"/>
<body name="1a_right_hand" pos=".18 .18 .18">
<geom name="1a_right_hand" type="sphere" size=".04" zaxis="1 1 1"/>
</body>
</body>
</body>
<body name="1a_left_upper_arm" pos="0 .17 .06">
<joint name="1a_left_shoulder1" axis="2 -1 1" range="-60 85"/>
<joint name="1a_left_shoulder2" axis="0 1 1" range="-60 85"/>
<geom name="1a_left_upper_arm" fromto="0 0 0 .16 .16 -.16" size=".04 .16"/>
<body name="1a_left_lower_arm" pos=".18 .18 -.18">
<joint name="1a_left_elbow" axis="0 -1 -1" range="-90 50" stiffness="0"/>
<geom name="1a_left_lower_arm" fromto=".01 -.01 .01 .17 -.17 .17" size=".031"/>
<body name="1a_left_hand" pos=".18 -.18 .18">
<geom name="1a_left_hand" type="sphere" size=".04" zaxis="1 -1 1"/>
</body>
</body>
</body>
</body>
<body name="2a_torso" pos="-1 1 1.5" childclass="body2">
<camera name="2a_back" pos="-3 0 1" xyaxes="0 -1 0 1 0 2" mode="trackcom"/>
<camera name="2a_side" pos="0 -3 1" xyaxes="1 0 0 0 1 2" mode="trackcom"/>
<freejoint name="2a_root"/>
<geom name="2a_torso" fromto="0 -.07 0 0 .07 0" size=".07"/>
<geom name="2a_upper_waist" fromto="-.01 -.06 -.12 -.01 .06 -.12" size=".06"/>
<body name="2a_head" pos="0 0 .19">
<geom name="2a_head" type="sphere" size=".09"/>
<camera name="2a_egocentric" pos=".09 0 0" xyaxes="0 -1 0 .1 0 1" fovy="80"/>
</body>
<body name="2a_lower_waist" pos="-.01 0 -.26">
<geom name="2a_lower_waist" fromto="0 -.06 0 0 .06 0" size=".06"/>
<joint name="2a_abdomen_z" pos="0 0 .065" axis="0 0 1" range="-45 45" class="big_stiff_joint"/>
<joint name="2a_abdomen_y" pos="0 0 .065" axis="0 1 0" range="-75 30" class="big_joint"/>
<body name="2a_pelvis" pos="0 0 -.165">
<joint name="2a_abdomen_x" pos="0 0 .1" axis="1 0 0" range="-35 35" class="big_joint"/>
<geom name="2a_butt" fromto="-.02 -.07 0 -.02 .07 0" size=".09"/>
<body name="2a_right_thigh" pos="0 -.1 -.04">
<joint name="2a_right_hip_x" axis="1 0 0" range="-25 5" class="big_joint"/>
<joint name="2a_right_hip_z" axis="0 0 1" range="-60 35" class="big_joint"/>
<joint name="2a_right_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
<geom name="2a_right_thigh" fromto="0 0 0 0 .01 -.34" size=".06"/>
<body name="2a_right_shin" pos="0 .01 -.403">
<joint name="2a_right_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
<geom name="2a_right_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
<body name="2a_right_foot" pos="0 0 -.39">
<joint name="2a_right_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
<joint name="2a_right_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
<geom name="2a_right_right_foot" fromto="-.07 -.02 0 .14 -.04 0" size=".027"/>
<geom name="2a_left_right_foot" fromto="-.07 0 0 .14 .02 0" size=".027"/>
</body>
</body>
</body>
<body name="2a_left_thigh" pos="0 .1 -.04">
<joint name="2a_left_hip_x" axis="-1 0 0" range="-25 5" class="big_joint"/>
<joint name="2a_left_hip_z" axis="0 0 -1" range="-60 35" class="big_joint"/>
<joint name="2a_left_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
<geom name="2a_left_thigh" fromto="0 0 0 0 -.01 -.34" size=".06"/>
<body name="2a_left_shin" pos="0 -.01 -.403">
<joint name="2a_left_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
<geom name="2a_left_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
<body name="2a_left_foot" pos="0 0 -.39">
<joint name="2a_left_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
<joint name="2a_left_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
<geom name="2a_left_left_foot" fromto="-.07 .02 0 .14 .04 0" size=".027"/>
<geom name="2a_right_left_foot" fromto="-.07 0 0 .14 -.02 0" size=".027"/>
</body>
</body>
</body>
</body>
</body>
<body name="2a_right_upper_arm" pos="0 -.17 .06">
<joint name="2a_right_shoulder1" axis="2 1 1" range="-85 60"/>
<joint name="2a_right_shoulder2" axis="0 -1 1" range="-85 60"/>
<geom name="2a_right_upper_arm" fromto="0 0 0 .16 -.16 -.16" size=".04 .16"/>
<body name="2a_right_lower_arm" pos=".18 -.18 -.18">
<joint name="2a_right_elbow" axis="0 -1 1" range="-90 50" stiffness="0"/>
<geom name="2a_right_lower_arm" fromto=".01 .01 .01 .17 .17 .17" size=".031"/>
<body name="2a_right_hand" pos=".18 .18 .18">
<geom name="2a_right_hand" type="sphere" size=".04" zaxis="1 1 1"/>
</body>
</body>
</body>
<body name="2a_left_upper_arm" pos="0 .17 .06">
<joint name="2a_left_shoulder1" axis="2 -1 1" range="-60 85"/>
<joint name="2a_left_shoulder2" axis="0 1 1" range="-60 85"/>
<geom name="2a_left_upper_arm" fromto="0 0 0 .16 .16 -.16" size=".04 .16"/>
<body name="2a_left_lower_arm" pos=".18 .18 -.18">
<joint name="2a_left_elbow" axis="0 -1 -1" range="-90 50" stiffness="0"/>
<geom name="2a_left_lower_arm" fromto=".01 -.01 .01 .17 -.17 .17" size=".031"/>
<body name="2a_left_hand" pos=".18 -.18 .18">
<geom name="2a_left_hand" type="sphere" size=".04" zaxis="1 -1 1"/>
</body>
</body>
</body>
</body>
<body name="3a_torso" pos="-1 -1 1.5" childclass="body3">
<camera name="3a_back" pos="-3 0 1" xyaxes="0 -1 0 1 0 2" mode="trackcom"/>
<camera name="3a_side" pos="0 -3 1" xyaxes="1 0 0 0 1 2" mode="trackcom"/>
<freejoint name="3a_root"/>
<geom name="3a_torso" fromto="0 -.07 0 0 .07 0" size=".07"/>
<geom name="3a_upper_waist" fromto="-.01 -.06 -.12 -.01 .06 -.12" size=".06"/>
<body name="3a_head" pos="0 0 .19">
<geom name="3a_head" type="sphere" size=".09"/>
<camera name="3a_egocentric" pos=".09 0 0" xyaxes="0 -1 0 .1 0 1" fovy="80"/>
</body>
<body name="3a_lower_waist" pos="-.01 0 -.26">
<geom name="3a_lower_waist" fromto="0 -.06 0 0 .06 0" size=".06"/>
<joint name="3a_abdomen_z" pos="0 0 .065" axis="0 0 1" range="-45 45" class="big_stiff_joint"/>
<joint name="3a_abdomen_y" pos="0 0 .065" axis="0 1 0" range="-75 30" class="big_joint"/>
<body name="3a_pelvis" pos="0 0 -.165">
<joint name="3a_abdomen_x" pos="0 0 .1" axis="1 0 0" range="-35 35" class="big_joint"/>
<geom name="3a_butt" fromto="-.02 -.07 0 -.02 .07 0" size=".09"/>
<body name="3a_right_thigh" pos="0 -.1 -.04">
<joint name="3a_right_hip_x" axis="1 0 0" range="-25 5" class="big_joint"/>
<joint name="3a_right_hip_z" axis="0 0 1" range="-60 35" class="big_joint"/>
<joint name="3a_right_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
<geom name="3a_right_thigh" fromto="0 0 0 0 .01 -.34" size=".06"/>
<body name="3a_right_shin" pos="0 .01 -.403">
<joint name="3a_right_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
<geom name="3a_right_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
<body name="3a_right_foot" pos="0 0 -.39">
<joint name="3a_right_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
<joint name="3a_right_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
<geom name="3a_right_right_foot" fromto="-.07 -.02 0 .14 -.04 0" size=".027"/>
<geom name="3a_left_right_foot" fromto="-.07 0 0 .14 .02 0" size=".027"/>
</body>
</body>
</body>
<body name="3a_left_thigh" pos="0 .1 -.04">
<joint name="3a_left_hip_x" axis="-1 0 0" range="-25 5" class="big_joint"/>
<joint name="3a_left_hip_z" axis="0 0 -1" range="-60 35" class="big_joint"/>
<joint name="3a_left_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
<geom name="3a_left_thigh" fromto="0 0 0 0 -.01 -.34" size=".06"/>
<body name="3a_left_shin" pos="0 -.01 -.403">
<joint name="3a_left_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
<geom name="3a_left_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
<body name="3a_left_foot" pos="0 0 -.39">
<joint name="3a_left_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
<joint name="3a_left_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
<geom name="3a_left_left_foot" fromto="-.07 .02 0 .14 .04 0" size=".027"/>
<geom name="3a_right_left_foot" fromto="-.07 0 0 .14 -.02 0" size=".027"/>
</body>
</body>
</body>
</body>
</body>
<body name="3a_right_upper_arm" pos="0 -.17 .06">
<joint name="3a_right_shoulder1" axis="2 1 1" range="-85 60"/>
<joint name="3a_right_shoulder2" axis="0 -1 1" range="-85 60"/>
<geom name="3a_right_upper_arm" fromto="0 0 0 .16 -.16 -.16" size=".04 .16"/>
<body name="3a_right_lower_arm" pos=".18 -.18 -.18">
<joint name="3a_right_elbow" axis="0 -1 1" range="-90 50" stiffness="0"/>
<geom name="3a_right_lower_arm" fromto=".01 .01 .01 .17 .17 .17" size=".031"/>
<body name="3a_right_hand" pos=".18 .18 .18">
<geom name="3a_right_hand" type="sphere" size=".04" zaxis="1 1 1"/>
</body>
</body>
</body>
<body name="3a_left_upper_arm" pos="0 .17 .06">
<joint name="3a_left_shoulder1" axis="2 -1 1" range="-60 85"/>
<joint name="3a_left_shoulder2" axis="0 1 1" range="-60 85"/>
<geom name="3a_left_upper_arm" fromto="0 0 0 .16 .16 -.16" size=".04 .16"/>
<body name="3a_left_lower_arm" pos=".18 .18 -.18">
<joint name="3a_left_elbow" axis="0 -1 -1" range="-90 50" stiffness="0"/>
<geom name="3a_left_lower_arm" fromto=".01 -.01 .01 .17 -.17 .17" size=".031"/>
<body name="3a_left_hand" pos=".18 -.18 .18">
<geom name="3a_left_hand" type="sphere" size=".04" zaxis="1 -1 1"/>
</body>
</body>
</body>
</body>
<body name="4a_torso" pos="-1 2 1.5" childclass="body4">
<camera name="4a_back" pos="-3 0 1" xyaxes="0 -1 0 1 0 2" mode="trackcom"/>
<camera name="4a_side" pos="0 -3 1" xyaxes="1 0 0 0 1 2" mode="trackcom"/>
<freejoint name="4a_root"/>
<geom name="4a_torso" fromto="0 -.07 0 0 .07 0" size=".07"/>
<geom name="4a_upper_waist" fromto="-.01 -.06 -.12 -.01 .06 -.12" size=".06"/>
<body name="4a_head" pos="0 0 .19">
<geom name="4a_head" type="sphere" size=".09"/>
<camera name="4a_egocentric" pos=".09 0 0" xyaxes="0 -1 0 .1 0 1" fovy="80"/>
</body>
<body name="4a_lower_waist" pos="-.01 0 -.26">
<geom name="4a_lower_waist" fromto="0 -.06 0 0 .06 0" size=".06"/>
<joint name="4a_abdomen_z" pos="0 0 .065" axis="0 0 1" range="-45 45" class="big_stiff_joint"/>
<joint name="4a_abdomen_y" pos="0 0 .065" axis="0 1 0" range="-75 30" class="big_joint"/>
<body name="4a_pelvis" pos="0 0 -.165">
<joint name="4a_abdomen_x" pos="0 0 .1" axis="1 0 0" range="-35 35" class="big_joint"/>
<geom name="4a_butt" fromto="-.02 -.07 0 -.02 .07 0" size=".09"/>
<body name="4a_right_thigh" pos="0 -.1 -.04">
<joint name="4a_right_hip_x" axis="1 0 0" range="-25 5" class="big_joint"/>
<joint name="4a_right_hip_z" axis="0 0 1" range="-60 35" class="big_joint"/>
<joint name="4a_right_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
<geom name="4a_right_thigh" fromto="0 0 0 0 .01 -.34" size=".06"/>
<body name="4a_right_shin" pos="0 .01 -.403">
<joint name="4a_right_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
<geom name="4a_right_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
<body name="4a_right_foot" pos="0 0 -.39">
<joint name="4a_right_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
<joint name="4a_right_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
<geom name="4a_right_right_foot" fromto="-.07 -.02 0 .14 -.04 0" size=".027"/>
<geom name="4a_left_right_foot" fromto="-.07 0 0 .14 .02 0" size=".027"/>
</body>
</body>
</body>
<body name="4a_left_thigh" pos="0 .1 -.04">
<joint name="4a_left_hip_x" axis="-1 0 0" range="-25 5" class="big_joint"/>
<joint name="4a_left_hip_z" axis="0 0 -1" range="-60 35" class="big_joint"/>
<joint name="4a_left_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
<geom name="4a_left_thigh" fromto="0 0 0 0 -.01 -.34" size=".06"/>
<body name="4a_left_shin" pos="0 -.01 -.403">
<joint name="4a_left_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
<geom name="4a_left_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
<body name="4a_left_foot" pos="0 0 -.39">
<joint name="4a_left_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
<joint name="4a_left_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
<geom name="4a_left_left_foot" fromto="-.07 .02 0 .14 .04 0" size=".027"/>
<geom name="4a_right_left_foot" fromto="-.07 0 0 .14 -.02 0" size=".027"/>
</body>
</body>
</body>
</body>
</body>
<body name="4a_right_upper_arm" pos="0 -.17 .06">
<joint name="4a_right_shoulder1" axis="2 1 1" range="-85 60"/>
<joint name="4a_right_shoulder2" axis="0 -1 1" range="-85 60"/>
<geom name="4a_right_upper_arm" fromto="0 0 0 .16 -.16 -.16" size=".04 .16"/>
<body name="4a_right_lower_arm" pos=".18 -.18 -.18">
<joint name="4a_right_elbow" axis="0 -1 1" range="-90 50" stiffness="0"/>
<geom name="4a_right_lower_arm" fromto=".01 .01 .01 .17 .17 .17" size=".031"/>
<body name="4a_right_hand" pos=".18 .18 .18">
<geom name="4a_right_hand" type="sphere" size=".04" zaxis="1 1 1"/>
</body>
</body>
</body>
<body name="4a_left_upper_arm" pos="0 .17 .06">
<joint name="4a_left_shoulder1" axis="2 -1 1" range="-60 85"/>
<joint name="4a_left_shoulder2" axis="0 1 1" range="-60 85"/>
<geom name="4a_left_upper_arm" fromto="0 0 0 .16 .16 -.16" size=".04 .16"/>
<body name="4a_left_lower_arm" pos=".18 .18 -.18">
<joint name="4a_left_elbow" axis="0 -1 -1" range="-90 50" stiffness="0"/>
<geom name="4a_left_lower_arm" fromto=".01 -.01 .01 .17 -.17 .17" size=".031"/>
<body name="4a_left_hand" pos=".18 -.18 .18">
<geom name="4a_left_hand" type="sphere" size=".04" zaxis="1 -1 1"/>
</body>
</body>
</body>
</body>
<body name="5a_torso" pos="-1 -2 1.5" childclass="body5">
<camera name="5a_back" pos="-3 0 1" xyaxes="0 -1 0 1 0 2" mode="trackcom"/>
<camera name="5a_side" pos="0 -3 1" xyaxes="1 0 0 0 1 2" mode="trackcom"/>
<freejoint name="5a_root"/>
<geom name="5a_torso" fromto="0 -.07 0 0 .07 0" size=".07"/>
<geom name="5a_upper_waist" fromto="-.01 -.06 -.12 -.01 .06 -.12" size=".06"/>
<body name="5a_head" pos="0 0 .19">
<geom name="5a_head" type="sphere" size=".09"/>
<camera name="5a_egocentric" pos=".09 0 0" xyaxes="0 -1 0 .1 0 1" fovy="80"/>
</body>
<body name="5a_lower_waist" pos="-.01 0 -.26">
<geom name="5a_lower_waist" fromto="0 -.06 0 0 .06 0" size=".06"/>
<joint name="5a_abdomen_z" pos="0 0 .065" axis="0 0 1" range="-45 45" class="big_stiff_joint"/>
<joint name="5a_abdomen_y" pos="0 0 .065" axis="0 1 0" range="-75 30" class="big_joint"/>
<body name="5a_pelvis" pos="0 0 -.165">
<joint name="5a_abdomen_x" pos="0 0 .1" axis="1 0 0" range="-35 35" class="big_joint"/>
<geom name="5a_butt" fromto="-.02 -.07 0 -.02 .07 0" size=".09"/>
<body name="5a_right_thigh" pos="0 -.1 -.04">
<joint name="5a_right_hip_x" axis="1 0 0" range="-25 5" class="big_joint"/>
<joint name="5a_right_hip_z" axis="0 0 1" range="-60 35" class="big_joint"/>
<joint name="5a_right_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
<geom name="5a_right_thigh" fromto="0 0 0 0 .01 -.34" size=".06"/>
<body name="5a_right_shin" pos="0 .01 -.403">
<joint name="5a_right_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
<geom name="5a_right_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
<body name="5a_right_foot" pos="0 0 -.39">
<joint name="5a_right_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
<joint name="5a_right_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
<geom name="5a_right_right_foot" fromto="-.07 -.02 0 .14 -.04 0" size=".027"/>
<geom name="5a_left_right_foot" fromto="-.07 0 0 .14 .02 0" size=".027"/>
</body>
</body>
</body>
<body name="5a_left_thigh" pos="0 .1 -.04">
<joint name="5a_left_hip_x" axis="-1 0 0" range="-25 5" class="big_joint"/>
<joint name="5a_left_hip_z" axis="0 0 -1" range="-60 35" class="big_joint"/>
<joint name="5a_left_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
<geom name="5a_left_thigh" fromto="0 0 0 0 -.01 -.34" size=".06"/>
<body name="5a_left_shin" pos="0 -.01 -.403">
<joint name="5a_left_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
<geom name="5a_left_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
<body name="5a_left_foot" pos="0 0 -.39">
<joint name="5a_left_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
<joint name="5a_left_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
<geom name="5a_left_left_foot" fromto="-.07 .02 0 .14 .04 0" size=".027"/>
<geom name="5a_right_left_foot" fromto="-.07 0 0 .14 -.02 0" size=".027"/>
</body>
</body>
</body>
</body>
</body>
<body name="5a_right_upper_arm" pos="0 -.17 .06">
<joint name="5a_right_shoulder1" axis="2 1 1" range="-85 60"/>
<joint name="5a_right_shoulder2" axis="0 -1 1" range="-85 60"/>
<geom name="5a_right_upper_arm" fromto="0 0 0 .16 -.16 -.16" size=".04 .16"/>
<body name="5a_right_lower_arm" pos=".18 -.18 -.18">
<joint name="5a_right_elbow" axis="0 -1 1" range="-90 50" stiffness="0"/>
<geom name="5a_right_lower_arm" fromto=".01 .01 .01 .17 .17 .17" size=".031"/>
<body name="5a_right_hand" pos=".18 .18 .18">
<geom name="5a_right_hand" type="sphere" size=".04" zaxis="1 1 1"/>
</body>
</body>
</body>
<body name="5a_left_upper_arm" pos="0 .17 .06">
<joint name="5a_left_shoulder1" axis="2 -1 1" range="-60 85"/>
<joint name="5a_left_shoulder2" axis="0 1 1" range="-60 85"/>
<geom name="5a_left_upper_arm" fromto="0 0 0 .16 .16 -.16" size=".04 .16"/>
<body name="5a_left_lower_arm" pos=".18 .18 -.18">
<joint name="5a_left_elbow" axis="0 -1 -1" range="-90 50" stiffness="0"/>
<geom name="5a_left_lower_arm" fromto=".01 -.01 .01 .17 -.17 .17" size=".031"/>
<body name="5a_left_hand" pos=".18 -.18 .18">
<geom name="5a_left_hand" type="sphere" size=".04" zaxis="1 -1 1"/>
</body>
</body>
</body>
</body>
</worldbody>
<actuator>
<motor gear="40" joint="1a_abdomen_y"/>
<motor gear="40" joint="1a_abdomen_z"/>
<motor gear="40" joint="1a_abdomen_x"/>
<motor gear="40" joint="1a_right_hip_x"/>
<motor gear="40" joint="1a_right_hip_z"/>
<motor gear="120" joint="1a_right_hip_y"/>
<motor gear="80" joint="1a_right_knee"/>
<motor gear="20" joint="1a_right_ankle_x"/>
<motor gear="20" joint="1a_right_ankle_y"/>
<motor gear="40" joint="1a_left_hip_x"/>
<motor gear="40" joint="1a_left_hip_z"/>
<motor gear="120" joint="1a_left_hip_y"/>
<motor gear="80" joint="1a_left_knee"/>
<motor gear="20" joint="1a_left_ankle_x"/>
<motor gear="20" joint="1a_left_ankle_y"/>
<motor gear="20" joint="1a_right_shoulder1"/>
<motor gear="20" joint="1a_right_shoulder2"/>
<motor gear="40" joint="1a_right_elbow"/>
<motor gear="20" joint="1a_left_shoulder1"/>
<motor gear="20" joint="1a_left_shoulder2"/>
<motor gear="40" joint="1a_left_elbow"/>
</actuator>
<actuator>
<motor gear="40" joint="2a_abdomen_y"/>
<motor gear="40" joint="2a_abdomen_z"/>
<motor gear="40" joint="2a_abdomen_x"/>
<motor gear="40" joint="2a_right_hip_x"/>
<motor gear="40" joint="2a_right_hip_z"/>
<motor gear="120" joint="2a_right_hip_y"/>
<motor gear="80" joint="2a_right_knee"/>
<motor gear="20" joint="2a_right_ankle_x"/>
<motor gear="20" joint="2a_right_ankle_y"/>
<motor gear="40" joint="2a_left_hip_x"/>
<motor gear="40" joint="2a_left_hip_z"/>
<motor gear="120" joint="2a_left_hip_y"/>
<motor gear="80" joint="2a_left_knee"/>
<motor gear="20" joint="2a_left_ankle_x"/>
<motor gear="20" joint="2a_left_ankle_y"/>
<motor gear="20" joint="2a_right_shoulder1"/>
<motor gear="20" joint="2a_right_shoulder2"/>
<motor gear="40" joint="2a_right_elbow"/>
<motor gear="20" joint="2a_left_shoulder1"/>
<motor gear="20" joint="2a_left_shoulder2"/>
<motor gear="40" joint="2a_left_elbow"/>
</actuator>
<actuator>
<motor gear="40" joint="3a_abdomen_y"/>
<motor gear="40" joint="3a_abdomen_z"/>
<motor gear="40" joint="3a_abdomen_x"/>
<motor gear="40" joint="3a_right_hip_x"/>
<motor gear="40" joint="3a_right_hip_z"/>
<motor gear="120" joint="3a_right_hip_y"/>
<motor gear="80" joint="3a_right_knee"/>
<motor gear="20" joint="3a_right_ankle_x"/>
<motor gear="20" joint="3a_right_ankle_y"/>
<motor gear="40" joint="3a_left_hip_x"/>
<motor gear="40" joint="3a_left_hip_z"/>
<motor gear="120" joint="3a_left_hip_y"/>
<motor gear="80" joint="3a_left_knee"/>
<motor gear="20" joint="3a_left_ankle_x"/>
<motor gear="20" joint="3a_left_ankle_y"/>
<motor gear="20" joint="3a_right_shoulder1"/>
<motor gear="20" joint="3a_right_shoulder2"/>
<motor gear="40" joint="3a_right_elbow"/>
<motor gear="20" joint="3a_left_shoulder1"/>
<motor gear="20" joint="3a_left_shoulder2"/>
<motor gear="40" joint="3a_left_elbow"/>
</actuator>
<actuator>
<motor gear="40" joint="4a_abdomen_y"/>
<motor gear="40" joint="4a_abdomen_z"/>
<motor gear="40" joint="4a_abdomen_x"/>
<motor gear="40" joint="4a_right_hip_x"/>
<motor gear="40" joint="4a_right_hip_z"/>
<motor gear="120" joint="4a_right_hip_y"/>
<motor gear="80" joint="4a_right_knee"/>
<motor gear="20" joint="4a_right_ankle_x"/>
<motor gear="20" joint="4a_right_ankle_y"/>
<motor gear="40" joint="4a_left_hip_x"/>
<motor gear="40" joint="4a_left_hip_z"/>
<motor gear="120" joint="4a_left_hip_y"/>
<motor gear="80" joint="4a_left_knee"/>
<motor gear="20" joint="4a_left_ankle_x"/>
<motor gear="20" joint="4a_left_ankle_y"/>
<motor gear="20" joint="4a_right_shoulder1"/>
<motor gear="20" joint="4a_right_shoulder2"/>
<motor gear="40" joint="4a_right_elbow"/>
<motor gear="20" joint="4a_left_shoulder1"/>
<motor gear="20" joint="4a_left_shoulder2"/>
<motor gear="40" joint="4a_left_elbow"/>
</actuator>
<actuator>
<motor gear="40" joint="5a_abdomen_y"/>
<motor gear="40" joint="5a_abdomen_z"/>
<motor gear="40" joint="5a_abdomen_x"/>
<motor gear="40" joint="5a_right_hip_x"/>
<motor gear="40" joint="5a_right_hip_z"/>
<motor gear="120" joint="5a_right_hip_y"/>
<motor gear="80" joint="5a_right_knee"/>
<motor gear="20" joint="5a_right_ankle_x"/>
<motor gear="20" joint="5a_right_ankle_y"/>
<motor gear="40" joint="5a_left_hip_x"/>
<motor gear="40" joint="5a_left_hip_z"/>
<motor gear="120" joint="5a_left_hip_y"/>
<motor gear="80" joint="5a_left_knee"/>
<motor gear="20" joint="5a_left_ankle_x"/>
<motor gear="20" joint="5a_left_ankle_y"/>
<motor gear="20" joint="5a_right_shoulder1"/>
<motor gear="20" joint="5a_right_shoulder2"/>
<motor gear="40" joint="5a_right_elbow"/>
<motor gear="20" joint="5a_left_shoulder1"/>
<motor gear="20" joint="5a_left_shoulder2"/>
<motor gear="40" joint="5a_left_elbow"/>
</actuator>
</mujoco>
mjx/mujoco/mjx/test_data/humanoid/06_humanoids.xml
+671
View File
@@ -0,0 +1,671 @@
<mujoco model="06 Humanoids">
<option timestep="0.005" solver="Newton" iterations="1" ls_iterations="4">
<flag eulerdamp="disable"/>
</option>
<custom>
<numeric data="24" name="max_contact_points"/>
<numeric data="24" name="max_geom_pairs"/>
</custom>
<size memory="100M"/>
<asset>
<texture type="skybox" builtin="gradient" rgb1=".3 .5 .7" rgb2="0 0 0" width="512" height="512"/>
<texture name="body" type="cube" builtin="flat" mark="cross" width="128" height="128"
rgb1="0.8 0.6 0.4" rgb2="0.8 0.6 0.4" markrgb="1 1 1" random="0.01"/>
<material name="body" texture="body" texuniform="true" rgba="0.8 0.6 .4 1"/>
<texture name="grid" type="2d" builtin="checker" width="512" height="512" rgb1=".1 .2 .3" rgb2=".2 .3 .4"/>
<material name="grid" texture="grid" texrepeat="1 1" texuniform="true" reflectance=".2"/>
</asset>
<default>
<motor ctrlrange="-1 1" ctrllimited="true"/>
<joint type="hinge" damping=".2" stiffness="1" armature=".01" limited="true" solimplimit="0 .99 .01"/>
<default class="big_joint">
<joint damping="5" stiffness="10"/>
<default class="big_stiff_joint">
<joint stiffness="20"/>
</default>
</default>
<default class="body1">
<geom type="capsule" condim="3" friction=".7" solimp=".9 .99 .003" solref=".015 1" material="body" contype="2" conaffinity="125"/>
</default>
<default class="body2">
<geom type="capsule" condim="3" friction=".7" solimp=".9 .99 .003" solref=".015 1" material="body" contype="4" conaffinity="123"/>
</default>
<default class="body3">
<geom type="capsule" condim="3" friction=".7" solimp=".9 .99 .003" solref=".015 1" material="body" contype="8" conaffinity="119"/>
</default>
<default class="body4">
<geom type="capsule" condim="3" friction=".7" solimp=".9 .99 .003" solref=".015 1" material="body" contype="16" conaffinity="111"/>
</default>
<default class="body5">
<geom type="capsule" condim="3" friction=".7" solimp=".9 .99 .003" solref=".015 1" material="body" contype="32" conaffinity="95"/>
</default>
<default class="body6">
<geom type="capsule" condim="3" friction=".7" solimp=".9 .99 .003" solref=".015 1" material="body" contype="64" conaffinity="63"/>
</default>
</default>
<visual>
<map force="0.1" zfar="30"/>
<rgba haze="0.15 0.25 0.35 1"/>
<quality shadowsize="4096"/>
<global offwidth="800" offheight="800"/>
</visual>
<worldbody>
<geom size="10 10 .05" type="plane" material="grid" condim="3"/>
<light dir=".2 1 -.4" diffuse=".8 .8 .8" specular="0.3 0.3 0.3" pos="-2 -10 4" cutoff="35"/>
<light dir="-.2 1 -.4" diffuse=".8 .8 .8" specular="0.3 0.3 0.3" pos="2 -10 4" cutoff="35"/>
<body name="1a_torso" pos="-1 0 1.5" childclass="body1">
<camera name="1a_back" pos="-3 0 1" xyaxes="0 -1 0 1 0 2" mode="trackcom"/>
<camera name="1a_side" pos="0 -3 1" xyaxes="1 0 0 0 1 2" mode="trackcom"/>
<freejoint name="1a_root"/>
<geom name="1a_torso" fromto="0 -.07 0 0 .07 0" size=".07"/>
<geom name="1a_upper_waist" fromto="-.01 -.06 -.12 -.01 .06 -.12" size=".06"/>
<body name="1a_head" pos="0 0 .19">
<geom name="1a_head" type="sphere" size=".09"/>
<camera name="1a_egocentric" pos=".09 0 0" xyaxes="0 -1 0 .1 0 1" fovy="80"/>
</body>
<body name="1a_lower_waist" pos="-.01 0 -.26">
<geom name="1a_lower_waist" fromto="0 -.06 0 0 .06 0" size=".06"/>
<joint name="1a_abdomen_z" pos="0 0 .065" axis="0 0 1" range="-45 45" class="big_stiff_joint"/>
<joint name="1a_abdomen_y" pos="0 0 .065" axis="0 1 0" range="-75 30" class="big_joint"/>
<body name="1a_pelvis" pos="0 0 -.165">
<joint name="1a_abdomen_x" pos="0 0 .1" axis="1 0 0" range="-35 35" class="big_joint"/>
<geom name="1a_butt" fromto="-.02 -.07 0 -.02 .07 0" size=".09"/>
<body name="1a_right_thigh" pos="0 -.1 -.04">
<joint name="1a_right_hip_x" axis="1 0 0" range="-25 5" class="big_joint"/>
<joint name="1a_right_hip_z" axis="0 0 1" range="-60 35" class="big_joint"/>
<joint name="1a_right_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
<geom name="1a_right_thigh" fromto="0 0 0 0 .01 -.34" size=".06"/>
<body name="1a_right_shin" pos="0 .01 -.403">
<joint name="1a_right_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
<geom name="1a_right_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
<body name="1a_right_foot" pos="0 0 -.39">
<joint name="1a_right_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
<joint name="1a_right_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
<geom name="1a_right_right_foot" fromto="-.07 -.02 0 .14 -.04 0" size=".027"/>
<geom name="1a_left_right_foot" fromto="-.07 0 0 .14 .02 0" size=".027"/>
</body>
</body>
</body>
<body name="1a_left_thigh" pos="0 .1 -.04">
<joint name="1a_left_hip_x" axis="-1 0 0" range="-25 5" class="big_joint"/>
<joint name="1a_left_hip_z" axis="0 0 -1" range="-60 35" class="big_joint"/>
<joint name="1a_left_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
<geom name="1a_left_thigh" fromto="0 0 0 0 -.01 -.34" size=".06"/>
<body name="1a_left_shin" pos="0 -.01 -.403">
<joint name="1a_left_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
<geom name="1a_left_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
<body name="1a_left_foot" pos="0 0 -.39">
<joint name="1a_left_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
<joint name="1a_left_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
<geom name="1a_left_left_foot" fromto="-.07 .02 0 .14 .04 0" size=".027"/>
<geom name="1a_right_left_foot" fromto="-.07 0 0 .14 -.02 0" size=".027"/>
</body>
</body>
</body>
</body>
</body>
<body name="1a_right_upper_arm" pos="0 -.17 .06">
<joint name="1a_right_shoulder1" axis="2 1 1" range="-85 60"/>
<joint name="1a_right_shoulder2" axis="0 -1 1" range="-85 60"/>
<geom name="1a_right_upper_arm" fromto="0 0 0 .16 -.16 -.16" size=".04 .16"/>
<body name="1a_right_lower_arm" pos=".18 -.18 -.18">
<joint name="1a_right_elbow" axis="0 -1 1" range="-90 50" stiffness="0"/>
<geom name="1a_right_lower_arm" fromto=".01 .01 .01 .17 .17 .17" size=".031"/>
<body name="1a_right_hand" pos=".18 .18 .18">
<geom name="1a_right_hand" type="sphere" size=".04" zaxis="1 1 1"/>
</body>
</body>
</body>
<body name="1a_left_upper_arm" pos="0 .17 .06">
<joint name="1a_left_shoulder1" axis="2 -1 1" range="-60 85"/>
<joint name="1a_left_shoulder2" axis="0 1 1" range="-60 85"/>
<geom name="1a_left_upper_arm" fromto="0 0 0 .16 .16 -.16" size=".04 .16"/>
<body name="1a_left_lower_arm" pos=".18 .18 -.18">
<joint name="1a_left_elbow" axis="0 -1 -1" range="-90 50" stiffness="0"/>
<geom name="1a_left_lower_arm" fromto=".01 -.01 .01 .17 -.17 .17" size=".031"/>
<body name="1a_left_hand" pos=".18 -.18 .18">
<geom name="1a_left_hand" type="sphere" size=".04" zaxis="1 -1 1"/>
</body>
</body>
</body>
</body>
<body name="2a_torso" pos="-1 1 1.5" childclass="body2">
<camera name="2a_back" pos="-3 0 1" xyaxes="0 -1 0 1 0 2" mode="trackcom"/>
<camera name="2a_side" pos="0 -3 1" xyaxes="1 0 0 0 1 2" mode="trackcom"/>
<freejoint name="2a_root"/>
<geom name="2a_torso" fromto="0 -.07 0 0 .07 0" size=".07"/>
<geom name="2a_upper_waist" fromto="-.01 -.06 -.12 -.01 .06 -.12" size=".06"/>
<body name="2a_head" pos="0 0 .19">
<geom name="2a_head" type="sphere" size=".09"/>
<camera name="2a_egocentric" pos=".09 0 0" xyaxes="0 -1 0 .1 0 1" fovy="80"/>
</body>
<body name="2a_lower_waist" pos="-.01 0 -.26">
<geom name="2a_lower_waist" fromto="0 -.06 0 0 .06 0" size=".06"/>
<joint name="2a_abdomen_z" pos="0 0 .065" axis="0 0 1" range="-45 45" class="big_stiff_joint"/>
<joint name="2a_abdomen_y" pos="0 0 .065" axis="0 1 0" range="-75 30" class="big_joint"/>
<body name="2a_pelvis" pos="0 0 -.165">
<joint name="2a_abdomen_x" pos="0 0 .1" axis="1 0 0" range="-35 35" class="big_joint"/>
<geom name="2a_butt" fromto="-.02 -.07 0 -.02 .07 0" size=".09"/>
<body name="2a_right_thigh" pos="0 -.1 -.04">
<joint name="2a_right_hip_x" axis="1 0 0" range="-25 5" class="big_joint"/>
<joint name="2a_right_hip_z" axis="0 0 1" range="-60 35" class="big_joint"/>
<joint name="2a_right_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
<geom name="2a_right_thigh" fromto="0 0 0 0 .01 -.34" size=".06"/>
<body name="2a_right_shin" pos="0 .01 -.403">
<joint name="2a_right_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
<geom name="2a_right_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
<body name="2a_right_foot" pos="0 0 -.39">
<joint name="2a_right_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
<joint name="2a_right_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
<geom name="2a_right_right_foot" fromto="-.07 -.02 0 .14 -.04 0" size=".027"/>
<geom name="2a_left_right_foot" fromto="-.07 0 0 .14 .02 0" size=".027"/>
</body>
</body>
</body>
<body name="2a_left_thigh" pos="0 .1 -.04">
<joint name="2a_left_hip_x" axis="-1 0 0" range="-25 5" class="big_joint"/>
<joint name="2a_left_hip_z" axis="0 0 -1" range="-60 35" class="big_joint"/>
<joint name="2a_left_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
<geom name="2a_left_thigh" fromto="0 0 0 0 -.01 -.34" size=".06"/>
<body name="2a_left_shin" pos="0 -.01 -.403">
<joint name="2a_left_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
<geom name="2a_left_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
<body name="2a_left_foot" pos="0 0 -.39">
<joint name="2a_left_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
<joint name="2a_left_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
<geom name="2a_left_left_foot" fromto="-.07 .02 0 .14 .04 0" size=".027"/>
<geom name="2a_right_left_foot" fromto="-.07 0 0 .14 -.02 0" size=".027"/>
</body>
</body>
</body>
</body>
</body>
<body name="2a_right_upper_arm" pos="0 -.17 .06">
<joint name="2a_right_shoulder1" axis="2 1 1" range="-85 60"/>
<joint name="2a_right_shoulder2" axis="0 -1 1" range="-85 60"/>
<geom name="2a_right_upper_arm" fromto="0 0 0 .16 -.16 -.16" size=".04 .16"/>
<body name="2a_right_lower_arm" pos=".18 -.18 -.18">
<joint name="2a_right_elbow" axis="0 -1 1" range="-90 50" stiffness="0"/>
<geom name="2a_right_lower_arm" fromto=".01 .01 .01 .17 .17 .17" size=".031"/>
<body name="2a_right_hand" pos=".18 .18 .18">
<geom name="2a_right_hand" type="sphere" size=".04" zaxis="1 1 1"/>
</body>
</body>
</body>
<body name="2a_left_upper_arm" pos="0 .17 .06">
<joint name="2a_left_shoulder1" axis="2 -1 1" range="-60 85"/>
<joint name="2a_left_shoulder2" axis="0 1 1" range="-60 85"/>
<geom name="2a_left_upper_arm" fromto="0 0 0 .16 .16 -.16" size=".04 .16"/>
<body name="2a_left_lower_arm" pos=".18 .18 -.18">
<joint name="2a_left_elbow" axis="0 -1 -1" range="-90 50" stiffness="0"/>
<geom name="2a_left_lower_arm" fromto=".01 -.01 .01 .17 -.17 .17" size=".031"/>
<body name="2a_left_hand" pos=".18 -.18 .18">
<geom name="2a_left_hand" type="sphere" size=".04" zaxis="1 -1 1"/>
</body>
</body>
</body>
</body>
<body name="3a_torso" pos="-1 -1 1.5" childclass="body3">
<camera name="3a_back" pos="-3 0 1" xyaxes="0 -1 0 1 0 2" mode="trackcom"/>
<camera name="3a_side" pos="0 -3 1" xyaxes="1 0 0 0 1 2" mode="trackcom"/>
<freejoint name="3a_root"/>
<geom name="3a_torso" fromto="0 -.07 0 0 .07 0" size=".07"/>
<geom name="3a_upper_waist" fromto="-.01 -.06 -.12 -.01 .06 -.12" size=".06"/>
<body name="3a_head" pos="0 0 .19">
<geom name="3a_head" type="sphere" size=".09"/>
<camera name="3a_egocentric" pos=".09 0 0" xyaxes="0 -1 0 .1 0 1" fovy="80"/>
</body>
<body name="3a_lower_waist" pos="-.01 0 -.26">
<geom name="3a_lower_waist" fromto="0 -.06 0 0 .06 0" size=".06"/>
<joint name="3a_abdomen_z" pos="0 0 .065" axis="0 0 1" range="-45 45" class="big_stiff_joint"/>
<joint name="3a_abdomen_y" pos="0 0 .065" axis="0 1 0" range="-75 30" class="big_joint"/>
<body name="3a_pelvis" pos="0 0 -.165">
<joint name="3a_abdomen_x" pos="0 0 .1" axis="1 0 0" range="-35 35" class="big_joint"/>
<geom name="3a_butt" fromto="-.02 -.07 0 -.02 .07 0" size=".09"/>
<body name="3a_right_thigh" pos="0 -.1 -.04">
<joint name="3a_right_hip_x" axis="1 0 0" range="-25 5" class="big_joint"/>
<joint name="3a_right_hip_z" axis="0 0 1" range="-60 35" class="big_joint"/>
<joint name="3a_right_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
<geom name="3a_right_thigh" fromto="0 0 0 0 .01 -.34" size=".06"/>
<body name="3a_right_shin" pos="0 .01 -.403">
<joint name="3a_right_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
<geom name="3a_right_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
<body name="3a_right_foot" pos="0 0 -.39">
<joint name="3a_right_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
<joint name="3a_right_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
<geom name="3a_right_right_foot" fromto="-.07 -.02 0 .14 -.04 0" size=".027"/>
<geom name="3a_left_right_foot" fromto="-.07 0 0 .14 .02 0" size=".027"/>
</body>
</body>
</body>
<body name="3a_left_thigh" pos="0 .1 -.04">
<joint name="3a_left_hip_x" axis="-1 0 0" range="-25 5" class="big_joint"/>
<joint name="3a_left_hip_z" axis="0 0 -1" range="-60 35" class="big_joint"/>
<joint name="3a_left_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
<geom name="3a_left_thigh" fromto="0 0 0 0 -.01 -.34" size=".06"/>
<body name="3a_left_shin" pos="0 -.01 -.403">
<joint name="3a_left_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
<geom name="3a_left_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
<body name="3a_left_foot" pos="0 0 -.39">
<joint name="3a_left_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
<joint name="3a_left_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
<geom name="3a_left_left_foot" fromto="-.07 .02 0 .14 .04 0" size=".027"/>
<geom name="3a_right_left_foot" fromto="-.07 0 0 .14 -.02 0" size=".027"/>
</body>
</body>
</body>
</body>
</body>
<body name="3a_right_upper_arm" pos="0 -.17 .06">
<joint name="3a_right_shoulder1" axis="2 1 1" range="-85 60"/>
<joint name="3a_right_shoulder2" axis="0 -1 1" range="-85 60"/>
<geom name="3a_right_upper_arm" fromto="0 0 0 .16 -.16 -.16" size=".04 .16"/>
<body name="3a_right_lower_arm" pos=".18 -.18 -.18">
<joint name="3a_right_elbow" axis="0 -1 1" range="-90 50" stiffness="0"/>
<geom name="3a_right_lower_arm" fromto=".01 .01 .01 .17 .17 .17" size=".031"/>
<body name="3a_right_hand" pos=".18 .18 .18">
<geom name="3a_right_hand" type="sphere" size=".04" zaxis="1 1 1"/>
</body>
</body>
</body>
<body name="3a_left_upper_arm" pos="0 .17 .06">
<joint name="3a_left_shoulder1" axis="2 -1 1" range="-60 85"/>
<joint name="3a_left_shoulder2" axis="0 1 1" range="-60 85"/>
<geom name="3a_left_upper_arm" fromto="0 0 0 .16 .16 -.16" size=".04 .16"/>
<body name="3a_left_lower_arm" pos=".18 .18 -.18">
<joint name="3a_left_elbow" axis="0 -1 -1" range="-90 50" stiffness="0"/>
<geom name="3a_left_lower_arm" fromto=".01 -.01 .01 .17 -.17 .17" size=".031"/>
<body name="3a_left_hand" pos=".18 -.18 .18">
<geom name="3a_left_hand" type="sphere" size=".04" zaxis="1 -1 1"/>
</body>
</body>
</body>
</body>
<body name="4a_torso" pos="-1 2 1.5" childclass="body4">
<camera name="4a_back" pos="-3 0 1" xyaxes="0 -1 0 1 0 2" mode="trackcom"/>
<camera name="4a_side" pos="0 -3 1" xyaxes="1 0 0 0 1 2" mode="trackcom"/>
<freejoint name="4a_root"/>
<geom name="4a_torso" fromto="0 -.07 0 0 .07 0" size=".07"/>
<geom name="4a_upper_waist" fromto="-.01 -.06 -.12 -.01 .06 -.12" size=".06"/>
<body name="4a_head" pos="0 0 .19">
<geom name="4a_head" type="sphere" size=".09"/>
<camera name="4a_egocentric" pos=".09 0 0" xyaxes="0 -1 0 .1 0 1" fovy="80"/>
</body>
<body name="4a_lower_waist" pos="-.01 0 -.26">
<geom name="4a_lower_waist" fromto="0 -.06 0 0 .06 0" size=".06"/>
<joint name="4a_abdomen_z" pos="0 0 .065" axis="0 0 1" range="-45 45" class="big_stiff_joint"/>
<joint name="4a_abdomen_y" pos="0 0 .065" axis="0 1 0" range="-75 30" class="big_joint"/>
<body name="4a_pelvis" pos="0 0 -.165">
<joint name="4a_abdomen_x" pos="0 0 .1" axis="1 0 0" range="-35 35" class="big_joint"/>
<geom name="4a_butt" fromto="-.02 -.07 0 -.02 .07 0" size=".09"/>
<body name="4a_right_thigh" pos="0 -.1 -.04">
<joint name="4a_right_hip_x" axis="1 0 0" range="-25 5" class="big_joint"/>
<joint name="4a_right_hip_z" axis="0 0 1" range="-60 35" class="big_joint"/>
<joint name="4a_right_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
<geom name="4a_right_thigh" fromto="0 0 0 0 .01 -.34" size=".06"/>
<body name="4a_right_shin" pos="0 .01 -.403">
<joint name="4a_right_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
<geom name="4a_right_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
<body name="4a_right_foot" pos="0 0 -.39">
<joint name="4a_right_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
<joint name="4a_right_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
<geom name="4a_right_right_foot" fromto="-.07 -.02 0 .14 -.04 0" size=".027"/>
<geom name="4a_left_right_foot" fromto="-.07 0 0 .14 .02 0" size=".027"/>
</body>
</body>
</body>
<body name="4a_left_thigh" pos="0 .1 -.04">
<joint name="4a_left_hip_x" axis="-1 0 0" range="-25 5" class="big_joint"/>
<joint name="4a_left_hip_z" axis="0 0 -1" range="-60 35" class="big_joint"/>
<joint name="4a_left_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
<geom name="4a_left_thigh" fromto="0 0 0 0 -.01 -.34" size=".06"/>
<body name="4a_left_shin" pos="0 -.01 -.403">
<joint name="4a_left_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
<geom name="4a_left_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
<body name="4a_left_foot" pos="0 0 -.39">
<joint name="4a_left_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
<joint name="4a_left_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
<geom name="4a_left_left_foot" fromto="-.07 .02 0 .14 .04 0" size=".027"/>
<geom name="4a_right_left_foot" fromto="-.07 0 0 .14 -.02 0" size=".027"/>
</body>
</body>
</body>
</body>
</body>
<body name="4a_right_upper_arm" pos="0 -.17 .06">
<joint name="4a_right_shoulder1" axis="2 1 1" range="-85 60"/>
<joint name="4a_right_shoulder2" axis="0 -1 1" range="-85 60"/>
<geom name="4a_right_upper_arm" fromto="0 0 0 .16 -.16 -.16" size=".04 .16"/>
<body name="4a_right_lower_arm" pos=".18 -.18 -.18">
<joint name="4a_right_elbow" axis="0 -1 1" range="-90 50" stiffness="0"/>
<geom name="4a_right_lower_arm" fromto=".01 .01 .01 .17 .17 .17" size=".031"/>
<body name="4a_right_hand" pos=".18 .18 .18">
<geom name="4a_right_hand" type="sphere" size=".04" zaxis="1 1 1"/>
</body>
</body>
</body>
<body name="4a_left_upper_arm" pos="0 .17 .06">
<joint name="4a_left_shoulder1" axis="2 -1 1" range="-60 85"/>
<joint name="4a_left_shoulder2" axis="0 1 1" range="-60 85"/>
<geom name="4a_left_upper_arm" fromto="0 0 0 .16 .16 -.16" size=".04 .16"/>
<body name="4a_left_lower_arm" pos=".18 .18 -.18">
<joint name="4a_left_elbow" axis="0 -1 -1" range="-90 50" stiffness="0"/>
<geom name="4a_left_lower_arm" fromto=".01 -.01 .01 .17 -.17 .17" size=".031"/>
<body name="4a_left_hand" pos=".18 -.18 .18">
<geom name="4a_left_hand" type="sphere" size=".04" zaxis="1 -1 1"/>
</body>
</body>
</body>
</body>
<body name="5a_torso" pos="-1 -2 1.5" childclass="body5">
<camera name="5a_back" pos="-3 0 1" xyaxes="0 -1 0 1 0 2" mode="trackcom"/>
<camera name="5a_side" pos="0 -3 1" xyaxes="1 0 0 0 1 2" mode="trackcom"/>
<freejoint name="5a_root"/>
<geom name="5a_torso" fromto="0 -.07 0 0 .07 0" size=".07"/>
<geom name="5a_upper_waist" fromto="-.01 -.06 -.12 -.01 .06 -.12" size=".06"/>
<body name="5a_head" pos="0 0 .19">
<geom name="5a_head" type="sphere" size=".09"/>
<camera name="5a_egocentric" pos=".09 0 0" xyaxes="0 -1 0 .1 0 1" fovy="80"/>
</body>
<body name="5a_lower_waist" pos="-.01 0 -.26">
<geom name="5a_lower_waist" fromto="0 -.06 0 0 .06 0" size=".06"/>
<joint name="5a_abdomen_z" pos="0 0 .065" axis="0 0 1" range="-45 45" class="big_stiff_joint"/>
<joint name="5a_abdomen_y" pos="0 0 .065" axis="0 1 0" range="-75 30" class="big_joint"/>
<body name="5a_pelvis" pos="0 0 -.165">
<joint name="5a_abdomen_x" pos="0 0 .1" axis="1 0 0" range="-35 35" class="big_joint"/>
<geom name="5a_butt" fromto="-.02 -.07 0 -.02 .07 0" size=".09"/>
<body name="5a_right_thigh" pos="0 -.1 -.04">
<joint name="5a_right_hip_x" axis="1 0 0" range="-25 5" class="big_joint"/>
<joint name="5a_right_hip_z" axis="0 0 1" range="-60 35" class="big_joint"/>
<joint name="5a_right_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
<geom name="5a_right_thigh" fromto="0 0 0 0 .01 -.34" size=".06"/>
<body name="5a_right_shin" pos="0 .01 -.403">
<joint name="5a_right_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
<geom name="5a_right_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
<body name="5a_right_foot" pos="0 0 -.39">
<joint name="5a_right_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
<joint name="5a_right_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
<geom name="5a_right_right_foot" fromto="-.07 -.02 0 .14 -.04 0" size=".027"/>
<geom name="5a_left_right_foot" fromto="-.07 0 0 .14 .02 0" size=".027"/>
</body>
</body>
</body>
<body name="5a_left_thigh" pos="0 .1 -.04">
<joint name="5a_left_hip_x" axis="-1 0 0" range="-25 5" class="big_joint"/>
<joint name="5a_left_hip_z" axis="0 0 -1" range="-60 35" class="big_joint"/>
<joint name="5a_left_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
<geom name="5a_left_thigh" fromto="0 0 0 0 -.01 -.34" size=".06"/>
<body name="5a_left_shin" pos="0 -.01 -.403">
<joint name="5a_left_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
<geom name="5a_left_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
<body name="5a_left_foot" pos="0 0 -.39">
<joint name="5a_left_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
<joint name="5a_left_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
<geom name="5a_left_left_foot" fromto="-.07 .02 0 .14 .04 0" size=".027"/>
<geom name="5a_right_left_foot" fromto="-.07 0 0 .14 -.02 0" size=".027"/>
</body>
</body>
</body>
</body>
</body>
<body name="5a_right_upper_arm" pos="0 -.17 .06">
<joint name="5a_right_shoulder1" axis="2 1 1" range="-85 60"/>
<joint name="5a_right_shoulder2" axis="0 -1 1" range="-85 60"/>
<geom name="5a_right_upper_arm" fromto="0 0 0 .16 -.16 -.16" size=".04 .16"/>
<body name="5a_right_lower_arm" pos=".18 -.18 -.18">
<joint name="5a_right_elbow" axis="0 -1 1" range="-90 50" stiffness="0"/>
<geom name="5a_right_lower_arm" fromto=".01 .01 .01 .17 .17 .17" size=".031"/>
<body name="5a_right_hand" pos=".18 .18 .18">
<geom name="5a_right_hand" type="sphere" size=".04" zaxis="1 1 1"/>
</body>
</body>
</body>
<body name="5a_left_upper_arm" pos="0 .17 .06">
<joint name="5a_left_shoulder1" axis="2 -1 1" range="-60 85"/>
<joint name="5a_left_shoulder2" axis="0 1 1" range="-60 85"/>
<geom name="5a_left_upper_arm" fromto="0 0 0 .16 .16 -.16" size=".04 .16"/>
<body name="5a_left_lower_arm" pos=".18 .18 -.18">
<joint name="5a_left_elbow" axis="0 -1 -1" range="-90 50" stiffness="0"/>
<geom name="5a_left_lower_arm" fromto=".01 -.01 .01 .17 -.17 .17" size=".031"/>
<body name="5a_left_hand" pos=".18 -.18 .18">
<geom name="5a_left_hand" type="sphere" size=".04" zaxis="1 -1 1"/>
</body>
</body>
</body>
</body>
<body name="6a_torso" pos="-1 3 1.5" childclass="body6">
<camera name="6a_back" pos="-3 0 1" xyaxes="0 -1 0 1 0 2" mode="trackcom"/>
<camera name="6a_side" pos="0 -3 1" xyaxes="1 0 0 0 1 2" mode="trackcom"/>
<freejoint name="6a_root"/>
<geom name="6a_torso" fromto="0 -.07 0 0 .07 0" size=".07"/>
<geom name="6a_upper_waist" fromto="-.01 -.06 -.12 -.01 .06 -.12" size=".06"/>
<body name="6a_head" pos="0 0 .19">
<geom name="6a_head" type="sphere" size=".09"/>
<camera name="6a_egocentric" pos=".09 0 0" xyaxes="0 -1 0 .1 0 1" fovy="80"/>
</body>
<body name="6a_lower_waist" pos="-.01 0 -.26">
<geom name="6a_lower_waist" fromto="0 -.06 0 0 .06 0" size=".06"/>
<joint name="6a_abdomen_z" pos="0 0 .065" axis="0 0 1" range="-45 45" class="big_stiff_joint"/>
<joint name="6a_abdomen_y" pos="0 0 .065" axis="0 1 0" range="-75 30" class="big_joint"/>
<body name="6a_pelvis" pos="0 0 -.165">
<joint name="6a_abdomen_x" pos="0 0 .1" axis="1 0 0" range="-35 35" class="big_joint"/>
<geom name="6a_butt" fromto="-.02 -.07 0 -.02 .07 0" size=".09"/>
<body name="6a_right_thigh" pos="0 -.1 -.04">
<joint name="6a_right_hip_x" axis="1 0 0" range="-25 5" class="big_joint"/>
<joint name="6a_right_hip_z" axis="0 0 1" range="-60 35" class="big_joint"/>
<joint name="6a_right_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
<geom name="6a_right_thigh" fromto="0 0 0 0 .01 -.34" size=".06"/>
<body name="6a_right_shin" pos="0 .01 -.403">
<joint name="6a_right_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
<geom name="6a_right_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
<body name="6a_right_foot" pos="0 0 -.39">
<joint name="6a_right_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
<joint name="6a_right_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
<geom name="6a_right_right_foot" fromto="-.07 -.02 0 .14 -.04 0" size=".027"/>
<geom name="6a_left_right_foot" fromto="-.07 0 0 .14 .02 0" size=".027"/>
</body>
</body>
</body>
<body name="6a_left_thigh" pos="0 .1 -.04">
<joint name="6a_left_hip_x" axis="-1 0 0" range="-25 5" class="big_joint"/>
<joint name="6a_left_hip_z" axis="0 0 -1" range="-60 35" class="big_joint"/>
<joint name="6a_left_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
<geom name="6a_left_thigh" fromto="0 0 0 0 -.01 -.34" size=".06"/>
<body name="6a_left_shin" pos="0 -.01 -.403">
<joint name="6a_left_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
<geom name="6a_left_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
<body name="6a_left_foot" pos="0 0 -.39">
<joint name="6a_left_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
<joint name="6a_left_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
<geom name="6a_left_left_foot" fromto="-.07 .02 0 .14 .04 0" size=".027"/>
<geom name="6a_right_left_foot" fromto="-.07 0 0 .14 -.02 0" size=".027"/>
</body>
</body>
</body>
</body>
</body>
<body name="6a_right_upper_arm" pos="0 -.17 .06">
<joint name="6a_right_shoulder1" axis="2 1 1" range="-85 60"/>
<joint name="6a_right_shoulder2" axis="0 -1 1" range="-85 60"/>
<geom name="6a_right_upper_arm" fromto="0 0 0 .16 -.16 -.16" size=".04 .16"/>
<body name="6a_right_lower_arm" pos=".18 -.18 -.18">
<joint name="6a_right_elbow" axis="0 -1 1" range="-90 50" stiffness="0"/>
<geom name="6a_right_lower_arm" fromto=".01 .01 .01 .17 .17 .17" size=".031"/>
<body name="6a_right_hand" pos=".18 .18 .18">
<geom name="6a_right_hand" type="sphere" size=".04" zaxis="1 1 1"/>
</body>
</body>
</body>
<body name="6a_left_upper_arm" pos="0 .17 .06">
<joint name="6a_left_shoulder1" axis="2 -1 1" range="-60 85"/>
<joint name="6a_left_shoulder2" axis="0 1 1" range="-60 85"/>
<geom name="6a_left_upper_arm" fromto="0 0 0 .16 .16 -.16" size=".04 .16"/>
<body name="6a_left_lower_arm" pos=".18 .18 -.18">
<joint name="6a_left_elbow" axis="0 -1 -1" range="-90 50" stiffness="0"/>
<geom name="6a_left_lower_arm" fromto=".01 -.01 .01 .17 -.17 .17" size=".031"/>
<body name="6a_left_hand" pos=".18 -.18 .18">
<geom name="6a_left_hand" type="sphere" size=".04" zaxis="1 -1 1"/>
</body>
</body>
</body>
</body>
</worldbody>
<actuator>
<motor gear="40" joint="1a_abdomen_y"/>
<motor gear="40" joint="1a_abdomen_z"/>
<motor gear="40" joint="1a_abdomen_x"/>
<motor gear="40" joint="1a_right_hip_x"/>
<motor gear="40" joint="1a_right_hip_z"/>
<motor gear="120" joint="1a_right_hip_y"/>
<motor gear="80" joint="1a_right_knee"/>
<motor gear="20" joint="1a_right_ankle_x"/>
<motor gear="20" joint="1a_right_ankle_y"/>
<motor gear="40" joint="1a_left_hip_x"/>
<motor gear="40" joint="1a_left_hip_z"/>
<motor gear="120" joint="1a_left_hip_y"/>
<motor gear="80" joint="1a_left_knee"/>
<motor gear="20" joint="1a_left_ankle_x"/>
<motor gear="20" joint="1a_left_ankle_y"/>
<motor gear="20" joint="1a_right_shoulder1"/>
<motor gear="20" joint="1a_right_shoulder2"/>
<motor gear="40" joint="1a_right_elbow"/>
<motor gear="20" joint="1a_left_shoulder1"/>
<motor gear="20" joint="1a_left_shoulder2"/>
<motor gear="40" joint="1a_left_elbow"/>
</actuator>
<actuator>
<motor gear="40" joint="2a_abdomen_y"/>
<motor gear="40" joint="2a_abdomen_z"/>
<motor gear="40" joint="2a_abdomen_x"/>
<motor gear="40" joint="2a_right_hip_x"/>
<motor gear="40" joint="2a_right_hip_z"/>
<motor gear="120" joint="2a_right_hip_y"/>
<motor gear="80" joint="2a_right_knee"/>
<motor gear="20" joint="2a_right_ankle_x"/>
<motor gear="20" joint="2a_right_ankle_y"/>
<motor gear="40" joint="2a_left_hip_x"/>
<motor gear="40" joint="2a_left_hip_z"/>
<motor gear="120" joint="2a_left_hip_y"/>
<motor gear="80" joint="2a_left_knee"/>
<motor gear="20" joint="2a_left_ankle_x"/>
<motor gear="20" joint="2a_left_ankle_y"/>
<motor gear="20" joint="2a_right_shoulder1"/>
<motor gear="20" joint="2a_right_shoulder2"/>
<motor gear="40" joint="2a_right_elbow"/>
<motor gear="20" joint="2a_left_shoulder1"/>
<motor gear="20" joint="2a_left_shoulder2"/>
<motor gear="40" joint="2a_left_elbow"/>
</actuator>
<actuator>
<motor gear="40" joint="3a_abdomen_y"/>
<motor gear="40" joint="3a_abdomen_z"/>
<motor gear="40" joint="3a_abdomen_x"/>
<motor gear="40" joint="3a_right_hip_x"/>
<motor gear="40" joint="3a_right_hip_z"/>
<motor gear="120" joint="3a_right_hip_y"/>
<motor gear="80" joint="3a_right_knee"/>
<motor gear="20" joint="3a_right_ankle_x"/>
<motor gear="20" joint="3a_right_ankle_y"/>
<motor gear="40" joint="3a_left_hip_x"/>
<motor gear="40" joint="3a_left_hip_z"/>
<motor gear="120" joint="3a_left_hip_y"/>
<motor gear="80" joint="3a_left_knee"/>
<motor gear="20" joint="3a_left_ankle_x"/>
<motor gear="20" joint="3a_left_ankle_y"/>
<motor gear="20" joint="3a_right_shoulder1"/>
<motor gear="20" joint="3a_right_shoulder2"/>
<motor gear="40" joint="3a_right_elbow"/>
<motor gear="20" joint="3a_left_shoulder1"/>
<motor gear="20" joint="3a_left_shoulder2"/>
<motor gear="40" joint="3a_left_elbow"/>
</actuator>
<actuator>
<motor gear="40" joint="4a_abdomen_y"/>
<motor gear="40" joint="4a_abdomen_z"/>
<motor gear="40" joint="4a_abdomen_x"/>
<motor gear="40" joint="4a_right_hip_x"/>
<motor gear="40" joint="4a_right_hip_z"/>
<motor gear="120" joint="4a_right_hip_y"/>
<motor gear="80" joint="4a_right_knee"/>
<motor gear="20" joint="4a_right_ankle_x"/>
<motor gear="20" joint="4a_right_ankle_y"/>
<motor gear="40" joint="4a_left_hip_x"/>
<motor gear="40" joint="4a_left_hip_z"/>
<motor gear="120" joint="4a_left_hip_y"/>
<motor gear="80" joint="4a_left_knee"/>
<motor gear="20" joint="4a_left_ankle_x"/>
<motor gear="20" joint="4a_left_ankle_y"/>
<motor gear="20" joint="4a_right_shoulder1"/>
<motor gear="20" joint="4a_right_shoulder2"/>
<motor gear="40" joint="4a_right_elbow"/>
<motor gear="20" joint="4a_left_shoulder1"/>
<motor gear="20" joint="4a_left_shoulder2"/>
<motor gear="40" joint="4a_left_elbow"/>
</actuator>
<actuator>
<motor gear="40" joint="5a_abdomen_y"/>
<motor gear="40" joint="5a_abdomen_z"/>
<motor gear="40" joint="5a_abdomen_x"/>
<motor gear="40" joint="5a_right_hip_x"/>
<motor gear="40" joint="5a_right_hip_z"/>
<motor gear="120" joint="5a_right_hip_y"/>
<motor gear="80" joint="5a_right_knee"/>
<motor gear="20" joint="5a_right_ankle_x"/>
<motor gear="20" joint="5a_right_ankle_y"/>
<motor gear="40" joint="5a_left_hip_x"/>
<motor gear="40" joint="5a_left_hip_z"/>
<motor gear="120" joint="5a_left_hip_y"/>
<motor gear="80" joint="5a_left_knee"/>
<motor gear="20" joint="5a_left_ankle_x"/>
<motor gear="20" joint="5a_left_ankle_y"/>
<motor gear="20" joint="5a_right_shoulder1"/>
<motor gear="20" joint="5a_right_shoulder2"/>
<motor gear="40" joint="5a_right_elbow"/>
<motor gear="20" joint="5a_left_shoulder1"/>
<motor gear="20" joint="5a_left_shoulder2"/>
<motor gear="40" joint="5a_left_elbow"/>
</actuator>
<actuator>
<motor gear="40" joint="6a_abdomen_y"/>
<motor gear="40" joint="6a_abdomen_z"/>
<motor gear="40" joint="6a_abdomen_x"/>
<motor gear="40" joint="6a_right_hip_x"/>
<motor gear="40" joint="6a_right_hip_z"/>
<motor gear="120" joint="6a_right_hip_y"/>
<motor gear="80" joint="6a_right_knee"/>
<motor gear="20" joint="6a_right_ankle_x"/>
<motor gear="20" joint="6a_right_ankle_y"/>
<motor gear="40" joint="6a_left_hip_x"/>
<motor gear="40" joint="6a_left_hip_z"/>
<motor gear="120" joint="6a_left_hip_y"/>
<motor gear="80" joint="6a_left_knee"/>
<motor gear="20" joint="6a_left_ankle_x"/>
<motor gear="20" joint="6a_left_ankle_y"/>
<motor gear="20" joint="6a_right_shoulder1"/>
<motor gear="20" joint="6a_right_shoulder2"/>
<motor gear="40" joint="6a_right_elbow"/>
<motor gear="20" joint="6a_left_shoulder1"/>
<motor gear="20" joint="6a_left_shoulder2"/>
<motor gear="40" joint="6a_left_elbow"/>
</actuator>
</mujoco>
mjx/mujoco/mjx/test_data/humanoid/07_humanoids.xml
+775
View File
@@ -0,0 +1,775 @@
<mujoco model="07 Humanoids">
<option timestep="0.005" solver="Newton" iterations="1" ls_iterations="4">
<flag eulerdamp="disable"/>
</option>
<custom>
<numeric data="28" name="max_contact_points"/>
<numeric data="28" name="max_geom_pairs"/>
</custom>
<size memory="100M"/>
<asset>
<texture type="skybox" builtin="gradient" rgb1=".3 .5 .7" rgb2="0 0 0" width="512" height="512"/>
<texture name="body" type="cube" builtin="flat" mark="cross" width="128" height="128"
rgb1="0.8 0.6 0.4" rgb2="0.8 0.6 0.4" markrgb="1 1 1" random="0.01"/>
<material name="body" texture="body" texuniform="true" rgba="0.8 0.6 .4 1"/>
<texture name="grid" type="2d" builtin="checker" width="512" height="512" rgb1=".1 .2 .3" rgb2=".2 .3 .4"/>
<material name="grid" texture="grid" texrepeat="1 1" texuniform="true" reflectance=".2"/>
</asset>
<default>
<motor ctrlrange="-1 1" ctrllimited="true"/>
<joint type="hinge" damping=".2" stiffness="1" armature=".01" limited="true" solimplimit="0 .99 .01"/>
<default class="big_joint">
<joint damping="5" stiffness="10"/>
<default class="big_stiff_joint">
<joint stiffness="20"/>
</default>
</default>
<default class="body1">
<geom type="capsule" condim="3" friction=".7" solimp=".9 .99 .003" solref=".015 1" material="body" contype="2" conaffinity="253"/>
</default>
<default class="body2">
<geom type="capsule" condim="3" friction=".7" solimp=".9 .99 .003" solref=".015 1" material="body" contype="4" conaffinity="251"/>
</default>
<default class="body3">
<geom type="capsule" condim="3" friction=".7" solimp=".9 .99 .003" solref=".015 1" material="body" contype="8" conaffinity="247"/>
</default>
<default class="body4">
<geom type="capsule" condim="3" friction=".7" solimp=".9 .99 .003" solref=".015 1" material="body" contype="16" conaffinity="239"/>
</default>
<default class="body5">
<geom type="capsule" condim="3" friction=".7" solimp=".9 .99 .003" solref=".015 1" material="body" contype="32" conaffinity="223"/>
</default>
<default class="body6">
<geom type="capsule" condim="3" friction=".7" solimp=".9 .99 .003" solref=".015 1" material="body" contype="64" conaffinity="191"/>
</default>
<default class="body7">
<geom type="capsule" condim="3" friction=".7" solimp=".9 .99 .003" solref=".015 1" material="body" contype="128" conaffinity="127"/>
</default>
</default>
<visual>
<map force="0.1" zfar="30"/>
<rgba haze="0.15 0.25 0.35 1"/>
<quality shadowsize="4096"/>
<global offwidth="800" offheight="800"/>
</visual>
<worldbody>
<geom size="10 10 .05" type="plane" material="grid" condim="3"/>
<light dir=".2 1 -.4" diffuse=".8 .8 .8" specular="0.3 0.3 0.3" pos="-2 -10 4" cutoff="35"/>
<light dir="-.2 1 -.4" diffuse=".8 .8 .8" specular="0.3 0.3 0.3" pos="2 -10 4" cutoff="35"/>
<body name="1a_torso" pos="-1 0 1.5" childclass="body1">
<camera name="1a_back" pos="-3 0 1" xyaxes="0 -1 0 1 0 2" mode="trackcom"/>
<camera name="1a_side" pos="0 -3 1" xyaxes="1 0 0 0 1 2" mode="trackcom"/>
<freejoint name="1a_root"/>
<geom name="1a_torso" fromto="0 -.07 0 0 .07 0" size=".07"/>
<geom name="1a_upper_waist" fromto="-.01 -.06 -.12 -.01 .06 -.12" size=".06"/>
<body name="1a_head" pos="0 0 .19">
<geom name="1a_head" type="sphere" size=".09"/>
<camera name="1a_egocentric" pos=".09 0 0" xyaxes="0 -1 0 .1 0 1" fovy="80"/>
</body>
<body name="1a_lower_waist" pos="-.01 0 -.26">
<geom name="1a_lower_waist" fromto="0 -.06 0 0 .06 0" size=".06"/>
<joint name="1a_abdomen_z" pos="0 0 .065" axis="0 0 1" range="-45 45" class="big_stiff_joint"/>
<joint name="1a_abdomen_y" pos="0 0 .065" axis="0 1 0" range="-75 30" class="big_joint"/>
<body name="1a_pelvis" pos="0 0 -.165">
<joint name="1a_abdomen_x" pos="0 0 .1" axis="1 0 0" range="-35 35" class="big_joint"/>
<geom name="1a_butt" fromto="-.02 -.07 0 -.02 .07 0" size=".09"/>
<body name="1a_right_thigh" pos="0 -.1 -.04">
<joint name="1a_right_hip_x" axis="1 0 0" range="-25 5" class="big_joint"/>
<joint name="1a_right_hip_z" axis="0 0 1" range="-60 35" class="big_joint"/>
<joint name="1a_right_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
<geom name="1a_right_thigh" fromto="0 0 0 0 .01 -.34" size=".06"/>
<body name="1a_right_shin" pos="0 .01 -.403">
<joint name="1a_right_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
<geom name="1a_right_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
<body name="1a_right_foot" pos="0 0 -.39">
<joint name="1a_right_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
<joint name="1a_right_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
<geom name="1a_right_right_foot" fromto="-.07 -.02 0 .14 -.04 0" size=".027"/>
<geom name="1a_left_right_foot" fromto="-.07 0 0 .14 .02 0" size=".027"/>
</body>
</body>
</body>
<body name="1a_left_thigh" pos="0 .1 -.04">
<joint name="1a_left_hip_x" axis="-1 0 0" range="-25 5" class="big_joint"/>
<joint name="1a_left_hip_z" axis="0 0 -1" range="-60 35" class="big_joint"/>
<joint name="1a_left_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
<geom name="1a_left_thigh" fromto="0 0 0 0 -.01 -.34" size=".06"/>
<body name="1a_left_shin" pos="0 -.01 -.403">
<joint name="1a_left_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
<geom name="1a_left_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
<body name="1a_left_foot" pos="0 0 -.39">
<joint name="1a_left_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
<joint name="1a_left_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
<geom name="1a_left_left_foot" fromto="-.07 .02 0 .14 .04 0" size=".027"/>
<geom name="1a_right_left_foot" fromto="-.07 0 0 .14 -.02 0" size=".027"/>
</body>
</body>
</body>
</body>
</body>
<body name="1a_right_upper_arm" pos="0 -.17 .06">
<joint name="1a_right_shoulder1" axis="2 1 1" range="-85 60"/>
<joint name="1a_right_shoulder2" axis="0 -1 1" range="-85 60"/>
<geom name="1a_right_upper_arm" fromto="0 0 0 .16 -.16 -.16" size=".04 .16"/>
<body name="1a_right_lower_arm" pos=".18 -.18 -.18">
<joint name="1a_right_elbow" axis="0 -1 1" range="-90 50" stiffness="0"/>
<geom name="1a_right_lower_arm" fromto=".01 .01 .01 .17 .17 .17" size=".031"/>
<body name="1a_right_hand" pos=".18 .18 .18">
<geom name="1a_right_hand" type="sphere" size=".04" zaxis="1 1 1"/>
</body>
</body>
</body>
<body name="1a_left_upper_arm" pos="0 .17 .06">
<joint name="1a_left_shoulder1" axis="2 -1 1" range="-60 85"/>
<joint name="1a_left_shoulder2" axis="0 1 1" range="-60 85"/>
<geom name="1a_left_upper_arm" fromto="0 0 0 .16 .16 -.16" size=".04 .16"/>
<body name="1a_left_lower_arm" pos=".18 .18 -.18">
<joint name="1a_left_elbow" axis="0 -1 -1" range="-90 50" stiffness="0"/>
<geom name="1a_left_lower_arm" fromto=".01 -.01 .01 .17 -.17 .17" size=".031"/>
<body name="1a_left_hand" pos=".18 -.18 .18">
<geom name="1a_left_hand" type="sphere" size=".04" zaxis="1 -1 1"/>
</body>
</body>
</body>
</body>
<body name="2a_torso" pos="-1 1 1.5" childclass="body2">
<camera name="2a_back" pos="-3 0 1" xyaxes="0 -1 0 1 0 2" mode="trackcom"/>
<camera name="2a_side" pos="0 -3 1" xyaxes="1 0 0 0 1 2" mode="trackcom"/>
<freejoint name="2a_root"/>
<geom name="2a_torso" fromto="0 -.07 0 0 .07 0" size=".07"/>
<geom name="2a_upper_waist" fromto="-.01 -.06 -.12 -.01 .06 -.12" size=".06"/>
<body name="2a_head" pos="0 0 .19">
<geom name="2a_head" type="sphere" size=".09"/>
<camera name="2a_egocentric" pos=".09 0 0" xyaxes="0 -1 0 .1 0 1" fovy="80"/>
</body>
<body name="2a_lower_waist" pos="-.01 0 -.26">
<geom name="2a_lower_waist" fromto="0 -.06 0 0 .06 0" size=".06"/>
<joint name="2a_abdomen_z" pos="0 0 .065" axis="0 0 1" range="-45 45" class="big_stiff_joint"/>
<joint name="2a_abdomen_y" pos="0 0 .065" axis="0 1 0" range="-75 30" class="big_joint"/>
<body name="2a_pelvis" pos="0 0 -.165">
<joint name="2a_abdomen_x" pos="0 0 .1" axis="1 0 0" range="-35 35" class="big_joint"/>
<geom name="2a_butt" fromto="-.02 -.07 0 -.02 .07 0" size=".09"/>
<body name="2a_right_thigh" pos="0 -.1 -.04">
<joint name="2a_right_hip_x" axis="1 0 0" range="-25 5" class="big_joint"/>
<joint name="2a_right_hip_z" axis="0 0 1" range="-60 35" class="big_joint"/>
<joint name="2a_right_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
<geom name="2a_right_thigh" fromto="0 0 0 0 .01 -.34" size=".06"/>
<body name="2a_right_shin" pos="0 .01 -.403">
<joint name="2a_right_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
<geom name="2a_right_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
<body name="2a_right_foot" pos="0 0 -.39">
<joint name="2a_right_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
<joint name="2a_right_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
<geom name="2a_right_right_foot" fromto="-.07 -.02 0 .14 -.04 0" size=".027"/>
<geom name="2a_left_right_foot" fromto="-.07 0 0 .14 .02 0" size=".027"/>
</body>
</body>
</body>
<body name="2a_left_thigh" pos="0 .1 -.04">
<joint name="2a_left_hip_x" axis="-1 0 0" range="-25 5" class="big_joint"/>
<joint name="2a_left_hip_z" axis="0 0 -1" range="-60 35" class="big_joint"/>
<joint name="2a_left_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
<geom name="2a_left_thigh" fromto="0 0 0 0 -.01 -.34" size=".06"/>
<body name="2a_left_shin" pos="0 -.01 -.403">
<joint name="2a_left_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
<geom name="2a_left_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
<body name="2a_left_foot" pos="0 0 -.39">
<joint name="2a_left_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
<joint name="2a_left_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
<geom name="2a_left_left_foot" fromto="-.07 .02 0 .14 .04 0" size=".027"/>
<geom name="2a_right_left_foot" fromto="-.07 0 0 .14 -.02 0" size=".027"/>
</body>
</body>
</body>
</body>
</body>
<body name="2a_right_upper_arm" pos="0 -.17 .06">
<joint name="2a_right_shoulder1" axis="2 1 1" range="-85 60"/>
<joint name="2a_right_shoulder2" axis="0 -1 1" range="-85 60"/>
<geom name="2a_right_upper_arm" fromto="0 0 0 .16 -.16 -.16" size=".04 .16"/>
<body name="2a_right_lower_arm" pos=".18 -.18 -.18">
<joint name="2a_right_elbow" axis="0 -1 1" range="-90 50" stiffness="0"/>
<geom name="2a_right_lower_arm" fromto=".01 .01 .01 .17 .17 .17" size=".031"/>
<body name="2a_right_hand" pos=".18 .18 .18">
<geom name="2a_right_hand" type="sphere" size=".04" zaxis="1 1 1"/>
</body>
</body>
</body>
<body name="2a_left_upper_arm" pos="0 .17 .06">
<joint name="2a_left_shoulder1" axis="2 -1 1" range="-60 85"/>
<joint name="2a_left_shoulder2" axis="0 1 1" range="-60 85"/>
<geom name="2a_left_upper_arm" fromto="0 0 0 .16 .16 -.16" size=".04 .16"/>
<body name="2a_left_lower_arm" pos=".18 .18 -.18">
<joint name="2a_left_elbow" axis="0 -1 -1" range="-90 50" stiffness="0"/>
<geom name="2a_left_lower_arm" fromto=".01 -.01 .01 .17 -.17 .17" size=".031"/>
<body name="2a_left_hand" pos=".18 -.18 .18">
<geom name="2a_left_hand" type="sphere" size=".04" zaxis="1 -1 1"/>
</body>
</body>
</body>
</body>
<body name="3a_torso" pos="-1 -1 1.5" childclass="body3">
<camera name="3a_back" pos="-3 0 1" xyaxes="0 -1 0 1 0 2" mode="trackcom"/>
<camera name="3a_side" pos="0 -3 1" xyaxes="1 0 0 0 1 2" mode="trackcom"/>
<freejoint name="3a_root"/>
<geom name="3a_torso" fromto="0 -.07 0 0 .07 0" size=".07"/>
<geom name="3a_upper_waist" fromto="-.01 -.06 -.12 -.01 .06 -.12" size=".06"/>
<body name="3a_head" pos="0 0 .19">
<geom name="3a_head" type="sphere" size=".09"/>
<camera name="3a_egocentric" pos=".09 0 0" xyaxes="0 -1 0 .1 0 1" fovy="80"/>
</body>
<body name="3a_lower_waist" pos="-.01 0 -.26">
<geom name="3a_lower_waist" fromto="0 -.06 0 0 .06 0" size=".06"/>
<joint name="3a_abdomen_z" pos="0 0 .065" axis="0 0 1" range="-45 45" class="big_stiff_joint"/>
<joint name="3a_abdomen_y" pos="0 0 .065" axis="0 1 0" range="-75 30" class="big_joint"/>
<body name="3a_pelvis" pos="0 0 -.165">
<joint name="3a_abdomen_x" pos="0 0 .1" axis="1 0 0" range="-35 35" class="big_joint"/>
<geom name="3a_butt" fromto="-.02 -.07 0 -.02 .07 0" size=".09"/>
<body name="3a_right_thigh" pos="0 -.1 -.04">
<joint name="3a_right_hip_x" axis="1 0 0" range="-25 5" class="big_joint"/>
<joint name="3a_right_hip_z" axis="0 0 1" range="-60 35" class="big_joint"/>
<joint name="3a_right_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
<geom name="3a_right_thigh" fromto="0 0 0 0 .01 -.34" size=".06"/>
<body name="3a_right_shin" pos="0 .01 -.403">
<joint name="3a_right_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
<geom name="3a_right_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
<body name="3a_right_foot" pos="0 0 -.39">
<joint name="3a_right_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
<joint name="3a_right_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
<geom name="3a_right_right_foot" fromto="-.07 -.02 0 .14 -.04 0" size=".027"/>
<geom name="3a_left_right_foot" fromto="-.07 0 0 .14 .02 0" size=".027"/>
</body>
</body>
</body>
<body name="3a_left_thigh" pos="0 .1 -.04">
<joint name="3a_left_hip_x" axis="-1 0 0" range="-25 5" class="big_joint"/>
<joint name="3a_left_hip_z" axis="0 0 -1" range="-60 35" class="big_joint"/>
<joint name="3a_left_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
<geom name="3a_left_thigh" fromto="0 0 0 0 -.01 -.34" size=".06"/>
<body name="3a_left_shin" pos="0 -.01 -.403">
<joint name="3a_left_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
<geom name="3a_left_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
<body name="3a_left_foot" pos="0 0 -.39">
<joint name="3a_left_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
<joint name="3a_left_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
<geom name="3a_left_left_foot" fromto="-.07 .02 0 .14 .04 0" size=".027"/>
<geom name="3a_right_left_foot" fromto="-.07 0 0 .14 -.02 0" size=".027"/>
</body>
</body>
</body>
</body>
</body>
<body name="3a_right_upper_arm" pos="0 -.17 .06">
<joint name="3a_right_shoulder1" axis="2 1 1" range="-85 60"/>
<joint name="3a_right_shoulder2" axis="0 -1 1" range="-85 60"/>
<geom name="3a_right_upper_arm" fromto="0 0 0 .16 -.16 -.16" size=".04 .16"/>
<body name="3a_right_lower_arm" pos=".18 -.18 -.18">
<joint name="3a_right_elbow" axis="0 -1 1" range="-90 50" stiffness="0"/>
<geom name="3a_right_lower_arm" fromto=".01 .01 .01 .17 .17 .17" size=".031"/>
<body name="3a_right_hand" pos=".18 .18 .18">
<geom name="3a_right_hand" type="sphere" size=".04" zaxis="1 1 1"/>
</body>
</body>
</body>
<body name="3a_left_upper_arm" pos="0 .17 .06">
<joint name="3a_left_shoulder1" axis="2 -1 1" range="-60 85"/>
<joint name="3a_left_shoulder2" axis="0 1 1" range="-60 85"/>
<geom name="3a_left_upper_arm" fromto="0 0 0 .16 .16 -.16" size=".04 .16"/>
<body name="3a_left_lower_arm" pos=".18 .18 -.18">
<joint name="3a_left_elbow" axis="0 -1 -1" range="-90 50" stiffness="0"/>
<geom name="3a_left_lower_arm" fromto=".01 -.01 .01 .17 -.17 .17" size=".031"/>
<body name="3a_left_hand" pos=".18 -.18 .18">
<geom name="3a_left_hand" type="sphere" size=".04" zaxis="1 -1 1"/>
</body>
</body>
</body>
</body>
<body name="4a_torso" pos="-1 2 1.5" childclass="body4">
<camera name="4a_back" pos="-3 0 1" xyaxes="0 -1 0 1 0 2" mode="trackcom"/>
<camera name="4a_side" pos="0 -3 1" xyaxes="1 0 0 0 1 2" mode="trackcom"/>
<freejoint name="4a_root"/>
<geom name="4a_torso" fromto="0 -.07 0 0 .07 0" size=".07"/>
<geom name="4a_upper_waist" fromto="-.01 -.06 -.12 -.01 .06 -.12" size=".06"/>
<body name="4a_head" pos="0 0 .19">
<geom name="4a_head" type="sphere" size=".09"/>
<camera name="4a_egocentric" pos=".09 0 0" xyaxes="0 -1 0 .1 0 1" fovy="80"/>
</body>
<body name="4a_lower_waist" pos="-.01 0 -.26">
<geom name="4a_lower_waist" fromto="0 -.06 0 0 .06 0" size=".06"/>
<joint name="4a_abdomen_z" pos="0 0 .065" axis="0 0 1" range="-45 45" class="big_stiff_joint"/>
<joint name="4a_abdomen_y" pos="0 0 .065" axis="0 1 0" range="-75 30" class="big_joint"/>
<body name="4a_pelvis" pos="0 0 -.165">
<joint name="4a_abdomen_x" pos="0 0 .1" axis="1 0 0" range="-35 35" class="big_joint"/>
<geom name="4a_butt" fromto="-.02 -.07 0 -.02 .07 0" size=".09"/>
<body name="4a_right_thigh" pos="0 -.1 -.04">
<joint name="4a_right_hip_x" axis="1 0 0" range="-25 5" class="big_joint"/>
<joint name="4a_right_hip_z" axis="0 0 1" range="-60 35" class="big_joint"/>
<joint name="4a_right_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
<geom name="4a_right_thigh" fromto="0 0 0 0 .01 -.34" size=".06"/>
<body name="4a_right_shin" pos="0 .01 -.403">
<joint name="4a_right_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
<geom name="4a_right_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
<body name="4a_right_foot" pos="0 0 -.39">
<joint name="4a_right_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
<joint name="4a_right_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
<geom name="4a_right_right_foot" fromto="-.07 -.02 0 .14 -.04 0" size=".027"/>
<geom name="4a_left_right_foot" fromto="-.07 0 0 .14 .02 0" size=".027"/>
</body>
</body>
</body>
<body name="4a_left_thigh" pos="0 .1 -.04">
<joint name="4a_left_hip_x" axis="-1 0 0" range="-25 5" class="big_joint"/>
<joint name="4a_left_hip_z" axis="0 0 -1" range="-60 35" class="big_joint"/>
<joint name="4a_left_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
<geom name="4a_left_thigh" fromto="0 0 0 0 -.01 -.34" size=".06"/>
<body name="4a_left_shin" pos="0 -.01 -.403">
<joint name="4a_left_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
<geom name="4a_left_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
<body name="4a_left_foot" pos="0 0 -.39">
<joint name="4a_left_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
<joint name="4a_left_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
<geom name="4a_left_left_foot" fromto="-.07 .02 0 .14 .04 0" size=".027"/>
<geom name="4a_right_left_foot" fromto="-.07 0 0 .14 -.02 0" size=".027"/>
</body>
</body>
</body>
</body>
</body>
<body name="4a_right_upper_arm" pos="0 -.17 .06">
<joint name="4a_right_shoulder1" axis="2 1 1" range="-85 60"/>
<joint name="4a_right_shoulder2" axis="0 -1 1" range="-85 60"/>
<geom name="4a_right_upper_arm" fromto="0 0 0 .16 -.16 -.16" size=".04 .16"/>
<body name="4a_right_lower_arm" pos=".18 -.18 -.18">
<joint name="4a_right_elbow" axis="0 -1 1" range="-90 50" stiffness="0"/>
<geom name="4a_right_lower_arm" fromto=".01 .01 .01 .17 .17 .17" size=".031"/>
<body name="4a_right_hand" pos=".18 .18 .18">
<geom name="4a_right_hand" type="sphere" size=".04" zaxis="1 1 1"/>
</body>
</body>
</body>
<body name="4a_left_upper_arm" pos="0 .17 .06">
<joint name="4a_left_shoulder1" axis="2 -1 1" range="-60 85"/>
<joint name="4a_left_shoulder2" axis="0 1 1" range="-60 85"/>
<geom name="4a_left_upper_arm" fromto="0 0 0 .16 .16 -.16" size=".04 .16"/>
<body name="4a_left_lower_arm" pos=".18 .18 -.18">
<joint name="4a_left_elbow" axis="0 -1 -1" range="-90 50" stiffness="0"/>
<geom name="4a_left_lower_arm" fromto=".01 -.01 .01 .17 -.17 .17" size=".031"/>
<body name="4a_left_hand" pos=".18 -.18 .18">
<geom name="4a_left_hand" type="sphere" size=".04" zaxis="1 -1 1"/>
</body>
</body>
</body>
</body>
<body name="5a_torso" pos="-1 -2 1.5" childclass="body5">
<camera name="5a_back" pos="-3 0 1" xyaxes="0 -1 0 1 0 2" mode="trackcom"/>
<camera name="5a_side" pos="0 -3 1" xyaxes="1 0 0 0 1 2" mode="trackcom"/>
<freejoint name="5a_root"/>
<geom name="5a_torso" fromto="0 -.07 0 0 .07 0" size=".07"/>
<geom name="5a_upper_waist" fromto="-.01 -.06 -.12 -.01 .06 -.12" size=".06"/>
<body name="5a_head" pos="0 0 .19">
<geom name="5a_head" type="sphere" size=".09"/>
<camera name="5a_egocentric" pos=".09 0 0" xyaxes="0 -1 0 .1 0 1" fovy="80"/>
</body>
<body name="5a_lower_waist" pos="-.01 0 -.26">
<geom name="5a_lower_waist" fromto="0 -.06 0 0 .06 0" size=".06"/>
<joint name="5a_abdomen_z" pos="0 0 .065" axis="0 0 1" range="-45 45" class="big_stiff_joint"/>
<joint name="5a_abdomen_y" pos="0 0 .065" axis="0 1 0" range="-75 30" class="big_joint"/>
<body name="5a_pelvis" pos="0 0 -.165">
<joint name="5a_abdomen_x" pos="0 0 .1" axis="1 0 0" range="-35 35" class="big_joint"/>
<geom name="5a_butt" fromto="-.02 -.07 0 -.02 .07 0" size=".09"/>
<body name="5a_right_thigh" pos="0 -.1 -.04">
<joint name="5a_right_hip_x" axis="1 0 0" range="-25 5" class="big_joint"/>
<joint name="5a_right_hip_z" axis="0 0 1" range="-60 35" class="big_joint"/>
<joint name="5a_right_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
<geom name="5a_right_thigh" fromto="0 0 0 0 .01 -.34" size=".06"/>
<body name="5a_right_shin" pos="0 .01 -.403">
<joint name="5a_right_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
<geom name="5a_right_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
<body name="5a_right_foot" pos="0 0 -.39">
<joint name="5a_right_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
<joint name="5a_right_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
<geom name="5a_right_right_foot" fromto="-.07 -.02 0 .14 -.04 0" size=".027"/>
<geom name="5a_left_right_foot" fromto="-.07 0 0 .14 .02 0" size=".027"/>
</body>
</body>
</body>
<body name="5a_left_thigh" pos="0 .1 -.04">
<joint name="5a_left_hip_x" axis="-1 0 0" range="-25 5" class="big_joint"/>
<joint name="5a_left_hip_z" axis="0 0 -1" range="-60 35" class="big_joint"/>
<joint name="5a_left_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
<geom name="5a_left_thigh" fromto="0 0 0 0 -.01 -.34" size=".06"/>
<body name="5a_left_shin" pos="0 -.01 -.403">
<joint name="5a_left_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
<geom name="5a_left_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
<body name="5a_left_foot" pos="0 0 -.39">
<joint name="5a_left_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
<joint name="5a_left_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
<geom name="5a_left_left_foot" fromto="-.07 .02 0 .14 .04 0" size=".027"/>
<geom name="5a_right_left_foot" fromto="-.07 0 0 .14 -.02 0" size=".027"/>
</body>
</body>
</body>
</body>
</body>
<body name="5a_right_upper_arm" pos="0 -.17 .06">
<joint name="5a_right_shoulder1" axis="2 1 1" range="-85 60"/>
<joint name="5a_right_shoulder2" axis="0 -1 1" range="-85 60"/>
<geom name="5a_right_upper_arm" fromto="0 0 0 .16 -.16 -.16" size=".04 .16"/>
<body name="5a_right_lower_arm" pos=".18 -.18 -.18">
<joint name="5a_right_elbow" axis="0 -1 1" range="-90 50" stiffness="0"/>
<geom name="5a_right_lower_arm" fromto=".01 .01 .01 .17 .17 .17" size=".031"/>
<body name="5a_right_hand" pos=".18 .18 .18">
<geom name="5a_right_hand" type="sphere" size=".04" zaxis="1 1 1"/>
</body>
</body>
</body>
<body name="5a_left_upper_arm" pos="0 .17 .06">
<joint name="5a_left_shoulder1" axis="2 -1 1" range="-60 85"/>
<joint name="5a_left_shoulder2" axis="0 1 1" range="-60 85"/>
<geom name="5a_left_upper_arm" fromto="0 0 0 .16 .16 -.16" size=".04 .16"/>
<body name="5a_left_lower_arm" pos=".18 .18 -.18">
<joint name="5a_left_elbow" axis="0 -1 -1" range="-90 50" stiffness="0"/>
<geom name="5a_left_lower_arm" fromto=".01 -.01 .01 .17 -.17 .17" size=".031"/>
<body name="5a_left_hand" pos=".18 -.18 .18">
<geom name="5a_left_hand" type="sphere" size=".04" zaxis="1 -1 1"/>
</body>
</body>
</body>
</body>
<body name="6a_torso" pos="-1 3 1.5" childclass="body6">
<camera name="6a_back" pos="-3 0 1" xyaxes="0 -1 0 1 0 2" mode="trackcom"/>
<camera name="6a_side" pos="0 -3 1" xyaxes="1 0 0 0 1 2" mode="trackcom"/>
<freejoint name="6a_root"/>
<geom name="6a_torso" fromto="0 -.07 0 0 .07 0" size=".07"/>
<geom name="6a_upper_waist" fromto="-.01 -.06 -.12 -.01 .06 -.12" size=".06"/>
<body name="6a_head" pos="0 0 .19">
<geom name="6a_head" type="sphere" size=".09"/>
<camera name="6a_egocentric" pos=".09 0 0" xyaxes="0 -1 0 .1 0 1" fovy="80"/>
</body>
<body name="6a_lower_waist" pos="-.01 0 -.26">
<geom name="6a_lower_waist" fromto="0 -.06 0 0 .06 0" size=".06"/>
<joint name="6a_abdomen_z" pos="0 0 .065" axis="0 0 1" range="-45 45" class="big_stiff_joint"/>
<joint name="6a_abdomen_y" pos="0 0 .065" axis="0 1 0" range="-75 30" class="big_joint"/>
<body name="6a_pelvis" pos="0 0 -.165">
<joint name="6a_abdomen_x" pos="0 0 .1" axis="1 0 0" range="-35 35" class="big_joint"/>
<geom name="6a_butt" fromto="-.02 -.07 0 -.02 .07 0" size=".09"/>
<body name="6a_right_thigh" pos="0 -.1 -.04">
<joint name="6a_right_hip_x" axis="1 0 0" range="-25 5" class="big_joint"/>
<joint name="6a_right_hip_z" axis="0 0 1" range="-60 35" class="big_joint"/>
<joint name="6a_right_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
<geom name="6a_right_thigh" fromto="0 0 0 0 .01 -.34" size=".06"/>
<body name="6a_right_shin" pos="0 .01 -.403">
<joint name="6a_right_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
<geom name="6a_right_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
<body name="6a_right_foot" pos="0 0 -.39">
<joint name="6a_right_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
<joint name="6a_right_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
<geom name="6a_right_right_foot" fromto="-.07 -.02 0 .14 -.04 0" size=".027"/>
<geom name="6a_left_right_foot" fromto="-.07 0 0 .14 .02 0" size=".027"/>
</body>
</body>
</body>
<body name="6a_left_thigh" pos="0 .1 -.04">
<joint name="6a_left_hip_x" axis="-1 0 0" range="-25 5" class="big_joint"/>
<joint name="6a_left_hip_z" axis="0 0 -1" range="-60 35" class="big_joint"/>
<joint name="6a_left_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
<geom name="6a_left_thigh" fromto="0 0 0 0 -.01 -.34" size=".06"/>
<body name="6a_left_shin" pos="0 -.01 -.403">
<joint name="6a_left_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
<geom name="6a_left_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
<body name="6a_left_foot" pos="0 0 -.39">
<joint name="6a_left_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
<joint name="6a_left_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
<geom name="6a_left_left_foot" fromto="-.07 .02 0 .14 .04 0" size=".027"/>
<geom name="6a_right_left_foot" fromto="-.07 0 0 .14 -.02 0" size=".027"/>
</body>
</body>
</body>
</body>
</body>
<body name="6a_right_upper_arm" pos="0 -.17 .06">
<joint name="6a_right_shoulder1" axis="2 1 1" range="-85 60"/>
<joint name="6a_right_shoulder2" axis="0 -1 1" range="-85 60"/>
<geom name="6a_right_upper_arm" fromto="0 0 0 .16 -.16 -.16" size=".04 .16"/>
<body name="6a_right_lower_arm" pos=".18 -.18 -.18">
<joint name="6a_right_elbow" axis="0 -1 1" range="-90 50" stiffness="0"/>
<geom name="6a_right_lower_arm" fromto=".01 .01 .01 .17 .17 .17" size=".031"/>
<body name="6a_right_hand" pos=".18 .18 .18">
<geom name="6a_right_hand" type="sphere" size=".04" zaxis="1 1 1"/>
</body>
</body>
</body>
<body name="6a_left_upper_arm" pos="0 .17 .06">
<joint name="6a_left_shoulder1" axis="2 -1 1" range="-60 85"/>
<joint name="6a_left_shoulder2" axis="0 1 1" range="-60 85"/>
<geom name="6a_left_upper_arm" fromto="0 0 0 .16 .16 -.16" size=".04 .16"/>
<body name="6a_left_lower_arm" pos=".18 .18 -.18">
<joint name="6a_left_elbow" axis="0 -1 -1" range="-90 50" stiffness="0"/>
<geom name="6a_left_lower_arm" fromto=".01 -.01 .01 .17 -.17 .17" size=".031"/>
<body name="6a_left_hand" pos=".18 -.18 .18">
<geom name="6a_left_hand" type="sphere" size=".04" zaxis="1 -1 1"/>
</body>
</body>
</body>
</body>
<body name="7a_torso" pos="-1 -3 1.5" childclass="body7">
<camera name="7a_back" pos="-3 0 1" xyaxes="0 -1 0 1 0 2" mode="trackcom"/>
<camera name="7a_side" pos="0 -3 1" xyaxes="1 0 0 0 1 2" mode="trackcom"/>
<freejoint name="7a_root"/>
<geom name="7a_torso" fromto="0 -.07 0 0 .07 0" size=".07"/>
<geom name="7a_upper_waist" fromto="-.01 -.06 -.12 -.01 .06 -.12" size=".06"/>
<body name="7a_head" pos="0 0 .19">
<geom name="7a_head" type="sphere" size=".09"/>
<camera name="7a_egocentric" pos=".09 0 0" xyaxes="0 -1 0 .1 0 1" fovy="80"/>
</body>
<body name="7a_lower_waist" pos="-.01 0 -.26">
<geom name="7a_lower_waist" fromto="0 -.06 0 0 .06 0" size=".06"/>
<joint name="7a_abdomen_z" pos="0 0 .065" axis="0 0 1" range="-45 45" class="big_stiff_joint"/>
<joint name="7a_abdomen_y" pos="0 0 .065" axis="0 1 0" range="-75 30" class="big_joint"/>
<body name="7a_pelvis" pos="0 0 -.165">
<joint name="7a_abdomen_x" pos="0 0 .1" axis="1 0 0" range="-35 35" class="big_joint"/>
<geom name="7a_butt" fromto="-.02 -.07 0 -.02 .07 0" size=".09"/>
<body name="7a_right_thigh" pos="0 -.1 -.04">
<joint name="7a_right_hip_x" axis="1 0 0" range="-25 5" class="big_joint"/>
<joint name="7a_right_hip_z" axis="0 0 1" range="-60 35" class="big_joint"/>
<joint name="7a_right_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
<geom name="7a_right_thigh" fromto="0 0 0 0 .01 -.34" size=".06"/>
<body name="7a_right_shin" pos="0 .01 -.403">
<joint name="7a_right_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
<geom name="7a_right_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
<body name="7a_right_foot" pos="0 0 -.39">
<joint name="7a_right_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
<joint name="7a_right_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
<geom name="7a_right_right_foot" fromto="-.07 -.02 0 .14 -.04 0" size=".027"/>
<geom name="7a_left_right_foot" fromto="-.07 0 0 .14 .02 0" size=".027"/>
</body>
</body>
</body>
<body name="7a_left_thigh" pos="0 .1 -.04">
<joint name="7a_left_hip_x" axis="-1 0 0" range="-25 5" class="big_joint"/>
<joint name="7a_left_hip_z" axis="0 0 -1" range="-60 35" class="big_joint"/>
<joint name="7a_left_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
<geom name="7a_left_thigh" fromto="0 0 0 0 -.01 -.34" size=".06"/>
<body name="7a_left_shin" pos="0 -.01 -.403">
<joint name="7a_left_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
<geom name="7a_left_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
<body name="7a_left_foot" pos="0 0 -.39">
<joint name="7a_left_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
<joint name="7a_left_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
<geom name="7a_left_left_foot" fromto="-.07 .02 0 .14 .04 0" size=".027"/>
<geom name="7a_right_left_foot" fromto="-.07 0 0 .14 -.02 0" size=".027"/>
</body>
</body>
</body>
</body>
</body>
<body name="7a_right_upper_arm" pos="0 -.17 .06">
<joint name="7a_right_shoulder1" axis="2 1 1" range="-85 60"/>
<joint name="7a_right_shoulder2" axis="0 -1 1" range="-85 60"/>
<geom name="7a_right_upper_arm" fromto="0 0 0 .16 -.16 -.16" size=".04 .16"/>
<body name="7a_right_lower_arm" pos=".18 -.18 -.18">
<joint name="7a_right_elbow" axis="0 -1 1" range="-90 50" stiffness="0"/>
<geom name="7a_right_lower_arm" fromto=".01 .01 .01 .17 .17 .17" size=".031"/>
<body name="7a_right_hand" pos=".18 .18 .18">
<geom name="7a_right_hand" type="sphere" size=".04" zaxis="1 1 1"/>
</body>
</body>
</body>
<body name="7a_left_upper_arm" pos="0 .17 .06">
<joint name="7a_left_shoulder1" axis="2 -1 1" range="-60 85"/>
<joint name="7a_left_shoulder2" axis="0 1 1" range="-60 85"/>
<geom name="7a_left_upper_arm" fromto="0 0 0 .16 .16 -.16" size=".04 .16"/>
<body name="7a_left_lower_arm" pos=".18 .18 -.18">
<joint name="7a_left_elbow" axis="0 -1 -1" range="-90 50" stiffness="0"/>
<geom name="7a_left_lower_arm" fromto=".01 -.01 .01 .17 -.17 .17" size=".031"/>
<body name="7a_left_hand" pos=".18 -.18 .18">
<geom name="7a_left_hand" type="sphere" size=".04" zaxis="1 -1 1"/>
</body>
</body>
</body>
</body>
</worldbody>
<actuator>
<motor gear="40" joint="1a_abdomen_y"/>
<motor gear="40" joint="1a_abdomen_z"/>
<motor gear="40" joint="1a_abdomen_x"/>
<motor gear="40" joint="1a_right_hip_x"/>
<motor gear="40" joint="1a_right_hip_z"/>
<motor gear="120" joint="1a_right_hip_y"/>
<motor gear="80" joint="1a_right_knee"/>
<motor gear="20" joint="1a_right_ankle_x"/>
<motor gear="20" joint="1a_right_ankle_y"/>
<motor gear="40" joint="1a_left_hip_x"/>
<motor gear="40" joint="1a_left_hip_z"/>
<motor gear="120" joint="1a_left_hip_y"/>
<motor gear="80" joint="1a_left_knee"/>
<motor gear="20" joint="1a_left_ankle_x"/>
<motor gear="20" joint="1a_left_ankle_y"/>
<motor gear="20" joint="1a_right_shoulder1"/>
<motor gear="20" joint="1a_right_shoulder2"/>
<motor gear="40" joint="1a_right_elbow"/>
<motor gear="20" joint="1a_left_shoulder1"/>
<motor gear="20" joint="1a_left_shoulder2"/>
<motor gear="40" joint="1a_left_elbow"/>
</actuator>
<actuator>
<motor gear="40" joint="2a_abdomen_y"/>
<motor gear="40" joint="2a_abdomen_z"/>
<motor gear="40" joint="2a_abdomen_x"/>
<motor gear="40" joint="2a_right_hip_x"/>
<motor gear="40" joint="2a_right_hip_z"/>
<motor gear="120" joint="2a_right_hip_y"/>
<motor gear="80" joint="2a_right_knee"/>
<motor gear="20" joint="2a_right_ankle_x"/>
<motor gear="20" joint="2a_right_ankle_y"/>
<motor gear="40" joint="2a_left_hip_x"/>
<motor gear="40" joint="2a_left_hip_z"/>
<motor gear="120" joint="2a_left_hip_y"/>
<motor gear="80" joint="2a_left_knee"/>
<motor gear="20" joint="2a_left_ankle_x"/>
<motor gear="20" joint="2a_left_ankle_y"/>
<motor gear="20" joint="2a_right_shoulder1"/>
<motor gear="20" joint="2a_right_shoulder2"/>
<motor gear="40" joint="2a_right_elbow"/>
<motor gear="20" joint="2a_left_shoulder1"/>
<motor gear="20" joint="2a_left_shoulder2"/>
<motor gear="40" joint="2a_left_elbow"/>
</actuator>
<actuator>
<motor gear="40" joint="3a_abdomen_y"/>
<motor gear="40" joint="3a_abdomen_z"/>
<motor gear="40" joint="3a_abdomen_x"/>
<motor gear="40" joint="3a_right_hip_x"/>
<motor gear="40" joint="3a_right_hip_z"/>
<motor gear="120" joint="3a_right_hip_y"/>
<motor gear="80" joint="3a_right_knee"/>
<motor gear="20" joint="3a_right_ankle_x"/>
<motor gear="20" joint="3a_right_ankle_y"/>
<motor gear="40" joint="3a_left_hip_x"/>
<motor gear="40" joint="3a_left_hip_z"/>
<motor gear="120" joint="3a_left_hip_y"/>
<motor gear="80" joint="3a_left_knee"/>
<motor gear="20" joint="3a_left_ankle_x"/>
<motor gear="20" joint="3a_left_ankle_y"/>
<motor gear="20" joint="3a_right_shoulder1"/>
<motor gear="20" joint="3a_right_shoulder2"/>
<motor gear="40" joint="3a_right_elbow"/>
<motor gear="20" joint="3a_left_shoulder1"/>
<motor gear="20" joint="3a_left_shoulder2"/>
<motor gear="40" joint="3a_left_elbow"/>
</actuator>
<actuator>
<motor gear="40" joint="4a_abdomen_y"/>
<motor gear="40" joint="4a_abdomen_z"/>
<motor gear="40" joint="4a_abdomen_x"/>
<motor gear="40" joint="4a_right_hip_x"/>
<motor gear="40" joint="4a_right_hip_z"/>
<motor gear="120" joint="4a_right_hip_y"/>
<motor gear="80" joint="4a_right_knee"/>
<motor gear="20" joint="4a_right_ankle_x"/>
<motor gear="20" joint="4a_right_ankle_y"/>
<motor gear="40" joint="4a_left_hip_x"/>
<motor gear="40" joint="4a_left_hip_z"/>
<motor gear="120" joint="4a_left_hip_y"/>
<motor gear="80" joint="4a_left_knee"/>
<motor gear="20" joint="4a_left_ankle_x"/>
<motor gear="20" joint="4a_left_ankle_y"/>
<motor gear="20" joint="4a_right_shoulder1"/>
<motor gear="20" joint="4a_right_shoulder2"/>
<motor gear="40" joint="4a_right_elbow"/>
<motor gear="20" joint="4a_left_shoulder1"/>
<motor gear="20" joint="4a_left_shoulder2"/>
<motor gear="40" joint="4a_left_elbow"/>
</actuator>
<actuator>
<motor gear="40" joint="5a_abdomen_y"/>
<motor gear="40" joint="5a_abdomen_z"/>
<motor gear="40" joint="5a_abdomen_x"/>
<motor gear="40" joint="5a_right_hip_x"/>
<motor gear="40" joint="5a_right_hip_z"/>
<motor gear="120" joint="5a_right_hip_y"/>
<motor gear="80" joint="5a_right_knee"/>
<motor gear="20" joint="5a_right_ankle_x"/>
<motor gear="20" joint="5a_right_ankle_y"/>
<motor gear="40" joint="5a_left_hip_x"/>
<motor gear="40" joint="5a_left_hip_z"/>
<motor gear="120" joint="5a_left_hip_y"/>
<motor gear="80" joint="5a_left_knee"/>
<motor gear="20" joint="5a_left_ankle_x"/>
<motor gear="20" joint="5a_left_ankle_y"/>
<motor gear="20" joint="5a_right_shoulder1"/>
<motor gear="20" joint="5a_right_shoulder2"/>
<motor gear="40" joint="5a_right_elbow"/>
<motor gear="20" joint="5a_left_shoulder1"/>
<motor gear="20" joint="5a_left_shoulder2"/>
<motor gear="40" joint="5a_left_elbow"/>
</actuator>
<actuator>
<motor gear="40" joint="6a_abdomen_y"/>
<motor gear="40" joint="6a_abdomen_z"/>
<motor gear="40" joint="6a_abdomen_x"/>
<motor gear="40" joint="6a_right_hip_x"/>
<motor gear="40" joint="6a_right_hip_z"/>
<motor gear="120" joint="6a_right_hip_y"/>
<motor gear="80" joint="6a_right_knee"/>
<motor gear="20" joint="6a_right_ankle_x"/>
<motor gear="20" joint="6a_right_ankle_y"/>
<motor gear="40" joint="6a_left_hip_x"/>
<motor gear="40" joint="6a_left_hip_z"/>
<motor gear="120" joint="6a_left_hip_y"/>
<motor gear="80" joint="6a_left_knee"/>
<motor gear="20" joint="6a_left_ankle_x"/>
<motor gear="20" joint="6a_left_ankle_y"/>
<motor gear="20" joint="6a_right_shoulder1"/>
<motor gear="20" joint="6a_right_shoulder2"/>
<motor gear="40" joint="6a_right_elbow"/>
<motor gear="20" joint="6a_left_shoulder1"/>
<motor gear="20" joint="6a_left_shoulder2"/>
<motor gear="40" joint="6a_left_elbow"/>
</actuator>
<actuator>
<motor gear="40" joint="7a_abdomen_y"/>
<motor gear="40" joint="7a_abdomen_z"/>
<motor gear="40" joint="7a_abdomen_x"/>
<motor gear="40" joint="7a_right_hip_x"/>
<motor gear="40" joint="7a_right_hip_z"/>
<motor gear="120" joint="7a_right_hip_y"/>
<motor gear="80" joint="7a_right_knee"/>
<motor gear="20" joint="7a_right_ankle_x"/>
<motor gear="20" joint="7a_right_ankle_y"/>
<motor gear="40" joint="7a_left_hip_x"/>
<motor gear="40" joint="7a_left_hip_z"/>
<motor gear="120" joint="7a_left_hip_y"/>
<motor gear="80" joint="7a_left_knee"/>
<motor gear="20" joint="7a_left_ankle_x"/>
<motor gear="20" joint="7a_left_ankle_y"/>
<motor gear="20" joint="7a_right_shoulder1"/>
<motor gear="20" joint="7a_right_shoulder2"/>
<motor gear="40" joint="7a_right_elbow"/>
<motor gear="20" joint="7a_left_shoulder1"/>
<motor gear="20" joint="7a_left_shoulder2"/>
<motor gear="40" joint="7a_left_elbow"/>
</actuator>
</mujoco>
mjx/mujoco/mjx/test_data/humanoid/08_humanoids.xml
+879
View File
@@ -0,0 +1,879 @@
<mujoco model="08 Humanoids">
<option timestep="0.005" solver="Newton" iterations="1" ls_iterations="4">
<flag eulerdamp="disable"/>
</option>
<custom>
<numeric data="32" name="max_contact_points"/>
<numeric data="32" name="max_geom_pairs"/>
</custom>
<size memory="100M"/>
<asset>
<texture type="skybox" builtin="gradient" rgb1=".3 .5 .7" rgb2="0 0 0" width="512" height="512"/>
<texture name="body" type="cube" builtin="flat" mark="cross" width="128" height="128"
rgb1="0.8 0.6 0.4" rgb2="0.8 0.6 0.4" markrgb="1 1 1" random="0.01"/>
<material name="body" texture="body" texuniform="true" rgba="0.8 0.6 .4 1"/>
<texture name="grid" type="2d" builtin="checker" width="512" height="512" rgb1=".1 .2 .3" rgb2=".2 .3 .4"/>
<material name="grid" texture="grid" texrepeat="1 1" texuniform="true" reflectance=".2"/>
</asset>
<default>
<motor ctrlrange="-1 1" ctrllimited="true"/>
<joint type="hinge" damping=".2" stiffness="1" armature=".01" limited="true" solimplimit="0 .99 .01"/>
<default class="big_joint">
<joint damping="5" stiffness="10"/>
<default class="big_stiff_joint">
<joint stiffness="20"/>
</default>
</default>
<default class="body1">
<geom type="capsule" condim="3" friction=".7" solimp=".9 .99 .003" solref=".015 1" material="body" contype="2" conaffinity="509"/>
</default>
<default class="body2">
<geom type="capsule" condim="3" friction=".7" solimp=".9 .99 .003" solref=".015 1" material="body" contype="4" conaffinity="507"/>
</default>
<default class="body3">
<geom type="capsule" condim="3" friction=".7" solimp=".9 .99 .003" solref=".015 1" material="body" contype="8" conaffinity="503"/>
</default>
<default class="body4">
<geom type="capsule" condim="3" friction=".7" solimp=".9 .99 .003" solref=".015 1" material="body" contype="16" conaffinity="495"/>
</default>
<default class="body5">
<geom type="capsule" condim="3" friction=".7" solimp=".9 .99 .003" solref=".015 1" material="body" contype="32" conaffinity="479"/>
</default>
<default class="body6">
<geom type="capsule" condim="3" friction=".7" solimp=".9 .99 .003" solref=".015 1" material="body" contype="64" conaffinity="447"/>
</default>
<default class="body7">
<geom type="capsule" condim="3" friction=".7" solimp=".9 .99 .003" solref=".015 1" material="body" contype="128" conaffinity="383"/>
</default>
<default class="body8">
<geom type="capsule" condim="3" friction=".7" solimp=".9 .99 .003" solref=".015 1" material="body" contype="256" conaffinity="255"/>
</default>
</default>
<visual>
<map force="0.1" zfar="30"/>
<rgba haze="0.15 0.25 0.35 1"/>
<quality shadowsize="4096"/>
<global offwidth="800" offheight="800"/>
</visual>
<worldbody>
<geom size="10 10 .05" type="plane" material="grid" condim="3"/>
<light dir=".2 1 -.4" diffuse=".8 .8 .8" specular="0.3 0.3 0.3" pos="-2 -10 4" cutoff="35"/>
<light dir="-.2 1 -.4" diffuse=".8 .8 .8" specular="0.3 0.3 0.3" pos="2 -10 4" cutoff="35"/>
<body name="1a_torso" pos="-1 0 1.5" childclass="body1">
<camera name="1a_back" pos="-3 0 1" xyaxes="0 -1 0 1 0 2" mode="trackcom"/>
<camera name="1a_side" pos="0 -3 1" xyaxes="1 0 0 0 1 2" mode="trackcom"/>
<freejoint name="1a_root"/>
<geom name="1a_torso" fromto="0 -.07 0 0 .07 0" size=".07"/>
<geom name="1a_upper_waist" fromto="-.01 -.06 -.12 -.01 .06 -.12" size=".06"/>
<body name="1a_head" pos="0 0 .19">
<geom name="1a_head" type="sphere" size=".09"/>
<camera name="1a_egocentric" pos=".09 0 0" xyaxes="0 -1 0 .1 0 1" fovy="80"/>
</body>
<body name="1a_lower_waist" pos="-.01 0 -.26">
<geom name="1a_lower_waist" fromto="0 -.06 0 0 .06 0" size=".06"/>
<joint name="1a_abdomen_z" pos="0 0 .065" axis="0 0 1" range="-45 45" class="big_stiff_joint"/>
<joint name="1a_abdomen_y" pos="0 0 .065" axis="0 1 0" range="-75 30" class="big_joint"/>
<body name="1a_pelvis" pos="0 0 -.165">
<joint name="1a_abdomen_x" pos="0 0 .1" axis="1 0 0" range="-35 35" class="big_joint"/>
<geom name="1a_butt" fromto="-.02 -.07 0 -.02 .07 0" size=".09"/>
<body name="1a_right_thigh" pos="0 -.1 -.04">
<joint name="1a_right_hip_x" axis="1 0 0" range="-25 5" class="big_joint"/>
<joint name="1a_right_hip_z" axis="0 0 1" range="-60 35" class="big_joint"/>
<joint name="1a_right_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
<geom name="1a_right_thigh" fromto="0 0 0 0 .01 -.34" size=".06"/>
<body name="1a_right_shin" pos="0 .01 -.403">
<joint name="1a_right_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
<geom name="1a_right_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
<body name="1a_right_foot" pos="0 0 -.39">
<joint name="1a_right_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
<joint name="1a_right_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
<geom name="1a_right_right_foot" fromto="-.07 -.02 0 .14 -.04 0" size=".027"/>
<geom name="1a_left_right_foot" fromto="-.07 0 0 .14 .02 0" size=".027"/>
</body>
</body>
</body>
<body name="1a_left_thigh" pos="0 .1 -.04">
<joint name="1a_left_hip_x" axis="-1 0 0" range="-25 5" class="big_joint"/>
<joint name="1a_left_hip_z" axis="0 0 -1" range="-60 35" class="big_joint"/>
<joint name="1a_left_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
<geom name="1a_left_thigh" fromto="0 0 0 0 -.01 -.34" size=".06"/>
<body name="1a_left_shin" pos="0 -.01 -.403">
<joint name="1a_left_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
<geom name="1a_left_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
<body name="1a_left_foot" pos="0 0 -.39">
<joint name="1a_left_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
<joint name="1a_left_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
<geom name="1a_left_left_foot" fromto="-.07 .02 0 .14 .04 0" size=".027"/>
<geom name="1a_right_left_foot" fromto="-.07 0 0 .14 -.02 0" size=".027"/>
</body>
</body>
</body>
</body>
</body>
<body name="1a_right_upper_arm" pos="0 -.17 .06">
<joint name="1a_right_shoulder1" axis="2 1 1" range="-85 60"/>
<joint name="1a_right_shoulder2" axis="0 -1 1" range="-85 60"/>
<geom name="1a_right_upper_arm" fromto="0 0 0 .16 -.16 -.16" size=".04 .16"/>
<body name="1a_right_lower_arm" pos=".18 -.18 -.18">
<joint name="1a_right_elbow" axis="0 -1 1" range="-90 50" stiffness="0"/>
<geom name="1a_right_lower_arm" fromto=".01 .01 .01 .17 .17 .17" size=".031"/>
<body name="1a_right_hand" pos=".18 .18 .18">
<geom name="1a_right_hand" type="sphere" size=".04" zaxis="1 1 1"/>
</body>
</body>
</body>
<body name="1a_left_upper_arm" pos="0 .17 .06">
<joint name="1a_left_shoulder1" axis="2 -1 1" range="-60 85"/>
<joint name="1a_left_shoulder2" axis="0 1 1" range="-60 85"/>
<geom name="1a_left_upper_arm" fromto="0 0 0 .16 .16 -.16" size=".04 .16"/>
<body name="1a_left_lower_arm" pos=".18 .18 -.18">
<joint name="1a_left_elbow" axis="0 -1 -1" range="-90 50" stiffness="0"/>
<geom name="1a_left_lower_arm" fromto=".01 -.01 .01 .17 -.17 .17" size=".031"/>
<body name="1a_left_hand" pos=".18 -.18 .18">
<geom name="1a_left_hand" type="sphere" size=".04" zaxis="1 -1 1"/>
</body>
</body>
</body>
</body>
<body name="2a_torso" pos="-1 1 1.5" childclass="body2">
<camera name="2a_back" pos="-3 0 1" xyaxes="0 -1 0 1 0 2" mode="trackcom"/>
<camera name="2a_side" pos="0 -3 1" xyaxes="1 0 0 0 1 2" mode="trackcom"/>
<freejoint name="2a_root"/>
<geom name="2a_torso" fromto="0 -.07 0 0 .07 0" size=".07"/>
<geom name="2a_upper_waist" fromto="-.01 -.06 -.12 -.01 .06 -.12" size=".06"/>
<body name="2a_head" pos="0 0 .19">
<geom name="2a_head" type="sphere" size=".09"/>
<camera name="2a_egocentric" pos=".09 0 0" xyaxes="0 -1 0 .1 0 1" fovy="80"/>
</body>
<body name="2a_lower_waist" pos="-.01 0 -.26">
<geom name="2a_lower_waist" fromto="0 -.06 0 0 .06 0" size=".06"/>
<joint name="2a_abdomen_z" pos="0 0 .065" axis="0 0 1" range="-45 45" class="big_stiff_joint"/>
<joint name="2a_abdomen_y" pos="0 0 .065" axis="0 1 0" range="-75 30" class="big_joint"/>
<body name="2a_pelvis" pos="0 0 -.165">
<joint name="2a_abdomen_x" pos="0 0 .1" axis="1 0 0" range="-35 35" class="big_joint"/>
<geom name="2a_butt" fromto="-.02 -.07 0 -.02 .07 0" size=".09"/>
<body name="2a_right_thigh" pos="0 -.1 -.04">
<joint name="2a_right_hip_x" axis="1 0 0" range="-25 5" class="big_joint"/>
<joint name="2a_right_hip_z" axis="0 0 1" range="-60 35" class="big_joint"/>
<joint name="2a_right_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
<geom name="2a_right_thigh" fromto="0 0 0 0 .01 -.34" size=".06"/>
<body name="2a_right_shin" pos="0 .01 -.403">
<joint name="2a_right_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
<geom name="2a_right_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
<body name="2a_right_foot" pos="0 0 -.39">
<joint name="2a_right_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
<joint name="2a_right_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
<geom name="2a_right_right_foot" fromto="-.07 -.02 0 .14 -.04 0" size=".027"/>
<geom name="2a_left_right_foot" fromto="-.07 0 0 .14 .02 0" size=".027"/>
</body>
</body>
</body>
<body name="2a_left_thigh" pos="0 .1 -.04">
<joint name="2a_left_hip_x" axis="-1 0 0" range="-25 5" class="big_joint"/>
<joint name="2a_left_hip_z" axis="0 0 -1" range="-60 35" class="big_joint"/>
<joint name="2a_left_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
<geom name="2a_left_thigh" fromto="0 0 0 0 -.01 -.34" size=".06"/>
<body name="2a_left_shin" pos="0 -.01 -.403">
<joint name="2a_left_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
<geom name="2a_left_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
<body name="2a_left_foot" pos="0 0 -.39">
<joint name="2a_left_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
<joint name="2a_left_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
<geom name="2a_left_left_foot" fromto="-.07 .02 0 .14 .04 0" size=".027"/>
<geom name="2a_right_left_foot" fromto="-.07 0 0 .14 -.02 0" size=".027"/>
</body>
</body>
</body>
</body>
</body>
<body name="2a_right_upper_arm" pos="0 -.17 .06">
<joint name="2a_right_shoulder1" axis="2 1 1" range="-85 60"/>
<joint name="2a_right_shoulder2" axis="0 -1 1" range="-85 60"/>
<geom name="2a_right_upper_arm" fromto="0 0 0 .16 -.16 -.16" size=".04 .16"/>
<body name="2a_right_lower_arm" pos=".18 -.18 -.18">
<joint name="2a_right_elbow" axis="0 -1 1" range="-90 50" stiffness="0"/>
<geom name="2a_right_lower_arm" fromto=".01 .01 .01 .17 .17 .17" size=".031"/>
<body name="2a_right_hand" pos=".18 .18 .18">
<geom name="2a_right_hand" type="sphere" size=".04" zaxis="1 1 1"/>
</body>
</body>
</body>
<body name="2a_left_upper_arm" pos="0 .17 .06">
<joint name="2a_left_shoulder1" axis="2 -1 1" range="-60 85"/>
<joint name="2a_left_shoulder2" axis="0 1 1" range="-60 85"/>
<geom name="2a_left_upper_arm" fromto="0 0 0 .16 .16 -.16" size=".04 .16"/>
<body name="2a_left_lower_arm" pos=".18 .18 -.18">
<joint name="2a_left_elbow" axis="0 -1 -1" range="-90 50" stiffness="0"/>
<geom name="2a_left_lower_arm" fromto=".01 -.01 .01 .17 -.17 .17" size=".031"/>
<body name="2a_left_hand" pos=".18 -.18 .18">
<geom name="2a_left_hand" type="sphere" size=".04" zaxis="1 -1 1"/>
</body>
</body>
</body>
</body>
<body name="3a_torso" pos="-1 -1 1.5" childclass="body3">
<camera name="3a_back" pos="-3 0 1" xyaxes="0 -1 0 1 0 2" mode="trackcom"/>
<camera name="3a_side" pos="0 -3 1" xyaxes="1 0 0 0 1 2" mode="trackcom"/>
<freejoint name="3a_root"/>
<geom name="3a_torso" fromto="0 -.07 0 0 .07 0" size=".07"/>
<geom name="3a_upper_waist" fromto="-.01 -.06 -.12 -.01 .06 -.12" size=".06"/>
<body name="3a_head" pos="0 0 .19">
<geom name="3a_head" type="sphere" size=".09"/>
<camera name="3a_egocentric" pos=".09 0 0" xyaxes="0 -1 0 .1 0 1" fovy="80"/>
</body>
<body name="3a_lower_waist" pos="-.01 0 -.26">
<geom name="3a_lower_waist" fromto="0 -.06 0 0 .06 0" size=".06"/>
<joint name="3a_abdomen_z" pos="0 0 .065" axis="0 0 1" range="-45 45" class="big_stiff_joint"/>
<joint name="3a_abdomen_y" pos="0 0 .065" axis="0 1 0" range="-75 30" class="big_joint"/>
<body name="3a_pelvis" pos="0 0 -.165">
<joint name="3a_abdomen_x" pos="0 0 .1" axis="1 0 0" range="-35 35" class="big_joint"/>
<geom name="3a_butt" fromto="-.02 -.07 0 -.02 .07 0" size=".09"/>
<body name="3a_right_thigh" pos="0 -.1 -.04">
<joint name="3a_right_hip_x" axis="1 0 0" range="-25 5" class="big_joint"/>
<joint name="3a_right_hip_z" axis="0 0 1" range="-60 35" class="big_joint"/>
<joint name="3a_right_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
<geom name="3a_right_thigh" fromto="0 0 0 0 .01 -.34" size=".06"/>
<body name="3a_right_shin" pos="0 .01 -.403">
<joint name="3a_right_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
<geom name="3a_right_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
<body name="3a_right_foot" pos="0 0 -.39">
<joint name="3a_right_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
<joint name="3a_right_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
<geom name="3a_right_right_foot" fromto="-.07 -.02 0 .14 -.04 0" size=".027"/>
<geom name="3a_left_right_foot" fromto="-.07 0 0 .14 .02 0" size=".027"/>
</body>
</body>
</body>
<body name="3a_left_thigh" pos="0 .1 -.04">
<joint name="3a_left_hip_x" axis="-1 0 0" range="-25 5" class="big_joint"/>
<joint name="3a_left_hip_z" axis="0 0 -1" range="-60 35" class="big_joint"/>
<joint name="3a_left_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
<geom name="3a_left_thigh" fromto="0 0 0 0 -.01 -.34" size=".06"/>
<body name="3a_left_shin" pos="0 -.01 -.403">
<joint name="3a_left_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
<geom name="3a_left_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
<body name="3a_left_foot" pos="0 0 -.39">
<joint name="3a_left_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
<joint name="3a_left_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
<geom name="3a_left_left_foot" fromto="-.07 .02 0 .14 .04 0" size=".027"/>
<geom name="3a_right_left_foot" fromto="-.07 0 0 .14 -.02 0" size=".027"/>
</body>
</body>
</body>
</body>
</body>
<body name="3a_right_upper_arm" pos="0 -.17 .06">
<joint name="3a_right_shoulder1" axis="2 1 1" range="-85 60"/>
<joint name="3a_right_shoulder2" axis="0 -1 1" range="-85 60"/>
<geom name="3a_right_upper_arm" fromto="0 0 0 .16 -.16 -.16" size=".04 .16"/>
<body name="3a_right_lower_arm" pos=".18 -.18 -.18">
<joint name="3a_right_elbow" axis="0 -1 1" range="-90 50" stiffness="0"/>
<geom name="3a_right_lower_arm" fromto=".01 .01 .01 .17 .17 .17" size=".031"/>
<body name="3a_right_hand" pos=".18 .18 .18">
<geom name="3a_right_hand" type="sphere" size=".04" zaxis="1 1 1"/>
</body>
</body>
</body>
<body name="3a_left_upper_arm" pos="0 .17 .06">
<joint name="3a_left_shoulder1" axis="2 -1 1" range="-60 85"/>
<joint name="3a_left_shoulder2" axis="0 1 1" range="-60 85"/>
<geom name="3a_left_upper_arm" fromto="0 0 0 .16 .16 -.16" size=".04 .16"/>
<body name="3a_left_lower_arm" pos=".18 .18 -.18">
<joint name="3a_left_elbow" axis="0 -1 -1" range="-90 50" stiffness="0"/>
<geom name="3a_left_lower_arm" fromto=".01 -.01 .01 .17 -.17 .17" size=".031"/>
<body name="3a_left_hand" pos=".18 -.18 .18">
<geom name="3a_left_hand" type="sphere" size=".04" zaxis="1 -1 1"/>
</body>
</body>
</body>
</body>
<body name="4a_torso" pos="-1 2 1.5" childclass="body4">
<camera name="4a_back" pos="-3 0 1" xyaxes="0 -1 0 1 0 2" mode="trackcom"/>
<camera name="4a_side" pos="0 -3 1" xyaxes="1 0 0 0 1 2" mode="trackcom"/>
<freejoint name="4a_root"/>
<geom name="4a_torso" fromto="0 -.07 0 0 .07 0" size=".07"/>
<geom name="4a_upper_waist" fromto="-.01 -.06 -.12 -.01 .06 -.12" size=".06"/>
<body name="4a_head" pos="0 0 .19">
<geom name="4a_head" type="sphere" size=".09"/>
<camera name="4a_egocentric" pos=".09 0 0" xyaxes="0 -1 0 .1 0 1" fovy="80"/>
</body>
<body name="4a_lower_waist" pos="-.01 0 -.26">
<geom name="4a_lower_waist" fromto="0 -.06 0 0 .06 0" size=".06"/>
<joint name="4a_abdomen_z" pos="0 0 .065" axis="0 0 1" range="-45 45" class="big_stiff_joint"/>
<joint name="4a_abdomen_y" pos="0 0 .065" axis="0 1 0" range="-75 30" class="big_joint"/>
<body name="4a_pelvis" pos="0 0 -.165">
<joint name="4a_abdomen_x" pos="0 0 .1" axis="1 0 0" range="-35 35" class="big_joint"/>
<geom name="4a_butt" fromto="-.02 -.07 0 -.02 .07 0" size=".09"/>
<body name="4a_right_thigh" pos="0 -.1 -.04">
<joint name="4a_right_hip_x" axis="1 0 0" range="-25 5" class="big_joint"/>
<joint name="4a_right_hip_z" axis="0 0 1" range="-60 35" class="big_joint"/>
<joint name="4a_right_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
<geom name="4a_right_thigh" fromto="0 0 0 0 .01 -.34" size=".06"/>
<body name="4a_right_shin" pos="0 .01 -.403">
<joint name="4a_right_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
<geom name="4a_right_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
<body name="4a_right_foot" pos="0 0 -.39">
<joint name="4a_right_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
<joint name="4a_right_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
<geom name="4a_right_right_foot" fromto="-.07 -.02 0 .14 -.04 0" size=".027"/>
<geom name="4a_left_right_foot" fromto="-.07 0 0 .14 .02 0" size=".027"/>
</body>
</body>
</body>
<body name="4a_left_thigh" pos="0 .1 -.04">
<joint name="4a_left_hip_x" axis="-1 0 0" range="-25 5" class="big_joint"/>
<joint name="4a_left_hip_z" axis="0 0 -1" range="-60 35" class="big_joint"/>
<joint name="4a_left_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
<geom name="4a_left_thigh" fromto="0 0 0 0 -.01 -.34" size=".06"/>
<body name="4a_left_shin" pos="0 -.01 -.403">
<joint name="4a_left_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
<geom name="4a_left_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
<body name="4a_left_foot" pos="0 0 -.39">
<joint name="4a_left_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
<joint name="4a_left_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
<geom name="4a_left_left_foot" fromto="-.07 .02 0 .14 .04 0" size=".027"/>
<geom name="4a_right_left_foot" fromto="-.07 0 0 .14 -.02 0" size=".027"/>
</body>
</body>
</body>
</body>
</body>
<body name="4a_right_upper_arm" pos="0 -.17 .06">
<joint name="4a_right_shoulder1" axis="2 1 1" range="-85 60"/>
<joint name="4a_right_shoulder2" axis="0 -1 1" range="-85 60"/>
<geom name="4a_right_upper_arm" fromto="0 0 0 .16 -.16 -.16" size=".04 .16"/>
<body name="4a_right_lower_arm" pos=".18 -.18 -.18">
<joint name="4a_right_elbow" axis="0 -1 1" range="-90 50" stiffness="0"/>
<geom name="4a_right_lower_arm" fromto=".01 .01 .01 .17 .17 .17" size=".031"/>
<body name="4a_right_hand" pos=".18 .18 .18">
<geom name="4a_right_hand" type="sphere" size=".04" zaxis="1 1 1"/>
</body>
</body>
</body>
<body name="4a_left_upper_arm" pos="0 .17 .06">
<joint name="4a_left_shoulder1" axis="2 -1 1" range="-60 85"/>
<joint name="4a_left_shoulder2" axis="0 1 1" range="-60 85"/>
<geom name="4a_left_upper_arm" fromto="0 0 0 .16 .16 -.16" size=".04 .16"/>
<body name="4a_left_lower_arm" pos=".18 .18 -.18">
<joint name="4a_left_elbow" axis="0 -1 -1" range="-90 50" stiffness="0"/>
<geom name="4a_left_lower_arm" fromto=".01 -.01 .01 .17 -.17 .17" size=".031"/>
<body name="4a_left_hand" pos=".18 -.18 .18">
<geom name="4a_left_hand" type="sphere" size=".04" zaxis="1 -1 1"/>
</body>
</body>
</body>
</body>
<body name="5a_torso" pos="-1 -2 1.5" childclass="body5">
<camera name="5a_back" pos="-3 0 1" xyaxes="0 -1 0 1 0 2" mode="trackcom"/>
<camera name="5a_side" pos="0 -3 1" xyaxes="1 0 0 0 1 2" mode="trackcom"/>
<freejoint name="5a_root"/>
<geom name="5a_torso" fromto="0 -.07 0 0 .07 0" size=".07"/>
<geom name="5a_upper_waist" fromto="-.01 -.06 -.12 -.01 .06 -.12" size=".06"/>
<body name="5a_head" pos="0 0 .19">
<geom name="5a_head" type="sphere" size=".09"/>
<camera name="5a_egocentric" pos=".09 0 0" xyaxes="0 -1 0 .1 0 1" fovy="80"/>
</body>
<body name="5a_lower_waist" pos="-.01 0 -.26">
<geom name="5a_lower_waist" fromto="0 -.06 0 0 .06 0" size=".06"/>
<joint name="5a_abdomen_z" pos="0 0 .065" axis="0 0 1" range="-45 45" class="big_stiff_joint"/>
<joint name="5a_abdomen_y" pos="0 0 .065" axis="0 1 0" range="-75 30" class="big_joint"/>
<body name="5a_pelvis" pos="0 0 -.165">
<joint name="5a_abdomen_x" pos="0 0 .1" axis="1 0 0" range="-35 35" class="big_joint"/>
<geom name="5a_butt" fromto="-.02 -.07 0 -.02 .07 0" size=".09"/>
<body name="5a_right_thigh" pos="0 -.1 -.04">
<joint name="5a_right_hip_x" axis="1 0 0" range="-25 5" class="big_joint"/>
<joint name="5a_right_hip_z" axis="0 0 1" range="-60 35" class="big_joint"/>
<joint name="5a_right_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
<geom name="5a_right_thigh" fromto="0 0 0 0 .01 -.34" size=".06"/>
<body name="5a_right_shin" pos="0 .01 -.403">
<joint name="5a_right_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
<geom name="5a_right_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
<body name="5a_right_foot" pos="0 0 -.39">
<joint name="5a_right_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
<joint name="5a_right_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
<geom name="5a_right_right_foot" fromto="-.07 -.02 0 .14 -.04 0" size=".027"/>
<geom name="5a_left_right_foot" fromto="-.07 0 0 .14 .02 0" size=".027"/>
</body>
</body>
</body>
<body name="5a_left_thigh" pos="0 .1 -.04">
<joint name="5a_left_hip_x" axis="-1 0 0" range="-25 5" class="big_joint"/>
<joint name="5a_left_hip_z" axis="0 0 -1" range="-60 35" class="big_joint"/>
<joint name="5a_left_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
<geom name="5a_left_thigh" fromto="0 0 0 0 -.01 -.34" size=".06"/>
<body name="5a_left_shin" pos="0 -.01 -.403">
<joint name="5a_left_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
<geom name="5a_left_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
<body name="5a_left_foot" pos="0 0 -.39">
<joint name="5a_left_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
<joint name="5a_left_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
<geom name="5a_left_left_foot" fromto="-.07 .02 0 .14 .04 0" size=".027"/>
<geom name="5a_right_left_foot" fromto="-.07 0 0 .14 -.02 0" size=".027"/>
</body>
</body>
</body>
</body>
</body>
<body name="5a_right_upper_arm" pos="0 -.17 .06">
<joint name="5a_right_shoulder1" axis="2 1 1" range="-85 60"/>
<joint name="5a_right_shoulder2" axis="0 -1 1" range="-85 60"/>
<geom name="5a_right_upper_arm" fromto="0 0 0 .16 -.16 -.16" size=".04 .16"/>
<body name="5a_right_lower_arm" pos=".18 -.18 -.18">
<joint name="5a_right_elbow" axis="0 -1 1" range="-90 50" stiffness="0"/>
<geom name="5a_right_lower_arm" fromto=".01 .01 .01 .17 .17 .17" size=".031"/>
<body name="5a_right_hand" pos=".18 .18 .18">
<geom name="5a_right_hand" type="sphere" size=".04" zaxis="1 1 1"/>
</body>
</body>
</body>
<body name="5a_left_upper_arm" pos="0 .17 .06">
<joint name="5a_left_shoulder1" axis="2 -1 1" range="-60 85"/>
<joint name="5a_left_shoulder2" axis="0 1 1" range="-60 85"/>
<geom name="5a_left_upper_arm" fromto="0 0 0 .16 .16 -.16" size=".04 .16"/>
<body name="5a_left_lower_arm" pos=".18 .18 -.18">
<joint name="5a_left_elbow" axis="0 -1 -1" range="-90 50" stiffness="0"/>
<geom name="5a_left_lower_arm" fromto=".01 -.01 .01 .17 -.17 .17" size=".031"/>
<body name="5a_left_hand" pos=".18 -.18 .18">
<geom name="5a_left_hand" type="sphere" size=".04" zaxis="1 -1 1"/>
</body>
</body>
</body>
</body>
<body name="6a_torso" pos="-1 3 1.5" childclass="body6">
<camera name="6a_back" pos="-3 0 1" xyaxes="0 -1 0 1 0 2" mode="trackcom"/>
<camera name="6a_side" pos="0 -3 1" xyaxes="1 0 0 0 1 2" mode="trackcom"/>
<freejoint name="6a_root"/>
<geom name="6a_torso" fromto="0 -.07 0 0 .07 0" size=".07"/>
<geom name="6a_upper_waist" fromto="-.01 -.06 -.12 -.01 .06 -.12" size=".06"/>
<body name="6a_head" pos="0 0 .19">
<geom name="6a_head" type="sphere" size=".09"/>
<camera name="6a_egocentric" pos=".09 0 0" xyaxes="0 -1 0 .1 0 1" fovy="80"/>
</body>
<body name="6a_lower_waist" pos="-.01 0 -.26">
<geom name="6a_lower_waist" fromto="0 -.06 0 0 .06 0" size=".06"/>
<joint name="6a_abdomen_z" pos="0 0 .065" axis="0 0 1" range="-45 45" class="big_stiff_joint"/>
<joint name="6a_abdomen_y" pos="0 0 .065" axis="0 1 0" range="-75 30" class="big_joint"/>
<body name="6a_pelvis" pos="0 0 -.165">
<joint name="6a_abdomen_x" pos="0 0 .1" axis="1 0 0" range="-35 35" class="big_joint"/>
<geom name="6a_butt" fromto="-.02 -.07 0 -.02 .07 0" size=".09"/>
<body name="6a_right_thigh" pos="0 -.1 -.04">
<joint name="6a_right_hip_x" axis="1 0 0" range="-25 5" class="big_joint"/>
<joint name="6a_right_hip_z" axis="0 0 1" range="-60 35" class="big_joint"/>
<joint name="6a_right_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
<geom name="6a_right_thigh" fromto="0 0 0 0 .01 -.34" size=".06"/>
<body name="6a_right_shin" pos="0 .01 -.403">
<joint name="6a_right_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
<geom name="6a_right_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
<body name="6a_right_foot" pos="0 0 -.39">
<joint name="6a_right_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
<joint name="6a_right_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
<geom name="6a_right_right_foot" fromto="-.07 -.02 0 .14 -.04 0" size=".027"/>
<geom name="6a_left_right_foot" fromto="-.07 0 0 .14 .02 0" size=".027"/>
</body>
</body>
</body>
<body name="6a_left_thigh" pos="0 .1 -.04">
<joint name="6a_left_hip_x" axis="-1 0 0" range="-25 5" class="big_joint"/>
<joint name="6a_left_hip_z" axis="0 0 -1" range="-60 35" class="big_joint"/>
<joint name="6a_left_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
<geom name="6a_left_thigh" fromto="0 0 0 0 -.01 -.34" size=".06"/>
<body name="6a_left_shin" pos="0 -.01 -.403">
<joint name="6a_left_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
<geom name="6a_left_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
<body name="6a_left_foot" pos="0 0 -.39">
<joint name="6a_left_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
<joint name="6a_left_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
<geom name="6a_left_left_foot" fromto="-.07 .02 0 .14 .04 0" size=".027"/>
<geom name="6a_right_left_foot" fromto="-.07 0 0 .14 -.02 0" size=".027"/>
</body>
</body>
</body>
</body>
</body>
<body name="6a_right_upper_arm" pos="0 -.17 .06">
<joint name="6a_right_shoulder1" axis="2 1 1" range="-85 60"/>
<joint name="6a_right_shoulder2" axis="0 -1 1" range="-85 60"/>
<geom name="6a_right_upper_arm" fromto="0 0 0 .16 -.16 -.16" size=".04 .16"/>
<body name="6a_right_lower_arm" pos=".18 -.18 -.18">
<joint name="6a_right_elbow" axis="0 -1 1" range="-90 50" stiffness="0"/>
<geom name="6a_right_lower_arm" fromto=".01 .01 .01 .17 .17 .17" size=".031"/>
<body name="6a_right_hand" pos=".18 .18 .18">
<geom name="6a_right_hand" type="sphere" size=".04" zaxis="1 1 1"/>
</body>
</body>
</body>
<body name="6a_left_upper_arm" pos="0 .17 .06">
<joint name="6a_left_shoulder1" axis="2 -1 1" range="-60 85"/>
<joint name="6a_left_shoulder2" axis="0 1 1" range="-60 85"/>
<geom name="6a_left_upper_arm" fromto="0 0 0 .16 .16 -.16" size=".04 .16"/>
<body name="6a_left_lower_arm" pos=".18 .18 -.18">
<joint name="6a_left_elbow" axis="0 -1 -1" range="-90 50" stiffness="0"/>
<geom name="6a_left_lower_arm" fromto=".01 -.01 .01 .17 -.17 .17" size=".031"/>
<body name="6a_left_hand" pos=".18 -.18 .18">
<geom name="6a_left_hand" type="sphere" size=".04" zaxis="1 -1 1"/>
</body>
</body>
</body>
</body>
<body name="7a_torso" pos="-1 -3 1.5" childclass="body7">
<camera name="7a_back" pos="-3 0 1" xyaxes="0 -1 0 1 0 2" mode="trackcom"/>
<camera name="7a_side" pos="0 -3 1" xyaxes="1 0 0 0 1 2" mode="trackcom"/>
<freejoint name="7a_root"/>
<geom name="7a_torso" fromto="0 -.07 0 0 .07 0" size=".07"/>
<geom name="7a_upper_waist" fromto="-.01 -.06 -.12 -.01 .06 -.12" size=".06"/>
<body name="7a_head" pos="0 0 .19">
<geom name="7a_head" type="sphere" size=".09"/>
<camera name="7a_egocentric" pos=".09 0 0" xyaxes="0 -1 0 .1 0 1" fovy="80"/>
</body>
<body name="7a_lower_waist" pos="-.01 0 -.26">
<geom name="7a_lower_waist" fromto="0 -.06 0 0 .06 0" size=".06"/>
<joint name="7a_abdomen_z" pos="0 0 .065" axis="0 0 1" range="-45 45" class="big_stiff_joint"/>
<joint name="7a_abdomen_y" pos="0 0 .065" axis="0 1 0" range="-75 30" class="big_joint"/>
<body name="7a_pelvis" pos="0 0 -.165">
<joint name="7a_abdomen_x" pos="0 0 .1" axis="1 0 0" range="-35 35" class="big_joint"/>
<geom name="7a_butt" fromto="-.02 -.07 0 -.02 .07 0" size=".09"/>
<body name="7a_right_thigh" pos="0 -.1 -.04">
<joint name="7a_right_hip_x" axis="1 0 0" range="-25 5" class="big_joint"/>
<joint name="7a_right_hip_z" axis="0 0 1" range="-60 35" class="big_joint"/>
<joint name="7a_right_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
<geom name="7a_right_thigh" fromto="0 0 0 0 .01 -.34" size=".06"/>
<body name="7a_right_shin" pos="0 .01 -.403">
<joint name="7a_right_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
<geom name="7a_right_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
<body name="7a_right_foot" pos="0 0 -.39">
<joint name="7a_right_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
<joint name="7a_right_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
<geom name="7a_right_right_foot" fromto="-.07 -.02 0 .14 -.04 0" size=".027"/>
<geom name="7a_left_right_foot" fromto="-.07 0 0 .14 .02 0" size=".027"/>
</body>
</body>
</body>
<body name="7a_left_thigh" pos="0 .1 -.04">
<joint name="7a_left_hip_x" axis="-1 0 0" range="-25 5" class="big_joint"/>
<joint name="7a_left_hip_z" axis="0 0 -1" range="-60 35" class="big_joint"/>
<joint name="7a_left_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
<geom name="7a_left_thigh" fromto="0 0 0 0 -.01 -.34" size=".06"/>
<body name="7a_left_shin" pos="0 -.01 -.403">
<joint name="7a_left_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
<geom name="7a_left_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
<body name="7a_left_foot" pos="0 0 -.39">
<joint name="7a_left_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
<joint name="7a_left_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
<geom name="7a_left_left_foot" fromto="-.07 .02 0 .14 .04 0" size=".027"/>
<geom name="7a_right_left_foot" fromto="-.07 0 0 .14 -.02 0" size=".027"/>
</body>
</body>
</body>
</body>
</body>
<body name="7a_right_upper_arm" pos="0 -.17 .06">
<joint name="7a_right_shoulder1" axis="2 1 1" range="-85 60"/>
<joint name="7a_right_shoulder2" axis="0 -1 1" range="-85 60"/>
<geom name="7a_right_upper_arm" fromto="0 0 0 .16 -.16 -.16" size=".04 .16"/>
<body name="7a_right_lower_arm" pos=".18 -.18 -.18">
<joint name="7a_right_elbow" axis="0 -1 1" range="-90 50" stiffness="0"/>
<geom name="7a_right_lower_arm" fromto=".01 .01 .01 .17 .17 .17" size=".031"/>
<body name="7a_right_hand" pos=".18 .18 .18">
<geom name="7a_right_hand" type="sphere" size=".04" zaxis="1 1 1"/>
</body>
</body>
</body>
<body name="7a_left_upper_arm" pos="0 .17 .06">
<joint name="7a_left_shoulder1" axis="2 -1 1" range="-60 85"/>
<joint name="7a_left_shoulder2" axis="0 1 1" range="-60 85"/>
<geom name="7a_left_upper_arm" fromto="0 0 0 .16 .16 -.16" size=".04 .16"/>
<body name="7a_left_lower_arm" pos=".18 .18 -.18">
<joint name="7a_left_elbow" axis="0 -1 -1" range="-90 50" stiffness="0"/>
<geom name="7a_left_lower_arm" fromto=".01 -.01 .01 .17 -.17 .17" size=".031"/>
<body name="7a_left_hand" pos=".18 -.18 .18">
<geom name="7a_left_hand" type="sphere" size=".04" zaxis="1 -1 1"/>
</body>
</body>
</body>
</body>
<body name="8a_torso" pos="-1 4 1.5" childclass="body8">
<camera name="8a_back" pos="-3 0 1" xyaxes="0 -1 0 1 0 2" mode="trackcom"/>
<camera name="8a_side" pos="0 -3 1" xyaxes="1 0 0 0 1 2" mode="trackcom"/>
<freejoint name="8a_root"/>
<geom name="8a_torso" fromto="0 -.07 0 0 .07 0" size=".07"/>
<geom name="8a_upper_waist" fromto="-.01 -.06 -.12 -.01 .06 -.12" size=".06"/>
<body name="8a_head" pos="0 0 .19">
<geom name="8a_head" type="sphere" size=".09"/>
<camera name="8a_egocentric" pos=".09 0 0" xyaxes="0 -1 0 .1 0 1" fovy="80"/>
</body>
<body name="8a_lower_waist" pos="-.01 0 -.26">
<geom name="8a_lower_waist" fromto="0 -.06 0 0 .06 0" size=".06"/>
<joint name="8a_abdomen_z" pos="0 0 .065" axis="0 0 1" range="-45 45" class="big_stiff_joint"/>
<joint name="8a_abdomen_y" pos="0 0 .065" axis="0 1 0" range="-75 30" class="big_joint"/>
<body name="8a_pelvis" pos="0 0 -.165">
<joint name="8a_abdomen_x" pos="0 0 .1" axis="1 0 0" range="-35 35" class="big_joint"/>
<geom name="8a_butt" fromto="-.02 -.07 0 -.02 .07 0" size=".09"/>
<body name="8a_right_thigh" pos="0 -.1 -.04">
<joint name="8a_right_hip_x" axis="1 0 0" range="-25 5" class="big_joint"/>
<joint name="8a_right_hip_z" axis="0 0 1" range="-60 35" class="big_joint"/>
<joint name="8a_right_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
<geom name="8a_right_thigh" fromto="0 0 0 0 .01 -.34" size=".06"/>
<body name="8a_right_shin" pos="0 .01 -.403">
<joint name="8a_right_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
<geom name="8a_right_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
<body name="8a_right_foot" pos="0 0 -.39">
<joint name="8a_right_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
<joint name="8a_right_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
<geom name="8a_right_right_foot" fromto="-.07 -.02 0 .14 -.04 0" size=".027"/>
<geom name="8a_left_right_foot" fromto="-.07 0 0 .14 .02 0" size=".027"/>
</body>
</body>
</body>
<body name="8a_left_thigh" pos="0 .1 -.04">
<joint name="8a_left_hip_x" axis="-1 0 0" range="-25 5" class="big_joint"/>
<joint name="8a_left_hip_z" axis="0 0 -1" range="-60 35" class="big_joint"/>
<joint name="8a_left_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
<geom name="8a_left_thigh" fromto="0 0 0 0 -.01 -.34" size=".06"/>
<body name="8a_left_shin" pos="0 -.01 -.403">
<joint name="8a_left_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
<geom name="8a_left_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
<body name="8a_left_foot" pos="0 0 -.39">
<joint name="8a_left_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
<joint name="8a_left_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
<geom name="8a_left_left_foot" fromto="-.07 .02 0 .14 .04 0" size=".027"/>
<geom name="8a_right_left_foot" fromto="-.07 0 0 .14 -.02 0" size=".027"/>
</body>
</body>
</body>
</body>
</body>
<body name="8a_right_upper_arm" pos="0 -.17 .06">
<joint name="8a_right_shoulder1" axis="2 1 1" range="-85 60"/>
<joint name="8a_right_shoulder2" axis="0 -1 1" range="-85 60"/>
<geom name="8a_right_upper_arm" fromto="0 0 0 .16 -.16 -.16" size=".04 .16"/>
<body name="8a_right_lower_arm" pos=".18 -.18 -.18">
<joint name="8a_right_elbow" axis="0 -1 1" range="-90 50" stiffness="0"/>
<geom name="8a_right_lower_arm" fromto=".01 .01 .01 .17 .17 .17" size=".031"/>
<body name="8a_right_hand" pos=".18 .18 .18">
<geom name="8a_right_hand" type="sphere" size=".04" zaxis="1 1 1"/>
</body>
</body>
</body>
<body name="8a_left_upper_arm" pos="0 .17 .06">
<joint name="8a_left_shoulder1" axis="2 -1 1" range="-60 85"/>
<joint name="8a_left_shoulder2" axis="0 1 1" range="-60 85"/>
<geom name="8a_left_upper_arm" fromto="0 0 0 .16 .16 -.16" size=".04 .16"/>
<body name="8a_left_lower_arm" pos=".18 .18 -.18">
<joint name="8a_left_elbow" axis="0 -1 -1" range="-90 50" stiffness="0"/>
<geom name="8a_left_lower_arm" fromto=".01 -.01 .01 .17 -.17 .17" size=".031"/>
<body name="8a_left_hand" pos=".18 -.18 .18">
<geom name="8a_left_hand" type="sphere" size=".04" zaxis="1 -1 1"/>
</body>
</body>
</body>
</body>
</worldbody>
<actuator>
<motor gear="40" joint="1a_abdomen_y"/>
<motor gear="40" joint="1a_abdomen_z"/>
<motor gear="40" joint="1a_abdomen_x"/>
<motor gear="40" joint="1a_right_hip_x"/>
<motor gear="40" joint="1a_right_hip_z"/>
<motor gear="120" joint="1a_right_hip_y"/>
<motor gear="80" joint="1a_right_knee"/>
<motor gear="20" joint="1a_right_ankle_x"/>
<motor gear="20" joint="1a_right_ankle_y"/>
<motor gear="40" joint="1a_left_hip_x"/>
<motor gear="40" joint="1a_left_hip_z"/>
<motor gear="120" joint="1a_left_hip_y"/>
<motor gear="80" joint="1a_left_knee"/>
<motor gear="20" joint="1a_left_ankle_x"/>
<motor gear="20" joint="1a_left_ankle_y"/>
<motor gear="20" joint="1a_right_shoulder1"/>
<motor gear="20" joint="1a_right_shoulder2"/>
<motor gear="40" joint="1a_right_elbow"/>
<motor gear="20" joint="1a_left_shoulder1"/>
<motor gear="20" joint="1a_left_shoulder2"/>
<motor gear="40" joint="1a_left_elbow"/>
</actuator>
<actuator>
<motor gear="40" joint="2a_abdomen_y"/>
<motor gear="40" joint="2a_abdomen_z"/>
<motor gear="40" joint="2a_abdomen_x"/>
<motor gear="40" joint="2a_right_hip_x"/>
<motor gear="40" joint="2a_right_hip_z"/>
<motor gear="120" joint="2a_right_hip_y"/>
<motor gear="80" joint="2a_right_knee"/>
<motor gear="20" joint="2a_right_ankle_x"/>
<motor gear="20" joint="2a_right_ankle_y"/>
<motor gear="40" joint="2a_left_hip_x"/>
<motor gear="40" joint="2a_left_hip_z"/>
<motor gear="120" joint="2a_left_hip_y"/>
<motor gear="80" joint="2a_left_knee"/>
<motor gear="20" joint="2a_left_ankle_x"/>
<motor gear="20" joint="2a_left_ankle_y"/>
<motor gear="20" joint="2a_right_shoulder1"/>
<motor gear="20" joint="2a_right_shoulder2"/>
<motor gear="40" joint="2a_right_elbow"/>
<motor gear="20" joint="2a_left_shoulder1"/>
<motor gear="20" joint="2a_left_shoulder2"/>
<motor gear="40" joint="2a_left_elbow"/>
</actuator>
<actuator>
<motor gear="40" joint="3a_abdomen_y"/>
<motor gear="40" joint="3a_abdomen_z"/>
<motor gear="40" joint="3a_abdomen_x"/>
<motor gear="40" joint="3a_right_hip_x"/>
<motor gear="40" joint="3a_right_hip_z"/>
<motor gear="120" joint="3a_right_hip_y"/>
<motor gear="80" joint="3a_right_knee"/>
<motor gear="20" joint="3a_right_ankle_x"/>
<motor gear="20" joint="3a_right_ankle_y"/>
<motor gear="40" joint="3a_left_hip_x"/>
<motor gear="40" joint="3a_left_hip_z"/>
<motor gear="120" joint="3a_left_hip_y"/>
<motor gear="80" joint="3a_left_knee"/>
<motor gear="20" joint="3a_left_ankle_x"/>
<motor gear="20" joint="3a_left_ankle_y"/>
<motor gear="20" joint="3a_right_shoulder1"/>
<motor gear="20" joint="3a_right_shoulder2"/>
<motor gear="40" joint="3a_right_elbow"/>
<motor gear="20" joint="3a_left_shoulder1"/>
<motor gear="20" joint="3a_left_shoulder2"/>
<motor gear="40" joint="3a_left_elbow"/>
</actuator>
<actuator>
<motor gear="40" joint="4a_abdomen_y"/>
<motor gear="40" joint="4a_abdomen_z"/>
<motor gear="40" joint="4a_abdomen_x"/>
<motor gear="40" joint="4a_right_hip_x"/>
<motor gear="40" joint="4a_right_hip_z"/>
<motor gear="120" joint="4a_right_hip_y"/>
<motor gear="80" joint="4a_right_knee"/>
<motor gear="20" joint="4a_right_ankle_x"/>
<motor gear="20" joint="4a_right_ankle_y"/>
<motor gear="40" joint="4a_left_hip_x"/>
<motor gear="40" joint="4a_left_hip_z"/>
<motor gear="120" joint="4a_left_hip_y"/>
<motor gear="80" joint="4a_left_knee"/>
<motor gear="20" joint="4a_left_ankle_x"/>
<motor gear="20" joint="4a_left_ankle_y"/>
<motor gear="20" joint="4a_right_shoulder1"/>
<motor gear="20" joint="4a_right_shoulder2"/>
<motor gear="40" joint="4a_right_elbow"/>
<motor gear="20" joint="4a_left_shoulder1"/>
<motor gear="20" joint="4a_left_shoulder2"/>
<motor gear="40" joint="4a_left_elbow"/>
</actuator>
<actuator>
<motor gear="40" joint="5a_abdomen_y"/>
<motor gear="40" joint="5a_abdomen_z"/>
<motor gear="40" joint="5a_abdomen_x"/>
<motor gear="40" joint="5a_right_hip_x"/>
<motor gear="40" joint="5a_right_hip_z"/>
<motor gear="120" joint="5a_right_hip_y"/>
<motor gear="80" joint="5a_right_knee"/>
<motor gear="20" joint="5a_right_ankle_x"/>
<motor gear="20" joint="5a_right_ankle_y"/>
<motor gear="40" joint="5a_left_hip_x"/>
<motor gear="40" joint="5a_left_hip_z"/>
<motor gear="120" joint="5a_left_hip_y"/>
<motor gear="80" joint="5a_left_knee"/>
<motor gear="20" joint="5a_left_ankle_x"/>
<motor gear="20" joint="5a_left_ankle_y"/>
<motor gear="20" joint="5a_right_shoulder1"/>
<motor gear="20" joint="5a_right_shoulder2"/>
<motor gear="40" joint="5a_right_elbow"/>
<motor gear="20" joint="5a_left_shoulder1"/>
<motor gear="20" joint="5a_left_shoulder2"/>
<motor gear="40" joint="5a_left_elbow"/>
</actuator>
<actuator>
<motor gear="40" joint="6a_abdomen_y"/>
<motor gear="40" joint="6a_abdomen_z"/>
<motor gear="40" joint="6a_abdomen_x"/>
<motor gear="40" joint="6a_right_hip_x"/>
<motor gear="40" joint="6a_right_hip_z"/>
<motor gear="120" joint="6a_right_hip_y"/>
<motor gear="80" joint="6a_right_knee"/>
<motor gear="20" joint="6a_right_ankle_x"/>
<motor gear="20" joint="6a_right_ankle_y"/>
<motor gear="40" joint="6a_left_hip_x"/>
<motor gear="40" joint="6a_left_hip_z"/>
<motor gear="120" joint="6a_left_hip_y"/>
<motor gear="80" joint="6a_left_knee"/>
<motor gear="20" joint="6a_left_ankle_x"/>
<motor gear="20" joint="6a_left_ankle_y"/>
<motor gear="20" joint="6a_right_shoulder1"/>
<motor gear="20" joint="6a_right_shoulder2"/>
<motor gear="40" joint="6a_right_elbow"/>
<motor gear="20" joint="6a_left_shoulder1"/>
<motor gear="20" joint="6a_left_shoulder2"/>
<motor gear="40" joint="6a_left_elbow"/>
</actuator>
<actuator>
<motor gear="40" joint="7a_abdomen_y"/>
<motor gear="40" joint="7a_abdomen_z"/>
<motor gear="40" joint="7a_abdomen_x"/>
<motor gear="40" joint="7a_right_hip_x"/>
<motor gear="40" joint="7a_right_hip_z"/>
<motor gear="120" joint="7a_right_hip_y"/>
<motor gear="80" joint="7a_right_knee"/>
<motor gear="20" joint="7a_right_ankle_x"/>
<motor gear="20" joint="7a_right_ankle_y"/>
<motor gear="40" joint="7a_left_hip_x"/>
<motor gear="40" joint="7a_left_hip_z"/>
<motor gear="120" joint="7a_left_hip_y"/>
<motor gear="80" joint="7a_left_knee"/>
<motor gear="20" joint="7a_left_ankle_x"/>
<motor gear="20" joint="7a_left_ankle_y"/>
<motor gear="20" joint="7a_right_shoulder1"/>
<motor gear="20" joint="7a_right_shoulder2"/>
<motor gear="40" joint="7a_right_elbow"/>
<motor gear="20" joint="7a_left_shoulder1"/>
<motor gear="20" joint="7a_left_shoulder2"/>
<motor gear="40" joint="7a_left_elbow"/>
</actuator>
<actuator>
<motor gear="40" joint="8a_abdomen_y"/>
<motor gear="40" joint="8a_abdomen_z"/>
<motor gear="40" joint="8a_abdomen_x"/>
<motor gear="40" joint="8a_right_hip_x"/>
<motor gear="40" joint="8a_right_hip_z"/>
<motor gear="120" joint="8a_right_hip_y"/>
<motor gear="80" joint="8a_right_knee"/>
<motor gear="20" joint="8a_right_ankle_x"/>
<motor gear="20" joint="8a_right_ankle_y"/>
<motor gear="40" joint="8a_left_hip_x"/>
<motor gear="40" joint="8a_left_hip_z"/>
<motor gear="120" joint="8a_left_hip_y"/>
<motor gear="80" joint="8a_left_knee"/>
<motor gear="20" joint="8a_left_ankle_x"/>
<motor gear="20" joint="8a_left_ankle_y"/>
<motor gear="20" joint="8a_right_shoulder1"/>
<motor gear="20" joint="8a_right_shoulder2"/>
<motor gear="40" joint="8a_right_elbow"/>
<motor gear="20" joint="8a_left_shoulder1"/>
<motor gear="20" joint="8a_left_shoulder2"/>
<motor gear="40" joint="8a_left_elbow"/>
</actuator>
</mujoco>
mjx/mujoco/mjx/test_data/humanoid/09_humanoids.xml
+983
View File
@@ -0,0 +1,983 @@
<mujoco model="09 Humanoids">
<option timestep="0.005" solver="Newton" iterations="1" ls_iterations="4">
<flag eulerdamp="disable"/>
</option>
<custom>
<numeric data="36" name="max_contact_points"/>
<numeric data="36" name="max_geom_pairs"/>
</custom>
<size memory="100M"/>
<asset>
<texture type="skybox" builtin="gradient" rgb1=".3 .5 .7" rgb2="0 0 0" width="512" height="512"/>
<texture name="body" type="cube" builtin="flat" mark="cross" width="128" height="128"
rgb1="0.8 0.6 0.4" rgb2="0.8 0.6 0.4" markrgb="1 1 1" random="0.01"/>
<material name="body" texture="body" texuniform="true" rgba="0.8 0.6 .4 1"/>
<texture name="grid" type="2d" builtin="checker" width="512" height="512" rgb1=".1 .2 .3" rgb2=".2 .3 .4"/>
<material name="grid" texture="grid" texrepeat="1 1" texuniform="true" reflectance=".2"/>
</asset>
<default>
<motor ctrlrange="-1 1" ctrllimited="true"/>
<joint type="hinge" damping=".2" stiffness="1" armature=".01" limited="true" solimplimit="0 .99 .01"/>
<default class="big_joint">
<joint damping="5" stiffness="10"/>
<default class="big_stiff_joint">
<joint stiffness="20"/>
</default>
</default>
<default class="body1">
<geom type="capsule" condim="3" friction=".7" solimp=".9 .99 .003" solref=".015 1" material="body" contype="2" conaffinity="1021"/>
</default>
<default class="body2">
<geom type="capsule" condim="3" friction=".7" solimp=".9 .99 .003" solref=".015 1" material="body" contype="4" conaffinity="1019"/>
</default>
<default class="body3">
<geom type="capsule" condim="3" friction=".7" solimp=".9 .99 .003" solref=".015 1" material="body" contype="8" conaffinity="1015"/>
</default>
<default class="body4">
<geom type="capsule" condim="3" friction=".7" solimp=".9 .99 .003" solref=".015 1" material="body" contype="16" conaffinity="1007"/>
</default>
<default class="body5">
<geom type="capsule" condim="3" friction=".7" solimp=".9 .99 .003" solref=".015 1" material="body" contype="32" conaffinity="991"/>
</default>
<default class="body6">
<geom type="capsule" condim="3" friction=".7" solimp=".9 .99 .003" solref=".015 1" material="body" contype="64" conaffinity="959"/>
</default>
<default class="body7">
<geom type="capsule" condim="3" friction=".7" solimp=".9 .99 .003" solref=".015 1" material="body" contype="128" conaffinity="895"/>
</default>
<default class="body8">
<geom type="capsule" condim="3" friction=".7" solimp=".9 .99 .003" solref=".015 1" material="body" contype="256" conaffinity="767"/>
</default>
<default class="body9">
<geom type="capsule" condim="3" friction=".7" solimp=".9 .99 .003" solref=".015 1" material="body" contype="512" conaffinity="511"/>
</default>
</default>
<visual>
<map force="0.1" zfar="30"/>
<rgba haze="0.15 0.25 0.35 1"/>
<quality shadowsize="4096"/>
<global offwidth="800" offheight="800"/>
</visual>
<worldbody>
<geom size="10 10 .05" type="plane" material="grid" condim="3"/>
<light dir=".2 1 -.4" diffuse=".8 .8 .8" specular="0.3 0.3 0.3" pos="-2 -10 4" cutoff="35"/>
<light dir="-.2 1 -.4" diffuse=".8 .8 .8" specular="0.3 0.3 0.3" pos="2 -10 4" cutoff="35"/>
<body name="1a_torso" pos="-1 0 1.5" childclass="body1">
<camera name="1a_back" pos="-3 0 1" xyaxes="0 -1 0 1 0 2" mode="trackcom"/>
<camera name="1a_side" pos="0 -3 1" xyaxes="1 0 0 0 1 2" mode="trackcom"/>
<freejoint name="1a_root"/>
<geom name="1a_torso" fromto="0 -.07 0 0 .07 0" size=".07"/>
<geom name="1a_upper_waist" fromto="-.01 -.06 -.12 -.01 .06 -.12" size=".06"/>
<body name="1a_head" pos="0 0 .19">
<geom name="1a_head" type="sphere" size=".09"/>
<camera name="1a_egocentric" pos=".09 0 0" xyaxes="0 -1 0 .1 0 1" fovy="80"/>
</body>
<body name="1a_lower_waist" pos="-.01 0 -.26">
<geom name="1a_lower_waist" fromto="0 -.06 0 0 .06 0" size=".06"/>
<joint name="1a_abdomen_z" pos="0 0 .065" axis="0 0 1" range="-45 45" class="big_stiff_joint"/>
<joint name="1a_abdomen_y" pos="0 0 .065" axis="0 1 0" range="-75 30" class="big_joint"/>
<body name="1a_pelvis" pos="0 0 -.165">
<joint name="1a_abdomen_x" pos="0 0 .1" axis="1 0 0" range="-35 35" class="big_joint"/>
<geom name="1a_butt" fromto="-.02 -.07 0 -.02 .07 0" size=".09"/>
<body name="1a_right_thigh" pos="0 -.1 -.04">
<joint name="1a_right_hip_x" axis="1 0 0" range="-25 5" class="big_joint"/>
<joint name="1a_right_hip_z" axis="0 0 1" range="-60 35" class="big_joint"/>
<joint name="1a_right_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
<geom name="1a_right_thigh" fromto="0 0 0 0 .01 -.34" size=".06"/>
<body name="1a_right_shin" pos="0 .01 -.403">
<joint name="1a_right_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
<geom name="1a_right_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
<body name="1a_right_foot" pos="0 0 -.39">
<joint name="1a_right_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
<joint name="1a_right_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
<geom name="1a_right_right_foot" fromto="-.07 -.02 0 .14 -.04 0" size=".027"/>
<geom name="1a_left_right_foot" fromto="-.07 0 0 .14 .02 0" size=".027"/>
</body>
</body>
</body>
<body name="1a_left_thigh" pos="0 .1 -.04">
<joint name="1a_left_hip_x" axis="-1 0 0" range="-25 5" class="big_joint"/>
<joint name="1a_left_hip_z" axis="0 0 -1" range="-60 35" class="big_joint"/>
<joint name="1a_left_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
<geom name="1a_left_thigh" fromto="0 0 0 0 -.01 -.34" size=".06"/>
<body name="1a_left_shin" pos="0 -.01 -.403">
<joint name="1a_left_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
<geom name="1a_left_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
<body name="1a_left_foot" pos="0 0 -.39">
<joint name="1a_left_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
<joint name="1a_left_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
<geom name="1a_left_left_foot" fromto="-.07 .02 0 .14 .04 0" size=".027"/>
<geom name="1a_right_left_foot" fromto="-.07 0 0 .14 -.02 0" size=".027"/>
</body>
</body>
</body>
</body>
</body>
<body name="1a_right_upper_arm" pos="0 -.17 .06">
<joint name="1a_right_shoulder1" axis="2 1 1" range="-85 60"/>
<joint name="1a_right_shoulder2" axis="0 -1 1" range="-85 60"/>
<geom name="1a_right_upper_arm" fromto="0 0 0 .16 -.16 -.16" size=".04 .16"/>
<body name="1a_right_lower_arm" pos=".18 -.18 -.18">
<joint name="1a_right_elbow" axis="0 -1 1" range="-90 50" stiffness="0"/>
<geom name="1a_right_lower_arm" fromto=".01 .01 .01 .17 .17 .17" size=".031"/>
<body name="1a_right_hand" pos=".18 .18 .18">
<geom name="1a_right_hand" type="sphere" size=".04" zaxis="1 1 1"/>
</body>
</body>
</body>
<body name="1a_left_upper_arm" pos="0 .17 .06">
<joint name="1a_left_shoulder1" axis="2 -1 1" range="-60 85"/>
<joint name="1a_left_shoulder2" axis="0 1 1" range="-60 85"/>
<geom name="1a_left_upper_arm" fromto="0 0 0 .16 .16 -.16" size=".04 .16"/>
<body name="1a_left_lower_arm" pos=".18 .18 -.18">
<joint name="1a_left_elbow" axis="0 -1 -1" range="-90 50" stiffness="0"/>
<geom name="1a_left_lower_arm" fromto=".01 -.01 .01 .17 -.17 .17" size=".031"/>
<body name="1a_left_hand" pos=".18 -.18 .18">
<geom name="1a_left_hand" type="sphere" size=".04" zaxis="1 -1 1"/>
</body>
</body>
</body>
</body>
<body name="2a_torso" pos="-1 1 1.5" childclass="body2">
<camera name="2a_back" pos="-3 0 1" xyaxes="0 -1 0 1 0 2" mode="trackcom"/>
<camera name="2a_side" pos="0 -3 1" xyaxes="1 0 0 0 1 2" mode="trackcom"/>
<freejoint name="2a_root"/>
<geom name="2a_torso" fromto="0 -.07 0 0 .07 0" size=".07"/>
<geom name="2a_upper_waist" fromto="-.01 -.06 -.12 -.01 .06 -.12" size=".06"/>
<body name="2a_head" pos="0 0 .19">
<geom name="2a_head" type="sphere" size=".09"/>
<camera name="2a_egocentric" pos=".09 0 0" xyaxes="0 -1 0 .1 0 1" fovy="80"/>
</body>
<body name="2a_lower_waist" pos="-.01 0 -.26">
<geom name="2a_lower_waist" fromto="0 -.06 0 0 .06 0" size=".06"/>
<joint name="2a_abdomen_z" pos="0 0 .065" axis="0 0 1" range="-45 45" class="big_stiff_joint"/>
<joint name="2a_abdomen_y" pos="0 0 .065" axis="0 1 0" range="-75 30" class="big_joint"/>
<body name="2a_pelvis" pos="0 0 -.165">
<joint name="2a_abdomen_x" pos="0 0 .1" axis="1 0 0" range="-35 35" class="big_joint"/>
<geom name="2a_butt" fromto="-.02 -.07 0 -.02 .07 0" size=".09"/>
<body name="2a_right_thigh" pos="0 -.1 -.04">
<joint name="2a_right_hip_x" axis="1 0 0" range="-25 5" class="big_joint"/>
<joint name="2a_right_hip_z" axis="0 0 1" range="-60 35" class="big_joint"/>
<joint name="2a_right_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
<geom name="2a_right_thigh" fromto="0 0 0 0 .01 -.34" size=".06"/>
<body name="2a_right_shin" pos="0 .01 -.403">
<joint name="2a_right_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
<geom name="2a_right_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
<body name="2a_right_foot" pos="0 0 -.39">
<joint name="2a_right_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
<joint name="2a_right_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
<geom name="2a_right_right_foot" fromto="-.07 -.02 0 .14 -.04 0" size=".027"/>
<geom name="2a_left_right_foot" fromto="-.07 0 0 .14 .02 0" size=".027"/>
</body>
</body>
</body>
<body name="2a_left_thigh" pos="0 .1 -.04">
<joint name="2a_left_hip_x" axis="-1 0 0" range="-25 5" class="big_joint"/>
<joint name="2a_left_hip_z" axis="0 0 -1" range="-60 35" class="big_joint"/>
<joint name="2a_left_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
<geom name="2a_left_thigh" fromto="0 0 0 0 -.01 -.34" size=".06"/>
<body name="2a_left_shin" pos="0 -.01 -.403">
<joint name="2a_left_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
<geom name="2a_left_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
<body name="2a_left_foot" pos="0 0 -.39">
<joint name="2a_left_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
<joint name="2a_left_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
<geom name="2a_left_left_foot" fromto="-.07 .02 0 .14 .04 0" size=".027"/>
<geom name="2a_right_left_foot" fromto="-.07 0 0 .14 -.02 0" size=".027"/>
</body>
</body>
</body>
</body>
</body>
<body name="2a_right_upper_arm" pos="0 -.17 .06">
<joint name="2a_right_shoulder1" axis="2 1 1" range="-85 60"/>
<joint name="2a_right_shoulder2" axis="0 -1 1" range="-85 60"/>
<geom name="2a_right_upper_arm" fromto="0 0 0 .16 -.16 -.16" size=".04 .16"/>
<body name="2a_right_lower_arm" pos=".18 -.18 -.18">
<joint name="2a_right_elbow" axis="0 -1 1" range="-90 50" stiffness="0"/>
<geom name="2a_right_lower_arm" fromto=".01 .01 .01 .17 .17 .17" size=".031"/>
<body name="2a_right_hand" pos=".18 .18 .18">
<geom name="2a_right_hand" type="sphere" size=".04" zaxis="1 1 1"/>
</body>
</body>
</body>
<body name="2a_left_upper_arm" pos="0 .17 .06">
<joint name="2a_left_shoulder1" axis="2 -1 1" range="-60 85"/>
<joint name="2a_left_shoulder2" axis="0 1 1" range="-60 85"/>
<geom name="2a_left_upper_arm" fromto="0 0 0 .16 .16 -.16" size=".04 .16"/>
<body name="2a_left_lower_arm" pos=".18 .18 -.18">
<joint name="2a_left_elbow" axis="0 -1 -1" range="-90 50" stiffness="0"/>
<geom name="2a_left_lower_arm" fromto=".01 -.01 .01 .17 -.17 .17" size=".031"/>
<body name="2a_left_hand" pos=".18 -.18 .18">
<geom name="2a_left_hand" type="sphere" size=".04" zaxis="1 -1 1"/>
</body>
</body>
</body>
</body>
<body name="3a_torso" pos="-1 -1 1.5" childclass="body3">
<camera name="3a_back" pos="-3 0 1" xyaxes="0 -1 0 1 0 2" mode="trackcom"/>
<camera name="3a_side" pos="0 -3 1" xyaxes="1 0 0 0 1 2" mode="trackcom"/>
<freejoint name="3a_root"/>
<geom name="3a_torso" fromto="0 -.07 0 0 .07 0" size=".07"/>
<geom name="3a_upper_waist" fromto="-.01 -.06 -.12 -.01 .06 -.12" size=".06"/>
<body name="3a_head" pos="0 0 .19">
<geom name="3a_head" type="sphere" size=".09"/>
<camera name="3a_egocentric" pos=".09 0 0" xyaxes="0 -1 0 .1 0 1" fovy="80"/>
</body>
<body name="3a_lower_waist" pos="-.01 0 -.26">
<geom name="3a_lower_waist" fromto="0 -.06 0 0 .06 0" size=".06"/>
<joint name="3a_abdomen_z" pos="0 0 .065" axis="0 0 1" range="-45 45" class="big_stiff_joint"/>
<joint name="3a_abdomen_y" pos="0 0 .065" axis="0 1 0" range="-75 30" class="big_joint"/>
<body name="3a_pelvis" pos="0 0 -.165">
<joint name="3a_abdomen_x" pos="0 0 .1" axis="1 0 0" range="-35 35" class="big_joint"/>
<geom name="3a_butt" fromto="-.02 -.07 0 -.02 .07 0" size=".09"/>
<body name="3a_right_thigh" pos="0 -.1 -.04">
<joint name="3a_right_hip_x" axis="1 0 0" range="-25 5" class="big_joint"/>
<joint name="3a_right_hip_z" axis="0 0 1" range="-60 35" class="big_joint"/>
<joint name="3a_right_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
<geom name="3a_right_thigh" fromto="0 0 0 0 .01 -.34" size=".06"/>
<body name="3a_right_shin" pos="0 .01 -.403">
<joint name="3a_right_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
<geom name="3a_right_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
<body name="3a_right_foot" pos="0 0 -.39">
<joint name="3a_right_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
<joint name="3a_right_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
<geom name="3a_right_right_foot" fromto="-.07 -.02 0 .14 -.04 0" size=".027"/>
<geom name="3a_left_right_foot" fromto="-.07 0 0 .14 .02 0" size=".027"/>
</body>
</body>
</body>
<body name="3a_left_thigh" pos="0 .1 -.04">
<joint name="3a_left_hip_x" axis="-1 0 0" range="-25 5" class="big_joint"/>
<joint name="3a_left_hip_z" axis="0 0 -1" range="-60 35" class="big_joint"/>
<joint name="3a_left_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
<geom name="3a_left_thigh" fromto="0 0 0 0 -.01 -.34" size=".06"/>
<body name="3a_left_shin" pos="0 -.01 -.403">
<joint name="3a_left_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
<geom name="3a_left_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
<body name="3a_left_foot" pos="0 0 -.39">
<joint name="3a_left_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
<joint name="3a_left_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
<geom name="3a_left_left_foot" fromto="-.07 .02 0 .14 .04 0" size=".027"/>
<geom name="3a_right_left_foot" fromto="-.07 0 0 .14 -.02 0" size=".027"/>
</body>
</body>
</body>
</body>
</body>
<body name="3a_right_upper_arm" pos="0 -.17 .06">
<joint name="3a_right_shoulder1" axis="2 1 1" range="-85 60"/>
<joint name="3a_right_shoulder2" axis="0 -1 1" range="-85 60"/>
<geom name="3a_right_upper_arm" fromto="0 0 0 .16 -.16 -.16" size=".04 .16"/>
<body name="3a_right_lower_arm" pos=".18 -.18 -.18">
<joint name="3a_right_elbow" axis="0 -1 1" range="-90 50" stiffness="0"/>
<geom name="3a_right_lower_arm" fromto=".01 .01 .01 .17 .17 .17" size=".031"/>
<body name="3a_right_hand" pos=".18 .18 .18">
<geom name="3a_right_hand" type="sphere" size=".04" zaxis="1 1 1"/>
</body>
</body>
</body>
<body name="3a_left_upper_arm" pos="0 .17 .06">
<joint name="3a_left_shoulder1" axis="2 -1 1" range="-60 85"/>
<joint name="3a_left_shoulder2" axis="0 1 1" range="-60 85"/>
<geom name="3a_left_upper_arm" fromto="0 0 0 .16 .16 -.16" size=".04 .16"/>
<body name="3a_left_lower_arm" pos=".18 .18 -.18">
<joint name="3a_left_elbow" axis="0 -1 -1" range="-90 50" stiffness="0"/>
<geom name="3a_left_lower_arm" fromto=".01 -.01 .01 .17 -.17 .17" size=".031"/>
<body name="3a_left_hand" pos=".18 -.18 .18">
<geom name="3a_left_hand" type="sphere" size=".04" zaxis="1 -1 1"/>
</body>
</body>
</body>
</body>
<body name="4a_torso" pos="-1 2 1.5" childclass="body4">
<camera name="4a_back" pos="-3 0 1" xyaxes="0 -1 0 1 0 2" mode="trackcom"/>
<camera name="4a_side" pos="0 -3 1" xyaxes="1 0 0 0 1 2" mode="trackcom"/>
<freejoint name="4a_root"/>
<geom name="4a_torso" fromto="0 -.07 0 0 .07 0" size=".07"/>
<geom name="4a_upper_waist" fromto="-.01 -.06 -.12 -.01 .06 -.12" size=".06"/>
<body name="4a_head" pos="0 0 .19">
<geom name="4a_head" type="sphere" size=".09"/>
<camera name="4a_egocentric" pos=".09 0 0" xyaxes="0 -1 0 .1 0 1" fovy="80"/>
</body>
<body name="4a_lower_waist" pos="-.01 0 -.26">
<geom name="4a_lower_waist" fromto="0 -.06 0 0 .06 0" size=".06"/>
<joint name="4a_abdomen_z" pos="0 0 .065" axis="0 0 1" range="-45 45" class="big_stiff_joint"/>
<joint name="4a_abdomen_y" pos="0 0 .065" axis="0 1 0" range="-75 30" class="big_joint"/>
<body name="4a_pelvis" pos="0 0 -.165">
<joint name="4a_abdomen_x" pos="0 0 .1" axis="1 0 0" range="-35 35" class="big_joint"/>
<geom name="4a_butt" fromto="-.02 -.07 0 -.02 .07 0" size=".09"/>
<body name="4a_right_thigh" pos="0 -.1 -.04">
<joint name="4a_right_hip_x" axis="1 0 0" range="-25 5" class="big_joint"/>
<joint name="4a_right_hip_z" axis="0 0 1" range="-60 35" class="big_joint"/>
<joint name="4a_right_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
<geom name="4a_right_thigh" fromto="0 0 0 0 .01 -.34" size=".06"/>
<body name="4a_right_shin" pos="0 .01 -.403">
<joint name="4a_right_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
<geom name="4a_right_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
<body name="4a_right_foot" pos="0 0 -.39">
<joint name="4a_right_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
<joint name="4a_right_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
<geom name="4a_right_right_foot" fromto="-.07 -.02 0 .14 -.04 0" size=".027"/>
<geom name="4a_left_right_foot" fromto="-.07 0 0 .14 .02 0" size=".027"/>
</body>
</body>
</body>
<body name="4a_left_thigh" pos="0 .1 -.04">
<joint name="4a_left_hip_x" axis="-1 0 0" range="-25 5" class="big_joint"/>
<joint name="4a_left_hip_z" axis="0 0 -1" range="-60 35" class="big_joint"/>
<joint name="4a_left_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
<geom name="4a_left_thigh" fromto="0 0 0 0 -.01 -.34" size=".06"/>
<body name="4a_left_shin" pos="0 -.01 -.403">
<joint name="4a_left_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
<geom name="4a_left_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
<body name="4a_left_foot" pos="0 0 -.39">
<joint name="4a_left_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
<joint name="4a_left_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
<geom name="4a_left_left_foot" fromto="-.07 .02 0 .14 .04 0" size=".027"/>
<geom name="4a_right_left_foot" fromto="-.07 0 0 .14 -.02 0" size=".027"/>
</body>
</body>
</body>
</body>
</body>
<body name="4a_right_upper_arm" pos="0 -.17 .06">
<joint name="4a_right_shoulder1" axis="2 1 1" range="-85 60"/>
<joint name="4a_right_shoulder2" axis="0 -1 1" range="-85 60"/>
<geom name="4a_right_upper_arm" fromto="0 0 0 .16 -.16 -.16" size=".04 .16"/>
<body name="4a_right_lower_arm" pos=".18 -.18 -.18">
<joint name="4a_right_elbow" axis="0 -1 1" range="-90 50" stiffness="0"/>
<geom name="4a_right_lower_arm" fromto=".01 .01 .01 .17 .17 .17" size=".031"/>
<body name="4a_right_hand" pos=".18 .18 .18">
<geom name="4a_right_hand" type="sphere" size=".04" zaxis="1 1 1"/>
</body>
</body>
</body>
<body name="4a_left_upper_arm" pos="0 .17 .06">
<joint name="4a_left_shoulder1" axis="2 -1 1" range="-60 85"/>
<joint name="4a_left_shoulder2" axis="0 1 1" range="-60 85"/>
<geom name="4a_left_upper_arm" fromto="0 0 0 .16 .16 -.16" size=".04 .16"/>
<body name="4a_left_lower_arm" pos=".18 .18 -.18">
<joint name="4a_left_elbow" axis="0 -1 -1" range="-90 50" stiffness="0"/>
<geom name="4a_left_lower_arm" fromto=".01 -.01 .01 .17 -.17 .17" size=".031"/>
<body name="4a_left_hand" pos=".18 -.18 .18">
<geom name="4a_left_hand" type="sphere" size=".04" zaxis="1 -1 1"/>
</body>
</body>
</body>
</body>
<body name="5a_torso" pos="-1 -2 1.5" childclass="body5">
<camera name="5a_back" pos="-3 0 1" xyaxes="0 -1 0 1 0 2" mode="trackcom"/>
<camera name="5a_side" pos="0 -3 1" xyaxes="1 0 0 0 1 2" mode="trackcom"/>
<freejoint name="5a_root"/>
<geom name="5a_torso" fromto="0 -.07 0 0 .07 0" size=".07"/>
<geom name="5a_upper_waist" fromto="-.01 -.06 -.12 -.01 .06 -.12" size=".06"/>
<body name="5a_head" pos="0 0 .19">
<geom name="5a_head" type="sphere" size=".09"/>
<camera name="5a_egocentric" pos=".09 0 0" xyaxes="0 -1 0 .1 0 1" fovy="80"/>
</body>
<body name="5a_lower_waist" pos="-.01 0 -.26">
<geom name="5a_lower_waist" fromto="0 -.06 0 0 .06 0" size=".06"/>
<joint name="5a_abdomen_z" pos="0 0 .065" axis="0 0 1" range="-45 45" class="big_stiff_joint"/>
<joint name="5a_abdomen_y" pos="0 0 .065" axis="0 1 0" range="-75 30" class="big_joint"/>
<body name="5a_pelvis" pos="0 0 -.165">
<joint name="5a_abdomen_x" pos="0 0 .1" axis="1 0 0" range="-35 35" class="big_joint"/>
<geom name="5a_butt" fromto="-.02 -.07 0 -.02 .07 0" size=".09"/>
<body name="5a_right_thigh" pos="0 -.1 -.04">
<joint name="5a_right_hip_x" axis="1 0 0" range="-25 5" class="big_joint"/>
<joint name="5a_right_hip_z" axis="0 0 1" range="-60 35" class="big_joint"/>
<joint name="5a_right_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
<geom name="5a_right_thigh" fromto="0 0 0 0 .01 -.34" size=".06"/>
<body name="5a_right_shin" pos="0 .01 -.403">
<joint name="5a_right_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
<geom name="5a_right_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
<body name="5a_right_foot" pos="0 0 -.39">
<joint name="5a_right_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
<joint name="5a_right_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
<geom name="5a_right_right_foot" fromto="-.07 -.02 0 .14 -.04 0" size=".027"/>
<geom name="5a_left_right_foot" fromto="-.07 0 0 .14 .02 0" size=".027"/>
</body>
</body>
</body>
<body name="5a_left_thigh" pos="0 .1 -.04">
<joint name="5a_left_hip_x" axis="-1 0 0" range="-25 5" class="big_joint"/>
<joint name="5a_left_hip_z" axis="0 0 -1" range="-60 35" class="big_joint"/>
<joint name="5a_left_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
<geom name="5a_left_thigh" fromto="0 0 0 0 -.01 -.34" size=".06"/>
<body name="5a_left_shin" pos="0 -.01 -.403">
<joint name="5a_left_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
<geom name="5a_left_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
<body name="5a_left_foot" pos="0 0 -.39">
<joint name="5a_left_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
<joint name="5a_left_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
<geom name="5a_left_left_foot" fromto="-.07 .02 0 .14 .04 0" size=".027"/>
<geom name="5a_right_left_foot" fromto="-.07 0 0 .14 -.02 0" size=".027"/>
</body>
</body>
</body>
</body>
</body>
<body name="5a_right_upper_arm" pos="0 -.17 .06">
<joint name="5a_right_shoulder1" axis="2 1 1" range="-85 60"/>
<joint name="5a_right_shoulder2" axis="0 -1 1" range="-85 60"/>
<geom name="5a_right_upper_arm" fromto="0 0 0 .16 -.16 -.16" size=".04 .16"/>
<body name="5a_right_lower_arm" pos=".18 -.18 -.18">
<joint name="5a_right_elbow" axis="0 -1 1" range="-90 50" stiffness="0"/>
<geom name="5a_right_lower_arm" fromto=".01 .01 .01 .17 .17 .17" size=".031"/>
<body name="5a_right_hand" pos=".18 .18 .18">
<geom name="5a_right_hand" type="sphere" size=".04" zaxis="1 1 1"/>
</body>
</body>
</body>
<body name="5a_left_upper_arm" pos="0 .17 .06">
<joint name="5a_left_shoulder1" axis="2 -1 1" range="-60 85"/>
<joint name="5a_left_shoulder2" axis="0 1 1" range="-60 85"/>
<geom name="5a_left_upper_arm" fromto="0 0 0 .16 .16 -.16" size=".04 .16"/>
<body name="5a_left_lower_arm" pos=".18 .18 -.18">
<joint name="5a_left_elbow" axis="0 -1 -1" range="-90 50" stiffness="0"/>
<geom name="5a_left_lower_arm" fromto=".01 -.01 .01 .17 -.17 .17" size=".031"/>
<body name="5a_left_hand" pos=".18 -.18 .18">
<geom name="5a_left_hand" type="sphere" size=".04" zaxis="1 -1 1"/>
</body>
</body>
</body>
</body>
<body name="6a_torso" pos="-1 3 1.5" childclass="body6">
<camera name="6a_back" pos="-3 0 1" xyaxes="0 -1 0 1 0 2" mode="trackcom"/>
<camera name="6a_side" pos="0 -3 1" xyaxes="1 0 0 0 1 2" mode="trackcom"/>
<freejoint name="6a_root"/>
<geom name="6a_torso" fromto="0 -.07 0 0 .07 0" size=".07"/>
<geom name="6a_upper_waist" fromto="-.01 -.06 -.12 -.01 .06 -.12" size=".06"/>
<body name="6a_head" pos="0 0 .19">
<geom name="6a_head" type="sphere" size=".09"/>
<camera name="6a_egocentric" pos=".09 0 0" xyaxes="0 -1 0 .1 0 1" fovy="80"/>
</body>
<body name="6a_lower_waist" pos="-.01 0 -.26">
<geom name="6a_lower_waist" fromto="0 -.06 0 0 .06 0" size=".06"/>
<joint name="6a_abdomen_z" pos="0 0 .065" axis="0 0 1" range="-45 45" class="big_stiff_joint"/>
<joint name="6a_abdomen_y" pos="0 0 .065" axis="0 1 0" range="-75 30" class="big_joint"/>
<body name="6a_pelvis" pos="0 0 -.165">
<joint name="6a_abdomen_x" pos="0 0 .1" axis="1 0 0" range="-35 35" class="big_joint"/>
<geom name="6a_butt" fromto="-.02 -.07 0 -.02 .07 0" size=".09"/>
<body name="6a_right_thigh" pos="0 -.1 -.04">
<joint name="6a_right_hip_x" axis="1 0 0" range="-25 5" class="big_joint"/>
<joint name="6a_right_hip_z" axis="0 0 1" range="-60 35" class="big_joint"/>
<joint name="6a_right_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
<geom name="6a_right_thigh" fromto="0 0 0 0 .01 -.34" size=".06"/>
<body name="6a_right_shin" pos="0 .01 -.403">
<joint name="6a_right_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
<geom name="6a_right_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
<body name="6a_right_foot" pos="0 0 -.39">
<joint name="6a_right_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
<joint name="6a_right_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
<geom name="6a_right_right_foot" fromto="-.07 -.02 0 .14 -.04 0" size=".027"/>
<geom name="6a_left_right_foot" fromto="-.07 0 0 .14 .02 0" size=".027"/>
</body>
</body>
</body>
<body name="6a_left_thigh" pos="0 .1 -.04">
<joint name="6a_left_hip_x" axis="-1 0 0" range="-25 5" class="big_joint"/>
<joint name="6a_left_hip_z" axis="0 0 -1" range="-60 35" class="big_joint"/>
<joint name="6a_left_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
<geom name="6a_left_thigh" fromto="0 0 0 0 -.01 -.34" size=".06"/>
<body name="6a_left_shin" pos="0 -.01 -.403">
<joint name="6a_left_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
<geom name="6a_left_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
<body name="6a_left_foot" pos="0 0 -.39">
<joint name="6a_left_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
<joint name="6a_left_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
<geom name="6a_left_left_foot" fromto="-.07 .02 0 .14 .04 0" size=".027"/>
<geom name="6a_right_left_foot" fromto="-.07 0 0 .14 -.02 0" size=".027"/>
</body>
</body>
</body>
</body>
</body>
<body name="6a_right_upper_arm" pos="0 -.17 .06">
<joint name="6a_right_shoulder1" axis="2 1 1" range="-85 60"/>
<joint name="6a_right_shoulder2" axis="0 -1 1" range="-85 60"/>
<geom name="6a_right_upper_arm" fromto="0 0 0 .16 -.16 -.16" size=".04 .16"/>
<body name="6a_right_lower_arm" pos=".18 -.18 -.18">
<joint name="6a_right_elbow" axis="0 -1 1" range="-90 50" stiffness="0"/>
<geom name="6a_right_lower_arm" fromto=".01 .01 .01 .17 .17 .17" size=".031"/>
<body name="6a_right_hand" pos=".18 .18 .18">
<geom name="6a_right_hand" type="sphere" size=".04" zaxis="1 1 1"/>
</body>
</body>
</body>
<body name="6a_left_upper_arm" pos="0 .17 .06">
<joint name="6a_left_shoulder1" axis="2 -1 1" range="-60 85"/>
<joint name="6a_left_shoulder2" axis="0 1 1" range="-60 85"/>
<geom name="6a_left_upper_arm" fromto="0 0 0 .16 .16 -.16" size=".04 .16"/>
<body name="6a_left_lower_arm" pos=".18 .18 -.18">
<joint name="6a_left_elbow" axis="0 -1 -1" range="-90 50" stiffness="0"/>
<geom name="6a_left_lower_arm" fromto=".01 -.01 .01 .17 -.17 .17" size=".031"/>
<body name="6a_left_hand" pos=".18 -.18 .18">
<geom name="6a_left_hand" type="sphere" size=".04" zaxis="1 -1 1"/>
</body>
</body>
</body>
</body>
<body name="7a_torso" pos="-1 -3 1.5" childclass="body7">
<camera name="7a_back" pos="-3 0 1" xyaxes="0 -1 0 1 0 2" mode="trackcom"/>
<camera name="7a_side" pos="0 -3 1" xyaxes="1 0 0 0 1 2" mode="trackcom"/>
<freejoint name="7a_root"/>
<geom name="7a_torso" fromto="0 -.07 0 0 .07 0" size=".07"/>
<geom name="7a_upper_waist" fromto="-.01 -.06 -.12 -.01 .06 -.12" size=".06"/>
<body name="7a_head" pos="0 0 .19">
<geom name="7a_head" type="sphere" size=".09"/>
<camera name="7a_egocentric" pos=".09 0 0" xyaxes="0 -1 0 .1 0 1" fovy="80"/>
</body>
<body name="7a_lower_waist" pos="-.01 0 -.26">
<geom name="7a_lower_waist" fromto="0 -.06 0 0 .06 0" size=".06"/>
<joint name="7a_abdomen_z" pos="0 0 .065" axis="0 0 1" range="-45 45" class="big_stiff_joint"/>
<joint name="7a_abdomen_y" pos="0 0 .065" axis="0 1 0" range="-75 30" class="big_joint"/>
<body name="7a_pelvis" pos="0 0 -.165">
<joint name="7a_abdomen_x" pos="0 0 .1" axis="1 0 0" range="-35 35" class="big_joint"/>
<geom name="7a_butt" fromto="-.02 -.07 0 -.02 .07 0" size=".09"/>
<body name="7a_right_thigh" pos="0 -.1 -.04">
<joint name="7a_right_hip_x" axis="1 0 0" range="-25 5" class="big_joint"/>
<joint name="7a_right_hip_z" axis="0 0 1" range="-60 35" class="big_joint"/>
<joint name="7a_right_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
<geom name="7a_right_thigh" fromto="0 0 0 0 .01 -.34" size=".06"/>
<body name="7a_right_shin" pos="0 .01 -.403">
<joint name="7a_right_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
<geom name="7a_right_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
<body name="7a_right_foot" pos="0 0 -.39">
<joint name="7a_right_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
<joint name="7a_right_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
<geom name="7a_right_right_foot" fromto="-.07 -.02 0 .14 -.04 0" size=".027"/>
<geom name="7a_left_right_foot" fromto="-.07 0 0 .14 .02 0" size=".027"/>
</body>
</body>
</body>
<body name="7a_left_thigh" pos="0 .1 -.04">
<joint name="7a_left_hip_x" axis="-1 0 0" range="-25 5" class="big_joint"/>
<joint name="7a_left_hip_z" axis="0 0 -1" range="-60 35" class="big_joint"/>
<joint name="7a_left_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
<geom name="7a_left_thigh" fromto="0 0 0 0 -.01 -.34" size=".06"/>
<body name="7a_left_shin" pos="0 -.01 -.403">
<joint name="7a_left_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
<geom name="7a_left_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
<body name="7a_left_foot" pos="0 0 -.39">
<joint name="7a_left_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
<joint name="7a_left_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
<geom name="7a_left_left_foot" fromto="-.07 .02 0 .14 .04 0" size=".027"/>
<geom name="7a_right_left_foot" fromto="-.07 0 0 .14 -.02 0" size=".027"/>
</body>
</body>
</body>
</body>
</body>
<body name="7a_right_upper_arm" pos="0 -.17 .06">
<joint name="7a_right_shoulder1" axis="2 1 1" range="-85 60"/>
<joint name="7a_right_shoulder2" axis="0 -1 1" range="-85 60"/>
<geom name="7a_right_upper_arm" fromto="0 0 0 .16 -.16 -.16" size=".04 .16"/>
<body name="7a_right_lower_arm" pos=".18 -.18 -.18">
<joint name="7a_right_elbow" axis="0 -1 1" range="-90 50" stiffness="0"/>
<geom name="7a_right_lower_arm" fromto=".01 .01 .01 .17 .17 .17" size=".031"/>
<body name="7a_right_hand" pos=".18 .18 .18">
<geom name="7a_right_hand" type="sphere" size=".04" zaxis="1 1 1"/>
</body>
</body>
</body>
<body name="7a_left_upper_arm" pos="0 .17 .06">
<joint name="7a_left_shoulder1" axis="2 -1 1" range="-60 85"/>
<joint name="7a_left_shoulder2" axis="0 1 1" range="-60 85"/>
<geom name="7a_left_upper_arm" fromto="0 0 0 .16 .16 -.16" size=".04 .16"/>
<body name="7a_left_lower_arm" pos=".18 .18 -.18">
<joint name="7a_left_elbow" axis="0 -1 -1" range="-90 50" stiffness="0"/>
<geom name="7a_left_lower_arm" fromto=".01 -.01 .01 .17 -.17 .17" size=".031"/>
<body name="7a_left_hand" pos=".18 -.18 .18">
<geom name="7a_left_hand" type="sphere" size=".04" zaxis="1 -1 1"/>
</body>
</body>
</body>
</body>
<body name="8a_torso" pos="-1 4 1.5" childclass="body8">
<camera name="8a_back" pos="-3 0 1" xyaxes="0 -1 0 1 0 2" mode="trackcom"/>
<camera name="8a_side" pos="0 -3 1" xyaxes="1 0 0 0 1 2" mode="trackcom"/>
<freejoint name="8a_root"/>
<geom name="8a_torso" fromto="0 -.07 0 0 .07 0" size=".07"/>
<geom name="8a_upper_waist" fromto="-.01 -.06 -.12 -.01 .06 -.12" size=".06"/>
<body name="8a_head" pos="0 0 .19">
<geom name="8a_head" type="sphere" size=".09"/>
<camera name="8a_egocentric" pos=".09 0 0" xyaxes="0 -1 0 .1 0 1" fovy="80"/>
</body>
<body name="8a_lower_waist" pos="-.01 0 -.26">
<geom name="8a_lower_waist" fromto="0 -.06 0 0 .06 0" size=".06"/>
<joint name="8a_abdomen_z" pos="0 0 .065" axis="0 0 1" range="-45 45" class="big_stiff_joint"/>
<joint name="8a_abdomen_y" pos="0 0 .065" axis="0 1 0" range="-75 30" class="big_joint"/>
<body name="8a_pelvis" pos="0 0 -.165">
<joint name="8a_abdomen_x" pos="0 0 .1" axis="1 0 0" range="-35 35" class="big_joint"/>
<geom name="8a_butt" fromto="-.02 -.07 0 -.02 .07 0" size=".09"/>
<body name="8a_right_thigh" pos="0 -.1 -.04">
<joint name="8a_right_hip_x" axis="1 0 0" range="-25 5" class="big_joint"/>
<joint name="8a_right_hip_z" axis="0 0 1" range="-60 35" class="big_joint"/>
<joint name="8a_right_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
<geom name="8a_right_thigh" fromto="0 0 0 0 .01 -.34" size=".06"/>
<body name="8a_right_shin" pos="0 .01 -.403">
<joint name="8a_right_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
<geom name="8a_right_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
<body name="8a_right_foot" pos="0 0 -.39">
<joint name="8a_right_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
<joint name="8a_right_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
<geom name="8a_right_right_foot" fromto="-.07 -.02 0 .14 -.04 0" size=".027"/>
<geom name="8a_left_right_foot" fromto="-.07 0 0 .14 .02 0" size=".027"/>
</body>
</body>
</body>
<body name="8a_left_thigh" pos="0 .1 -.04">
<joint name="8a_left_hip_x" axis="-1 0 0" range="-25 5" class="big_joint"/>
<joint name="8a_left_hip_z" axis="0 0 -1" range="-60 35" class="big_joint"/>
<joint name="8a_left_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
<geom name="8a_left_thigh" fromto="0 0 0 0 -.01 -.34" size=".06"/>
<body name="8a_left_shin" pos="0 -.01 -.403">
<joint name="8a_left_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
<geom name="8a_left_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
<body name="8a_left_foot" pos="0 0 -.39">
<joint name="8a_left_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
<joint name="8a_left_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
<geom name="8a_left_left_foot" fromto="-.07 .02 0 .14 .04 0" size=".027"/>
<geom name="8a_right_left_foot" fromto="-.07 0 0 .14 -.02 0" size=".027"/>
</body>
</body>
</body>
</body>
</body>
<body name="8a_right_upper_arm" pos="0 -.17 .06">
<joint name="8a_right_shoulder1" axis="2 1 1" range="-85 60"/>
<joint name="8a_right_shoulder2" axis="0 -1 1" range="-85 60"/>
<geom name="8a_right_upper_arm" fromto="0 0 0 .16 -.16 -.16" size=".04 .16"/>
<body name="8a_right_lower_arm" pos=".18 -.18 -.18">
<joint name="8a_right_elbow" axis="0 -1 1" range="-90 50" stiffness="0"/>
<geom name="8a_right_lower_arm" fromto=".01 .01 .01 .17 .17 .17" size=".031"/>
<body name="8a_right_hand" pos=".18 .18 .18">
<geom name="8a_right_hand" type="sphere" size=".04" zaxis="1 1 1"/>
</body>
</body>
</body>
<body name="8a_left_upper_arm" pos="0 .17 .06">
<joint name="8a_left_shoulder1" axis="2 -1 1" range="-60 85"/>
<joint name="8a_left_shoulder2" axis="0 1 1" range="-60 85"/>
<geom name="8a_left_upper_arm" fromto="0 0 0 .16 .16 -.16" size=".04 .16"/>
<body name="8a_left_lower_arm" pos=".18 .18 -.18">
<joint name="8a_left_elbow" axis="0 -1 -1" range="-90 50" stiffness="0"/>
<geom name="8a_left_lower_arm" fromto=".01 -.01 .01 .17 -.17 .17" size=".031"/>
<body name="8a_left_hand" pos=".18 -.18 .18">
<geom name="8a_left_hand" type="sphere" size=".04" zaxis="1 -1 1"/>
</body>
</body>
</body>
</body>
<body name="9a_torso" pos="-1 -4 1.5" childclass="body9">
<camera name="9a_back" pos="-3 0 1" xyaxes="0 -1 0 1 0 2" mode="trackcom"/>
<camera name="9a_side" pos="0 -3 1" xyaxes="1 0 0 0 1 2" mode="trackcom"/>
<freejoint name="9a_root"/>
<geom name="9a_torso" fromto="0 -.07 0 0 .07 0" size=".07"/>
<geom name="9a_upper_waist" fromto="-.01 -.06 -.12 -.01 .06 -.12" size=".06"/>
<body name="9a_head" pos="0 0 .19">
<geom name="9a_head" type="sphere" size=".09"/>
<camera name="9a_egocentric" pos=".09 0 0" xyaxes="0 -1 0 .1 0 1" fovy="80"/>
</body>
<body name="9a_lower_waist" pos="-.01 0 -.26">
<geom name="9a_lower_waist" fromto="0 -.06 0 0 .06 0" size=".06"/>
<joint name="9a_abdomen_z" pos="0 0 .065" axis="0 0 1" range="-45 45" class="big_stiff_joint"/>
<joint name="9a_abdomen_y" pos="0 0 .065" axis="0 1 0" range="-75 30" class="big_joint"/>
<body name="9a_pelvis" pos="0 0 -.165">
<joint name="9a_abdomen_x" pos="0 0 .1" axis="1 0 0" range="-35 35" class="big_joint"/>
<geom name="9a_butt" fromto="-.02 -.07 0 -.02 .07 0" size=".09"/>
<body name="9a_right_thigh" pos="0 -.1 -.04">
<joint name="9a_right_hip_x" axis="1 0 0" range="-25 5" class="big_joint"/>
<joint name="9a_right_hip_z" axis="0 0 1" range="-60 35" class="big_joint"/>
<joint name="9a_right_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
<geom name="9a_right_thigh" fromto="0 0 0 0 .01 -.34" size=".06"/>
<body name="9a_right_shin" pos="0 .01 -.403">
<joint name="9a_right_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
<geom name="9a_right_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
<body name="9a_right_foot" pos="0 0 -.39">
<joint name="9a_right_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
<joint name="9a_right_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
<geom name="9a_right_right_foot" fromto="-.07 -.02 0 .14 -.04 0" size=".027"/>
<geom name="9a_left_right_foot" fromto="-.07 0 0 .14 .02 0" size=".027"/>
</body>
</body>
</body>
<body name="9a_left_thigh" pos="0 .1 -.04">
<joint name="9a_left_hip_x" axis="-1 0 0" range="-25 5" class="big_joint"/>
<joint name="9a_left_hip_z" axis="0 0 -1" range="-60 35" class="big_joint"/>
<joint name="9a_left_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
<geom name="9a_left_thigh" fromto="0 0 0 0 -.01 -.34" size=".06"/>
<body name="9a_left_shin" pos="0 -.01 -.403">
<joint name="9a_left_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
<geom name="9a_left_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
<body name="9a_left_foot" pos="0 0 -.39">
<joint name="9a_left_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
<joint name="9a_left_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
<geom name="9a_left_left_foot" fromto="-.07 .02 0 .14 .04 0" size=".027"/>
<geom name="9a_right_left_foot" fromto="-.07 0 0 .14 -.02 0" size=".027"/>
</body>
</body>
</body>
</body>
</body>
<body name="9a_right_upper_arm" pos="0 -.17 .06">
<joint name="9a_right_shoulder1" axis="2 1 1" range="-85 60"/>
<joint name="9a_right_shoulder2" axis="0 -1 1" range="-85 60"/>
<geom name="9a_right_upper_arm" fromto="0 0 0 .16 -.16 -.16" size=".04 .16"/>
<body name="9a_right_lower_arm" pos=".18 -.18 -.18">
<joint name="9a_right_elbow" axis="0 -1 1" range="-90 50" stiffness="0"/>
<geom name="9a_right_lower_arm" fromto=".01 .01 .01 .17 .17 .17" size=".031"/>
<body name="9a_right_hand" pos=".18 .18 .18">
<geom name="9a_right_hand" type="sphere" size=".04" zaxis="1 1 1"/>
</body>
</body>
</body>
<body name="9a_left_upper_arm" pos="0 .17 .06">
<joint name="9a_left_shoulder1" axis="2 -1 1" range="-60 85"/>
<joint name="9a_left_shoulder2" axis="0 1 1" range="-60 85"/>
<geom name="9a_left_upper_arm" fromto="0 0 0 .16 .16 -.16" size=".04 .16"/>
<body name="9a_left_lower_arm" pos=".18 .18 -.18">
<joint name="9a_left_elbow" axis="0 -1 -1" range="-90 50" stiffness="0"/>
<geom name="9a_left_lower_arm" fromto=".01 -.01 .01 .17 -.17 .17" size=".031"/>
<body name="9a_left_hand" pos=".18 -.18 .18">
<geom name="9a_left_hand" type="sphere" size=".04" zaxis="1 -1 1"/>
</body>
</body>
</body>
</body>
</worldbody>
<actuator>
<motor gear="40" joint="1a_abdomen_y"/>
<motor gear="40" joint="1a_abdomen_z"/>
<motor gear="40" joint="1a_abdomen_x"/>
<motor gear="40" joint="1a_right_hip_x"/>
<motor gear="40" joint="1a_right_hip_z"/>
<motor gear="120" joint="1a_right_hip_y"/>
<motor gear="80" joint="1a_right_knee"/>
<motor gear="20" joint="1a_right_ankle_x"/>
<motor gear="20" joint="1a_right_ankle_y"/>
<motor gear="40" joint="1a_left_hip_x"/>
<motor gear="40" joint="1a_left_hip_z"/>
<motor gear="120" joint="1a_left_hip_y"/>
<motor gear="80" joint="1a_left_knee"/>
<motor gear="20" joint="1a_left_ankle_x"/>
<motor gear="20" joint="1a_left_ankle_y"/>
<motor gear="20" joint="1a_right_shoulder1"/>
<motor gear="20" joint="1a_right_shoulder2"/>
<motor gear="40" joint="1a_right_elbow"/>
<motor gear="20" joint="1a_left_shoulder1"/>
<motor gear="20" joint="1a_left_shoulder2"/>
<motor gear="40" joint="1a_left_elbow"/>
</actuator>
<actuator>
<motor gear="40" joint="2a_abdomen_y"/>
<motor gear="40" joint="2a_abdomen_z"/>
<motor gear="40" joint="2a_abdomen_x"/>
<motor gear="40" joint="2a_right_hip_x"/>
<motor gear="40" joint="2a_right_hip_z"/>
<motor gear="120" joint="2a_right_hip_y"/>
<motor gear="80" joint="2a_right_knee"/>
<motor gear="20" joint="2a_right_ankle_x"/>
<motor gear="20" joint="2a_right_ankle_y"/>
<motor gear="40" joint="2a_left_hip_x"/>
<motor gear="40" joint="2a_left_hip_z"/>
<motor gear="120" joint="2a_left_hip_y"/>
<motor gear="80" joint="2a_left_knee"/>
<motor gear="20" joint="2a_left_ankle_x"/>
<motor gear="20" joint="2a_left_ankle_y"/>
<motor gear="20" joint="2a_right_shoulder1"/>
<motor gear="20" joint="2a_right_shoulder2"/>
<motor gear="40" joint="2a_right_elbow"/>
<motor gear="20" joint="2a_left_shoulder1"/>
<motor gear="20" joint="2a_left_shoulder2"/>
<motor gear="40" joint="2a_left_elbow"/>
</actuator>
<actuator>
<motor gear="40" joint="3a_abdomen_y"/>
<motor gear="40" joint="3a_abdomen_z"/>
<motor gear="40" joint="3a_abdomen_x"/>
<motor gear="40" joint="3a_right_hip_x"/>
<motor gear="40" joint="3a_right_hip_z"/>
<motor gear="120" joint="3a_right_hip_y"/>
<motor gear="80" joint="3a_right_knee"/>
<motor gear="20" joint="3a_right_ankle_x"/>
<motor gear="20" joint="3a_right_ankle_y"/>
<motor gear="40" joint="3a_left_hip_x"/>
<motor gear="40" joint="3a_left_hip_z"/>
<motor gear="120" joint="3a_left_hip_y"/>
<motor gear="80" joint="3a_left_knee"/>
<motor gear="20" joint="3a_left_ankle_x"/>
<motor gear="20" joint="3a_left_ankle_y"/>
<motor gear="20" joint="3a_right_shoulder1"/>
<motor gear="20" joint="3a_right_shoulder2"/>
<motor gear="40" joint="3a_right_elbow"/>
<motor gear="20" joint="3a_left_shoulder1"/>
<motor gear="20" joint="3a_left_shoulder2"/>
<motor gear="40" joint="3a_left_elbow"/>
</actuator>
<actuator>
<motor gear="40" joint="4a_abdomen_y"/>
<motor gear="40" joint="4a_abdomen_z"/>
<motor gear="40" joint="4a_abdomen_x"/>
<motor gear="40" joint="4a_right_hip_x"/>
<motor gear="40" joint="4a_right_hip_z"/>
<motor gear="120" joint="4a_right_hip_y"/>
<motor gear="80" joint="4a_right_knee"/>
<motor gear="20" joint="4a_right_ankle_x"/>
<motor gear="20" joint="4a_right_ankle_y"/>
<motor gear="40" joint="4a_left_hip_x"/>
<motor gear="40" joint="4a_left_hip_z"/>
<motor gear="120" joint="4a_left_hip_y"/>
<motor gear="80" joint="4a_left_knee"/>
<motor gear="20" joint="4a_left_ankle_x"/>
<motor gear="20" joint="4a_left_ankle_y"/>
<motor gear="20" joint="4a_right_shoulder1"/>
<motor gear="20" joint="4a_right_shoulder2"/>
<motor gear="40" joint="4a_right_elbow"/>
<motor gear="20" joint="4a_left_shoulder1"/>
<motor gear="20" joint="4a_left_shoulder2"/>
<motor gear="40" joint="4a_left_elbow"/>
</actuator>
<actuator>
<motor gear="40" joint="5a_abdomen_y"/>
<motor gear="40" joint="5a_abdomen_z"/>
<motor gear="40" joint="5a_abdomen_x"/>
<motor gear="40" joint="5a_right_hip_x"/>
<motor gear="40" joint="5a_right_hip_z"/>
<motor gear="120" joint="5a_right_hip_y"/>
<motor gear="80" joint="5a_right_knee"/>
<motor gear="20" joint="5a_right_ankle_x"/>
<motor gear="20" joint="5a_right_ankle_y"/>
<motor gear="40" joint="5a_left_hip_x"/>
<motor gear="40" joint="5a_left_hip_z"/>
<motor gear="120" joint="5a_left_hip_y"/>
<motor gear="80" joint="5a_left_knee"/>
<motor gear="20" joint="5a_left_ankle_x"/>
<motor gear="20" joint="5a_left_ankle_y"/>
<motor gear="20" joint="5a_right_shoulder1"/>
<motor gear="20" joint="5a_right_shoulder2"/>
<motor gear="40" joint="5a_right_elbow"/>
<motor gear="20" joint="5a_left_shoulder1"/>
<motor gear="20" joint="5a_left_shoulder2"/>
<motor gear="40" joint="5a_left_elbow"/>
</actuator>
<actuator>
<motor gear="40" joint="6a_abdomen_y"/>
<motor gear="40" joint="6a_abdomen_z"/>
<motor gear="40" joint="6a_abdomen_x"/>
<motor gear="40" joint="6a_right_hip_x"/>
<motor gear="40" joint="6a_right_hip_z"/>
<motor gear="120" joint="6a_right_hip_y"/>
<motor gear="80" joint="6a_right_knee"/>
<motor gear="20" joint="6a_right_ankle_x"/>
<motor gear="20" joint="6a_right_ankle_y"/>
<motor gear="40" joint="6a_left_hip_x"/>
<motor gear="40" joint="6a_left_hip_z"/>
<motor gear="120" joint="6a_left_hip_y"/>
<motor gear="80" joint="6a_left_knee"/>
<motor gear="20" joint="6a_left_ankle_x"/>
<motor gear="20" joint="6a_left_ankle_y"/>
<motor gear="20" joint="6a_right_shoulder1"/>
<motor gear="20" joint="6a_right_shoulder2"/>
<motor gear="40" joint="6a_right_elbow"/>
<motor gear="20" joint="6a_left_shoulder1"/>
<motor gear="20" joint="6a_left_shoulder2"/>
<motor gear="40" joint="6a_left_elbow"/>
</actuator>
<actuator>
<motor gear="40" joint="7a_abdomen_y"/>
<motor gear="40" joint="7a_abdomen_z"/>
<motor gear="40" joint="7a_abdomen_x"/>
<motor gear="40" joint="7a_right_hip_x"/>
<motor gear="40" joint="7a_right_hip_z"/>
<motor gear="120" joint="7a_right_hip_y"/>
<motor gear="80" joint="7a_right_knee"/>
<motor gear="20" joint="7a_right_ankle_x"/>
<motor gear="20" joint="7a_right_ankle_y"/>
<motor gear="40" joint="7a_left_hip_x"/>
<motor gear="40" joint="7a_left_hip_z"/>
<motor gear="120" joint="7a_left_hip_y"/>
<motor gear="80" joint="7a_left_knee"/>
<motor gear="20" joint="7a_left_ankle_x"/>
<motor gear="20" joint="7a_left_ankle_y"/>
<motor gear="20" joint="7a_right_shoulder1"/>
<motor gear="20" joint="7a_right_shoulder2"/>
<motor gear="40" joint="7a_right_elbow"/>
<motor gear="20" joint="7a_left_shoulder1"/>
<motor gear="20" joint="7a_left_shoulder2"/>
<motor gear="40" joint="7a_left_elbow"/>
</actuator>
<actuator>
<motor gear="40" joint="8a_abdomen_y"/>
<motor gear="40" joint="8a_abdomen_z"/>
<motor gear="40" joint="8a_abdomen_x"/>
<motor gear="40" joint="8a_right_hip_x"/>
<motor gear="40" joint="8a_right_hip_z"/>
<motor gear="120" joint="8a_right_hip_y"/>
<motor gear="80" joint="8a_right_knee"/>
<motor gear="20" joint="8a_right_ankle_x"/>
<motor gear="20" joint="8a_right_ankle_y"/>
<motor gear="40" joint="8a_left_hip_x"/>
<motor gear="40" joint="8a_left_hip_z"/>
<motor gear="120" joint="8a_left_hip_y"/>
<motor gear="80" joint="8a_left_knee"/>
<motor gear="20" joint="8a_left_ankle_x"/>
<motor gear="20" joint="8a_left_ankle_y"/>
<motor gear="20" joint="8a_right_shoulder1"/>
<motor gear="20" joint="8a_right_shoulder2"/>
<motor gear="40" joint="8a_right_elbow"/>
<motor gear="20" joint="8a_left_shoulder1"/>
<motor gear="20" joint="8a_left_shoulder2"/>
<motor gear="40" joint="8a_left_elbow"/>
</actuator>
<actuator>
<motor gear="40" joint="9a_abdomen_y"/>
<motor gear="40" joint="9a_abdomen_z"/>
<motor gear="40" joint="9a_abdomen_x"/>
<motor gear="40" joint="9a_right_hip_x"/>
<motor gear="40" joint="9a_right_hip_z"/>
<motor gear="120" joint="9a_right_hip_y"/>
<motor gear="80" joint="9a_right_knee"/>
<motor gear="20" joint="9a_right_ankle_x"/>
<motor gear="20" joint="9a_right_ankle_y"/>
<motor gear="40" joint="9a_left_hip_x"/>
<motor gear="40" joint="9a_left_hip_z"/>
<motor gear="120" joint="9a_left_hip_y"/>
<motor gear="80" joint="9a_left_knee"/>
<motor gear="20" joint="9a_left_ankle_x"/>
<motor gear="20" joint="9a_left_ankle_y"/>
<motor gear="20" joint="9a_right_shoulder1"/>
<motor gear="20" joint="9a_right_shoulder2"/>
<motor gear="40" joint="9a_right_elbow"/>
<motor gear="20" joint="9a_left_shoulder1"/>
<motor gear="20" joint="9a_left_shoulder2"/>
<motor gear="40" joint="9a_left_elbow"/>
</actuator>
</mujoco>
mjx/mujoco/mjx/test_data/humanoid/10_humanoids.xml
+1087
View File
File diff suppressed because it is too large Load Diff
mjx/mujoco/mjx/test_data/shadow_hand/right_hand.xml
+5 -5
View File
@@ -156,7 +156,7 @@
diaginertia="1.28092e-06 1.12092e-06 5.3e-07"/>
<joint name="rh_FFJ1" class="middle_distal"/>
<geom class="plastic_visual" mesh="f_distal_pst"/>
<geom class="plastic_collision" type="mesh" mesh="f_distal_pst_214"/>
<geom class="plastic_collision" type="mesh" mesh="f_distal_pst_214" contype="0" conaffinity="2"/>
</body>
</body>
</body>
@@ -181,7 +181,7 @@
diaginertia="1.28092e-06 1.12092e-06 5.3e-07"/>
<joint name="rh_MFJ1" class="middle_distal"/>
<geom class="plastic_visual" mesh="f_distal_pst"/>
<geom class="plastic_collision" type="mesh" mesh="f_distal_pst_214"/>
<geom class="plastic_collision" type="mesh" mesh="f_distal_pst_214" contype="0" conaffinity="2"/>
</body>
</body>
</body>
@@ -206,7 +206,7 @@
diaginertia="1.28092e-06 1.12092e-06 5.3e-07"/>
<joint name="rh_RFJ1" class="middle_distal"/>
<geom class="plastic_visual" mesh="f_distal_pst"/>
<geom class="plastic_collision" type="mesh" mesh="f_distal_pst_214"/>
<geom class="plastic_collision" type="mesh" mesh="f_distal_pst_214" contype="0" conaffinity="2"/>
</body>
</body>
</body>
@@ -236,7 +236,7 @@
diaginertia="1.28092e-06 1.12092e-06 5.3e-07"/>
<joint name="rh_LFJ1" class="middle_distal"/>
<geom class="plastic_visual" mesh="f_distal_pst"/>
<geom class="plastic_collision" type="mesh" mesh="f_distal_pst_214"/>
<geom class="plastic_collision" type="mesh" mesh="f_distal_pst_214" contype="0" conaffinity="2"/>
</body>
</body>
</body>
@@ -266,7 +266,7 @@
diaginertia="2.37794e-06 2.27794e-06 1e-06"/>
<joint name="rh_THJ1" class="thdistal"/>
<geom class="plastic_visual" mesh="th_distal_pst"/>
<geom class="plastic_collision" type="mesh" mesh="th_distal_pst_190"/>
<geom class="plastic_collision" type="mesh" mesh="th_distal_pst_190" contype="0" conaffinity="2"/>
</body>
</body>
</body>
mjx/mujoco/mjx/testspeed.py
+41 -84
View File
@@ -14,110 +14,67 @@
# ==============================================================================
"""Run benchmarks on various devices."""
import os
import time
from typing import Sequence, Tuple
from typing import Sequence
from absl import app
from absl import flags
from etils import epath
import jax
import mujoco
from mujoco import mjx
FLAGS = flags.FLAGS
flags.DEFINE_string('mjcf', None, 'path to model', required=True)
flags.DEFINE_integer('nstep', 1000, 'number of steps per rollout')
flags.DEFINE_integer('batch_size', 1024, 'number of parallel rollouts')
flags.DEFINE_integer('unroll', 1, 'loop unroll length')
flags.DEFINE_enum('solver', 'cg', ['cg', 'newton'], 'constraint solver')
flags.DEFINE_integer('iterations', 1, 'number of solver iterations')
flags.DEFINE_integer('ls_iterations', 4, 'number of linesearch iterations')
flags.DEFINE_enum('output', 'text', ['text', 'tsv'], 'format to print results')
def _measure(fn, *args) -> Tuple[float, float]:
"""Reports jit time and op time for a function."""
beg = time.perf_counter()
compiled_fn = fn.lower(*args).compile()
end = time.perf_counter()
jit_time = end - beg
beg = time.perf_counter()
result = compiled_fn(*args)
jax.block_until_ready(result)
end = time.perf_counter()
run_time = end - beg
return jit_time, run_time
_MJCF = flags.DEFINE_string('mjcf', None, 'path to model', required=True)
_BASE_PATH = flags.DEFINE_string(
'base_path', None, 'base path, defaults to mujoco.mjx resource path'
)
_NSTEP = flags.DEFINE_integer('nstep', 1000, 'number of steps per rollout')
_BATCH_SIZE = flags.DEFINE_integer(
'batch_size', 1024, 'number of parallel rollouts'
)
_UNROLL = flags.DEFINE_integer('unroll', 1, 'loop unroll length')
_SOLVER = flags.DEFINE_enum(
'solver', 'cg', ['cg', 'newton'], 'constraint solver'
)
_ITERATIONS = flags.DEFINE_integer(
'iterations', 1, 'number of solver iterations'
)
_LS_ITERATIONS = flags.DEFINE_integer(
'ls_iterations', 4, 'number of linesearch iterations'
)
_OUTPUT = flags.DEFINE_enum(
'output', 'text', ['text', 'tsv'], 'format to print results'
)
def _main(argv: Sequence[str]):
"""Benchmark a model."""
xla_flags = os.environ.get('XLA_FLAGS', '')
xla_flags += ' --xla_gpu_triton_gemm_any=True'
os.environ['XLA_FLAGS'] = xla_flags
f = epath.resource_path('mujoco.mjx') / 'test_data' / FLAGS.mjcf
"""Runs testpeed function."""
base_path = _BASE_PATH.value or epath.resource_path('mujoco.mjx')
f = base_path / 'test_data' / _MJCF.value
m = mujoco.MjModel.from_xml_path(f.as_posix())
m.opt.solver = {
'cg': mujoco.mjtSolver.mjSOL_CG,
'newton': mujoco.mjtSolver.mjSOL_NEWTON,
}[FLAGS.solver.lower()]
m.opt.iterations = FLAGS.iterations
m.opt.ls_iterations = FLAGS.ls_iterations
m = mjx.put_model(m)
if FLAGS.output == 'text':
print(f"Rolling out {FLAGS.nstep} steps at dt = {m.opt.timestep:.3f}...")
print(f'Rolling out {_NSTEP.value} steps at dt = {m.opt.timestep:.3f}...')
jit_time, run_time, steps = mjx.benchmark(
m,
_NSTEP.value,
_BATCH_SIZE.value,
_UNROLL.value,
_SOLVER.value,
_ITERATIONS.value,
_LS_ITERATIONS.value,
)
@jax.pmap
def init(key):
key = jax.random.split(key, FLAGS.batch_size // jax.device_count())
@jax.vmap
def random_init(key):
d = mjx.make_data(m)
qvel = 0.01 * jax.random.normal(key, shape=(m.nv,))
d = d.replace(qvel=qvel)
return d
return random_init(key)
key = jax.random.split(jax.random.key(0), jax.device_count())
d = init(key)
jax.block_until_ready(d)
@jax.pmap
def unroll(d):
@jax.vmap
def step(d, _):
d = mjx.step(m, d)
return d, None
d, _ = jax.lax.scan(step, d, None, length=FLAGS.nstep, unroll=FLAGS.unroll)
return d
jit_time, run_time = _measure(unroll, d)
steps = FLAGS.nstep * FLAGS.batch_size
if FLAGS.output == 'text':
name = argv[0]
if _OUTPUT.value == 'text':
print(f"""
Summary for {FLAGS.batch_size} parallel rollouts
Summary for {_BATCH_SIZE.value} parallel rollouts
Total JIT time: {jit_time:.2f} s
Total simulation time: {run_time:.2f} s
Total steps per second: { steps / run_time:.0f}
Total realtime factor: { steps * m.opt.timestep / run_time:.2f} x
Total time per step: { 1e6 * run_time / steps:.2f} µs""")
elif FLAGS.output == 'tsv':
name = argv[0].split('/')[-1].replace('testspeed_', '')
print(f"{name}\tjit: {jit_time:.2f}s\tsteps/second: {steps / run_time:.0f}")
elif _OUTPUT.value == 'tsv':
name = name.split('/')[-1].replace('testspeed_', '')
print(f'{name}\tjit: {jit_time:.2f}s\tsteps/second: {steps / run_time:.0f}')
def main():
mjx/tutorial.ipynb
+1 -1
View File
@@ -367,7 +367,7 @@
"rng = jax.random.split(rng, 4096)\n",
"batch = jax.vmap(lambda rng: mjx_data.replace(qpos=jax.random.uniform(rng, (1,))))(rng)\n",
"\n",
"jit_step = jax.vmap(mjx.step, in_axes=(None, 0))\n",
"jit_step = jax.jit(jax.vmap(mjx.step, in_axes=(None, 0)))\n",
"batch = jit_step(mjx_model, batch)\n",
"\n",
"print(batch.qpos)"
python/least_squares.ipynb
+2 -2
View File
@@ -234,10 +234,10 @@
"$$\n",
"\\begin{aligned}\n",
"x^* &= \\arg \\min_x f(x)\\\\\n",
"\\textrm{s.t.} &\\quad l \\succcurlyeq x \\succcurlyeq u\n",
"\\textrm{s.t.} &\\quad l \\preccurlyeq x \\preccurlyeq u\n",
"\\end{aligned}\n",
"$$\n",
"Where $l$ and $u$ are respectively lower and upper bound vectors and the inequalities $l \\succcurlyeq x \\succcurlyeq u$ are read elementwise. Solving for the minimizing $\\delta x$ is now a bit more involved than solving a linear system, and the optimization method is now referred to as [Sequential Quadratic Programming](https://en.wikipedia.org/wiki/Sequential_quadratic_programming) (SQP). Fortunately MuJoCo has an efficient box-constrained QP solver: the [mju_boxQP](https://mujoco.readthedocs.io/en/latest/APIreference/APIfunctions.html#mju-boxqp) function."
"Where $l$ and $u$ are respectively lower and upper bound vectors and the inequalities $l \\preccurlyeq x \\preccurlyeq u$ are read elementwise. Solving for the minimizing $\\delta x$ is now a bit more involved than solving a linear system, and the optimization method is now referred to as [Sequential Quadratic Programming](https://en.wikipedia.org/wiki/Sequential_quadratic_programming) (SQP). Fortunately MuJoCo has an efficient box-constrained QP solver: the [mju_boxQP](https://mujoco.readthedocs.io/en/latest/APIreference/APIfunctions.html#mju-boxqp) function."
]
},
{
python/mujoco/bindings_test.py
+4 -4
View File
@@ -623,7 +623,7 @@ class MuJoCoBindingsTest(parameterized.TestCase):
self.assertEqual(struct, struct2)
self.assertNotEqual(struct, 3)
self.assertNotEqual(struct, None)
self.assertIsNotNone(struct)
# mutable structs shouldn't declare __hash__
with self.assertRaises(TypeError):
@@ -705,7 +705,8 @@ class MuJoCoBindingsTest(parameterized.TestCase):
# Check that the output argument must have the correct dtype.
with self.assertRaises(TypeError):
mujoco.mju_rotVecQuat(vec, quat, res=np.zeros(3, int))
res = np.zeros(3, np.int32)
mujoco.mju_rotVecQuat(res, vec, quat)
def test_getsetstate(self): # pylint: disable=invalid-name
mujoco.mj_step(self.model, self.data)
@@ -850,8 +851,7 @@ Euler integrator, semi-implicit in velocity.
self.assertEqual(mujoco.mjtEnableBit.mjENBL_OVERRIDE, 1<<0)
self.assertEqual(mujoco.mjtEnableBit.mjENBL_ENERGY, 1<<1)
self.assertEqual(mujoco.mjtEnableBit.mjENBL_FWDINV, 1<<2)
self.assertEqual(mujoco.mjtEnableBit.mjENBL_SENSORNOISE, 1<<4)
self.assertEqual(mujoco.mjtEnableBit.mjNENABLE, 7)
self.assertEqual(mujoco.mjtEnableBit.mjNENABLE, 6)
self.assertEqual(mujoco.mjtGeom.mjGEOM_PLANE, 0)
self.assertEqual(mujoco.mjtGeom.mjGEOM_HFIELD, 1)
self.assertEqual(mujoco.mjtGeom.mjGEOM_SPHERE, 2)
python/mujoco/testdata/usd_golden.usda
Vendored
+71
View File
@@ -0,0 +1,71 @@
#usda 1.0
(
defaultPrim = "World"
doc = """Generated from Composed Stage of root layer
"""
endTimeCode = 1
startTimeCode = 0
timeCodesPerSecond = 24
upAxis = "Z"
)
def Xform "World"
{
def Xform "Light_Xform_0"
{
double3 xformOp:translate
uniform token[] xformOpOrder = ["xformOp:translate"]
def SphereLight "Light_0"
{
bool inputs:normalize = 1
float inputs:radius = 0.3
bool treatAsPoint = 0
}
}
def Xform "CubeMesh_Xform_white_box"
{
matrix4d xformOp:transform.timeSamples = {
0: ( (0, 0, 0, 0), (0, 0, 0, 0), (0, 0, 0, 0), (0, 0, 0, 1) ),
}
uniform token[] xformOpOrder = ["xformOp:transform"]
def Mesh "CubeMesh_white_box" (
apiSchemas = ["MaterialBindingAPI"]
)
{
int[] faceVertexCounts = [3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3]
int[] faceVertexIndices = [4, 7, 5, 4, 6, 7, 0, 2, 4, 2, 6, 4, 0, 1, 2, 1, 3, 2, 1, 5, 7, 1, 7, 3, 2, 3, 7, 2, 7, 6, 0, 4, 1, 1, 4, 5]
rel material:binding = </World/_materials/Material_white_box>
point3f[] points = [(-1, -1, -1), (1, -1, -1), (-1, -1, 1), (1, -1, 1), (-1, 1, -1), (1, 1, -1), (-1, 1, 1), (1, 1, 1)]
texCoord2f[] primvars:UVMap = [] (
interpolation = "faceVarying"
)
int[] primvars:UVMap:indices = [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35]
uniform token subdivisionScheme = "none"
token visibility.timeSamples = {
0: "inherited",
}
}
}
def "_materials"
{
def Material "Material_white_box"
{
token outputs:surface.connect = </World/_materials/Material_white_box/Principled_BSDF.outputs:surface>
def Shader "Principled_BSDF"
{
uniform token info:id = "UsdPreviewSurface"
color3f inputs:diffuseColor = (1, 1, 1)
float inputs:metallic = 0.5
float inputs:opacity = 1
float inputs:roughness = 0.5
token outputs:surface
}
}
}
}
python/mujoco/usd/component.py
+14 -126
View File
@@ -15,6 +15,7 @@
from typing import List, Optional, Tuple
import mujoco
import mujoco.usd.utils
import numpy as np
# TODO: b/288149332 - Remove once USD Python Binding works well with pytype.
@@ -38,7 +39,7 @@ class USDMesh:
geom: mujoco.MjvGeom,
obj_name: str,
dataid: int,
rgba: Tuple[int, ...] = (1, 1, 1, 1),
rgba: np.ndarray = np.array([1, 1, 1, 1]),
texture_file: Optional[str] = None,
):
self.stage = stage
@@ -236,7 +237,7 @@ class USDPrimitiveMesh:
stage: Usd.Stage,
geom: mujoco.MjvGeom,
obj_name: str,
rgba: Tuple[int, ...] = (1, 1, 1, 1),
rgba: np.ndarray = np.array([1, 1, 1, 1]),
texture_file: Optional[str] = None,
):
self.stage = stage
@@ -245,7 +246,10 @@ class USDPrimitiveMesh:
self.rgba = rgba
self.texture_file = texture_file
self.usd_prim = Usd.Prim()
self.usd_mesh = UsdGeom.Mesh()
self.prim_mesh = None
self.transform_op = Gf.Matrix4d(1.)
def _set_refinement_properties(self):
self.usd_prim.GetAttribute("subdivisionScheme").Set("none")
@@ -374,119 +378,6 @@ class USDPrimitiveMesh:
self.usd_prim.GetAttribute("visibility").Set("invisible", frame)
class USDPrimitive:
def __init__(
self,
stage: Usd.Stage,
geom: mujoco.MjvGeom,
obj_name: str,
rgba: Tuple[int, ...] = (1, 1, 1, 1),
texture_file: Optional[str] = None,
):
self.stage = stage
self.geom = geom
self.obj_name = obj_name
self.rgba = rgba
self.texture_file = texture_file
def _set_refinement_properties(self):
self.usd_prim.GetAttribute("subdivisionScheme").Set("none")
def _attach_material(self):
mtl_path = Sdf.Path(f"/World/_materials/Material_{self.obj_name}")
mtl = UsdShade.Material.Define(self.stage, mtl_path)
if self.texture_file:
bsdf_shader = UsdShade.Shader.Define(
self.stage, mtl_path.AppendPath("Principled_BSDF")
)
image_shader = UsdShade.Shader.Define(
self.stage, mtl_path.AppendPath("Image_Texture")
)
uvmap_shader = UsdShade.Shader.Define(
self.stage, mtl_path.AppendPath("uvmap")
)
# settings the bsdf shader attributes
bsdf_shader.CreateIdAttr("UsdPreviewSurface")
bsdf_shader.CreateInput(
"diffuseColor", Sdf.ValueTypeNames.Color3f
).ConnectToSource(image_shader.ConnectableAPI(), "rgb")
bsdf_shader.CreateInput("opacity", Sdf.ValueTypeNames.Float).Set(
float(self.rgba[-1])
)
bsdf_shader.CreateInput("metallic", Sdf.ValueTypeNames.Float).Set(
self.geom.shininess
)
bsdf_shader.CreateInput("roughness", Sdf.ValueTypeNames.Float).Set(
1.0 - self.geom.shininess
)
mtl.CreateSurfaceOutput().ConnectToSource(
bsdf_shader.ConnectableAPI(), "surface"
)
self.usd_primitive_shape.GetPrim().ApplyAPI(UsdShade.MaterialBindingAPI)
UsdShade.MaterialBindingAPI(self.usd_primitive_shape).Bind(mtl)
# setting the image texture attributes
image_shader.CreateIdAttr("UsdUVTexture")
image_shader.CreateInput("file", Sdf.ValueTypeNames.Asset).Set(
self.texture_file
)
image_shader.CreateInput(
"sourceColorSpace", Sdf.ValueTypeNames.Token
).Set("sRGB")
image_shader.CreateInput("st", Sdf.ValueTypeNames.Float2).ConnectToSource(
uvmap_shader.ConnectableAPI(), "result"
)
image_shader.CreateOutput("rgb", Sdf.ValueTypeNames.Float3)
# setting uvmap shader attributes
uvmap_shader.CreateIdAttr("UsdPrimvarReader_float2")
uvmap_shader.CreateInput("varname", Sdf.ValueTypeNames.Token).Set("UVMap")
uvmap_shader.CreateOutput("results", Sdf.ValueTypeNames.Float2)
else:
bsdf_shader = UsdShade.Shader.Define(
self.stage, mtl_path.AppendPath("Principled_BSDF")
)
# settings the bsdf shader attributes
bsdf_shader.CreateIdAttr("UsdPreviewSurface")
bsdf_shader.CreateInput("diffuseColor", Sdf.ValueTypeNames.Color3f).Set(
tuple(self.rgba[:3])
)
bsdf_shader.CreateInput("opacity", Sdf.ValueTypeNames.Float).Set(
float(self.rgba[-1])
)
bsdf_shader.CreateInput("metallic", Sdf.ValueTypeNames.Float).Set(
self.geom.shininess
)
bsdf_shader.CreateInput("roughness", Sdf.ValueTypeNames.Float).Set(
1.0 - self.geom.shininess
)
mtl.CreateSurfaceOutput().ConnectToSource(
bsdf_shader.ConnectableAPI(), "surface"
)
self.usd_primitive_shape.GetPrim().ApplyAPI(UsdShade.MaterialBindingAPI)
UsdShade.MaterialBindingAPI(self.usd_primitive_shape).Bind(mtl)
def update(self, pos: np.ndarray, mat: np.ndarray, visible: bool, frame: int):
transformation_mat = mujoco.usd.utils.create_transform_matrix(
rotation_matrix=mat, translation_vector=pos
).T
self.transform_op.Set(Gf.Matrix4d(transformation_mat.tolist()), frame)
self.update_visibility(visible, frame)
def update_visibility(self, visible: bool, frame: int):
if visible:
self.usd_prim.GetAttribute("visibility").Set("inherited", frame)
else:
self.usd_prim.GetAttribute("visibility").Set("invisible", frame)
class USDCapsule(USDPrimitive):
def __init__(
@@ -494,7 +385,7 @@ class USDCapsule(USDPrimitive):
stage: Usd.Stage,
geom: mujoco.MjvGeom,
obj_name: str,
rgba: Tuple[int, ...] = (1, 1, 1, 1),
rgba: np.ndarray = np.array([1, 1, 1, 1]),
texture_file: Optional[str] = None,
):
@@ -514,7 +405,7 @@ class USDCapsule(USDPrimitive):
self._set_size_attributes()
self._attach_material()
self._set_refinement_properties()
# self._set_refinement_properties()
def _set_size_attributes(self):
self.usd_primitive_shape.GetRadiusAttr().Set(float(self.geom.size[0]))
@@ -530,7 +421,7 @@ class USDEllipsoid(USDPrimitive):
stage: Usd.Stage,
geom: mujoco.MjvGeom,
obj_name: str,
rgba: Tuple[int, ...] = (1, 1, 1, 1),
rgba: np.ndarray = np.array([1, 1, 1, 1]),
texture_file: Optional[str] = None,
):
@@ -550,7 +441,7 @@ class USDEllipsoid(USDPrimitive):
self._set_size_attributes()
self._attach_material()
self._set_refinement_properties()
# self._set_refinement_properties()
def _set_size_attributes(self):
self.scale_op.Set(Gf.Vec3d(self.geom.size.tolist()))
@@ -563,7 +454,7 @@ class USDCubeMesh(USDPrimitiveMesh):
stage: Usd.Stage,
geom: mujoco.MjvGeom,
obj_name: str,
rgba: Tuple[int, ...] = (1, 1, 1, 1),
rgba: np.ndarray = np.array([1, 1, 1, 1]),
texture_file: Optional[str] = None,
):
@@ -579,8 +470,6 @@ class USDCubeMesh(USDPrimitiveMesh):
width=self.geom.size[0] * 2,
height=self.geom.size[1] * 2,
depth=self.geom.size[2] * 2,
create_uv_map=True,
map_texture_to_each_face=True,
)
self.prim_mesh.translate(-self.prim_mesh.get_center())
@@ -617,7 +506,7 @@ class USDSphereMesh(USDPrimitiveMesh):
stage: Usd.Stage,
geom: mujoco.MjvGeom,
obj_name: str,
rgba: Tuple[int, ...] = (1, 1, 1, 1),
rgba: np.ndarray = np.array([1, 1, 1, 1]),
texture_file: Optional[str] = None,
):
@@ -667,7 +556,7 @@ class USDCylinderMesh(USDPrimitiveMesh):
stage: Usd.Stage,
geom: mujoco.MjvGeom,
obj_name: str,
rgba: Tuple[int, ...] = (1, 1, 1, 1),
rgba: np.ndarray = np.array([1, 1, 1, 1]),
texture_file: Optional[str] = None,
):
@@ -719,7 +608,7 @@ class USDPlaneMesh(USDPrimitiveMesh):
stage: Usd.Stage,
geom: mujoco.MjvGeom,
obj_name: str,
rgba: Tuple[int, ...] = (1, 1, 1, 1),
rgba: np.ndarray = np.array([1, 1, 1, 1]),
texture_file: Optional[str] = None,
):
@@ -814,7 +703,6 @@ class USDDomeLight:
self.usd_light.GetNormalizeAttr().Set(True)
def update(self, intensity: int, color: np.ndarray, frame: int):
self.usd_light.GetIntensityAttr().Set(intensity)
self.usd_light.GetExposureAttr().Set(0.0)
self.usd_light.GetColorAttr().Set(Gf.Vec3d(color.tolist()))
python/mujoco/usd/demo.py
+1 -1
View File
@@ -14,6 +14,6 @@ if __name__ == "__main__":
exp.update_scene(d)
exp.save_scene(filetype="usd")
exp.save_scene(filetype="usda")
python/mujoco/usd/exporter.py
+22 -16
View File
@@ -15,6 +15,7 @@
import os
import mujoco
import mujoco.usd.component as component_module
import numpy as np
import scipy
import termcolor
@@ -235,7 +236,7 @@ class USDExporter:
# handles meshes in scene
if geom.type == mujoco.mjtGeom.mjGEOM_MESH:
usd_geom = mujoco.usd.component.USDMesh(
usd_geom = component_module.USDMesh(
stage=self.stage,
model=self.model,
geom=geom,
@@ -245,7 +246,7 @@ class USDExporter:
texture_file=texture_file,
)
elif geom.type == mujoco.mjtGeom.mjGEOM_PLANE:
usd_geom = mujoco.usd.component.USDPlaneMesh(
usd_geom = component_module.USDPlaneMesh(
stage=self.stage,
geom=geom,
obj_name=geom_name,
@@ -253,7 +254,7 @@ class USDExporter:
texture_file=texture_file,
)
elif geom.type == mujoco.mjtGeom.mjGEOM_SPHERE:
usd_geom = mujoco.usd.component.USDSphereMesh(
usd_geom = component_module.USDSphereMesh(
stage=self.stage,
geom=geom,
obj_name=geom_name,
@@ -261,7 +262,7 @@ class USDExporter:
texture_file=texture_file,
)
elif geom.type == mujoco.mjtGeom.mjGEOM_CAPSULE:
usd_geom = mujoco.usd.component.USDCapsule(
usd_geom = component_module.USDCapsule(
stage=self.stage,
geom=geom,
obj_name=geom_name,
@@ -269,7 +270,7 @@ class USDExporter:
texture_file=texture_file,
)
elif geom.type == mujoco.mjtGeom.mjGEOM_ELLIPSOID:
usd_geom = mujoco.usd.component.USDEllipsoid(
usd_geom = component_module.USDEllipsoid(
stage=self.stage,
geom=geom,
obj_name=geom_name,
@@ -277,7 +278,7 @@ class USDExporter:
texture_file=texture_file,
)
elif geom.type == mujoco.mjtGeom.mjGEOM_CYLINDER:
usd_geom = mujoco.usd.component.USDCylinderMesh(
usd_geom = component_module.USDCylinderMesh(
stage=self.stage,
geom=geom,
obj_name=geom_name,
@@ -285,7 +286,7 @@ class USDExporter:
texture_file=texture_file,
)
elif geom.type == mujoco.mjtGeom.mjGEOM_BOX:
usd_geom = mujoco.usd.component.USDCubeMesh(
usd_geom = component_module.USDCubeMesh(
stage=self.stage,
geom=geom,
obj_name=geom_name,
@@ -331,7 +332,8 @@ class USDExporter:
for i in range(self.scene.nlight):
light = self.scene.lights[i]
if not np.allclose(light.pos, [0, 0, 0]):
self.usd_lights.append(mujoco.usd.component.USDSphereLight(stage=self.stage, obj_name=i))
self.usd_lights.append
(component_module.USDSphereLight(stage=self.stage, obj_name=str(i)))
else:
self.usd_lights.append(None)
@@ -342,7 +344,7 @@ class USDExporter:
if np.allclose(light.pos, [0, 0, 0]):
continue
if self.usd_lights[i] is None:
if i >= len(self.usd_lights) or self.usd_lights[i] is None:
continue
self.usd_lights[i].update(
@@ -356,7 +358,8 @@ class USDExporter:
self.usd_cameras = []
if self.camera_names is not None:
for name in self.camera_names:
self.usd_cameras.append(mujoco.usd.USDCamera(stage=self.stage, obj_name=name))
self.usd_cameras.append(
component_module.USDCamera(stage=self.stage, obj_name=name))
def _update_cameras(
self,
@@ -372,7 +375,8 @@ class USDExporter:
data, scene_option=scene_option, camera=camera_name
)
avg_camera = mujoco.mjv_averageCamera(self.scene.camera[0], self.scene.camera[1])
avg_camera = mujoco.mjv_averageCamera(
self.scene.camera[0], self.scene.camera[1])
forward = avg_camera.forward
up = avg_camera.up
@@ -396,11 +400,12 @@ class USDExporter:
):
if light_type == "sphere":
new_light = mujoco.usd.component.USDSphereLight(stage=self.stage, obj_name=obj_name, radius=radius)
new_light = component_module.USDSphereLight(stage=self.stage, obj_name=str(objid), radius=radius)
new_light.update(pos=pos, intensity=intensity, color=color, frame=0)
new_light.update(pos=np.array(pos), intensity=intensity, color=color, frame=0)
elif light_type == "dome":
new_light = mujoco.usd.component.USDDomeLight(stage=self.stage, obj_name=obj_name)
new_light = component_module.USDDomeLight(
stage=self.stage, obj_name=str(objid))
new_light.update(intensity=intensity, color=color, frame=0)
@@ -410,11 +415,12 @@ class USDExporter:
rotation_xyz: List[float],
obj_name: Optional[str] = "camera_1",
):
new_camera = mujoco.usd.component.USDCamera(stage=self.stage, obj_name=obj_name)
new_camera = component_module.USDCamera(
stage=self.stage, obj_name=str(objid))
r = scipy.spatial.transform.Rotation.from_euler(
"xyz", rotation_xyz, degrees=True)
new_camera.update(cam_pos=pos, cam_mat=r.as_matrix(), frame=0)
new_camera.update(cam_pos=np.array(pos), cam_mat=r.as_matrix(), frame=0)
def save_scene(self, filetype: str = "usd"):
self.stage.SetEndTimeCode(self.frame_count)
python/mujoco/usd/exporter_test.py
+70
View File
@@ -0,0 +1,70 @@
# Copyright 2024 DeepMind Technologies Limited
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
# ==============================================================================
"""Tests for the MuJoCo USD Exporter."""
import logging
import os
import mujoco
from absl.testing import absltest
from etils import epath
# Open3D and USD are not fully supported on all MuJoCo architectures.
# pylint: disable=python.style(g-import-not-at-top)
execute_test = True
try:
from mujoco.usd import exporter as exporter_module
from pxr import Usd
except ImportError:
logging.warning('Skipping test due to missing import')
execute_test = False
# pylint: enable=python.style(g-import-not-at-top)
class ExporterTest(absltest.TestCase):
def test_usd_export(self):
if not execute_test:
return
output_dir = os.getenv('TEST_UNDECLARED_OUTPUTS_DIR')
xml = """
<mujoco>
<worldbody>
<camera name="closeup" pos="0 -6 0" xyaxes="1 0 0 0 1 100"/>
<geom name="white_box" type="box" size="1 1 1" rgba="1 1 1 1"/>
</worldbody>
</mujoco>
"""
model = mujoco.MjModel.from_xml_string(xml)
data = mujoco.MjData(model)
exporter = exporter_module.USDExporter(
model,
output_directory_name="mujoco_usdpkg",
output_directory_root=output_dir,
)
exporter.update_scene(data)
exporter.save_scene("export.usda")
with open(os.path.join(
output_dir, "mujoco_usdpkg/frames", "frame_1_.export.usda"), "r") as f:
golden_path = os.path.join(
epath.resource_path("mujoco"), "testdata", "usd_golden.usda")
with open(golden_path, "r") as golden_file:
self.assertEqual(f.read(), golden_file.read())
if __name__ == "__main__":
absltest.main()
python/mujoco/usd/shapes.py
View File
python/mujoco/viewer.py
+6
View File
@@ -169,6 +169,9 @@ def _file_loader(path: str) -> _LoaderWithPathType:
"""Loads an MJCF model from file path."""
def load(path=path) -> Tuple[mujoco.MjModel, mujoco.MjData, str]:
if len(path) >= 4 and path[-4:] == '.mjb':
m = mujoco.MjModel.from_binary_path(path)
else:
m = mujoco.MjModel.from_xml_path(path)
d = mujoco.MjData(m)
return m, d, path
@@ -199,6 +202,9 @@ def _reload(
path = load_tuple[2] if len(load_tuple) == 3 else ''
simulate.load(m, d, path)
# Make sure any load_error message is cleared
simulate.load_error = ''
if notify_loaded:
notify_loaded()
src/engine/engine_collision_box.c
+4 -4
View File
@@ -638,7 +638,7 @@ int mjc_BoxBox(const mjModel* M, const mjData* D, mjContact* con, int g1, int g2
mju_sub3(tmp1, pos1, pos2);
mju_rotVecMatT(pos12, tmp1, mat2);
mju_mulMatTMat(rot, mat1, mat2, 3, 3, 3);
mju_mulMatTMat3(rot, mat1, mat2);
mju_transpose(rott, rot, 3, 3);
for (i = 0; i < 9; i++)
@@ -804,7 +804,7 @@ int mjc_BoxBox(const mjModel* M, const mjData* D, mjContact* con, int g1, int g2
}
if (q2) {
mju_mulMatMatT(r, rotmore, rot, 3, 3, 3);
mju_mulMatMatT3(r, rotmore, rot);
// mju_rotVecMat(p,pos12,rotmore);
// mju_rotVecMat(tmp1,size2,rotmore);
@@ -956,7 +956,7 @@ int mjc_BoxBox(const mjModel* M, const mjData* D, mjContact* con, int g1, int g2
}
mju_mulMatMatT(r, q2 ? mat2 : mat1, rotmore, 3, 3, 3);
mju_mulMatMatT3(r, q2 ? mat2 : mat1, rotmore);
mju_copy3(p, q2 ? pos2 : pos1);
tmp2[0] = (q2 ? -1 : 1) * r[2];
@@ -1319,7 +1319,7 @@ edgeedge:
}
}
mju_mulMatMatT(r, mat1, rotmore, 3, 3, 3);
mju_mulMatMatT3(r, mat1, rotmore);
mju_rotVecMat(tmp1, rnorm, r);
src/engine/engine_collision_convex.c
+2 -2
View File
@@ -318,7 +318,7 @@ static void mju_rotateFrame(const mjtNum origin[3], const mjtNum rot[9],
mjtNum mat[9], vec[3], rel[3];
// rotate frame: xmat = rot*xmat
mju_mulMatMat(mat, rot, xmat, 3, 3, 3);
mju_mulMatMat3(mat, rot, xmat);
mju_copy(xmat, mat, 9);
// vector to rotation origin: rel = origin - xpos
@@ -682,7 +682,7 @@ int mjc_ConvexHField(const mjModel* m, const mjData* d,
}
// express geom2 mat in heightfield frame
mju_mulMatTMat(mat, mat1, mat2, 3, 3, 3);
mju_mulMatTMat3(mat, mat1, mat2);
//------------------------------------- AABB computation, box-box test
src/engine/engine_collision_sdf.c
+12 -3
View File
@@ -140,9 +140,18 @@ static void geomGradient(mjtNum gradient[3], const mjModel* m, const mjData* d,
b[2] = a[2] / size[2];
mjtNum k0 = mju_norm3(a);
mjtNum k1 = mju_norm3(b);
gradient[0] = a[0]*(2.*k0 - 1.) / k1 + k0*(k0 - 1.) * b[0]/(k1*k1);
gradient[1] = a[1]*(2.*k0 - 1.) / k1 + k0*(k0 - 1.) * b[1]/(k1*k1);
gradient[2] = a[2]*(2.*k0 - 1.) / k1 + k0*(k0 - 1.) * b[2]/(k1*k1);
mjtNum invK0 = 1. / k0;
mjtNum invK1 = 1. / k1;
mjtNum gk0[3] = {b[0]*invK0, b[1]*invK0, b[2]*invK0};
mjtNum gk1[3] = {b[0]*invK1/(size[0]*size[0]),
b[1]*invK1/(size[1]*size[1]),
b[2]*invK1/(size[2]*size[2])};
mjtNum df_dk0 = (2.*k0 - 1.) * invK1;
mjtNum df_dk1 = k0*(k0 - 1.) * invK1 * invK1;
gradient[0] = gk0[0]*df_dk0 - gk1[0]*df_dk1;
gradient[1] = gk0[1]*df_dk0 - gk1[1]*df_dk1;
gradient[2] = gk0[2]*df_dk0 - gk1[2]*df_dk1;
mju_normalize3(gradient);
break;
case mjGEOM_CYLINDER:
c = mju_sqrt(x[0]*x[0]+x[1]*x[1]);
src/engine/engine_derivative.c
+1 -1
View File
@@ -251,7 +251,7 @@ void mjd_subQuat(const mjtNum qa[4], const mjtNum qb[4], mjtNum Da[9], mjtNum Db
// add term linear in K * K
mjtNum KK[9];
mju_mulMatMat(KK, K, K, 3, 3, 3);
mju_mulMatMat3(KK, K, K);
mjtNum coef = 1.0 - (half_angle < 6e-8 ? 1.0 : half_angle / mju_tan(half_angle));
mju_addToScl(Da_tmp, KK, coef, 9);
src/engine/engine_forward.c
+53 -26
View File
@@ -243,6 +243,19 @@ static mjtNum nextActivation(const mjModel* m, const mjData* d,
// clamp vector to range
static void clampVec(mjtNum* vec, const mjtNum* range, const mjtByte* limited, int n,
const int* index) {
for (int i=0; i < n; i++) {
int j = index ? index[i] : i;
if (limited[i]) {
vec[j] = mju_clip(vec[j], range[2*i], range[2*i + 1]);
}
}
}
// (qpos, qvel, ctrl, act) => (qfrc_actuator, actuator_force, act_dot)
void mj_fwdActuation(const mjModel* m, mjData* d) {
TM_START;
@@ -262,18 +275,9 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
// local, clamped copy of ctrl
mj_markStack(d);
mjtNum *ctrl = mj_stackAllocNum(d, nu);
if (mjDISABLED(mjDSBL_CLAMPCTRL)) {
mju_copy(ctrl, d->ctrl, nu);
} else {
for (int i=0; i < nu; i++) {
// clamp ctrl
if (m->actuator_ctrllimited[i]) {
mjtNum *ctrlrange = m->actuator_ctrlrange + 2*i;
ctrl[i] = mju_clip(d->ctrl[i], ctrlrange[0], ctrlrange[1]);
} else {
ctrl[i] = d->ctrl[i];
}
}
if (!mjDISABLED(mjDSBL_CLAMPCTRL)) {
clampVec(ctrl, m->actuator_ctrlrange, m->actuator_ctrllimited, nu, NULL);
}
// check controls, set all to 0 if any are bad
@@ -459,25 +463,47 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
}
// clamp actuator_force
for (int i=0; i < nu; i++) {
if (m->actuator_forcelimited[i]) {
mjtNum *forcerange = m->actuator_forcerange + 2*i;
force[i] = mju_clip(force[i], forcerange[0], forcerange[1]);
}
}
clampVec(force, m->actuator_forcerange, m->actuator_forcelimited, nu, NULL);
// qfrc_actuator = moment' * force
mju_mulMatTVec(d->qfrc_actuator, moment, force, nu, nv);
// clamp qfrc_actuator
// actuator-level gravity compensation
if (!mjDISABLED(mjDSBL_GRAVITY) && mju_norm3(m->opt.gravity)) {
int njnt = m->njnt;
for (int i=0; i < njnt; i++) {
if (m->jnt_actfrclimited[i]) {
mjtNum *forcerange = m->jnt_actfrcrange + 2*i;
mjtNum *qfrc = d->qfrc_actuator + m->jnt_dofadr[i];
qfrc[0] = mju_clip(qfrc[0], forcerange[0], forcerange[1]);
// skip if gravcomp added as passive force
if (!m->jnt_actgravcomp[i]) {
continue;
}
// get number of dofs for this joint
int dofnum;
switch (m->jnt_type[i]) {
case mjJNT_HINGE:
case mjJNT_SLIDE:
dofnum = 1;
break;
case mjJNT_BALL:
dofnum = 3;
break;
case mjJNT_FREE:
dofnum = 6;
break;
}
// add gravcomp force
int dofadr = m->jnt_dofadr[i];
for (int j=0; j < dofnum; j++) {
d->qfrc_actuator[dofadr+j] += d->qfrc_gravcomp[dofadr+j];
}
}
}
// clamp qfrc_actuator to joint-level actuator force limits
clampVec(d->qfrc_actuator, m->jnt_actfrcrange, m->jnt_actfrclimited, m->njnt, m->jnt_dofadr);
mj_freeStack(d);
TM_END(mjTIMER_ACTUATION);
@@ -489,13 +515,13 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
void mj_fwdAcceleration(const mjModel* m, mjData* d) {
int nv = m->nv;
// qforce = sum of all non-constraint forces
// qfrc_smooth = sum of all non-constraint forces
mju_sub(d->qfrc_smooth, d->qfrc_passive, d->qfrc_bias, nv); // qfrc_bias is negative
mju_addTo(d->qfrc_smooth, d->qfrc_applied, nv);
mju_addTo(d->qfrc_smooth, d->qfrc_actuator, nv);
mj_xfrcAccumulate(m, d, d->qfrc_smooth);
// qacc_smooth = M \ qfr_smooth
// qacc_smooth = M \ qfrc_smooth
mj_solveM(m, d, d->qacc_smooth, d->qfrc_smooth, 1);
}
@@ -706,9 +732,10 @@ void mj_fwdConstraint(const mjModel* m, mjData* d) {
// advance state and time given activation derivatives, acceleration, and optional velocity
static void mj_advance(const mjModel* m, mjData* d,
const mjtNum* act_dot, const mjtNum* qacc, const mjtNum* qvel) {
// advance activations and clamp
// advance activations
if (m->na && !mjDISABLED(mjDSBL_ACTUATION)) {
for (int i=0; i < m->nu; i++) {
int nu = m->nu;
for (int i=0; i < nu; i++) {
int actadr = m->actuator_actadr[i];
int actadr_end = actadr + m->actuator_actnum[i];
for (int j=actadr; j < actadr_end; j++) {
src/engine/engine_passive.c
+32 -1
View File
@@ -255,8 +255,39 @@ void mj_passive(const mjModel* m, mjData* d) {
// add passive forces into qfrc_passive
mju_add(d->qfrc_passive, d->qfrc_spring, d->qfrc_damper, nv);
if (has_gravcomp) mju_addTo(d->qfrc_passive, d->qfrc_gravcomp, nv);
if (has_fluid) mju_addTo(d->qfrc_passive, d->qfrc_fluid, nv);
if (has_gravcomp) {
int njnt = m->njnt;
for (int i=0; i < njnt; i++) {
// skip if gravcomp added via actuators
if (m->jnt_actgravcomp[i]) {
continue;
}
// get number of dofs for this joint
int dofnum;
switch (m->jnt_type[i]) {
case mjJNT_HINGE:
case mjJNT_SLIDE:
dofnum = 1;
break;
case mjJNT_BALL:
dofnum = 3;
break;
case mjJNT_FREE:
dofnum = 6;
break;
}
// add gravcomp force
int dofadr = m->jnt_dofadr[i];
for (int j=0; j < dofnum; j++) {
d->qfrc_passive[dofadr+j] += d->qfrc_gravcomp[dofadr+j];
}
}
}
// user callback: add custom passive forces
if (mjcb_passive) {
src/engine/engine_print.c
+9 -8
View File
@@ -361,14 +361,6 @@ void mj_printFormattedModel(const mjModel* m, const char* filename, const char*
}
if (m->nbody) fprintf(fp, "\n");
// BVHs
for (int i=0; i < m->nbvh; i++) {
fprintf(fp, "\nBVH %d:\n", i);
object_class = &m->nbvh;
MJMODEL_POINTERS
}
if (m->nbvh) fprintf(fp, "\n");
// joints
for (int i=0; i < m->njnt; i++) {
fprintf(fp, "\nJOINT %d:\n", i);
@@ -732,6 +724,15 @@ void mj_printFormattedModel(const mjModel* m, const char* filename, const char*
#undef X
// BVHs
fprintf(fp, "BVH:\n");
fprintf(fp, " %-8s%-8s%-8s%-10s%-s\n","id", "depth", "nodeid", "child[0]" ,"child[1]");
for (int i=0; i < m->nbvh; i++) {
fprintf(fp, " %-8d%-8d% -8d% -10d% -d\n",
i, m->bvh_depth[i], m->bvh_nodeid[i], m->bvh_child[2*i], m->bvh_child[2*i+1]);
}
fprintf(fp, "\n");
if (filename) {
fclose(fp);
}
src/engine/engine_resource.c
+14 -17
View File
@@ -34,6 +34,7 @@
#include <mujoco/mjplugin.h>
#include "engine/engine_plugin.h"
#include "engine/engine_util_errmem.h"
#include "engine/engine_util_misc.h"
// internal helper for mju_fileToMemory (closes fp automatically)
static void* _fileToMemory(FILE* fp, const char* filename, size_t* filesize);
@@ -113,10 +114,8 @@ mjResource* mju_openResource(const char* name, char* error, size_t error_sz) {
memcpy(&spec->mtime, &file_stat.st_mtime, sizeof(time_t));
} else {
memset(&spec->mtime, 0, sizeof(time_t));
resource->timestamp[0] = '\0';
}
strftime(resource->timestamp, 512, "%Y-%m-%d-%H:%M:%S",
localtime(&(spec->mtime)));
mju_encodeBase64(resource->timestamp, (uint8_t*) &spec->mtime, sizeof(time_t));
return resource;
}
@@ -199,19 +198,6 @@ void mju_getResourceDir(mjResource* resource, const char** dir, int* ndir) {
// modified callback for OS filesystem
static int mju_isModifiedFile(const char* name, const file_spec* spec) {
if (spec != NULL) {
struct stat file_stat;
if (stat(name, &file_stat) == 0) {
return difftime(spec->mtime, file_stat.st_mtime) < 0;
}
}
return 1; // modified (default)
}
// return 0 if the resource's timestamp matches the provided timestamp
// return > 0 if the the resource is younger than the given timestamp
// return < 0 if the resource is older than the given timestamp
@@ -224,7 +210,18 @@ int mju_isModifiedResource(const mjResource* resource, const char* timestamp) {
return 1; // default (modified)
}
return mju_isModifiedFile(resource->name, (file_spec*) resource->data);
// fallback to OS filesystem
if (mju_isValidBase64(timestamp) != sizeof(time_t)) {
return 1; // error (assume modified)
}
time_t time1, time2;
mju_decodeBase64((uint8_t*) &time1, timestamp);
time2 = ((file_spec*) resource->data)->mtime;
double diff = difftime(time2, time1);
if (diff < 0) return -1;
if (diff > 0) return 1;
return 0;
}
src/engine/engine_sensor.c
+1 -85
View File
@@ -32,78 +32,9 @@
#include "engine/engine_util_spatial.h"
//-------------------------------- utility ---------------------------------------------------------
// add sensor noise after each stage
static void add_noise(const mjModel* m, mjData* d, mjtStage stage) {
int adr, dim;
mjtNum rnd[4], noise, quat[4], res[4];
// process sensors matching stage and having positive noise
for (int i=0; i < m->nsensor; i++) {
if (m->sensor_needstage[i] == stage && m->sensor_noise[i] > 0) {
// get sensor info
adr = m->sensor_adr[i];
dim = m->sensor_dim[i];
noise = m->sensor_noise[i];
// real or positive: add noise directly, with clamp for positive
if (m->sensor_datatype[i] == mjDATATYPE_REAL ||
m->sensor_datatype[i] == mjDATATYPE_POSITIVE) {
for (int j=0; j < dim; j++) {
// get random numbers; use only the first one
rnd[0] = mju_standardNormal(rnd+1);
// positive
if (m->sensor_datatype[i] == mjDATATYPE_POSITIVE) {
// add noise only if positive, keep it positive
if (d->sensordata[adr+j] > 0) {
d->sensordata[adr+j] = mju_max(0, d->sensordata[adr+j]+rnd[0]*noise);
}
}
// real
else {
d->sensordata[adr+j] += rnd[0]*noise;
}
}
}
// axis or quat: rotate around random axis by random angle
else {
// get four random numbers
rnd[0] = mju_standardNormal(rnd+1);
rnd[2] = mju_standardNormal(rnd+3);
// scale angle, normalize axis, make quaternion
rnd[0] *= noise;
mju_normalize3(rnd+1);
mju_axisAngle2Quat(quat, rnd+1, rnd[0]);
// axis
if (m->sensor_datatype[i] == mjDATATYPE_AXIS) {
// apply quaternion rotation to axis, assign
mju_rotVecQuat(res, d->sensordata+adr, quat);
mju_copy3(d->sensordata+adr, res);
}
// quaternion
else if (m->sensor_datatype[i] == mjDATATYPE_QUATERNION) {
// apply quaternion rotation to quaternion, assign
mju_mulQuat(d->sensordata+adr, d->sensordata+adr, quat);
}
// unknown datatype
else {
mjERROR("unknown datatype in sensor %d", i);
}
}
}
}
}
// apply cutoff after each stage
static void apply_cutoff(const mjModel* m, mjData* d, mjtStage stage) {
// process sensors matching stage and having positive cutoff
@@ -442,11 +373,6 @@ void mj_sensorPos(const mjModel* m, mjData* d) {
mjcb_sensor(m, d, mjSTAGE_POS);
}
// add noise if enabled
if (mjENABLED(mjENBL_SENSORNOISE)) {
add_noise(m, d, mjSTAGE_POS);
}
// compute plugin sensor values
if (m->nplugin) {
const int nslot = mjp_pluginCount();
@@ -623,11 +549,6 @@ void mj_sensorVel(const mjModel* m, mjData* d) {
mjcb_sensor(m, d, mjSTAGE_VEL);
}
// add noise if enabled
if (mjENABLED(mjENBL_SENSORNOISE)) {
add_noise(m, d, mjSTAGE_VEL);
}
// trigger computation of plugins
if (m->nplugin) {
const int nslot = mjp_pluginCount();
@@ -839,11 +760,6 @@ void mj_sensorAcc(const mjModel* m, mjData* d) {
mjcb_sensor(m, d, mjSTAGE_ACC);
}
// add noise if enabled
if (mjENABLED(mjENBL_SENSORNOISE)) {
add_noise(m, d, mjSTAGE_ACC);
}
// trigger computation of plugins
if (m->nplugin) {
const int nslot = mjp_pluginCount();
src/engine/engine_support.c
+5 -6
View File
@@ -66,7 +66,6 @@ const char* mjENABLESTRING[mjNENABLE] = {
"Override",
"Energy",
"Fwdinv",
"Sensornoise",
"InvDiscrete",
"MultiCCD",
"Island"
@@ -815,8 +814,8 @@ void mj_angmomMat(const mjModel* m, mjData* d, mjtNum* mat, int body) {
// term1 = body angular momentum about self COM in world frame
mjtNum tmp1[9], tmp2[9];
mju_mulMatMat(tmp1, ximat, inertia, 3, 3, 3); // tmp1 = ximat * inertia
mju_mulMatMatT(tmp2, tmp1, ximat, 3, 3, 3); // tmp2 = ximat * inertia * ximat^T
mju_mulMatMat3(tmp1, ximat, inertia); // tmp1 = ximat * inertia
mju_mulMatMatT3(tmp2, tmp1, ximat); // tmp2 = ximat * inertia * ximat^T
mju_mulMatMat(term1, tmp2, jacr, 3, 3, nv); // term1 = ximat * inertia * ximat^T * jacr
// location of body COM w.r.t subtree COM
@@ -1050,11 +1049,11 @@ static int _getnumadr(const mjModel* m, mjtObj type, int** padr, int* mapadr) {
}
// get string hash, see http://www.cse.yorku.ca/~oz/hash.html
uint64_t mj_hashdjb2(const char* s, uint64_t n) {
uint64_t mj_hashString(const char* s, uint64_t n) {
uint64_t h = 5381;
int c;
while ((c = *s++)) {
h = ((h << 5) + h) + c;
h = ((h << 5) + h) ^ c;
}
return h % n;
}
@@ -1070,7 +1069,7 @@ int mj_name2id(const mjModel* m, int type, const char* name) {
// search
if (num) { // look up at hash address
uint64_t hash = mj_hashdjb2(name, num);
uint64_t hash = mj_hashString(name, num);
uint64_t i = hash;
do {
src/engine/engine_support.h
+1 -1
View File
@@ -114,7 +114,7 @@ MJAPI void mj_angmomMat(const mjModel* m, mjData* d, mjtNum* mat, int body);
//-------------------------- name functions --------------------------------------------------------
// get string hash, see http://www.cse.yorku.ca/~oz/hash.html
uint64_t mj_hashdjb2(const char* s, uint64_t n);
uint64_t mj_hashString(const char* s, uint64_t n);
// get id of object with the specified mjtObj type and name, returns -1 if id not found
MJAPI int mj_name2id(const mjModel* m, int type, const char* name);
src/engine/engine_util_blas.c
+45
View File
@@ -180,6 +180,51 @@ void mju_rotVecMatT(mjtNum res[3], const mjtNum vec[3], const mjtNum mat[9]) {
// multiply 3x3 matrices,
void mju_mulMatMat3(mjtNum res[9], const mjtNum a[9], const mjtNum b[9]) {
res[0] = a[0]*b[0] + a[1]*b[3] + a[2]*b[6];
res[1] = a[0]*b[1] + a[1]*b[4] + a[2]*b[7];
res[2] = a[0]*b[2] + a[1]*b[5] + a[2]*b[8];
res[3] = a[3]*b[0] + a[4]*b[3] + a[5]*b[6];
res[4] = a[3]*b[1] + a[4]*b[4] + a[5]*b[7];
res[5] = a[3]*b[2] + a[4]*b[5] + a[5]*b[8];
res[6] = a[6]*b[0] + a[7]*b[3] + a[8]*b[6];
res[7] = a[6]*b[1] + a[7]*b[4] + a[8]*b[7];
res[8] = a[6]*b[2] + a[7]*b[5] + a[8]*b[8];
}
// multiply 3x3 matrices, first argument transposed
void mju_mulMatTMat3(mjtNum res[9], const mjtNum a[9], const mjtNum b[9]) {
res[0] = a[0]*b[0] + a[3]*b[3] + a[6]*b[6];
res[1] = a[0]*b[1] + a[3]*b[4] + a[6]*b[7];
res[2] = a[0]*b[2] + a[3]*b[5] + a[6]*b[8];
res[3] = a[1]*b[0] + a[4]*b[3] + a[7]*b[6];
res[4] = a[1]*b[1] + a[4]*b[4] + a[7]*b[7];
res[5] = a[1]*b[2] + a[4]*b[5] + a[7]*b[8];
res[6] = a[2]*b[0] + a[5]*b[3] + a[8]*b[6];
res[7] = a[2]*b[1] + a[5]*b[4] + a[8]*b[7];
res[8] = a[2]*b[2] + a[5]*b[5] + a[8]*b[8];
}
// multiply 3x3 matrices, second argument transposed
void mju_mulMatMatT3(mjtNum res[9], const mjtNum a[9], const mjtNum b[9]) {
res[0] = a[0]*b[0] + a[1]*b[1] + a[2]*b[2];
res[1] = a[0]*b[3] + a[1]*b[4] + a[2]*b[5];
res[2] = a[0]*b[6] + a[1]*b[7] + a[2]*b[8];
res[3] = a[3]*b[0] + a[4]*b[1] + a[5]*b[2];
res[4] = a[3]*b[3] + a[4]*b[4] + a[5]*b[5];
res[5] = a[3]*b[6] + a[4]*b[7] + a[5]*b[8];
res[6] = a[6]*b[0] + a[7]*b[1] + a[8]*b[2];
res[7] = a[6]*b[3] + a[7]*b[4] + a[8]*b[5];
res[8] = a[6]*b[6] + a[7]*b[7] + a[8]*b[8];
}
//------------------------------ 4D vector and matrix-vector operations ----------------------------
// res = 0
src/engine/engine_util_blas.h
+8
View File
@@ -109,6 +109,14 @@ MJAPI void mju_rotVecMat(mjtNum res[3], const mjtNum vec[3], const mjtNum mat[9]
// multiply vector by transposed 3D rotation matrix
MJAPI void mju_rotVecMatT(mjtNum res[3], const mjtNum vec[3], const mjtNum mat[9]);
// multiply 3x3 matrices
MJAPI void mju_mulMatMat3(mjtNum res[9], const mjtNum mat1[9], const mjtNum mat2[9]);
// multiply 3x3 matrices, first argument transposed
MJAPI void mju_mulMatTMat3(mjtNum res[9], const mjtNum a[9], const mjtNum b[9]);
// multiply 3x3 matrices, second argument transposed
MJAPI void mju_mulMatMatT3(mjtNum res[9], const mjtNum mat1[9], const mjtNum mat2[9]);
//------------------------------ 4D/quaternion operations ------------------------------------------
src/engine/engine_util_misc.c
+1 -1
View File
@@ -639,7 +639,7 @@ static uint32_t _decode(char ch) {
// returns number of chars written in buf: 4 * [(ndata + 2) / 3] + 1
size_t mju_encodeBase64(char* buf, const uint8_t* data, size_t ndata) {
static const char *table =
"ABCDEFGHIJKLMNOPQRSTUBWXYZabcdefghijklmnopqrstuvwxyz0123456789+/";
"ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz0123456789+/";
int i = 0, j = 0;
src/engine/engine_util_solve.c
+2 -2
View File
@@ -721,8 +721,8 @@ int mju_eig3(mjtNum eigval[3], mjtNum eigvec[9], mjtNum quat[4], const mjtNum ma
for (iter=0; iter < 500; iter++) {
// make quaternion matrix eigvec, compute D = eigvec'*mat*eigvec
mju_quat2Mat(eigvec, quat);
mju_mulMatTMat(tmp, eigvec, mat, 3, 3, 3);
mju_mulMatMat(D, tmp, eigvec, 3, 3, 3);
mju_mulMatTMat3(tmp, eigvec, mat);
mju_mulMatMat3(D, tmp, eigvec);
// assign eigenvalues
eigval[0] = D[0];
src/engine/engine_vfs.c
+24 -1
View File
@@ -347,6 +347,29 @@ static void vfs_getdir_callback(mjResource* resource, const char** dir, int* ndi
}
// modified callback for the VFS resource provider
// return > 0 if modified and 0 if unmodified
static int vfs_modified_callback(const mjResource* resource, const char* timestamp) {
uint64_t filestamp;
if (mju_isValidBase64(timestamp) > sizeof(uint64_t)) {
return 2; // error (assume modified)
}
mju_decodeBase64((uint8_t*) &filestamp, timestamp);
if (!filestamp) return 3; // no hash (assume modified)
if (resource) {
const mjVFS* vfs = (const mjVFS*) resource->data;
int i = mj_findFileVFS(vfs, resource->name);
if (i < 0) return 4; // missing file (assume modified)
if (!vfs->filestamp[i]) return 5; // missing filestamp (assume modified)
if (vfs->filestamp[i] == filestamp) {
return 0; // unmodified
}
}
return 1; // modified
}
// open VFS resource
mjResource* mju_openVfsResource(const char* name, const mjVFS* vfs) {
@@ -362,7 +385,7 @@ mjResource* mju_openVfsResource(const char* name, const mjVFS* vfs) {
.read = &vfs_read_callback,
.close = &vfs_close_callback,
.getdir = &vfs_getdir_callback,
.modified = NULL
.modified = &vfs_modified_callback,
};
// create resource
src/user/CMakeLists.txt
+2
View File
@@ -15,6 +15,8 @@
set(MUJOCO_USER_SRCS
user_api.cc
user_api.h
user_cache.cc
user_cache.h
user_composite.cc
user_composite.h
user_flexcomp.cc
src/user/user_api.cc
+205 -137
View File
@@ -13,80 +13,121 @@
// limitations under the License.
#include "user/user_api.h"
#include <cstddef>
#include <functional>
#include <map>
#include <new>
#include <string>
#include <string_view>
#include <utility>
#include <vector>
#include <mujoco/mujoco.h>
#include "user/user_model.h"
#include "user/user_objects.h"
#include "user/user_cache.h"
#include "xml/xml_util.h"
// prepend prefix
template <typename T>
static T& operator+(std::string_view prefix, T& base) {
base.prefix = std::string(prefix);
return base;
}
// append suffix
template <typename T>
static T& operator+(T& base, std::string_view suffix) {
base.suffix = std::string(suffix);
return base;
}
// create model
mjSpec* mjm_createSpec() {
mjSpec* mjs_createSpec() {
mjCModel* modelC = new mjCModel;
return &modelC->spec;
}
// copy model
mjSpec* mjs_copySpec(const mjSpec* s) {
mjCModel* modelC = new mjCModel(*static_cast<mjCModel*>(s->element));
return &modelC->spec;
}
// copy back model
void mjm_copyBack(mjSpec* s, const mjModel* m) {
mjCModel* modelC = reinterpret_cast<mjCModel*>(s->element);
void mjs_copyBack(mjSpec* s, const mjModel* m) {
mjCModel* modelC = static_cast<mjCModel*>(s->element);
modelC->CopyBack(m);
}
// compile model
mjModel* mjm_compile(mjSpec* s, const mjVFS* vfs) {
mjCModel* modelC = reinterpret_cast<mjCModel*>(s->element);
mjModel* mjs_compile(mjSpec* s, const mjVFS* vfs) {
mjCModel* modelC = static_cast<mjCModel*>(s->element);
return modelC->Compile(vfs);
}
// attach body to a frame of the parent
int mjs_attachBody(mjsFrame* parent, const mjsBody* child,
const char* prefix, const char* suffix) {
mjCFrame* frame_parent = static_cast<mjCFrame*>(parent->element);
mjCBody* child_body = static_cast<mjCBody*>(child->element);
*frame_parent += std::string(prefix) + *child_body + std::string(suffix);
return 0;
}
// get error message from model
const char* mjm_getError(mjSpec* s) {
mjCModel* modelC = reinterpret_cast<mjCModel*>(s->element);
const char* mjs_getError(mjSpec* s) {
mjCModel* modelC = static_cast<mjCModel*>(s->element);
return modelC->GetError().message;
}
// check if model has warnings
int mjm_isWarning(mjSpec* s) {
mjCModel* modelC = reinterpret_cast<mjCModel*>(s->element);
int mjs_isWarning(mjSpec* s) {
mjCModel* modelC = static_cast<mjCModel*>(s->element);
return modelC->GetError().warning;
}
// delete model
void mjm_deleteSpec(mjSpec* s) {
mjCModel* model = reinterpret_cast<mjCModel*>(s->element);
void mjs_deleteSpec(mjSpec* s) {
mjCModel* model = static_cast<mjCModel*>(s->element);
delete model;
}
// add child body to body, return child spec
mjmBody* mjm_addBody(mjmBody* bodyspec, mjmDefault* defspec) {
mjCDef* def = defspec ? reinterpret_cast<mjCDef*>(defspec->element) : 0;
mjCBody* body = reinterpret_cast<mjCBody*>(bodyspec->element)->AddBody(def);
mjsBody* mjs_addBody(mjsBody* bodyspec, mjsDefault* defspec) {
mjCDef* def = defspec ? static_cast<mjCDef*>(defspec->element) : 0;
mjCBody* body = static_cast<mjCBody*>(bodyspec->element)->AddBody(def);
return &body->spec;
}
// add site to body, return site spec
mjmSite* mjm_addSite(mjmBody* bodyspec, mjmDefault* defspec) {
mjCDef* def = defspec ? reinterpret_cast<mjCDef*>(defspec->element) : 0;
mjCBody* body = reinterpret_cast<mjCBody*>(bodyspec->element);
mjsSite* mjs_addSite(mjsBody* bodyspec, mjsDefault* defspec) {
mjCDef* def = defspec ? static_cast<mjCDef*>(defspec->element) : 0;
mjCBody* body = static_cast<mjCBody*>(bodyspec->element);
mjCSite* site = body->AddSite(def);
return &site->spec;
}
@@ -94,9 +135,9 @@ mjmSite* mjm_addSite(mjmBody* bodyspec, mjmDefault* defspec) {
// add joint to body
mjmJoint* mjm_addJoint(mjmBody* bodyspec, mjmDefault* defspec) {
mjCDef* def = defspec ? reinterpret_cast<mjCDef*>(defspec->element) : 0;
mjCBody* body = reinterpret_cast<mjCBody*>(bodyspec->element);
mjsJoint* mjs_addJoint(mjsBody* bodyspec, mjsDefault* defspec) {
mjCDef* def = defspec ? static_cast<mjCDef*>(defspec->element) : 0;
mjCBody* body = static_cast<mjCBody*>(bodyspec->element);
mjCJoint* joint = body->AddJoint(def);
return &joint->spec;
}
@@ -104,8 +145,8 @@ mjmJoint* mjm_addJoint(mjmBody* bodyspec, mjmDefault* defspec) {
// add free joint to body
mjmJoint* mjm_addFreeJoint(mjmBody* bodyspec) {
mjCBody* body = reinterpret_cast<mjCBody*>(bodyspec->element);
mjsJoint* mjs_addFreeJoint(mjsBody* bodyspec) {
mjCBody* body = static_cast<mjCBody*>(bodyspec->element);
mjCJoint* joint = body->AddFreeJoint();
return &joint->spec;
}
@@ -113,9 +154,9 @@ mjmJoint* mjm_addFreeJoint(mjmBody* bodyspec) {
// add geom to body
mjmGeom* mjm_addGeom(mjmBody* bodyspec, mjmDefault* defspec) {
mjCDef* def = defspec ? reinterpret_cast<mjCDef*>(defspec->element) : 0;
mjCBody* body = reinterpret_cast<mjCBody*>(bodyspec->element);
mjsGeom* mjs_addGeom(mjsBody* bodyspec, mjsDefault* defspec) {
mjCDef* def = defspec ? static_cast<mjCDef*>(defspec->element) : 0;
mjCBody* body = static_cast<mjCBody*>(bodyspec->element);
mjCGeom* geom = body->AddGeom(def);
return &geom->spec;
}
@@ -123,9 +164,9 @@ mjmGeom* mjm_addGeom(mjmBody* bodyspec, mjmDefault* defspec) {
// add camera to body
mjmCamera* mjm_addCamera(mjmBody* bodyspec, mjmDefault* defspec) {
mjCDef* def = defspec ? reinterpret_cast<mjCDef*>(defspec->element) : 0;
mjCBody* body = reinterpret_cast<mjCBody*>(bodyspec->element);
mjsCamera* mjs_addCamera(mjsBody* bodyspec, mjsDefault* defspec) {
mjCDef* def = defspec ? static_cast<mjCDef*>(defspec->element) : 0;
mjCBody* body = static_cast<mjCBody*>(bodyspec->element);
mjCCamera* camera = body->AddCamera(def);
return &camera->spec;
}
@@ -133,9 +174,9 @@ mjmCamera* mjm_addCamera(mjmBody* bodyspec, mjmDefault* defspec) {
// add light to body
mjmLight* mjm_addLight(mjmBody* bodyspec, mjmDefault* defspec) {
mjCDef* def = defspec ? reinterpret_cast<mjCDef*>(defspec->element) : 0;
mjCBody* body = reinterpret_cast<mjCBody*>(bodyspec->element);
mjsLight* mjs_addLight(mjsBody* bodyspec, mjsDefault* defspec) {
mjCDef* def = defspec ? static_cast<mjCDef*>(defspec->element) : 0;
mjCBody* body = static_cast<mjCBody*>(bodyspec->element);
mjCLight* light = body->AddLight(def);
return &light->spec;
}
@@ -143,8 +184,8 @@ mjmLight* mjm_addLight(mjmBody* bodyspec, mjmDefault* defspec) {
// add flex to model
mjmFlex* mjm_addFlex(mjSpec* s) {
mjCModel* modelC = reinterpret_cast<mjCModel*>(s->element);
mjsFlex* mjs_addFlex(mjSpec* s) {
mjCModel* modelC = static_cast<mjCModel*>(s->element);
mjCFlex* flex = modelC->AddFlex();
return &flex->spec;
}
@@ -152,22 +193,23 @@ mjmFlex* mjm_addFlex(mjSpec* s) {
// add frame to body
mjmFrame* mjm_addFrame(mjmBody* bodyspec, mjmFrame* parentframe) {
mjsFrame* mjs_addFrame(mjsBody* bodyspec, mjsFrame* parentframe) {
mjCFrame* parentframeC = 0;
if (parentframe) {
parentframeC = reinterpret_cast<mjCFrame*>(parentframe->element);
parentframeC = static_cast<mjCFrame*>(parentframe->element);
}
mjCBody* body = reinterpret_cast<mjCBody*>(bodyspec->element);
mjCBody* body = static_cast<mjCBody*>(bodyspec->element);
mjCFrame* frameC = body->AddFrame(parentframeC);
frameC->SetParent(body);
return &frameC->spec;
}
// add mesh to model
mjmMesh* mjm_addMesh(mjSpec* s, mjmDefault* defspec) {
mjCDef* def = defspec ? reinterpret_cast<mjCDef*>(defspec->element) : 0;
mjCModel* modelC = reinterpret_cast<mjCModel*>(s->element);
mjsMesh* mjs_addMesh(mjSpec* s, mjsDefault* defspec) {
mjCDef* def = defspec ? static_cast<mjCDef*>(defspec->element) : 0;
mjCModel* modelC = static_cast<mjCModel*>(s->element);
mjCMesh* mesh = modelC->AddMesh(def);
return &mesh->spec;
}
@@ -175,8 +217,8 @@ mjmMesh* mjm_addMesh(mjSpec* s, mjmDefault* defspec) {
// add height field to model
mjmHField* mjm_addHField(mjSpec* s) {
mjCModel* modelC = reinterpret_cast<mjCModel*>(s->element);
mjsHField* mjs_addHField(mjSpec* s) {
mjCModel* modelC = static_cast<mjCModel*>(s->element);
mjCHField* heightField = modelC->AddHField();
return &heightField->spec;
}
@@ -184,8 +226,8 @@ mjmHField* mjm_addHField(mjSpec* s) {
// add skin to model
mjmSkin* mjm_addSkin(mjSpec* s) {
mjCModel* modelC = reinterpret_cast<mjCModel*>(s->element);
mjsSkin* mjs_addSkin(mjSpec* s) {
mjCModel* modelC = static_cast<mjCModel*>(s->element);
mjCSkin* skin = modelC->AddSkin();
return &skin->spec;
}
@@ -193,8 +235,8 @@ mjmSkin* mjm_addSkin(mjSpec* s) {
// add texture to model
mjmTexture* mjm_addTexture(mjSpec* s) {
mjCModel* modelC = reinterpret_cast<mjCModel*>(s->element);
mjsTexture* mjs_addTexture(mjSpec* s) {
mjCModel* modelC = static_cast<mjCModel*>(s->element);
mjCTexture* texture = modelC->AddTexture();
return &texture->spec;
}
@@ -202,9 +244,9 @@ mjmTexture* mjm_addTexture(mjSpec* s) {
// add material to model
mjmMaterial* mjm_addMaterial(mjSpec* s, mjmDefault* defspec) {
mjCModel* modelC = reinterpret_cast<mjCModel*>(s->element);
mjCDef* def = defspec ? reinterpret_cast<mjCDef*>(defspec->element) : 0;
mjsMaterial* mjs_addMaterial(mjSpec* s, mjsDefault* defspec) {
mjCModel* modelC = static_cast<mjCModel*>(s->element);
mjCDef* def = defspec ? static_cast<mjCDef*>(defspec->element) : 0;
mjCMaterial* material = modelC->AddMaterial(def);
return &material->spec;
}
@@ -212,9 +254,9 @@ mjmMaterial* mjm_addMaterial(mjSpec* s, mjmDefault* defspec) {
// add pair to model
mjmPair* mjm_addPair(mjSpec* s, mjmDefault* defspec) {
mjCModel* modelC = reinterpret_cast<mjCModel*>(s->element);
mjCDef* def = defspec ? reinterpret_cast<mjCDef*>(defspec->element) : 0;
mjsPair* mjs_addPair(mjSpec* s, mjsDefault* defspec) {
mjCModel* modelC = static_cast<mjCModel*>(s->element);
mjCDef* def = defspec ? static_cast<mjCDef*>(defspec->element) : 0;
mjCPair* pair = modelC->AddPair(def);
return &pair->spec;
}
@@ -222,8 +264,8 @@ mjmPair* mjm_addPair(mjSpec* s, mjmDefault* defspec) {
// add pair exclusion to model
mjmExclude* mjm_addExclude(mjSpec* s) {
mjCModel* modelC = reinterpret_cast<mjCModel*>(s->element);
mjsExclude* mjs_addExclude(mjSpec* s) {
mjCModel* modelC = static_cast<mjCModel*>(s->element);
mjCBodyPair* bodypair = modelC->AddExclude();
return &bodypair->spec;
}
@@ -231,9 +273,9 @@ mjmExclude* mjm_addExclude(mjSpec* s) {
// add equality to model
mjmEquality* mjm_addEquality(mjSpec* s, mjmDefault* defspec) {
mjCModel* modelC = reinterpret_cast<mjCModel*>(s->element);
mjCDef* def = defspec ? reinterpret_cast<mjCDef*>(defspec->element) : 0;
mjsEquality* mjs_addEquality(mjSpec* s, mjsDefault* defspec) {
mjCModel* modelC = static_cast<mjCModel*>(s->element);
mjCDef* def = defspec ? static_cast<mjCDef*>(defspec->element) : 0;
mjCEquality* equality = modelC->AddEquality(def);
return &equality->spec;
}
@@ -241,9 +283,9 @@ mjmEquality* mjm_addEquality(mjSpec* s, mjmDefault* defspec) {
// add tendon to model
mjmTendon* mjm_addTendon(mjSpec* s, mjmDefault* defspec) {
mjCModel* modelC = reinterpret_cast<mjCModel*>(s->element);
mjCDef* def = defspec ? reinterpret_cast<mjCDef*>(defspec->element) : 0;
mjsTendon* mjs_addTendon(mjSpec* s, mjsDefault* defspec) {
mjCModel* modelC = static_cast<mjCModel*>(s->element);
mjCDef* def = defspec ? static_cast<mjCDef*>(defspec->element) : 0;
mjCTendon* tendon = modelC->AddTendon(def);
return &tendon->spec;
}
@@ -251,8 +293,8 @@ mjmTendon* mjm_addTendon(mjSpec* s, mjmDefault* defspec) {
// wrap site using tendon
MJAPI mjmWrap* mjm_wrapSite(mjmTendon* tendonspec, const char* name) {
mjCTendon* tendon = reinterpret_cast<mjCTendon*>(tendonspec->element);
MJAPI mjsWrap* mjs_wrapSite(mjsTendon* tendonspec, const char* name) {
mjCTendon* tendon = static_cast<mjCTendon*>(tendonspec->element);
tendon->WrapSite(name);
return &tendon->path.back()->spec;
}
@@ -260,8 +302,8 @@ MJAPI mjmWrap* mjm_wrapSite(mjmTendon* tendonspec, const char* name) {
// wrap geom using tendon
mjmWrap* mjm_wrapGeom(mjmTendon* tendonspec, const char* name, const char* sidesite) {
mjCTendon* tendon = reinterpret_cast<mjCTendon*>(tendonspec->element);
mjsWrap* mjs_wrapGeom(mjsTendon* tendonspec, const char* name, const char* sidesite) {
mjCTendon* tendon = static_cast<mjCTendon*>(tendonspec->element);
tendon->WrapGeom(name, sidesite);
return &tendon->path.back()->spec;
}
@@ -269,8 +311,8 @@ mjmWrap* mjm_wrapGeom(mjmTendon* tendonspec, const char* name, const char* sides
// wrap joint using tendon
mjmWrap* mjm_wrapJoint(mjmTendon* tendonspec, const char* name, double coef) {
mjCTendon* tendon = reinterpret_cast<mjCTendon*>(tendonspec->element);
mjsWrap* mjs_wrapJoint(mjsTendon* tendonspec, const char* name, double coef) {
mjCTendon* tendon = static_cast<mjCTendon*>(tendonspec->element);
tendon->WrapJoint(name, coef);
return &tendon->path.back()->spec;
}
@@ -278,8 +320,8 @@ mjmWrap* mjm_wrapJoint(mjmTendon* tendonspec, const char* name, double coef) {
// wrap pulley using tendon
mjmWrap* mjm_wrapPulley(mjmTendon* tendonspec, double divisor) {
mjCTendon* tendon = reinterpret_cast<mjCTendon*>(tendonspec->element);
mjsWrap* mjs_wrapPulley(mjsTendon* tendonspec, double divisor) {
mjCTendon* tendon = static_cast<mjCTendon*>(tendonspec->element);
tendon->WrapPulley(divisor);
return &tendon->path.back()->spec;
}
@@ -287,9 +329,9 @@ mjmWrap* mjm_wrapPulley(mjmTendon* tendonspec, double divisor) {
// add actuator to model
mjmActuator* mjm_addActuator(mjSpec* s, mjmDefault* defspec) {
mjCModel* modelC = reinterpret_cast<mjCModel*>(s->element);
mjCDef* def = defspec ? reinterpret_cast<mjCDef*>(defspec->element) : 0;
mjsActuator* mjs_addActuator(mjSpec* s, mjsDefault* defspec) {
mjCModel* modelC = static_cast<mjCModel*>(s->element);
mjCDef* def = defspec ? static_cast<mjCDef*>(defspec->element) : 0;
mjCActuator* actuator = modelC->AddActuator(def);
return &actuator->spec;
}
@@ -297,8 +339,8 @@ mjmActuator* mjm_addActuator(mjSpec* s, mjmDefault* defspec) {
// add sensor to model
mjmSensor* mjm_addSensor(mjSpec* s) {
mjCModel* modelC = reinterpret_cast<mjCModel*>(s->element);
mjsSensor* mjs_addSensor(mjSpec* s) {
mjCModel* modelC = static_cast<mjCModel*>(s->element);
mjCSensor* sensor = modelC->AddSensor();
return &sensor->spec;
}
@@ -306,8 +348,8 @@ mjmSensor* mjm_addSensor(mjSpec* s) {
// add numeric to model
mjmNumeric* mjm_addNumeric(mjSpec* s) {
mjCModel* modelC = reinterpret_cast<mjCModel*>(s->element);
mjsNumeric* mjs_addNumeric(mjSpec* s) {
mjCModel* modelC = static_cast<mjCModel*>(s->element);
mjCNumeric* numeric = modelC->AddNumeric();
return &numeric->spec;
}
@@ -315,8 +357,8 @@ mjmNumeric* mjm_addNumeric(mjSpec* s) {
// add text to model
mjmText* mjm_addText(mjSpec* s) {
mjCModel* modelC = reinterpret_cast<mjCModel*>(s->element);
mjsText* mjs_addText(mjSpec* s) {
mjCModel* modelC = static_cast<mjCModel*>(s->element);
mjCText* text = modelC->AddText();
return &text->spec;
}
@@ -324,8 +366,8 @@ mjmText* mjm_addText(mjSpec* s) {
// add tuple to model
mjmTuple* mjm_addTuple(mjSpec* s) {
mjCModel* modelC = reinterpret_cast<mjCModel*>(s->element);
mjsTuple* mjs_addTuple(mjSpec* s) {
mjCModel* modelC = static_cast<mjCModel*>(s->element);
mjCTuple* tuple = modelC->AddTuple();
return &tuple->spec;
}
@@ -333,8 +375,8 @@ mjmTuple* mjm_addTuple(mjSpec* s) {
// add keyframe to model
mjmKey* mjm_addKey(mjSpec* s) {
mjCModel* modelC = reinterpret_cast<mjCModel*>(s->element);
mjsKey* mjs_addKey(mjSpec* s) {
mjCModel* modelC = static_cast<mjCModel*>(s->element);
mjCKey* key = modelC->AddKey();
return &key->spec;
}
@@ -342,18 +384,18 @@ mjmKey* mjm_addKey(mjSpec* s) {
// add plugin to model
mjmPlugin* mjm_addPlugin(mjSpec* s) {
mjCModel* modelC = reinterpret_cast<mjCModel*>(s->element);
mjsPlugin* mjs_addPlugin(mjSpec* s) {
mjCModel* modelC = static_cast<mjCModel*>(s->element);
mjCPlugin* plugin = modelC->AddPlugin();
plugin->spec.instance = (mjElement)plugin;
plugin->spec.instance = static_cast<mjElement*>(plugin);
return &plugin->spec;
}
// add default to model
mjmDefault* mjm_addDefault(mjSpec* s, const char* classname, int parentid, int* id) {
mjCModel* modelC = reinterpret_cast<mjCModel*>(s->element);
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);
if (def) {
@@ -366,22 +408,22 @@ mjmDefault* mjm_addDefault(mjSpec* s, const char* classname, int parentid, int*
// get objects
mjSpec* mjm_getSpec(mjmBody* body) {
return &(reinterpret_cast<mjCBody*>(body->element)->model->spec);
mjSpec* mjs_getSpec(mjsBody* body) {
return &(static_cast<mjCBody*>(body->element)->model->spec);
}
// get default
mjmDefault* mjm_getDefault(mjElement element) {
return &(reinterpret_cast<mjCBase*>(element)->def->spec);
mjsDefault* mjs_getDefault(mjElement* element) {
return &(static_cast<mjCBase*>(element)->def->spec);
}
// Find default with given name in model.
mjmDefault* mjm_findDefault(mjSpec* s, const char* classname) {
mjCModel* modelC = reinterpret_cast<mjCModel*>(s->element);
mjsDefault* mjs_findDefault(mjSpec* s, const char* classname) {
mjCModel* modelC = static_cast<mjCModel*>(s->element);
mjCDef* cdef = modelC->FindDef(classname);
if (!cdef) {
return nullptr;
@@ -392,8 +434,8 @@ mjmDefault* mjm_findDefault(mjSpec* s, const char* classname) {
// get default[0] from model
mjmDefault* mjm_getSpecDefault(mjSpec* s) {
mjCModel* modelC = reinterpret_cast<mjCModel*>(s->element);
mjsDefault* mjs_getSpecDefault(mjSpec* s) {
mjCModel* modelC = static_cast<mjCModel*>(s->element);
mjCDef* def = modelC->defaults[0];
if (!def) {
return nullptr;
@@ -404,70 +446,83 @@ mjmDefault* mjm_getSpecDefault(mjSpec* s) {
// find body in model by name
mjmBody* mjm_findBody(mjSpec* s, const char* name) {
mjCModel* model = reinterpret_cast<mjCModel*>(s->element);
mjCBase* body = model->FindObject(mjOBJ_BODY, std::string(name));
if (!body) {
return 0;
mjsBody* mjs_findBody(mjSpec* s, const char* name) {
mjCModel* model = static_cast<mjCModel*>(s->element);
mjCBase* body = 0;
if (model->IsCompiled()) {
body = model->FindObject(mjOBJ_BODY, std::string(name)); // fast lookup
} else {
body = model->FindBody(model->GetWorld(), std::string(name)); // recursive search
}
return &(static_cast<mjCBody*>(body)->spec);
return body ? &(static_cast<mjCBody*>(body)->spec) : nullptr;
}
// find child of a body by name
mjmBody* mjm_findChild(mjmBody* bodyspec, const char* name) {
mjCBody* body = reinterpret_cast<mjCBody*>(bodyspec->element);
mjsBody* mjs_findChild(mjsBody* bodyspec, const char* name) {
mjCBody* body = static_cast<mjCBody*>(bodyspec->element);
mjCBase* child = body->FindObject(mjOBJ_BODY, std::string(name));
if (!child) {
return 0;
}
return &(static_cast<mjCBody*>(child)->spec);
return child ? &(static_cast<mjCBody*>(child)->spec) : nullptr;
}
// find mesh by name
mjmMesh* mjm_findMesh(mjSpec* s, const char* name) {
mjCModel* model = reinterpret_cast<mjCModel*>(s->element);
mjsMesh* mjs_findMesh(mjSpec* s, const char* name) {
mjCModel* model = static_cast<mjCModel*>(s->element);
mjCMesh* mesh = (mjCMesh*)model->FindObject(mjOBJ_MESH, std::string(name));
if (!mesh) {
return nullptr;
}
return &(static_cast<mjCMesh*>(mesh)->spec);
return mesh ? &(static_cast<mjCMesh*>(mesh)->spec) : nullptr;
}
// find frame by name
mjsFrame* mjs_findFrame(mjSpec* s, const char* name) {
mjCModel* model = static_cast<mjCModel*>(s->element);
mjCFrame* frame = (mjCFrame*)model->FindFrame(model->GetWorld(), std::string(name));
return frame ? &(static_cast<mjCFrame*>(frame)->spec) : nullptr;
}
// set frame
void mjm_setFrame(mjElement dest, mjmFrame* frame) {
void mjs_setFrame(mjElement* dest, mjsFrame* frame) {
if (!frame) {
return;
}
mjCFrame* frameC = reinterpret_cast<mjCFrame*>(frame->element);
mjCBase* baseC = reinterpret_cast<mjCBase*>(dest);
mjCFrame* frameC = static_cast<mjCFrame*>(frame->element);
mjCBase* baseC = static_cast<mjCBase*>(dest);
baseC->SetFrame(frameC);
}
// Resolve alternative orientations.
const char* mjs_resolveOrientation(double quat[4], mjtByte degree, const char* sequence,
const mjsOrientation* orientation) {
return ResolveOrientation(quat, degree, sequence, *orientation);
}
// get id
int mjm_getId(mjElement element) {
return reinterpret_cast<mjCBase*>(element)->id;
int mjs_getId(mjElement* element) {
return static_cast<mjCBase*>(element)->id;
}
// set default
void mjm_setDefault(mjElement element, mjmDefault* defspec) {
mjCBase* baseC = reinterpret_cast<mjCBase*>(element);
baseC->def = reinterpret_cast<mjCDef*>(defspec->element);
void mjs_setDefault(mjElement* element, mjsDefault* defspec) {
mjCBase* baseC = static_cast<mjCBase*>(element);
baseC->def = static_cast<mjCDef*>(defspec->element);
}
// set string
void mjm_setString(mjString dest, const char* text) {
void mjs_setString(mjString dest, const char* text) {
std::string* str = reinterpret_cast<std::string*>(dest);
*str = std::string(text);
}
@@ -475,10 +530,10 @@ void mjm_setString(mjString dest, const char* text) {
// Set specific entry in destination string vector.
mjtByte mjm_setInStringVec(mjStringVec dest, int i, const char* text) {
mjtByte mjs_setInStringVec(mjStringVec dest, int i, const char* text) {
std::vector<std::string>* v = reinterpret_cast<std::vector<std::string>*>(dest);
if (v->size() <= i) {
mju_error("Requested index in mjm_setInStringVec is out of bounds");
mju_error("Requested index in mjs_setInStringVec is out of bounds");
return 0;
}
v->at(i) = std::string(text);
@@ -488,7 +543,7 @@ mjtByte mjm_setInStringVec(mjStringVec dest, int i, const char* text) {
// split text and copy into string array
void mjm_setStringVec(mjStringVec dest, const char* text) {
void mjs_setStringVec(mjStringVec dest, const char* text) {
std::vector<std::string>* v = reinterpret_cast<std::vector<std::string>*>(dest);
*v = mjXUtil::String2Vector<std::string>(text);
}
@@ -496,14 +551,14 @@ void mjm_setStringVec(mjStringVec dest, const char* text) {
// add text entry to destination string vector
void mjm_appendString(mjStringVec dest, const char* text) {
void mjs_appendString(mjStringVec dest, const char* text) {
std::vector<std::string>* v = reinterpret_cast<std::vector<std::string>*>(dest);
v->push_back(std::string(text));
}
// copy int array to vector
void mjm_setInt(mjIntVec dest, const int* array, int size) {
void mjs_setInt(mjIntVec dest, const int* array, int size) {
std::vector<int>* v = reinterpret_cast<std::vector<int>*>(dest);
v->assign(size, 0.0);
for (int i = 0; i < size; ++i) {
@@ -514,7 +569,7 @@ void mjm_setInt(mjIntVec dest, const int* array, int size) {
// append int array to vector of arrays
void mjm_appendIntVec(mjIntVecVec dest, const int* array, int size) {
void mjs_appendIntVec(mjIntVecVec dest, const int* array, int size) {
std::vector<std::vector<int>>* v = reinterpret_cast<std::vector<std::vector<int>>*>(dest);
v->push_back(std::vector<int>(array, array + size));
}
@@ -522,7 +577,7 @@ void mjm_appendIntVec(mjIntVecVec dest, const int* array, int size) {
// copy float array to vector
void mjm_setFloat(mjFloatVec dest, const float* array, int size) {
void mjs_setFloat(mjFloatVec dest, const float* array, int size) {
std::vector<float>* v = reinterpret_cast<std::vector<float>*>(dest);
v->assign(size, 0.0);
for (int i = 0; i < size; ++i) {
@@ -534,7 +589,7 @@ void mjm_setFloat(mjFloatVec dest, const float* array, int size) {
// append float array to vector of arrays
void mjm_appendFloatVec(mjFloatVecVec dest, const float* array, int size) {
void mjs_appendFloatVec(mjFloatVecVec dest, const float* array, int size) {
std::vector<std::vector<float>>* v = reinterpret_cast<std::vector<std::vector<float>>*>(dest);
v->push_back(std::vector<float>(array, array + size));
}
@@ -542,7 +597,7 @@ void mjm_appendFloatVec(mjFloatVecVec dest, const float* array, int size) {
// copy double array to vector
void mjm_setDouble(mjDoubleVec dest, const double* array, int size) {
void mjs_setDouble(mjDoubleVec dest, const double* array, int size) {
std::vector<double>* v = reinterpret_cast<std::vector<double>*>(dest);
v->assign(size, 0.0);
for (int i = 0; i < size; ++i) {
@@ -553,7 +608,7 @@ void mjm_setDouble(mjDoubleVec dest, const double* array, int size) {
// get string
const char* mjm_getString(const mjString source) {
const char* mjs_getString(const mjString source) {
std::string* str = reinterpret_cast<std::string*>(source);
if (!str) {
return nullptr;
@@ -564,7 +619,7 @@ const char* mjm_getString(const mjString source) {
// get double array
const double* mjm_getDouble(const mjDoubleVec source, int* size) {
const double* mjs_getDouble(const mjDoubleVec source, int* size) {
std::vector<double>* v = reinterpret_cast<std::vector<double>*>(source);
if (size) {
*size = v->size();
@@ -575,8 +630,8 @@ const double* mjm_getDouble(const mjDoubleVec source, int* size) {
// set plugin attributes
void mjm_setPluginAttributes(mjmPlugin* plugin, void* attributes) {
mjCPlugin* pluginC = reinterpret_cast<mjCPlugin*>(plugin->instance);
void mjs_setPluginAttributes(mjsPlugin* plugin, void* attributes) {
mjCPlugin* pluginC = static_cast<mjCPlugin*>(plugin->instance);
std::map<std::string, std::string, std::less<>>* config_attribs =
reinterpret_cast<std::map<std::string, std::string, std::less<>>*>(attributes);
pluginC->config_attribs = std::move(*config_attribs);
@@ -585,8 +640,8 @@ void mjm_setPluginAttributes(mjmPlugin* plugin, void* attributes) {
// Set active plugins.
void mjm_setActivePlugins(mjSpec* s, void* activeplugins) {
mjCModel* modelC = reinterpret_cast<mjCModel*>(s->element);
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);
@@ -595,9 +650,22 @@ void mjm_setActivePlugins(mjSpec* s, void* activeplugins) {
// compute full inertia
const char* mjm_setFullInertia(mjmBody* bodyspec, double quat[4], double inertia[3]) {
mjCBody* body = reinterpret_cast<mjCBody*>(bodyspec->element);
const char* mjs_setFullInertia(mjsBody* bodyspec, double quat[4], double inertia[3]) {
mjCBody* body = static_cast<mjCBody*>(bodyspec->element);
return body->FullInertia(quat, inertia);
}
// -------------------------- GLOBAL ASSET CACHE -------------------------------
void mj_setCacheSize(mjCache cache, std::size_t size) {
mjCCache* ccache = reinterpret_cast<mjCCache*>(cache);
if (ccache) {
ccache->SetMaxSize(size);
}
}
mjCache mj_globalCache() {
return NULL; // currently disabled
}
src/user/user_api.h
+222 -190
View File
@@ -29,7 +29,6 @@ extern "C" {
//---------------------------------- handles to internal objects -----------------------------------
typedef struct _mjElement* mjElement;
typedef struct _mjString* mjString;
typedef struct _mjStringVec* mjStringVec;
typedef struct _mjIntVec* mjIntVec;
@@ -77,10 +76,15 @@ typedef enum _mjtInertiaFromGeom {
} mjtInertiaFromGeom;
//---------------------------------- attribute structs (mjm) ---------------------------------------
//---------------------------------- attribute structs (mjs) ---------------------------------------
typedef struct _mjElement { // element type, do not modify
mjtObj elemtype; // element type
} mjElement;
typedef struct _mjSpec { // model specification
mjElement element; // internal, do not modify
mjElement* element; // object type
mjStatistic stat; // statistics override (if defined)
// compiler settings
@@ -135,40 +139,40 @@ typedef struct _mjSpec { // model specification
} mjSpec;
typedef struct _mjmOrientation { // alternative orientation specifiers
typedef struct _mjsOrientation { // alternative orientation specifiers
double axisangle[4]; // rotation axis and angle
double xyaxes[6]; // x and y axes
double zaxis[3]; // z axis (use minimal rotation)
double euler[3]; // euler angles
} mjmOrientation;
} mjsOrientation;
typedef struct _mjmPlugin { // plugin specification
mjElement instance; // internal, do not modify
typedef struct _mjsPlugin { // plugin specification
mjElement* instance; // object type
mjString name; // name
mjString instance_name; // instance name
int plugin_slot; // global registered slot number of the plugin
mjtByte active; // is the plugin active
mjString info; // message appended to compiler errors
} mjmPlugin;
} mjsPlugin;
typedef struct _mjmBody { // body specification
mjElement element; // internal, do not modify
typedef struct _mjsBody { // body specification
mjElement* element; // object type
mjString name; // name
mjString childclass; // childclass name
// body frame
double pos[3]; // frame position
double quat[4]; // frame orientation
mjmOrientation alt; // frame alternative orientation
mjsOrientation alt; // frame alternative orientation
// inertial frame
double mass; // mass
double ipos[3]; // inertial frame position
double iquat[4]; // inertial frame orientation
double inertia[3]; // diagonal inertia (in i-frame)
mjmOrientation ialt; // inertial frame alternative orientation
mjsOrientation ialt; // inertial frame alternative orientation
double fullinertia[6]; // non-axis-aligned inertia matrix
// other
@@ -176,24 +180,24 @@ typedef struct _mjmBody { // body specification
double gravcomp; // gravity compensation
mjDoubleVec userdata; // user data
mjtByte explicitinertial; // whether to save the body with explicit inertial clause
mjmPlugin plugin; // passive force plugin
mjsPlugin plugin; // passive force plugin
mjString info; // message appended to compiler errors
} mjmBody;
} mjsBody;
typedef struct _mjmFrame { // frame specification
mjElement element; // internal, do not modify
typedef struct _mjsFrame { // frame specification
mjElement* element; // object type
mjString name; // name
mjString childclass; // childclass name
double pos[3]; // position
double quat[4]; // orientation
mjmOrientation alt; // alternative orientation
mjsOrientation alt; // alternative orientation
mjString info; // message appended to compiler errors
} mjmFrame;
} mjsFrame;
typedef struct _mjmJoint { // joint specification
mjElement element; // internal, do not modify
typedef struct _mjsJoint { // joint specification
mjElement* element; // object type
mjString name; // name
mjString classname; // class name
mjtJoint type; // joint type
@@ -226,14 +230,15 @@ typedef struct _mjmJoint { // 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
} mjmJoint;
} mjsJoint;
typedef struct _mjmGeom { // geom specification
mjElement element; // internal, do not modify
typedef struct _mjsGeom { // geom specification
mjElement* element; // object type
mjString name; // name
mjString classname; // classname
mjtGeom type; // geom type
@@ -241,7 +246,7 @@ typedef struct _mjmGeom { // geom specification
// frame, size
double pos[3]; // position
double quat[4]; // orientation
mjmOrientation alt; // alternative orientation
mjsOrientation alt; // alternative orientation
double fromto[6]; // alternative for capsule, cylinder, box, ellipsoid
double size[3]; // type-specific size
@@ -276,20 +281,20 @@ typedef struct _mjmGeom { // geom specification
mjString meshname; // mesh attached to geom
double fitscale; // scale mesh uniformly
mjDoubleVec userdata; // user data
mjmPlugin plugin; // sdf plugin
mjsPlugin plugin; // sdf plugin
mjString info; // message appended to compiler errors
} mjmGeom;
} mjsGeom;
typedef struct _mjmSite { // site specification
mjElement element; // internal, do not modify
typedef struct _mjsSite { // site specification
mjElement* element; // object type
mjString name; // name
mjString classname; // class name
// frame, size
double pos[3]; // position
double quat[4]; // orientation
mjmOrientation alt; // alternative orientation
mjsOrientation alt; // alternative orientation
double fromto[6]; // alternative for capsule, cylinder, box, ellipsoid
double size[3]; // geom size
@@ -302,18 +307,18 @@ typedef struct _mjmSite { // site specification
// other
mjDoubleVec userdata; // user data
mjString info; // message appended to compiler errors
} mjmSite;
} mjsSite;
typedef struct _mjmCamera { // camera specification
mjElement element; // internal, do not modify
typedef struct _mjsCamera { // camera specification
mjElement* element; // object type
mjString name; // name
mjString classname; // class name
// extrinsics
double pos[3]; // position
double quat[4]; // orientation
mjmOrientation alt; // alternative orientation
mjsOrientation alt; // alternative orientation
mjtCamLight mode; // tracking mode
mjString targetbody; // target body for tracking/targeting
@@ -331,11 +336,11 @@ typedef struct _mjmCamera { // camera specification
// other
mjDoubleVec userdata; // user data
mjString info; // message appended to compiler errors
} mjmCamera;
} mjsCamera;
typedef struct _mjmLight { // light specification
mjElement element; // internal, do not modify
typedef struct _mjsLight { // light specification
mjElement* element; // object type
mjString name; // name
mjString classname; // class name
@@ -358,11 +363,11 @@ typedef struct _mjmLight { // light specification
// other
mjString info; // message appended to compiler errors
} mjmLight;
} mjsLight;
typedef struct _mjmFlex {
mjElement element; // internal, do not modify
typedef struct _mjsFlex {
mjElement* element; // object type
mjString name; // name
mjString classname; // class name
@@ -399,11 +404,11 @@ typedef struct _mjmFlex {
// other
mjString info; // message appended to compiler errors
} mjmFlex;
} mjsFlex;
typedef struct _mjmMesh { // mesh specification
mjElement element; // internal, do not modify
typedef struct _mjsMesh { // mesh specification
mjElement* element; // object type
mjString name; // name
mjString classname; // class name
mjString content_type; // content type of file
@@ -418,13 +423,13 @@ typedef struct _mjmMesh { // mesh specification
mjIntVec userface; // user vertex indices
mjIntVec userfacenormal; // user normal indices
mjIntVec userfacetexcoord; // user texcoord indices
mjmPlugin plugin; // sdf plugin
mjsPlugin plugin; // sdf plugin
mjString info; // message appended to compiler errors
} mjmMesh;
} mjsMesh;
typedef struct _mjmHField { // height field specification
mjElement element; // internal, do not modify
typedef struct _mjsHField { // height field specification
mjElement* element; // object type
mjString name; // name
mjString content_type; // content type of file
mjString file; // file: (nrow, ncol, [elevation data])
@@ -433,12 +438,12 @@ typedef struct _mjmHField { // height field specification
int ncol; // number of columns
mjFloatVec userdata; // user-provided elevation data
mjString info; // message appended to compiler errors
} mjmHField;
} mjsHField;
typedef struct _mjmSkin { // skin specification
mjElement element; // internal, do not modify
typedef struct _mjsSkin { // skin specification
mjElement* element; // object type
mjString name; // name
mjString classname; // class name
mjString file; // skin file
@@ -461,11 +466,11 @@ typedef struct _mjmSkin { // skin specification
// other
mjString info; // message appended to compiler errors
} mjmSkin;
} mjsSkin;
typedef struct _mjmTexture { // texture specification
mjElement element; // internal, do not modify
typedef struct _mjsTexture { // texture specification
mjElement* element; // object type
mjString name; // name
mjString classname; // class name
mjtTexture type; // texture type
@@ -495,11 +500,11 @@ typedef struct _mjmTexture { // texture specification
// other
mjString info; // message appended to compiler errors
} mjmTexture;
} mjsTexture;
typedef struct _mjmMaterial { // material specification
mjElement element; // internal, do not modify
typedef struct _mjsMaterial { // material specification
mjElement* element; // object type
mjString name; // name
mjString classname; // class name
mjString texture; // name of texture (empty: none)
@@ -511,11 +516,11 @@ typedef struct _mjmMaterial { // material specification
float reflectance; // reflectance
float rgba[4]; // rgba
mjString info; // message appended to compiler errors
} mjmMaterial;
} mjsMaterial;
typedef struct _mjmPair {
mjElement element; // internal, do not modify
typedef struct _mjsPair {
mjElement* element; // object type
mjString name; // name
mjString classname; // class name
mjString geomname1; // name of geom 1
@@ -530,20 +535,20 @@ typedef struct _mjmPair {
double gap; // include in solver if dist<margin-gap
double friction[5]; // full contact friction
mjString info; // message appended to errors
} mjmPair;
} mjsPair;
typedef struct _mjmExclude {
mjElement element; // internal, do not modify
typedef struct _mjsExclude {
mjElement* element; // object type
mjString name; // name
mjString bodyname1; // name of geom 1
mjString bodyname2; // name of geom 2
mjString info; // message appended to errors
} mjmExclude;
} mjsExclude;
typedef struct _mjmEquality { // equality specification
mjElement element; // internal, do not modify
typedef struct _mjsEquality { // equality specification
mjElement* element; // object type
mjString name; // name
mjString classname; // class name
mjtEq type; // constraint type
@@ -554,11 +559,11 @@ typedef struct _mjmEquality { // equality specification
mjtNum solref[mjNREF]; // solver reference
mjtNum solimp[mjNIMP]; // solver impedance
mjString info; // message appended to errors
} mjmEquality;
} mjsEquality;
typedef struct _mjmTendon { // tendon specification
mjElement element; // internal, do not modify
typedef struct _mjsTendon { // tendon specification
mjElement* element; // object type
mjString name; // name
mjString classname; // class name
@@ -586,17 +591,17 @@ typedef struct _mjmTendon { // tendon specification
// other
mjDoubleVec userdata; // user data
mjString info; // message appended to errors
} mjmTendon;
} mjsTendon;
typedef struct _mjmWrap { // wrapping object specification
mjElement element; // internal, do not modify
typedef struct _mjsWrap { // wrapping object specification
mjElement* element; // object type
mjString info; // message appended to errors
} mjmWrap;
} mjsWrap;
typedef struct _mjmActuator { // actuator specification
mjElement element; // internal, do not modify
typedef struct _mjsActuator { // actuator specification
mjElement* element; // object type
mjString name; // name
mjString classname; // class name
@@ -634,13 +639,13 @@ typedef struct _mjmActuator { // actuator specification
// other
int group; // group
mjDoubleVec userdata; // user data
mjmPlugin plugin; // actuator plugin
mjsPlugin plugin; // actuator plugin
mjString info; // message appended to compiler errors
} mjmActuator;
} mjsActuator;
typedef struct _mjmSensor { // sensor specfication
mjElement element; // internal, do not modify
typedef struct _mjsSensor { // sensor specification
mjElement* element; // object type
mjString name; // name
mjString classname; // class name
@@ -662,40 +667,40 @@ typedef struct _mjmSensor { // sensor specfication
// other
mjDoubleVec userdata; // user data
mjmPlugin plugin; // sensor plugin
mjsPlugin plugin; // sensor plugin
mjString info; // message appended to compiler errors
} mjmSensor;
} mjsSensor;
typedef struct _mjmNumeric { // custom numeric field specification
mjElement element; // internal, do not modify
typedef struct _mjsNumeric { // custom numeric field specification
mjElement* element; // object type
mjString name; // name
mjDoubleVec data; // initialization data
int size; // array size, can be bigger than data size
mjString info; // message appended to compiler errors
} mjmNumeric;
} mjsNumeric;
typedef struct _mjmText { // custom text specification
mjElement element; // internal, do not modify
typedef struct _mjsText { // custom text specification
mjElement* element; // object type
mjString name; // name
mjString data; // text string
mjString info; // message appended to compiler errors
} mjmText;
} mjsText;
typedef struct _mjmTuple { // tuple specification
mjElement element; // internal, do not modify
typedef struct _mjsTuple { // tuple specification
mjElement* element; // object type
mjString name; // name
mjIntVec objtype; // object types
mjStringVec objname; // object names
mjDoubleVec objprm; // object parameters
mjString info; // message appended to compiler errors
} mjmTuple;
} mjsTuple;
typedef struct _mjmKey { // keyframe specification
mjElement element; // internal, do not modify
typedef struct _mjsKey { // keyframe specification
mjElement* element; // object type
mjString name; // name
double time; // time
mjDoubleVec qpos; // qpos
@@ -705,283 +710,310 @@ typedef struct _mjmKey { // keyframe specification
mjDoubleVec mquat; // mocap quat
mjDoubleVec ctrl; // ctrl
mjString info; // message appended to compiler errors
} mjmKey;
} mjsKey;
typedef struct _mjmDefault { // default specification
typedef struct _mjsDefault { // default specification
mjString name; // name
mjElement element; // internal, do not modify
mjmJoint* joint; // joint defaults
mjmGeom* geom; // geom defaults
mjmSite* site; // site defaults
mjmCamera* camera; // camera defaults
mjmLight* light; // light defaults
mjmFlex* flex; // flex defaults
mjmMesh* mesh; // mesh defaults
mjmMaterial* material; // material defaults
mjmPair* pair; // pair defaults
mjmEquality* equality; // equality defaults
mjmTendon* tendon; // tendon defaults
mjmActuator* actuator; // actuator defaults
} mjmDefault;
mjElement* element; // object type
mjsJoint* joint; // joint defaults
mjsGeom* geom; // geom defaults
mjsSite* site; // site defaults
mjsCamera* camera; // camera defaults
mjsLight* light; // light defaults
mjsFlex* flex; // flex defaults
mjsMesh* mesh; // mesh defaults
mjsMaterial* material; // material defaults
mjsPair* pair; // pair defaults
mjsEquality* equality; // equality defaults
mjsTendon* tendon; // tendon defaults
mjsActuator* actuator; // actuator defaults
} mjsDefault;
//---------------------------------- API functions -------------------------------------------------
// Create model.
MJAPI mjSpec* mjm_createSpec();
MJAPI mjSpec* mjs_createSpec();
// Copy model.
MJAPI mjSpec* mjs_copySpec(const mjSpec* s);
// Copy back model.
MJAPI void mjm_copyBack(mjSpec* s, const mjModel* m);
MJAPI void mjs_copyBack(mjSpec* s, const mjModel* m);
// Compile model.
MJAPI mjModel* mjm_compile(mjSpec* s, const mjVFS* vfs);
MJAPI mjModel* mjs_compile(mjSpec* s, const mjVFS* vfs);
// Attach child body to a frame of the parent, return 0 if success
MJAPI int mjs_attachBody(mjsFrame* parent, const mjsBody* child,
const char* prefix, const char* suffix);
// Get error message from model.
MJAPI const char* mjm_getError(mjSpec* s);
MJAPI const char* mjs_getError(mjSpec* s);
// Return 1 if model has warnings.
MJAPI int mjm_isWarning(mjSpec* s);
MJAPI int mjs_isWarning(mjSpec* s);
// Delete model.
MJAPI void mjm_deleteSpec(mjSpec* s);
MJAPI void mjs_deleteSpec(mjSpec* s);
// Add child body to body, return child spec.
MJAPI mjmBody* mjm_addBody(mjmBody* body, mjmDefault* def);
MJAPI mjsBody* mjs_addBody(mjsBody* body, mjsDefault* def);
// Add site to body, return site spec.
MJAPI mjmSite* mjm_addSite(mjmBody* body, mjmDefault* def);
MJAPI mjsSite* mjs_addSite(mjsBody* body, mjsDefault* def);
// Add joint to body.
MJAPI mjmJoint* mjm_addJoint(mjmBody* body, mjmDefault* def);
MJAPI mjsJoint* mjs_addJoint(mjsBody* body, mjsDefault* def);
// Add freejoint to body.
MJAPI mjmJoint* mjm_addFreeJoint(mjmBody* body);
MJAPI mjsJoint* mjs_addFreeJoint(mjsBody* body);
// Add geom to body.
MJAPI mjmGeom* mjm_addGeom(mjmBody* body, mjmDefault* def);
MJAPI mjsGeom* mjs_addGeom(mjsBody* body, mjsDefault* def);
// Add camera to body.
MJAPI mjmCamera* mjm_addCamera(mjmBody* body, mjmDefault* def);
MJAPI mjsCamera* mjs_addCamera(mjsBody* body, mjsDefault* def);
// Add light to body.
MJAPI mjmLight* mjm_addLight(mjmBody* body, mjmDefault* def);
MJAPI mjsLight* mjs_addLight(mjsBody* body, mjsDefault* def);
// Add frame to body.
MJAPI mjmFrame* mjm_addFrame(mjmBody* body, mjmFrame* parentframe);
MJAPI mjsFrame* mjs_addFrame(mjsBody* body, mjsFrame* parentframe);
// Add flex to model.
MJAPI mjmFlex* mjm_addFlex(mjSpec* s);
MJAPI mjsFlex* mjs_addFlex(mjSpec* s);
// Add mesh to model.
MJAPI mjmMesh* mjm_addMesh(mjSpec* s, mjmDefault* def);
MJAPI mjsMesh* mjs_addMesh(mjSpec* s, mjsDefault* def);
// Add height field to model.
MJAPI mjmHField* mjm_addHField(mjSpec* s);
MJAPI mjsHField* mjs_addHField(mjSpec* s);
// Add skin to model.
MJAPI mjmSkin* mjm_addSkin(mjSpec* s);
MJAPI mjsSkin* mjs_addSkin(mjSpec* s);
// Add texture to model.
MJAPI mjmTexture* mjm_addTexture(mjSpec* s);
MJAPI mjsTexture* mjs_addTexture(mjSpec* s);
// Add material to model.
MJAPI mjmMaterial* mjm_addMaterial(mjSpec* s, mjmDefault* def);
MJAPI mjsMaterial* mjs_addMaterial(mjSpec* s, mjsDefault* def);
// Add pair to model.
MJAPI mjmPair* mjm_addPair(mjSpec* s, mjmDefault* def);
MJAPI mjsPair* mjs_addPair(mjSpec* s, mjsDefault* def);
// Add excluded body pair to model.
MJAPI mjmExclude* mjm_addExclude(mjSpec* s);
MJAPI mjsExclude* mjs_addExclude(mjSpec* s);
// Add equality to model.
MJAPI mjmEquality* mjm_addEquality(mjSpec* s, mjmDefault* def);
MJAPI mjsEquality* mjs_addEquality(mjSpec* s, mjsDefault* def);
// Add tendon to model.
MJAPI mjmTendon* mjm_addTendon(mjSpec* s, mjmDefault* def);
MJAPI mjsTendon* mjs_addTendon(mjSpec* s, mjsDefault* def);
// Wrap site using tendon.
MJAPI mjmWrap* mjm_wrapSite(mjmTendon* tendon, const char* name);
MJAPI mjsWrap* mjs_wrapSite(mjsTendon* tendon, const char* name);
// Wrap geom using tendon.
MJAPI mjmWrap* mjm_wrapGeom(mjmTendon* tendon, const char* name, const char* sidesite);
MJAPI mjsWrap* mjs_wrapGeom(mjsTendon* tendon, const char* name, const char* sidesite);
// Wrap joint using tendon.
MJAPI mjmWrap* mjm_wrapJoint(mjmTendon* tendon, const char* name, double coef);
MJAPI mjsWrap* mjs_wrapJoint(mjsTendon* tendon, const char* name, double coef);
// Wrap pulley using tendon.
MJAPI mjmWrap* mjm_wrapPulley(mjmTendon* tendon, double divisor);
MJAPI mjsWrap* mjs_wrapPulley(mjsTendon* tendon, double divisor);
// Add actuator to model.
MJAPI mjmActuator* mjm_addActuator(mjSpec* s, mjmDefault* def);
MJAPI mjsActuator* mjs_addActuator(mjSpec* s, mjsDefault* def);
// Add sensor to model.
MJAPI mjmSensor* mjm_addSensor(mjSpec* s);
MJAPI mjsSensor* mjs_addSensor(mjSpec* s);
// Add numeric to model.
MJAPI mjmNumeric* mjm_addNumeric(mjSpec* s);
MJAPI mjsNumeric* mjs_addNumeric(mjSpec* s);
// Add text to model.
MJAPI mjmText* mjm_addText(mjSpec* s);
MJAPI mjsText* mjs_addText(mjSpec* s);
// Add tuple to model.
MJAPI mjmTuple* mjm_addTuple(mjSpec* s);
MJAPI mjsTuple* mjs_addTuple(mjSpec* s);
// Add keyframe to model.
MJAPI mjmKey* mjm_addKey(mjSpec* s);
MJAPI mjsKey* mjs_addKey(mjSpec* s);
// Add plugin to model.
MJAPI mjmPlugin* mjm_addPlugin(mjSpec* s);
MJAPI mjsPlugin* mjs_addPlugin(mjSpec* s);
// Add default to model.
MJAPI mjmDefault* mjm_addDefault(mjSpec* s, const char* classname, int parentid, int* id);
MJAPI mjsDefault* mjs_addDefault(mjSpec* s, const char* classname, int parentid, int* id);
// Get model spec from body.
MJAPI mjSpec* mjm_getSpec(mjmBody* body);
MJAPI mjSpec* mjs_getSpec(mjsBody* body);
// Get default corresponding to an mjElement.
MJAPI mjmDefault* mjm_getDefault(mjElement element);
MJAPI mjsDefault* mjs_getDefault(mjElement* element);
// Find default in model by class name.
MJAPI mjmDefault* mjm_findDefault(mjSpec* s, const char* classname);
MJAPI mjsDefault* mjs_findDefault(mjSpec* s, const char* classname);
// Get global default from model.
MJAPI mjmDefault* mjm_getSpecDefault(mjSpec* s);
MJAPI mjsDefault* mjs_getSpecDefault(mjSpec* s);
// Find body in model by name.
MJAPI mjmBody* mjm_findBody(mjSpec* s, const char* name);
MJAPI mjsBody* mjs_findBody(mjSpec* s, const char* name);
// Find child body by name.
MJAPI mjmBody* mjm_findChild(mjmBody* body, const char* name);
MJAPI mjsBody* mjs_findChild(mjsBody* body, const char* name);
// Find mesh by name.
MJAPI mjmMesh* mjm_findMesh(mjSpec* s, const char* name);
MJAPI mjsMesh* mjs_findMesh(mjSpec* s, const char* name);
// Find frame by name.
MJAPI mjsFrame* mjs_findFrame(mjSpec* s, const char* name);
// Get element id.
MJAPI int mjm_getId(mjElement element);
MJAPI int mjs_getId(mjElement* element);
// Copy text to string.
MJAPI void mjm_setString(mjString dest, const char* text);
MJAPI void mjs_setString(mjString dest, const char* text);
// Split text to entries and copy to string vector.
MJAPI void mjm_setStringVec(mjStringVec dest, const char* text);
MJAPI void mjs_setStringVec(mjStringVec dest, const char* text);
// Set entry in string vector.
MJAPI mjtByte mjm_setInStringVec(mjStringVec dest, int i, const char* text);
MJAPI mjtByte mjs_setInStringVec(mjStringVec dest, int i, const char* text);
// Append text entry to string vector.
MJAPI void mjm_appendString(mjStringVec dest, const char* text);
MJAPI void mjs_appendString(mjStringVec dest, const char* text);
// Copy int array to vector.
MJAPI void mjm_setInt(mjIntVec dest, const int* array, int size);
MJAPI void mjs_setInt(mjIntVec dest, const int* array, int size);
// Append int array to vector of arrays.
MJAPI void mjm_appendIntVec(mjIntVecVec dest, const int* array, int size);
MJAPI void mjs_appendIntVec(mjIntVecVec dest, const int* array, int size);
// Copy float array to vector.
MJAPI void mjm_setFloat(mjFloatVec dest, const float* array, int size);
MJAPI void mjs_setFloat(mjFloatVec dest, const float* array, int size);
// Append float array to vector of arrays.
MJAPI void mjm_appendFloatVec(mjFloatVecVec dest, const float* array, int size);
MJAPI void mjs_appendFloatVec(mjFloatVecVec dest, const float* array, int size);
// Copy double array to vector.
MJAPI void mjm_setDouble(mjDoubleVec dest, const double* array, int size);
MJAPI void mjs_setDouble(mjDoubleVec dest, const double* array, int size);
// Get string contents.
MJAPI const char* mjm_getString(mjString source);
MJAPI const char* mjs_getString(mjString source);
// Get double array contents and optionally its size.
MJAPI const double* mjm_getDouble(mjDoubleVec source, int* size);
MJAPI const double* mjs_getDouble(mjDoubleVec source, int* size);
// Set plugin attributes.
MJAPI void mjm_setPluginAttributes(mjmPlugin* plugin, void* attributes);
MJAPI void mjs_setPluginAttributes(mjsPlugin* plugin, void* attributes);
// Set active plugins.
MJAPI void mjm_setActivePlugins(mjSpec* s, void* activeplugins);
MJAPI void mjs_setActivePlugins(mjSpec* s, void* activeplugins);
// Set default.
MJAPI void mjm_setDefault(mjElement element, mjmDefault* def);
MJAPI void mjs_setDefault(mjElement* element, mjsDefault* def);
// Set frame.
MJAPI void mjm_setFrame(mjElement dest, mjmFrame* frame);
MJAPI void mjs_setFrame(mjElement* dest, mjsFrame* frame);
// Resolve alternative orientations to quat.
MJAPI const char* mjs_resolveOrientation(double quat[4], mjtByte degree, const char* sequence,
const mjsOrientation* orientation);
// Compute quat and inertia from body->fullinertia.
MJAPI const char* mjm_setFullInertia(mjmBody* body, double quat[4], double inertia[3]);
MJAPI const char* mjs_setFullInertia(mjsBody* body, double quat[4], double inertia[3]);
//---------------------------------- Initialization functions --------------------------------------
// Default model attributes.
MJAPI void mjm_defaultSpec(mjSpec& model);
MJAPI void mjs_defaultSpec(mjSpec& model);
// Default orientation attributes.
MJAPI void mjs_defaultOrientation(mjsOrientation& orient);
// Default body attributes.
MJAPI void mjm_defaultBody(mjmBody& body);
MJAPI void mjs_defaultBody(mjsBody& body);
// Default frame attributes.
MJAPI void mjm_defaultFrame(mjmFrame& frame);
MJAPI void mjs_defaultFrame(mjsFrame& frame);
// Default joint attributes.
MJAPI void mjm_defaultJoint(mjmJoint& joint);
MJAPI void mjs_defaultJoint(mjsJoint& joint);
// Default geom attributes.
MJAPI void mjm_defaultGeom(mjmGeom& geom);
MJAPI void mjs_defaultGeom(mjsGeom& geom);
// Default site attributes.
MJAPI void mjm_defaultSite(mjmSite& site);
MJAPI void mjs_defaultSite(mjsSite& site);
// Default camera attributes.
MJAPI void mjm_defaultCamera(mjmCamera& camera);
MJAPI void mjs_defaultCamera(mjsCamera& camera);
// Default light attributes.
MJAPI void mjm_defaultLight(mjmLight& light);
MJAPI void mjs_defaultLight(mjsLight& light);
// Default flex attributes.
MJAPI void mjm_defaultFlex(mjmFlex& flex);
MJAPI void mjs_defaultFlex(mjsFlex& flex);
// Default mesh attributes.
MJAPI void mjm_defaultMesh(mjmMesh& mesh);
MJAPI void mjs_defaultMesh(mjsMesh& mesh);
// Default height field attributes.
MJAPI void mjm_defaultHField(mjmHField& hfield);
MJAPI void mjs_defaultHField(mjsHField& hfield);
// Default skin attributes.
MJAPI void mjm_defaultSkin(mjmSkin& skin);
MJAPI void mjs_defaultSkin(mjsSkin& skin);
// Default texture attributes.
MJAPI void mjm_defaultTexture(mjmTexture& texture);
MJAPI void mjs_defaultTexture(mjsTexture& texture);
// Default material attributes.
MJAPI void mjm_defaultMaterial(mjmMaterial& material);
MJAPI void mjs_defaultMaterial(mjsMaterial& material);
// Default pair attributes.
MJAPI void mjm_defaultPair(mjmPair& pair);
MJAPI void mjs_defaultPair(mjsPair& pair);
// Default equality attributes.
MJAPI void mjm_defaultEquality(mjmEquality& equality);
MJAPI void mjs_defaultEquality(mjsEquality& equality);
// Default tendon attributes.
MJAPI void mjm_defaultTendon(mjmTendon& tendon);
MJAPI void mjs_defaultTendon(mjsTendon& tendon);
// Default actuator attributes.
MJAPI void mjm_defaultActuator(mjmActuator& actuator);
MJAPI void mjs_defaultActuator(mjsActuator& actuator);
// Default sensor attributes.
MJAPI void mjm_defaultSensor(mjmSensor& sensor);
MJAPI void mjs_defaultSensor(mjsSensor& sensor);
// Default numeric attributes.
MJAPI void mjm_defaultNumeric(mjmNumeric& numeric);
MJAPI void mjs_defaultNumeric(mjsNumeric& numeric);
// Default text attributes.
MJAPI void mjm_defaultText(mjmText& text);
MJAPI void mjs_defaultText(mjsText& text);
// Default tuple attributes.
MJAPI void mjm_defaultTuple(mjmTuple& tuple);
MJAPI void mjs_defaultTuple(mjsTuple& tuple);
// Default keyframe attributes.
MJAPI void mjm_defaultKey(mjmKey& key);
MJAPI void mjs_defaultKey(mjsKey& key);
// Default plugin attributes.
MJAPI void mjm_defaultPlugin(mjmPlugin& plugin);
MJAPI void mjs_defaultPlugin(mjsPlugin& plugin);
//------------------------- Cache functions ------------------------------------
typedef struct _mjCache* mjCache;
// Set the size of the cache in bytes.
MJAPI void mj_setCacheSize(mjCache cache, size_t size);
// Get internal global cache context.
MJAPI mjCache mj_globalCache();
#ifdef __cplusplus
}
src/user/user_cache.cc
+2 -1
View File
@@ -39,7 +39,8 @@ template<typename T> std::size_t mjCAsset::Add(const std::string& name,
const uint8_t* ptr = reinterpret_cast<const uint8_t*>(data);
mjCAssetData& block = it->second;
block.bytes = std::make_shared<uint8_t[]>(nbytes);
block.bytes = std::shared_ptr<uint8_t>(new uint8_t[nbytes],
[](uint8_t *p) {delete [] p;});
std::copy(ptr, ptr + nbytes, block.bytes.get());
block.nbytes = nbytes;
nbytes_ += nbytes;
src/user/user_cache.h
+18 -17
View File
@@ -30,7 +30,7 @@
// data associated with an asset
struct mjCAssetData {
std::shared_ptr<uint8_t[]> bytes; // raw serialized bytes of cached data
std::shared_ptr<uint8_t> bytes; // raw serialized bytes of cached data
std::size_t nbytes; // number of bytes stored
};
@@ -80,6 +80,11 @@ class mjCAsset {
return blocks_.find(name) != blocks_.end();
}
const std::string& Timestamp() const { return timestamp_; }
const std::string& Id() const { return id_; }
std::size_t InsertNum() const { return insert_num_; }
std::size_t AccessCount() const { return access_count_; }
private:
mjCAsset() = default;
@@ -105,10 +110,6 @@ class mjCAsset {
void SetTimestamp(std::string timestamp) { timestamp_ = timestamp; }
// accessors
const std::string& Id() const { return id_; }
const std::string& Timestamp() const { return timestamp_; }
std::size_t InsertNum() const { return insert_num_; }
std::size_t AccessCount() const { return access_count_; }
std::size_t BytesCount() const { return nbytes_; }
const std::unordered_map<std::string, mjCAssetData>& Blocks() const {
return blocks_;
@@ -128,14 +129,23 @@ class mjCAsset {
std::set<std::string> references_;
};
struct mjCAssetCompare {
bool operator()(const mjCAsset* e1, const mjCAsset* e2) const {
if (e1->AccessCount() != e2->AccessCount()) {
return e1->AccessCount() < e2->AccessCount();
}
return e1->InsertNum() < e2->InsertNum();
}
};
// the class container for a thread-safe asset cache
class mjCCache {
public:
explicit mjCCache(std::size_t size) : max_size_(size) {}
// move only
mjCCache(mjCCache&& other) = default;
mjCCache& operator=(mjCCache&& other) = default;
mjCCache(mjCCache&& other) = delete;
mjCCache& operator=(mjCCache&& other) = delete;
mjCCache(const mjCCache& other) = delete;
mjCCache& operator=(const mjCCache& other) = delete;
@@ -186,20 +196,11 @@ class mjCCache {
std::size_t size_ = 0; // current size of the cache in bytes
std::size_t max_size_ = 0; // max size of the cache in bytes
// compare function for the priority queue
static constexpr auto compare_ = [](const mjCAsset* e1,
const mjCAsset* e2) {
if (e1->AccessCount() != e2->AccessCount()) {
return e1->AccessCount() < e2->AccessCount();
}
return e1->InsertNum() < e2->InsertNum();
};
// internal constant look up table for assets
std::unordered_map<std::string, mjCAsset> lookup_;
// internal priority queue for the cache
std::set<mjCAsset*, decltype(compare_)> entries_;
std::set<mjCAsset*, mjCAssetCompare> entries_;
// models using the cache along with the assets they reference
std::unordered_map<std::string, std::unordered_set<mjCAsset*>> models_;
src/user/user_composite.cc
+155 -155
View File
@@ -66,7 +66,7 @@ mjCComposite::mjCComposite(void) {
mj_defaultSolRefImp(solrefsmooth, solimpsmooth);
// plugin variables
mjm_defaultPlugin(plugin);
mjs_defaultPlugin(plugin);
plugin_name = "";
plugin_instance_name = "";
plugin.name = (mjString)&plugin_name;
@@ -223,7 +223,7 @@ void mjCComposite::SetDefault(void) {
// make composite object
bool mjCComposite::Make(mjSpec* spec, mjmBody* body, char* error, int error_sz) {
bool mjCComposite::Make(mjSpec* spec, mjsBody* body, char* error, int error_sz) {
mjCModel* model = (mjCModel*)spec->element;
// check geom type
@@ -306,7 +306,7 @@ bool mjCComposite::Make(mjSpec* spec, mjmBody* body, char* error, int error_sz)
// overwrite plugin name
if (plugin_instance_name.empty() && plugin.active) {
plugin_instance_name = "composite" + prefix;
((mjCPlugin*)plugin.instance)->name = plugin_instance_name;
(static_cast<mjCPlugin*>(plugin.instance))->name = plugin_instance_name;
}
// dispatch
@@ -350,7 +350,7 @@ bool mjCComposite::Make(mjSpec* spec, mjmBody* body, char* error, int error_sz)
bool mjCComposite::MakeParticle(mjCModel* model, mjmBody* body, char* error, int error_sz) {
bool mjCComposite::MakeParticle(mjCModel* model, mjsBody* body, char* error, int error_sz) {
char txt[100];
std::vector<int> face;
@@ -432,7 +432,7 @@ bool mjCComposite::MakeParticle(mjCModel* model, mjmBody* body, char* error, int
mjtNum t = 1;
if (dim == 2 && plugin.active) {
try {
mjCPlugin* pplugin = (mjCPlugin*)plugin.instance;
mjCPlugin* pplugin = static_cast<mjCPlugin*>(plugin.instance);
t = std::stod(pplugin->config_attribs["thickness"], nullptr);
} catch (const std::invalid_argument& e) {
return comperr(error, "Invalid thickness attribute", error_sz);
@@ -464,13 +464,13 @@ bool mjCComposite::MakeParticle(mjCModel* model, mjmBody* body, char* error, int
// create bodies and geoms
for (int i=0; i<uservert.size()/3; i++) {
// create body
mjmBody* b = mjm_addBody(body, NULL);
mjsBody* b = mjs_addBody(body, NULL);
if (!username.empty()) {
mjm_setString(b->name, username[i].c_str());
mjs_setString(b->name, username[i].c_str());
} else {
mju::sprintf_arr(txt, "%sB%d", prefix.c_str(), i);
mjm_setString(b->name, txt);
mjs_setString(b->name, txt);
}
// set body position
@@ -481,8 +481,8 @@ bool mjCComposite::MakeParticle(mjCModel* model, mjmBody* body, char* error, int
// add slider joints if none defined
if (!add[mjCOMPKIND_PARTICLE]) {
for (int i=0; i<3; i++) {
mjmJoint* jnt = mjm_addJoint(b, &defjoint[mjCOMPKIND_JOINT][0].spec);
mjm_setDefault(jnt->element, mjm_getDefault(body->element));
mjsJoint* jnt = mjs_addJoint(b, &defjoint[mjCOMPKIND_JOINT][0].spec);
mjs_setDefault(jnt->element, mjs_getDefault(body->element));
jnt->type = mjJNT_SLIDE;
mjuu_setvec(jnt->pos, 0, 0, 0);
mjuu_setvec(jnt->axis, 0, 0, 0);
@@ -493,30 +493,30 @@ bool mjCComposite::MakeParticle(mjCModel* model, mjmBody* body, char* error, int
// add user-specified joints
else {
for (auto defjnt : defjoint[mjCOMPKIND_PARTICLE]) {
mjmJoint* jnt = mjm_addJoint(b, &defjnt.spec);
mjm_setDefault(jnt->element, mjm_getDefault(body->element));
mjsJoint* jnt = mjs_addJoint(b, &defjnt.spec);
mjs_setDefault(jnt->element, mjs_getDefault(body->element));
}
}
// add geom
mjmGeom* g = mjm_addGeom(b, &def[0].spec);
mjm_setDefault(g->element, mjm_getDefault(body->element));
mjsGeom* g = mjs_addGeom(b, &def[0].spec);
mjs_setDefault(g->element, mjs_getDefault(body->element));
// add site
mjmSite* s = mjm_addSite(b, &def[0].spec);
mjm_setDefault(s->element, mjm_getDefault(body->element));
mjsSite* s = mjs_addSite(b, &def[0].spec);
mjs_setDefault(s->element, mjs_getDefault(body->element));
s->type = mjGEOM_SPHERE;
mju::sprintf_arr(txt, "%sS%d", prefix.c_str(), i);
mjm_setString(s->name, txt);
mjs_setString(s->name, txt);
// add plugin
if (plugin.active) {
mjmPlugin* pplugin = &b->plugin;
mjCPlugin* cplugin = (mjCPlugin*)plugin.instance;
mjsPlugin* pplugin = &b->plugin;
mjCPlugin* cplugin = static_cast<mjCPlugin*>(plugin.instance);
pplugin->active = true;
pplugin->instance = (mjElement)plugin.instance;
mjm_setString(pplugin->instance_name, plugin_instance_name.c_str());
mjm_setString(pplugin->name, mjm_getString(plugin.name));
pplugin->instance = plugin.instance;
mjs_setString(pplugin->instance_name, plugin_instance_name.c_str());
mjs_setString(pplugin->name, mjs_getString(plugin.name));
if (i==0 && !cplugin->config_attribs["face"].empty()) {
return comperr(error, "Face attribute already exists in plugin", error_sz);
@@ -560,18 +560,18 @@ bool mjCComposite::MakeParticle(mjCModel* model, mjmBody* body, char* error, int
mju::sprintf_arr(txt2, "%sS%d", prefix.c_str(), v1);
// create tendon
mjmTendon* ten = mjm_addTendon(&model->spec, &def[mjCOMPKIND_TENDON].spec);
mjm_setDefault(ten->element, &model->defaults[0]->spec);
mjm_setString(ten->name, txt0);
mjsTendon* ten = mjs_addTendon(&model->spec, &def[mjCOMPKIND_TENDON].spec);
mjs_setDefault(ten->element, &model->defaults[0]->spec);
mjs_setString(ten->name, txt0);
ten->group = 4;
mjm_wrapSite(ten, txt1);
mjm_wrapSite(ten, txt2);
mjs_wrapSite(ten, txt1);
mjs_wrapSite(ten, txt2);
// add equality constraint
mjmEquality* eq = mjm_addEquality(&model->spec, &def[mjCOMPKIND_TENDON].spec);
mjm_setDefault(eq->element, &model->defaults[0]->spec);
mjsEquality* eq = mjs_addEquality(&model->spec, &def[mjCOMPKIND_TENDON].spec);
mjs_setDefault(eq->element, &model->defaults[0]->spec);
eq->type = mjEQ_TENDON;
mjm_setString(eq->name1, mjm_getString(ten->name));
mjs_setString(eq->name1, mjs_getString(ten->name));
}
}
@@ -593,7 +593,7 @@ bool mjCComposite::MakeParticle(mjCModel* model, mjmBody* body, char* error, int
// make grid connected with tendons
bool mjCComposite::MakeGrid(mjCModel* model, mjmBody* body, char* error, int error_sz) {
bool mjCComposite::MakeGrid(mjCModel* model, mjsBody* body, char* error, int error_sz) {
char txt[100], txt1[100], txt2[100];
// check dimensionality
@@ -615,9 +615,9 @@ bool mjCComposite::MakeGrid(mjCModel* model, mjmBody* body, char* error, int err
for (int ix=0; ix<count[0]; ix++) {
for (int iy=0; iy<count[1]; iy++) {
// create body
mjmBody* b = mjm_addBody(body, NULL);
mjsBody* b = mjs_addBody(body, NULL);
mju::sprintf_arr(txt, "%sB%d_%d", prefix.c_str(), ix, iy);
mjm_setString(b->name, txt);
mjs_setString(b->name, txt);
// set body position
b->pos[0] = offset[0] + spacing*(ix - 0.5*count[0]);
@@ -625,18 +625,18 @@ bool mjCComposite::MakeGrid(mjCModel* model, mjmBody* body, char* error, int err
b->pos[2] = offset[2];
// add geom
mjmGeom* g = mjm_addGeom(b, &def[0].spec);
mjm_setDefault(g->element, mjm_getDefault(body->element));
mjsGeom* g = mjs_addGeom(b, &def[0].spec);
mjs_setDefault(g->element, mjs_getDefault(body->element));
g->type = mjGEOM_SPHERE;
mju::sprintf_arr(txt, "%sG%d_%d", prefix.c_str(), ix, iy);
mjm_setString(g->name, txt);
mjs_setString(g->name, txt);
// add site
mjmSite* s = mjm_addSite(b, &def[0].spec);
mjm_setDefault(s->element, mjm_getDefault(body->element));
mjsSite* s = mjs_addSite(b, &def[0].spec);
mjs_setDefault(s->element, mjs_getDefault(body->element));
s->type = mjGEOM_SPHERE;
mju::sprintf_arr(txt, "%sS%d_%d", prefix.c_str(), ix, iy);
mjm_setString(s->name, txt);
mjs_setString(s->name, txt);
// skip pinned elements
bool skip = false;
@@ -651,12 +651,12 @@ bool mjCComposite::MakeGrid(mjCModel* model, mjmBody* body, char* error, int err
}
// add slider joint
mjmJoint* jnt[3];
mjsJoint* jnt[3];
for (int i=0; i<3; i++) {
jnt[i] = mjm_addJoint(b, &defjoint[mjCOMPKIND_JOINT][0].spec);
mjm_setDefault(jnt[i]->element, mjm_getDefault(body->element));
jnt[i] = mjs_addJoint(b, &defjoint[mjCOMPKIND_JOINT][0].spec);
mjs_setDefault(jnt[i]->element, mjs_getDefault(body->element));
mju::sprintf_arr(txt, "%sJ%d_%d_%d", prefix.c_str(), i, ix, iy);
mjm_setString(jnt[i]->name, txt);
mjs_setString(jnt[i]->name, txt);
jnt[i]->type = mjJNT_SLIDE;
mjuu_setvec(jnt[i]->pos, 0, 0, 0);
mjuu_setvec(jnt[i]->axis, 0, 0, 0);
@@ -682,10 +682,10 @@ bool mjCComposite::MakeGrid(mjCModel* model, mjmBody* body, char* error, int err
ten->WrapSite(txt2);
// add equality constraint
mjmEquality* eq = mjm_addEquality(&model->spec, &def[mjCOMPKIND_TENDON].spec);
mjm_setDefault(eq->element, &model->defaults[0]->spec);
mjsEquality* eq = mjs_addEquality(&model->spec, &def[mjCOMPKIND_TENDON].spec);
mjs_setDefault(eq->element, &model->defaults[0]->spec);
eq->type = mjEQ_TENDON;
mjm_setString(eq->name1, ten->name.c_str());
mjs_setString(eq->name1, ten->name.c_str());
}
}
}
@@ -709,7 +709,7 @@ bool mjCComposite::MakeGrid(mjCModel* model, mjmBody* body, char* error, int err
bool mjCComposite::MakeCable(mjCModel* model, mjmBody* body, char* error, int error_sz) {
bool mjCComposite::MakeCable(mjCModel* model, mjsBody* body, char* error, int error_sz) {
// check dim
if (dim!=1) {
return comperr(error, "Cable must be one-dimensional", error_sz);
@@ -723,9 +723,9 @@ bool mjCComposite::MakeCable(mjCModel* model, mjmBody* body, char* error, int er
}
// add name to model
mjmText* pte = mjm_addText(&model->spec);
mjm_setString(pte->name, ("composite_" + prefix).c_str());
mjm_setString(pte->data, ("rope_" + prefix).c_str());
mjsText* pte = mjs_addText(&model->spec);
mjs_setString(pte->name, ("composite_" + prefix).c_str());
mjs_setString(pte->data, ("rope_" + prefix).c_str());
// populate uservert if not specified
if (uservert.empty()) {
@@ -780,7 +780,7 @@ bool mjCComposite::MakeCable(mjCModel* model, mjmBody* body, char* error, int er
mjmBody* mjCComposite::AddCableBody(mjCModel* model, mjmBody* body, int ix, mjtNum normal[3], mjtNum prev_quat[4]) {
mjsBody* mjCComposite::AddCableBody(mjCModel* model, mjsBody* body, int ix, mjtNum normal[3], mjtNum prev_quat[4]) {
char txt_geom[100], txt_site[100], txt_slide[100];
char this_body[100], next_body[100], this_joint[100];
mjtNum dquat[4], this_quat[4];
@@ -836,8 +836,8 @@ mjmBody* mjCComposite::AddCableBody(mjCModel* model, mjmBody* body, int ix, mjtN
mju::sprintf_arr(txt_slide, "%sJs%d", prefix.c_str(), ix);
// add body
body = mjm_addBody(body, 0);
mjm_setString(body->name, this_body);
body = mjs_addBody(body, 0);
mjs_setString(body->name, this_body);
if (first) {
mjuu_setvec(body->pos, offset[0]+uservert[3*ix],
offset[1]+uservert[3*ix+1],
@@ -851,9 +851,9 @@ mjmBody* mjCComposite::AddCableBody(mjCModel* model, mjmBody* body, int ix, mjtN
}
// add geom
mjmGeom* geom = mjm_addGeom(body, &def[0].spec);
mjm_setDefault(geom->element, mjm_getDefault(body->element));
mjm_setString(geom->name, txt_geom);
mjsGeom* geom = mjs_addGeom(body, &def[0].spec);
mjs_setDefault(geom->element, mjs_getDefault(body->element));
mjs_setString(geom->name, txt_geom);
if (def[0].spec.geom->type==mjGEOM_CYLINDER ||
def[0].spec.geom->type==mjGEOM_CAPSULE) {
mjuu_zerovec(geom->fromto, 6);
@@ -866,11 +866,11 @@ mjmBody* mjCComposite::AddCableBody(mjCModel* model, mjmBody* body, int ix, mjtN
// add plugin
if (plugin.active) {
mjmPlugin* pplugin = &body->plugin;
mjsPlugin* pplugin = &body->plugin;
pplugin->active = true;
pplugin->instance = (mjElement)plugin.instance;
mjm_setString(pplugin->name, mjm_getString(plugin.name));
mjm_setString(pplugin->instance_name, plugin_instance_name.c_str());
pplugin->instance = plugin.instance;
mjs_setString(pplugin->name, mjs_getString(plugin.name));
mjs_setString(pplugin->instance_name, plugin_instance_name.c_str());
}
// update orientation
@@ -878,27 +878,27 @@ mjmBody* mjCComposite::AddCableBody(mjCModel* model, mjmBody* body, int ix, mjtN
// add curvature joint
if (!first || strcmp(initial.c_str(), "none")) {
mjmJoint* jnt = mjm_addJoint(body, &defjoint[mjCOMPKIND_JOINT][0].spec);
mjm_setDefault(jnt->element, mjm_getDefault(body->element));
mjsJoint* jnt = mjs_addJoint(body, &defjoint[mjCOMPKIND_JOINT][0].spec);
mjs_setDefault(jnt->element, mjs_getDefault(body->element));
jnt->type = (first && strcmp(initial.c_str(), "free")==0) ? mjJNT_FREE : mjJNT_BALL;
jnt->damping = jnt->type==mjJNT_FREE ? 0 : jnt->damping;
jnt->armature = jnt->type==mjJNT_FREE ? 0 : jnt->armature;
jnt->frictionloss = jnt->type==mjJNT_FREE ? 0 : jnt->frictionloss;
mjm_setString(jnt->name, this_joint);
mjs_setString(jnt->name, this_joint);
}
// exclude contact pair
if (!last) {
mjmExclude* exclude = mjm_addExclude(&model->spec);
mjm_setString(exclude->bodyname1, std::string(this_body).c_str());
mjm_setString(exclude->bodyname2, std::string(next_body).c_str());
mjsExclude* exclude = mjs_addExclude(&model->spec);
mjs_setString(exclude->bodyname1, std::string(this_body).c_str());
mjs_setString(exclude->bodyname2, std::string(next_body).c_str());
}
// add site at the boundary
if (last || first) {
mjmSite* site = mjm_addSite(body, &def[0].spec);
mjm_setDefault(site->element, mjm_getDefault(body->element));
mjm_setString(site->name, txt_site);
mjsSite* site = mjs_addSite(body, &def[0].spec);
mjs_setDefault(site->element, mjs_getDefault(body->element));
mjs_setString(site->name, txt_site);
mjuu_setvec(site->pos, last ? length : 0, 0, 0);
mjuu_setvec(site->quat, 1, 0, 0, 0);
}
@@ -908,7 +908,7 @@ mjmBody* mjCComposite::AddCableBody(mjCModel* model, mjmBody* body, int ix, mjtN
// make rope
bool mjCComposite::MakeRope(mjCModel* model, mjmBody* body, char* error, int error_sz) {
bool mjCComposite::MakeRope(mjCModel* model, mjsBody* body, char* error, int error_sz) {
// check dim
if (dim!=1) {
return comperr(error, "Rope must be one-dimensional", error_sz);
@@ -917,7 +917,7 @@ bool mjCComposite::MakeRope(mjCModel* model, mjmBody* body, char* error, int err
// check root body name prefix
char txt[200];
mju::sprintf_arr(txt, "%sB", prefix.c_str());
std::string body_name = mjm_getString(body->name);
std::string body_name = mjs_getString(body->name);
if (std::strncmp(txt, body_name.substr(0, strlen(txt)).c_str(), mju::sizeof_arr(txt))) {
mju::strcat_arr(txt, " must be the beginning of root body name");
return comperr(error, txt, error_sz);
@@ -937,7 +937,7 @@ bool mjCComposite::MakeRope(mjCModel* model, mjmBody* body, char* error, int err
AddRopeBody(model, body, ox, ox);
// add elements: right
mjmBody* pbody = body;
mjsBody* pbody = body;
for (int ix=ox; ix<count[0]-1; ix++) {
pbody = AddRopeBody(model, pbody, ix, ix+1);
}
@@ -953,20 +953,20 @@ bool mjCComposite::MakeRope(mjCModel* model, mjmBody* body, char* error, int err
char txt2[200];
// add equality constraint
mjmEquality* eq = mjm_addEquality(&model->spec, 0);
mjsEquality* eq = mjs_addEquality(&model->spec, 0);
eq->type = mjEQ_CONNECT;
mju::sprintf_arr(txt, "%sB0", prefix.c_str());
mju::sprintf_arr(txt2, "%sB%d", prefix.c_str(), count[0]-1);
mjm_setString(eq->name1, txt);
mjm_setString(eq->name2, txt2);
mjs_setString(eq->name1, txt);
mjs_setString(eq->name2, txt2);
mjuu_setvec(eq->data, -0.5*spacing, 0, 0);
mju_copy(eq->solref, solrefsmooth, mjNREF);
mju_copy(eq->solimp, solimpsmooth, mjNIMP);
// remove contact between connected bodies
mjmExclude* pair = mjm_addExclude(&model->spec);
mjm_setString(pair->bodyname1, std::string(txt).c_str());
mjm_setString(pair->bodyname2, std::string(txt2).c_str());
mjsExclude* pair = mjs_addExclude(&model->spec);
mjs_setString(pair->bodyname1, std::string(txt).c_str());
mjs_setString(pair->bodyname2, std::string(txt2).c_str());
}
return true;
@@ -975,16 +975,16 @@ bool mjCComposite::MakeRope(mjCModel* model, mjmBody* body, char* error, int err
// add child body for cloth
mjmBody* mjCComposite::AddRopeBody(mjCModel* model, mjmBody* body, int ix, int ix1) {
mjsBody* mjCComposite::AddRopeBody(mjCModel* model, mjsBody* body, int ix, int ix1) {
char txt[100];
bool isroot = (ix==ix1);
double dx = spacing*(ix1-ix);
// add child if not root
if (!isroot) {
body = mjm_addBody(body, 0);
body = mjs_addBody(body, 0);
mju::sprintf_arr(txt, "%sB%d", prefix.c_str(), ix1);
mjm_setString(body->name, txt);
mjs_setString(body->name, txt);
// loop
if (type==mjCOMPTYPE_LOOP) {
@@ -1007,10 +1007,10 @@ mjmBody* mjCComposite::AddRopeBody(mjCModel* model, mjmBody* body, int ix, int i
}
// add geom
mjmGeom* geom = mjm_addGeom(body, &def[0].spec);
mjm_setDefault(geom->element, mjm_getDefault(body->element));
mjsGeom* geom = mjs_addGeom(body, &def[0].spec);
mjs_setDefault(geom->element, mjs_getDefault(body->element));
mju::sprintf_arr(txt, "%sG%d", prefix.c_str(), ix1);
mjm_setString(geom->name, txt);
mjs_setString(geom->name, txt);
mjuu_setvec(geom->pos, 0, 0, 0);
mjuu_setvec(geom->quat, sqrt(0.5), 0, sqrt(0.5), 0);
@@ -1022,10 +1022,10 @@ mjmBody* mjCComposite::AddRopeBody(mjCModel* model, mjmBody* body, int ix, int i
// add main joint
for (int i=0; i<2; i++) {
// add joint
mjmJoint* jnt = mjm_addJoint(body, &defjoint[mjCOMPKIND_JOINT][0].spec);
mjm_setDefault(jnt->element, mjm_getDefault(body->element));
mjsJoint* jnt = mjs_addJoint(body, &defjoint[mjCOMPKIND_JOINT][0].spec);
mjs_setDefault(jnt->element, mjs_getDefault(body->element));
mju::sprintf_arr(txt, "%sJ%d_%d", prefix.c_str(), i, ix1);
mjm_setString(jnt->name, txt);
mjs_setString(jnt->name, txt);
jnt->type = mjJNT_HINGE;
mjuu_setvec(jnt->pos, -0.5*dx, 0, 0);
mjuu_setvec(jnt->axis, 0, 0, 0);
@@ -1035,37 +1035,37 @@ mjmBody* mjCComposite::AddRopeBody(mjCModel* model, mjmBody* body, int ix, int i
// add twist joint
if (add[mjCOMPKIND_TWIST]) {
// add joint
mjmJoint* jnt = mjm_addJoint(body, &defjoint[mjCOMPKIND_TWIST][0].spec);
mjm_setDefault(jnt->element, mjm_getDefault(body->element));
mjsJoint* jnt = mjs_addJoint(body, &defjoint[mjCOMPKIND_TWIST][0].spec);
mjs_setDefault(jnt->element, mjs_getDefault(body->element));
mju::sprintf_arr(txt, "%sJT%d", prefix.c_str(), ix1);
mjm_setString(jnt->name, txt);
mjs_setString(jnt->name, txt);
jnt->type = mjJNT_HINGE;
mjuu_setvec(jnt->pos, -0.5*dx, 0, 0);
mjuu_setvec(jnt->axis, 1, 0, 0);
// add constraint
mjmEquality* eq = mjm_addEquality(&model->spec, &def[mjCOMPKIND_TWIST].spec);
mjm_setDefault(eq->element, &model->defaults[0]->spec);
mjsEquality* eq = mjs_addEquality(&model->spec, &def[mjCOMPKIND_TWIST].spec);
mjs_setDefault(eq->element, &model->defaults[0]->spec);
eq->type = mjEQ_JOINT;
mjm_setString(eq->name1, mjm_getString(jnt->name));
mjs_setString(eq->name1, mjs_getString(jnt->name));
}
// add stretch joint
if (add[mjCOMPKIND_STRETCH]) {
// add joint
mjmJoint* jnt = mjm_addJoint(body, &defjoint[mjCOMPKIND_STRETCH][0].spec);
mjm_setDefault(jnt->element, mjm_getDefault(body->element));
mjsJoint* jnt = mjs_addJoint(body, &defjoint[mjCOMPKIND_STRETCH][0].spec);
mjs_setDefault(jnt->element, mjs_getDefault(body->element));
mju::sprintf_arr(txt, "%sJS%d", prefix.c_str(), ix1);
mjm_setString(jnt->name, txt);
mjs_setString(jnt->name, txt);
jnt->type = mjJNT_SLIDE;
mjuu_setvec(jnt->pos, -0.5*dx, 0, 0);
mjuu_setvec(jnt->axis, 1, 0, 0);
// add constraint
mjmEquality* eq = mjm_addEquality(&model->spec, &def[mjCOMPKIND_STRETCH].spec);
mjm_setDefault(eq->element, &model->defaults[0]->spec);
mjsEquality* eq = mjs_addEquality(&model->spec, &def[mjCOMPKIND_STRETCH].spec);
mjs_setDefault(eq->element, &model->defaults[0]->spec);
eq->type = mjEQ_JOINT;
mjm_setString(eq->name1, mjm_getString(jnt->name));
mjs_setString(eq->name1, mjs_getString(jnt->name));
}
return body;
@@ -1110,7 +1110,7 @@ void mjCComposite::BoxProject(double* pos) {
// make 3d box, ellipsoid or cylinder
bool mjCComposite::MakeBox(mjCModel* model, mjmBody* body, char* error, int error_sz) {
bool mjCComposite::MakeBox(mjCModel* model, mjsBody* body, char* error, int error_sz) {
char txt[100];
// check dim
@@ -1119,11 +1119,11 @@ bool mjCComposite::MakeBox(mjCModel* model, mjmBody* body, char* error, int erro
}
// center geom: two times bigger
mjmGeom* geom = mjm_addGeom(body, &def[0].spec);
mjm_setDefault(geom->element, mjm_getDefault(body->element));
mjsGeom* geom = mjs_addGeom(body, &def[0].spec);
mjs_setDefault(geom->element, mjs_getDefault(body->element));
geom->type = mjGEOM_SPHERE;
mju::sprintf_arr(txt, "%sGcenter", prefix.c_str());
mjm_setString(geom->name, txt);
mjs_setString(geom->name, txt);
mjuu_setvec(geom->pos, 0, 0, 0);
geom->size[0] *= 2;
geom->size[1] = 0;
@@ -1143,9 +1143,9 @@ bool mjCComposite::MakeBox(mjCModel* model, mjmBody* body, char* error, int erro
iy==0 || iy==count[1]-1 ||
iz==0 || iz==count[2]-1) {
// create body
mjmBody* b = mjm_addBody(body, NULL);
mjsBody* b = mjs_addBody(body, NULL);
mju::sprintf_arr(txt, "%sB%d_%d_%d", prefix.c_str(), ix, iy, iz);
mjm_setString(b->name, txt);
mjs_setString(b->name, txt);
// set body position (+/- 1)
b->pos[0] = 2.0*ix/(count[0]-1) - 1;
@@ -1160,10 +1160,10 @@ bool mjCComposite::MakeBox(mjCModel* model, mjmBody* body, char* error, int erro
mjuu_normvec(b->alt.zaxis, 3);
// add geom
mjmGeom* g = mjm_addGeom(b, &def[0].spec);
mjm_setDefault(g->element, mjm_getDefault(body->element));
mjsGeom* g = mjs_addGeom(b, &def[0].spec);
mjs_setDefault(g->element, mjs_getDefault(body->element));
mju::sprintf_arr(txt, "%sG%d_%d_%d", prefix.c_str(), ix, iy, iz);
mjm_setString(g->name, txt);
mjs_setString(g->name, txt);
// offset inwards, enforce sphere or capsule
if (g->type==mjGEOM_CAPSULE) {
@@ -1174,22 +1174,22 @@ bool mjCComposite::MakeBox(mjCModel* model, mjmBody* body, char* error, int erro
}
// add slider joint
mjmJoint* jnt = mjm_addJoint(b, &defjoint[mjCOMPKIND_JOINT][0].spec);
mjm_setDefault(jnt->element, mjm_getDefault(body->element));
mjsJoint* jnt = mjs_addJoint(b, &defjoint[mjCOMPKIND_JOINT][0].spec);
mjs_setDefault(jnt->element, mjs_getDefault(body->element));
mju::sprintf_arr(txt, "%sJ%d_%d_%d", prefix.c_str(), ix, iy, iz);
mjm_setString(jnt->name, txt);
mjs_setString(jnt->name, txt);
jnt->type = mjJNT_SLIDE;
mjuu_setvec(jnt->pos, 0, 0, 0);
mjuu_setvec(jnt->axis, 0, 0, 1);
// add fix constraint
mjmEquality* eq = mjm_addEquality(&model->spec, &def[mjCOMPKIND_JOINT].spec);
mjm_setDefault(eq->element, &model->defaults[0]->spec);
mjsEquality* eq = mjs_addEquality(&model->spec, &def[mjCOMPKIND_JOINT].spec);
mjs_setDefault(eq->element, &model->defaults[0]->spec);
eq->type = mjEQ_JOINT;
mjm_setString(eq->name1, mjm_getString(jnt->name));
mjs_setString(eq->name1, mjs_getString(jnt->name));
// add joint to tendon
ten->WrapJoint(std::string(mjm_getString(jnt->name)), 1);
ten->WrapJoint(std::string(mjs_getString(jnt->name)), 1);
// add neighbor constraints
for (int i=0; i<3; i++) {
@@ -1203,12 +1203,12 @@ bool mjCComposite::MakeBox(mjCModel* model, mjmBody* body, char* error, int erro
char txt2[200];
mju::sprintf_arr(txt2,
"%sJ%d_%d_%d", prefix.c_str(), ix1, iy1, iz1);
mjmEquality* eqn = mjm_addEquality(&model->spec, 0);
mjsEquality* eqn = mjs_addEquality(&model->spec, 0);
mju_copy(eqn->solref, solrefsmooth, mjNREF);
mju_copy(eqn->solimp, solimpsmooth, mjNIMP);
eqn->type = mjEQ_JOINT;
mjm_setString(eqn->name1, txt);
mjm_setString(eqn->name2, txt2);
mjs_setString(eqn->name1, txt);
mjs_setString(eqn->name2, txt2);
}
}
}
@@ -1217,10 +1217,10 @@ bool mjCComposite::MakeBox(mjCModel* model, mjmBody* body, char* error, int erro
}
// finalize fixed tendon
mjmEquality* eqt = mjm_addEquality(&model->spec, &def[mjCOMPKIND_TENDON].spec);
mjm_setDefault(eqt->element, &model->defaults[0]->spec);
mjsEquality* eqt = mjs_addEquality(&model->spec, &def[mjCOMPKIND_TENDON].spec);
mjs_setDefault(eqt->element, &model->defaults[0]->spec);
eqt->type = mjEQ_TENDON;
mjm_setString(eqt->name1, ten->name.c_str());
mjs_setString(eqt->name1, ten->name.c_str());
// skin
if (skin) {
@@ -1253,10 +1253,10 @@ void mjCComposite::MakeShear(mjCModel* model) {
ten->name = txt;
// equality constraint
mjmEquality* eq = mjm_addEquality(&model->spec, &def[mjCOMPKIND_SHEAR].spec);
mjm_setDefault(eq->element, &model->defaults[0]->spec);
mjsEquality* eq = mjs_addEquality(&model->spec, &def[mjCOMPKIND_SHEAR].spec);
mjs_setDefault(eq->element, &model->defaults[0]->spec);
eq->type = mjEQ_TENDON;
mjm_setString(eq->name1, txt);
mjs_setString(eq->name1, txt);
}
}
}
@@ -1264,18 +1264,18 @@ void mjCComposite::MakeShear(mjCModel* model) {
// copy local vectors to skin
void mjCComposite::CopyIntoSkin(mjmSkin* skin) {
mjm_setInt(skin->face, face.data(), face.size());
mjm_setFloat(skin->vert, vert.data(), vert.size());
mjm_setFloat(skin->bindpos, bindpos.data(), bindpos.size());
mjm_setFloat(skin->bindquat, bindquat.data(), bindquat.size());
mjm_setFloat(skin->texcoord, texcoord.data(), texcoord.size());
void mjCComposite::CopyIntoSkin(mjsSkin* skin) {
mjs_setInt(skin->face, face.data(), face.size());
mjs_setFloat(skin->vert, vert.data(), vert.size());
mjs_setFloat(skin->bindpos, bindpos.data(), bindpos.size());
mjs_setFloat(skin->bindquat, bindquat.data(), bindquat.size());
mjs_setFloat(skin->texcoord, texcoord.data(), texcoord.size());
for (int i=0; i<vertid.size(); i++) {
mjm_appendIntVec(skin->vertid, vertid[i].data(), vertid[i].size());
mjs_appendIntVec(skin->vertid, vertid[i].data(), vertid[i].size());
}
for (int i=0; i< vertweight.size(); i++) {
mjm_appendFloatVec(skin->vertweight, vertweight[i].data(), vertweight[i].size());
mjs_appendFloatVec(skin->vertweight, vertweight[i].data(), vertweight[i].size());
}
face.clear();
@@ -1295,10 +1295,10 @@ void mjCComposite::MakeSkin2(mjCModel* model, mjtNum inflate) {
int N = count[0]*count[1];
// add skin, set name and material
mjmSkin* skin = mjm_addSkin(&model->spec);
mjsSkin* skin = mjs_addSkin(&model->spec);
mju::sprintf_arr(txt, "%sSkin", prefix.c_str());
mjm_setString(skin->name, txt);
mjm_setString(skin->material, skinmaterial.c_str());
mjs_setString(skin->name, txt);
mjs_setString(skin->material, skinmaterial.c_str());
mjuu_copyvec(skin->rgba, skinrgba, 4);
skin->inflate = inflate;
skin->group = skingroup;
@@ -1316,7 +1316,7 @@ void mjCComposite::MakeSkin2(mjCModel* model, mjtNum inflate) {
vert.push_back(0);
mju::sprintf_arr(txt, "%sB%d", prefix.c_str(), i);
mjm_appendString(skin->bodyname, txt);
mjs_appendString(skin->bodyname, txt);
bindpos.push_back(0);
bindpos.push_back(0);
bindpos.push_back(0);
@@ -1426,7 +1426,7 @@ void mjCComposite::MakeSkin2(mjCModel* model, mjtNum inflate) {
// add bones in 2D
void mjCComposite::MakeClothBones(mjCModel* model, mjmSkin* skin) {
void mjCComposite::MakeClothBones(mjCModel* model, mjsSkin* skin) {
char txt[100];
int N = count[0]*count[1];
@@ -1441,7 +1441,7 @@ void mjCComposite::MakeClothBones(mjCModel* model, mjmSkin* skin) {
}
// bind pose
mjm_appendString(skin->bodyname, txt);
mjs_appendString(skin->bodyname, txt);
bindpos.push_back(0);
bindpos.push_back(0);
bindpos.push_back(0);
@@ -1459,7 +1459,7 @@ void mjCComposite::MakeClothBones(mjCModel* model, mjmSkin* skin) {
void mjCComposite::MakeClothBonesSubgrid(mjCModel* model, mjmSkin* skin) {
void mjCComposite::MakeClothBonesSubgrid(mjCModel* model, mjsSkin* skin) {
char txt[100];
// populate bones
@@ -1473,7 +1473,7 @@ void mjCComposite::MakeClothBonesSubgrid(mjCModel* model, mjmSkin* skin) {
}
// bind pose
mjm_appendString(skin->bodyname, txt);
mjs_appendString(skin->bodyname, txt);
bindpos.push_back(ix*spacing);
bindpos.push_back(iy*spacing);
bindpos.push_back(0);
@@ -1492,7 +1492,7 @@ void mjCComposite::MakeClothBonesSubgrid(mjCModel* model, mjmSkin* skin) {
// add bones to 1D
void mjCComposite::MakeCableBones(mjCModel* model, mjmSkin* skin) {
void mjCComposite::MakeCableBones(mjCModel* model, mjsSkin* skin) {
char this_body[100];
int N = count[0]*count[1];
@@ -1510,14 +1510,14 @@ void mjCComposite::MakeCableBones(mjCModel* model, mjmSkin* skin) {
// bind pose
if (iy==0) {
mjm_appendString(skin->bodyname, this_body);
mjs_appendString(skin->bodyname, this_body);
bindpos.push_back((ix==count[0]-1) ? -2*def[0].spec.geom->size[0] : 0);
bindpos.push_back(-def[0].spec.geom->size[1]);
bindpos.push_back(0);
bindquat.push_back(1); bindquat.push_back(0);
bindquat.push_back(0); bindquat.push_back(0);
} else {
mjm_appendString(skin->bodyname, this_body);
mjs_appendString(skin->bodyname, this_body);
bindpos.push_back((ix==count[0]-1) ? -2*def[0].spec.geom->size[0] : 0);
bindpos.push_back(def[0].spec.geom->size[1]);
bindpos.push_back(0);
@@ -1534,7 +1534,7 @@ void mjCComposite::MakeCableBones(mjCModel* model, mjmSkin* skin) {
void mjCComposite::MakeCableBonesSubgrid(mjCModel* model, mjmSkin* skin) {
void mjCComposite::MakeCableBonesSubgrid(mjCModel* model, mjsSkin* skin) {
// populate bones
for (int ix=0; ix<count[0]; ix++) {
for (int iy=0; iy<count[1]; iy++) {
@@ -1563,7 +1563,7 @@ void mjCComposite::MakeCableBonesSubgrid(mjCModel* model, mjmSkin* skin) {
bindpos.push_back(0);
bindpos.push_back(0);
}
mjm_appendString(skin->bodyname, txt);
mjs_appendString(skin->bodyname, txt);
bindquat.push_back(1);
bindquat.push_back(0);
bindquat.push_back(0);
@@ -1888,10 +1888,10 @@ void mjCComposite::MakeSkin2Subgrid(mjCModel* model, mjtNum inflate) {
// add skin, set name and material
char txt[100];
mjmSkin* skin = mjm_addSkin(&model->spec);
mjsSkin* skin = mjs_addSkin(&model->spec);
mju::sprintf_arr(txt, "%sSkin", prefix.c_str());
mjm_setString(skin->name, txt);
mjm_setString(skin->material, skinmaterial.c_str());
mjs_setString(skin->name, txt);
mjs_setString(skin->material, skinmaterial.c_str());
mjuu_copyvec(skin->rgba, skinrgba, 4);
skin->inflate = inflate;
skin->group = skingroup;
@@ -2043,10 +2043,10 @@ void mjCComposite::MakeSkin3(mjCModel* model) {
mju::sprintf_arr(cnt2, "%d", count[2]-1);
// add skin, set name and material
mjmSkin* skin = mjm_addSkin(&model->spec);
mjsSkin* skin = mjs_addSkin(&model->spec);
mju::sprintf_arr(txt, "%sSkin", prefix.c_str());
mjm_setString(skin->name, txt);
mjm_setString(skin->material, skinmaterial.c_str());
mjs_setString(skin->name, txt);
mjs_setString(skin->material, skinmaterial.c_str());
mjuu_copyvec(skin->rgba, skinrgba, 4);
skin->inflate = skininflate;
skin->group = skingroup;
@@ -2191,7 +2191,7 @@ void mjCComposite::MakeSkin3(mjCModel* model) {
// make one face of 3D skin, box
void mjCComposite::MakeSkin3Box(mjmSkin* skin, int c0, int c1, int side,
void mjCComposite::MakeSkin3Box(mjsSkin* skin, int c0, int c1, int side,
int& vcnt, const char* format) {
char txt[100];
@@ -2224,7 +2224,7 @@ void mjCComposite::MakeSkin3Box(mjmSkin* skin, int c0, int c1, int side,
mju::sprintf_arr(txt, format, prefix.c_str(), i0, i1);
// bind pose: origin
mjm_appendString(skin->bodyname, txt);
mjs_appendString(skin->bodyname, txt);
bindpos.push_back(0);
bindpos.push_back(0);
bindpos.push_back(0);
@@ -2246,7 +2246,7 @@ void mjCComposite::MakeSkin3Box(mjmSkin* skin, int c0, int c1, int side,
// make one face of 3D skin, smooth
void mjCComposite::MakeSkin3Smooth(mjmSkin* skin, int c0, int c1, int side,
void mjCComposite::MakeSkin3Smooth(mjsSkin* skin, int c0, int c1, int side,
const std::map<string, int>& vmap, const char* format) {
char txt00[100], txt01[100], txt10[100], txt11[100];
@@ -2281,7 +2281,7 @@ void mjCComposite::MakeSkin3Smooth(mjmSkin* skin, int c0, int c1, int side,
}
// bind pose: origin
mjm_appendString(skin->bodyname, txt00);
mjs_appendString(skin->bodyname, txt00);
bindpos.push_back(0);
bindpos.push_back(0);
bindpos.push_back(0);
src/user/user_composite.h
+16 -16
View File
@@ -69,25 +69,25 @@ class mjCComposite {
bool AddDefaultJoint(char* error = NULL, int error_sz = 0);
void AdjustSoft(mjtNum* solref, mjtNum* solimp, int level);
bool Make(mjSpec* spec, mjmBody* body, char* error, int error_sz);
bool Make(mjSpec* spec, mjsBody* body, char* error, int error_sz);
bool MakeParticle(mjCModel* model, mjmBody* body, char* error, int error_sz);
bool MakeGrid(mjCModel* model, mjmBody* body, char* error, int error_sz);
bool MakeRope(mjCModel* model, mjmBody* body, char* error, int error_sz);
bool MakeCable(mjCModel* model, mjmBody* body, char* error, int error_sz);
bool MakeBox(mjCModel* model, mjmBody* body, char* error, int error_sz);
bool MakeParticle(mjCModel* model, mjsBody* body, char* error, int error_sz);
bool MakeGrid(mjCModel* model, mjsBody* body, char* error, int error_sz);
bool MakeRope(mjCModel* model, mjsBody* body, char* error, int error_sz);
bool MakeCable(mjCModel* model, mjsBody* body, char* error, int error_sz);
bool MakeBox(mjCModel* model, mjsBody* body, char* error, int error_sz);
void MakeShear(mjCModel* model);
void MakeSkin2(mjCModel* model, mjtNum inflate);
void MakeSkin2Subgrid(mjCModel* model, mjtNum inflate);
void MakeClothBones(mjCModel* model, mjmSkin* skin);
void MakeClothBonesSubgrid(mjCModel* model, mjmSkin* skin);
void MakeCableBones(mjCModel* model, mjmSkin* skin);
void MakeCableBonesSubgrid(mjCModel* model, mjmSkin* skin);
void MakeClothBones(mjCModel* model, mjsSkin* skin);
void MakeClothBonesSubgrid(mjCModel* model, mjsSkin* skin);
void MakeCableBones(mjCModel* model, mjsSkin* skin);
void MakeCableBonesSubgrid(mjCModel* model, mjsSkin* skin);
void MakeSkin3(mjCModel* model);
void MakeSkin3Box(mjmSkin* skin, int c0, int c1, int side, int& vcnt, const char* format);
void MakeSkin3Smooth(mjmSkin* skin, int c0, int c1, int side,
void MakeSkin3Box(mjsSkin* skin, int c0, int c1, int side, int& vcnt, const char* format);
void MakeSkin3Smooth(mjsSkin* skin, int c0, int c1, int side,
const std::map<std::string, int>& vmap, const char* format);
void BoxProject(double* pos);
@@ -116,7 +116,7 @@ class mjCComposite {
// plugin support
std::string plugin_name;
std::string plugin_instance_name;
mjmPlugin plugin;
mjsPlugin plugin;
// skin
bool skin; // generate skin
@@ -136,11 +136,11 @@ class mjCComposite {
int dim; // dimensionality
private:
mjmBody* AddRopeBody(mjCModel* model, mjmBody* body, int ix, int ix1);
mjmBody* AddCableBody(mjCModel* model, mjmBody* body, int ix, mjtNum normal[3], mjtNum prev_quat[4]);
mjsBody* AddRopeBody(mjCModel* model, mjsBody* body, int ix, int ix1);
mjsBody* AddCableBody(mjCModel* model, mjsBody* body, int ix, mjtNum normal[3], mjtNum prev_quat[4]);
// temporary skin vectors
void CopyIntoSkin(mjmSkin* skin);
void CopyIntoSkin(mjsSkin* skin);
std::vector<int> face;
std::vector<float> vert;
std::vector<float> bindpos;
src/user/user_flexcomp.cc
+502 -241
View File
File diff suppressed because it is too large Load Diff
src/user/user_flexcomp.h
+8 -3
View File
@@ -42,7 +42,7 @@ typedef enum _mjtFcompType {
class mjCFlexcomp {
public:
mjCFlexcomp(void);
bool Make(mjSpec* spec, mjmBody* body, char* error, int error_sz);
bool Make(mjSpec* spec, mjsBody* body, char* error, int error_sz);
bool MakeGrid(char* error, int error_sz);
bool MakeBox(char* error, int error_sz);
@@ -50,6 +50,11 @@ class mjCFlexcomp {
bool MakeMesh(mjCModel* model, char* error, int error_sz);
bool MakeGMSH(mjCModel* model, char* error, int error_sz);
void LoadGMSH(mjCModel* model, mjResource* resource);
void LoadGMSH41(char* buffer, int binary, int nodeend, int nodebegin,
int elemend, int elembegin);
void LoadGMSH22(char* buffer, int binary, int nodeend, int nodebegin,
int elemend, int elembegin);
int GridID(int ix, int iy);
int GridID(int ix, int iy, int iz);
@@ -79,7 +84,7 @@ class mjCFlexcomp {
// pose transform relative to parent body
double pos[3]; // position
double quat[4]; // orientation
mjCAlternative alt; // alternative orientation
mjsOrientation alt; // alternative orientation
// set by user or computed internally
bool rigid; // all vertices are in parent body (all pinned)
@@ -93,7 +98,7 @@ class mjCFlexcomp {
// plugin support
std::string plugin_name;
std::string plugin_instance_name;
mjmPlugin plugin;
mjsPlugin plugin;
};
#endif // MUJOCO_SRC_USER_USER_FLEXCOMP_H_
src/user/user_init.cc
+54 -47
View File
@@ -22,7 +22,7 @@
// default model attributes
void mjm_defaultSpec(mjSpec& model) {
void mjs_defaultSpec(mjSpec& model) {
memset(&model, 0, sizeof(mjSpec));
// default statistics
@@ -75,9 +75,16 @@ void mjm_defaultSpec(mjSpec& model) {
// default orientation attributes
void mjs_defaultOrientation(mjsOrientation& orient) {
orient.axisangle[0] = orient.xyaxes[0] = orient.zaxis[0] = orient.euler[0] = mjNAN;
}
// default body attributes
void mjm_defaultBody(mjmBody& body) {
memset(&body, 0, sizeof(mjmBody));
void mjs_defaultBody(mjsBody& body) {
memset(&body, 0, sizeof(mjsBody));
// body frame
body.pos[0] = mjNAN;
@@ -94,8 +101,8 @@ void mjm_defaultBody(mjmBody& body) {
// default frame attributes
void mjm_defaultFrame(mjmFrame& frame) {
memset(&frame, 0, sizeof(mjmFrame));
void mjs_defaultFrame(mjsFrame& frame) {
memset(&frame, 0, sizeof(mjsFrame));
mju_zero3(frame.pos);
mjuu_setvec(frame.quat, 1, 0, 0, 0);
frame.alt.axisangle[0] = frame.alt.xyaxes[0] = frame.alt.zaxis[0] = frame.alt.euler[0] = mjNAN;
@@ -104,8 +111,8 @@ void mjm_defaultFrame(mjmFrame& frame) {
// default joint attributes
void mjm_defaultJoint(mjmJoint& joint) {
memset(&joint, 0, sizeof(mjmJoint));
void mjs_defaultJoint(mjsJoint& joint) {
memset(&joint, 0, sizeof(mjsJoint));
joint.type = mjJNT_HINGE;
joint.axis[2] = 1;
@@ -119,8 +126,8 @@ void mjm_defaultJoint(mjmJoint& joint) {
// default geom attributes
void mjm_defaultGeom(mjmGeom& geom) {
memset(&geom, 0, sizeof(mjmGeom));
void mjs_defaultGeom(mjsGeom& geom) {
memset(&geom, 0, sizeof(mjsGeom));
// type
geom.type = mjGEOM_SPHERE;
@@ -163,9 +170,9 @@ void mjm_defaultGeom(mjmGeom& geom) {
// default site attributes
void mjm_defaultSite(mjmSite& site) {
void mjs_defaultSite(mjsSite& site) {
// set everything to 0 / false / NULL
memset(&site, 0, sizeof(mjmSite));
memset(&site, 0, sizeof(mjsSite));
// type
site.type = mjGEOM_SPHERE;
@@ -184,8 +191,8 @@ void mjm_defaultSite(mjmSite& site) {
// default cam attributes
void mjm_defaultCamera(mjmCamera& cam) {
memset(&cam, 0, sizeof(mjmCamera));
void mjs_defaultCamera(mjsCamera& cam) {
memset(&cam, 0, sizeof(mjsCamera));
// type
cam.mode = mjCAMLIGHT_FIXED;
@@ -203,8 +210,8 @@ void mjm_defaultCamera(mjmCamera& cam) {
// default light attributes
void mjm_defaultLight(mjmLight& light) {
memset(&light, 0, sizeof(mjmLight));
void mjs_defaultLight(mjsLight& light) {
memset(&light, 0, sizeof(mjsLight));
// mode
light.mode = mjCAMLIGHT_FIXED;
@@ -223,8 +230,8 @@ void mjm_defaultLight(mjmLight& light) {
// default flex attributes
void mjm_defaultFlex(mjmFlex& flex) {
memset(&flex, 0, sizeof(mjmFlex));
void mjs_defaultFlex(mjsFlex& flex) {
memset(&flex, 0, sizeof(mjsFlex));
// set contact defaults
flex.contype = 1;
@@ -247,8 +254,8 @@ void mjm_defaultFlex(mjmFlex& flex) {
// default mesh attributes
void mjm_defaultMesh(mjmMesh& mesh) {
memset(&mesh, 0, sizeof(mjmMesh));
void mjs_defaultMesh(mjsMesh& mesh) {
memset(&mesh, 0, sizeof(mjsMesh));
mjuu_setvec(mesh.refpos, 0, 0, 0);
mjuu_setvec(mesh.refquat, 1, 0, 0, 0);
mjuu_setvec(mesh.scale, 1, 1, 1);
@@ -258,15 +265,15 @@ void mjm_defaultMesh(mjmMesh& mesh) {
// default height field attributes
void mjm_defaultHField(mjmHField& hfield) {
memset(&hfield, 0, sizeof(mjmHField));
void mjs_defaultHField(mjsHField& hfield) {
memset(&hfield, 0, sizeof(mjsHField));
}
// default skin attributes
void mjm_defaultSkin(mjmSkin& skin) {
memset(&skin, 0, sizeof(mjmSkin));
void mjs_defaultSkin(mjsSkin& skin) {
memset(&skin, 0, sizeof(mjsSkin));
skin.rgba[0] = skin.rgba[1] = skin.rgba[2] = 0.5f;
skin.rgba[3] = 1.0f;
skin.inflate = 0;
@@ -276,8 +283,8 @@ void mjm_defaultSkin(mjmSkin& skin) {
// default texture attributes
void mjm_defaultTexture(mjmTexture& texture) {
memset(&texture, 0, sizeof(mjmTexture));
void mjs_defaultTexture(mjsTexture& texture) {
memset(&texture, 0, sizeof(mjsTexture));
texture.type = mjTEXTURE_CUBE;
mjuu_setvec(texture.rgb1, 0.8, 0.8, 0.8);
mjuu_setvec(texture.rgb2, 0.5, 0.5, 0.5);
@@ -290,8 +297,8 @@ void mjm_defaultTexture(mjmTexture& texture) {
// default material attributes
void mjm_defaultMaterial(mjmMaterial& material) {
memset(&material, 0, sizeof(mjmMaterial));
void mjs_defaultMaterial(mjsMaterial& material) {
memset(&material, 0, sizeof(mjsMaterial));
material.texuniform = false;
material.texrepeat[0] = material.texrepeat[1] = 1;
material.emission = 0;
@@ -304,8 +311,8 @@ void mjm_defaultMaterial(mjmMaterial& material) {
// default pair attributes
void mjm_defaultPair(mjmPair& pair) {
memset(&pair, 0, sizeof(mjmPair));
void mjs_defaultPair(mjsPair& pair) {
memset(&pair, 0, sizeof(mjsPair));
pair.condim = 3;
mj_defaultSolRefImp(pair.solref, pair.solimp);
pair.friction[0] = 1;
@@ -318,8 +325,8 @@ void mjm_defaultPair(mjmPair& pair) {
// default equality attributes
void mjm_defaultEquality(mjmEquality& equality) {
memset(&equality, 0, sizeof(mjmEquality));
void mjs_defaultEquality(mjsEquality& equality) {
memset(&equality, 0, sizeof(mjsEquality));
equality.type = mjEQ_CONNECT;
equality.active = 1;
mj_defaultSolRefImp(equality.solref, equality.solimp);
@@ -330,8 +337,8 @@ void mjm_defaultEquality(mjmEquality& equality) {
// default tendon attributes
void mjm_defaultTendon(mjmTendon& tendon) {
memset(&tendon, 0, sizeof(mjmTendon));
void mjs_defaultTendon(mjsTendon& tendon) {
memset(&tendon, 0, sizeof(mjsTendon));
tendon.limited = mjLIMITED_AUTO;
tendon.springlength[0] = tendon.springlength[1] = -1;
mj_defaultSolRefImp(tendon.solref_limit, tendon.solimp_limit);
@@ -344,8 +351,8 @@ void mjm_defaultTendon(mjmTendon& tendon) {
// default actuator attributes
void mjm_defaultActuator(mjmActuator& actuator) {
memset(&actuator, 0, sizeof(mjmActuator));
void mjs_defaultActuator(mjsActuator& actuator) {
memset(&actuator, 0, sizeof(mjsActuator));
// gain, bias
actuator.gaintype = mjGAIN_FIXED;
@@ -370,8 +377,8 @@ void mjm_defaultActuator(mjmActuator& actuator) {
// default sensor attributes
void mjm_defaultSensor(mjmSensor& sensor) {
memset(&sensor, 0, sizeof(mjmSensor));
void mjs_defaultSensor(mjsSensor& sensor) {
memset(&sensor, 0, sizeof(mjsSensor));
sensor.type = mjSENS_TOUCH;
sensor.datatype = mjDATATYPE_REAL;
sensor.needstage = mjSTAGE_ACC;
@@ -380,36 +387,36 @@ void mjm_defaultSensor(mjmSensor& sensor) {
// Default numeric attributes.
void mjm_defaultNumeric(mjmNumeric& numeric) {
memset(&numeric, 0, sizeof(mjmNumeric));
void mjs_defaultNumeric(mjsNumeric& numeric) {
memset(&numeric, 0, sizeof(mjsNumeric));
}
// Default text attributes.
void mjm_defaultText(mjmText& text) {
memset(&text, 0, sizeof(mjmText));
void mjs_defaultText(mjsText& text) {
memset(&text, 0, sizeof(mjsText));
}
// Default tuple attributes.
void mjm_defaultTuple(mjmTuple& tuple) {
memset(&tuple, 0, sizeof(mjmTuple));
void mjs_defaultTuple(mjsTuple& tuple) {
memset(&tuple, 0, sizeof(mjsTuple));
}
// Default keyframe attributes.
void mjm_defaultKey(mjmKey& key) {
memset(&key, 0, sizeof(mjmKey));
void mjs_defaultKey(mjsKey& key) {
memset(&key, 0, sizeof(mjsKey));
}
// default plugin attributes
void mjm_defaultPlugin(mjmPlugin& plugin) {
memset(&plugin, 0, sizeof(mjmPlugin));
void mjs_defaultPlugin(mjsPlugin& plugin) {
memset(&plugin, 0, sizeof(mjsPlugin));
plugin.plugin_slot = -1;
}
src/user/user_mesh.cc
+235 -31
View File
@@ -13,6 +13,7 @@
// limitations under the License.
#include <algorithm>
#include <climits>
#include <cmath>
#include <csetjmp>
#include <cstddef>
@@ -60,6 +61,7 @@
#include "engine/engine_util_misc.h"
#include "engine/engine_util_solve.h"
#include "engine/engine_util_spatial.h"
#include "user/user_cache.h"
#include "user/user_model.h"
#include "user/user_objects.h"
#include "user/user_util.h"
@@ -129,7 +131,8 @@ static void ReadFromBuffer(T* dst, const char* src) {
//------------------ class mjCMesh implementation --------------------------------------------------
mjCMesh::mjCMesh(mjCModel* _model, mjCDef* _def) {
mjm_defaultMesh(spec);
mjs_defaultMesh(spec);
elemtype = mjOBJ_MESH;
// clear internal variables
mjuu_setvec(pos_surface_, 0, 0, 0);
@@ -194,46 +197,67 @@ mjCMesh& mjCMesh::operator=(const mjCMesh& other) {
if (this != &other) {
this->spec = other.spec;
*static_cast<mjCMesh_*>(this) = static_cast<const mjCMesh_&>(other);
*static_cast<mjmMesh*>(this) = static_cast<const mjmMesh&>(other);
*static_cast<mjsMesh*>(this) = static_cast<const mjsMesh&>(other);
if (other.vert_) {
size_t nvert = 3*other.nvert_*sizeof(float);
this->vert_ = (float*)mju_malloc(nvert);
memcpy(this->vert_, other.vert_, nvert);
} else {
this->vert_ = NULL;
}
if (other.normal_) {
size_t nnormal = 3*other.nnormal_*sizeof(float);
this->normal_ = (float*)mju_malloc(nnormal);
memcpy(this->normal_, other.normal_, nnormal);
} else {
this->normal_ = NULL;
}
if (other.center_) {
size_t ncenter = 3*other.nface_*sizeof(double);
this->center_ = (double*)mju_malloc(ncenter);
memcpy(this->center_, other.center_, ncenter);
} else {
this->center_ = NULL;
}
if (other.texcoord_) {
size_t ntexcoord = 2*other.ntexcoord_*sizeof(float);
this->texcoord_ = (float*)mju_malloc(ntexcoord);
memcpy(this->texcoord_, other.texcoord_, ntexcoord);
} else {
this->texcoord_ = NULL;
}
if (other.face_) {
size_t nface = 3*other.nface_*sizeof(int);
this->face_ = (int*)mju_malloc(nface);
memcpy(this->face_, other.face_, nface);
} else {
this->face_ = NULL;
}
if (other.facenormal_) {
size_t nfacenormal = 3*other.nface_*sizeof(int);
this->facenormal_ = (int*)mju_malloc(nfacenormal);
memcpy(this->facenormal_, other.facenormal_, nfacenormal);
} else {
this->facenormal_ = NULL;
}
if (other.facetexcoord_) {
size_t nfacetexcoord = 3*other.nface_*sizeof(int);
this->facetexcoord_ = (int*)mju_malloc(nfacetexcoord);
memcpy(this->facetexcoord_, other.facetexcoord_, nfacetexcoord);
} else {
this->facetexcoord_ = NULL;
}
if (other.graph_) {
size_t szgraph = szgraph_*sizeof(int);
this->graph_ = (int*)mju_malloc(szgraph);
memcpy(this->graph_, other.graph_, szgraph);
} else {
this->graph_ = NULL;
}
if (other.plugin.instance) {
mjCPlugin* new_plugin = new mjCPlugin(*static_cast<mjCPlugin*>(other.plugin.instance));
plugin = new_plugin->spec;
model->plugins.push_back(new_plugin);
}
}
PointToLocal();
@@ -243,7 +267,7 @@ mjCMesh& mjCMesh::operator=(const mjCMesh& other) {
void mjCMesh::PointToLocal() {
spec.element = (mjElement)this;
spec.element = static_cast<mjElement*>(this);
spec.name = (mjString)&name;
spec.classname = (mjString)&classname;
spec.file = (mjString)&spec_file_;
@@ -260,7 +284,7 @@ void mjCMesh::PointToLocal() {
void mjCMesh::CopyFromSpec() {
*static_cast<mjmMesh*>(this) = spec;
*static_cast<mjsMesh*>(this) = spec;
file_ = spec_file_;
content_type_ = spec_content_type_;
uservert_ = spec_uservert_;
@@ -336,8 +360,9 @@ void mjCMesh::LoadSDF() {
name.c_str(), id);
}
mjCPlugin* plugin_instance = (mjCPlugin*)plugin.instance;
mjCPlugin* plugin_instance = static_cast<mjCPlugin*>(plugin.instance);
model->ResolvePlugin(this, plugin_name, plugin_instance_name, &plugin_instance);
plugin.instance = plugin_instance;
const mjpPlugin* pplugin = mjp_getPluginAtSlot(plugin_instance->spec.plugin_slot);
if (!(pplugin->capabilityflags & mjPLUGIN_SDF)) {
throw mjCError(this, "plugin '%s' does not support signed distance fields", pplugin->name);
@@ -410,6 +435,7 @@ void mjCMesh::LoadSDF() {
// compiler
void mjCMesh::Compile(const mjVFS* vfs) {
CopyFromSpec();
visual_ = true;
// load file
if (!file_.empty()) {
@@ -526,7 +552,8 @@ void mjCMesh::Compile(const mjVFS* vfs) {
// check vertices exist
for (int i=0; i<userface_.size(); i++) {
if (userface_[i] >= nvert_ || userface_[i] < 0) {
throw mjCError(this, "index in face does not exist in vertex array");
throw mjCError(this, "in face %d, vertex index %d does not exist",
nullptr, i / 3, userface_[i]);
}
}
@@ -967,6 +994,19 @@ void mjCMesh::RemoveRepeated() {
void mjCMesh::LoadOBJ(mjResource* resource) {
tinyobj::ObjReader objReader;
const void* bytes = nullptr;
// try loading from cache
mjCCache *cache = reinterpret_cast<mjCCache*>(mj_globalCache());
if (cache) {
auto asset = cache->Get(resource->name);
if (asset.has_value() &&
!mju_isModifiedResource(resource, asset->Timestamp().c_str())) {
if (LoadCachedOBJ(asset.value())) {
return;
}
}
}
int buffer_sz = mju_readResource(resource, &bytes);
if (buffer_sz < 0) {
throw mjCError(this, "could not read OBJ file '%s'", resource->name);
@@ -1030,9 +1070,117 @@ void mjCMesh::LoadOBJ(mjResource* resource) {
for (int i=0; i<usertexcoord_.size()/2; i++) {
usertexcoord_[2*i+1] = 1-usertexcoord_[2*i+1];
}
// try caching asset
if (cache) {
mjCAsset asset("", resource->name, resource->timestamp);
asset.AddVector("uservert_", uservert_);
asset.AddVector("usernormal_", usernormal_);
asset.AddVector("usertexcoord_", usertexcoord_);
if (!objReader.GetShapes().empty()) {
const auto& mesh = objReader.GetShapes()[0].mesh;
std::vector<int> vertex_index;
vertex_index.reserve(mesh.indices.size());
std::vector<int> normal_index;
normal_index.reserve(mesh.indices.size());
std::vector<int> texcoord_index;
texcoord_index.reserve(mesh.indices.size());
for (tinyobj::index_t index : mesh.indices) {
vertex_index.push_back(index.vertex_index);
normal_index.push_back(index.normal_index);
texcoord_index.push_back(index.texcoord_index);
}
asset.AddVector("num_face_vertices", mesh.num_face_vertices);
asset.AddVector("vertex_index", vertex_index);
asset.AddVector("normal_index", normal_index);
asset.AddVector("texcoord_index", texcoord_index);
} else {
asset.AddVector("num_face_vertices", std::vector<unsigned char>());
asset.AddVector("vertex_index", std::vector<int>());
asset.AddVector("normal_index", std::vector<int>());
asset.AddVector("texcoord_index", std::vector<int>());
}
cache->Insert(std::move(asset));
}
}
// load OBJ from cached asset, return true on success
bool mjCMesh::LoadCachedOBJ(const mjCAsset& asset) {
// check that asset has all data
if (!asset.HasData("uservert_") || !asset.HasData("usernormal_")
|| !asset.HasData("usertexcoord_") || !asset.HasData("num_face_vertices")
|| !asset.HasData("vertex_index") || !asset.HasData("normal_index")
|| !asset.HasData("texcoord_index")) {
return false;
}
uservert_ = asset.GetVector<float>("uservert_").value();
usernormal_ = asset.GetVector<float>("usernormal_").value();
usertexcoord_ = asset.GetVector<float>("usertexcoord_").value();
vector<int> vertex_index = asset.GetVector<int>("vertex_index").value();
vector<int> normal_index = asset.GetVector<int>("normal_index").value();
vector<int> texcoord_index = asset.GetVector<int>("texcoord_index").value();
vector<unsigned char> num_face_vertices =
asset.GetVector<unsigned char>("num_face_vertices").value();
bool righthand = (scale[0] * scale[1] * scale[2]) > 0;
for (int face = 0, i = 0; i < vertex_index.size();) {
int nfacevert = num_face_vertices[face];
if (nfacevert < 3 || nfacevert > 4) {
throw mjCError(
this, "only tri or quad meshes are supported for OBJ (file '%s')",
asset.Id().c_str());
}
userface_.push_back(vertex_index[i]);
userface_.push_back(vertex_index[i + (righthand == 1 ? 1 : 2)]);
userface_.push_back(vertex_index[i + (righthand == 1 ? 2 : 1)]);
if (!usernormal_.empty()) {
userfacenormal_.push_back(normal_index[i]);
userfacenormal_.push_back(normal_index[i + (righthand == 1 ? 1 : 2)]);
userfacenormal_.push_back(normal_index[i + (righthand == 1 ? 2 : 1)]);
}
if (!usertexcoord_.empty()) {
userfacetexcoord_.push_back(texcoord_index[i]);
userfacetexcoord_.push_back(texcoord_index[i + (righthand == 1 ? 1 : 2)]);
userfacetexcoord_.push_back(texcoord_index[i + (righthand == 1 ? 2 : 1)]);
}
if (nfacevert == 4) {
userface_.push_back(vertex_index[i]);
userface_.push_back(vertex_index[i + (righthand == 1 ? 2 : 3)]);
userface_.push_back(vertex_index[i + (righthand == 1 ? 3 : 2)]);
if (!usernormal_.empty()) {
userfacenormal_.push_back(normal_index[i]);
userfacenormal_.push_back(normal_index[i + (righthand == 1 ? 1 : 2)]);
userfacenormal_.push_back(normal_index[i + (righthand == 1 ? 2 : 1)]);
}
if (!usertexcoord_.empty()) {
userfacetexcoord_.push_back(texcoord_index[i]);
userfacetexcoord_.push_back(texcoord_index[i + (righthand == 1 ? 1 : 2)]);
userfacetexcoord_.push_back(texcoord_index[i + (righthand == 1 ? 2 : 1)]);
}
}
i += nfacevert;
++face;
}
return true;
}
// load STL binary mesh
void mjCMesh::LoadSTL(mjResource* resource) {
bool righthand = (scale[0]*scale[1]*scale[2]>0);
@@ -1146,6 +1294,12 @@ void mjCMesh::LoadMSH(mjResource* resource) {
throw mjCError(this, "invalid sizes in MSH file '%s'", resource->name);
}
if (nvert_ >= INT_MAX / sizeof(float) / 3 ||
nnormal_ >= INT_MAX / sizeof(float) / 3 ||
ntexcoord_ >= INT_MAX / sizeof(float) / 2 ||
nface_ >= INT_MAX / sizeof(int) / 3) {
throw mjCError(this, "too large sizes in MSH file '%s'.", resource->name);
}
// check file size
if (buffer_sz != 4*sizeof(int) + 3*nvert_*sizeof(float) + 3*nnormal_*sizeof(float) +
2*ntexcoord_*sizeof(float) + 3*nface_*sizeof(int)) {
@@ -1493,7 +1647,7 @@ void mjCMesh::CheckMesh(mjtGeomInertia type) {
if (!processed_) {
return;
}
if (invalidorientation_.first>=0 || invalidorientation_.second>=0)
if ((invalidorientation_.first>=0 || invalidorientation_.second>=0) && model->exactmeshinertia)
throw mjCError(this,
"faces of mesh '%s' have inconsistent orientation. Please check the "
"faces containing the vertices %d and %d.",
@@ -1910,7 +2064,8 @@ void mjCMesh::MakeCenter(void) {
// constructor
mjCSkin::mjCSkin(mjCModel* _model) {
mjm_defaultSkin(spec);
mjs_defaultSkin(spec);
elemtype = mjOBJ_SKIN;
// set model pointer
model = _model;
@@ -1939,8 +2094,26 @@ mjCSkin::mjCSkin(mjCModel* _model) {
mjCSkin::mjCSkin(const mjCSkin& other) {
*this = other;
}
mjCSkin& mjCSkin::operator=(const mjCSkin& other) {
if (this != &other) {
this->spec = other.spec;
*static_cast<mjCSkin_*>(this) = static_cast<const mjCSkin_&>(other);
*static_cast<mjsSkin*>(this) = static_cast<const mjsSkin&>(other);
}
PointToLocal();
return *this;
}
void mjCSkin::PointToLocal() {
spec.element = (mjElement)this;
spec.element = static_cast<mjElement*>(this);
spec.name = (mjString)&name;
spec.classname = (mjString)&classname;
spec.file = (mjString)&spec_file_;
@@ -1958,8 +2131,16 @@ void mjCSkin::PointToLocal() {
void mjCSkin::NameSpace(const mjCModel* m) {
for (int i=0; i<(int)spec_bodyname_.size(); i++) {
spec_bodyname_[i] = m->prefix + spec_bodyname_[i] + m->suffix;
}
}
void mjCSkin::CopyFromSpec() {
*static_cast<mjmSkin*>(this) = spec;
*static_cast<mjsSkin*>(this) = spec;
file_ = spec_file_;
material_ = spec_material_;
vert_ = spec_vert_;
@@ -2001,6 +2182,20 @@ mjCSkin::~mjCSkin() {
void mjCSkin::ResolveReferences(const mjCModel* m) {
size_t nbone = bodyname_.size();
bodyid.resize(nbone);
for (int i=0; i<nbone; i++) {
mjCBase* pbody = m->FindObject(mjOBJ_BODY, bodyname_[i]);
if (!pbody) {
throw mjCError(this, "unknown body '%s' in skin", bodyname_[i].c_str());
}
bodyid[i] = pbody->id;
}
}
// compiler
void mjCSkin::Compile(const mjVFS* vfs) {
CopyFromSpec();
@@ -2081,14 +2276,7 @@ void mjCSkin::Compile(const mjVFS* vfs) {
}
// resolve body names
bodyid.resize(nbone);
for (int i=0; i<nbone; i++) {
mjCBase* pbody = model->FindObject(mjOBJ_BODY, bodyname_[i]);
if (!pbody) {
throw mjCError(this, "unknown body '%s' in skin", bodyname_[i].c_str());
}
bodyid[i] = pbody->id;
}
ResolveReferences(model);
// resolve material name
mjCBase* pmat = model->FindObject(mjOBJ_MATERIAL, material_);
@@ -2301,7 +2489,8 @@ constexpr int eledge[3][6][2] = {{{ 0, 1}, {-1, -1}, {-1, -1},
// constructor
mjCFlex::mjCFlex(mjCModel* _model) {
mjm_defaultFlex(spec);
mjs_defaultFlex(spec);
elemtype = mjOBJ_FLEX;
// set model
model = _model;
@@ -2328,7 +2517,7 @@ mjCFlex& mjCFlex::operator=(const mjCFlex& other) {
if (this != &other) {
this->spec = other.spec;
*static_cast<mjCFlex_*>(this) = static_cast<const mjCFlex_&>(other);
*static_cast<mjmFlex*>(this) = static_cast<const mjmFlex&>(other);
*static_cast<mjsFlex*>(this) = static_cast<const mjsFlex&>(other);
}
PointToLocal();
return *this;
@@ -2336,7 +2525,7 @@ mjCFlex& mjCFlex::operator=(const mjCFlex& other) {
void mjCFlex::PointToLocal() {
spec.element = (mjElement)this;
spec.element = static_cast<mjElement*>(this);
spec.name = (mjString)&name;
spec.classname = (mjString)&classname;
spec.material = (mjString)&spec_material_;
@@ -2348,8 +2537,17 @@ void mjCFlex::PointToLocal() {
}
void mjCFlex::NameSpace(const mjCModel* m) {
for (int i=0; i<(int)spec_vertbody_.size(); i++) {
spec_vertbody_[i] = m->prefix + spec_vertbody_[i] + m->suffix;
}
}
void mjCFlex::CopyFromSpec() {
*static_cast<mjmFlex*>(this) = spec;
*static_cast<mjsFlex*>(this) = spec;
spec.info = (mjString)&info;
material_ = spec_material_;
vertbody_ = spec_vertbody_;
@@ -2380,6 +2578,18 @@ void mjCFlex::DelTexcoord() {
}
void mjCFlex::ResolveReferences(const mjCModel* m) {
for (int i=0; i<(int)vertbody_.size(); i++) {
mjCBase* pbody = m->FindObject(mjOBJ_BODY, vertbody_[i]);
if (pbody) {
vertbodyid.push_back(pbody->id);
} else {
throw mjCError(this, "unkown body '%s' in flex", vertbody_[i].c_str());
}
}
}
// compiler
void mjCFlex::Compile(const mjVFS* vfs) {
CopyFromSpec();
@@ -2441,14 +2651,7 @@ void mjCFlex::Compile(const mjVFS* vfs) {
}
// resolve body ids
for (int i=0; i<(int)vertbody_.size(); i++) {
mjCBase* pbody = model->FindObject(mjOBJ_BODY, vertbody_[i]);
if (pbody) {
vertbodyid.push_back(pbody->id);
} else {
throw mjCError(this, "unkown body '%s' in flex", vertbody_[i].c_str());
}
}
ResolveReferences(model);
// process elements
for (int e=0; e<(int)elem_.size()/(dim+1); e++) {
@@ -2564,7 +2767,8 @@ void mjCFlex::Compile(const mjVFS* vfs) {
for (int i=0; i<(int)vertbodyid.size(); i++) {
if (model->bodies[vertbodyid[i]]->plugin.instance) {
mjCPlugin* plugin_instance = (mjCPlugin*)model->bodies[vertbodyid[i]]->plugin.instance;
mjCPlugin* plugin_instance =
static_cast<mjCPlugin*>(model->bodies[vertbodyid[i]]->plugin.instance);
plugin_instance->config_attribs["face"] = userface;
plugin_instance->config_attribs["edge"] = useredge;
}
src/user/user_model.cc
+321 -146
View File
@@ -83,7 +83,8 @@ static void copyvec(T1* dest, T2* src, int n) {
// constructor
mjCModel::mjCModel() {
mjm_defaultSpec(spec);
mjs_defaultSpec(spec);
elemtype = mjOBJ_UNKNOWN;
spec_comment_.clear();
spec_modelfiledir_.clear();
spec_meshdir_.clear();
@@ -120,6 +121,8 @@ mjCModel::mjCModel() {
tuples.clear();
keys.clear();
defaults.clear();
prefix = "";
suffix = "";
Clear();
//------------------------ master default set
@@ -138,6 +141,152 @@ mjCModel::mjCModel() {
world->def = defaults[0];
bodies.push_back(world);
// create mjCBase lists from children lists
CreateObjectLists();
// point to model from spec
PointToLocal();
// this class allocated the plugins
plugin_owner = true;
}
mjCModel::mjCModel(const mjCModel& other) {
*this = other;
}
mjCModel& mjCModel::operator=(const mjCModel& other) {
if (this != &other) {
plugin_owner = false;
this->spec = other.spec;
*static_cast<mjCModel_*>(this) = static_cast<const mjCModel_&>(other);
*static_cast<mjSpec*>(this) = static_cast<const mjSpec&>(other);
// the world copy constructor takes care of copying the tree
mjCBody* world = new mjCBody(*other.bodies[0], this);
bodies.push_back(world);
// add everything else
*this += other;
// copy name maps
for (int i=0; i<mjNOBJECT; i++) {
ids[i] = other.ids[i];
}
}
return *this;
}
// copy vector of elements from another model to this model
template <class T>
void mjCModel::CopyList(std::vector<T*>& dest,
const std::vector<T*>& source,
std::map<mjCDef*, int>& def_map,
const std::vector<mjCDef*>& defaults) {
// loop over the elements from the other model
for (T* element : source) {
try {
// try to find the referenced object in this model
element->NameSpace(element->model);
element->CopyFromSpec();
element->ResolveReferences(this);
} catch (mjCError err) {
// if not present, skip the element
continue;
}
// copy the element from the other model to this model
dest.push_back(new T(*element));
dest.back()->model = this;
dest.back()->def = defaults[def_map[element->def]];
}
}
mjCModel& mjCModel::operator+=(const mjCModel& other) {
if (this != &other) {
// create global lists
MakeLists(bodies[0]);
CreateObjectLists();
ProcessLists();
// copy all elements not in the tree
std::map<mjCDef*, int> def_map;
for (int i = 0; i < other.defaults.size(); i++) {
defaults.push_back(new mjCDef(*other.defaults[i]));
def_map[other.defaults[i]] = i;
}
CopyList(flexes, other.flexes, def_map, defaults);
CopyList(meshes, other.meshes, def_map, defaults);
CopyList(skins, other.skins, def_map, defaults);
CopyList(hfields, other.hfields, def_map, defaults);
CopyList(textures, other.textures, def_map, defaults);
CopyList(materials, other.materials, def_map, defaults);
CopyList(pairs, other.pairs, def_map, defaults);
CopyList(excludes, other.excludes, def_map, defaults);
CopyList(tendons, other.tendons, def_map, defaults);
CopyList(equalities, other.equalities, def_map, defaults);
CopyList(actuators, other.actuators, def_map, defaults);
CopyList(sensors, other.sensors, def_map, defaults);
CopyList(numerics, other.numerics, def_map, defaults);
CopyList(texts, other.texts, def_map, defaults);
CopyList(tuples, other.tuples, def_map, defaults);
CopyList(keys, other.keys, def_map, defaults);
// plugins are global
plugins = other.plugins;
active_plugins = other.active_plugins;
// update defaults for the copied objects
for (int i = 1; i < other.bodies.size(); i++) {
bodies[i]->def = defaults[def_map[other.bodies[i]->def]];
}
for (int i = 0; i < other.joints.size(); i++) {
joints[i]->def = defaults[def_map[other.joints[i]->def]];
}
for (int i = 0; i < other.geoms.size(); i++) {
geoms[i]->def = defaults[def_map[other.geoms[i]->def]];
}
for (int i = 0; i < other.sites.size(); i++) {
sites[i]->def = defaults[def_map[other.sites[i]->def]];
}
for (int i = 0; i < other.cameras.size(); i++) {
cameras[i]->def = defaults[def_map[other.cameras[i]->def]];
}
for (int i = 0; i < other.lights.size(); i++) {
lights[i]->def = defaults[def_map[other.lights[i]->def]];
}
// cast children to mjCBase
// restore to the same state as other
if (!compiled) {
mjCBody* world = bodies[0];
bodies.clear();
frames.clear();
joints.clear();
geoms.clear();
sites.clear();
cameras.clear();
lights.clear();
bodies.push_back(world);
}
}
PointToLocal();
return *this;
}
// TODO: we should not use C-type casting with multiple C++ inheritance
void mjCModel::CreateObjectLists() {
for (int i = 0; i < mjNOBJECT; ++i) {
object_lists[i] = nullptr;
}
@@ -166,23 +315,12 @@ mjCModel::mjCModel() {
object_lists[mjOBJ_TUPLE] = (std::vector<mjCBase*>*) &tuples;
object_lists[mjOBJ_KEY] = (std::vector<mjCBase*>*) &keys;
object_lists[mjOBJ_PLUGIN] = (std::vector<mjCBase*>*) &plugins;
// point to model from spec
PointToLocal();
}
mjCModel::mjCModel(const mjCModel& other) {
*this = other;
PointToLocal();
}
void mjCModel::PointToLocal() {
spec.element = (mjElement)this;
spec.element = static_cast<mjElement*>(this);
spec.comment = (mjString)&spec_comment_;
spec.modelfiledir = (mjString)&spec_modelfiledir_;
spec.modelname = (mjString)&spec_modelname_;
@@ -230,9 +368,12 @@ mjCModel::~mjCModel() {
for (int i=0; i<texts.size(); i++) delete texts[i];
for (int i=0; i<tuples.size(); i++) delete tuples[i];
for (int i=0; i<keys.size(); i++) delete keys[i];
for (int i=0; i<plugins.size(); i++) delete plugins[i];
for (int i=0; i<defaults.size(); i++) delete defaults[i];
if (plugin_owner) {
for (int i=0; i<plugins.size(); i++) delete plugins[i];
}
// clear pointer lists created in model construction
flexes.clear();
meshes.clear();
@@ -339,7 +480,6 @@ void mjCModel::Clear(void) {
hasImplicitPluginElem = false;
compiled = false;
errInfo = mjCError();
fixCount = 0;
qpos0.clear();
}
@@ -498,35 +638,28 @@ mjCBase* mjCModel::GetObject(mjtObj type, int id) {
//------------------------ API FOR ACCESS TO PRIVATE VARIABLES -------------------------------------
// compiled flag
bool mjCModel::IsCompiled(void) {
bool mjCModel::IsCompiled(void) const {
return compiled;
}
// number of massless bodies that were fixed
int mjCModel::GetFixed(void) {
return fixCount;
}
// get reference of error object
const mjCError& mjCModel::GetError(void) {
const mjCError& mjCModel::GetError(void) const {
return errInfo;
}
// pointer to world body
mjCBody* mjCModel::GetWorld(void) {
mjCBody* mjCModel::GetWorld(void) const {
return bodies[0];
}
// find default class name in array
mjCDef* mjCModel::FindDef(string name) {
mjCDef* mjCModel::FindDef(string name) const {
for (int i=0; i<(int)defaults.size(); i++) {
if (defaults[i]->name==name) {
return defaults[i];
@@ -589,7 +722,7 @@ static T* findobject(std::string_view name, const vector<T*>& list, const mjKeyM
}
// find object in global lists given string type and name
mjCBase* mjCModel::FindObject(mjtObj type, string name) {
mjCBase* mjCModel::FindObject(mjtObj type, string name) const {
if (!object_lists[type]) {
return nullptr;
}
@@ -598,8 +731,46 @@ mjCBase* mjCModel::FindObject(mjtObj type, string name) {
// find body by name
mjCBody* mjCModel::FindBody(mjCBody* body, std::string name) const {
if (body->name == name) {
return body;
}
for (auto child : body->bodies) {
auto candidate = FindBody(child, name);
if (candidate) {
return candidate;
}
}
return nullptr;
}
// find frame by name
mjCFrame* mjCModel::FindFrame(mjCBody* body, std::string name) const{
for (auto frame : body->frames) {
if (frame->name == name) {
return frame;
}
}
for (auto body : body->bodies) {
auto candidate = FindFrame(body, name);
if (candidate) {
return candidate;
}
}
return nullptr;
}
// detect null pose
bool mjCModel::IsNullPose(const mjtNum* pos, const mjtNum* quat) {
bool mjCModel::IsNullPose(const mjtNum* pos, const mjtNum* quat) const {
bool result = true;
// check position if given
@@ -761,112 +932,154 @@ void mjCModel::DeleteAll<mjCTexture>(std::vector<mjCTexture*>& elements) {
elements.clear();
}
// set nuser fields
void mjCModel::SetNuser(){
if (nuser_body == -1) {
nuser_body = 0;
for (int i=0; i<bodies.size(); i++) {
nuser_body = mjMAX(nuser_body, bodies[i]->spec_userdata_.size());
}
}
if (nuser_jnt == -1) {
nuser_jnt = 0;
for (int i=0; i<joints.size(); i++) {
nuser_jnt = mjMAX(nuser_jnt, joints[i]->spec_userdata_.size());
}
}
if (nuser_geom == -1) {
nuser_geom = 0;
for (int i=0; i<geoms.size(); i++) {
nuser_geom = mjMAX(nuser_geom, geoms[i]->spec_userdata_.size());
}
}
if (nuser_site == -1) {
nuser_site = 0;
for (int i=0; i<sites.size(); i++) {
nuser_site = mjMAX(nuser_site, sites[i]->spec_userdata_.size());
}
}
if (nuser_cam == -1) {
nuser_cam = 0;
for (int i=0; i<cameras.size(); i++) {
nuser_cam = mjMAX(nuser_cam, cameras[i]->spec_userdata_.size());
}
}
if (nuser_tendon == -1) {
nuser_tendon = 0;
for (int i=0; i<tendons.size(); i++) {
nuser_tendon = mjMAX(nuser_tendon, tendons[i]->spec_userdata_.size());
}
}
if (nuser_actuator == -1) {
nuser_actuator = 0;
for (int i=0; i<actuators.size(); i++) {
nuser_actuator = mjMAX(nuser_actuator, actuators[i]->spec_userdata_.size());
}
}
if (nuser_sensor == -1) {
nuser_sensor = 0;
for (int i=0; i<sensors.size(); i++) {
nuser_sensor = mjMAX(nuser_sensor, sensors[i]->spec_userdata_.size());
}
}
}
// index assets
void mjCModel::IndexAssets(bool discard) {
// assets referenced in geoms
for (int i=0; i<geoms.size(); i++) {
mjCGeom* pgeom = geoms[i];
mjCGeom* geom = geoms[i];
// find material by name
if (!pgeom->get_material().empty()) {
mjCBase* m = FindObject(mjOBJ_MATERIAL, pgeom->get_material());
if (m) {
pgeom->matid = m->id;
if (!geom->get_material().empty()) {
mjCBase* material = FindObject(mjOBJ_MATERIAL, geom->get_material());
if (material) {
geom->matid = material->id;
} else {
throw mjCError(pgeom, "material '%s' not found in geom %d", pgeom->get_material().c_str(), i);
throw mjCError(geom, "material '%s' not found in geom %d", geom->get_material().c_str(), i);
}
}
// find mesh by name
if (!pgeom->get_meshname().empty()) {
mjCBase* m = FindObject(mjOBJ_MESH, pgeom->get_meshname());
if (m) {
if (discard && geoms[i]->visual_) {
// do not associate with a mesh
pgeom->mesh = nullptr;
} else {
// associate mesh with geom
pgeom->mesh = (mjCMesh*)m;
// mark mesh as not visual
// this is irreversible so only performed when IndexAssets is called with discard
if (discard) {
pgeom->mesh->SetNotVisual();
}
if (!geom->get_meshname().empty()) {
mjCBase* mesh = FindObject(mjOBJ_MESH, geom->get_meshname());
if (mesh) {
if (!geom->visual_) {
((mjCMesh*)mesh)->SetNotVisual(); // reset to true by mesh->Compile()
}
geom->mesh = (discard && geom->visual_) ? nullptr : (mjCMesh*)mesh;
} else {
throw mjCError(pgeom, "mesh '%s' not found in geom %d", pgeom->get_meshname().c_str(), i);
throw mjCError(geom, "mesh '%s' not found in geom %d", geom->get_meshname().c_str(), i);
}
}
// find hfield by name
if (!pgeom->get_hfieldname().empty()) {
mjCBase* m = FindObject(mjOBJ_HFIELD, pgeom->get_hfieldname());
if (m) {
pgeom->hfield = (mjCHField*)m;
if (!geom->get_hfieldname().empty()) {
mjCBase* hfield = FindObject(mjOBJ_HFIELD, geom->get_hfieldname());
if (hfield) {
geom->hfield = (mjCHField*)hfield;
} else {
throw mjCError(pgeom, "hfield '%s' not found in geom %d", pgeom->get_hfieldname().c_str(), i);
throw mjCError(geom, "hfield '%s' not found in geom %d", geom->get_hfieldname().c_str(), i);
}
}
}
// assets referenced in skins
for (int i=0; i<skins.size(); i++) {
mjCSkin* pskin = skins[i];
mjCSkin* skin = skins[i];
// find material by name
if (!pskin->material_.empty()) {
mjCBase* m = FindObject(mjOBJ_MATERIAL, pskin->material_);
if (m) {
pskin->matid = m->id;
if (!skin->material_.empty()) {
mjCBase* material = FindObject(mjOBJ_MATERIAL, skin->material_);
if (material) {
skin->matid = material->id;
} else {
throw mjCError(pskin, "material '%s' not found in skin %d", pskin->material_.c_str(), i);
throw mjCError(skin, "material '%s' not found in skin %d", skin->material_.c_str(), i);
}
}
}
// materials referenced in sites
for (int i=0; i<sites.size(); i++) {
mjCSite* psite = sites[i];
mjCSite* site = sites[i];
// find material by name
if (!psite->material_.empty()) {
mjCBase* m = FindObject(mjOBJ_MATERIAL, psite->get_material());
if (m) {
psite->matid = m->id;
if (!site->material_.empty()) {
mjCBase* material = FindObject(mjOBJ_MATERIAL, site->get_material());
if (material) {
site->matid = material->id;
} else {
throw mjCError(psite, "material '%s' not found in site %d", psite->material_.c_str(), i);
throw mjCError(site, "material '%s' not found in site %d", site->material_.c_str(), i);
}
}
}
// materials referenced in tendons
for (int i=0; i<tendons.size(); i++) {
mjCTendon* pten = tendons[i];
mjCTendon* tendon = tendons[i];
// find material by name
if (!pten->material_.empty()) {
mjCBase* m = FindObject(mjOBJ_MATERIAL, pten->material_);
if (m) {
pten->matid = m->id;
if (!tendon->material_.empty()) {
mjCBase* material = FindObject(mjOBJ_MATERIAL, tendon->material_);
if (material) {
tendon->matid = material->id;
} else {
throw mjCError(pten, "material '%s' not found in tendon %d", pten->material_.c_str(), i);
throw mjCError(tendon, "material '%s' not found in tendon %d", tendon->material_.c_str(), i);
}
}
}
// textures referenced in materials
for (int i=0; i<materials.size(); i++) {
mjCMaterial* pmat = materials[i];
mjCMaterial* material = materials[i];
// find texture by name
if (!pmat->texture_.empty()) {
mjCBase* m = FindObject(mjOBJ_TEXTURE, pmat->texture_);
if (m) {
pmat->texid = m->id;
if (!material->texture_.empty()) {
mjCBase* texture = FindObject(mjOBJ_TEXTURE, material->texture_);
if (texture) {
material->texid = texture->id;
} else {
throw mjCError(pmat, "texture '%s' not found in material %d", pmat->texture_.c_str(), i);
throw mjCError(material, "texture '%s' not found in material %d", material->texture_.c_str(), i);
}
}
}
@@ -1331,7 +1544,7 @@ static int namelist(vector<T*>& list, int adr, int* name_adr, char* names, int*
continue;
}
uint64_t j = mj_hashdjb2(list[i]->name.c_str(), map_size);
uint64_t j = mj_hashString(list[i]->name.c_str(), map_size);
// find first empty slot using linear probing
for (; map[j]!=-1; j=(j+1) % map_size) {}
@@ -1555,6 +1768,7 @@ void mjCModel::CopyTree(mjModel* m) {
m->jnt_group[jid] = pj->group;
m->jnt_limited[jid] = (mjtByte)pj->is_limited();
m->jnt_actfrclimited[jid] = (mjtByte)pj->is_actfrclimited();
m->jnt_actgravcomp[jid] = pj->actgravcomp;
m->jnt_qposadr[jid] = qposadr;
m->jnt_dofadr[jid] = dofadr;
m->jnt_bodyid[jid] = pj->body->id;
@@ -2144,7 +2358,7 @@ void mjCModel::CopyObjects(mjModel* m) {
m->hfield_adr[i] = data_adr;
// copy elevation data
memcpy(m->hfield_data + data_adr, phf->data, phf->nrow*phf->ncol*sizeof(float));
memcpy(m->hfield_data + data_adr, phf->data.data(), phf->nrow*phf->ncol*sizeof(float));
// advance counter
data_adr += phf->nrow*phf->ncol;
@@ -2163,7 +2377,7 @@ void mjCModel::CopyObjects(mjModel* m) {
m->tex_adr[i] = data_adr;
// copy rgb data
memcpy(m->tex_rgb + data_adr, ptex->rgb, 3*ptex->width*ptex->height);
memcpy(m->tex_rgb + data_adr, ptex->rgb.data(), 3*ptex->width*ptex->height);
// advance counter
data_adr += 3*ptex->width*ptex->height;
@@ -2719,6 +2933,21 @@ static void processlist(mjListKeyMap& ids, vector<T*>& list,
}
// set object ids, check for repeated names
void mjCModel::ProcessLists() {
for (int i = 0; i < mjNOBJECT; i++) {
if (i != mjOBJ_XBODY && object_lists[i]) {
processlist(ids, *object_lists[i], (mjtObj) i);
}
}
// check repeated names in meta elements
processlist(ids, frames, mjOBJ_FRAME);
}
// error handler for low-level engine
static thread_local std::jmp_buf error_jmp_buf;
static thread_local char errortext[500] = "";
@@ -2852,14 +3081,7 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) {
CheckEmptyNames();
// set object ids, check for repeated names
for (int i = 0; i < mjNOBJECT; i++) {
if (i != mjOBJ_XBODY && object_lists[i]) {
processlist(ids, *object_lists[i], (mjtObj) i);
}
}
// check repeated names in meta elements
processlist(ids, frames, mjOBJ_FRAME);
ProcessLists();
// delete visual assets
if (discardvisual) {
@@ -2869,8 +3091,8 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) {
DeleteAll(textures);
}
// convert names into indices
IndexAssets(false);
// map names to asset references
IndexAssets(/*discard=*/false);
// mark meshes that need convex hull
for (int i=0; i<geoms.size(); i++) {
@@ -2880,61 +3102,14 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) {
}
}
// automatically set nuser fields
SetNuser();
// compile meshes (needed for geom compilation)
for (int i=0; i<meshes.size(); i++) {
meshes[i]->Compile(vfs);
}
// automatically set nuser fields
if (nuser_body == -1) {
nuser_body = 0;
for (int i=0; i<bodies.size(); i++) {
nuser_body = mjMAX(nuser_body, bodies[i]->spec_userdata_.size());
}
}
if (nuser_jnt == -1) {
nuser_jnt = 0;
for (int i=0; i<joints.size(); i++) {
nuser_jnt = mjMAX(nuser_jnt, joints[i]->spec_userdata_.size());
}
}
if (nuser_geom == -1) {
nuser_geom = 0;
for (int i=0; i<geoms.size(); i++) {
nuser_geom = mjMAX(nuser_geom, geoms[i]->spec_userdata_.size());
}
}
if (nuser_site == -1) {
nuser_site = 0;
for (int i=0; i<sites.size(); i++) {
nuser_site = mjMAX(nuser_site, sites[i]->spec_userdata_.size());
}
}
if (nuser_cam == -1) {
nuser_cam = 0;
for (int i=0; i<cameras.size(); i++) {
nuser_cam = mjMAX(nuser_cam, cameras[i]->spec_userdata_.size());
}
}
if (nuser_tendon == -1) {
nuser_tendon = 0;
for (int i=0; i<tendons.size(); i++) {
nuser_tendon = mjMAX(nuser_tendon, tendons[i]->spec_userdata_.size());
}
}
if (nuser_actuator == -1) {
nuser_actuator = 0;
for (int i=0; i<actuators.size(); i++) {
nuser_actuator = mjMAX(nuser_actuator, actuators[i]->spec_userdata_.size());
}
}
if (nuser_sensor == -1) {
nuser_sensor = 0;
for (int i=0; i<sensors.size(); i++) {
nuser_sensor = mjMAX(nuser_sensor, sensors[i]->spec_userdata_.size());
}
}
// compile objects in kinematic tree
for (int i=0; i<bodies.size(); i++) {
bodies[i]->Compile(); // also compiles joints, geoms, sites, cameras, lights
@@ -3057,7 +3232,7 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) {
std::vector<std::vector<int>> plugin_to_actuators(nplugin);
for (int i = 0; i < nu; ++i) {
if (actuators[i]->plugin.active) {
int actuator_plugin = ((mjCPlugin*)actuators[i]->plugin.instance)->id;
int actuator_plugin = static_cast<mjCPlugin*>(actuators[i]->plugin.instance)->id;
m->actuator_plugin[i] = actuator_plugin;
plugin_to_actuators[actuator_plugin].push_back(i);
} else {
@@ -3067,7 +3242,7 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) {
for (int i = 0; i < nbody; ++i) {
if (bodies[i]->plugin.active) {
m->body_plugin[i] = ((mjCPlugin*)bodies[i]->plugin.instance)->id;
m->body_plugin[i] = static_cast<mjCPlugin*>(bodies[i]->plugin.instance)->id;
} else {
m->body_plugin[i] = -1;
}
@@ -3075,7 +3250,7 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) {
for (int i = 0; i < ngeom; ++i) {
if (geoms[i]->plugin.active) {
m->geom_plugin[i] = ((mjCPlugin*)geoms[i]->plugin.instance)->id;
m->geom_plugin[i] = static_cast<mjCPlugin*>(geoms[i]->plugin.instance)->id;
} else {
m->geom_plugin[i] = -1;
}
@@ -3084,7 +3259,7 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) {
std::vector<std::vector<int>> plugin_to_sensors(nplugin);
for (int i = 0; i < nsensor; ++i) {
if (sensors[i]->type == mjSENS_PLUGIN) {
int sensor_plugin = ((mjCPlugin*)sensors[i]->plugin.instance)->id;
int sensor_plugin = static_cast<mjCPlugin*>(sensors[i]->plugin.instance)->id;
m->sensor_plugin[i] = sensor_plugin;
plugin_to_sensors[sensor_plugin].push_back(i);
} else {
src/user/user_model.h
+173 -148
View File
@@ -33,133 +33,14 @@
typedef std::map<std::string, int, std::less<> > mjKeyMap;
typedef std::array<mjKeyMap, mjNOBJECT> mjListKeyMap;
// mjCModel contains everything needed to generate the low-level model.
// It can be constructed manually by calling 'Add' functions and setting
// the public fields of the various objects. Alternatively it can constructed
// by loading an XML file via mjCXML. Once an mjCModel object is
// constructed, 'Compile' can be called to generate the corresponding mjModel object
// (which is the low-level model). The mjCModel object can then be deleted.
class mjCModel : private mjSpec {
friend class mjCBody;
friend class mjCCamera;
friend class mjCGeom;
friend class mjCFlex;
friend class mjCHField;
friend class mjCFrame;
friend class mjCJoint;
friend class mjCEquality;
friend class mjCMesh;
friend class mjCSkin;
friend class mjCSite;
friend class mjCTendon;
friend class mjCTexture;
friend class mjCActuator;
friend class mjCSensor;
friend class mjCDef;
friend class mjXReader;
friend class mjXWriter;
class mjCModel_ : public mjElement {
public:
mjCModel();
mjCModel(const mjCModel& other);
~mjCModel();
void CopyFromSpec(); // copy spec to private attributes
void PointToLocal();
// attach namespaces
std::string prefix;
std::string suffix;
mjSpec spec;
mjModel* Compile(const mjVFS* vfs = nullptr); // construct mjModel
bool CopyBack(const mjModel*); // DECOMPILER: copy numeric back
void FuseStatic(); // fuse static bodies with parent
void FuseReindex(mjCBody* body); // reindex elements during fuse
// API for adding model elements
mjCFlex* AddFlex();
mjCMesh* AddMesh(mjCDef* def = nullptr);
mjCSkin* AddSkin();
mjCHField* AddHField();
mjCTexture* AddTexture();
mjCMaterial* AddMaterial(mjCDef* def = nullptr);
mjCPair* AddPair(mjCDef* def = nullptr); // geom pair for inclusion
mjCBodyPair* AddExclude(); // body pair for exclusion
mjCEquality* AddEquality(mjCDef* def = nullptr); // equality constraint
mjCTendon* AddTendon(mjCDef* def = nullptr);
mjCActuator* AddActuator(mjCDef* def = nullptr);
mjCSensor* AddSensor();
mjCNumeric* AddNumeric();
mjCText* AddText();
mjCTuple* AddTuple();
mjCKey* AddKey();
mjCPlugin* AddPlugin();
// delete elements marked as discard=true
template <class T> void Delete(std::vector<T*>& elements,
const std::vector<bool>& discard);
// delete all elements
template <class T> void DeleteAll(std::vector<T*>& elements);
// API for access to model elements (outside tree)
int NumObjects(mjtObj type); // number of objects in specified list
mjCBase* GetObject(mjtObj type, int id); // pointer to specified object
// API for access to other variables
bool IsCompiled(); // is model already compiled
int GetFixed(); // number of fixed massless bodies
const mjCError& GetError(void); // get reference of error object
mjCBody* GetWorld(); // pointer to world body
mjCDef* FindDef(std::string name); // find default class name
mjCDef* AddDef(std::string name, int parentid); // add default class to array
mjCBase* FindObject(mjtObj type, std::string name); // find object given type and name
bool IsNullPose(const mjtNum* pos, const mjtNum* quat); // detect null pose
// accessors
std::string get_meshdir(void) const { return meshdir_; }
std::string get_texturedir(void) const { return texturedir_; }
// 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);
// if asset name is missing, set to filename
template<class T> void SetDefaultNames(std::vector<T*>& assets);
// delete material from object
template <class T> void DeleteMaterial(std::vector<T*>& list,
std::string_view name = "");
// compile phases
void MakeLists(mjCBody* body); // make lists of bodies, geoms, joints, sites
void IndexAssets(bool discard); // convert asset names into indices
void CheckEmptyNames(); // check empty names
void SetSizes(); // compute sizes
void AutoSpringDamper(mjModel*); // automatic stiffness and damping computation
void LengthRange(mjModel*, mjData*); // compute actuator lengthrange
void CopyNames(mjModel*); // copy names, compute name addresses
void CopyPaths(mjModel*); // copy paths, compute path addresses
void CopyObjects(mjModel*); // copy objects outside kinematic tree
void CopyTree(mjModel*); // copy objects inside kinematic tree
protected:
bool compiled; // already compiled flag
// sizes set from object list lengths
int nbody; // number of bodies
@@ -224,6 +105,168 @@ class mjCModel : private mjSpec {
int nD; // number of non-zeros in sparse dof-dof matrix
int nB; // number of non-zeros in sparse body-dof matrix
// statistics, as computed by mj_setConst
double meaninertia_auto; // mean diagonal inertia, as computed by mj_setConst
double meanmass_auto; // mean body mass, as computed by mj_setConst
double meansize_auto; // mean body size, as computed by mj_setConst
double extent_auto; // spatial extent, as computed by mj_setConst
double center_auto[3]; // center of model, as computed by mj_setConst
// save qpos0, to recognize changed key_qpos in write
std::vector<mjtNum> qpos0;
// variable-size attributes
std::string comment_; // comment at top of XML
std::string modelfiledir_; // path to model file
std::string modelname_;
std::string meshdir_;
std::string texturedir_;
std::string spec_comment_;
std::string spec_modelfiledir_;
std::string spec_modelname_;
std::string spec_meshdir_;
std::string spec_texturedir_;
};
// mjCModel contains everything needed to generate the low-level model.
// It can be constructed manually by calling 'Add' functions and setting
// the public fields of the various objects. Alternatively it can constructed
// by loading an XML file via mjCXML. Once an mjCModel object is
// constructed, 'Compile' can be called to generate the corresponding mjModel object
// (which is the low-level model). The mjCModel object can then be deleted.
class mjCModel : public mjCModel_, private mjSpec {
friend class mjCBase;
friend class mjCBody;
friend class mjCCamera;
friend class mjCGeom;
friend class mjCFlex;
friend class mjCHField;
friend class mjCFrame;
friend class mjCJoint;
friend class mjCEquality;
friend class mjCMesh;
friend class mjCSkin;
friend class mjCSite;
friend class mjCTendon;
friend class mjCTexture;
friend class mjCActuator;
friend class mjCSensor;
friend class mjCDef;
friend class mjXReader;
friend class mjXWriter;
public:
mjCModel();
mjCModel(const mjCModel& other);
mjCModel& operator=(const mjCModel& other);
mjCModel& operator+=(const mjCModel& other);
~mjCModel();
void CopyFromSpec(); // copy spec to private attributes
void PointToLocal();
mjSpec spec;
mjModel* Compile(const mjVFS* vfs = nullptr); // construct mjModel
bool CopyBack(const mjModel*); // DECOMPILER: copy numeric back
void FuseStatic(); // fuse static bodies with parent
void FuseReindex(mjCBody* body); // reindex elements during fuse
// API for adding model elements
mjCFlex* AddFlex();
mjCMesh* AddMesh(mjCDef* def = nullptr);
mjCSkin* AddSkin();
mjCHField* AddHField();
mjCTexture* AddTexture();
mjCMaterial* AddMaterial(mjCDef* def = nullptr);
mjCPair* AddPair(mjCDef* def = nullptr); // geom pair for inclusion
mjCBodyPair* AddExclude(); // body pair for exclusion
mjCEquality* AddEquality(mjCDef* def = nullptr); // equality constraint
mjCTendon* AddTendon(mjCDef* def = nullptr);
mjCActuator* AddActuator(mjCDef* def = nullptr);
mjCSensor* AddSensor();
mjCNumeric* AddNumeric();
mjCText* AddText();
mjCTuple* AddTuple();
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 elements marked as discard=true
template <class T> void Delete(std::vector<T*>& elements,
const std::vector<bool>& discard);
// delete all elements
template <class T> void DeleteAll(std::vector<T*>& elements);
// API for access to model elements (outside tree)
int NumObjects(mjtObj type); // number of objects in specified list
mjCBase* GetObject(mjtObj type, int id); // pointer to specified object
// 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
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
mjCFrame* FindFrame(mjCBody* body, std::string name) const; // find frame given name
bool IsNullPose(const mjtNum* pos, const mjtNum* quat) const; // detect null pose
// accessors
std::string get_meshdir(void) const { return meshdir_; }
std::string get_texturedir(void) const { return texturedir_; }
// 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);
// if asset name is missing, set to filename
template<class T> void SetDefaultNames(std::vector<T*>& assets);
// delete material from object
template <class T> void DeleteMaterial(std::vector<T*>& list,
std::string_view name = "");
// compile phases
void MakeLists(mjCBody* body); // make lists of bodies, geoms, joints, sites
void SetNuser(); // set nuser fields
void IndexAssets(bool discard); // convert asset names into indices
void CheckEmptyNames(); // check empty names
void SetSizes(); // compute sizes
void AutoSpringDamper(mjModel*); // automatic stiffness and damping computation
void LengthRange(mjModel*, mjData*); // compute actuator lengthrange
void CopyNames(mjModel*); // copy names, compute name addresses
void CopyPaths(mjModel*); // copy paths, compute path addresses
void CopyObjects(mjModel*); // copy objects outside kinematic tree
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
@@ -255,32 +298,14 @@ class mjCModel : private mjSpec {
// array of pointers to each object list (enumerated by type)
std::array<std::vector<mjCBase*>*, mjNOBJECT> object_lists;
// statistics, as computed by mj_setConst
double meaninertia_auto; // mean diagonal inertia, as computed by mj_setConst
double meanmass_auto; // mean body mass, as computed by mj_setConst
double meansize_auto; // mean body size, as computed by mj_setConst
double extent_auto; // spatial extent, as computed by mj_setConst
double center_auto[3]; // center of model, as computed by mj_setConst
// create mjCBase lists from children lists
void CreateObjectLists();
// populate objects ids
void ProcessLists();
mjListKeyMap ids; // map from object names to ids
bool compiled; // already compiled flag (cannot be compiled again)
mjCError errInfo; // last error info
int fixCount; // how many bodies have been fixed
// save qpos0, to recognize changed key_qpos in write
std::vector<mjtNum> qpos0;
// variable-size attributes
std::string comment_; // comment at top of XML
std::string modelfiledir_; // path to model file
std::string modelname_;
std::string meshdir_;
std::string texturedir_;
std::string spec_comment_;
std::string spec_modelfiledir_;
std::string spec_modelname_;
std::string spec_meshdir_;
std::string spec_texturedir_;
bool plugin_owner; // this class allocated the plugins
};
#endif // MUJOCO_SRC_USER_USER_MODEL_H_
src/user/user_objects.cc
+821 -457
View File
File diff suppressed because it is too large Load Diff
src/user/user_objects.h
+135 -82
View File
@@ -15,10 +15,8 @@
#ifndef MUJOCO_SRC_USER_USER_OBJECTS_H_
#define MUJOCO_SRC_USER_USER_OBJECTS_H_
#include <array>
#include <functional>
#include <map>
#include <optional>
#include <string>
#include <string_view>
#include <utility>
@@ -28,10 +26,10 @@
#include <mujoco/mjmodel.h>
#include <mujoco/mjplugin.h>
#include "user/user_api.h"
#include "user/user_cache.h"
// forward declarations of all mjC/X classes
class mjCError;
class mjCAlternative;
class mjCBase;
class mjCBody;
class mjCFrame;
@@ -83,13 +81,10 @@ class [[nodiscard]] mjCError {
// alternative specifications of frame orientation
class mjCAlternative : public mjmOrientation {
public:
mjCAlternative(); // constuctor
const char* Set(double* quat, // set frame quat
const char* ResolveOrientation(double* quat, // set frame quat
bool degree, // angle format: degree/radian
const char* sequence); // euler sequence format: "xyz"
};
const char* sequence, // euler sequence format: "xyz"
const mjsOrientation& orient);
@@ -144,7 +139,27 @@ class mjCBoundingVolumeHierarchy : public mjCBoundingVolumeHierarchy_ {
mjCBoundingVolume* GetBoundingVolume(int id);
private:
int MakeBVH(std::vector<const mjCBoundingVolume*>& elements, int lev = 0);
// internal class used during BVH construction, for partial sorting of bounding volumes
struct BVElement {
const mjCBoundingVolume* e;
// position of the element in the BVH axes
mjtNum lpos[3];
};
struct BVElementCompare {
int axis = 0;
bool operator()(const BVElement& e1, const BVElement& e2) const {
if (e1.lpos[axis] != e2.lpos[axis]) {
return e1.lpos[axis] < e2.lpos[axis];
}
// comparing pointers gives a stable sort, because they both come from the same array
return e1.e < e2.e;
}
};
int MakeBVH(std::vector<BVElement>::iterator elements_begin,
std::vector<BVElement>::iterator elements_end, int lev = 0);
};
@@ -152,13 +167,14 @@ class mjCBoundingVolumeHierarchy : public mjCBoundingVolumeHierarchy_ {
//------------------------- class mjCBase ----------------------------------------------------------
// Generic functionality for all derived classes
class mjCBase_ {
class mjCBase_ : public mjElement {
public:
int id; // object id
std::string name; // object name
std::string classname; // defaults class name
int id; // object id
std::string info; // error message info set by the user
mjCModel* model; // pointer to model that created object
std::string prefix; // prefix for model operations
std::string suffix; // suffix for model operations
};
class mjCBase : public mjCBase_ {
@@ -178,11 +194,18 @@ class mjCBase : public mjCBase_ {
// Copy spec into private attributes
virtual void CopyFromSpec() {}
// Throws an error if any of the references is missing
virtual void ResolveReferences(const mjCModel* m) {}
// Appends prefix and suffix to reference
virtual void NameSpace(const mjCModel* m) {}
// Copy assignment
mjCBase& operator=(const mjCBase& other);
mjCDef* def; // defaults class used to init this object
mjCFrame* frame; // pointer to frame transformation
mjCModel* model; // pointer to model that created object
protected:
mjCBase(); // constructor
@@ -196,10 +219,6 @@ class mjCBase : public mjCBase_ {
// Describes a rigid body
class mjCBody_ : public mjCBase {
public:
mjCAlternative alt_;
mjCAlternative ialt_;
protected:
// variables computed by 'Compile' and 'AddXXX'
int parentid; // parent index in global array
@@ -226,12 +245,13 @@ class mjCBody_ : public mjCBase {
std::vector<double> spec_userdata_;
};
class mjCBody : public mjCBody_, private mjmBody {
class mjCBody : public mjCBody_, private mjsBody {
friend class mjCJoint;
friend class mjCGeom;
friend class mjCSite;
friend class mjCCamera;
friend class mjCComposite;
friend class mjCFrame;
friend class mjCLight;
friend class mjCFlex;
friend class mjCFlexcomp;
@@ -253,11 +273,17 @@ class mjCBody : public mjCBody_, private mjmBody {
mjCCamera* AddCamera(mjCDef* = 0);
mjCLight* AddLight(mjCDef* = 0);
// API for adding existing objects to body
mjCBody& operator+=(mjCBody& other);
// API for accessing objects
int NumObjects(mjtObj type);
mjCBase* GetObject(mjtObj type, int id);
mjCBase* FindObject(mjtObj type, std::string name, bool recursive = true);
// Propagate suffix and prefix to the whole tree
void NameSpace(const mjCModel* m);
// set explicitinertial to true
void MakeInertialExplicit();
@@ -266,7 +292,7 @@ class mjCBody : public mjCBody_, private mjmBody {
const char* FullInertia(double quat[4], double inertia[3]);
// variables set by user
mjmBody spec;
mjsBody spec;
// inherited
using mjCBase::name;
@@ -278,7 +304,7 @@ class mjCBody : public mjCBody_, private mjmBody {
private:
mjCBody(mjCModel*); // constructor
mjCBody(const mjCBody& other); // copy constructor
mjCBody(const mjCBody& other, mjCModel* _model); // copy constructor
mjCBody& operator=(const mjCBody& other); // copy assignment
~mjCBody(); // destructor
@@ -306,10 +332,9 @@ class mjCBody : public mjCBody_, private mjmBody {
class mjCFrame_ : public mjCBase {
protected:
bool compiled; // frame already compiled
mjCAlternative alt_;
};
class mjCFrame : public mjCFrame_, private mjmFrame {
class mjCFrame : public mjCFrame_, private mjsFrame {
friend class mjCBase;
friend class mjCBody;
friend class mjCGeom;
@@ -320,19 +345,25 @@ class mjCFrame : public mjCFrame_, private mjmFrame {
friend class mjCModel;
public:
mjmFrame spec;
mjsFrame spec;
using mjCBase::name;
using mjCBase::classname;
using mjCBase::info;
void CopyFromSpec(void);
void PointToLocal(void);
void SetParent(mjCBody* _body);
mjCFrame& operator+=(const mjCBody& other);
mjCFrame& operator+=(const mjCFrame& other);
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
};
@@ -349,7 +380,7 @@ class mjCJoint_ : public mjCBase {
std::vector<double> spec_userdata_;
};
class mjCJoint : public mjCJoint_, private mjmJoint {
class mjCJoint : public mjCJoint_, private mjsJoint {
friend class mjCDef;
friend class mjCEquality;
friend class mjCBody;
@@ -359,7 +390,7 @@ class mjCJoint : public mjCJoint_, private mjmJoint {
friend class mjXURDF;
public:
mjmJoint spec;
mjsJoint spec;
using mjCBase::name;
using mjCBase::classname;
using mjCBase::info;
@@ -393,7 +424,6 @@ class mjCGeom_ : public mjCBase {
bool inferinertia; // true if inertia should be computed from geom
protected:
mjCAlternative alt_;
bool visual_; // true: geom does not collide and is unreferenced
int matid; // id of geom's material
mjCMesh* mesh; // geom's mesh
@@ -417,7 +447,7 @@ class mjCGeom_ : public mjCBase {
std::vector<double> spec_userdata_;
};
class mjCGeom : public mjCGeom_, private mjmGeom {
class mjCGeom : public mjCGeom_, private mjsGeom {
friend class mjCDef;
friend class mjCMesh;
friend class mjCPair;
@@ -429,7 +459,7 @@ class mjCGeom : public mjCGeom_, private mjmGeom {
public:
using mjCBase::name;
mjmGeom spec; // variables set by user
mjsGeom spec; // variables set by user
double GetVolume(void); // compute geom volume
void SetInertia(void); // compute and set geom inertia
bool IsVisual(void) const { return visual_; }
@@ -473,8 +503,6 @@ class mjCGeom : public mjCGeom_, private mjmGeom {
class mjCSite_ : public mjCBase {
protected:
mjCAlternative alt_;
// variable-size data
std::string material_;
std::vector<double> userdata_;
@@ -486,7 +514,7 @@ class mjCSite_ : public mjCBase {
int matid; // material id for rendering
};
class mjCSite : public mjCSite_, private mjmSite {
class mjCSite : public mjCSite_, private mjsSite {
friend class mjCDef;
friend class mjCBody;
friend class mjCModel;
@@ -494,9 +522,9 @@ class mjCSite : public mjCSite_, private mjmSite {
friend class mjXURDF;
public:
mjmSite spec; // variables set by user
mjsSite spec; // variables set by user
// use strings from mjCBase rather than mjStrings from mjmSite
// use strings from mjCBase rather than mjStrings from mjsSite
using mjCBase::name;
using mjCBase::classname;
using mjCBase::info;
@@ -525,14 +553,13 @@ class mjCCamera_ : public mjCBase {
protected:
mjCBody* body; // camera's body
int targetbodyid; // id of target body; -1: none
mjCAlternative alt_;
std::string targetbody_;
std::string spec_targetbody_;
std::vector<double> userdata_;
std::vector<double> spec_userdata_;
};
class mjCCamera : public mjCCamera_, private mjmCamera {
class mjCCamera : public mjCCamera_, private mjsCamera {
friend class mjCDef;
friend class mjCBody;
friend class mjCModel;
@@ -540,7 +567,7 @@ class mjCCamera : public mjCCamera_, private mjmCamera {
friend class mjXWriter;
public:
mjmCamera spec;
mjsCamera spec;
using mjCBase::name;
using mjCBase::classname;
using mjCBase::info;
@@ -572,14 +599,14 @@ class mjCLight_ : public mjCBase {
std::string spec_targetbody_;
};
class mjCLight : public mjCLight_, private mjmLight {
class mjCLight : public mjCLight_, private mjsLight {
friend class mjCDef;
friend class mjCBody;
friend class mjCModel;
friend class mjXWriter;
public:
mjmLight spec;
mjsLight spec;
using mjCBase::name;
using mjCBase::classname;
using mjCBase::info;
@@ -633,7 +660,7 @@ class mjCFlex_ : public mjCBase {
std::vector<float> spec_texcoord_;
};
class mjCFlex: public mjCFlex_, private mjmFlex {
class mjCFlex: public mjCFlex_, private mjsFlex {
friend class mjCDef;
friend class mjCModel;
friend class mjCFlexcomp;
@@ -641,13 +668,15 @@ class mjCFlex: public mjCFlex_, private mjmFlex {
friend class mjXWriter;
public:
mjmFlex spec;
mjsFlex spec;
using mjCBase::name;
using mjCBase::classname;
using mjCBase::info;
void CopyFromSpec(void);
void PointToLocal(void);
void ResolveReferences(const mjCModel* m);
void NameSpace(const mjCModel* m);
// used by mjXWriter and mjCModel
const std::string& get_material() { return material_; }
@@ -735,7 +764,8 @@ class mjCMesh_ : public mjCBase {
std::vector<double> face_aabb_; // bounding boxes of all faces
};
class mjCMesh: public mjCMesh_, private mjmMesh {
class mjCMesh: public mjCMesh_, private mjsMesh {
friend class mjCModel;
friend class mjCFlexcomp;
friend class mjXWriter;
public:
@@ -744,7 +774,7 @@ class mjCMesh: public mjCMesh_, private mjmMesh {
mjCMesh& operator=(const mjCMesh& other); // copy assignment
~mjCMesh();
mjmMesh spec;
mjsMesh spec;
using mjCBase::name;
using mjCBase::classname;
using mjCBase::info;
@@ -808,6 +838,7 @@ class mjCMesh: public mjCMesh_, private mjmMesh {
private:
void LoadOBJ(mjResource* resource); // load mesh in wavefront OBJ format
bool LoadCachedOBJ(const mjCAsset& asset); // load OBJ from cache asset, return true on success
void LoadSTL(mjResource* resource); // load mesh in STL BIN format
void LoadMSH(mjResource* resource); // load mesh in MSH BIN format
void LoadSDF(); // generate mesh using marching cubes
@@ -870,12 +901,12 @@ class mjCSkin_ : public mjCBase {
std::vector<int> bodyid; // body ids
};
class mjCSkin: public mjCSkin_, private mjmSkin {
class mjCSkin: public mjCSkin_, private mjsSkin {
friend class mjCModel;
friend class mjXWriter;
public:
mjmSkin spec;
mjsSkin spec;
using mjCBase::name;
using mjCBase::classname;
using mjCBase::info;
@@ -901,6 +932,8 @@ class mjCSkin: public mjCSkin_, private mjmSkin {
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
void LoadSKN(mjResource* resource); // load skin in SKN BIN format
};
@@ -912,6 +945,8 @@ class mjCSkin: public mjCSkin_, private mjmSkin {
class mjCHField_ : public mjCBase {
protected:
std::vector<float> data; // elevation data, row-major format
std::string file_;
std::string content_type_;
std::vector<float> userdata_;
@@ -920,13 +955,13 @@ class mjCHField_ : public mjCBase {
std::vector<float> spec_userdata_;
};
class mjCHField : public mjCHField_, private mjmHField {
class mjCHField : public mjCHField_, private mjsHField {
friend class mjCGeom;
friend class mjCModel;
friend class mjXWriter;
public:
mjmHField spec;
mjsHField spec;
using mjCBase::name;
using mjCBase::info;
@@ -944,7 +979,6 @@ class mjCHField : public mjCHField_, private mjmHField {
mjCHField& operator=(const mjCHField& other); // copy assignment
~mjCHField(); // destructor
float* data; // elevation data, row-major format
void Compile(const mjVFS* vfs); // compiler
void LoadCustom(mjResource* resource); // load from custom format
@@ -958,6 +992,8 @@ class mjCHField : public mjCHField_, private mjmHField {
class mjCTexture_ : public mjCBase {
protected:
std::vector<mjtByte> rgb; // rgb data
std::string file_;
std::string content_type_;
std::vector<std::string> cubefiles_;
@@ -966,7 +1002,7 @@ class mjCTexture_ : public mjCBase {
std::vector<std::string> spec_cubefiles_;
};
class mjCTexture : public mjCTexture_, private mjmTexture {
class mjCTexture : public mjCTexture_, private mjsTexture {
friend class mjCModel;
friend class mjXReader;
friend class mjXWriter;
@@ -974,7 +1010,7 @@ class mjCTexture : public mjCTexture_, private mjmTexture {
public:
~mjCTexture(); // destructor
mjmTexture spec;
mjsTexture spec;
using mjCBase::name;
using mjCBase::classname;
using mjCBase::info;
@@ -1009,8 +1045,6 @@ class mjCTexture : public mjCTexture_, private mjmTexture {
void LoadCustom(mjResource* resource,
std::vector<unsigned char>& image,
unsigned int& w, unsigned int& h);
mjtByte* rgb; // rgb data
};
@@ -1025,13 +1059,13 @@ class mjCMaterial_ : public mjCBase {
std::string spec_texture_;
};
class mjCMaterial : public mjCMaterial_, private mjmMaterial {
class mjCMaterial : public mjCMaterial_, private mjsMaterial {
friend class mjCDef;
friend class mjCModel;
friend class mjXWriter;
public:
mjmMaterial spec;
mjsMaterial spec;
using mjCBase::name;
using mjCBase::classname;
using mjCBase::info;
@@ -1064,20 +1098,22 @@ class mjCPair_ : public mjCBase {
std::string spec_geomname2_;
};
class mjCPair : public mjCPair_, private mjmPair {
class mjCPair : public mjCPair_, private mjsPair {
friend class mjCDef;
friend class mjCBody;
friend class mjCModel;
friend class mjXWriter;
public:
mjmPair spec;
mjsPair spec;
using mjCBase::name;
using mjCBase::classname;
using mjCBase::info;
void CopyFromSpec();
void PointToLocal();
void ResolveReferences(const mjCModel* m);
void NameSpace(const mjCModel* m);
std::string get_geomname1() { return geomname1_; }
std::string get_geomname2() { return geomname2_; }
@@ -1114,17 +1150,19 @@ class mjCBodyPair_ : public mjCBase {
std::string spec_bodyname2_;
};
class mjCBodyPair : public mjCBodyPair_, private mjmExclude {
class mjCBodyPair : public mjCBodyPair_, private mjsExclude {
friend class mjCBody;
friend class mjCModel;
public:
mjmExclude spec;
mjsExclude spec;
using mjCBase::name;
using mjCBase::info;
void CopyFromSpec();
void PointToLocal();
void ResolveReferences(const mjCModel* m);
void NameSpace(const mjCModel* m);
std::string get_bodyname1() const { return bodyname1_; }
std::string get_bodyname2() const { return bodyname2_; }
@@ -1156,20 +1194,22 @@ class mjCEquality_ : public mjCBase {
std::string spec_name2_;
};
class mjCEquality : public mjCEquality_, private mjmEquality {
class mjCEquality : public mjCEquality_, private mjsEquality {
friend class mjCDef;
friend class mjCBody;
friend class mjCModel;
friend class mjXWriter;
public:
mjmEquality spec;
mjsEquality spec;
using mjCBase::name;
using mjCBase::classname;
using mjCBase::info;
void CopyFromSpec();
void PointToLocal();
void ResolveReferences(const mjCModel* m);
void NameSpace(const mjCModel* m);
private:
mjCEquality(mjCModel* = 0, mjCDef* = 0); // constructor
@@ -1195,13 +1235,13 @@ class mjCTendon_ : public mjCBase {
std::vector<double> spec_userdata_;
};
class mjCTendon : public mjCTendon_, private mjmTendon {
class mjCTendon : public mjCTendon_, private mjsTendon {
friend class mjCDef;
friend class mjCModel;
friend class mjXWriter;
public:
mjmTendon spec;
mjsTendon spec;
using mjCBase::name;
using mjCBase::classname;
using mjCBase::info;
@@ -1227,6 +1267,9 @@ class mjCTendon : public mjCTendon_, private mjmTendon {
void CopyFromSpec();
void PointToLocal();
void ResolveReferences(const mjCModel* m);
void NameSpace(const mjCModel* m);
void SetModel(mjCModel* _model);
bool is_limited() const;
@@ -1252,15 +1295,18 @@ class mjCWrap_ : public mjCBase {
std::string sidesite; // name of side site
};
class mjCWrap : public mjCWrap_, private mjmWrap {
class mjCWrap : public mjCWrap_, private mjsWrap {
friend class mjCTendon;
friend class mjCModel;
public:
mjmWrap spec;
mjsWrap spec;
using mjCBase::info;
void PointToLocal();
void ResolveReferences(const mjCModel* m);
void NameSpace(const mjCModel* m);
mjCBase* obj; // wrap object pointer
private:
@@ -1268,8 +1314,6 @@ class mjCWrap : public mjCWrap_, private mjmWrap {
mjCWrap(const mjCWrap& other); // copy constructor
mjCWrap& operator=(const mjCWrap& other); // copy assignment
void Compile(void); // compiler
mjCTendon* tendon; // tendon owning this wrap
};
@@ -1293,12 +1337,12 @@ class mjCPlugin : public mjCPlugin_ {
friend class mjXWriter;
public:
mjmPlugin spec;
mjsPlugin spec;
mjCBase* parent; // parent object (only used when generating error message)
mjCPlugin(const mjCPlugin& other); // copy constructor
private:
mjCPlugin(mjCModel*); // constructor
mjCPlugin(const mjCPlugin& other); // copy constructor
mjCPlugin& operator=(const mjCPlugin& other); // copy assignment
void Compile(void); // compiler
@@ -1326,13 +1370,13 @@ class mjCActuator_ : public mjCBase {
std::vector<double> spec_userdata_;
};
class mjCActuator : public mjCActuator_, private mjmActuator {
class mjCActuator : public mjCActuator_, private mjsActuator {
friend class mjCDef;
friend class mjCModel;
friend class mjXWriter;
public:
mjmActuator spec;
mjsActuator spec;
using mjCBase::name;
using mjCBase::classname;
using mjCBase::info;
@@ -1355,6 +1399,10 @@ class mjCActuator : public mjCActuator_, private mjmActuator {
void Compile(void); // compiler
void CopyFromSpec();
void PointToLocal();
void ResolveReferences(const mjCModel* m);
void NameSpace(const mjCModel* m);
mjCBase* ptarget; // transmission target
};
@@ -1377,13 +1425,13 @@ class mjCSensor_ : public mjCBase {
std::vector<double> spec_userdata_;
};
class mjCSensor : public mjCSensor_, private mjmSensor {
class mjCSensor : public mjCSensor_, private mjsSensor {
friend class mjCDef;
friend class mjCModel;
friend class mjXWriter;
public:
mjmSensor spec;
mjsSensor spec;
using mjCBase::name;
using mjCBase::classname;
using mjCBase::info;
@@ -1400,9 +1448,12 @@ class mjCSensor : public mjCSensor_, private mjmSensor {
void Compile(void); // compiler
void CopyFromSpec();
void MakePointerLocal();
void PointToLocal();
void ResolveReferences(const mjCModel* m);
void NameSpace(const mjCModel* m);
mjCBase* obj; // sensorized object
mjCBase* ref; // sensorized reference
};
@@ -1416,12 +1467,12 @@ class mjCNumeric_ : public mjCBase {
std::vector<double> spec_data_;
};
class mjCNumeric : public mjCNumeric_, private mjmNumeric {
class mjCNumeric : public mjCNumeric_, private mjsNumeric {
friend class mjCModel;
friend class mjXWriter;
public:
mjmNumeric spec;
mjsNumeric spec;
using mjCBase::name;
using mjCBase::info;
@@ -1448,12 +1499,12 @@ class mjCText_ : public mjCBase {
std::string spec_data_;
};
class mjCText : public mjCText_, private mjmText {
class mjCText : public mjCText_, private mjsText {
friend class mjCModel;
friend class mjXWriter;
public:
mjmText spec;
mjsText spec;
using mjCBase::name;
using mjCBase::info;
@@ -1485,17 +1536,19 @@ class mjCTuple_ : public mjCBase {
std::vector<double> spec_objprm_;
};
class mjCTuple : public mjCTuple_, private mjmTuple {
class mjCTuple : public mjCTuple_, private mjsTuple {
friend class mjCModel;
friend class mjXWriter;
public:
mjmTuple spec;
mjsTuple spec;
using mjCBase::name;
using mjCBase::info;
void PointToLocal();
void CopyFromSpec();
void ResolveReferences(const mjCModel* m);
void NameSpace(const mjCModel* m);
private:
mjCTuple(mjCModel*); // constructor
@@ -1527,12 +1580,12 @@ class mjCKey_ : public mjCBase {
std::vector<double> spec_ctrl_;
};
class mjCKey : public mjCKey_, private mjmKey {
class mjCKey : public mjCKey_, private mjsKey {
friend class mjCModel;
friend class mjXWriter;
public:
mjmKey spec;
mjsKey spec;
using mjCBase::name;
using mjCBase::info;
@@ -1553,7 +1606,7 @@ class mjCKey : public mjCKey_, private mjmKey {
//------------------------- class mjCDef -----------------------------------------------------------
// Describes one set of defaults
class mjCDef {
class mjCDef : public mjElement {
friend class mjXWriter;
public:
@@ -1569,7 +1622,7 @@ class mjCDef {
int parentid; // id of parent class
std::vector<int> childid; // ids of child classes
mjmDefault spec;
mjsDefault spec;
// default objects (TODO: they should become private)
mjCJoint joint;
src/xml/xml.cc
+38 -7
View File
@@ -15,7 +15,7 @@
#include "xml/xml.h"
#include <locale.h>
#include "user/user_api.h"
#include <cstring>
#if defined(__APPLE__) || defined(__FreeBSD__)
#include <xlocale.h>
@@ -29,6 +29,7 @@
#include "tinyxml2.h"
#include <mujoco/mujoco.h>
#include <mujoco/mjmodel.h>
#include <mujoco/mjplugin.h>
#include "cc/array_safety.h"
@@ -344,14 +345,14 @@ mjSpec* mjParseXML(const char* filename, const mjVFS* vfs,
}
// create model, set filedir
model = mjm_createSpec();
model = mjs_createSpec();
const char* dir;
int ndir = 0;
mju_getResourceDir(resource, &dir, &ndir);
if (dir != nullptr) {
mjm_setString(model->modelfiledir, std::string(dir, ndir).c_str());
mjs_setString(model->modelfiledir, std::string(dir, ndir).c_str());
} else {
mjm_setString(model->modelfiledir, "");
mjs_setString(model->modelfiledir, "");
}
// close resource
@@ -363,8 +364,8 @@ 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(mjm_getString(model->modelfiledir));
mjIncludeXML(parser, root, mjm_getString(model->modelfiledir), vfs, included);
parser.SetModelFileDir(mjs_getString(model->modelfiledir));
mjIncludeXML(parser, root, mjs_getString(model->modelfiledir), vfs, included);
// parse MuJoCo model
parser.SetModel(model);
@@ -393,9 +394,39 @@ mjSpec* mjParseXML(const char* filename, const mjVFS* vfs,
// catch known errors
catch (mjXError err) {
mjCopyError(error, err.message, error_sz);
mjm_deleteSpec(model);
mjs_deleteSpec(model);
return nullptr;
}
return model;
}
static void RegisterResourceProvider() {
// register string resource provider if not registered before
if (mjp_getResourceProvider("LoadModelFromString:") == nullptr) {
mjpResourceProvider resourceProvider;
mjp_defaultResourceProvider(&resourceProvider);
resourceProvider.prefix = "LoadModelFromString";
resourceProvider.open = +[](mjResource* resource) {
resource->data = &(resource->name[strlen("LoadModelFromString:")]);
return 1;
};
resourceProvider.read =
+[](mjResource* resource, const void** buffer) {
*buffer = resource->data;
return (int) strlen((const char*) resource->data);
};
resourceProvider.close = +[](mjResource* resource) {};
mjp_registerResourceProvider(&resourceProvider);
}
}
mjSpec* ParseSpecFromString(std::string_view xml, char* error,
int error_size, mjVFS* vfs) {
RegisterResourceProvider();
std::string xml2 = {xml.begin(), xml.end()};
std::string str = "LoadModelFromString:" + xml2;
return mjParseXML(str.c_str(), vfs, error, error_size);
}
src/xml/xml.h
+5
View File
@@ -29,5 +29,10 @@ std::string mjWriteXML(mjSpec* model, char* error, int error_sz);
// Main parser function
MJAPI mjSpec* mjParseXML(const char* filename, const mjVFS* vfs, char* error, int error_sz);
// Returns a newly-allocated mjSpec, loaded from the contents of xml.
// On failure returns nullptr and populates the error array if present.
MJAPI mjSpec* ParseSpecFromString(std::string_view xml, char* error = nullptr,
int error_size = 0, mjVFS* vfs = nullptr);
#endif // MUJOCO_SRC_XML_XML_H_
src/xml/xml_api.cc
+7 -7
View File
@@ -56,7 +56,7 @@ std::optional<std::string> GlobalModel::ToXML(const mjModel* m, char* error,
mjCopyError(error, "No XML model loaded", error_sz);
return std::nullopt;
}
mjm_copyBack(model_, m);
mjs_copyBack(model_, m);
std::string result = mjWriteXML(model_, error, error_sz);
if (result.empty()) {
return std::nullopt;
@@ -67,7 +67,7 @@ std::optional<std::string> GlobalModel::ToXML(const mjModel* m, char* error,
void GlobalModel::Set(mjSpec* model) {
std::lock_guard<std::mutex> lock(*mutex_);
if (model_ != nullptr) {
mjm_deleteSpec(model_);
mjs_deleteSpec(model_);
}
model_ = model;
}
@@ -93,21 +93,21 @@ mjModel* mj_loadXML(const char* filename, const mjVFS* vfs,
// parse new model
std::unique_ptr<mjSpec, std::function<void(mjSpec*)>> model(
mjParseXML(filename, vfs, error, error_sz),
[](mjSpec* m) { mjm_deleteSpec(m); });
[](mjSpec* m) { mjs_deleteSpec(m); });
if (!model) {
return nullptr;
}
// compile new model
mjModel* m = mjm_compile(model.get(), vfs);
mjModel* m = mjs_compile(model.get(), vfs);
if (!m) {
mjCopyError(error, mjm_getError(model.get()), error_sz);
mjCopyError(error, mjs_getError(model.get()), error_sz);
return nullptr;
}
// handle compile warning
if (mjm_isWarning(model.get())) {
mjCopyError(error, mjm_getError(model.get()), error_sz);
if (mjs_isWarning(model.get())) {
mjCopyError(error, mjs_getError(model.get()), error_sz);
} else if (error) {
error[0] = '\0';
}
src/xml/xml_base.cc
+2 -1
View File
@@ -24,6 +24,7 @@
#include "user/user_api.h"
#include "user/user_model.h"
#include "user/user_objects.h"
#include "xml/xml_util.h"
#include "tinyxml2.h"
namespace {
@@ -51,7 +52,7 @@ void mjXBase::SetModel(mjSpec* _model) {
// read alternative orientation specification
int mjXBase::ReadAlternative(XMLElement* elem, mjmOrientation& alt) {
int mjXBase::ReadAlternative(XMLElement* elem, mjsOrientation& alt) {
string text;
int read = (int)(elem->Attribute("quat") != 0) +
(ReadAttr(elem, "axisangle", 4, alt.axisangle, text) ? 1 : 0) +
src/xml/xml_base.h
+1 -1
View File
@@ -90,7 +90,7 @@ class mjXBase : public mjXUtil {
virtual void SetModel(mjSpec*);
// read alternative orientation specification
static int ReadAlternative(tinyxml2::XMLElement* elem, mjmOrientation& alt);
static int ReadAlternative(tinyxml2::XMLElement* elem, mjsOrientation& alt);
protected:
mjSpec* model; // internally-allocated model
src/xml/xml_native_reader.cc
+259 -256
View File
File diff suppressed because it is too large Load Diff
src/xml/xml_native_reader.h
+19 -19
View File
@@ -54,8 +54,8 @@ class mjXReader : public mjXBase {
void Visual(tinyxml2::XMLElement* section); // visual section
void Statistic(tinyxml2::XMLElement* section); // statistic section
void Asset(tinyxml2::XMLElement* section); // asset section
void Body(tinyxml2::XMLElement* section, mjmBody* pbody,
mjmFrame* pframe); // body/world section
void Body(tinyxml2::XMLElement* section, mjsBody* pbody,
mjsFrame* pframe); // body/world section
void Contact(tinyxml2::XMLElement* section); // contact section
void Deformable(tinyxml2::XMLElement* section); // deformable section
void Equality(tinyxml2::XMLElement* section); // equality section
@@ -65,25 +65,25 @@ class mjXReader : public mjXBase {
void Keyframe(tinyxml2::XMLElement* section); // keyframe section
// single element parsers, used in defaults and main body
void OneFlex(tinyxml2::XMLElement* elem, mjmFlex* pflex);
void OneMesh(tinyxml2::XMLElement* elem, mjmMesh* pmesh);
void OneSkin(tinyxml2::XMLElement* elem, mjmSkin* pskin);
void OneMaterial(tinyxml2::XMLElement* elem, mjmMaterial* pmaterial);
void OneJoint(tinyxml2::XMLElement* elem, mjmJoint* pjoint);
void OneGeom(tinyxml2::XMLElement* elem, mjmGeom* pgeom);
void OneSite(tinyxml2::XMLElement* elem, mjmSite* site);
void OneCamera(tinyxml2::XMLElement* elem, mjmCamera* pcamera);
void OneLight(tinyxml2::XMLElement* elem, mjmLight* plight);
void OnePair(tinyxml2::XMLElement* elem, mjmPair* ppair);
void OneEquality(tinyxml2::XMLElement* elem, mjmEquality* pequality);
void OneTendon(tinyxml2::XMLElement* elem, mjmTendon* ptendon);
void OneActuator(tinyxml2::XMLElement* elem, mjmActuator* pactuator);
void OneComposite(tinyxml2::XMLElement* elem, mjmBody* pbody, mjmDefault* def);
void OneFlexcomp(tinyxml2::XMLElement* elem, mjmBody* pbody);
void OnePlugin(tinyxml2::XMLElement* elem, mjmPlugin* plugin);
void OneFlex(tinyxml2::XMLElement* elem, mjsFlex* pflex);
void OneMesh(tinyxml2::XMLElement* elem, mjsMesh* pmesh);
void OneSkin(tinyxml2::XMLElement* elem, mjsSkin* pskin);
void OneMaterial(tinyxml2::XMLElement* elem, mjsMaterial* pmaterial);
void OneJoint(tinyxml2::XMLElement* elem, mjsJoint* pjoint);
void OneGeom(tinyxml2::XMLElement* elem, mjsGeom* pgeom);
void OneSite(tinyxml2::XMLElement* elem, mjsSite* site);
void OneCamera(tinyxml2::XMLElement* elem, mjsCamera* pcamera);
void OneLight(tinyxml2::XMLElement* elem, mjsLight* plight);
void OnePair(tinyxml2::XMLElement* elem, mjsPair* ppair);
void OneEquality(tinyxml2::XMLElement* elem, mjsEquality* pequality);
void OneTendon(tinyxml2::XMLElement* elem, mjsTendon* ptendon);
void OneActuator(tinyxml2::XMLElement* elem, mjsActuator* pactuator);
void OneComposite(tinyxml2::XMLElement* elem, mjsBody* pbody, mjsDefault* def);
void OneFlexcomp(tinyxml2::XMLElement* elem, mjsBody* pbody);
void OnePlugin(tinyxml2::XMLElement* elem, mjsPlugin* plugin);
mjXSchema schema; // schema used for validation
mjmDefault* GetClass(tinyxml2::XMLElement* section); // get default class name
mjsDefault* GetClass(tinyxml2::XMLElement* section); // get default class name
bool readingdefaults; // true while reading defaults
src/xml/xml_native_writer.cc
+17 -17
View File
@@ -307,6 +307,7 @@ void mjXWriter::OneJoint(XMLElement* elem, mjCJoint* pjoint, mjCDef* def) {
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);
@@ -553,33 +554,33 @@ void mjXWriter::OneEquality(XMLElement* elem, mjCEquality* peq, mjCDef* def) {
switch (peq->type) {
case mjEQ_CONNECT:
WriteAttrTxt(elem, "body1", mjm_getString(peq->name1));
WriteAttrTxt(elem, "body2", mjm_getString(peq->name2));
WriteAttrTxt(elem, "body1", mjs_getString(peq->name1));
WriteAttrTxt(elem, "body2", mjs_getString(peq->name2));
WriteAttr(elem, "anchor", 3, peq->data);
break;
case mjEQ_WELD:
WriteAttrTxt(elem, "body1", mjm_getString(peq->name1));
WriteAttrTxt(elem, "body2", mjm_getString(peq->name2));
WriteAttrTxt(elem, "body1", mjs_getString(peq->name1));
WriteAttrTxt(elem, "body2", mjs_getString(peq->name2));
WriteAttr(elem, "anchor", 3, peq->data);
WriteAttr(elem, "torquescale", 1, peq->data+10);
WriteAttr(elem, "relpose", 7, peq->data+3);
break;
case mjEQ_JOINT:
WriteAttrTxt(elem, "joint1", mjm_getString(peq->name1));
WriteAttrTxt(elem, "joint2", mjm_getString(peq->name2));
WriteAttrTxt(elem, "joint1", mjs_getString(peq->name1));
WriteAttrTxt(elem, "joint2", mjs_getString(peq->name2));
WriteAttr(elem, "polycoef", 5, peq->data);
break;
case mjEQ_TENDON:
WriteAttrTxt(elem, "tendon1", mjm_getString(peq->name1));
WriteAttrTxt(elem, "tendon2", mjm_getString(peq->name2));
WriteAttrTxt(elem, "tendon1", mjs_getString(peq->name1));
WriteAttrTxt(elem, "tendon2", mjs_getString(peq->name2));
WriteAttr(elem, "polycoef", 5, peq->data);
break;
case mjEQ_FLEX:
WriteAttrTxt(elem, "flex", mjm_getString(peq->name1));
WriteAttrTxt(elem, "flex", mjs_getString(peq->name1));
break;
default:
@@ -731,16 +732,16 @@ void mjXWriter::OneActuator(XMLElement* elem, mjCActuator* pact, mjCDef* def) {
// write plugin
void mjXWriter::OnePlugin(XMLElement* elem, mjmPlugin* plugin) {
const std::string instance_name = std::string(mjm_getString(plugin->instance_name));
const std::string plugin_name = std::string(mjm_getString(plugin->name));
void mjXWriter::OnePlugin(XMLElement* elem, 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()) {
WriteAttrTxt(elem, "instance", instance_name);
} else {
WriteAttrTxt(elem, "plugin", plugin_name);
const mjpPlugin* pplugin = mjp_getPluginAtSlot(
((mjCPlugin*)plugin->instance)->spec.plugin_slot);
const char* c = &((mjCPlugin*)plugin->instance)->flattened_attributes[0];
static_cast<mjCPlugin*>(plugin->instance)->spec.plugin_slot);
const char* c = &(static_cast<mjCPlugin*>(plugin->instance)->flattened_attributes[0]);
for (int i = 0; i < pplugin->nattribute; ++i) {
std::string value(c);
if (!value.empty()) {
@@ -783,13 +784,13 @@ string mjXWriter::Write(char *error, size_t error_sz) {
// create document and root
XMLDocument doc;
XMLElement* root = doc.NewElement("mujoco");
root->SetAttribute("model", mjm_getString(model->modelname));
root->SetAttribute("model", mjs_getString(model->modelname));
// insert root
doc.InsertFirstChild(root);
// write comment if present
string text = mjm_getString(model->comment);
string text = mjs_getString(model->comment);
if (!text.empty()) {
XMLComment* comment = doc.NewComment(text.c_str());
root->LinkEndChild(comment);
@@ -937,7 +938,6 @@ void mjXWriter::Option(XMLElement* root) {
WRITEENBL("energy", mjENBL_ENERGY)
WRITEENBL("fwdinv", mjENBL_FWDINV)
WRITEENBL("invdiscrete", mjENBL_INVDISCRETE)
WRITEENBL("sensornoise", mjENBL_SENSORNOISE)
WRITEENBL("multiccd", mjENBL_MULTICCD)
WRITEENBL("island", mjENBL_ISLAND)
#undef WRITEENBL
src/xml/xml_native_writer.h
+1 -1
View File
@@ -72,7 +72,7 @@ class mjXWriter : public mjXBase {
void OneEquality(tinyxml2::XMLElement* elem, mjCEquality* pequality, mjCDef* def);
void OneTendon(tinyxml2::XMLElement* elem, mjCTendon* ptendon, mjCDef* def);
void OneActuator(tinyxml2::XMLElement* elem, mjCActuator* pactuator, mjCDef* def);
void OnePlugin(tinyxml2::XMLElement* elem, mjmPlugin* plugin);
void OnePlugin(tinyxml2::XMLElement* elem, mjsPlugin* plugin);
bool writingdefaults; // true during defaults write
};
src/xml/xml_urdf.cc
+50 -50
View File
@@ -20,7 +20,6 @@
#include <mujoco/mjmodel.h>
#include "user/user_api.h"
#include "user/user_objects.h"
#include "user/user_util.h"
#include "xml/xml_native_reader.h"
#include "xml/xml_urdf.h"
@@ -112,7 +111,7 @@ void mjXURDF::Parse(
// get model name
std::string modelname;
if (ReadAttrTxt(root, "name", modelname)) {
mjm_setString(model->modelname, modelname.c_str());
mjs_setString(model->modelname, modelname.c_str());
}
// find and register all materials
@@ -206,16 +205,16 @@ void mjXURDF::Parse(
// override the pose for the base link and add a free joint
for (int i = 0; i < (int)urName.size(); i++) {
if (urParent[i] < 0) {
mjmBody* world = mjm_findBody(model, "world");
mjmBody* pbody = mjm_findChild(world, urName[i].c_str());
mjsBody* world = mjs_findBody(model, "world");
mjsBody* pbody = mjs_findChild(world, urName[i].c_str());
mjuu_copyvec(pbody->pos, pos, 3);
mjuu_copyvec(pbody->quat, quat, 4);
// add a free joint to allow motion of the body
// if the mass is 0, assume the object is static
if (!static_body && pbody->mass > 0) {
mjmJoint* pjoint = mjm_addJoint(pbody, 0);
mjm_setString(pjoint->name, (urName[i] + "_free_joint").c_str());
mjsJoint* pjoint = mjs_addJoint(pbody, 0);
mjs_setString(pjoint->name, (urName[i] + "_free_joint").c_str());
pjoint->type = mjJNT_FREE;
}
}
@@ -226,14 +225,14 @@ void mjXURDF::Parse(
void mjXURDF::Body(XMLElement* body_elem) {
std::string name, text;
XMLElement *elem, *temp, *temp1;
mjmBody *pbody, *world;
mjmGeom* pgeom;
mjsBody *pbody, *world;
mjsGeom* pgeom;
// get body name and pointer to mjmBody
// get body name and pointer to mjsBody
ReadAttrTxt(body_elem, "name", name, true);
name = GetPrefixedName(name);
world = mjm_findBody(model, "world");
pbody = mjm_findChild(world, name.c_str());
world = mjs_findBody(model, "world");
pbody = mjs_findChild(world, name.c_str());
if (!pbody) {
throw mjXError(body_elem, "URDF body not found"); // SHOULD NOT OCCUR
}
@@ -272,7 +271,7 @@ void mjXURDF::Body(XMLElement* body_elem) {
// lquat = rotation from specified to default (joint/body) inertial frame
double lquat[4] = {1, 0, 0, 0};
double tmpquat[4] = {1, 0, 0, 0};
const char* altres = mjm_setFullInertia(pbody, lquat, pbody->inertia);
const char* altres = mjs_setFullInertia(pbody, lquat, pbody->inertia);
// inertia are sometimes 0 in URDF files: ignore error in altres, fix later
(void) altres;
@@ -326,7 +325,7 @@ void mjXURDF::Body(XMLElement* body_elem) {
mjXUtil::ReadAttrTxt(elem, "name", geom_name);
name = GetPrefixedName(name);
if (urGeomNames.find(geom_name) == urGeomNames.end()) {
mjm_setString(pgeom->name, geom_name.c_str());
mjs_setString(pgeom->name, geom_name.c_str());
urGeomNames.insert(geom_name);
} else {
std::cerr << "WARNING: Geom with duplicate name '" << geom_name
@@ -349,7 +348,7 @@ void mjXURDF::Body(XMLElement* body_elem) {
mjXUtil::ReadAttrTxt(elem, "name", geom_name);
geom_name = GetPrefixedName(geom_name);
if (urGeomNames.find(geom_name) == urGeomNames.end()) {
mjm_setString(pgeom->name, geom_name.c_str());
mjs_setString(pgeom->name, geom_name.c_str());
urGeomNames.insert(geom_name);
} else {
std::cerr << "WARNING: Geom with duplicate name '" << geom_name
@@ -374,8 +373,8 @@ void mjXURDF::Parse(XMLElement* root) {
void mjXURDF::Joint(XMLElement* joint_elem) {
std::string jntname, name, text;
XMLElement *elem;
mjmBody *pbody, *parent, *world;
mjmJoint *pjoint=0, *pjoint1=0, *pjoint2=0;
mjsBody *pbody, *parent, *world;
mjsJoint *pjoint=0, *pjoint1=0, *pjoint2=0;
int jointtype;
// get type and name
@@ -390,8 +389,8 @@ void mjXURDF::Joint(XMLElement* joint_elem) {
elem = FindSubElem(joint_elem, "parent", true);
ReadAttrTxt(elem, "link", name, true);
name = GetPrefixedName(name);
world = mjm_findBody(model, "world");
parent = mjm_findChild(world, name.c_str());
world = mjs_findBody(model, "world");
parent = mjs_findChild(world, name.c_str());
if (!parent) { // SHOULD NOT OCCUR
throw mjXError(elem, "invalid parent name in URDF joint definition");
}
@@ -400,8 +399,8 @@ void mjXURDF::Joint(XMLElement* joint_elem) {
elem = FindSubElem(joint_elem, "child", true);
ReadAttrTxt(elem, "link", name, true);
name = GetPrefixedName(name);
world = mjm_findBody(model, "world");
pbody = mjm_findChild(world, name.c_str());
world = mjs_findBody(model, "world");
pbody = mjs_findChild(world, name.c_str());
if (!pbody) { // SHOULD NOT OCCUR
throw mjXError(elem, "invalid child name in URDF joint definition");
}
@@ -418,16 +417,16 @@ void mjXURDF::Joint(XMLElement* joint_elem) {
switch (jointtype) {
case 0: // revolute
case 1: // continuous
pjoint = mjm_addJoint(pbody, 0);
mjm_setString(pjoint->name, jntname.c_str());
pjoint = mjs_addJoint(pbody, 0);
mjs_setString(pjoint->name, jntname.c_str());
pjoint->type = mjJNT_HINGE;
mjuu_setvec(pjoint->pos, 0, 0, 0);
mjuu_copyvec(pjoint->axis, axis, 3);
break;
case 2: // prismatic
pjoint = mjm_addJoint(pbody, 0);
mjm_setString(pjoint->name, jntname.c_str());
pjoint = mjs_addJoint(pbody, 0);
mjs_setString(pjoint->name, jntname.c_str());
pjoint->type = mjJNT_SLIDE;
mjuu_setvec(pjoint->pos, 0, 0, 0);
mjuu_copyvec(pjoint->axis, axis, 3);
@@ -437,8 +436,8 @@ void mjXURDF::Joint(XMLElement* joint_elem) {
return;
case 4: // floating
pjoint = mjm_addJoint(pbody, 0);
mjm_setString(pjoint->name, jntname.c_str());
pjoint = mjs_addJoint(pbody, 0);
mjs_setString(pjoint->name, jntname.c_str());
pjoint->type = mjJNT_FREE;
break;
@@ -448,8 +447,8 @@ void mjXURDF::Joint(XMLElement* joint_elem) {
mjuu_quat2mat(mat, quat);
// construct slider along x
pjoint = mjm_addJoint(pbody, 0);
mjm_setString(pjoint->name, (jntname + "_TX").c_str());
pjoint = mjs_addJoint(pbody, 0);
mjs_setString(pjoint->name, (jntname + "_TX").c_str());
pjoint->type = mjJNT_SLIDE;
tmpaxis[0] = mat[0];
tmpaxis[1] = mat[3];
@@ -458,8 +457,8 @@ void mjXURDF::Joint(XMLElement* joint_elem) {
mjuu_copyvec(pjoint->axis, tmpaxis, 3);
// construct slider along y
pjoint1 = mjm_addJoint(pbody, 0);
mjm_setString(pjoint1->name, (jntname + "_TY").c_str());
pjoint1 = mjs_addJoint(pbody, 0);
mjs_setString(pjoint1->name, (jntname + "_TY").c_str());
pjoint1->type = mjJNT_SLIDE;
tmpaxis[0] = mat[1];
tmpaxis[1] = mat[4];
@@ -468,8 +467,8 @@ void mjXURDF::Joint(XMLElement* joint_elem) {
mjuu_copyvec(pjoint1->axis, tmpaxis, 3);
// construct hinge around z = locaxis
pjoint2 = mjm_addJoint(pbody, 0);
mjm_setString(pjoint2->name, (jntname + "_RZ").c_str());
pjoint2 = mjs_addJoint(pbody, 0);
mjs_setString(pjoint2->name, (jntname + "_RZ").c_str());
pjoint2->type = mjJNT_HINGE;
mjuu_setvec(pjoint2->pos, 0, 0, 0);
mjuu_copyvec(pjoint2->axis, axis, 3);
@@ -506,7 +505,7 @@ void mjXURDF::Joint(XMLElement* joint_elem) {
// parse origin and geometry elements of visual or collision
mjmGeom* mjXURDF::Geom(XMLElement* geom_elem, mjmBody* pbody, bool collision) {
mjsGeom* mjXURDF::Geom(XMLElement* geom_elem, mjsBody* pbody, bool collision) {
XMLElement *elem, *temp;
std::string text, meshfile;
@@ -514,8 +513,8 @@ mjmGeom* mjXURDF::Geom(XMLElement* geom_elem, mjmBody* pbody, bool collision) {
elem = FindSubElem(geom_elem, "geometry", true);
// add BOX geom, modify type later
mjmGeom* pgeom = mjm_addGeom(pbody, 0);
mjm_setString(pgeom->name, "");
mjsGeom* pgeom = mjs_addGeom(pbody, 0);
mjs_setString(pgeom->name, "");
pgeom->type = mjGEOM_BOX;
if (collision) {
pgeom->contype = 1;
@@ -576,19 +575,19 @@ mjmGeom* mjXURDF::Geom(XMLElement* geom_elem, mjmBody* pbody, bool collision) {
meshname = mjuu_stripext(meshname);
// look for existing mesh
mjmMesh* mesh = mjm_findMesh(model, meshname.c_str());
mjmMesh* pmesh = 0;
mjsMesh* mesh = mjs_findMesh(model, meshname.c_str());
mjsMesh* pmesh = 0;
// does not exist: create
if (!mesh) {
pmesh = mjm_addMesh(model, 0);
pmesh = mjs_addMesh(model, 0);
}
// exists with different scale: append name with '1', create
else if (mesh->scale[0]!=meshscale[0] ||
mesh->scale[1]!=meshscale[1] ||
mesh->scale[2]!=meshscale[2]) {
pmesh = mjm_addMesh(model, 0);
pmesh = mjs_addMesh(model, 0);
meshname = meshname + "1";
}
@@ -598,9 +597,9 @@ mjmGeom* mjXURDF::Geom(XMLElement* geom_elem, mjmBody* pbody, bool collision) {
}
// set fields
mjm_setString(pmesh->file, meshfile.c_str());
mjm_setString(pmesh->name, meshname.c_str());
mjm_setString(pgeom->meshname, meshname.c_str());
mjs_setString(pmesh->file, meshfile.c_str());
mjs_setString(pmesh->name, meshname.c_str());
mjs_setString(pgeom->meshname, meshname.c_str());
pmesh->scale[0] = meshscale[0];
pmesh->scale[1] = meshscale[1];
pmesh->scale[2] = meshscale[2];
@@ -633,9 +632,10 @@ void mjXURDF::Origin(XMLElement* origin_elem, double* pos, double* quat) {
ReadAttr(temp, "xyz", 3, pos, text);
// orientation
mjCAlternative alt;
mjsOrientation alt;
mjs_defaultOrientation(alt);
if (ReadAttr(temp, "rpy", 3, alt.euler, text)) {
alt.Set(quat, 0, "XYZ");
mjs_resolveOrientation(quat, 0, "XYZ", &alt);
}
}
}
@@ -683,22 +683,22 @@ void mjXURDF::AddBody(std::string name) {
// add body with given number to the mjCModel tree, process children
void mjXURDF::AddToTree(int n) {
// get pointer to parent in mjCModel tree
mjmBody *parent = 0, *child = 0, *world = 0;
mjsBody *parent = 0, *child = 0, *world = 0;
if (urParent[n]>=0) {
world = mjm_findBody(model, "world");
parent = mjm_findChild(world, urName[urParent[n]].c_str());
world = mjs_findBody(model, "world");
parent = mjs_findChild(world, urName[urParent[n]].c_str());
if (!parent)
throw mjXError(0, "URDF body parent should already be in tree: %s",
urName[urParent[n]].c_str()); // SHOULD NOT OCCUR
} else {
parent = mjm_findBody(model, "world");
parent = mjs_findBody(model, "world");
}
// add this body
if (urName[n] != "world") {
child = mjm_addBody(parent, 0);
mjm_setString(child->name, urName[n].c_str());
child = mjs_addBody(parent, 0);
mjs_setString(child->name, urName[n].c_str());
}
// add children recursively
src/xml/xml_urdf.h
+2 -2
View File
@@ -51,8 +51,8 @@ class mjXURDF : public mjXBase {
void AddToTree(int n); // add body to mjCModel tree
void Body(tinyxml2::XMLElement* body_elem); // parse body
void Joint(tinyxml2::XMLElement* joint_elem); // parse joint
mjmGeom* Geom(tinyxml2::XMLElement* geom_elem,
mjmBody* pbody, bool collision); // parse origin and geometry of geom
mjsGeom* Geom(tinyxml2::XMLElement* geom_elem,
mjsBody* pbody, bool collision); // parse origin and geometry of geom
void Origin(tinyxml2::XMLElement* origin_elem, double* pos, double* quat); // parse origin element
void MakeMaterials(tinyxml2::XMLElement* elem); // find all materials recursively
src/xml/xml_util.cc
+3 -4
View File
@@ -523,14 +523,13 @@ bool mjXUtil::SameVector(const T* vec1, const T* vec2, int n) {
return false;
}
bool same = true;
for (int i=0; i<n; i++) {
for (int i = 0; i < n; i++) {
if (std::abs(vec1[i] - vec2[i]) > std::numeric_limits<T>::epsilon()) {
same = false;
return false;
}
}
return same;
return true;
}
template bool mjXUtil::SameVector(const double* vec1, const double* vec2, int n);
test/engine/engine_forward_test.cc
+74
View File
@@ -765,6 +765,80 @@ TEST_F(ActuatorTest, ActuatorForceClamping) {
mj_deleteModel(model);
}
// Apply gravity compensation via actuators
TEST_F(ActuatorTest, ActuatorGravcomp) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body gravcomp="1">
<joint name="joint" type="slide" axis="0 0 1"
actuatorfrcrange="-2 2" actuatorgravcomp="true"/>
<geom type="box" size=".05 .05 .05" mass="1"/>
</body>
</worldbody>
<actuator>
<motor name="actuator" joint="joint"/>
</actuator>
<sensor>
<actuatorfrc actuator="actuator"/>
<jointactuatorfrc joint="joint"/>
</sensor>
</mujoco>
)";
mjModel* model = LoadModelFromString(xml);
mjData* data = mj_makeData(model);
mj_forward(model, data);
// expect force clamping as specified in the model
EXPECT_EQ(data->actuator_force[0], 0);
EXPECT_EQ(data->qfrc_actuator[0], 2);
EXPECT_EQ(data->qfrc_passive[0], 0);
EXPECT_EQ(data->sensordata[0], 0);
EXPECT_EQ(data->sensordata[1], 2);
// reduce gravity so gravcomp is not clamped
model->opt.gravity[2] = -1;
mj_forward(model, data);
EXPECT_EQ(data->actuator_force[0], 0);
EXPECT_EQ(data->qfrc_actuator[0], 1);
EXPECT_EQ(data->qfrc_passive[0], 0);
EXPECT_EQ(data->sensordata[0], 0);
EXPECT_EQ(data->sensordata[1], 1);
// add control, see that it adds up
data->ctrl[0] = 0.5;
mj_forward(model, data);
EXPECT_EQ(data->actuator_force[0], 0.5);
EXPECT_EQ(data->qfrc_actuator[0], 1.5);
EXPECT_EQ(data->qfrc_passive[0], 0);
EXPECT_EQ(data->sensordata[0], 0.5);
EXPECT_EQ(data->sensordata[1], 1.5);
// add larger control, expect clamping
data->ctrl[0] = 1.5;
mj_forward(model, data);
EXPECT_EQ(data->actuator_force[0], 1.5);
EXPECT_EQ(data->qfrc_actuator[0], 2);
EXPECT_EQ(data->qfrc_passive[0], 0);
EXPECT_EQ(data->sensordata[0], 1.5);
EXPECT_EQ(data->sensordata[1], 2);
// disable actgravcomp, expect gravcomp as a passive force
model->jnt_actgravcomp[0] = 0;
mj_forward(model, data);
EXPECT_EQ(data->actuator_force[0], 1.5);
EXPECT_EQ(data->qfrc_actuator[0], 1.5);
EXPECT_EQ(data->qfrc_passive[0], 1);
EXPECT_EQ(data->sensordata[0], 1.5);
EXPECT_EQ(data->sensordata[1], 1.5);
mj_deleteData(data);
mj_deleteModel(model);
}
// ----------------------- filterexact actuators -------------------------------
using FilterExactTest = MujocoTest;
test/engine/engine_resource_test.cc
+33
View File
@@ -15,7 +15,10 @@
// Tests for engine/engine_resource.c
#include <array>
#include <cstdint>
#include <cstring>
#include <ctime>
#include <string>
#include <gmock/gmock.h>
#include <gtest/gtest.h>
@@ -24,6 +27,7 @@
#include <mujoco/mujoco.h>
#include "src/engine/engine_plugin.h"
#include "src/engine/engine_resource.h"
#include "src/engine/engine_util_misc.h"
#include "test/fixture.h"
namespace mujoco {
@@ -327,5 +331,34 @@ TEST_F(ResourceTest, NameWithInvalidPrefix) {
ASSERT_THAT(resource, IsNull());
}
TEST_F(ResourceTest, OSFilesystemTimestamps) {
time_t t;
// some random file
const char* const file = "engine/testdata/collision_box/boxbox_deep.xml";
const std::string xml_path = GetTestDataFilePath(file);
mjResource* resource = mju_openResource(xml_path.c_str(), nullptr, 0);
mju_decodeBase64((uint8_t*) &t, resource->timestamp);
// equal timestamps
EXPECT_EQ(mju_isModifiedResource(resource, resource->timestamp), 0);
std::array<char, 512> test_timestamp;
// older resource timestamp
t++;
mju_encodeBase64(test_timestamp.data(), (uint8_t*) &t, sizeof(time_t));
EXPECT_EQ(mju_isModifiedResource(resource, test_timestamp.data()), -1);
// newer resource timestamp
t -= 2;
mju_encodeBase64(test_timestamp.data(), (uint8_t*) &t, sizeof(time_t));
EXPECT_EQ(mju_isModifiedResource(resource, test_timestamp.data()), 1);
mju_closeResource(resource);
}
} // namespace
} // namespace mujoco
test/engine/engine_util_blas_test.cc
+33
View File
@@ -95,5 +95,38 @@ TEST_F(EngineUtilBlasTest, MjuSymmetrize) {
3, 3.5, 3));
}
TEST_F(EngineUtilBlasTest, MjuMulMat3) {
const mjtNum mat1[9] = {
1, 2, 3,
4, 5, 6,
7, 8, 9
};
const mjtNum mat2[9] = {
2, 3, 4,
5, 6, 7,
8, 9, 10
};
mjtNum res1[9] = {0};
mjtNum res2[9] = {0};
mju_mulMatMat3(res1, mat1, mat2);
mju_mulMatMat(res2, mat1, mat2, 3, 3, 3);
for (int i = 0; i < 9; ++i) {
EXPECT_EQ(res1[i], res2[i]);
}
mju_mulMatTMat3(res1, mat1, mat2);
mju_mulMatTMat(res2, mat1, mat2, 3, 3, 3);
for (int i = 0; i < 9; ++i) {
EXPECT_EQ(res1[i], res2[i]);
}
mju_mulMatMatT3(res1, mat1, mat2);
mju_mulMatMatT(res2, mat1, mat2, 3, 3, 3);
for (int i = 0; i < 9; ++i) {
EXPECT_EQ(res1[i], res2[i]);
}
}
} // namespace
} // namespace mujoco
test/engine/engine_vfs_test.cc
+31 -2
View File
@@ -20,6 +20,8 @@
#include <gmock/gmock.h>
#include <gtest/gtest.h>
#include <mujoco/mujoco.h>
#include "src/engine/engine_resource.h"
#include "src/engine/engine_vfs.h"
#include "test/fixture.h"
namespace mujoco {
@@ -28,7 +30,7 @@ namespace {
using ::testing::NotNull;
using EngineVfsTest = MujocoTest;
TEST_F(EngineVfsTest, AddFileTest) {
TEST_F(EngineVfsTest, AddFile) {
constexpr char path[] = "engine/testdata/actuation/";
const std::string dir = GetTestDataFilePath(path);
std::string file1 = "activation.xml";
@@ -81,7 +83,7 @@ TEST_F(EngineVfsTest, AddFileTest) {
mj_deleteVFS(vfs.get());
}
TEST_F(EngineVfsTest, AddBufferTest) {
TEST_F(EngineVfsTest, AddBuffer) {
auto vfs = std::make_unique<mjVFS>();
mj_defaultVFS(vfs.get());
std::string buffer = "<mujoco/>";
@@ -94,5 +96,32 @@ TEST_F(EngineVfsTest, AddBufferTest) {
mj_deleteVFS(vfs.get());
}
TEST_F(EngineVfsTest, Timestamps) {
static constexpr char cube[] = R"(
v -0.500000 -0.500000 0.500000
v 0.500000 -0.500000 0.500000
v -0.500000 0.500000 0.500000
v 0.500000 0.500000 0.500000
v -0.500000 0.500000 -0.500000
v 0.500000 0.500000 -0.500000
v -0.500000 -0.500000 -0.500000
v 0.500000 -0.500000 -0.500000)";
auto vfs = std::make_unique<mjVFS>();
mj_defaultVFS(vfs.get());
mj_addBufferVFS(vfs.get(), "cube.obj", cube, sizeof(cube));
mjResource* resource = mju_openVfsResource("cube.obj", vfs.get());
// same timestamps
EXPECT_EQ(mju_isModifiedResource(resource, resource->timestamp), 0);
// different timestamps
EXPECT_EQ(mju_isModifiedResource(resource, "QQ=="), 1);
mju_closeResource(resource);
mj_deleteVFS(vfs.get());
}
} // namespace
} // namespace mujoco
test/engine/testdata/actuation/refsite.xml
Vendored
+6 -6
View File
@@ -6,7 +6,7 @@
(e.g. 4 consecutive ball joints is not a realistic kinematic design)
-->
<option viscosity="10" integrator="implicit">
<flag gravity="disable" contact="disable"/>
<flag contact="disable"/>
</option>
<statistic meansize=".05"/>
@@ -29,7 +29,7 @@
commands is achievable by individual joint actuators with the specified torque limits.
See https://mujoco.readthedocs.io/en/latest//modeling.html#actuator-force-clamping
-->
<joint stiffness="1e-1" actuatorfrcrange="-1 1"/>
<joint armature="1e-3" damping="1e-3" stiffness="1e-2" actuatorfrcrange="-1 1" actuatorgravcomp="true"/>
<site type="box" size=".012 .012 .012" rgba=".7 .7 .8 1"/>
</default>
@@ -37,19 +37,19 @@
<light pos="0 0 2"/>
<geom type="box" size=".25 .25 .01" pos="0 0 -.01"/>
<site name="reference" pos="0 0 .25"/>
<body name="arm" pos="-.25 .25 0">
<body name="arm" pos="-.25 .25 0" gravcomp="1">
<joint axis="1 0 0"/>
<joint axis="0 1 0"/>
<geom type="box" size=".01" fromto="0 0 0 0 0 .25"/>
<body pos="0 0 .25">
<body pos="0 0 .25" gravcomp="1">
<joint axis="0 1 0"/>
<joint axis="0 0 1"/>
<geom type="box" size=".01" fromto="0 0 0 .25 0 0"/>
<body pos=".25 0 0">
<body pos=".25 0 0" gravcomp="1">
<joint axis="1 0 0"/>
<joint axis="0 0 1"/>
<geom type="box" size=".01" fromto="0 0 0 0 -.2 0"/>
<body pos="0 -.2 0">
<body pos="0 -.2 0" gravcomp="1">
<joint axis="1 0 0"/>
<joint axis="0 0 1"/>
<joint axis="0 1 0"/>
test/sample/testspeed_test.sh
+8 -2
View File
@@ -32,8 +32,14 @@ test_model() {
# this test can take several minutes under ASAN
return 0
fi
if [[ "$model" == */benchmark/testdata/humanoid200.xml
|| "$model" == */engine/testdata/collision_convex/stacked_boxes.xml
if [[ "$model" == */benchmark/testdata/humanoid200.xml ||
"$model" == */engine/testdata/collision_convex/stacked_boxes.xml ||
"$model" == */user/testdata/shark_41_ascii_gmshApp.xml ||
"$model" == */user/testdata/shark_22_ascii_fTetWild.xml ||
"$model" == */user/testdata/shark_22_ascii_gmshApp.xml ||
"$model" == */user/testdata/shark_22_binary_fTetWild.xml ||
"$model" == */user/testdata/shark_41_binary_gmshApp.xml ||
"$model" == */user/testdata/shark_22_binary_gmshApp.xml
]]; then
iterations=2
fi
test/testdata/model.xml
Vendored
+16 -2
View File
@@ -17,8 +17,13 @@
1.618 0 -1
-1.618 0 1
-1.618 0 -1"/>
<hfield name="hfield" nrow="3" ncol="3" size=".2 .2 .1 .03"/>
<hfield name="hfield" nrow="3" ncol="3" size=".2 .2 .03 .03"
elevation="1 0 1
0 1 0
1 0 1"/>
<texture builtin="gradient" height="100" rgb1="1 1 1" rgb2="0 0 0" type="skybox" width="100"/>
<texture name="grid" type="cube" builtin="checker" width="32" height="32" rgb1=".1 .2 .3" rgb2="1 1 1"/>
<material name="grid" texture="grid" texrepeat="1 1" texuniform="true" reflectance=".2"/>
</asset>
<default>
@@ -41,7 +46,7 @@
<geom name="floor" type="plane" size="4 4 .1"/>
<geom type="hfield" hfield="hfield" pos="-.4 .6 .05" rgba="0 0 1 1"/>
<body name="head" pos="0 0 .7">
<geom type="ellipsoid" size=".2 .2 .4" rgba="1 1 0 1" density="100"/>
<geom type="ellipsoid" size=".2 .2 .4" density="100" material="grid"/>
<site name="head" pos="0 0 .4" size=".1 .1 .05" type="box"/>
<site name="anchor0" pos="0 .2 0"/>
<site name="rf" pos="0 0 -0.41" zaxis="0 0 -1"/>
@@ -161,4 +166,13 @@
qpos="0.0133625 -0.013799 0.659429 0.999776 0.019159 0.00902704 0.000572574 -0.00776575 0.0983289 0.999861 -0.00869796 -0.0141805 0.000912197 0.00700633 0.0930296 0.00637469 -0.00313231 0.119692 0.0051478 -0.0469067 0 -0.500224 0.300659 0.69913 1 -0.000367766 -0.000265698 -0.000109715 -0.319851 0.525628 0.702735 0.29858 0.698263 -0.0490604 -0.648746 -0.579432 0.4206 0.749601 0.900145 -0.208463 0.346664 -0.161576 -0.200033 0.300084 0.976421 0.878429 -0.00171382 0.00127086 -0.477867"
qvel="0.508861 -0.243772 -0.327162 0.549686 1.13746 0.0282475 -0.0865243 1.06414 -0.239958 -0.532111 -0.00133789 -0.0654491 0.533148 -0.253161 0.122149 0.824764 0.723529 -0.674175 0 -0.00154509 -0.00568917 0.00479921 0.00541817 0.00115895 0.00317796 -0.160161 -0.599488 -1.38103 14.9789 -1.05454 -13.9252 -0.31223 -0.0102486 -0.405851 5.43992 -4.71514 0.114928 -0.00123758 0.000187845 -0.244109 -0.190199 0.0697975 -16.3286"/>
</keyframe>
<custom>
<numeric name="numeric" data="1 2 3"/>
<text name="text" data="abc"/>
<tuple name="tuple">
<element objtype="body" objname="cylinder" prm="1"/>
<element objtype="joint" objname="hipz_0" prm="2"/>
</tuple>
</custom>
</mujoco>

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