Merge branch 'google-deepmind:main' into main
This commit is contained in:
Abhishek Joshi
@@ -250,7 +250,7 @@ jobs:
|
|||||||
shell: bash
|
shell: bash
|
||||||
run:
|
run:
|
||||||
source ${{ matrix.tmpdir }}/venv/bin/activate &&
|
source ${{ matrix.tmpdir }}/venv/bin/activate &&
|
||||||
pytest -n auto -v --pyargs mujoco.mjx
|
pytest -n auto -v -k 'not IntegrationTest' --pyargs mujoco.mjx
|
||||||
- name: Notify team chat
|
- name: Notify team chat
|
||||||
shell: bash
|
shell: bash
|
||||||
env:
|
env:
|
||||||
|
|||||||
+1
-1
@@ -28,7 +28,7 @@ set(MSVC_INCREMENTAL_DEFAULT ON)
|
|||||||
|
|
||||||
project(
|
project(
|
||||||
mujoco
|
mujoco
|
||||||
VERSION 3.1.4
|
VERSION 3.1.5
|
||||||
DESCRIPTION "MuJoCo Physics Simulator"
|
DESCRIPTION "MuJoCo Physics Simulator"
|
||||||
HOMEPAGE_URL "https://mujoco.org"
|
HOMEPAGE_URL "https://mujoco.org"
|
||||||
)
|
)
|
||||||
|
|||||||
Vendored
+4
-4
@@ -1,6 +1,6 @@
|
|||||||
1 VERSIONINFO
|
1 VERSIONINFO
|
||||||
FILEVERSION 3,1,4,0
|
FILEVERSION 3,1,5,0
|
||||||
PRODUCTVERSION 3,1,4,0
|
PRODUCTVERSION 3,1,5,0
|
||||||
FILEOS 0x4
|
FILEOS 0x4
|
||||||
FILETYPE 0x1
|
FILETYPE 0x1
|
||||||
{
|
{
|
||||||
@@ -9,9 +9,9 @@ FILETYPE 0x1
|
|||||||
BLOCK "040904b0"
|
BLOCK "040904b0"
|
||||||
{
|
{
|
||||||
VALUE "ProductName", "MuJoCo"
|
VALUE "ProductName", "MuJoCo"
|
||||||
VALUE "ProductVersion", "3.1.4"
|
VALUE "ProductVersion", "3.1.5"
|
||||||
VALUE "FileDescription", "MuJoCo"
|
VALUE "FileDescription", "MuJoCo"
|
||||||
VALUE "FileVersion", "3.1.4"
|
VALUE "FileVersion", "3.1.5"
|
||||||
VALUE "InternalName", "mujoco.dll"
|
VALUE "InternalName", "mujoco.dll"
|
||||||
VALUE "OriginalFilename", "mujoco.dll"
|
VALUE "OriginalFilename", "mujoco.dll"
|
||||||
VALUE "CompanyName", "Google DeepMind"
|
VALUE "CompanyName", "Google DeepMind"
|
||||||
|
|||||||
Vendored
+4
-4
@@ -1,8 +1,8 @@
|
|||||||
MUJOCO ICON "mujoco.ico"
|
MUJOCO ICON "mujoco.ico"
|
||||||
|
|
||||||
1 VERSIONINFO
|
1 VERSIONINFO
|
||||||
FILEVERSION 3,1,4,0
|
FILEVERSION 3,1,5,0
|
||||||
PRODUCTVERSION 3,1,4,0
|
PRODUCTVERSION 3,1,5,0
|
||||||
FILEOS 0x4
|
FILEOS 0x4
|
||||||
FILETYPE 0x1
|
FILETYPE 0x1
|
||||||
{
|
{
|
||||||
@@ -11,9 +11,9 @@ FILETYPE 0x1
|
|||||||
BLOCK "040904b0"
|
BLOCK "040904b0"
|
||||||
{
|
{
|
||||||
VALUE "ProductName", "MuJoCo"
|
VALUE "ProductName", "MuJoCo"
|
||||||
VALUE "ProductVersion", "3.1.4"
|
VALUE "ProductVersion", "3.1.5"
|
||||||
VALUE "FileDescription", "MuJoCo"
|
VALUE "FileDescription", "MuJoCo"
|
||||||
VALUE "FileVersion", "3.1.4"
|
VALUE "FileVersion", "3.1.5"
|
||||||
VALUE "InternalName", "simulate.exe"
|
VALUE "InternalName", "simulate.exe"
|
||||||
VALUE "OriginalFilename", "simulate.exe"
|
VALUE "OriginalFilename", "simulate.exe"
|
||||||
VALUE "CompanyName", "Google DeepMind"
|
VALUE "CompanyName", "Google DeepMind"
|
||||||
|
|||||||
@@ -522,7 +522,7 @@ shown in the table below. Their names are in the format ``mjKEY_XXX``. They corr
|
|||||||
- Maximum number of UI rectangles.
|
- Maximum number of UI rectangles.
|
||||||
Defined in `mjui.h <https://github.com/google-deepmind/mujoco/blob/main/include/mujoco/mjui.h>`_.
|
Defined in `mjui.h <https://github.com/google-deepmind/mujoco/blob/main/include/mujoco/mjui.h>`_.
|
||||||
* - ``mjVERSION_HEADER``
|
* - ``mjVERSION_HEADER``
|
||||||
- 314
|
- 315
|
||||||
- The version of the MuJoCo headers; changes with every release. This is an integer equal to 100x the software
|
- The version of the MuJoCo headers; changes with every release. This is an integer equal to 100x the software
|
||||||
version, so 210 corresponds to version 2.1. Defined in mujoco.h. The API function :ref:`mj_version` returns a
|
version, so 210 corresponds to version 2.1. Defined in mujoco.h. The API function :ref:`mj_version` returns a
|
||||||
number with the same meaning but for the compiled library.
|
number with the same meaning but for the compiled library.
|
||||||
|
|||||||
@@ -2840,6 +2840,16 @@ mju_quatZ2Vec
|
|||||||
|
|
||||||
Construct quaternion performing rotation from z-axis to given vector.
|
Construct quaternion performing rotation from z-axis to given vector.
|
||||||
|
|
||||||
|
.. _mju_euler2Quat:
|
||||||
|
|
||||||
|
mju_euler2Quat
|
||||||
|
~~~~~~~~~~~~~~
|
||||||
|
|
||||||
|
.. mujoco-include:: mju_euler2Quat
|
||||||
|
|
||||||
|
Convert sequence of Euler angles (radians) to quaternion.
|
||||||
|
seq[0,1,2] must be in 'xyzXYZ', lower/upper-case mean intrinsic/extrinsic rotations.
|
||||||
|
|
||||||
.. _Poses:
|
.. _Poses:
|
||||||
|
|
||||||
Poses
|
Poses
|
||||||
|
|||||||
+977
-876
File diff suppressed because it is too large
Load Diff
+125
-121
@@ -7,6 +7,42 @@
|
|||||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
|
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
|
||||||
| mujoco |br| |L| | | .. table:: |
|
| mujoco |br| |L| | | .. table:: |
|
||||||
|
| :ref:`option | \* | :class: mjcf-attributes |
|
||||||
|
| <option>` | | |
|
||||||
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
|
| | | | :ref:`timestep<option-timestep>` | :ref:`apirate<option-apirate>` | :ref:`impratio<option-impratio>` | :ref:`tolerance<option-tolerance>` | |
|
||||||
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
|
| | | | :ref:`ls_tolerance<option-ls_tolerance>` | :ref:`noslip_tolerance<option-noslip_tolerance>` | :ref:`mpr_tolerance<option-mpr_tolerance>` | :ref:`gravity<option-gravity>` | |
|
||||||
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
|
| | | | :ref:`wind<option-wind>` | :ref:`magnetic<option-magnetic>` | :ref:`density<option-density>` | :ref:`viscosity<option-viscosity>` | |
|
||||||
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
|
| | | | :ref:`o_margin<option-o_margin>` | :ref:`o_solref<option-o_solref>` | :ref:`o_solimp<option-o_solimp>` | :ref:`o_friction<option-o_friction>` | |
|
||||||
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
|
| | | | :ref:`integrator<option-integrator>` | :ref:`cone<option-cone>` | :ref:`jacobian<option-jacobian>` | :ref:`solver<option-solver>` | |
|
||||||
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
|
| | | | :ref:`iterations<option-iterations>` | :ref:`ls_iterations<option-ls_iterations>` | :ref:`noslip_iterations<option-noslip_iterations>` | :ref:`mpr_iterations<option-mpr_iterations>` | |
|
||||||
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
|
| | | | :ref:`sdf_iterations<option-sdf_iterations>` | :ref:`sdf_initpoints<option-sdf_initpoints>` | :ref:`actuatorgroupdisable<option-actuatorgroupdisable>` | | |
|
||||||
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
|
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
|
||||||
|
| |_| option |br| |_| |L| | | .. table:: |
|
||||||
|
| :ref:`flag | ? | :class: mjcf-attributes |
|
||||||
|
| <option-flag>` | | |
|
||||||
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
|
| | | | :ref:`constraint<option-flag-constraint>` | :ref:`equality<option-flag-equality>` | :ref:`frictionloss<option-flag-frictionloss>` | :ref:`limit<option-flag-limit>` | |
|
||||||
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
|
| | | | :ref:`contact<option-flag-contact>` | :ref:`passive<option-flag-passive>` | :ref:`gravity<option-flag-gravity>` | :ref:`clampctrl<option-flag-clampctrl>` | |
|
||||||
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
|
| | | | :ref:`warmstart<option-flag-warmstart>` | :ref:`filterparent<option-flag-filterparent>` | :ref:`actuation<option-flag-actuation>` | :ref:`refsafe<option-flag-refsafe>` | |
|
||||||
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
|
| | | | :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:`multiccd<option-flag-multiccd>` | |
|
||||||
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
|
| | | | :ref:`island<option-flag-island>` | | | | |
|
||||||
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
|
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
|
||||||
|
| mujoco |br| |L| | | .. table:: |
|
||||||
| :ref:`compiler | \* | :class: mjcf-attributes |
|
| :ref:`compiler | \* | :class: mjcf-attributes |
|
||||||
| <compiler>` | | |
|
| <compiler>` | | |
|
||||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
@@ -55,83 +91,6 @@
|
|||||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
|
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
|
||||||
| mujoco |br| |L| | | *no attributes* |
|
| mujoco |br| |L| | | *no attributes* |
|
||||||
| :ref:`visual<visual>` | | |
|
|
||||||
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
|
|
||||||
| |_| visual |br| |_| |L| | | .. table:: |
|
|
||||||
| :ref:`global | ? | :class: mjcf-attributes |
|
|
||||||
| <visual-global>` | | |
|
|
||||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
|
||||||
| | | | :ref:`fovy<visual-global-fovy>` | :ref:`ipd<visual-global-ipd>` | :ref:`azimuth<visual-global-azimuth>` | :ref:`elevation<visual-global-elevation>` | |
|
|
||||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
|
||||||
| | | | :ref:`linewidth<visual-global-linewidth>` | :ref:`glow<visual-global-glow>` | :ref:`offwidth<visual-global-offwidth>` | :ref:`offheight<visual-global-offheight>` | |
|
|
||||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
|
||||||
| | | | :ref:`realtime<visual-global-realtime>` | :ref:`ellipsoidinertia<visual-global-ellipsoidinertia>` | :ref:`bvactive<visual-global-bvactive>` | | |
|
|
||||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
|
||||||
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
|
|
||||||
| |_| visual |br| |_| |L| | | .. table:: |
|
|
||||||
| :ref:`quality | ? | :class: mjcf-attributes |
|
|
||||||
| <visual-quality>` | | |
|
|
||||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
|
||||||
| | | | :ref:`shadowsize<visual-quality-shadowsize>` | :ref:`offsamples<visual-quality-offsamples>` | :ref:`numslices<visual-quality-numslices>` | :ref:`numstacks<visual-quality-numstacks>` | |
|
|
||||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
|
||||||
| | | | :ref:`numquads<visual-quality-numquads>` | | | | |
|
|
||||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
|
||||||
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
|
|
||||||
| |_| visual |br| |_| |L| | | .. table:: |
|
|
||||||
| :ref:`headlight | ? | :class: mjcf-attributes |
|
|
||||||
| <visual-headlight>` | | |
|
|
||||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
|
||||||
| | | | :ref:`ambient<visual-headlight-ambient>` | :ref:`diffuse<visual-headlight-diffuse>` | :ref:`specular<visual-headlight-specular>` | :ref:`active<visual-headlight-active>` | |
|
|
||||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
|
||||||
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
|
|
||||||
| |_| visual |br| |_| |L| | | .. table:: |
|
|
||||||
| :ref:`map | ? | :class: mjcf-attributes |
|
|
||||||
| <visual-map>` | | |
|
|
||||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
|
||||||
| | | | :ref:`stiffness<visual-map-stiffness>` | :ref:`stiffnessrot<visual-map-stiffnessrot>` | :ref:`force<visual-map-force>` | :ref:`torque<visual-map-torque>` | |
|
|
||||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
|
||||||
| | | | :ref:`alpha<visual-map-alpha>` | :ref:`fogstart<visual-map-fogstart>` | :ref:`fogend<visual-map-fogend>` | :ref:`znear<visual-map-znear>` | |
|
|
||||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
|
||||||
| | | | :ref:`zfar<visual-map-zfar>` | :ref:`haze<visual-map-haze>` | :ref:`shadowclip<visual-map-shadowclip>` | :ref:`shadowscale<visual-map-shadowscale>` | |
|
|
||||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
|
||||||
| | | | :ref:`actuatortendon<visual-map-actuatortendon>` | | | | |
|
|
||||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
|
||||||
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
|
|
||||||
| |_| visual |br| |_| |L| | | .. table:: |
|
|
||||||
| :ref:`scale | ? | :class: mjcf-attributes |
|
|
||||||
| <visual-scale>` | | |
|
|
||||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
|
||||||
| | | | :ref:`forcewidth<visual-scale-forcewidth>` | :ref:`contactwidth<visual-scale-contactwidth>` | :ref:`contactheight<visual-scale-contactheight>` | :ref:`connect<visual-scale-connect>` | |
|
|
||||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
|
||||||
| | | | :ref:`com<visual-scale-com>` | :ref:`camera<visual-scale-camera>` | :ref:`light<visual-scale-light>` | :ref:`selectpoint<visual-scale-selectpoint>` | |
|
|
||||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
|
||||||
| | | | :ref:`jointlength<visual-scale-jointlength>` | :ref:`jointwidth<visual-scale-jointwidth>` | :ref:`actuatorlength<visual-scale-actuatorlength>` | :ref:`actuatorwidth<visual-scale-actuatorwidth>` | |
|
|
||||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
|
||||||
| | | | :ref:`framelength<visual-scale-framelength>` | :ref:`framewidth<visual-scale-framewidth>` | :ref:`constraint<visual-scale-constraint>` | :ref:`slidercrank<visual-scale-slidercrank>` | |
|
|
||||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
|
||||||
| | | | :ref:`frustum<visual-scale-frustum>` | | | | |
|
|
||||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
|
||||||
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
|
|
||||||
| |_| visual |br| |_| |L| | | .. table:: |
|
|
||||||
| :ref:`rgba | ? | :class: mjcf-attributes |
|
|
||||||
| <visual-rgba>` | | |
|
|
||||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
|
||||||
| | | | :ref:`fog<visual-rgba-fog>` | :ref:`haze<visual-rgba-haze>` | :ref:`force<visual-rgba-force>` | :ref:`inertia<visual-rgba-inertia>` | |
|
|
||||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
|
||||||
| | | | :ref:`joint<visual-rgba-joint>` | :ref:`actuator<visual-rgba-actuator>` | :ref:`actuatornegative<visual-rgba-actuatornegative>` | :ref:`actuatorpositive<visual-rgba-actuatorpositive>` | |
|
|
||||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
|
||||||
| | | | :ref:`com<visual-rgba-com>` | :ref:`camera<visual-rgba-camera>` | :ref:`light<visual-rgba-light>` | :ref:`selectpoint<visual-rgba-selectpoint>` | |
|
|
||||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
|
||||||
| | | | :ref:`connect<visual-rgba-connect>` | :ref:`contactpoint<visual-rgba-contactpoint>` | :ref:`contactforce<visual-rgba-contactforce>` | :ref:`contactfriction<visual-rgba-contactfriction>` | |
|
|
||||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
|
||||||
| | | | :ref:`contacttorque<visual-rgba-contacttorque>` | :ref:`contactgap<visual-rgba-contactgap>` | :ref:`rangefinder<visual-rgba-rangefinder>` | :ref:`constraint<visual-rgba-constraint>` | |
|
|
||||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
|
||||||
| | | | :ref:`slidercrank<visual-rgba-slidercrank>` | :ref:`crankbroken<visual-rgba-crankbroken>` | :ref:`frustum<visual-rgba-frustum>` | :ref:`bv<visual-rgba-bv>` | |
|
|
||||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
|
||||||
| | | | :ref:`bvactive<visual-rgba-bvactive>` | | | | |
|
|
||||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
|
||||||
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
|
|
||||||
| mujoco |br| |L| | | *no attributes* |
|
|
||||||
| :ref:`asset<asset>` | | |
|
| :ref:`asset<asset>` | | |
|
||||||
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
|
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
|
||||||
| |_| asset |br| |_| |L| | | .. table:: |
|
| |_| asset |br| |_| |L| | | .. table:: |
|
||||||
@@ -213,43 +172,7 @@
|
|||||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
| | | | :ref:`texuniform<asset-material-texuniform>` | :ref:`emission<asset-material-emission>` | :ref:`specular<asset-material-specular>` | :ref:`shininess<asset-material-shininess>` | |
|
| | | | :ref:`texuniform<asset-material-texuniform>` | :ref:`emission<asset-material-emission>` | :ref:`specular<asset-material-specular>` | :ref:`shininess<asset-material-shininess>` | |
|
||||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
| | | | :ref:`reflectance<asset-material-reflectance>` | :ref:`rgba<asset-material-rgba>` | | | |
|
| | | | :ref:`reflectance<asset-material-reflectance>` | :ref:`metallic<asset-material-metallic>` | :ref:`roughness<asset-material-roughness>` | :ref:`rgba<asset-material-rgba>` | |
|
||||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
|
||||||
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
|
|
||||||
| mujoco |br| |L| | | .. table:: |
|
|
||||||
| :ref:`option | \* | :class: mjcf-attributes |
|
|
||||||
| <option>` | | |
|
|
||||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
|
||||||
| | | | :ref:`timestep<option-timestep>` | :ref:`apirate<option-apirate>` | :ref:`impratio<option-impratio>` | :ref:`tolerance<option-tolerance>` | |
|
|
||||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
|
||||||
| | | | :ref:`ls_tolerance<option-ls_tolerance>` | :ref:`noslip_tolerance<option-noslip_tolerance>` | :ref:`mpr_tolerance<option-mpr_tolerance>` | :ref:`gravity<option-gravity>` | |
|
|
||||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
|
||||||
| | | | :ref:`wind<option-wind>` | :ref:`magnetic<option-magnetic>` | :ref:`density<option-density>` | :ref:`viscosity<option-viscosity>` | |
|
|
||||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
|
||||||
| | | | :ref:`o_margin<option-o_margin>` | :ref:`o_solref<option-o_solref>` | :ref:`o_solimp<option-o_solimp>` | :ref:`o_friction<option-o_friction>` | |
|
|
||||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
|
||||||
| | | | :ref:`integrator<option-integrator>` | :ref:`cone<option-cone>` | :ref:`jacobian<option-jacobian>` | :ref:`solver<option-solver>` | |
|
|
||||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
|
||||||
| | | | :ref:`iterations<option-iterations>` | :ref:`ls_iterations<option-ls_iterations>` | :ref:`noslip_iterations<option-noslip_iterations>` | :ref:`mpr_iterations<option-mpr_iterations>` | |
|
|
||||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
|
||||||
| | | | :ref:`sdf_iterations<option-sdf_iterations>` | :ref:`sdf_initpoints<option-sdf_initpoints>` | :ref:`actuatorgroupdisable<option-actuatorgroupdisable>` | | |
|
|
||||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
|
||||||
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
|
|
||||||
| |_| option |br| |_| |L| | | .. table:: |
|
|
||||||
| :ref:`flag | ? | :class: mjcf-attributes |
|
|
||||||
| <option-flag>` | | |
|
|
||||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
|
||||||
| | | | :ref:`constraint<option-flag-constraint>` | :ref:`equality<option-flag-equality>` | :ref:`frictionloss<option-flag-frictionloss>` | :ref:`limit<option-flag-limit>` | |
|
|
||||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
|
||||||
| | | | :ref:`contact<option-flag-contact>` | :ref:`passive<option-flag-passive>` | :ref:`gravity<option-flag-gravity>` | :ref:`clampctrl<option-flag-clampctrl>` | |
|
|
||||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
|
||||||
| | | | :ref:`warmstart<option-flag-warmstart>` | :ref:`filterparent<option-flag-filterparent>` | :ref:`actuation<option-flag-actuation>` | :ref:`refsafe<option-flag-refsafe>` | |
|
|
||||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
|
||||||
| | | | :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:`multiccd<option-flag-multiccd>` | |
|
|
||||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
|
||||||
| | | | :ref:`island<option-flag-island>` | | | | |
|
|
||||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
|
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
|
||||||
| mujoco |br| |L| | | .. table:: |
|
| mujoco |br| |L| | | .. table:: |
|
||||||
@@ -369,11 +292,11 @@
|
|||||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
| | | | :ref:`name<body-light-name>` | :ref:`class<body-light-class>` | :ref:`directional<body-light-directional>` | :ref:`castshadow<body-light-castshadow>` | |
|
| | | | :ref:`name<body-light-name>` | :ref:`class<body-light-class>` | :ref:`directional<body-light-directional>` | :ref:`castshadow<body-light-castshadow>` | |
|
||||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
| | | | :ref:`active<body-light-active>` | :ref:`pos<body-light-pos>` | :ref:`dir<body-light-dir>` | :ref:`attenuation<body-light-attenuation>` | |
|
| | | | :ref:`active<body-light-active>` | :ref:`pos<body-light-pos>` | :ref:`dir<body-light-dir>` | :ref:`bulbradius<body-light-bulbradius>` | |
|
||||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
| | | | :ref:`cutoff<body-light-cutoff>` | :ref:`exponent<body-light-exponent>` | :ref:`ambient<body-light-ambient>` | :ref:`diffuse<body-light-diffuse>` | |
|
| | | | :ref:`attenuation<body-light-attenuation>` | :ref:`cutoff<body-light-cutoff>` | :ref:`exponent<body-light-exponent>` | :ref:`ambient<body-light-ambient>` | |
|
||||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
| | | | :ref:`specular<body-light-specular>` | :ref:`mode<body-light-mode>` | :ref:`target<body-light-target>` | | |
|
| | | | :ref:`diffuse<body-light-diffuse>` | :ref:`specular<body-light-specular>` | :ref:`mode<body-light-mode>` | :ref:`target<body-light-target>` | |
|
||||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
|
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
|
||||||
| |_| body |br| |_| |L| | | .. table:: |
|
| |_| body |br| |_| |L| | | .. table:: |
|
||||||
@@ -1287,6 +1210,83 @@
|
|||||||
| | | | :ref:`act<keyframe-key-act>` | :ref:`mpos<keyframe-key-mpos>` | :ref:`mquat<keyframe-key-mquat>` | :ref:`ctrl<keyframe-key-ctrl>` | |
|
| | | | :ref:`act<keyframe-key-act>` | :ref:`mpos<keyframe-key-mpos>` | :ref:`mquat<keyframe-key-mquat>` | :ref:`ctrl<keyframe-key-ctrl>` | |
|
||||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
|
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
|
||||||
|
| mujoco |br| |L| | | *no attributes* |
|
||||||
|
| :ref:`visual<visual>` | | |
|
||||||
|
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
|
||||||
|
| |_| visual |br| |_| |L| | | .. table:: |
|
||||||
|
| :ref:`global | ? | :class: mjcf-attributes |
|
||||||
|
| <visual-global>` | | |
|
||||||
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
|
| | | | :ref:`fovy<visual-global-fovy>` | :ref:`ipd<visual-global-ipd>` | :ref:`azimuth<visual-global-azimuth>` | :ref:`elevation<visual-global-elevation>` | |
|
||||||
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
|
| | | | :ref:`linewidth<visual-global-linewidth>` | :ref:`glow<visual-global-glow>` | :ref:`offwidth<visual-global-offwidth>` | :ref:`offheight<visual-global-offheight>` | |
|
||||||
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
|
| | | | :ref:`realtime<visual-global-realtime>` | :ref:`ellipsoidinertia<visual-global-ellipsoidinertia>` | :ref:`bvactive<visual-global-bvactive>` | | |
|
||||||
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
|
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
|
||||||
|
| |_| visual |br| |_| |L| | | .. table:: |
|
||||||
|
| :ref:`quality | ? | :class: mjcf-attributes |
|
||||||
|
| <visual-quality>` | | |
|
||||||
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
|
| | | | :ref:`shadowsize<visual-quality-shadowsize>` | :ref:`offsamples<visual-quality-offsamples>` | :ref:`numslices<visual-quality-numslices>` | :ref:`numstacks<visual-quality-numstacks>` | |
|
||||||
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
|
| | | | :ref:`numquads<visual-quality-numquads>` | | | | |
|
||||||
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
|
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
|
||||||
|
| |_| visual |br| |_| |L| | | .. table:: |
|
||||||
|
| :ref:`headlight | ? | :class: mjcf-attributes |
|
||||||
|
| <visual-headlight>` | | |
|
||||||
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
|
| | | | :ref:`ambient<visual-headlight-ambient>` | :ref:`diffuse<visual-headlight-diffuse>` | :ref:`specular<visual-headlight-specular>` | :ref:`active<visual-headlight-active>` | |
|
||||||
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
|
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
|
||||||
|
| |_| visual |br| |_| |L| | | .. table:: |
|
||||||
|
| :ref:`map | ? | :class: mjcf-attributes |
|
||||||
|
| <visual-map>` | | |
|
||||||
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
|
| | | | :ref:`stiffness<visual-map-stiffness>` | :ref:`stiffnessrot<visual-map-stiffnessrot>` | :ref:`force<visual-map-force>` | :ref:`torque<visual-map-torque>` | |
|
||||||
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
|
| | | | :ref:`alpha<visual-map-alpha>` | :ref:`fogstart<visual-map-fogstart>` | :ref:`fogend<visual-map-fogend>` | :ref:`znear<visual-map-znear>` | |
|
||||||
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
|
| | | | :ref:`zfar<visual-map-zfar>` | :ref:`haze<visual-map-haze>` | :ref:`shadowclip<visual-map-shadowclip>` | :ref:`shadowscale<visual-map-shadowscale>` | |
|
||||||
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
|
| | | | :ref:`actuatortendon<visual-map-actuatortendon>` | | | | |
|
||||||
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
|
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
|
||||||
|
| |_| visual |br| |_| |L| | | .. table:: |
|
||||||
|
| :ref:`scale | ? | :class: mjcf-attributes |
|
||||||
|
| <visual-scale>` | | |
|
||||||
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
|
| | | | :ref:`forcewidth<visual-scale-forcewidth>` | :ref:`contactwidth<visual-scale-contactwidth>` | :ref:`contactheight<visual-scale-contactheight>` | :ref:`connect<visual-scale-connect>` | |
|
||||||
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
|
| | | | :ref:`com<visual-scale-com>` | :ref:`camera<visual-scale-camera>` | :ref:`light<visual-scale-light>` | :ref:`selectpoint<visual-scale-selectpoint>` | |
|
||||||
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
|
| | | | :ref:`jointlength<visual-scale-jointlength>` | :ref:`jointwidth<visual-scale-jointwidth>` | :ref:`actuatorlength<visual-scale-actuatorlength>` | :ref:`actuatorwidth<visual-scale-actuatorwidth>` | |
|
||||||
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
|
| | | | :ref:`framelength<visual-scale-framelength>` | :ref:`framewidth<visual-scale-framewidth>` | :ref:`constraint<visual-scale-constraint>` | :ref:`slidercrank<visual-scale-slidercrank>` | |
|
||||||
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
|
| | | | :ref:`frustum<visual-scale-frustum>` | | | | |
|
||||||
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
|
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
|
||||||
|
| |_| visual |br| |_| |L| | | .. table:: |
|
||||||
|
| :ref:`rgba | ? | :class: mjcf-attributes |
|
||||||
|
| <visual-rgba>` | | |
|
||||||
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
|
| | | | :ref:`fog<visual-rgba-fog>` | :ref:`haze<visual-rgba-haze>` | :ref:`force<visual-rgba-force>` | :ref:`inertia<visual-rgba-inertia>` | |
|
||||||
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
|
| | | | :ref:`joint<visual-rgba-joint>` | :ref:`actuator<visual-rgba-actuator>` | :ref:`actuatornegative<visual-rgba-actuatornegative>` | :ref:`actuatorpositive<visual-rgba-actuatorpositive>` | |
|
||||||
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
|
| | | | :ref:`com<visual-rgba-com>` | :ref:`camera<visual-rgba-camera>` | :ref:`light<visual-rgba-light>` | :ref:`selectpoint<visual-rgba-selectpoint>` | |
|
||||||
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
|
| | | | :ref:`connect<visual-rgba-connect>` | :ref:`contactpoint<visual-rgba-contactpoint>` | :ref:`contactforce<visual-rgba-contactforce>` | :ref:`contactfriction<visual-rgba-contactfriction>` | |
|
||||||
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
|
| | | | :ref:`contacttorque<visual-rgba-contacttorque>` | :ref:`contactgap<visual-rgba-contactgap>` | :ref:`rangefinder<visual-rgba-rangefinder>` | :ref:`constraint<visual-rgba-constraint>` | |
|
||||||
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
|
| | | | :ref:`slidercrank<visual-rgba-slidercrank>` | :ref:`crankbroken<visual-rgba-crankbroken>` | :ref:`frustum<visual-rgba-frustum>` | :ref:`bv<visual-rgba-bv>` | |
|
||||||
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
|
| | | | :ref:`bvactive<visual-rgba-bvactive>` | | | | |
|
||||||
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
|
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
|
||||||
| mujoco |br| |L| | | .. table:: |
|
| mujoco |br| |L| | | .. table:: |
|
||||||
| :ref:`default | R | :class: mjcf-attributes |
|
| :ref:`default | R | :class: mjcf-attributes |
|
||||||
| <default>` | | |
|
| <default>` | | |
|
||||||
@@ -1307,7 +1307,9 @@
|
|||||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
| | | | :ref:`texture<default-material-texture>` | :ref:`emission<default-material-emission>` | :ref:`specular<default-material-specular>` | :ref:`shininess<default-material-shininess>` | |
|
| | | | :ref:`texture<default-material-texture>` | :ref:`emission<default-material-emission>` | :ref:`specular<default-material-specular>` | :ref:`shininess<default-material-shininess>` | |
|
||||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
| | | | :ref:`reflectance<default-material-reflectance>` | :ref:`rgba<default-material-rgba>` | :ref:`texrepeat<default-material-texrepeat>` | :ref:`texuniform<default-material-texuniform>` | |
|
| | | | :ref:`reflectance<default-material-reflectance>` | :ref:`metallic<default-material-metallic>` | :ref:`roughness<default-material-roughness>` | :ref:`rgba<default-material-rgba>` | |
|
||||||
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
|
| | | | :ref:`texrepeat<default-material-texrepeat>` | :ref:`texuniform<default-material-texuniform>` | | | |
|
||||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
|
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
|
||||||
| |_| default |br| |_| |L| | | .. table:: |
|
| |_| default |br| |_| |L| | | .. table:: |
|
||||||
@@ -1378,11 +1380,13 @@
|
|||||||
| :ref:`light | ? | :class: mjcf-attributes |
|
| :ref:`light | ? | :class: mjcf-attributes |
|
||||||
| <default-light>` | | |
|
| <default-light>` | | |
|
||||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
| | | | :ref:`pos<default-light-pos>` | :ref:`dir<default-light-dir>` | :ref:`directional<default-light-directional>` | :ref:`castshadow<default-light-castshadow>` | |
|
| | | | :ref:`pos<default-light-pos>` | :ref:`dir<default-light-dir>` | :ref:`bulbradius<default-light-bulbradius>` | :ref:`directional<default-light-directional>` | |
|
||||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
| | | | :ref:`active<default-light-active>` | :ref:`attenuation<default-light-attenuation>` | :ref:`cutoff<default-light-cutoff>` | :ref:`exponent<default-light-exponent>` | |
|
| | | | :ref:`castshadow<default-light-castshadow>` | :ref:`active<default-light-active>` | :ref:`attenuation<default-light-attenuation>` | :ref:`cutoff<default-light-cutoff>` | |
|
||||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
| | | | :ref:`ambient<default-light-ambient>` | :ref:`diffuse<default-light-diffuse>` | :ref:`specular<default-light-specular>` | :ref:`mode<default-light-mode>` | |
|
| | | | :ref:`exponent<default-light-exponent>` | :ref:`ambient<default-light-ambient>` | :ref:`diffuse<default-light-diffuse>` | :ref:`specular<default-light-specular>` | |
|
||||||
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
|
| | | | :ref:`mode<default-light-mode>` | | | | |
|
||||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
|
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
|
||||||
| |_| default |br| |_| |L| | | .. table:: |
|
| |_| default |br| |_| |L| | | .. table:: |
|
||||||
|
|||||||
+70
-8
@@ -5,6 +5,64 @@ Changelog
|
|||||||
Upcoming version (not yet released)
|
Upcoming version (not yet released)
|
||||||
-----------------------------------
|
-----------------------------------
|
||||||
|
|
||||||
|
General
|
||||||
|
^^^^^^^
|
||||||
|
|
||||||
|
.. youtube:: 5k0_wsIRAFc
|
||||||
|
:align: right
|
||||||
|
:width: 240px
|
||||||
|
|
||||||
|
1. Added the :ref:`replicate<replicate>` to MJCF, a :ref:`meta-element<meta-element>` which permits to repeat a subtree
|
||||||
|
with incremental translational and rotational offsets.
|
||||||
|
2. Added ``mjModel.mesh_scale``: the scaling applied to asset vertices, as specified in the
|
||||||
|
:ref:`scale<asset-mesh-scale>` attribute.
|
||||||
|
3. Added visual properties which are ignored by the native renderer, but can be used by external renderers:
|
||||||
|
|
||||||
|
- :ref:`light/bulbradius<body-light-bulbradius>` attribute and corresponding ``mjModel.light_bulbradius`` field.
|
||||||
|
- :ref:`material/metallic<asset-material-metallic>` attribute and corresponding ``mjModel.material_metallic`` field.
|
||||||
|
- :ref:`material/roughness<asset-material-roughness>` attribute and corresponding ``mjModel.material_roughness``
|
||||||
|
field.
|
||||||
|
4. The type of the ``size`` argument of :ref:`mj_stackAllocNum` and :ref:`mj_stackAllocInt` was changed from ``int``
|
||||||
|
to ``size_t``.
|
||||||
|
5. Added support for gmsh format version 2.2 surface meshes in :ref:`flexcomp<body-flexcomp-file>`.
|
||||||
|
|
||||||
|
MJX
|
||||||
|
^^^
|
||||||
|
.. admonition:: Breaking API changes
|
||||||
|
:class: attention
|
||||||
|
|
||||||
|
6. Removed deprecated ``mjx.device_get_into`` and ``mjx.device_put`` functions as they lack critical new
|
||||||
|
functionality.
|
||||||
|
|
||||||
|
**Migration:** Use ``mjx.get_data_into`` instead of ``mjx.device_get_into``, and ``mjx.put_data`` instead of
|
||||||
|
``mjx.device_put``.
|
||||||
|
|
||||||
|
7. Added cylinder plane collisions.
|
||||||
|
8. Added ``efc_type`` to ``mjx.Data`` and ``dim``, ``efc_address`` to ``mjx.Contact``.
|
||||||
|
9. Added ``geom`` to ``mjx.Contact`` and marked ``geom1``, ``geom2`` deprecated.
|
||||||
|
10. Added ``ne``, ``nf``, ``nl``, ``nefc``, and ``ncon`` to ``mjx.Data`` to match ``mujoco.MjData``.
|
||||||
|
11. Given the above added fields, removed ``mjx.get_params``, ``mjx.ncon``, and ``mjx.count_constraints``.
|
||||||
|
12. Changed the way meshes are organized on device to speed up collision detection when a mesh is replicated for many
|
||||||
|
geoms.
|
||||||
|
13. Fixed a bug where capsules might be ignored in broadphase colliision checking.
|
||||||
|
14. Added cylinder collisions using SDFs.
|
||||||
|
15. Added support for all :ref:`condim <coContact>`: 1, 3, 4, 6.
|
||||||
|
16. Add support functions for ``id2name`` and ``name2id``, MJX versions of :ref:`mj_id2name` and :ref:`mj_name2id`.
|
||||||
|
17. Added support for :ref:`gravcomp<body-gravcomp>` and :ref:`actuatorgravcomp<body-joint-actuatorgravcomp>`.
|
||||||
|
18. Fixed a bug in ``mjx.ray`` for sometimes allowed negative distances for ray-mesh tests.
|
||||||
|
|
||||||
|
Bug fixes
|
||||||
|
^^^^^^^^^
|
||||||
|
19. Defaults of lights were not being saved, now fixed.
|
||||||
|
20. Prevent overwriting of frame names by body names when saving an XML. Bug introduced in 3.1.4.
|
||||||
|
21. Fixed bug in Python binding of :ref:`mj_saveModel`: ``buffer`` argument was documented as optional but was actually
|
||||||
|
not optional.
|
||||||
|
22. Fixed bug that prevented memory allocations larger than 2.15 GB.
|
||||||
|
|
||||||
|
|
||||||
|
Version 3.1.4 (April 10th, 2024)
|
||||||
|
--------------------------------
|
||||||
|
|
||||||
General
|
General
|
||||||
^^^^^^^
|
^^^^^^^
|
||||||
.. admonition:: Breaking API changes
|
.. admonition:: Breaking API changes
|
||||||
@@ -14,7 +72,7 @@ General
|
|||||||
:ref:`corresponding attribute<CSensor>` are kept and now function as a convenient location for the user to save
|
: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:
|
standard-deviation information for their own use. This feature was removed because:
|
||||||
|
|
||||||
- There was no mechanism to seed the randon noise generator.
|
- There was no mechanism to seed the random noise generator.
|
||||||
- It was not thread-safe, even if seeding would have been provided, sampling on multiple threads would lead to
|
- It was not thread-safe, even if seeding would have been provided, sampling on multiple threads would lead to
|
||||||
non-reproducible results.
|
non-reproducible results.
|
||||||
- This feature was seen as overreach by the engine. Adding noise should be the user's responsibility.
|
- This feature was seen as overreach by the engine. Adding noise should be the user's responsibility.
|
||||||
@@ -27,19 +85,23 @@ General
|
|||||||
model
|
model
|
||||||
`refsite.xml <https://github.com/google-deepmind/mujoco/blob/main/test/engine/testdata/actuation/refsite.xml>`__,
|
`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.
|
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>`__.
|
3. Added support for gmsh format 2.2 , tetrahedral mesh, as generated by e.g. `fTetwild <https://github.com/wildmeshing/fTetWild>`__.
|
||||||
|
|
||||||
|
4. Added :ref:`mju_euler2Quat` for converting an Euler-angle sequence to quaternion.
|
||||||
|
|
||||||
MJX
|
MJX
|
||||||
^^^
|
^^^
|
||||||
4. Improved performance of SAT for convex collisions.
|
5. Improved performance of SAT for convex collisions.
|
||||||
5. Fixed bug for sphere/capsule-convex deep penetration.
|
6. 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. 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.
|
8. Throw an error for margin/gap for convex mesh collisions, since they are not supported.
|
||||||
|
9. Added ellipsoid plane collisions.
|
||||||
|
10. Added support for userdata.
|
||||||
|
11. Added ellipsoid-ellipsoid and ellipsoid-capsule collisions using signed distance functions (SDFs).
|
||||||
|
|
||||||
Simulate
|
Simulate
|
||||||
^^^^^^^^
|
^^^^^^^^
|
||||||
8. Fixed bug in order of enable flag strings. Before this change, using the simulate UI to toggle the
|
12. 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
|
:ref:`invdiscrete<option-flag-invdiscrete>` or the (now removed) ``sensornoise`` flags would actually toggle the
|
||||||
other flag.
|
other flag.
|
||||||
|
|
||||||
@@ -50,7 +112,7 @@ Python bindings
|
|||||||
:align: right
|
:align: right
|
||||||
:width: 240px
|
:width: 240px
|
||||||
|
|
||||||
9. Added the ``mujoco.minimize`` Python module for nonlinear least-squares, designed for System Identification (sysID).
|
13. 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
|
The sysID tutorial is work in progress, but a pedagogical colab notebook with examples, including Inverse
|
||||||
Kinematics, is available here: |ls_colab|
|
Kinematics, is available here: |ls_colab|
|
||||||
|br| The video on the right shows example clips from the tutorial.
|
|br| The video on the right shows example clips from the tutorial.
|
||||||
|
|||||||
@@ -22,8 +22,8 @@ been described elsewhere.
|
|||||||
|
|
||||||
.. _Motivation:
|
.. _Motivation:
|
||||||
|
|
||||||
Motivation for soft contact model
|
Soft contact model
|
||||||
---------------------------------
|
------------------
|
||||||
|
|
||||||
Robots as well as humans interact with their environment primarily through physical contact. Given the increasing
|
Robots as well as humans interact with their environment primarily through physical contact. Given the increasing
|
||||||
importance of physics modeling in robotics, machine learning, animation, virtual reality, biomechanics and other fields,
|
importance of physics modeling in robotics, machine learning, animation, virtual reality, biomechanics and other fields,
|
||||||
@@ -1557,8 +1557,10 @@ Forward dynamics
|
|||||||
~~~~~~~~~~~~~~~~
|
~~~~~~~~~~~~~~~~
|
||||||
|
|
||||||
The source file `engine_forward.c <https://github.com/google-deepmind/mujoco/blob/main/src/engine/engine_forward.c>`__
|
The source file `engine_forward.c <https://github.com/google-deepmind/mujoco/blob/main/src/engine/engine_forward.c>`__
|
||||||
contains the high-level forward dynamics pipeline:
|
contains the high-level forward dynamics pipeline.
|
||||||
|
|
||||||
|
Top level
|
||||||
|
^^^^^^^^^
|
||||||
- The top-level function :ref:`mj_step` invokes the entire sequence of computations below.
|
- The top-level function :ref:`mj_step` invokes the entire sequence of computations below.
|
||||||
- :ref:`mj_forward` invokes only stages **2-22**, computing the continuous-time forward dynamics, ending with the
|
- :ref:`mj_forward` invokes only stages **2-22**, computing the continuous-time forward dynamics, ending with the
|
||||||
acceleration ``mjData.qacc``.
|
acceleration ``mjData.qacc``.
|
||||||
@@ -1566,7 +1568,10 @@ contains the high-level forward dynamics pipeline:
|
|||||||
distinct phases. This allows the user to write controllers that depend on quantities derived from the positions and
|
distinct phases. This allows the user to write controllers that depend on quantities derived from the positions and
|
||||||
velocities (but not forces, since those have not yet been computed). Note that the :ref:`mj_step1` → :ref:`mj_step2`
|
velocities (but not forces, since those have not yet been computed). Note that the :ref:`mj_step1` → :ref:`mj_step2`
|
||||||
pipeline does not support the Runge Kutta integrator.
|
pipeline does not support the Runge Kutta integrator.
|
||||||
|
- :ref:`mj_fwdPosition` invokes stages **2-11**, the position-dependent part of the pipeline.
|
||||||
|
|
||||||
|
Stages
|
||||||
|
^^^^^^
|
||||||
1. Check the positions and velocities for invalid or unacceptably large real values indicating divergence. If divergence
|
1. Check the positions and velocities for invalid or unacceptably large real values indicating divergence. If divergence
|
||||||
is detected, the state is automatically reset and the corresponding warning is raised:
|
is detected, the state is automatically reset and the corresponding warning is raised:
|
||||||
:ref:`mj_checkPos`, :ref:`mj_checkVel`
|
:ref:`mj_checkPos`, :ref:`mj_checkVel`
|
||||||
@@ -1581,9 +1586,9 @@ contains the high-level forward dynamics pipeline:
|
|||||||
8. Construct the list of active contacts. This includes both broad-phase and near-phase collision detection:
|
8. Construct the list of active contacts. This includes both broad-phase and near-phase collision detection:
|
||||||
:ref:`mj_collision`
|
:ref:`mj_collision`
|
||||||
9. Construct the constraint Jacobian and compute the constraint residuals: :ref:`mj_makeConstraint`
|
9. Construct the constraint Jacobian and compute the constraint residuals: :ref:`mj_makeConstraint`
|
||||||
10. Compute the matrices and vectors needed by the constraint solvers: :ref:`mj_projectConstraint`
|
10. Compute the tendon lengths and moment arms. This includes the computation of minimal-length paths for spatial
|
||||||
11. Compute the tendon lengths and moment arms. This includes the computation of minimal-length paths for spatial
|
|
||||||
tendons: :ref:`mj_transmission`
|
tendons: :ref:`mj_transmission`
|
||||||
|
11. Compute the matrices and vectors needed by the constraint solvers: :ref:`mj_projectConstraint`
|
||||||
12. Compute sensor data that only depends on position, and the potential energy if enabled: :ref:`mj_sensorPos`,
|
12. Compute sensor data that only depends on position, and the potential energy if enabled: :ref:`mj_sensorPos`,
|
||||||
:ref:`mj_energyPos`
|
:ref:`mj_energyPos`
|
||||||
13. Compute the tendon, flex edge and actuator velocities: :ref:`mj_fwdVelocity`
|
13. Compute the tendon, flex edge and actuator velocities: :ref:`mj_fwdVelocity`
|
||||||
@@ -1658,10 +1663,10 @@ important caveats:
|
|||||||
systems with contact. Contact events have high `Lyapunov exponents
|
systems with contact. Contact events have high `Lyapunov exponents
|
||||||
<https://en.wikipedia.org/wiki/Lyapunov_exponent>`__; this is a property of any rigid-body simulator (and indeed of
|
<https://en.wikipedia.org/wiki/Lyapunov_exponent>`__; this is a property of any rigid-body simulator (and indeed of
|
||||||
`real-world physics <https://en.wikipedia.org/wiki/Roulette>`__) and is not MuJoCo-specific.
|
`real-world physics <https://en.wikipedia.org/wiki/Roulette>`__) and is not MuJoCo-specific.
|
||||||
- Exact reproducibillity is only guaranteed within a **single version**. Small numerical differences are quite common
|
- Exact reproducibility is only guaranteed within a **single version**, on the **same architecture**. Small numerical
|
||||||
between versioned releases, for example due to code optimizations. This means that when saving an initial state and an
|
differences are quite common between versioned releases, for example due to code optimizations. This means that when
|
||||||
open-loop control sequence, the resulting rolled-out trajectory will be identical within the same version but will
|
saving an initial state and an open-loop control sequence, the resulting rolled-out trajectory will be identical
|
||||||
likely be different between MuJoCo versions.
|
within the same version, but will likely be different between MuJoCo versions or different operating systems.
|
||||||
|
|
||||||
.. _derivatives:
|
.. _derivatives:
|
||||||
|
|
||||||
|
|||||||
Binary file not shown.
|
After Width: | Height: | Size: 59 KiB |
@@ -922,6 +922,7 @@ struct mjModel_ {
|
|||||||
int nD; // number of non-zeros in sparse dof-dof matrix
|
int nD; // number of non-zeros in sparse dof-dof matrix
|
||||||
int nB; // number of non-zeros in sparse body-dof matrix
|
int nB; // number of non-zeros in sparse body-dof matrix
|
||||||
int ntree; // number of kinematic trees under world body
|
int ntree; // number of kinematic trees under world body
|
||||||
|
int ngravcomp; // number of bodies with nonzero gravcomp
|
||||||
int nemax; // number of potential equality-constraint rows
|
int nemax; // number of potential equality-constraint rows
|
||||||
int njmax; // number of available rows in constraint Jacobian
|
int njmax; // number of available rows in constraint Jacobian
|
||||||
int nconmax; // number of potential contacts in contact list
|
int nconmax; // number of potential contacts in contact list
|
||||||
@@ -1079,6 +1080,7 @@ struct mjModel_ {
|
|||||||
int* light_targetbodyid; // id of targeted body; -1: none (nlight x 1)
|
int* light_targetbodyid; // id of targeted body; -1: none (nlight x 1)
|
||||||
mjtByte* light_directional; // directional light (nlight x 1)
|
mjtByte* light_directional; // directional light (nlight x 1)
|
||||||
mjtByte* light_castshadow; // does light cast shadows (nlight x 1)
|
mjtByte* light_castshadow; // does light cast shadows (nlight x 1)
|
||||||
|
float* light_bulbradius; // light radius for soft shadows (nlight x 1)
|
||||||
mjtByte* light_active; // is light on (nlight x 1)
|
mjtByte* light_active; // is light on (nlight x 1)
|
||||||
mjtNum* light_pos; // position rel. to body frame (nlight x 3)
|
mjtNum* light_pos; // position rel. to body frame (nlight x 3)
|
||||||
mjtNum* light_dir; // direction rel. to body frame (nlight x 3)
|
mjtNum* light_dir; // direction rel. to body frame (nlight x 3)
|
||||||
@@ -1165,6 +1167,7 @@ struct mjModel_ {
|
|||||||
int* mesh_facenormal; // normal face data (nmeshface x 3)
|
int* mesh_facenormal; // normal face data (nmeshface x 3)
|
||||||
int* mesh_facetexcoord; // texture face data (nmeshface x 3)
|
int* mesh_facetexcoord; // texture face data (nmeshface x 3)
|
||||||
int* mesh_graph; // convex graph data (nmeshgraph x 1)
|
int* mesh_graph; // convex graph data (nmeshgraph x 1)
|
||||||
|
mjtNum* mesh_scale; // scaling applied to asset vertices (nmesh x 3)
|
||||||
mjtNum* mesh_pos; // translation applied to asset vertices (nmesh x 3)
|
mjtNum* mesh_pos; // translation applied to asset vertices (nmesh x 3)
|
||||||
mjtNum* mesh_quat; // rotation applied to asset vertices (nmesh x 4)
|
mjtNum* mesh_quat; // rotation applied to asset vertices (nmesh x 4)
|
||||||
int* mesh_pathadr; // address of asset path for mesh; -1: none (nmesh x 1)
|
int* mesh_pathadr; // address of asset path for mesh; -1: none (nmesh x 1)
|
||||||
@@ -1199,7 +1202,7 @@ struct mjModel_ {
|
|||||||
int* hfield_ncol; // number of columns in grid (nhfield x 1)
|
int* hfield_ncol; // number of columns in grid (nhfield x 1)
|
||||||
int* hfield_adr; // address in hfield_data (nhfield x 1)
|
int* hfield_adr; // address in hfield_data (nhfield x 1)
|
||||||
float* hfield_data; // elevation data (nhfielddata x 1)
|
float* hfield_data; // elevation data (nhfielddata x 1)
|
||||||
int* hfield_pathadr; // address of asset path for hfield; -1: none (nhfield x 1)
|
int* hfield_pathadr; // address of hfield asset path; -1: none (nhfield x 1)
|
||||||
|
|
||||||
// textures
|
// textures
|
||||||
int* tex_type; // texture type (mjtTexture) (ntex x 1)
|
int* tex_type; // texture type (mjtTexture) (ntex x 1)
|
||||||
@@ -1207,7 +1210,7 @@ struct mjModel_ {
|
|||||||
int* tex_width; // number of columns in texture image (ntex x 1)
|
int* tex_width; // number of columns in texture image (ntex x 1)
|
||||||
int* tex_adr; // address in rgb (ntex x 1)
|
int* tex_adr; // address in rgb (ntex x 1)
|
||||||
mjtByte* tex_rgb; // rgb (alpha = 1) (ntexdata x 1)
|
mjtByte* tex_rgb; // rgb (alpha = 1) (ntexdata x 1)
|
||||||
int* tex_pathadr; // address of asset path for texture; -1: none (ntex x 1)
|
int* tex_pathadr; // address of texture asset path; -1: none (ntex x 1)
|
||||||
|
|
||||||
// materials
|
// materials
|
||||||
int* mat_texid; // texture id; -1: none (nmat x 1)
|
int* mat_texid; // texture id; -1: none (nmat x 1)
|
||||||
@@ -1217,6 +1220,8 @@ struct mjModel_ {
|
|||||||
float* mat_specular; // specular (x white) (nmat x 1)
|
float* mat_specular; // specular (x white) (nmat x 1)
|
||||||
float* mat_shininess; // shininess coef (nmat x 1)
|
float* mat_shininess; // shininess coef (nmat x 1)
|
||||||
float* mat_reflectance; // reflectance (0: disable) (nmat x 1)
|
float* mat_reflectance; // reflectance (0: disable) (nmat x 1)
|
||||||
|
float* mat_metallic; // metallic coef (nmat x 1)
|
||||||
|
float* mat_roughness; // roughness coef (nmat x 1)
|
||||||
float* mat_rgba; // rgba (nmat x 4)
|
float* mat_rgba; // rgba (nmat x 4)
|
||||||
|
|
||||||
// predefined geom pairs for collision detection; has precedence over exclude
|
// predefined geom pairs for collision detection; has precedence over exclude
|
||||||
@@ -2020,6 +2025,7 @@ struct mjvLight_ { // OpenGL light
|
|||||||
mjtByte headlight; // headlight
|
mjtByte headlight; // headlight
|
||||||
mjtByte directional; // directional light
|
mjtByte directional; // directional light
|
||||||
mjtByte castshadow; // does light cast shadows
|
mjtByte castshadow; // does light cast shadows
|
||||||
|
float bulbradius; // bulb radius for soft shadows
|
||||||
};
|
};
|
||||||
typedef struct mjvLight_ mjvLight;
|
typedef struct mjvLight_ mjvLight;
|
||||||
struct mjvOption_ { // abstract visualization options
|
struct mjvOption_ { // abstract visualization options
|
||||||
@@ -2234,6 +2240,7 @@ struct mjvSceneState_ {
|
|||||||
|
|
||||||
mjtByte* light_directional;
|
mjtByte* light_directional;
|
||||||
mjtByte* light_castshadow;
|
mjtByte* light_castshadow;
|
||||||
|
float* light_bulbradius;
|
||||||
mjtByte* light_active;
|
mjtByte* light_active;
|
||||||
float* light_attenuation;
|
float* light_attenuation;
|
||||||
float* light_cutoff;
|
float* light_cutoff;
|
||||||
@@ -2301,6 +2308,8 @@ struct mjvSceneState_ {
|
|||||||
float* mat_specular;
|
float* mat_specular;
|
||||||
float* mat_shininess;
|
float* mat_shininess;
|
||||||
float* mat_reflectance;
|
float* mat_reflectance;
|
||||||
|
float* mat_metallic;
|
||||||
|
float* mat_roughness;
|
||||||
float* mat_rgba;
|
float* mat_rgba;
|
||||||
|
|
||||||
int* eq_type;
|
int* eq_type;
|
||||||
@@ -2445,8 +2454,8 @@ void mj_resetDataKeyframe(const mjModel* m, mjData* d, int key);
|
|||||||
void mj_markStack(mjData* d);
|
void mj_markStack(mjData* d);
|
||||||
void mj_freeStack(mjData* d);
|
void mj_freeStack(mjData* d);
|
||||||
void* mj_stackAllocByte(mjData* d, size_t bytes, size_t alignment);
|
void* mj_stackAllocByte(mjData* d, size_t bytes, size_t alignment);
|
||||||
mjtNum* mj_stackAllocNum(mjData* d, int size);
|
mjtNum* mj_stackAllocNum(mjData* d, size_t size);
|
||||||
int* mj_stackAllocInt(mjData* d, int size);
|
int* mj_stackAllocInt(mjData* d, size_t size);
|
||||||
void mj_deleteData(mjData* d);
|
void mj_deleteData(mjData* d);
|
||||||
void mj_resetCallbacks(void);
|
void mj_resetCallbacks(void);
|
||||||
void mj_setConst(mjModel* m, mjData* d);
|
void mj_setConst(mjModel* m, mjData* d);
|
||||||
@@ -2742,6 +2751,7 @@ void mju_mat2Quat(mjtNum quat[4], const mjtNum mat[9]);
|
|||||||
void mju_derivQuat(mjtNum res[4], const mjtNum quat[4], const mjtNum vel[3]);
|
void mju_derivQuat(mjtNum res[4], const mjtNum quat[4], const mjtNum vel[3]);
|
||||||
void mju_quatIntegrate(mjtNum quat[4], const mjtNum vel[3], mjtNum scale);
|
void mju_quatIntegrate(mjtNum quat[4], const mjtNum vel[3], mjtNum scale);
|
||||||
void mju_quatZ2Vec(mjtNum quat[4], const mjtNum vec[3]);
|
void mju_quatZ2Vec(mjtNum quat[4], const mjtNum vec[3]);
|
||||||
|
void mju_euler2Quat(mjtNum quat[4], const mjtNum euler[3], const char* seq);
|
||||||
void mju_mulPose(mjtNum posres[3], mjtNum quatres[4],
|
void mju_mulPose(mjtNum posres[3], mjtNum quatres[4],
|
||||||
const mjtNum pos1[3], const mjtNum quat1[4],
|
const mjtNum pos1[3], const mjtNum quat1[4],
|
||||||
const mjtNum pos2[3], const mjtNum quat2[4]);
|
const mjtNum pos2[3], const mjtNum quat2[4]);
|
||||||
|
|||||||
+17
-12
@@ -66,25 +66,32 @@ directly from the top-level ``mjx`` module.
|
|||||||
Structs
|
Structs
|
||||||
-------
|
-------
|
||||||
|
|
||||||
Before running MJX functions on an accelerator device, structs must be copied onto the device via the ``mjx.device_put``
|
Before running MJX functions on an accelerator device, structs must be copied onto the device via the ``mjx.put_model`` and ``mjx.put_data``
|
||||||
function. Placing an :ref:`mjModel` on device yields an ``mjx.Model``. Placing an :ref:`mjData` on device yields
|
functions. Placing an :ref:`mjModel` on device yields an ``mjx.Model``. Placing an :ref:`mjData` on device yields
|
||||||
an ``mjx.Data``:
|
an ``mjx.Data``:
|
||||||
|
|
||||||
.. code-block:: python
|
.. code-block:: python
|
||||||
|
|
||||||
model = mujoco.MjModel.from_xml_string("...")
|
model = mujoco.MjModel.from_xml_string("...")
|
||||||
data = mujoco.MjData(model)
|
data = mujoco.MjData(model)
|
||||||
mjx_model = mjx.device_put(model)
|
mjx_model = mjx.put_model(model)
|
||||||
mjx_data = mjx.device_put(data)
|
mjx_data = mjx.put_data(data)
|
||||||
|
|
||||||
These MJX variants mirror their MuJoCo counterparts but have three key differences:
|
These MJX variants mirror their MuJoCo counterparts but have a few key differences:
|
||||||
|
|
||||||
#. Fields in ``mjx.Model`` and ``mjx.Data`` are JAX arrays copied onto device, instead of numpy arrays.
|
#. ``mjx.Model`` and ``mjx.Data`` contain JAX arrays that are copied onto device.
|
||||||
#. Some fields are missing from ``mjx.Model`` and ``mjx.Data`` for features that are
|
#. Some fields are missing from ``mjx.Model`` and ``mjx.Data`` for features that are
|
||||||
:ref:`unsupported <mjxFeatureParity>` in MJX.
|
:ref:`unsupported <mjxFeatureParity>` in MJX.
|
||||||
#. Arrays in ``mjx.Model`` and ``mjx.Data`` support adding batch dimensions. Batch dimensions are a natural way to
|
#. JAX arrays in ``mjx.Model`` and ``mjx.Data`` support adding batch dimensions. Batch dimensions are a natural way to
|
||||||
express domain randomization (in the case of ``mjx.Model``) or high-throughput simulation for reinforcement learning
|
express domain randomization (in the case of ``mjx.Model``) or high-throughput simulation for reinforcement learning
|
||||||
(in the case of ``mjx.Data``).
|
(in the case of ``mjx.Data``).
|
||||||
|
#. Numpy arrays in ``mjx.Model`` and ``mjx.Data`` are structural fields that control the output of JIT compilation.
|
||||||
|
Modifying these arrays will force JAX to recompile MJX functions. As an example,
|
||||||
|
``jnt_limited`` is a numpy array passed by reference from :ref:`mjModel`, which determines if joint limit
|
||||||
|
constraints should be applied. If ``jnt_limited`` is modified, JAX will
|
||||||
|
re-compile MJX functions.
|
||||||
|
On the other hand, ``jnt_range`` is a JAX array that can be modified at runtime, and will only apply to joints with limits
|
||||||
|
as specified by the ``jnt_limited`` field.
|
||||||
|
|
||||||
|
|
||||||
Neither ``mjx.Model`` nor ``mjx.Data`` are meant to be constructed manually. An ``mjx.Data`` may be created by calling
|
Neither ``mjx.Model`` nor ``mjx.Data`` are meant to be constructed manually. An ``mjx.Data`` may be created by calling
|
||||||
@@ -191,7 +198,7 @@ The following features are **fully supported** in MJX:
|
|||||||
* - :ref:`Geom <mjtGeom>`
|
* - :ref:`Geom <mjtGeom>`
|
||||||
- ``PLANE``, ``SPHERE``, ``CAPSULE``, ``BOX``, ``MESH``
|
- ``PLANE``, ``SPHERE``, ``CAPSULE``, ``BOX``, ``MESH``
|
||||||
* - :ref:`Constraint <mjtConstraint>`
|
* - :ref:`Constraint <mjtConstraint>`
|
||||||
- ``EQUALITY``, ``LIMIT_JOINT``, ``CONTACT_PYRAMIDAL``
|
- ``EQUALITY``, ``LIMIT_JOINT``, ``CONTACT_FRICTIONLESS``, ``CONTACT_PYRAMIDAL``
|
||||||
* - :ref:`Equality <mjtEq>`
|
* - :ref:`Equality <mjtEq>`
|
||||||
- ``CONNECT``, ``WELD``, ``JOINT``
|
- ``CONNECT``, ``WELD``, ``JOINT``
|
||||||
* - :ref:`Integrator <mjtIntegrator>`
|
* - :ref:`Integrator <mjtIntegrator>`
|
||||||
@@ -199,7 +206,7 @@ The following features are **fully supported** in MJX:
|
|||||||
* - :ref:`Cone <mjtCone>`
|
* - :ref:`Cone <mjtCone>`
|
||||||
- ``PYRAMIDAL``
|
- ``PYRAMIDAL``
|
||||||
* - :ref:`Condim <coContact>`
|
* - :ref:`Condim <coContact>`
|
||||||
- 3
|
- 1, 3, 4, 6
|
||||||
* - :ref:`Solver <mjtSolver>`
|
* - :ref:`Solver <mjtSolver>`
|
||||||
- ``CG``, ``NEWTON``
|
- ``CG``, ``NEWTON``
|
||||||
* - Fluid Model
|
* - Fluid Model
|
||||||
@@ -217,10 +224,8 @@ The following features are **in development** and coming soon:
|
|||||||
- Feature
|
- Feature
|
||||||
* - :ref:`Geom <mjtGeom>`
|
* - :ref:`Geom <mjtGeom>`
|
||||||
- ``SDF``, ``HFIELD``, ``ELLIPSOID``, ``CYLINDER``
|
- ``SDF``, ``HFIELD``, ``ELLIPSOID``, ``CYLINDER``
|
||||||
* - :ref:`Condim <coContact>`
|
|
||||||
- 1, 4, 6
|
|
||||||
* - :ref:`Constraint <mjtConstraint>`
|
* - :ref:`Constraint <mjtConstraint>`
|
||||||
- :ref:`Frictionloss <coFriction>`, ``CONTACT_FRICTIONLESS``, ``CONTACT_ELLIPTIC``, ``FRICTION_DOF``
|
- :ref:`Frictionloss <coFriction>`, ``CONTACT_ELLIPTIC``, ``FRICTION_DOF``
|
||||||
* - :ref:`Integrator <mjtIntegrator>`
|
* - :ref:`Integrator <mjtIntegrator>`
|
||||||
- ``IMPLICIT``, ``IMPLICITFAST``
|
- ``IMPLICIT``, ``IMPLICITFAST``
|
||||||
* - Dynamics
|
* - Dynamics
|
||||||
|
|||||||
@@ -129,3 +129,17 @@ Drones
|
|||||||
- Preview
|
- Preview
|
||||||
* - `Skydio X2 <https://github.com/google-deepmind/mujoco_menagerie/tree/main/skydio_x2>`_
|
* - `Skydio X2 <https://github.com/google-deepmind/mujoco_menagerie/tree/main/skydio_x2>`_
|
||||||
- .. image:: https://raw.githubusercontent.com/google-deepmind/mujoco_menagerie/main/skydio_x2/x2.png
|
- .. image:: https://raw.githubusercontent.com/google-deepmind/mujoco_menagerie/main/skydio_x2/x2.png
|
||||||
|
* - `Bitcraze Crazyflie 2 <https://github.com/google-deepmind/mujoco_menagerie/tree/main/bitcraze_crazyflie_2>`_
|
||||||
|
- .. image:: https://raw.githubusercontent.com/google-deepmind/mujoco_menagerie/main/bitcraze_crazyflie_2/cf2.png
|
||||||
|
|
||||||
|
|
||||||
|
Biomechanical
|
||||||
|
^^^^^^^^^^^^^
|
||||||
|
|
||||||
|
.. list-table::
|
||||||
|
:header-rows: 1
|
||||||
|
|
||||||
|
* - Model
|
||||||
|
- Preview
|
||||||
|
* - `Fruitfly <https://github.com/google-deepmind/mujoco_menagerie/tree/main/flybody>`_
|
||||||
|
- .. image:: https://raw.githubusercontent.com/google-deepmind/mujoco_menagerie/main/flybody/flybody.png
|
||||||
|
|||||||
+2
-2
@@ -30,14 +30,14 @@ _____
|
|||||||
|
|
||||||
The MuJoCo app needs to be run at least once before the native library can be used, in order to register the library as
|
The MuJoCo app needs to be run at least once before the native library can be used, in order to register the library as
|
||||||
a trusted binary. Then, copy the dynamic library file from
|
a trusted binary. Then, copy the dynamic library file from
|
||||||
``/Applications/MuJoCo.app/Contents/Frameworks/mujoco.framework/Versions/Current/libmujoco.3.1.4.dylib`` (it can be
|
``/Applications/MuJoCo.app/Contents/Frameworks/mujoco.framework/Versions/Current/libmujoco.3.1.5.dylib`` (it can be
|
||||||
found by browsing the contents of ``MuJoCo.app``) and rename it as ``mujoco.dylib``.
|
found by browsing the contents of ``MuJoCo.app``) and rename it as ``mujoco.dylib``.
|
||||||
|
|
||||||
Linux
|
Linux
|
||||||
_____
|
_____
|
||||||
|
|
||||||
Expand the ``tar.gz`` archive to ``~/.mujoco``. Then copy the dynamic library from
|
Expand the ``tar.gz`` archive to ``~/.mujoco``. Then copy the dynamic library from
|
||||||
``~/.mujoco/mujoco-3.1.4/lib/libmujoco.so.3.1.4`` and rename it as ``libmujoco.so``.
|
``~/.mujoco/mujoco-3.1.5/lib/libmujoco.so.3.1.5`` and rename it as ``libmujoco.so``.
|
||||||
|
|
||||||
Windows
|
Windows
|
||||||
_______
|
_______
|
||||||
|
|||||||
@@ -637,6 +637,7 @@ struct mjModel_ {
|
|||||||
int nD; // number of non-zeros in sparse dof-dof matrix
|
int nD; // number of non-zeros in sparse dof-dof matrix
|
||||||
int nB; // number of non-zeros in sparse body-dof matrix
|
int nB; // number of non-zeros in sparse body-dof matrix
|
||||||
int ntree; // number of kinematic trees under world body
|
int ntree; // number of kinematic trees under world body
|
||||||
|
int ngravcomp; // number of bodies with nonzero gravcomp
|
||||||
int nemax; // number of potential equality-constraint rows
|
int nemax; // number of potential equality-constraint rows
|
||||||
int njmax; // number of available rows in constraint Jacobian
|
int njmax; // number of available rows in constraint Jacobian
|
||||||
int nconmax; // number of potential contacts in contact list
|
int nconmax; // number of potential contacts in contact list
|
||||||
@@ -794,6 +795,7 @@ struct mjModel_ {
|
|||||||
int* light_targetbodyid; // id of targeted body; -1: none (nlight x 1)
|
int* light_targetbodyid; // id of targeted body; -1: none (nlight x 1)
|
||||||
mjtByte* light_directional; // directional light (nlight x 1)
|
mjtByte* light_directional; // directional light (nlight x 1)
|
||||||
mjtByte* light_castshadow; // does light cast shadows (nlight x 1)
|
mjtByte* light_castshadow; // does light cast shadows (nlight x 1)
|
||||||
|
float* light_bulbradius; // light radius for soft shadows (nlight x 1)
|
||||||
mjtByte* light_active; // is light on (nlight x 1)
|
mjtByte* light_active; // is light on (nlight x 1)
|
||||||
mjtNum* light_pos; // position rel. to body frame (nlight x 3)
|
mjtNum* light_pos; // position rel. to body frame (nlight x 3)
|
||||||
mjtNum* light_dir; // direction rel. to body frame (nlight x 3)
|
mjtNum* light_dir; // direction rel. to body frame (nlight x 3)
|
||||||
@@ -880,6 +882,7 @@ struct mjModel_ {
|
|||||||
int* mesh_facenormal; // normal face data (nmeshface x 3)
|
int* mesh_facenormal; // normal face data (nmeshface x 3)
|
||||||
int* mesh_facetexcoord; // texture face data (nmeshface x 3)
|
int* mesh_facetexcoord; // texture face data (nmeshface x 3)
|
||||||
int* mesh_graph; // convex graph data (nmeshgraph x 1)
|
int* mesh_graph; // convex graph data (nmeshgraph x 1)
|
||||||
|
mjtNum* mesh_scale; // scaling applied to asset vertices (nmesh x 3)
|
||||||
mjtNum* mesh_pos; // translation applied to asset vertices (nmesh x 3)
|
mjtNum* mesh_pos; // translation applied to asset vertices (nmesh x 3)
|
||||||
mjtNum* mesh_quat; // rotation applied to asset vertices (nmesh x 4)
|
mjtNum* mesh_quat; // rotation applied to asset vertices (nmesh x 4)
|
||||||
int* mesh_pathadr; // address of asset path for mesh; -1: none (nmesh x 1)
|
int* mesh_pathadr; // address of asset path for mesh; -1: none (nmesh x 1)
|
||||||
@@ -914,7 +917,7 @@ struct mjModel_ {
|
|||||||
int* hfield_ncol; // number of columns in grid (nhfield x 1)
|
int* hfield_ncol; // number of columns in grid (nhfield x 1)
|
||||||
int* hfield_adr; // address in hfield_data (nhfield x 1)
|
int* hfield_adr; // address in hfield_data (nhfield x 1)
|
||||||
float* hfield_data; // elevation data (nhfielddata x 1)
|
float* hfield_data; // elevation data (nhfielddata x 1)
|
||||||
int* hfield_pathadr; // address of asset path for hfield; -1: none (nhfield x 1)
|
int* hfield_pathadr; // address of hfield asset path; -1: none (nhfield x 1)
|
||||||
|
|
||||||
// textures
|
// textures
|
||||||
int* tex_type; // texture type (mjtTexture) (ntex x 1)
|
int* tex_type; // texture type (mjtTexture) (ntex x 1)
|
||||||
@@ -922,7 +925,7 @@ struct mjModel_ {
|
|||||||
int* tex_width; // number of columns in texture image (ntex x 1)
|
int* tex_width; // number of columns in texture image (ntex x 1)
|
||||||
int* tex_adr; // address in rgb (ntex x 1)
|
int* tex_adr; // address in rgb (ntex x 1)
|
||||||
mjtByte* tex_rgb; // rgb (alpha = 1) (ntexdata x 1)
|
mjtByte* tex_rgb; // rgb (alpha = 1) (ntexdata x 1)
|
||||||
int* tex_pathadr; // address of asset path for texture; -1: none (ntex x 1)
|
int* tex_pathadr; // address of texture asset path; -1: none (ntex x 1)
|
||||||
|
|
||||||
// materials
|
// materials
|
||||||
int* mat_texid; // texture id; -1: none (nmat x 1)
|
int* mat_texid; // texture id; -1: none (nmat x 1)
|
||||||
@@ -932,6 +935,8 @@ struct mjModel_ {
|
|||||||
float* mat_specular; // specular (x white) (nmat x 1)
|
float* mat_specular; // specular (x white) (nmat x 1)
|
||||||
float* mat_shininess; // shininess coef (nmat x 1)
|
float* mat_shininess; // shininess coef (nmat x 1)
|
||||||
float* mat_reflectance; // reflectance (0: disable) (nmat x 1)
|
float* mat_reflectance; // reflectance (0: disable) (nmat x 1)
|
||||||
|
float* mat_metallic; // metallic coef (nmat x 1)
|
||||||
|
float* mat_roughness; // roughness coef (nmat x 1)
|
||||||
float* mat_rgba; // rgba (nmat x 4)
|
float* mat_rgba; // rgba (nmat x 4)
|
||||||
|
|
||||||
// predefined geom pairs for collision detection; has precedence over exclude
|
// predefined geom pairs for collision detection; has precedence over exclude
|
||||||
|
|||||||
@@ -263,6 +263,7 @@ struct mjvLight_ { // OpenGL light
|
|||||||
mjtByte headlight; // headlight
|
mjtByte headlight; // headlight
|
||||||
mjtByte directional; // directional light
|
mjtByte directional; // directional light
|
||||||
mjtByte castshadow; // does light cast shadows
|
mjtByte castshadow; // does light cast shadows
|
||||||
|
float bulbradius; // bulb radius for soft shadows
|
||||||
};
|
};
|
||||||
typedef struct mjvLight_ mjvLight;
|
typedef struct mjvLight_ mjvLight;
|
||||||
|
|
||||||
@@ -493,6 +494,7 @@ struct mjvSceneState_ {
|
|||||||
|
|
||||||
mjtByte* light_directional;
|
mjtByte* light_directional;
|
||||||
mjtByte* light_castshadow;
|
mjtByte* light_castshadow;
|
||||||
|
float* light_bulbradius;
|
||||||
mjtByte* light_active;
|
mjtByte* light_active;
|
||||||
float* light_attenuation;
|
float* light_attenuation;
|
||||||
float* light_cutoff;
|
float* light_cutoff;
|
||||||
@@ -560,6 +562,8 @@ struct mjvSceneState_ {
|
|||||||
float* mat_specular;
|
float* mat_specular;
|
||||||
float* mat_shininess;
|
float* mat_shininess;
|
||||||
float* mat_reflectance;
|
float* mat_reflectance;
|
||||||
|
float* mat_metallic;
|
||||||
|
float* mat_roughness;
|
||||||
float* mat_rgba;
|
float* mat_rgba;
|
||||||
|
|
||||||
int* eq_type;
|
int* eq_type;
|
||||||
|
|||||||
@@ -139,6 +139,7 @@
|
|||||||
X ( njmax ) \
|
X ( njmax ) \
|
||||||
X ( nconmax ) \
|
X ( nconmax ) \
|
||||||
XMJV( ntree ) \
|
XMJV( ntree ) \
|
||||||
|
X ( ngravcomp ) \
|
||||||
X ( nuserdata ) \
|
X ( nuserdata ) \
|
||||||
XMJV( nsensordata ) \
|
XMJV( nsensordata ) \
|
||||||
X ( npluginstate ) \
|
X ( npluginstate ) \
|
||||||
@@ -292,6 +293,7 @@
|
|||||||
X ( int, light_targetbodyid, nlight, 1 ) \
|
X ( int, light_targetbodyid, nlight, 1 ) \
|
||||||
XMJV( mjtByte, light_directional, nlight, 1 ) \
|
XMJV( mjtByte, light_directional, nlight, 1 ) \
|
||||||
XMJV( mjtByte, light_castshadow, nlight, 1 ) \
|
XMJV( mjtByte, light_castshadow, nlight, 1 ) \
|
||||||
|
XMJV( float, light_bulbradius, nlight, 1 ) \
|
||||||
XMJV( mjtByte, light_active, nlight, 1 ) \
|
XMJV( mjtByte, light_active, nlight, 1 ) \
|
||||||
X ( mjtNum, light_pos, nlight, 3 ) \
|
X ( mjtNum, light_pos, nlight, 3 ) \
|
||||||
X ( mjtNum, light_dir, nlight, 3 ) \
|
X ( mjtNum, light_dir, nlight, 3 ) \
|
||||||
@@ -365,6 +367,7 @@
|
|||||||
XMJV( int, mesh_bvhadr, nmesh, 1 ) \
|
XMJV( int, mesh_bvhadr, nmesh, 1 ) \
|
||||||
XMJV( int, mesh_bvhnum, nmesh, 1 ) \
|
XMJV( int, mesh_bvhnum, nmesh, 1 ) \
|
||||||
XMJV( int, mesh_graphadr, nmesh, 1 ) \
|
XMJV( int, mesh_graphadr, nmesh, 1 ) \
|
||||||
|
X ( mjtNum, mesh_scale, nmesh, 3 ) \
|
||||||
X ( mjtNum, mesh_pos, nmesh, 3 ) \
|
X ( mjtNum, mesh_pos, nmesh, 3 ) \
|
||||||
X ( mjtNum, mesh_quat, nmesh, 4 ) \
|
X ( mjtNum, mesh_quat, nmesh, 4 ) \
|
||||||
X ( float, mesh_vert, nmeshvert, 3 ) \
|
X ( float, mesh_vert, nmeshvert, 3 ) \
|
||||||
@@ -416,6 +419,8 @@
|
|||||||
XMJV( float, mat_specular, nmat, 1 ) \
|
XMJV( float, mat_specular, nmat, 1 ) \
|
||||||
XMJV( float, mat_shininess, nmat, 1 ) \
|
XMJV( float, mat_shininess, nmat, 1 ) \
|
||||||
XMJV( float, mat_reflectance, nmat, 1 ) \
|
XMJV( float, mat_reflectance, nmat, 1 ) \
|
||||||
|
XMJV( float, mat_metallic, nmat, 1 ) \
|
||||||
|
XMJV( float, mat_roughness, nmat, 1 ) \
|
||||||
XMJV( float, mat_rgba, nmat, 4 ) \
|
XMJV( float, mat_rgba, nmat, 4 ) \
|
||||||
X ( int, pair_dim, npair, 1 ) \
|
X ( int, pair_dim, npair, 1 ) \
|
||||||
X ( int, pair_geom1, npair, 1 ) \
|
X ( int, pair_geom1, npair, 1 ) \
|
||||||
|
|||||||
@@ -24,7 +24,7 @@ extern "C" {
|
|||||||
#endif
|
#endif
|
||||||
|
|
||||||
// header version; should match the library version as returned by mj_version()
|
// header version; should match the library version as returned by mj_version()
|
||||||
#define mjVERSION_HEADER 314
|
#define mjVERSION_HEADER 315
|
||||||
|
|
||||||
// needed to define size_t, fabs and log10
|
// needed to define size_t, fabs and log10
|
||||||
#include <stdlib.h>
|
#include <stdlib.h>
|
||||||
@@ -203,10 +203,10 @@ MJAPI void mj_freeStack(mjData* d);
|
|||||||
MJAPI void* mj_stackAllocByte(mjData* d, size_t bytes, size_t alignment);
|
MJAPI void* mj_stackAllocByte(mjData* d, size_t bytes, size_t alignment);
|
||||||
|
|
||||||
// Allocate array of mjtNums on mjData stack. Call mju_error on stack overflow.
|
// Allocate array of mjtNums on mjData stack. Call mju_error on stack overflow.
|
||||||
MJAPI mjtNum* mj_stackAllocNum(mjData* d, int size);
|
MJAPI mjtNum* mj_stackAllocNum(mjData* d, size_t size);
|
||||||
|
|
||||||
// Allocate array of ints on mjData stack. Call mju_error on stack overflow.
|
// Allocate array of ints on mjData stack. Call mju_error on stack overflow.
|
||||||
MJAPI int* mj_stackAllocInt(mjData* d, int size);
|
MJAPI int* mj_stackAllocInt(mjData* d, size_t size);
|
||||||
|
|
||||||
// Free memory allocation in mjData.
|
// Free memory allocation in mjData.
|
||||||
MJAPI void mj_deleteData(mjData* d);
|
MJAPI void mj_deleteData(mjData* d);
|
||||||
@@ -1033,7 +1033,6 @@ MJAPI void mju_transformSpatial(mjtNum res[6], const mjtNum vec[6], int flg_forc
|
|||||||
const mjtNum newpos[3], const mjtNum oldpos[3],
|
const mjtNum newpos[3], const mjtNum oldpos[3],
|
||||||
const mjtNum rotnew2old[9]);
|
const mjtNum rotnew2old[9]);
|
||||||
|
|
||||||
|
|
||||||
//---------------------------------- Quaternions ---------------------------------------------------
|
//---------------------------------- Quaternions ---------------------------------------------------
|
||||||
|
|
||||||
// Rotate vector by quaternion.
|
// Rotate vector by quaternion.
|
||||||
@@ -1072,6 +1071,9 @@ MJAPI void mju_quatIntegrate(mjtNum quat[4], const mjtNum vel[3], mjtNum scale);
|
|||||||
// Construct quaternion performing rotation from z-axis to given vector.
|
// Construct quaternion performing rotation from z-axis to given vector.
|
||||||
MJAPI void mju_quatZ2Vec(mjtNum quat[4], const mjtNum vec[3]);
|
MJAPI void mju_quatZ2Vec(mjtNum quat[4], const mjtNum vec[3]);
|
||||||
|
|
||||||
|
// Convert sequence of Euler angles (radians) to quaternion.
|
||||||
|
// seq[0,1,2] must be in 'xyzXYZ', lower/upper-case mean intrinsic/extrinsic rotations.
|
||||||
|
MJAPI void mju_euler2Quat(mjtNum quat[4], const mjtNum euler[3], const char* seq);
|
||||||
|
|
||||||
//---------------------------------- Poses ---------------------------------------------------------
|
//---------------------------------- Poses ---------------------------------------------------------
|
||||||
|
|
||||||
|
|||||||
+30
-2
@@ -774,7 +774,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([
|
|||||||
),
|
),
|
||||||
FunctionParameterDecl(
|
FunctionParameterDecl(
|
||||||
name='size',
|
name='size',
|
||||||
type=ValueType(name='int'),
|
type=ValueType(name='size_t'),
|
||||||
),
|
),
|
||||||
),
|
),
|
||||||
doc='Allocate array of mjtNums on mjData stack. Call mju_error on stack overflow.', # pylint: disable=line-too-long
|
doc='Allocate array of mjtNums on mjData stack. Call mju_error on stack overflow.', # pylint: disable=line-too-long
|
||||||
@@ -794,7 +794,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([
|
|||||||
),
|
),
|
||||||
FunctionParameterDecl(
|
FunctionParameterDecl(
|
||||||
name='size',
|
name='size',
|
||||||
type=ValueType(name='int'),
|
type=ValueType(name='size_t'),
|
||||||
),
|
),
|
||||||
),
|
),
|
||||||
doc='Allocate array of ints on mjData stack. Call mju_error on stack overflow.', # pylint: disable=line-too-long
|
doc='Allocate array of ints on mjData stack. Call mju_error on stack overflow.', # pylint: disable=line-too-long
|
||||||
@@ -6982,6 +6982,34 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([
|
|||||||
),
|
),
|
||||||
doc='Construct quaternion performing rotation from z-axis to given vector.', # pylint: disable=line-too-long
|
doc='Construct quaternion performing rotation from z-axis to given vector.', # pylint: disable=line-too-long
|
||||||
)),
|
)),
|
||||||
|
('mju_euler2Quat',
|
||||||
|
FunctionDecl(
|
||||||
|
name='mju_euler2Quat',
|
||||||
|
return_type=ValueType(name='void'),
|
||||||
|
parameters=(
|
||||||
|
FunctionParameterDecl(
|
||||||
|
name='quat',
|
||||||
|
type=ArrayType(
|
||||||
|
inner_type=ValueType(name='mjtNum'),
|
||||||
|
extents=(4,),
|
||||||
|
),
|
||||||
|
),
|
||||||
|
FunctionParameterDecl(
|
||||||
|
name='euler',
|
||||||
|
type=ArrayType(
|
||||||
|
inner_type=ValueType(name='mjtNum', is_const=True),
|
||||||
|
extents=(3,),
|
||||||
|
),
|
||||||
|
),
|
||||||
|
FunctionParameterDecl(
|
||||||
|
name='seq',
|
||||||
|
type=PointerType(
|
||||||
|
inner_type=ValueType(name='char', is_const=True),
|
||||||
|
),
|
||||||
|
),
|
||||||
|
),
|
||||||
|
doc="Convert sequence of Euler angles (radians) to quaternion. seq[0,1,2] must be in 'xyzXYZ', lower/upper-case mean intrinsic/extrinsic rotations.", # pylint: disable=line-too-long
|
||||||
|
)),
|
||||||
('mju_mulPose',
|
('mju_mulPose',
|
||||||
FunctionDecl(
|
FunctionDecl(
|
||||||
name='mju_mulPose',
|
name='mju_mulPose',
|
||||||
|
|||||||
+62
-3
@@ -1200,6 +1200,11 @@ STRUCTS: Mapping[str, StructDecl] = dict([
|
|||||||
type=ValueType(name='int'),
|
type=ValueType(name='int'),
|
||||||
doc='number of kinematic trees under world body',
|
doc='number of kinematic trees under world body',
|
||||||
),
|
),
|
||||||
|
StructFieldDecl(
|
||||||
|
name='ngravcomp',
|
||||||
|
type=ValueType(name='int'),
|
||||||
|
doc='number of bodies with nonzero gravcomp',
|
||||||
|
),
|
||||||
StructFieldDecl(
|
StructFieldDecl(
|
||||||
name='nemax',
|
name='nemax',
|
||||||
type=ValueType(name='int'),
|
type=ValueType(name='int'),
|
||||||
@@ -2095,6 +2100,13 @@ STRUCTS: Mapping[str, StructDecl] = dict([
|
|||||||
),
|
),
|
||||||
doc='does light cast shadows (nlight x 1)',
|
doc='does light cast shadows (nlight x 1)',
|
||||||
),
|
),
|
||||||
|
StructFieldDecl(
|
||||||
|
name='light_bulbradius',
|
||||||
|
type=PointerType(
|
||||||
|
inner_type=ValueType(name='float'),
|
||||||
|
),
|
||||||
|
doc='light radius for soft shadows (nlight x 1)',
|
||||||
|
),
|
||||||
StructFieldDecl(
|
StructFieldDecl(
|
||||||
name='light_active',
|
name='light_active',
|
||||||
type=PointerType(
|
type=PointerType(
|
||||||
@@ -2655,6 +2667,13 @@ STRUCTS: Mapping[str, StructDecl] = dict([
|
|||||||
),
|
),
|
||||||
doc='convex graph data (nmeshgraph x 1)', # pylint: disable=line-too-long
|
doc='convex graph data (nmeshgraph x 1)', # pylint: disable=line-too-long
|
||||||
),
|
),
|
||||||
|
StructFieldDecl(
|
||||||
|
name='mesh_scale',
|
||||||
|
type=PointerType(
|
||||||
|
inner_type=ValueType(name='mjtNum'),
|
||||||
|
),
|
||||||
|
doc='scaling applied to asset vertices (nmesh x 3)',
|
||||||
|
),
|
||||||
StructFieldDecl(
|
StructFieldDecl(
|
||||||
name='mesh_pos',
|
name='mesh_pos',
|
||||||
type=PointerType(
|
type=PointerType(
|
||||||
@@ -2870,7 +2889,7 @@ STRUCTS: Mapping[str, StructDecl] = dict([
|
|||||||
type=PointerType(
|
type=PointerType(
|
||||||
inner_type=ValueType(name='int'),
|
inner_type=ValueType(name='int'),
|
||||||
),
|
),
|
||||||
doc='address of asset path for hfield; -1: none (nhfield x 1)',
|
doc='address of hfield asset path; -1: none (nhfield x 1)',
|
||||||
),
|
),
|
||||||
StructFieldDecl(
|
StructFieldDecl(
|
||||||
name='tex_type',
|
name='tex_type',
|
||||||
@@ -2905,14 +2924,14 @@ STRUCTS: Mapping[str, StructDecl] = dict([
|
|||||||
type=PointerType(
|
type=PointerType(
|
||||||
inner_type=ValueType(name='mjtByte'),
|
inner_type=ValueType(name='mjtByte'),
|
||||||
),
|
),
|
||||||
doc='rgb (alpha = 1) (ntexdata x 1)', # pylint: disable=line-too-long
|
doc='rgb (alpha = 1) (ntexdata x 1)',
|
||||||
),
|
),
|
||||||
StructFieldDecl(
|
StructFieldDecl(
|
||||||
name='tex_pathadr',
|
name='tex_pathadr',
|
||||||
type=PointerType(
|
type=PointerType(
|
||||||
inner_type=ValueType(name='int'),
|
inner_type=ValueType(name='int'),
|
||||||
),
|
),
|
||||||
doc='address of asset path for texture; -1: none (ntex x 1)',
|
doc='address of texture asset path; -1: none (ntex x 1)',
|
||||||
),
|
),
|
||||||
StructFieldDecl(
|
StructFieldDecl(
|
||||||
name='mat_texid',
|
name='mat_texid',
|
||||||
@@ -2963,6 +2982,20 @@ STRUCTS: Mapping[str, StructDecl] = dict([
|
|||||||
),
|
),
|
||||||
doc='reflectance (0: disable) (nmat x 1)',
|
doc='reflectance (0: disable) (nmat x 1)',
|
||||||
),
|
),
|
||||||
|
StructFieldDecl(
|
||||||
|
name='mat_metallic',
|
||||||
|
type=PointerType(
|
||||||
|
inner_type=ValueType(name='float'),
|
||||||
|
),
|
||||||
|
doc='metallic coef (nmat x 1)',
|
||||||
|
),
|
||||||
|
StructFieldDecl(
|
||||||
|
name='mat_roughness',
|
||||||
|
type=PointerType(
|
||||||
|
inner_type=ValueType(name='float'),
|
||||||
|
),
|
||||||
|
doc='roughness coef (nmat x 1)',
|
||||||
|
),
|
||||||
StructFieldDecl(
|
StructFieldDecl(
|
||||||
name='mat_rgba',
|
name='mat_rgba',
|
||||||
type=PointerType(
|
type=PointerType(
|
||||||
@@ -5610,6 +5643,11 @@ STRUCTS: Mapping[str, StructDecl] = dict([
|
|||||||
type=ValueType(name='mjtByte'),
|
type=ValueType(name='mjtByte'),
|
||||||
doc='does light cast shadows',
|
doc='does light cast shadows',
|
||||||
),
|
),
|
||||||
|
StructFieldDecl(
|
||||||
|
name='bulbradius',
|
||||||
|
type=ValueType(name='float'),
|
||||||
|
doc='bulb radius for soft shadows',
|
||||||
|
),
|
||||||
),
|
),
|
||||||
)),
|
)),
|
||||||
('mjvOption',
|
('mjvOption',
|
||||||
@@ -6705,6 +6743,13 @@ STRUCTS: Mapping[str, StructDecl] = dict([
|
|||||||
),
|
),
|
||||||
doc='',
|
doc='',
|
||||||
),
|
),
|
||||||
|
StructFieldDecl(
|
||||||
|
name='light_bulbradius',
|
||||||
|
type=PointerType(
|
||||||
|
inner_type=ValueType(name='float'),
|
||||||
|
),
|
||||||
|
doc='',
|
||||||
|
),
|
||||||
StructFieldDecl(
|
StructFieldDecl(
|
||||||
name='light_active',
|
name='light_active',
|
||||||
type=PointerType(
|
type=PointerType(
|
||||||
@@ -7132,6 +7177,20 @@ STRUCTS: Mapping[str, StructDecl] = dict([
|
|||||||
),
|
),
|
||||||
doc='',
|
doc='',
|
||||||
),
|
),
|
||||||
|
StructFieldDecl(
|
||||||
|
name='mat_metallic',
|
||||||
|
type=PointerType(
|
||||||
|
inner_type=ValueType(name='float'),
|
||||||
|
),
|
||||||
|
doc='',
|
||||||
|
),
|
||||||
|
StructFieldDecl(
|
||||||
|
name='mat_roughness',
|
||||||
|
type=PointerType(
|
||||||
|
inner_type=ValueType(name='float'),
|
||||||
|
),
|
||||||
|
doc='',
|
||||||
|
),
|
||||||
StructFieldDecl(
|
StructFieldDecl(
|
||||||
name='mat_rgba',
|
name='mat_rgba',
|
||||||
type=PointerType(
|
type=PointerType(
|
||||||
|
|||||||
@@ -16,12 +16,7 @@
|
|||||||
|
|
||||||
# pylint:disable=g-importing-member
|
# pylint:disable=g-importing-member
|
||||||
from mujoco.mjx._src.collision_driver import collision
|
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
|
from mujoco.mjx._src.constraint import make_constraint
|
||||||
from mujoco.mjx._src.device import device_get_into
|
|
||||||
from mujoco.mjx._src.device import device_put
|
|
||||||
from mujoco.mjx._src.forward import euler
|
from mujoco.mjx._src.forward import euler
|
||||||
from mujoco.mjx._src.forward import forward
|
from mujoco.mjx._src.forward import forward
|
||||||
from mujoco.mjx._src.forward import fwd_acceleration
|
from mujoco.mjx._src.forward import fwd_acceleration
|
||||||
@@ -47,7 +42,9 @@ from mujoco.mjx._src.smooth import rne
|
|||||||
from mujoco.mjx._src.smooth import transmission
|
from mujoco.mjx._src.smooth import transmission
|
||||||
from mujoco.mjx._src.solver import solve
|
from mujoco.mjx._src.solver import solve
|
||||||
from mujoco.mjx._src.support import full_m
|
from mujoco.mjx._src.support import full_m
|
||||||
|
from mujoco.mjx._src.support import id2name
|
||||||
from mujoco.mjx._src.support import is_sparse
|
from mujoco.mjx._src.support import is_sparse
|
||||||
from mujoco.mjx._src.support import mul_m
|
from mujoco.mjx._src.support import mul_m
|
||||||
|
from mujoco.mjx._src.support import name2id
|
||||||
from mujoco.mjx._src.test_util import benchmark
|
from mujoco.mjx._src.test_util import benchmark
|
||||||
from mujoco.mjx._src.types import *
|
from mujoco.mjx._src.types import *
|
||||||
|
|||||||
@@ -1,67 +0,0 @@
|
|||||||
# Copyright 2023 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.
|
|
||||||
# ==============================================================================
|
|
||||||
"""Collision base."""
|
|
||||||
|
|
||||||
import dataclasses
|
|
||||||
from typing import Dict, List, Optional, Tuple
|
|
||||||
|
|
||||||
import jax
|
|
||||||
# pylint: disable=g-importing-member
|
|
||||||
from mujoco.mjx._src.dataclasses import PyTreeNode
|
|
||||||
from mujoco.mjx._src.types import GeomType
|
|
||||||
# pylint: enable=g-importing-member
|
|
||||||
|
|
||||||
Contact = Tuple[jax.Array, jax.Array, jax.Array]
|
|
||||||
|
|
||||||
|
|
||||||
@dataclasses.dataclass
|
|
||||||
class Candidate:
|
|
||||||
geom1: int
|
|
||||||
geom2: int
|
|
||||||
ipair: int
|
|
||||||
geomp: int # priority geom
|
|
||||||
dim: int
|
|
||||||
|
|
||||||
|
|
||||||
CandidateSet = Dict[
|
|
||||||
Tuple[GeomType, GeomType, Tuple[int, ...], Tuple[int, ...]],
|
|
||||||
List[Candidate],
|
|
||||||
]
|
|
||||||
|
|
||||||
|
|
||||||
class GeomInfo(PyTreeNode):
|
|
||||||
"""Collision info for a geom."""
|
|
||||||
|
|
||||||
geom_id: jax.Array
|
|
||||||
pos: jax.Array
|
|
||||||
mat: jax.Array
|
|
||||||
size: jax.Array
|
|
||||||
face: Optional[jax.Array] = None
|
|
||||||
vert: Optional[jax.Array] = None
|
|
||||||
edge_dir: Optional[jax.Array] = None
|
|
||||||
facenorm: Optional[jax.Array] = None
|
|
||||||
edge: Optional[jax.Array] = None
|
|
||||||
edge_face_normal: Optional[jax.Array] = None
|
|
||||||
|
|
||||||
|
|
||||||
class SolverParams(PyTreeNode):
|
|
||||||
"""Contact solver params."""
|
|
||||||
|
|
||||||
friction: jax.Array
|
|
||||||
solref: jax.Array
|
|
||||||
solreffriction: jax.Array
|
|
||||||
solimp: jax.Array
|
|
||||||
margin: jax.Array
|
|
||||||
gap: jax.Array
|
|
||||||
@@ -20,12 +20,53 @@ from typing import Tuple
|
|||||||
import jax
|
import jax
|
||||||
from jax import numpy as jp
|
from jax import numpy as jp
|
||||||
from mujoco.mjx._src import math
|
from mujoco.mjx._src import math
|
||||||
|
from mujoco.mjx._src import mesh
|
||||||
# pylint: disable=g-importing-member
|
# pylint: disable=g-importing-member
|
||||||
from mujoco.mjx._src.collision_base import Contact
|
from mujoco.mjx._src.collision_types import Collision
|
||||||
from mujoco.mjx._src.collision_base import GeomInfo
|
from mujoco.mjx._src.collision_types import ConvexInfo
|
||||||
|
from mujoco.mjx._src.collision_types import FunctionKey
|
||||||
|
from mujoco.mjx._src.collision_types import GeomInfo
|
||||||
|
from mujoco.mjx._src.types import Data
|
||||||
|
from mujoco.mjx._src.types import GeomType
|
||||||
|
from mujoco.mjx._src.types import Model
|
||||||
# pylint: enable=g-importing-member
|
# pylint: enable=g-importing-member
|
||||||
|
|
||||||
|
|
||||||
|
def collider(ncon: int):
|
||||||
|
"""Wraps collision functions for use by collision_driver."""
|
||||||
|
|
||||||
|
def wrapper(func):
|
||||||
|
def collide(
|
||||||
|
m: Model, d: Data, key: FunctionKey, geom: jax.Array
|
||||||
|
) -> Collision:
|
||||||
|
g1, g2 = geom.T
|
||||||
|
infos = [
|
||||||
|
GeomInfo(d.geom_xpos[g1], d.geom_xmat[g1], m.geom_size[g1]),
|
||||||
|
GeomInfo(d.geom_xpos[g2], d.geom_xmat[g2], m.geom_size[g2]),
|
||||||
|
]
|
||||||
|
in_axes = [0, 0]
|
||||||
|
for i in [0, 1]:
|
||||||
|
if key.types[i] == GeomType.BOX:
|
||||||
|
infos[i] = mesh.box(infos[i])
|
||||||
|
in_axes[i] = jax.tree_util.tree_map(lambda x: None, infos[i]).replace(
|
||||||
|
pos=0, mat=0, face=0, vert=0
|
||||||
|
)
|
||||||
|
elif key.types[i] == GeomType.MESH:
|
||||||
|
infos[i] = mesh.convex(m, key.data_ids[i], infos[i])
|
||||||
|
in_axes[i] = jax.tree_util.tree_map(lambda x: None, infos[i]).replace(
|
||||||
|
pos=0, mat=0
|
||||||
|
)
|
||||||
|
dist, pos, frame = jax.vmap(func, in_axes=in_axes)(*infos)
|
||||||
|
if ncon > 1:
|
||||||
|
return jax.tree_util.tree_map(jp.concatenate, (dist, pos, frame))
|
||||||
|
return dist, pos, frame
|
||||||
|
|
||||||
|
collide.ncon = ncon
|
||||||
|
return collide
|
||||||
|
|
||||||
|
return wrapper
|
||||||
|
|
||||||
|
|
||||||
def _closest_segment_point_plane(
|
def _closest_segment_point_plane(
|
||||||
a: jax.Array, b: jax.Array, p0: jax.Array, plane_normal: jax.Array
|
a: jax.Array, b: jax.Array, p0: jax.Array, plane_normal: jax.Array
|
||||||
) -> jax.Array:
|
) -> jax.Array:
|
||||||
@@ -178,7 +219,8 @@ def _manifold_points(
|
|||||||
return jp.array([a_idx, b_idx, c_idx, d_idx])
|
return jp.array([a_idx, b_idx, c_idx, d_idx])
|
||||||
|
|
||||||
|
|
||||||
def plane_convex(plane: GeomInfo, convex: GeomInfo) -> Contact:
|
@collider(ncon=4)
|
||||||
|
def plane_convex(plane: GeomInfo, convex: ConvexInfo) -> Collision:
|
||||||
"""Calculates contacts between a plane and a convex object."""
|
"""Calculates contacts between a plane and a convex object."""
|
||||||
vert = convex.vert
|
vert = convex.vert
|
||||||
|
|
||||||
@@ -186,7 +228,7 @@ def plane_convex(plane: GeomInfo, convex: GeomInfo) -> Contact:
|
|||||||
plane_pos = convex.mat.T @ (plane.pos - convex.pos)
|
plane_pos = convex.mat.T @ (plane.pos - convex.pos)
|
||||||
n = convex.mat.T @ plane.mat[:, 2]
|
n = convex.mat.T @ plane.mat[:, 2]
|
||||||
support = (plane_pos - vert) @ n
|
support = (plane_pos - vert) @ n
|
||||||
idx = _manifold_points(vert, support > 0, n)
|
idx = _manifold_points(vert, support > jp.maximum(0, support.max() - 1e-4), n)
|
||||||
pos = vert[idx]
|
pos = vert[idx]
|
||||||
|
|
||||||
# convert to world frame
|
# convert to world frame
|
||||||
@@ -200,10 +242,11 @@ def plane_convex(plane: GeomInfo, convex: GeomInfo) -> Contact:
|
|||||||
return dist, pos, frame
|
return dist, pos, frame
|
||||||
|
|
||||||
|
|
||||||
def sphere_convex(sphere: GeomInfo, convex: GeomInfo) -> Contact:
|
@collider(ncon=1)
|
||||||
|
def sphere_convex(sphere: GeomInfo, convex: ConvexInfo) -> Collision:
|
||||||
"""Calculates contact between a sphere and a convex object."""
|
"""Calculates contact between a sphere and a convex object."""
|
||||||
faces = convex.face
|
faces = convex.face
|
||||||
normals = convex.facenorm
|
normals = convex.face_normal
|
||||||
|
|
||||||
# Put sphere in convex frame.
|
# Put sphere in convex frame.
|
||||||
sphere_pos = convex.mat.T @ (sphere.pos - convex.pos)
|
sphere_pos = convex.mat.T @ (sphere.pos - convex.pos)
|
||||||
@@ -262,16 +305,15 @@ def sphere_convex(sphere: GeomInfo, convex: GeomInfo) -> Contact:
|
|||||||
n = convex.mat @ n
|
n = convex.mat @ n
|
||||||
pos = convex.mat @ pos + convex.pos
|
pos = convex.mat @ pos + convex.pos
|
||||||
|
|
||||||
return jax.tree_map(
|
return dist, pos, math.make_frame(n)
|
||||||
lambda x: jp.expand_dims(x, axis=0), (dist, pos, math.make_frame(n))
|
|
||||||
)
|
|
||||||
|
|
||||||
|
|
||||||
def capsule_convex(cap: GeomInfo, convex: GeomInfo) -> Contact:
|
@collider(ncon=2)
|
||||||
|
def capsule_convex(cap: GeomInfo, convex: ConvexInfo) -> Collision:
|
||||||
"""Calculates contacts between a capsule and a convex object."""
|
"""Calculates contacts between a capsule and a convex object."""
|
||||||
# Get convex transformed normals, faces, and vertices.
|
# Get convex transformed normals, faces, and vertices.
|
||||||
faces = convex.face
|
faces = convex.face
|
||||||
normals = convex.facenorm
|
normals = convex.face_normal
|
||||||
|
|
||||||
# Put capsule in convex frame.
|
# Put capsule in convex frame.
|
||||||
cap_pos = convex.mat.T @ (cap.pos - convex.pos)
|
cap_pos = convex.mat.T @ (cap.pos - convex.pos)
|
||||||
@@ -347,7 +389,7 @@ def capsule_convex(cap: GeomInfo, convex: GeomInfo) -> Contact:
|
|||||||
degenerate_edge_dir,
|
degenerate_edge_dir,
|
||||||
edge_closest_pt,
|
edge_closest_pt,
|
||||||
cap_closest_pt,
|
cap_closest_pt,
|
||||||
) = jax.tree_map(lambda x, i=e_idx: jp.take(x, i, axis=0), res)
|
) = jax.tree_util.tree_map(lambda x, i=e_idx: jp.take(x, i, axis=0), res)
|
||||||
|
|
||||||
edge_face_normals = edge_face_normal[e_idx]
|
edge_face_normals = edge_face_normal[e_idx]
|
||||||
edge_voronoi_front = ((edge_face_normals @ edge_axis) < 0).all()
|
edge_voronoi_front = ((edge_face_normals @ edge_axis) < 0).all()
|
||||||
@@ -869,10 +911,9 @@ def _sat_gaussmap(
|
|||||||
return dist, pos, normal
|
return dist, pos, normal
|
||||||
|
|
||||||
|
|
||||||
def convex_convex(c1: GeomInfo, c2: GeomInfo) -> Contact:
|
@collider(ncon=4)
|
||||||
|
def convex_convex(c1: ConvexInfo, c2: ConvexInfo) -> Collision:
|
||||||
"""Calculates contacts between two convex objects."""
|
"""Calculates contacts between two convex objects."""
|
||||||
if c1.face is None or c2.face is None or c1.vert is None or c2.vert is None:
|
|
||||||
raise AssertionError('Mesh info missing.')
|
|
||||||
# pad face vertices so that we can broadcast between geom1 and geom2
|
# pad face vertices so that we can broadcast between geom1 and geom2
|
||||||
# face has shape (n_face, n_vert, 3)
|
# face has shape (n_face, n_vert, 3)
|
||||||
nvert1, nvert2 = c1.face.shape[1], c2.face.shape[1]
|
nvert1, nvert2 = c1.face.shape[1], c2.face.shape[1]
|
||||||
@@ -895,14 +936,14 @@ def convex_convex(c1: GeomInfo, c2: GeomInfo) -> Contact:
|
|||||||
to_local_mat = c2.mat.T @ c1.mat
|
to_local_mat = c2.mat.T @ c1.mat
|
||||||
|
|
||||||
faces1 = to_local_pos + faces1 @ to_local_mat.T
|
faces1 = to_local_pos + faces1 @ to_local_mat.T
|
||||||
normals1 = c1.facenorm @ to_local_mat.T
|
normals1 = c1.face_normal @ to_local_mat.T
|
||||||
normals2 = c2.facenorm
|
normals2 = c2.face_normal
|
||||||
|
|
||||||
vertices1 = to_local_pos + c1.vert @ to_local_mat.T
|
vertices1 = to_local_pos + c1.vert @ to_local_mat.T
|
||||||
vertices2 = c2.vert
|
vertices2 = c2.vert
|
||||||
|
|
||||||
unique_edges1 = jp.take(vertices1, c1.edge, axis=0)
|
unique_edges1 = jp.take(vertices1, c1.edge_dir, axis=0)
|
||||||
unique_edges2 = jp.take(vertices2, c2.edge, axis=0)
|
unique_edges2 = jp.take(vertices2, c2.edge_dir, axis=0)
|
||||||
|
|
||||||
edges1 = jp.take(vertices1, c1.edge, axis=0)
|
edges1 = jp.take(vertices1, c1.edge, axis=0)
|
||||||
edges2 = jp.take(vertices2, c2.edge, axis=0)
|
edges2 = jp.take(vertices2, c2.edge, axis=0)
|
||||||
@@ -944,13 +985,6 @@ def convex_convex(c1: GeomInfo, c2: GeomInfo) -> Contact:
|
|||||||
pos = c2.pos + pos @ c2.mat.T
|
pos = c2.pos + pos @ c2.mat.T
|
||||||
normal = normal @ c2.mat.T
|
normal = normal @ c2.mat.T
|
||||||
normal = -normal if swapped else normal
|
normal = -normal if swapped else normal
|
||||||
|
|
||||||
frame = jax.vmap(math.make_frame)(normal)
|
frame = jax.vmap(math.make_frame)(normal)
|
||||||
|
|
||||||
return dist, pos, frame
|
return dist, pos, frame
|
||||||
|
|
||||||
|
|
||||||
# store ncon as function attributes
|
|
||||||
plane_convex.ncon = 4
|
|
||||||
sphere_convex.ncon = 1
|
|
||||||
capsule_convex.ncon = 2
|
|
||||||
convex_convex.ncon = 4
|
|
||||||
|
|||||||
@@ -12,37 +12,63 @@
|
|||||||
# See the License for the specific language governing permissions and
|
# See the License for the specific language governing permissions and
|
||||||
# limitations under the License.
|
# limitations under the License.
|
||||||
# ==============================================================================
|
# ==============================================================================
|
||||||
"""Collide geometries."""
|
"""Runs collision checking for all geoms in a Model.
|
||||||
|
|
||||||
from typing import Callable, Dict, List, Optional, Sequence, Tuple, Union
|
To do this, collision_driver builds a collision function table, and then runs
|
||||||
|
the collision functions serially on the parameters in the table.
|
||||||
|
|
||||||
|
For example, if a Model has three geoms:
|
||||||
|
|
||||||
|
geom | type
|
||||||
|
---------------
|
||||||
|
1 | sphere
|
||||||
|
2 | capsule
|
||||||
|
3 | sphere
|
||||||
|
|
||||||
|
collision_driver organizes it into these functions and runs them:
|
||||||
|
|
||||||
|
function | geom pair
|
||||||
|
--------------------------
|
||||||
|
sphere_sphere | (1, 3)
|
||||||
|
sphere_capsule | (1, 2), (2, 3)
|
||||||
|
|
||||||
|
|
||||||
|
Besides collision function, function tables are keyed on mesh id and condim,
|
||||||
|
in order to guarantee static shapes for contacts and jacobians.
|
||||||
|
"""
|
||||||
|
|
||||||
|
import itertools
|
||||||
|
from typing import Dict, Iterator, List, Tuple, Union
|
||||||
|
|
||||||
import jax
|
import jax
|
||||||
from jax import numpy as jp
|
from jax import numpy as jp
|
||||||
import mujoco
|
import mujoco
|
||||||
from mujoco.mjx._src import collision_base
|
|
||||||
from mujoco.mjx._src import mesh
|
|
||||||
from mujoco.mjx._src import support
|
from mujoco.mjx._src import support
|
||||||
# pylint: disable=g-importing-member
|
# pylint: disable=g-importing-member
|
||||||
from mujoco.mjx._src.collision_base import Candidate
|
|
||||||
from mujoco.mjx._src.collision_base import CandidateSet
|
|
||||||
from mujoco.mjx._src.collision_base import GeomInfo
|
|
||||||
from mujoco.mjx._src.collision_base import SolverParams
|
|
||||||
from mujoco.mjx._src.collision_convex import capsule_convex
|
from mujoco.mjx._src.collision_convex import capsule_convex
|
||||||
from mujoco.mjx._src.collision_convex import convex_convex
|
from mujoco.mjx._src.collision_convex import convex_convex
|
||||||
from mujoco.mjx._src.collision_convex import plane_convex
|
from mujoco.mjx._src.collision_convex import plane_convex
|
||||||
from mujoco.mjx._src.collision_convex import sphere_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 capsule_capsule
|
||||||
from mujoco.mjx._src.collision_primitive import plane_capsule
|
from mujoco.mjx._src.collision_primitive import plane_capsule
|
||||||
|
from mujoco.mjx._src.collision_primitive import plane_cylinder
|
||||||
from mujoco.mjx._src.collision_primitive import plane_ellipsoid
|
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 plane_sphere
|
||||||
from mujoco.mjx._src.collision_primitive import sphere_capsule
|
from mujoco.mjx._src.collision_primitive import sphere_capsule
|
||||||
from mujoco.mjx._src.collision_primitive import sphere_sphere
|
from mujoco.mjx._src.collision_primitive import sphere_sphere
|
||||||
|
from mujoco.mjx._src.collision_sdf import capsule_cylinder
|
||||||
|
from mujoco.mjx._src.collision_sdf import capsule_ellipsoid
|
||||||
|
from mujoco.mjx._src.collision_sdf import cylinder_cylinder
|
||||||
|
from mujoco.mjx._src.collision_sdf import ellipsoid_cylinder
|
||||||
|
from mujoco.mjx._src.collision_sdf import ellipsoid_ellipsoid
|
||||||
|
from mujoco.mjx._src.collision_types import FunctionKey
|
||||||
from mujoco.mjx._src.types import Contact
|
from mujoco.mjx._src.types import Contact
|
||||||
from mujoco.mjx._src.types import Data
|
from mujoco.mjx._src.types import Data
|
||||||
from mujoco.mjx._src.types import DisableBit
|
from mujoco.mjx._src.types import DisableBit
|
||||||
from mujoco.mjx._src.types import GeomType
|
from mujoco.mjx._src.types import GeomType
|
||||||
from mujoco.mjx._src.types import Model
|
from mujoco.mjx._src.types import Model
|
||||||
# pylint: enable=g-importing-member
|
# pylint: enable=g-importing-member
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
# pair-wise collision functions
|
# pair-wise collision functions
|
||||||
_COLLISION_FUNC = {
|
_COLLISION_FUNC = {
|
||||||
@@ -50,6 +76,7 @@ _COLLISION_FUNC = {
|
|||||||
(GeomType.PLANE, GeomType.CAPSULE): plane_capsule,
|
(GeomType.PLANE, GeomType.CAPSULE): plane_capsule,
|
||||||
(GeomType.PLANE, GeomType.BOX): plane_convex,
|
(GeomType.PLANE, GeomType.BOX): plane_convex,
|
||||||
(GeomType.PLANE, GeomType.ELLIPSOID): plane_ellipsoid,
|
(GeomType.PLANE, GeomType.ELLIPSOID): plane_ellipsoid,
|
||||||
|
(GeomType.PLANE, GeomType.CYLINDER): plane_cylinder,
|
||||||
(GeomType.PLANE, GeomType.MESH): plane_convex,
|
(GeomType.PLANE, GeomType.MESH): plane_convex,
|
||||||
(GeomType.SPHERE, GeomType.SPHERE): sphere_sphere,
|
(GeomType.SPHERE, GeomType.SPHERE): sphere_sphere,
|
||||||
(GeomType.SPHERE, GeomType.CAPSULE): sphere_capsule,
|
(GeomType.SPHERE, GeomType.CAPSULE): sphere_capsule,
|
||||||
@@ -57,378 +84,315 @@ _COLLISION_FUNC = {
|
|||||||
(GeomType.SPHERE, GeomType.MESH): sphere_convex,
|
(GeomType.SPHERE, GeomType.MESH): sphere_convex,
|
||||||
(GeomType.CAPSULE, GeomType.CAPSULE): capsule_capsule,
|
(GeomType.CAPSULE, GeomType.CAPSULE): capsule_capsule,
|
||||||
(GeomType.CAPSULE, GeomType.BOX): capsule_convex,
|
(GeomType.CAPSULE, GeomType.BOX): capsule_convex,
|
||||||
|
(GeomType.CAPSULE, GeomType.ELLIPSOID): capsule_ellipsoid,
|
||||||
|
(GeomType.CAPSULE, GeomType.CYLINDER): capsule_cylinder,
|
||||||
(GeomType.CAPSULE, GeomType.MESH): capsule_convex,
|
(GeomType.CAPSULE, GeomType.MESH): capsule_convex,
|
||||||
|
(GeomType.ELLIPSOID, GeomType.ELLIPSOID): ellipsoid_ellipsoid,
|
||||||
|
(GeomType.ELLIPSOID, GeomType.CYLINDER): ellipsoid_cylinder,
|
||||||
|
(GeomType.CYLINDER, GeomType.CYLINDER): cylinder_cylinder,
|
||||||
(GeomType.BOX, GeomType.BOX): convex_convex,
|
(GeomType.BOX, GeomType.BOX): convex_convex,
|
||||||
(GeomType.BOX, GeomType.MESH): convex_convex,
|
(GeomType.BOX, GeomType.MESH): convex_convex,
|
||||||
(GeomType.MESH, GeomType.MESH): convex_convex,
|
(GeomType.MESH, GeomType.MESH): convex_convex,
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
def get_collision_fn(
|
# geoms for which we ignore broadphase
|
||||||
key: Tuple[Union[GeomType, mujoco.mjtGeom], Union[GeomType, mujoco.mjtGeom]]
|
_GEOM_NO_BROADPHASE = {GeomType.HFIELD, GeomType.PLANE}
|
||||||
) -> Optional[Callable[[GeomInfo, GeomInfo], collision_base.Contact]]:
|
|
||||||
"""Returns a collision function given a pair of geom types."""
|
|
||||||
return _COLLISION_FUNC.get(key, None)
|
|
||||||
|
|
||||||
|
|
||||||
def _add_candidate(
|
def has_collision_fn(t1: GeomType, t2: GeomType) -> bool:
|
||||||
result: CandidateSet,
|
"""Returns True if a collision function exists for a pair of geom types."""
|
||||||
|
return (t1, t2) in _COLLISION_FUNC
|
||||||
|
|
||||||
|
|
||||||
|
def geom_pairs(
|
||||||
m: Union[Model, mujoco.MjModel],
|
m: Union[Model, mujoco.MjModel],
|
||||||
g1: int,
|
) -> Iterator[Tuple[int, int, int]]:
|
||||||
g2: int,
|
"""Yields geom pairs to check for collisions.
|
||||||
ipair: int = -1,
|
|
||||||
):
|
|
||||||
"""Adds a candidate to test for collision."""
|
|
||||||
t1, t2 = m.geom_type[g1], m.geom_type[g2]
|
|
||||||
if t1 > t2:
|
|
||||||
t1, t2, g1, g2 = t2, t1, g2, g1
|
|
||||||
|
|
||||||
# MuJoCo does not collide planes with other planes or hfields
|
Args:
|
||||||
if t1 == GeomType.PLANE and t2 == GeomType.PLANE:
|
m: a MuJoCo or MJX model
|
||||||
return
|
|
||||||
if t1 == GeomType.PLANE and t2 == GeomType.HFIELD:
|
|
||||||
return
|
|
||||||
|
|
||||||
def mesh_key(i):
|
Yields:
|
||||||
convex_data = [[None] * m.ngeom] * 3
|
geom1, geom2, and pair index if defined in <pair> (else -1)
|
||||||
if isinstance(m, Model):
|
"""
|
||||||
convex_data = [
|
pairs = set()
|
||||||
m.geom_convex_face,
|
|
||||||
m.geom_convex_vert,
|
|
||||||
m.geom_convex_edge_dir,
|
|
||||||
]
|
|
||||||
elif isinstance(m, mujoco.MjModel):
|
|
||||||
kwargs = mesh.get(m)
|
|
||||||
convex_data = [
|
|
||||||
kwargs['geom_convex_face'],
|
|
||||||
kwargs['geom_convex_vert'],
|
|
||||||
kwargs['geom_convex_edge_dir'],
|
|
||||||
]
|
|
||||||
key = tuple((-1,) if v[i] is None else v[i].shape for v in convex_data)
|
|
||||||
return key
|
|
||||||
|
|
||||||
k1, k2 = mesh_key(g1), mesh_key(g2)
|
for i in range(m.npair):
|
||||||
|
g1, g2 = m.pair_geom1[i], m.pair_geom2[i]
|
||||||
|
# order pairs by geom_type for correct function mapping
|
||||||
|
if m.geom_type[g1] > m.geom_type[g2]:
|
||||||
|
g1, g2 = g2, g1
|
||||||
|
pairs.add((g1, g2))
|
||||||
|
yield g1, g2, i
|
||||||
|
|
||||||
candidates = {(c.geom1, c.geom2) for c in result.get((t1, t2, k1, k2), [])}
|
|
||||||
if (g1, g2) in candidates:
|
|
||||||
return
|
|
||||||
|
|
||||||
if ipair > -1:
|
|
||||||
candidate = Candidate(g1, g2, ipair, -1, m.pair_dim[ipair])
|
|
||||||
elif m.geom_priority[g1] != m.geom_priority[g2]:
|
|
||||||
gp = g1 if m.geom_priority[g1] > m.geom_priority[g2] else g2
|
|
||||||
candidate = Candidate(g1, g2, -1, gp, m.geom_condim[gp])
|
|
||||||
else:
|
|
||||||
dim = max(m.geom_condim[g1], m.geom_condim[g2])
|
|
||||||
candidate = Candidate(g1, g2, -1, -1, dim)
|
|
||||||
|
|
||||||
result.setdefault((t1, t2, k1, k2), []).append(candidate)
|
|
||||||
|
|
||||||
|
|
||||||
def _pair_params(
|
|
||||||
m: Model,
|
|
||||||
candidates: Sequence[Candidate],
|
|
||||||
) -> SolverParams:
|
|
||||||
"""Gets solver params for pair geoms."""
|
|
||||||
ipair = jp.array([c.ipair for c in candidates])
|
|
||||||
friction = jp.clip(m.pair_friction[ipair], a_min=mujoco.mjMINMU)
|
|
||||||
solref = m.pair_solref[ipair]
|
|
||||||
solreffriction = m.pair_solreffriction[ipair]
|
|
||||||
solimp = m.pair_solimp[ipair]
|
|
||||||
margin = m.pair_margin[ipair]
|
|
||||||
gap = m.pair_gap[ipair]
|
|
||||||
|
|
||||||
return SolverParams(friction, solref, solreffriction, solimp, margin, gap)
|
|
||||||
|
|
||||||
|
|
||||||
def _priority_params(
|
|
||||||
m: Model,
|
|
||||||
candidates: Sequence[Candidate],
|
|
||||||
) -> SolverParams:
|
|
||||||
"""Gets solver params from priority geoms."""
|
|
||||||
geomp = jp.array([c.geomp for c in candidates])
|
|
||||||
friction = m.geom_friction[geomp][:, jp.array([0, 0, 1, 2, 2])]
|
|
||||||
solref = m.geom_solref[geomp]
|
|
||||||
solreffriction = jp.zeros(geomp.shape + (mujoco.mjNREF,))
|
|
||||||
solimp = m.geom_solimp[geomp]
|
|
||||||
g = jp.array([(c.geom1, c.geom2) for c in candidates])
|
|
||||||
margin = jp.amax(m.geom_margin[g.T], axis=0)
|
|
||||||
gap = jp.amax(m.geom_gap[g.T], axis=0)
|
|
||||||
|
|
||||||
return SolverParams(friction, solref, solreffriction, solimp, margin, gap)
|
|
||||||
|
|
||||||
|
|
||||||
def _dynamic_params(
|
|
||||||
m: Model,
|
|
||||||
candidates: Sequence[Candidate],
|
|
||||||
) -> SolverParams:
|
|
||||||
"""Gets solver params for dynamic geoms."""
|
|
||||||
g1 = jp.array([c.geom1 for c in candidates])
|
|
||||||
g2 = jp.array([c.geom2 for c in candidates])
|
|
||||||
|
|
||||||
friction = jp.maximum(m.geom_friction[g1], m.geom_friction[g2])
|
|
||||||
# copy friction terms for the full geom pair
|
|
||||||
friction = friction[:, jp.array([0, 0, 1, 2, 2])]
|
|
||||||
|
|
||||||
minval = jp.array(mujoco.mjMINVAL)
|
|
||||||
solmix1, solmix2 = m.geom_solmix[g1], m.geom_solmix[g2]
|
|
||||||
mix = solmix1 / (solmix1 + solmix2)
|
|
||||||
mix = jp.where((solmix1 < minval) & (solmix2 < minval), 0.5, mix)
|
|
||||||
mix = jp.where((solmix1 < minval) & (solmix2 >= minval), 0.0, mix)
|
|
||||||
mix_fn = jax.vmap(lambda a, b, m: m * a + (1 - m) * b)
|
|
||||||
|
|
||||||
solref1, solref2 = m.geom_solref[g1], m.geom_solref[g2]
|
|
||||||
solref = jp.minimum(solref1, solref2)
|
|
||||||
s_mix = mix_fn(solref1, solref2, mix)
|
|
||||||
solref = jp.where((solref1[0] > 0) & (solref2[0] > 0), s_mix, solref)
|
|
||||||
solreffriction = jp.zeros(g1.shape + (mujoco.mjNREF,))
|
|
||||||
solimp = mix_fn(m.geom_solimp[g1], m.geom_solimp[g2], mix)
|
|
||||||
margin = jp.maximum(m.geom_margin[g1], m.geom_margin[g2])
|
|
||||||
gap = jp.maximum(m.geom_gap[g1], m.geom_gap[g2])
|
|
||||||
|
|
||||||
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]]]]:
|
|
||||||
"""Returns geom pair info for calculating collision."""
|
|
||||||
def mesh_info(geom):
|
|
||||||
g = jp.array(geom)
|
|
||||||
info = GeomInfo(
|
|
||||||
g,
|
|
||||||
d.geom_xpos[g],
|
|
||||||
d.geom_xmat[g],
|
|
||||||
m.geom_size[g],
|
|
||||||
)
|
|
||||||
in_axes = jax.tree_map(lambda x: 0, info)
|
|
||||||
is_mesh = m.geom_convex_face[geom[0]] is not None
|
|
||||||
if is_mesh:
|
|
||||||
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_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]),
|
|
||||||
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]
|
|
||||||
),
|
|
||||||
)
|
|
||||||
in_axes = in_axes.replace(
|
|
||||||
face=0,
|
|
||||||
vert=0,
|
|
||||||
edge_dir=0,
|
|
||||||
facenorm=0,
|
|
||||||
edge=0,
|
|
||||||
edge_face_normal=0,
|
|
||||||
)
|
|
||||||
return info, in_axes
|
|
||||||
|
|
||||||
info1, in_axes1 = mesh_info(geom1)
|
|
||||||
info2, in_axes2 = mesh_info(geom2)
|
|
||||||
return info1, info2, [in_axes1, in_axes2]
|
|
||||||
|
|
||||||
|
|
||||||
def _body_pair_filter(
|
|
||||||
m: Union[Model, mujoco.MjModel], b1: int, b2: int
|
|
||||||
) -> bool:
|
|
||||||
"""Filters body pairs for collision."""
|
|
||||||
dsbl_filterparent = m.opt.disableflags & DisableBit.FILTERPARENT
|
|
||||||
weld1 = m.body_weldid[b1]
|
|
||||||
weld2 = m.body_weldid[b2]
|
|
||||||
parent_weld1 = m.body_weldid[m.body_parentid[weld1]]
|
|
||||||
parent_weld2 = m.body_weldid[m.body_parentid[weld2]]
|
|
||||||
|
|
||||||
if weld1 == weld2:
|
|
||||||
# filter out self-collisions
|
|
||||||
return True
|
|
||||||
|
|
||||||
if (
|
|
||||||
not dsbl_filterparent
|
|
||||||
and weld1 != 0
|
|
||||||
and weld2 != 0
|
|
||||||
and (weld1 == parent_weld2 or weld2 == parent_weld1)
|
|
||||||
):
|
|
||||||
# filter out parent-child collisions
|
|
||||||
return True
|
|
||||||
|
|
||||||
return False
|
|
||||||
|
|
||||||
|
|
||||||
def _broadphase_enabled(
|
|
||||||
geom_types: Tuple[GeomType, GeomType],
|
|
||||||
n_pairs: int,
|
|
||||||
max_pairs: int,
|
|
||||||
) -> bool:
|
|
||||||
return (
|
|
||||||
GeomType.PLANE not in geom_types
|
|
||||||
and max_pairs > -1
|
|
||||||
and n_pairs > max_pairs
|
|
||||||
)
|
|
||||||
|
|
||||||
|
|
||||||
def _collide_geoms(
|
|
||||||
m: Model,
|
|
||||||
d: Data,
|
|
||||||
geom_types: Tuple[GeomType, GeomType],
|
|
||||||
candidates: Sequence[Candidate],
|
|
||||||
) -> Contact:
|
|
||||||
"""Collides a geom pair."""
|
|
||||||
fn = get_collision_fn(geom_types)
|
|
||||||
if not fn:
|
|
||||||
return Contact.zero()
|
|
||||||
|
|
||||||
geom1, geom2, params = get_params(m, candidates)
|
|
||||||
g1, g2, in_axes = _pair_info(m, d, geom1, geom2)
|
|
||||||
|
|
||||||
# Run a crude version of broadphase.
|
|
||||||
max_pairs = int(support.get_custom_numeric(m, 'max_geom_pairs'))
|
|
||||||
run_broadphase = _broadphase_enabled(geom_types, len(geom1), max_pairs)
|
|
||||||
n_pairs = max_pairs if run_broadphase else len(geom1)
|
|
||||||
if run_broadphase:
|
|
||||||
# broadphase over geom pairs, using bounding spheres
|
|
||||||
size1 = jp.max(m.geom_size[g1.geom_id], axis=-1)
|
|
||||||
size2 = jp.max(m.geom_size[g2.geom_id], axis=-1)
|
|
||||||
dists = jax.vmap(jp.linalg.norm)(g2.pos - g1.pos) - (size1 + size2)
|
|
||||||
_, idx = jax.lax.top_k(-dists, k=n_pairs)
|
|
||||||
g1, g2, params = jax.tree_map(
|
|
||||||
lambda x, idx=idx: x[idx, ...], (g1, g2, params)
|
|
||||||
)
|
|
||||||
|
|
||||||
# call contact function
|
|
||||||
res = jax.vmap(fn, in_axes=in_axes)(g1, g2)
|
|
||||||
dist, pos, frame = jax.tree_map(jp.concatenate, res)
|
|
||||||
|
|
||||||
# repeat params by the number of contacts per geom pair
|
|
||||||
geom1, geom2, params = jax.tree_map(
|
|
||||||
lambda x: jp.repeat(x, fn.ncon, axis=0), # pytype: disable=attribute-error
|
|
||||||
(g1.geom_id, g2.geom_id, params),
|
|
||||||
)
|
|
||||||
|
|
||||||
con = Contact(
|
|
||||||
dist=dist,
|
|
||||||
pos=pos,
|
|
||||||
frame=frame,
|
|
||||||
includemargin=params.margin - params.gap,
|
|
||||||
friction=params.friction,
|
|
||||||
solref=params.solref,
|
|
||||||
solreffriction=params.solreffriction,
|
|
||||||
solimp=params.solimp,
|
|
||||||
geom1=geom1,
|
|
||||||
geom2=geom2,
|
|
||||||
)
|
|
||||||
return con
|
|
||||||
|
|
||||||
|
|
||||||
def collision_candidates(m: Union[Model, mujoco.MjModel]) -> CandidateSet:
|
|
||||||
"""Returns candidates for collision checking."""
|
|
||||||
candidate_set = {}
|
|
||||||
|
|
||||||
for ipair in range(m.npair):
|
|
||||||
g1, g2 = m.pair_geom1[ipair], m.pair_geom2[ipair]
|
|
||||||
_add_candidate(candidate_set, m, g1, g2, ipair)
|
|
||||||
|
|
||||||
body_pairs = []
|
|
||||||
exclude_signature = set(m.exclude_signature)
|
exclude_signature = set(m.exclude_signature)
|
||||||
geom_con = m.geom_contype | m.geom_conaffinity
|
geom_con = m.geom_contype | m.geom_conaffinity
|
||||||
|
filterparent = not (m.opt.disableflags & DisableBit.FILTERPARENT)
|
||||||
b_start = m.body_geomadr
|
b_start = m.body_geomadr
|
||||||
b_end = b_start + m.body_geomnum
|
b_end = b_start + m.body_geomnum
|
||||||
|
|
||||||
for b1 in range(m.nbody):
|
for b1 in range(m.nbody):
|
||||||
if not geom_con[b_start[b1]:b_end[b1]].any():
|
if not geom_con[b_start[b1]:b_end[b1]].any():
|
||||||
continue
|
continue
|
||||||
|
w1 = m.body_weldid[b1]
|
||||||
|
w1_p = m.body_weldid[m.body_parentid[w1]]
|
||||||
|
|
||||||
for b2 in range(b1, m.nbody):
|
for b2 in range(b1, m.nbody):
|
||||||
if not geom_con[b_start[b2]:b_end[b2]].any():
|
if not geom_con[b_start[b2]:b_end[b2]].any():
|
||||||
continue
|
continue
|
||||||
signature = (b1 << 16) + (b2)
|
signature = (b1 << 16) + (b2)
|
||||||
if signature in exclude_signature:
|
if signature in exclude_signature:
|
||||||
continue
|
continue
|
||||||
if _body_pair_filter(m, b1, b2):
|
w2 = m.body_weldid[b2]
|
||||||
|
# ignore self-collisions
|
||||||
|
if w1 == w2:
|
||||||
continue
|
continue
|
||||||
body_pairs.append((b1, b2))
|
w2_p = m.body_weldid[m.body_parentid[w2]]
|
||||||
|
# ignore parent-child collisions
|
||||||
|
if filterparent and w1 != 0 and w2 != 0 and (w1 == w2_p or w2 == w1_p):
|
||||||
|
continue
|
||||||
|
g1_range = [g for g in range(b_start[b1], b_end[b1]) if geom_con[g]]
|
||||||
|
g2_range = [g for g in range(b_start[b2], b_end[b2]) if geom_con[g]]
|
||||||
|
|
||||||
for b1, b2 in body_pairs:
|
for g1, g2 in itertools.product(g1_range, g2_range):
|
||||||
for g1 in range(b_start[b1], b_end[b1]):
|
t1, t2 = m.geom_type[g1], m.geom_type[g2]
|
||||||
if not geom_con[g1]:
|
# order pairs by geom_type for correct function mapping
|
||||||
|
if t1 > t2:
|
||||||
|
g1, g2, t1, t2 = g2, g1, t2, t1
|
||||||
|
# ignore plane<>plane and plane<>hfield
|
||||||
|
if (t1, t2) == (GeomType.PLANE, GeomType.PLANE):
|
||||||
continue
|
continue
|
||||||
for g2 in range(b_start[b2], b_end[b2]):
|
if (t1, t2) == (GeomType.PLANE, GeomType.HFIELD):
|
||||||
if not geom_con[g2]:
|
|
||||||
continue
|
continue
|
||||||
|
# geoms must match contype and conaffinity on some bit
|
||||||
mask = m.geom_contype[g1] & m.geom_conaffinity[g2]
|
mask = m.geom_contype[g1] & m.geom_conaffinity[g2]
|
||||||
mask |= m.geom_contype[g2] & m.geom_conaffinity[g1]
|
mask |= m.geom_contype[g2] & m.geom_conaffinity[g1]
|
||||||
if mask != 0:
|
if not mask:
|
||||||
_add_candidate(candidate_set, m, g1, g2)
|
|
||||||
|
|
||||||
return candidate_set
|
|
||||||
|
|
||||||
|
|
||||||
def ncon(m: Union[Model, mujoco.MjModel]) -> int:
|
|
||||||
"""Returns the number of contacts computed in MJX given a model."""
|
|
||||||
if m.opt.disableflags & DisableBit.CONTACT:
|
|
||||||
return 0
|
|
||||||
|
|
||||||
candidates = collision_candidates(m)
|
|
||||||
max_count = int(support.get_custom_numeric(m, 'max_contact_points'))
|
|
||||||
max_pairs = int(support.get_custom_numeric(m, 'max_geom_pairs'))
|
|
||||||
|
|
||||||
count = 0
|
|
||||||
for k, v in candidates.items():
|
|
||||||
fn = get_collision_fn(k[0:2])
|
|
||||||
if fn is None:
|
|
||||||
continue
|
continue
|
||||||
run_broadphase = _broadphase_enabled((k[0], k[1]), len(v), max_pairs)
|
|
||||||
n_pair = max_pairs if run_broadphase else len(v)
|
|
||||||
count += n_pair * fn.ncon # pytype: disable=attribute-error
|
|
||||||
|
|
||||||
return min(max_count, count) if max_count > -1 else count
|
if (g1, g2) not in pairs:
|
||||||
|
pairs.add((g1, g2))
|
||||||
|
yield g1, g2, -1
|
||||||
|
|
||||||
|
|
||||||
|
def _geom_groups(
|
||||||
|
m: Union[Model, mujoco.MjModel],
|
||||||
|
) -> Dict[FunctionKey, List[Tuple[int, int, int]]]:
|
||||||
|
"""Returns geom pairs to check for collision grouped by collision function.
|
||||||
|
|
||||||
|
The grouping consists of:
|
||||||
|
- The collision function to run, which is determined by geom types
|
||||||
|
- For mesh geoms, convex functions are run for each distinct mesh in the
|
||||||
|
model, because the convex functions expect static mesh size. If a sphere
|
||||||
|
collides with a cube and a tetrahedron, sphere_convex is called twice.
|
||||||
|
- The condim of the collision. This ensures that the size of the resulting
|
||||||
|
constraint jacobian is determined at compile time.
|
||||||
|
|
||||||
|
Args:
|
||||||
|
m: a MuJoCo or MJX model
|
||||||
|
|
||||||
|
Returns:
|
||||||
|
a dict with grouping key and values geom1, geom2, pair index
|
||||||
|
"""
|
||||||
|
groups = {}
|
||||||
|
|
||||||
|
for g1, g2, ip in geom_pairs(m):
|
||||||
|
types = m.geom_type[g1], m.geom_type[g2]
|
||||||
|
data_ids = m.geom_dataid[g1], m.geom_dataid[g2]
|
||||||
|
if ip > -1:
|
||||||
|
condim = m.pair_dim[ip]
|
||||||
|
elif m.geom_priority[g1] > m.geom_priority[g2]:
|
||||||
|
condim = m.geom_condim[g1]
|
||||||
|
elif m.geom_priority[g1] < m.geom_priority[g2]:
|
||||||
|
condim = m.geom_condim[g2]
|
||||||
|
else:
|
||||||
|
condim = max(m.geom_condim[g1], m.geom_condim[g2])
|
||||||
|
|
||||||
|
key = FunctionKey(types, data_ids, condim)
|
||||||
|
groups.setdefault(key, []).append((g1, g2, ip))
|
||||||
|
|
||||||
|
return groups
|
||||||
|
|
||||||
|
|
||||||
|
def _contact_groups(m: Model, d: Data) -> Dict[FunctionKey, Contact]:
|
||||||
|
"""Returns contact groups to check for collisions.
|
||||||
|
|
||||||
|
Contacts are grouped the same way as _geom_groups. Only one contact is
|
||||||
|
emitted per geom pair, even if the collision function emits multiple contacts.
|
||||||
|
|
||||||
|
Args:
|
||||||
|
m: MJX model
|
||||||
|
d: MJX data
|
||||||
|
|
||||||
|
Returns:
|
||||||
|
a dict where the key is the grouping and value is a Contact
|
||||||
|
"""
|
||||||
|
groups = {}
|
||||||
|
eps = mujoco.mjMINVAL
|
||||||
|
|
||||||
|
for key, geom_ids in _geom_groups(m).items():
|
||||||
|
geom = np.array(geom_ids)
|
||||||
|
geom1, geom2, ip = geom.T
|
||||||
|
geom1, geom2, ip = geom1[ip == -1], geom2[ip == -1], ip[ip != -1]
|
||||||
|
params = []
|
||||||
|
|
||||||
|
if ip.size > 0:
|
||||||
|
# pair contacts get their params from m.pair_* fields
|
||||||
|
params.append((
|
||||||
|
m.pair_margin[ip] - m.pair_gap[ip],
|
||||||
|
jp.clip(m.pair_friction[ip], a_min=eps),
|
||||||
|
m.pair_solref[ip],
|
||||||
|
m.pair_solreffriction[ip],
|
||||||
|
m.pair_solimp[ip]
|
||||||
|
))
|
||||||
|
if geom1.size > 0 and geom2.size > 0:
|
||||||
|
# other contacts get their params from geom fields
|
||||||
|
margin = jp.maximum(m.geom_margin[geom1], m.geom_margin[geom2])
|
||||||
|
gap = jp.maximum(m.geom_gap[geom1], m.geom_gap[geom2])
|
||||||
|
solmix1, solmix2 = m.geom_solmix[geom1], m.geom_solmix[geom2]
|
||||||
|
mix = solmix1 / (solmix1 + solmix2)
|
||||||
|
mix = jp.where((solmix1 < eps) & (solmix2 < eps), 0.5, mix)
|
||||||
|
mix = jp.where((solmix1 < eps) & (solmix2 >= eps), 0.0, mix)
|
||||||
|
mix = jp.where((solmix1 >= eps) & (solmix2 < eps), 1.0, mix)
|
||||||
|
mix = mix[:, None] # for correct broadcasting
|
||||||
|
# friction: max
|
||||||
|
friction = jp.maximum(m.geom_friction[geom1], m.geom_friction[geom2])
|
||||||
|
solref1, solref2 = m.geom_solref[geom1], m.geom_solref[geom2]
|
||||||
|
# reference standard: mix
|
||||||
|
solref_standard = mix * solref1 + (1 - mix) * solref2
|
||||||
|
# reference direct: min
|
||||||
|
solref_direct = jp.minimum(solref1, solref2)
|
||||||
|
is_standard = (solref1[:, [0, 0]] > 0) & (solref2[:, [0, 0]] > 0)
|
||||||
|
solref = jp.where(is_standard, solref_standard, solref_direct)
|
||||||
|
solreffriction = jp.zeros(geom1.shape + (mujoco.mjNREF,))
|
||||||
|
# impedance: mix
|
||||||
|
solimp = mix * m.geom_solimp[geom1] + (1 - mix) * m.geom_solimp[geom2]
|
||||||
|
|
||||||
|
pri = m.geom_priority[geom1] != m.geom_priority[geom2]
|
||||||
|
if pri.any():
|
||||||
|
# use priority geom when specified instead of mixing
|
||||||
|
gp1, gp2 = m.geom_priority[geom1], m.geom_priority[geom2]
|
||||||
|
gp = np.where(gp1 > gp2, geom1, geom2)[pri]
|
||||||
|
friction = friction.at[pri].set(m.geom_friction[gp])
|
||||||
|
solref = solref.at[pri].set(m.geom_solref[gp])
|
||||||
|
solimp = solimp.at[pri].set(m.geom_solimp[gp])
|
||||||
|
|
||||||
|
# unpack 5d friction:
|
||||||
|
friction = friction[:, [0, 0, 1, 2, 2]]
|
||||||
|
params.append((margin - gap, friction, solref, solreffriction, solimp))
|
||||||
|
|
||||||
|
params = map(jp.concatenate, zip(*params))
|
||||||
|
includemargin, friction, solref, solreffriction, solimp = params
|
||||||
|
|
||||||
|
groups[key] = Contact(
|
||||||
|
# dist, pos, frame get filled in by collision functions:
|
||||||
|
dist=None,
|
||||||
|
pos=None,
|
||||||
|
frame=None,
|
||||||
|
includemargin=includemargin,
|
||||||
|
friction=friction,
|
||||||
|
solref=solref,
|
||||||
|
solreffriction=solreffriction,
|
||||||
|
solimp=solimp,
|
||||||
|
dim=d.contact.dim,
|
||||||
|
geom1=jp.array(geom[:, 0]),
|
||||||
|
geom2=jp.array(geom[:, 1]),
|
||||||
|
geom=jp.array(geom[:, :2]),
|
||||||
|
efc_address=d.contact.efc_address,
|
||||||
|
)
|
||||||
|
|
||||||
|
return groups
|
||||||
|
|
||||||
|
|
||||||
|
def _numeric(m: Union[Model, mujoco.MjModel], name: str) -> int:
|
||||||
|
id_ = support.name2id(m, mujoco.mjtObj.mjOBJ_NUMERIC, name)
|
||||||
|
return int(m.numeric_data[id_]) if id_ >= 0 else -1
|
||||||
|
|
||||||
|
|
||||||
|
def make_condim(m: Union[Model, mujoco.MjModel]) -> np.ndarray:
|
||||||
|
"""Returns the dims of the contacts for a Model."""
|
||||||
|
if m.opt.disableflags & DisableBit.CONTACT:
|
||||||
|
return np.empty(0, dtype=int)
|
||||||
|
|
||||||
|
group_counts = {k: len(v) for k, v in _geom_groups(m).items()}
|
||||||
|
|
||||||
|
# max_geom_pairs limits the number of pairs we process in a collision function
|
||||||
|
# by first running a primitive broad phase culling on the pairs
|
||||||
|
max_geom_pairs = _numeric(m, 'max_geom_pairs')
|
||||||
|
|
||||||
|
if max_geom_pairs > -1:
|
||||||
|
for k in group_counts:
|
||||||
|
if set(k.types) & _GEOM_NO_BROADPHASE:
|
||||||
|
continue
|
||||||
|
group_counts[k] = min(group_counts[k], max_geom_pairs)
|
||||||
|
|
||||||
|
# max_contact_points limits the number of contacts emitted by selecting the
|
||||||
|
# contacts with the most penetration after calling collision functions
|
||||||
|
max_contact_points = _numeric(m, 'max_contact_points')
|
||||||
|
|
||||||
|
condim_counts = {}
|
||||||
|
for k, v in group_counts.items():
|
||||||
|
func = _COLLISION_FUNC[k.types]
|
||||||
|
num_contacts = condim_counts.get(k.condim, 0) + func.ncon * v # pytype: disable=attribute-error
|
||||||
|
if max_contact_points > -1:
|
||||||
|
num_contacts = min(max_contact_points, num_contacts)
|
||||||
|
condim_counts[k.condim] = num_contacts
|
||||||
|
|
||||||
|
dims = sum(([c] * condim_counts[c] for c in sorted(condim_counts)), [])
|
||||||
|
|
||||||
|
return np.array(dims)
|
||||||
|
|
||||||
|
|
||||||
def collision(m: Model, d: Data) -> Data:
|
def collision(m: Model, d: Data) -> Data:
|
||||||
"""Collides geometries."""
|
"""Collides geometries."""
|
||||||
if ncon(m) == 0:
|
if d.ncon == 0:
|
||||||
return d.replace(contact=Contact.zero())
|
return d
|
||||||
|
|
||||||
candidate_set = collision_candidates(m)
|
groups = _contact_groups(m, d)
|
||||||
|
max_geom_pairs = _numeric(m, 'max_geom_pairs')
|
||||||
|
max_contact_points = _numeric(m, 'max_contact_points')
|
||||||
|
|
||||||
contacts = []
|
# run collision functions on groups
|
||||||
for key, candidates in candidate_set.items():
|
for key, contact in groups.items():
|
||||||
geom_types = key[0:2]
|
# determine which contacts we'll use for collision testing by running a
|
||||||
contacts.append(_collide_geoms(m, d, geom_types, candidates))
|
# broad phase cull if requested
|
||||||
|
if (
|
||||||
|
max_geom_pairs > -1
|
||||||
|
and contact.geom.shape[0] > max_geom_pairs
|
||||||
|
and not set(key.types) & _GEOM_NO_BROADPHASE
|
||||||
|
):
|
||||||
|
pos1, pos2 = d.geom_xpos[contact.geom.T]
|
||||||
|
size1, size2 = m.geom_rbound[contact.geom.T]
|
||||||
|
dist = jax.vmap(jp.linalg.norm)(pos2 - pos1) - (size1 + size2)
|
||||||
|
_, idx = jax.lax.top_k(-dist, k=max_geom_pairs)
|
||||||
|
contact = jax.tree_util.tree_map(lambda x, idx=idx: x[idx], contact)
|
||||||
|
|
||||||
if not contacts:
|
# run the collision function specified by the grouping key
|
||||||
raise RuntimeError('No contacts found.')
|
func = _COLLISION_FUNC[key.types]
|
||||||
|
dist, pos, frame = func(m, d, key, contact.geom)
|
||||||
|
ncon = func.ncon # pytype: disable=attribute-error
|
||||||
|
if ncon > 1:
|
||||||
|
# repeat contacts to match the number of collisions returned
|
||||||
|
repeat_fn = lambda x, r=ncon: jp.repeat(x, r, axis=0)
|
||||||
|
contact = jax.tree_util.tree_map(repeat_fn, contact)
|
||||||
|
groups[key] = contact.replace(dist=dist, pos=pos, frame=frame)
|
||||||
|
|
||||||
contact = jax.tree_map(lambda *x: jp.concatenate(x), *contacts)
|
# collapse contacts together, ensuring they are grouped by condim
|
||||||
|
condim_groups = {}
|
||||||
|
for key, contact in groups.items():
|
||||||
|
condim_groups.setdefault(key.condim, []).append(contact)
|
||||||
|
|
||||||
max_contact_points = int(support.get_custom_numeric(m, 'max_contact_points'))
|
# limit the number of contacts per condim group if requested
|
||||||
if max_contact_points > -1 and contact.dist.shape[0] > max_contact_points:
|
if max_contact_points > -1:
|
||||||
# get top-k contacts
|
for key, contacts in condim_groups.items():
|
||||||
|
contact = jax.tree_util.tree_map(lambda *x: jp.concatenate(x), *contacts)
|
||||||
|
if contact.geom.shape[0] > max_contact_points:
|
||||||
_, idx = jax.lax.top_k(-contact.dist, k=max_contact_points)
|
_, idx = jax.lax.top_k(-contact.dist, k=max_contact_points)
|
||||||
contact = jax.tree_map(lambda x, idx=idx: jp.take(x, idx, axis=0), contact)
|
contact = jax.tree_util.tree_map(lambda x, idx=idx: x[idx], contact)
|
||||||
|
condim_groups[key] = [contact]
|
||||||
|
|
||||||
|
contacts = sum([condim_groups[k] for k in sorted(condim_groups)], [])
|
||||||
|
contact = jax.tree_util.tree_map(lambda *x: jp.concatenate(x), *contacts)
|
||||||
|
|
||||||
return d.replace(contact=contact)
|
return d.replace(contact=contact)
|
||||||
|
|||||||
@@ -220,6 +220,73 @@ class EllipsoidCollisionTest(parameterized.TestCase):
|
|||||||
_assert_attr_eq(
|
_assert_attr_eq(
|
||||||
dx.contact, d.contact, field.name, 'ellipsoid-plane', 1e-5)
|
dx.contact, d.contact, field.name, 'ellipsoid-plane', 1e-5)
|
||||||
|
|
||||||
|
_ELLIPSOID_ELLIPSOID = """
|
||||||
|
<mujoco>
|
||||||
|
<worldbody>
|
||||||
|
<body>
|
||||||
|
<geom size=".15 .03 .05" type="ellipsoid"/>
|
||||||
|
</body>
|
||||||
|
<body pos="0 0 0.09">
|
||||||
|
<freejoint/>
|
||||||
|
<geom size=".15 .03 .05" type="ellipsoid"/>
|
||||||
|
</body>
|
||||||
|
</worldbody>
|
||||||
|
</mujoco>
|
||||||
|
"""
|
||||||
|
|
||||||
|
def test_ellipsoid_ellipsoid(self):
|
||||||
|
"""Tests ellipsoid ellipsoid contact."""
|
||||||
|
d, dx = _collide(self._ELLIPSOID_ELLIPSOID)
|
||||||
|
self.assertLess(dx.contact.dist[0], 0)
|
||||||
|
for field in dataclasses.fields(Contact):
|
||||||
|
_assert_attr_eq(
|
||||||
|
dx.contact, d.contact, field.name, 'ellipsoid-ellipsoid', 1e-5)
|
||||||
|
|
||||||
|
_ELLIPSOID_CAPSULE = """
|
||||||
|
<mujoco>
|
||||||
|
<worldbody>
|
||||||
|
<body>
|
||||||
|
<geom size=".15 .03 .05" type="ellipsoid"/>
|
||||||
|
</body>
|
||||||
|
<body pos="0 0 0.0999">
|
||||||
|
<freejoint/>
|
||||||
|
<geom size=".05" fromto="-.1 0 0 .1 0 0" type="capsule"/>
|
||||||
|
</body>
|
||||||
|
</worldbody>
|
||||||
|
</mujoco>
|
||||||
|
"""
|
||||||
|
|
||||||
|
def test_capsule_ellipsoid(self):
|
||||||
|
"""Tests ellipsoid capsule contact."""
|
||||||
|
d, dx = _collide(self._ELLIPSOID_CAPSULE)
|
||||||
|
self.assertLess(dx.contact.dist[0], 0)
|
||||||
|
for field in dataclasses.fields(Contact):
|
||||||
|
_assert_attr_eq(
|
||||||
|
dx.contact, d.contact, field.name, 'ellipsoid-capsule', 1e-4)
|
||||||
|
|
||||||
|
_ELLIPSOID_CYLINDER = """
|
||||||
|
<mujoco>
|
||||||
|
<worldbody>
|
||||||
|
<body>
|
||||||
|
<geom size=".15 .05" type="cylinder"/>
|
||||||
|
</body>
|
||||||
|
<body pos="0 0 0.09">
|
||||||
|
<freejoint/>
|
||||||
|
<geom size=".15 .03 .05" type="ellipsoid"/>
|
||||||
|
</body>
|
||||||
|
</worldbody>
|
||||||
|
</mujoco>
|
||||||
|
"""
|
||||||
|
|
||||||
|
def test_ellipsoid_cylinder(self):
|
||||||
|
"""Tests ellipsoid cylinder contact."""
|
||||||
|
d, dx = _collide(self._ELLIPSOID_CYLINDER)
|
||||||
|
d.contact.pos[0][2] = 0.04 # MJX finds the deepest point on the surface
|
||||||
|
self.assertLess(dx.contact.dist[0], 0)
|
||||||
|
for field in dataclasses.fields(Contact):
|
||||||
|
_assert_attr_eq(
|
||||||
|
dx.contact, d.contact, field.name, 'ellipsoid-cylinder', 1e-4)
|
||||||
|
|
||||||
|
|
||||||
class CapsuleCollisionTest(parameterized.TestCase):
|
class CapsuleCollisionTest(parameterized.TestCase):
|
||||||
_CAP_PLANE = """
|
_CAP_PLANE = """
|
||||||
@@ -358,7 +425,8 @@ class CapsuleCollisionTest(parameterized.TestCase):
|
|||||||
self.assertEqual(c.pos.shape[0], 2)
|
self.assertEqual(c.pos.shape[0], 2)
|
||||||
self.assertGreater(c.dist[1], 0)
|
self.assertGreater(c.dist[1], 0)
|
||||||
# extract the contact point with penetration
|
# extract the contact point with penetration
|
||||||
c = jax.tree_map(lambda x: jp.take(x, 0, axis=0)[None], dx.contact)
|
c = jax.tree_util.tree_map(lambda x: x[:1], dx.contact)
|
||||||
|
c = c.replace(dim=c.dim[:1], efc_address=c.efc_address[:1])
|
||||||
for field in dataclasses.fields(Contact):
|
for field in dataclasses.fields(Contact):
|
||||||
_assert_attr_eq(c, d.contact, field.name, 'capsule_convex_edge', 1e-4)
|
_assert_attr_eq(c, d.contact, field.name, 'capsule_convex_edge', 1e-4)
|
||||||
|
|
||||||
@@ -393,7 +461,8 @@ class CapsuleCollisionTest(parameterized.TestCase):
|
|||||||
self.assertEqual(c.pos.shape[0], 2)
|
self.assertEqual(c.pos.shape[0], 2)
|
||||||
self.assertGreater(c.dist[1], 0)
|
self.assertGreater(c.dist[1], 0)
|
||||||
# extract the contact point with penetration
|
# extract the contact point with penetration
|
||||||
c = jax.tree_map(lambda x: jp.take(x, 0, axis=0)[None], dx.contact)
|
c = jax.tree_util.tree_map(lambda x: x[:1], dx.contact)
|
||||||
|
c = c.replace(dim=c.dim[:1], efc_address=c.efc_address[:1])
|
||||||
for field in dataclasses.fields(Contact):
|
for field in dataclasses.fields(Contact):
|
||||||
_assert_attr_eq(c, d.contact, field.name, 'edge_shallow_tip1', 1e-4)
|
_assert_attr_eq(c, d.contact, field.name, 'edge_shallow_tip1', 1e-4)
|
||||||
np.testing.assert_array_almost_equal(
|
np.testing.assert_array_almost_equal(
|
||||||
@@ -413,7 +482,8 @@ class CapsuleCollisionTest(parameterized.TestCase):
|
|||||||
self.assertEqual(c.pos.shape[0], 2)
|
self.assertEqual(c.pos.shape[0], 2)
|
||||||
self.assertGreater(c.dist[1], 0)
|
self.assertGreater(c.dist[1], 0)
|
||||||
# extract the contact point with penetration
|
# extract the contact point with penetration
|
||||||
c = jax.tree_map(lambda x: jp.take(x, 0, axis=0)[None], dx.contact)
|
c = jax.tree_util.tree_map(lambda x: x[:1], dx.contact)
|
||||||
|
c = c.replace(dim=c.dim[:1], efc_address=c.efc_address[:1])
|
||||||
for field in dataclasses.fields(Contact):
|
for field in dataclasses.fields(Contact):
|
||||||
_assert_attr_eq(c, d.contact, field.name, 'edge_shallow_tip2', 1e-4)
|
_assert_attr_eq(c, d.contact, field.name, 'edge_shallow_tip2', 1e-4)
|
||||||
np.testing.assert_array_almost_equal(
|
np.testing.assert_array_almost_equal(
|
||||||
@@ -421,6 +491,51 @@ class CapsuleCollisionTest(parameterized.TestCase):
|
|||||||
)
|
)
|
||||||
|
|
||||||
|
|
||||||
|
class CylinderTest(absltest.TestCase):
|
||||||
|
"""Tests the cylinder contact functions."""
|
||||||
|
|
||||||
|
_CYLINDER_PLANE = """
|
||||||
|
<mujoco>
|
||||||
|
<worldbody>
|
||||||
|
<geom size="40 40 40" type="plane"/>
|
||||||
|
<body pos="0 0 0.04">
|
||||||
|
<joint type="free"/>
|
||||||
|
<geom fromto="-0.1 0 0 0.1 0 0" size="0.05" type="cylinder"/>
|
||||||
|
</body>
|
||||||
|
</worldbody>
|
||||||
|
</mujoco>
|
||||||
|
"""
|
||||||
|
|
||||||
|
def test_cylinder_plane(self):
|
||||||
|
d, dx = _collide(self._CYLINDER_PLANE)
|
||||||
|
|
||||||
|
# cylinder is lying flat
|
||||||
|
np.testing.assert_array_less(dx.contact.dist[:2], 0)
|
||||||
|
np.testing.assert_array_less(-dx.contact.dist[2:], 0)
|
||||||
|
|
||||||
|
# sort position for comparison
|
||||||
|
idx = np.lexsort((dx.contact.pos[:, 0], dx.contact.pos[:, 1]))
|
||||||
|
dx = dx.tree_replace({'contact.pos': dx.contact.pos[idx]})
|
||||||
|
idx = np.lexsort((d.contact.pos[:, 0], d.contact.pos[:, 1]))
|
||||||
|
d.contact.pos[:] = d.contact.pos[idx]
|
||||||
|
|
||||||
|
# extract the contact points with penetration
|
||||||
|
c = jax.tree_util.tree_map(lambda x: x[:2], dx.contact)
|
||||||
|
c = c.replace(dim=c.dim[:2], efc_address=c.efc_address[:2])
|
||||||
|
for field in dataclasses.fields(Contact):
|
||||||
|
_assert_attr_eq(c, d.contact, field.name, 'cylinder_plane', 1e-5)
|
||||||
|
|
||||||
|
# cylinder is vertical
|
||||||
|
xml = self._CYLINDER_PLANE.replace(
|
||||||
|
'<geom fromto="-0.1 0 0 0.1 0 0"', '<geom fromto="0 0 -0.1 0 0 0.1"')
|
||||||
|
xml = xml.replace('pos="0 0 0.04"', 'pos="0 0 0.095"')
|
||||||
|
d, dx = _collide(xml)
|
||||||
|
|
||||||
|
np.testing.assert_array_less(dx.contact.dist, 0)
|
||||||
|
for field in dataclasses.fields(Contact):
|
||||||
|
_assert_attr_eq(dx.contact, d.contact, field.name, 'cylinder_plane', 1e-5)
|
||||||
|
|
||||||
|
|
||||||
class ConvexTest(absltest.TestCase):
|
class ConvexTest(absltest.TestCase):
|
||||||
"""Tests the convex contact functions."""
|
"""Tests the convex contact functions."""
|
||||||
|
|
||||||
@@ -443,7 +558,8 @@ class ConvexTest(absltest.TestCase):
|
|||||||
np.testing.assert_array_less(dx.contact.dist[:2], 0)
|
np.testing.assert_array_less(dx.contact.dist[:2], 0)
|
||||||
np.testing.assert_array_less(-dx.contact.dist[2:], 0)
|
np.testing.assert_array_less(-dx.contact.dist[2:], 0)
|
||||||
# extract the contact points with penetration
|
# extract the contact points with penetration
|
||||||
c = jax.tree_map(lambda x: jp.take(x, jp.array([0, 1]), axis=0), dx.contact)
|
c = jax.tree_util.tree_map(lambda x: jp.take(x, jp.array([0, 1]), axis=0), dx.contact)
|
||||||
|
c = c.replace(dim=c.dim[[0, 1]], efc_address=c.efc_address[[0, 1]])
|
||||||
for field in dataclasses.fields(Contact):
|
for field in dataclasses.fields(Contact):
|
||||||
_assert_attr_eq(c, d.contact, field.name, 'box_plane', 1e-5)
|
_assert_attr_eq(c, d.contact, field.name, 'box_plane', 1e-5)
|
||||||
|
|
||||||
@@ -527,7 +643,8 @@ class ConvexTest(absltest.TestCase):
|
|||||||
np.testing.assert_array_less(dx.contact.dist[:1], 0)
|
np.testing.assert_array_less(dx.contact.dist[:1], 0)
|
||||||
np.testing.assert_array_less(-dx.contact.dist[1:], 0)
|
np.testing.assert_array_less(-dx.contact.dist[1:], 0)
|
||||||
# extract the contact point with penetration
|
# extract the contact point with penetration
|
||||||
c = jax.tree_map(lambda x: jp.take(x, 0, axis=0)[None], dx.contact)
|
c = jax.tree_util.tree_map(lambda x: x[:1], dx.contact)
|
||||||
|
c = c.replace(dim=c.dim[:1], efc_address=c.efc_address[:1])
|
||||||
for field in dataclasses.fields(Contact):
|
for field in dataclasses.fields(Contact):
|
||||||
_assert_attr_eq(c, d.contact, field.name, 'box_box_edge', 1e-2)
|
_assert_attr_eq(c, d.contact, field.name, 'box_box_edge', 1e-2)
|
||||||
|
|
||||||
@@ -675,28 +792,29 @@ class BodyPairFilterTest(absltest.TestCase):
|
|||||||
self.assertEqual(dx.contact.pos.shape[0], 1)
|
self.assertEqual(dx.contact.pos.shape[0], 1)
|
||||||
|
|
||||||
|
|
||||||
class NconTest(parameterized.TestCase):
|
class DimTest(parameterized.TestCase):
|
||||||
"""Tests ncon."""
|
"""Tests contact dim."""
|
||||||
|
|
||||||
def test_ncon(self):
|
def test_ncon(self):
|
||||||
m = test_util.load_test_file('constraints.xml')
|
m = test_util.load_test_file('constraints.xml')
|
||||||
ncon = collision_driver.ncon(m)
|
dim = collision_driver.make_condim(m)
|
||||||
self.assertEqual(ncon, 16)
|
expected = [1] * 4 + [3] * 20 + [4] * 4 + [6] * 4
|
||||||
|
np.testing.assert_array_equal(dim, np.array(expected))
|
||||||
|
|
||||||
def test_disable_contact(self):
|
def test_disable_contact(self):
|
||||||
m = test_util.load_test_file('constraints.xml')
|
m = test_util.load_test_file('constraints.xml')
|
||||||
m.opt.disableflags |= DisableBit.CONTACT
|
m.opt.disableflags |= DisableBit.CONTACT
|
||||||
ncon = collision_driver.ncon(m)
|
dim = collision_driver.make_condim(m)
|
||||||
self.assertEqual(ncon, 0)
|
self.assertEqual(dim.size, 0)
|
||||||
|
|
||||||
def test_ncon_meshes(self):
|
def test_ncon_meshes(self):
|
||||||
m = test_util.load_test_file('shadow_hand/scene_right.xml')
|
m = test_util.load_test_file('shadow_hand/scene_right.xml')
|
||||||
|
|
||||||
ncon = collision_driver.ncon(m)
|
ncon = collision_driver.make_condim(m).size
|
||||||
self.assertEqual(ncon, 15)
|
self.assertEqual(ncon, 15)
|
||||||
|
|
||||||
mx = mjx.put_model(m)
|
mx = mjx.put_model(m)
|
||||||
ncon = collision_driver.ncon(mx)
|
ncon = collision_driver.make_condim(mx).size
|
||||||
self.assertEqual(ncon, 15)
|
self.assertEqual(ncon, 15)
|
||||||
|
|
||||||
# get rid of max_contact_points, test only max_geom_pairs
|
# get rid of max_contact_points, test only max_geom_pairs
|
||||||
@@ -707,11 +825,11 @@ class NconTest(parameterized.TestCase):
|
|||||||
if name_ == 'max_contact_points':
|
if name_ == 'max_contact_points':
|
||||||
m.numeric_data[m.numeric_adr[i]] = -1
|
m.numeric_data[m.numeric_adr[i]] = -1
|
||||||
|
|
||||||
ncon = collision_driver.ncon(m)
|
ncon = collision_driver.make_condim(m).size
|
||||||
self.assertEqual(ncon, 98)
|
self.assertEqual(ncon, 98)
|
||||||
|
|
||||||
mx = mjx.put_model(m)
|
mx = mjx.put_model(m)
|
||||||
ncon = collision_driver.ncon(mx)
|
ncon = collision_driver.make_condim(mx).size
|
||||||
self.assertEqual(ncon, 98)
|
self.assertEqual(ncon, 98)
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
@@ -20,34 +20,53 @@ import jax
|
|||||||
from jax import numpy as jp
|
from jax import numpy as jp
|
||||||
from mujoco.mjx._src import math
|
from mujoco.mjx._src import math
|
||||||
# pylint: disable=g-importing-member
|
# pylint: disable=g-importing-member
|
||||||
from mujoco.mjx._src.collision_base import Contact
|
from mujoco.mjx._src.collision_types import Collision
|
||||||
from mujoco.mjx._src.collision_base import GeomInfo
|
from mujoco.mjx._src.collision_types import GeomInfo
|
||||||
|
from mujoco.mjx._src.types import Data
|
||||||
|
from mujoco.mjx._src.types import Model
|
||||||
# pylint: enable=g-importing-member
|
# pylint: enable=g-importing-member
|
||||||
|
|
||||||
|
|
||||||
|
def collider(ncon: int):
|
||||||
|
"""Wraps collision functions for use by collision_driver."""
|
||||||
|
def wrapper(func):
|
||||||
|
def collide(m: Model, d: Data, _, geom: jax.Array) -> Collision:
|
||||||
|
g1, g2 = geom.T
|
||||||
|
info1 = GeomInfo(d.geom_xpos[g1], d.geom_xmat[g1], m.geom_size[g1])
|
||||||
|
info2 = GeomInfo(d.geom_xpos[g2], d.geom_xmat[g2], m.geom_size[g2])
|
||||||
|
dist, pos, frame = jax.vmap(func)(info1, info2)
|
||||||
|
if ncon > 1:
|
||||||
|
return jax.tree_util.tree_map(jp.concatenate, (dist, pos, frame))
|
||||||
|
return dist, pos, frame
|
||||||
|
|
||||||
|
collide.ncon = ncon
|
||||||
|
return collide
|
||||||
|
|
||||||
|
return wrapper
|
||||||
|
|
||||||
|
|
||||||
def _plane_sphere(
|
def _plane_sphere(
|
||||||
plane_normal: jax.Array,
|
plane_normal: jax.Array,
|
||||||
plane_pos: jax.Array,
|
plane_pos: jax.Array,
|
||||||
sphere_pos: jax.Array,
|
sphere_pos: jax.Array,
|
||||||
radius: jax.Array,
|
sphere_radius: jax.Array,
|
||||||
) -> Tuple[jax.Array, jax.Array]:
|
) -> Tuple[jax.Array, jax.Array]:
|
||||||
"""Returns the penetration and contact point between a plane and sphere."""
|
"""Returns the distance and contact point between a plane and sphere."""
|
||||||
cdist = jp.dot(sphere_pos - plane_pos, plane_normal)
|
dist = jp.dot(sphere_pos - plane_pos, plane_normal) - sphere_radius
|
||||||
dist = cdist - radius
|
pos = sphere_pos - plane_normal * (sphere_radius + 0.5 * dist)
|
||||||
pos = sphere_pos - plane_normal * (radius + 0.5 * dist)
|
|
||||||
return dist, pos
|
return dist, pos
|
||||||
|
|
||||||
|
|
||||||
def plane_sphere(plane: GeomInfo, sphere: GeomInfo) -> Contact:
|
@collider(ncon=1)
|
||||||
|
def plane_sphere(plane: GeomInfo, sphere: GeomInfo) -> Collision:
|
||||||
"""Calculates contact between a plane and a sphere."""
|
"""Calculates contact between a plane and a sphere."""
|
||||||
n = plane.mat[:, 2]
|
n = plane.mat[:, 2]
|
||||||
dist, pos = _plane_sphere(n, plane.pos, sphere.pos, sphere.size[0])
|
dist, pos = _plane_sphere(n, plane.pos, sphere.pos, sphere.size[0])
|
||||||
return jax.tree_map(
|
return dist, pos, math.make_frame(n)
|
||||||
lambda x: jp.expand_dims(x, axis=0), (dist, pos, math.make_frame(n))
|
|
||||||
)
|
|
||||||
|
|
||||||
|
|
||||||
def plane_capsule(plane: GeomInfo, cap: GeomInfo) -> Contact:
|
@collider(ncon=2)
|
||||||
|
def plane_capsule(plane: GeomInfo, cap: GeomInfo) -> Collision:
|
||||||
"""Calculates two contacts between a capsule and a plane."""
|
"""Calculates two contacts between a capsule and a plane."""
|
||||||
n, axis = plane.mat[:, 2], cap.mat[:, 2]
|
n, axis = plane.mat[:, 2], cap.mat[:, 2]
|
||||||
# align contact frames with capsule axis
|
# align contact frames with capsule axis
|
||||||
@@ -56,16 +75,17 @@ def plane_capsule(plane: GeomInfo, cap: GeomInfo) -> Contact:
|
|||||||
b = jp.where(b_norm < 0.5, jp.where((-0.5 < n[1]) & (n[1] < 0.5), y, z), b)
|
b = jp.where(b_norm < 0.5, jp.where((-0.5 < n[1]) & (n[1] < 0.5), y, z), b)
|
||||||
frame = jp.array([[n, b, jp.cross(n, b)]])
|
frame = jp.array([[n, b, jp.cross(n, b)]])
|
||||||
segment = axis * cap.size[1]
|
segment = axis * cap.size[1]
|
||||||
contacts = []
|
collisions = []
|
||||||
for offset in [segment, -segment]:
|
for offset in [segment, -segment]:
|
||||||
dist, pos = _plane_sphere(n, plane.pos, cap.pos + offset, cap.size[0])
|
dist, pos = _plane_sphere(n, plane.pos, cap.pos + offset, cap.size[0])
|
||||||
dist = jp.expand_dims(dist, axis=0)
|
dist = jp.expand_dims(dist, axis=0)
|
||||||
pos = jp.expand_dims(pos, axis=0)
|
pos = jp.expand_dims(pos, axis=0)
|
||||||
contacts.append((dist, pos, frame))
|
collisions.append((dist, pos, frame))
|
||||||
return jax.tree_map(lambda *x: jp.concatenate(x), *contacts)
|
return jax.tree_util.tree_map(lambda *x: jp.concatenate(x), *collisions)
|
||||||
|
|
||||||
|
|
||||||
def plane_ellipsoid(plane: GeomInfo, ellipsoid: GeomInfo) -> Contact:
|
@collider(ncon=1)
|
||||||
|
def plane_ellipsoid(plane: GeomInfo, ellipsoid: GeomInfo) -> Collision:
|
||||||
"""Calculates one contact between an ellipsoid and a plane."""
|
"""Calculates one contact between an ellipsoid and a plane."""
|
||||||
n = plane.mat[:, 2]
|
n = plane.mat[:, 2]
|
||||||
size = ellipsoid.size
|
size = ellipsoid.size
|
||||||
@@ -73,14 +93,72 @@ def plane_ellipsoid(plane: GeomInfo, ellipsoid: GeomInfo) -> Contact:
|
|||||||
pos = ellipsoid.pos + ellipsoid.mat @ (sphere_support * size)
|
pos = ellipsoid.pos + ellipsoid.mat @ (sphere_support * size)
|
||||||
dist = jp.dot(n, pos - plane.pos)
|
dist = jp.dot(n, pos - plane.pos)
|
||||||
pos = pos - n * dist * 0.5
|
pos = pos - n * dist * 0.5
|
||||||
return jax.tree_map(
|
return dist, pos, math.make_frame(n)
|
||||||
lambda x: jp.expand_dims(x, axis=0), (dist, pos, math.make_frame(n))
|
|
||||||
|
|
||||||
|
@collider(ncon=3)
|
||||||
|
def plane_cylinder(plane: GeomInfo, cylinder: GeomInfo) -> Collision:
|
||||||
|
"""Calculates one contact between an cylinder and a plane."""
|
||||||
|
n = plane.mat[:, 2]
|
||||||
|
axis = cylinder.mat[:, 2]
|
||||||
|
|
||||||
|
# make sure axis points towards plane
|
||||||
|
prjaxis = jp.dot(n, axis)
|
||||||
|
sign = -math.sign(prjaxis)
|
||||||
|
axis, prjaxis = axis * sign, prjaxis * sign
|
||||||
|
|
||||||
|
# compute normal distance to cylinder center
|
||||||
|
dist0 = jp.dot(cylinder.pos - plane.pos, n)
|
||||||
|
|
||||||
|
# remove component of -normal along axis, compute length
|
||||||
|
vec = axis * prjaxis - n
|
||||||
|
len_ = math.norm(vec)
|
||||||
|
|
||||||
|
vec = jp.where(
|
||||||
|
len_ < 1e-12,
|
||||||
|
# disk parallel to plane: pick x-axis of cylinder, scale by radius
|
||||||
|
cylinder.mat[:, 0] * cylinder.size[0],
|
||||||
|
# general configuration: normalize vector, scale by radius
|
||||||
|
vec / len_ * cylinder.size[0]
|
||||||
)
|
)
|
||||||
|
|
||||||
|
# project vector on normal
|
||||||
|
prjvec = jp.dot(vec, n)
|
||||||
|
|
||||||
|
# scale axis by half-length
|
||||||
|
axis *= cylinder.size[1]
|
||||||
|
prjaxis *= cylinder.size[1]
|
||||||
|
|
||||||
|
# compute sideways vector: vec1
|
||||||
|
prjvec1 = -prjvec * 0.5
|
||||||
|
vec1 = math.normalize(jp.cross(vec, axis)) * cylinder.size[0]
|
||||||
|
vec1 *= jp.sqrt(3.0) * 0.5
|
||||||
|
|
||||||
|
# disk parallel to plane
|
||||||
|
d1 = dist0 + prjaxis + prjvec
|
||||||
|
d2 = dist0 + prjaxis + prjvec1
|
||||||
|
dist = jp.array([d1, d2, d2])
|
||||||
|
pos = cylinder.pos + axis + jp.array([
|
||||||
|
vec - n * d1 * 0.5,
|
||||||
|
vec1 + vec * -0.5 - n * d2 * 0.5,
|
||||||
|
-vec1 + vec * -0.5 - n * d2 * 0.5,
|
||||||
|
])
|
||||||
|
|
||||||
|
# cylinder parallel to plane
|
||||||
|
cond = jp.abs(prjaxis) < 1e-3
|
||||||
|
d3 = dist0 - prjaxis + prjvec
|
||||||
|
dist = jp.where(cond, dist.at[1].set(d3), dist)
|
||||||
|
pos = jp.where(
|
||||||
|
cond, pos.at[1].set(cylinder.pos + vec - axis - n * d3 * 0.5), pos
|
||||||
|
)
|
||||||
|
|
||||||
|
frame = jp.stack([math.make_frame(n)] * 3, axis=0)
|
||||||
|
return dist, pos, frame
|
||||||
|
|
||||||
|
|
||||||
def _sphere_sphere(
|
def _sphere_sphere(
|
||||||
pos1: jax.Array, radius1: jax.Array, pos2: jax.Array, radius2: jax.Array
|
pos1: jax.Array, radius1: jax.Array, pos2: jax.Array, radius2: jax.Array
|
||||||
) -> Contact:
|
) -> Tuple[jax.Array, jax.Array, jax.Array]:
|
||||||
"""Returns the penetration, contact point, and normal between two spheres."""
|
"""Returns the penetration, contact point, and normal between two spheres."""
|
||||||
n, dist = math.normalize_with_norm(pos2 - pos1)
|
n, dist = math.normalize_with_norm(pos2 - pos1)
|
||||||
n = jp.where(dist == 0.0, jp.array([1.0, 0.0, 0.0]), n)
|
n = jp.where(dist == 0.0, jp.array([1.0, 0.0, 0.0]), n)
|
||||||
@@ -89,15 +167,15 @@ def _sphere_sphere(
|
|||||||
return dist, pos, n
|
return dist, pos, n
|
||||||
|
|
||||||
|
|
||||||
def sphere_sphere(s1: GeomInfo, s2: GeomInfo) -> Contact:
|
@collider(ncon=1)
|
||||||
|
def sphere_sphere(s1: GeomInfo, s2: GeomInfo) -> Collision:
|
||||||
"""Calculates contact between two spheres."""
|
"""Calculates contact between two spheres."""
|
||||||
dist, pos, n = _sphere_sphere(s1.pos, s1.size[0], s2.pos, s2.size[0])
|
dist, pos, n = _sphere_sphere(s1.pos, s1.size[0], s2.pos, s2.size[0])
|
||||||
return jax.tree_map(
|
return dist, pos, math.make_frame(n)
|
||||||
lambda x: jp.expand_dims(x, axis=0), (dist, pos, math.make_frame(n))
|
|
||||||
)
|
|
||||||
|
|
||||||
|
|
||||||
def sphere_capsule(sphere: GeomInfo, cap: GeomInfo) -> Contact:
|
@collider(ncon=1)
|
||||||
|
def sphere_capsule(sphere: GeomInfo, cap: GeomInfo) -> Collision:
|
||||||
"""Calculates one contact between a sphere and a capsule."""
|
"""Calculates one contact between a sphere and a capsule."""
|
||||||
axis, length = cap.mat[:, 2], cap.size[1]
|
axis, length = cap.mat[:, 2], cap.size[1]
|
||||||
segment = axis * length
|
segment = axis * length
|
||||||
@@ -105,19 +183,14 @@ def sphere_capsule(sphere: GeomInfo, cap: GeomInfo) -> Contact:
|
|||||||
cap.pos - segment, cap.pos + segment, sphere.pos
|
cap.pos - segment, cap.pos + segment, sphere.pos
|
||||||
)
|
)
|
||||||
dist, pos, n = _sphere_sphere(sphere.pos, sphere.size[0], pt, cap.size[0])
|
dist, pos, n = _sphere_sphere(sphere.pos, sphere.size[0], pt, cap.size[0])
|
||||||
return jax.tree_map(
|
return dist, pos, math.make_frame(n)
|
||||||
lambda x: jp.expand_dims(x, axis=0), (dist, pos, math.make_frame(n))
|
|
||||||
)
|
|
||||||
|
|
||||||
|
|
||||||
def capsule_capsule(cap1: GeomInfo, cap2: GeomInfo) -> Contact:
|
@collider(ncon=1)
|
||||||
|
def capsule_capsule(cap1: GeomInfo, cap2: GeomInfo) -> Collision:
|
||||||
"""Calculates one contact between two capsules."""
|
"""Calculates one contact between two capsules."""
|
||||||
axis1, length1, axis2, length2 = (
|
axis1, length1 = cap1.mat[:, 2], cap1.size[1]
|
||||||
cap1.mat[:, 2],
|
axis2, length2 = cap2.mat[:, 2], cap2.size[1]
|
||||||
cap1.size[1],
|
|
||||||
cap2.mat[:, 2],
|
|
||||||
cap2.size[1],
|
|
||||||
)
|
|
||||||
seg1, seg2 = axis1 * length1, axis2 * length2
|
seg1, seg2 = axis1 * length1, axis2 * length2
|
||||||
pt1, pt2 = math.closest_segment_to_segment_points(
|
pt1, pt2 = math.closest_segment_to_segment_points(
|
||||||
cap1.pos - seg1,
|
cap1.pos - seg1,
|
||||||
@@ -127,14 +200,4 @@ def capsule_capsule(cap1: GeomInfo, cap2: GeomInfo) -> Contact:
|
|||||||
)
|
)
|
||||||
radius1, radius2 = cap1.size[0], cap2.size[0]
|
radius1, radius2 = cap1.size[0], cap2.size[0]
|
||||||
dist, pos, n = _sphere_sphere(pt1, radius1, pt2, radius2)
|
dist, pos, n = _sphere_sphere(pt1, radius1, pt2, radius2)
|
||||||
return jax.tree_map(
|
return dist, pos, math.make_frame(n)
|
||||||
lambda x: jp.expand_dims(x, axis=0), (dist, pos, math.make_frame(n))
|
|
||||||
)
|
|
||||||
|
|
||||||
# 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
|
|
||||||
|
|||||||
@@ -0,0 +1,243 @@
|
|||||||
|
# Copyright 2023 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.
|
||||||
|
# ==============================================================================
|
||||||
|
"""Collision functions for shapes represented as signed distance functions (SDF).
|
||||||
|
|
||||||
|
A signed distance function at a given point in space is the shortest distance to
|
||||||
|
a surface. This enables to define a geometry implicitly and exactly.
|
||||||
|
|
||||||
|
See https://iquilezles.org/articles/distfunctions/ for a list of analytic SDFs.
|
||||||
|
"""
|
||||||
|
|
||||||
|
import functools
|
||||||
|
from typing import Callable
|
||||||
|
from typing import Tuple
|
||||||
|
|
||||||
|
import jax
|
||||||
|
from jax import numpy as jp
|
||||||
|
from mujoco.mjx._src import math
|
||||||
|
# pylint: disable=g-importing-member
|
||||||
|
from mujoco.mjx._src.collision_types import Collision
|
||||||
|
from mujoco.mjx._src.collision_types import GeomInfo
|
||||||
|
from mujoco.mjx._src.dataclasses import PyTreeNode
|
||||||
|
from mujoco.mjx._src.types import Data
|
||||||
|
from mujoco.mjx._src.types import Model
|
||||||
|
# pylint: enable=g-importing-member
|
||||||
|
|
||||||
|
# the SDF function takes position in, and returns a distance or objective
|
||||||
|
SDFFn = Callable[[jax.Array], jax.Array]
|
||||||
|
|
||||||
|
|
||||||
|
def collider(ncon: int):
|
||||||
|
"""Wraps collision functions for use by collision_driver."""
|
||||||
|
def wrapper(func):
|
||||||
|
def collide(m: Model, d: Data, _, geom: jax.Array) -> Collision:
|
||||||
|
g1, g2 = geom.T
|
||||||
|
info1 = GeomInfo(d.geom_xpos[g1], d.geom_xmat[g1], m.geom_size[g1])
|
||||||
|
info2 = GeomInfo(d.geom_xpos[g2], d.geom_xmat[g2], m.geom_size[g2])
|
||||||
|
dist, pos, frame = jax.vmap(func)(info1, info2)
|
||||||
|
if ncon > 1:
|
||||||
|
return jax.tree_util.tree_map(jp.concatenate, (dist, pos, frame))
|
||||||
|
return dist, pos, frame
|
||||||
|
|
||||||
|
collide.ncon = ncon
|
||||||
|
return collide
|
||||||
|
|
||||||
|
return wrapper
|
||||||
|
|
||||||
|
|
||||||
|
def _plane(pos: jax.Array, size: jax.Array) -> jax.Array:
|
||||||
|
del size
|
||||||
|
return pos[2]
|
||||||
|
|
||||||
|
|
||||||
|
def _sphere(pos: jax.Array, size: jax.Array):
|
||||||
|
return math.norm(pos) - size[0]
|
||||||
|
|
||||||
|
|
||||||
|
def _capsule(pos: jax.Array, size: jax.Array):
|
||||||
|
pa = -size[1] * jp.array([0, 0, 1])
|
||||||
|
pb = size[1] * jp.array([0, 0, 1])
|
||||||
|
ab = pb - pa
|
||||||
|
ap = pos - pa
|
||||||
|
denom = ab.dot(ab)
|
||||||
|
denom = jp.where(jp.abs(denom) < 1e-12, 1e-12 * math.sign(denom), denom)
|
||||||
|
t = ab.dot(ap) / denom
|
||||||
|
t = jp.clip(t, 0, 1)
|
||||||
|
c = pa + t * ab
|
||||||
|
return math.norm(pos - c) - size[0]
|
||||||
|
|
||||||
|
|
||||||
|
def _ellipsoid(pos: jax.Array, size: jax.Array) -> jax.Array:
|
||||||
|
k0 = math.norm(pos / size)
|
||||||
|
k1 = math.norm(pos / (size*size))
|
||||||
|
return k0 * (k0 - 1.0) / (k1 + (k1 == 0.0) * 1e-12)
|
||||||
|
|
||||||
|
|
||||||
|
@jax.custom_jvp
|
||||||
|
def _cylinder(pos: jax.Array, size: jax.Array) -> jax.Array:
|
||||||
|
a0 = jp.sqrt(pos[0] * pos[0] + pos[1] * pos[1]) - size[0]
|
||||||
|
a1 = jp.abs(pos[2]) - size[1]
|
||||||
|
b0 = jp.maximum(a0, 0)
|
||||||
|
b1 = jp.maximum(a1, 0)
|
||||||
|
return jp.minimum(jp.maximum(a0, a1), 0) + jp.sqrt(b0 * b0 + b1 * b1)
|
||||||
|
|
||||||
|
|
||||||
|
def _cylinder_grad(x: jax.Array, size: jax.Array) -> jax.Array:
|
||||||
|
"""Gradient of the cylinder SDF wrt query point and singularities removed."""
|
||||||
|
c = jp.sqrt(x[0]*x[0]+x[1]*x[1])
|
||||||
|
e = jp.abs(x[2])
|
||||||
|
a = jp.array([c - size[0], e - size[1]])
|
||||||
|
b = jp.array([jp.maximum(a[0], 0), jp.maximum(a[1], 0)])
|
||||||
|
j = jp.argmax(a)
|
||||||
|
bnorm = jp.sqrt(b[0]*b[0] + b[1]*b[1])
|
||||||
|
bnorm += jp.allclose(bnorm, 0) * 1e-12
|
||||||
|
grada = jp.array([
|
||||||
|
x[0] / (c + jp.allclose(c, 0) * 1e-12),
|
||||||
|
x[1] / (c + jp.allclose(c, 0) * 1e-12),
|
||||||
|
x[2] / (e + jp.allclose(e, 0) * 1e-12),
|
||||||
|
])
|
||||||
|
gradm = jp.array([[grada[0], grada[1], 0], [0, 0, grada[2]]])
|
||||||
|
gradb = grada * b[jp.array([0, 0, 1])] / bnorm
|
||||||
|
return jp.where(a[j] < 0, gradm[j], gradb)
|
||||||
|
|
||||||
|
|
||||||
|
@_cylinder.defjvp
|
||||||
|
def cylinder_jvp(primals, tangents):
|
||||||
|
x, y = primals
|
||||||
|
x_dot, _ = tangents
|
||||||
|
primal_out = _cylinder(x, y)
|
||||||
|
tangent_out = jp.dot(_cylinder_grad(x, y), x_dot)
|
||||||
|
return primal_out, tangent_out
|
||||||
|
|
||||||
|
|
||||||
|
def _to_local(f: SDFFn, pos: jax.Array, mat: jax.Array)-> SDFFn:
|
||||||
|
return lambda p: f(mat.T @ (p - pos))
|
||||||
|
|
||||||
|
|
||||||
|
def _intersect(d1: SDFFn, d2: SDFFn) -> SDFFn:
|
||||||
|
return lambda p: jp.maximum(d1(p), d2(p))
|
||||||
|
|
||||||
|
|
||||||
|
def _clearance(d1: SDFFn, d2: SDFFn) -> SDFFn:
|
||||||
|
return lambda p: (d1(p) + d2(p) + jp.abs(_intersect(d1, d2)(p))).squeeze()
|
||||||
|
|
||||||
|
|
||||||
|
class GradientState(PyTreeNode):
|
||||||
|
dist: jax.Array
|
||||||
|
x: jax.Array
|
||||||
|
|
||||||
|
|
||||||
|
def _gradient_step(objective: SDFFn, state: GradientState) -> GradientState:
|
||||||
|
"""Performs a step of gradient descent."""
|
||||||
|
# TODO: find better parameters
|
||||||
|
amin = 1e-4 # minimum value for line search factor scaling the gradient
|
||||||
|
amax = 2. # maximum value for line search factor scaling the gradient
|
||||||
|
nlinesearch = 10 # line search points
|
||||||
|
grad = jax.grad(objective)(state.x)
|
||||||
|
alpha = jp.geomspace(amin, amax, nlinesearch).reshape(nlinesearch, -1)
|
||||||
|
candidates = state.x - alpha * grad.reshape(-1, 3)
|
||||||
|
values = jax.vmap(objective)(candidates)
|
||||||
|
idx = jp.argmin(values)
|
||||||
|
return state.replace(x=candidates[idx], dist=values[idx])
|
||||||
|
|
||||||
|
|
||||||
|
def _gradient_descent(
|
||||||
|
objective: SDFFn,
|
||||||
|
x: jax.Array,
|
||||||
|
niter: int,
|
||||||
|
) -> Tuple[jax.Array, jax.Array]:
|
||||||
|
"""Performs gradient descent with backtracking line search."""
|
||||||
|
state = GradientState(
|
||||||
|
dist=1e10,
|
||||||
|
x=x,
|
||||||
|
)
|
||||||
|
|
||||||
|
state, _ = jax.lax.scan(
|
||||||
|
lambda s, _: (_gradient_step(objective, s), None), state, (), length=niter
|
||||||
|
)
|
||||||
|
return state.dist, state.x
|
||||||
|
|
||||||
|
|
||||||
|
def _optim(
|
||||||
|
d1, d2, info1: GeomInfo, info2: GeomInfo, x0: jax.Array,
|
||||||
|
) -> Collision:
|
||||||
|
"""Optimizes the clearance function."""
|
||||||
|
d1 = functools.partial(d1, size=info1.size)
|
||||||
|
d1 = _to_local(d1, info1.pos, info1.mat)
|
||||||
|
d2 = functools.partial(d2, size=info2.size)
|
||||||
|
d2 = _to_local(d2, info2.pos, info2.mat)
|
||||||
|
fn = _clearance(d1, d2)
|
||||||
|
_, pos = _gradient_descent(fn, x0, 10)
|
||||||
|
dist = d1(pos) + d2(pos)
|
||||||
|
n = jax.grad(d1)(pos) - jax.grad(d2)(pos)
|
||||||
|
return dist, pos, math.make_frame(n)
|
||||||
|
|
||||||
|
|
||||||
|
@collider(ncon=1)
|
||||||
|
def capsule_ellipsoid(c: GeomInfo, e: GeomInfo) -> Collision:
|
||||||
|
""""Calculates contact between a capsule and an ellipsoid."""
|
||||||
|
x0 = 0.5 * (c.pos + e.pos)
|
||||||
|
return _optim(_capsule, _ellipsoid, c, e, x0)
|
||||||
|
|
||||||
|
|
||||||
|
@collider(ncon=2)
|
||||||
|
def capsule_cylinder(ca: GeomInfo, cy: GeomInfo) -> Collision:
|
||||||
|
""""Calculates contact between a capsule and a cylinder."""
|
||||||
|
# TODO: improve robustness
|
||||||
|
# Near sharp corners, the SDF might give the penetration depth with respect
|
||||||
|
# to a surface that is not in collision. Possible solutions is to find the
|
||||||
|
# contact points analytically or to change the SDF depending on the relative
|
||||||
|
# pose of the bodies.
|
||||||
|
mid = 0.5 * (ca.pos + cy.pos)
|
||||||
|
vec = ca.mat[:, 2] * ca.size[1]
|
||||||
|
x0 = jp.array([mid - vec, mid + vec])
|
||||||
|
optim_ = functools.partial(_optim, _capsule, _cylinder, ca, cy)
|
||||||
|
return jax.vmap(optim_)(x0)
|
||||||
|
|
||||||
|
|
||||||
|
@collider(ncon=1)
|
||||||
|
def ellipsoid_ellipsoid(e1: GeomInfo, e2: GeomInfo) -> Collision:
|
||||||
|
""""Calculates contact between two ellipsoids."""
|
||||||
|
x0 = 0.5 * (e1.pos + e2.pos)
|
||||||
|
return _optim(_ellipsoid, _ellipsoid, e1, e2, x0)
|
||||||
|
|
||||||
|
|
||||||
|
@collider(ncon=1)
|
||||||
|
def ellipsoid_cylinder(e: GeomInfo, c: GeomInfo) -> Collision:
|
||||||
|
""""Calculates contact between and ellipsoid and a cylinder."""
|
||||||
|
x0 = 0.5 * (e.pos + c.pos)
|
||||||
|
return _optim(_ellipsoid, _cylinder, e, c, x0)
|
||||||
|
|
||||||
|
|
||||||
|
@collider(ncon=4)
|
||||||
|
def cylinder_cylinder(c1: GeomInfo, c2: GeomInfo) -> Collision:
|
||||||
|
""""Calculates contact between a cylinder and a cylinder."""
|
||||||
|
# TODO: improve robustness
|
||||||
|
# Near sharp corners, the SDF might give the penetration depth with respect
|
||||||
|
# to a surface that is not in collision. Possible solutions is to find the
|
||||||
|
# contact points analytically or to change the SDF depending on the relative
|
||||||
|
# pose of the bodies.
|
||||||
|
basis = math.make_frame(c2.pos - c1.pos)
|
||||||
|
mid = 0.5 * (c1.pos + c2.pos)
|
||||||
|
r = jp.maximum(c1.size[0], c2.size[0])
|
||||||
|
x0 = jp.array([
|
||||||
|
mid + r * basis[1],
|
||||||
|
mid + r * basis[2],
|
||||||
|
mid - r * basis[1],
|
||||||
|
mid - r * basis[2],
|
||||||
|
])
|
||||||
|
optim_ = functools.partial(_optim, _cylinder, _cylinder, c1, c2)
|
||||||
|
return jax.vmap(optim_)(x0)
|
||||||
|
|
||||||
@@ -0,0 +1,65 @@
|
|||||||
|
# Copyright 2023 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.
|
||||||
|
# ==============================================================================
|
||||||
|
"""Collision base types."""
|
||||||
|
|
||||||
|
import dataclasses
|
||||||
|
from typing import Tuple
|
||||||
|
import jax
|
||||||
|
# pylint: disable=g-importing-member
|
||||||
|
from mujoco.mjx._src.dataclasses import PyTreeNode
|
||||||
|
# pylint: enable=g-importing-member
|
||||||
|
|
||||||
|
|
||||||
|
class GeomInfo(PyTreeNode):
|
||||||
|
"""Geom propertes of primitive and SDF shapes."""
|
||||||
|
pos: jax.Array
|
||||||
|
mat: jax.Array
|
||||||
|
size: jax.Array
|
||||||
|
|
||||||
|
|
||||||
|
class ConvexInfo(PyTreeNode):
|
||||||
|
"""Geom propertes of convex meshes."""
|
||||||
|
pos: jax.Array
|
||||||
|
mat: jax.Array
|
||||||
|
vert: jax.Array
|
||||||
|
face: jax.Array
|
||||||
|
face_normal: jax.Array
|
||||||
|
edge: jax.Array
|
||||||
|
edge_face_normal: jax.Array
|
||||||
|
edge_dir: jax.Array
|
||||||
|
|
||||||
|
|
||||||
|
# Collision returned by collision functions:
|
||||||
|
# - distance distance between nearest points; neg: penetration
|
||||||
|
# - position (3,) position of contact point: midpoint between geoms
|
||||||
|
# - frame (3, 3) normal is in [0, :], points from geom[0] to geom[1]
|
||||||
|
Collision = Tuple[jax.Array, jax.Array, jax.Array]
|
||||||
|
|
||||||
|
|
||||||
|
@dataclasses.dataclass(frozen=True)
|
||||||
|
class FunctionKey:
|
||||||
|
"""Specifies how geom pairs group into collision_driver's function table.
|
||||||
|
|
||||||
|
Attributes:
|
||||||
|
types: geom type pair, which determines the collision function
|
||||||
|
data_ids: geom data id pair: mesh id for mesh geoms, otherwise -1.
|
||||||
|
Meshes have distinct face/vertex counts, so must occupy distinct
|
||||||
|
entries in the collision function table.
|
||||||
|
condim: grouping by condim of the colliision ensures that the size of the
|
||||||
|
resulting constraint jacobian is determined at compile time.
|
||||||
|
"""
|
||||||
|
types: Tuple[int, int]
|
||||||
|
data_ids: Tuple[int, int]
|
||||||
|
condim: int
|
||||||
@@ -24,6 +24,7 @@ from mujoco.mjx._src import math
|
|||||||
from mujoco.mjx._src import support
|
from mujoco.mjx._src import support
|
||||||
# pylint: disable=g-importing-member
|
# pylint: disable=g-importing-member
|
||||||
from mujoco.mjx._src.dataclasses import PyTreeNode
|
from mujoco.mjx._src.dataclasses import PyTreeNode
|
||||||
|
from mujoco.mjx._src.types import ConstraintType
|
||||||
from mujoco.mjx._src.types import Contact
|
from mujoco.mjx._src.types import Contact
|
||||||
from mujoco.mjx._src.types import Data
|
from mujoco.mjx._src.types import Data
|
||||||
from mujoco.mjx._src.types import DisableBit
|
from mujoco.mjx._src.types import DisableBit
|
||||||
@@ -34,6 +35,9 @@ from mujoco.mjx._src.types import Model
|
|||||||
import numpy as np
|
import numpy as np
|
||||||
|
|
||||||
|
|
||||||
|
_CONDIM_EFC_COUNT = {1: 1, 3: 4, 4: 6, 6: 10}
|
||||||
|
|
||||||
|
|
||||||
class _Efc(PyTreeNode):
|
class _Efc(PyTreeNode):
|
||||||
"""Support data for creating constraint matrices."""
|
"""Support data for creating constraint matrices."""
|
||||||
J: jax.Array
|
J: jax.Array
|
||||||
@@ -111,7 +115,7 @@ def _instantiate_equality_connect(m: Model, d: Data) -> Optional[_Efc]:
|
|||||||
return j, cpos, jp.repeat(math.norm(cpos), 3)
|
return j, cpos, jp.repeat(math.norm(cpos), 3)
|
||||||
|
|
||||||
# concatenate to drop connect grouping dimension
|
# concatenate to drop connect grouping dimension
|
||||||
j, pos, pos_norm = jax.tree_map(jp.concatenate, fn(data, id1, id2))
|
j, pos, pos_norm = jax.tree_util.tree_map(jp.concatenate, fn(data, id1, id2))
|
||||||
invweight = m.body_invweight0[id1, 0] + m.body_invweight0[id2, 0]
|
invweight = m.body_invweight0[id1, 0] + m.body_invweight0[id2, 0]
|
||||||
invweight = jp.repeat(invweight, 3)
|
invweight = jp.repeat(invweight, 3)
|
||||||
solref = jp.tile(m.eq_solref[ids], (3, 1))
|
solref = jp.tile(m.eq_solref[ids], (3, 1))
|
||||||
@@ -164,7 +168,7 @@ def _instantiate_equality_weld(m: Model, d: Data) -> Optional[_Efc]:
|
|||||||
return j, pos, jp.repeat(math.norm(pos), 6)
|
return j, pos, jp.repeat(math.norm(pos), 6)
|
||||||
|
|
||||||
# concatenate to drop weld grouping dimension
|
# concatenate to drop weld grouping dimension
|
||||||
j, pos, pos_norm = jax.tree_map(jp.concatenate, fn(data, id1, id2))
|
j, pos, pos_norm = jax.tree_util.tree_map(jp.concatenate, fn(data, id1, id2))
|
||||||
invweight = m.body_invweight0[id1] + m.body_invweight0[id2]
|
invweight = m.body_invweight0[id1] + m.body_invweight0[id2]
|
||||||
invweight = jp.repeat(invweight, 3)
|
invweight = jp.repeat(invweight, 3)
|
||||||
solref = jp.tile(m.eq_solref[ids], (6, 1))
|
solref = jp.tile(m.eq_solref[ids], (6, 1))
|
||||||
@@ -277,73 +281,109 @@ def _instantiate_limit_slide_hinge(m: Model, d: Data) -> Optional[_Efc]:
|
|||||||
def _instantiate_contact(m: Model, d: Data) -> Optional[_Efc]:
|
def _instantiate_contact(m: Model, d: Data) -> Optional[_Efc]:
|
||||||
"""Calculates constraint rows for contacts."""
|
"""Calculates constraint rows for contacts."""
|
||||||
|
|
||||||
if collision_driver.ncon(m) == 0:
|
if d.ncon == 0:
|
||||||
return None
|
return None
|
||||||
|
|
||||||
|
def contact_efc(c: Contact, condim: int):
|
||||||
|
|
||||||
@jax.vmap
|
@jax.vmap
|
||||||
def fn(c: Contact):
|
def fn(c: Contact):
|
||||||
dist = c.dist - c.includemargin
|
dist = c.dist - c.includemargin
|
||||||
geom_bodyid = jp.array(m.geom_bodyid)
|
|
||||||
body1, body2 = geom_bodyid[c.geom1], geom_bodyid[c.geom2]
|
|
||||||
diff = support.jac_dif_pair(m, d, c.pos, body1, body2)
|
|
||||||
t = m.body_invweight0[body1, 0] + m.body_invweight0[body2, 0]
|
|
||||||
|
|
||||||
# rotate Jacobian differences to contact frame
|
|
||||||
diff_con = c.frame @ diff.T
|
|
||||||
|
|
||||||
# TODO(robotics-simulation): add support for other friction dimensions
|
|
||||||
# 4 pyramidal friction directions
|
|
||||||
js, invweights = [], []
|
|
||||||
for diff_tan, friction in zip(diff_con[1:], c.friction[:2]):
|
|
||||||
for f in (friction, -friction):
|
|
||||||
js.append(diff_con[0] + diff_tan * f)
|
|
||||||
invweights.append((t + f * f * t) * 2 * f * f / m.opt.impratio)
|
|
||||||
|
|
||||||
active = dist < 0
|
active = dist < 0
|
||||||
j, invweight = jp.stack(js) * active, jp.stack(invweights)
|
body1, body2 = jp.array(m.geom_bodyid)[c.geom]
|
||||||
pos = jp.repeat(dist, 4) * active
|
jac1p, jac1r = support.jac(m, d, c.pos, body1)
|
||||||
solref, solimp = jp.tile(c.solref, (4, 1)), jp.tile(c.solimp, (4, 1))
|
jac2p, jac2r = support.jac(m, d, c.pos, body2)
|
||||||
|
diff = c.frame @ (jac2p - jac1p).T
|
||||||
|
if condim > 3: # only calculate rotational diff if needed
|
||||||
|
diff = jp.concatenate((diff, c.frame @ (jac2r - jac1r).T), axis=0)
|
||||||
|
tran = m.body_invweight0[body1, 0] + m.body_invweight0[body2, 0]
|
||||||
|
|
||||||
return j, invweight, pos, solref, solimp
|
if condim == 1:
|
||||||
|
return diff[0] * active, tran, dist * active, c.solref, c.solimp
|
||||||
|
|
||||||
res = fn(d.contact)
|
# a pair of opposing pyramid edges per friction dimension
|
||||||
# remove contact grouping dimension:
|
# repeat friction directions with positive and negative sign
|
||||||
j, invweight, pos, solref, solimp = jax.tree_map(jp.concatenate, res)
|
fri = jp.repeat(c.friction[: condim - 1], 2, axis=0).at[1::2].mul(-1)
|
||||||
|
# repeat condims of jacdiff to match +/- friction directions
|
||||||
|
j = diff[0] + jp.repeat(diff[1:condim], 2, axis=0) * fri[:, None]
|
||||||
|
# pyramidal has common invweight across all edges
|
||||||
|
diag_approx = tran + fri[0] * fri[0] * tran
|
||||||
|
inv_w = diag_approx * 2 * fri[0] * fri[0] / m.opt.impratio
|
||||||
|
repeat_fn = lambda x: jp.repeat(x[None], (condim - 1) * 2, axis=0)
|
||||||
|
inv_w, pos, solref, solimp = jax.tree_util.tree_map(
|
||||||
|
repeat_fn, (inv_w, dist, c.solref, c.solimp)
|
||||||
|
)
|
||||||
|
return j * active, inv_w, pos * active, solref, solimp
|
||||||
|
|
||||||
|
return fn(c)
|
||||||
|
|
||||||
|
# group efc calculations by condim
|
||||||
|
dims, begs = np.unique(d.contact.dim, return_index=True)
|
||||||
|
efcs = []
|
||||||
|
for i in range(len(dims)):
|
||||||
|
dim, beg = dims[i], begs[i]
|
||||||
|
end = begs[i + 1] if i < len(dims) - 1 else None
|
||||||
|
c = jax.tree_util.tree_map(lambda x, b=beg, e=end: x[b:e], d.contact)
|
||||||
|
efc = contact_efc(c, dim)
|
||||||
|
if dim > 1:
|
||||||
|
# remove efc grouping dimension
|
||||||
|
efc = jax.tree_util.tree_map(jp.concatenate, efc)
|
||||||
|
efcs.append(efc)
|
||||||
|
|
||||||
|
efc = jax.tree_util.tree_map(lambda *x: jp.concatenate(x), *efcs)
|
||||||
|
j, invweight, pos, solref, solimp = efc
|
||||||
frictionloss = jp.zeros_like(pos)
|
frictionloss = jp.zeros_like(pos)
|
||||||
|
|
||||||
return _Efc(j, pos, pos, invweight, solref, solimp, frictionloss)
|
return _Efc(j, pos, pos, invweight, solref, solimp, frictionloss)
|
||||||
|
|
||||||
|
|
||||||
def count_constraints(
|
def counts(efc_type: np.ndarray) -> Tuple[int, int, int, int]:
|
||||||
m: Union[Model, mujoco.MjModel], d: Optional[Data] = None
|
|
||||||
) -> Tuple[int, int, int, int]:
|
|
||||||
"""Returns equality, friction, limit, and contact constraint counts."""
|
"""Returns equality, friction, limit, and contact constraint counts."""
|
||||||
if m.opt.disableflags & DisableBit.CONSTRAINT:
|
ne = (efc_type == ConstraintType.EQUALITY).sum()
|
||||||
return 0, 0, 0, 0
|
nf = 0 # no support for friction loss yet
|
||||||
|
nl = (efc_type == ConstraintType.LIMIT_JOINT).sum()
|
||||||
if m.opt.disableflags & DisableBit.EQUALITY:
|
nc_f = (efc_type == ConstraintType.CONTACT_FRICTIONLESS).sum()
|
||||||
ne = 0
|
nc_p = (efc_type == ConstraintType.CONTACT_PYRAMIDAL).sum()
|
||||||
else:
|
nc = nc_f + nc_p
|
||||||
ne_connect = (m.eq_type == EqType.CONNECT).sum()
|
|
||||||
ne_weld = (m.eq_type == EqType.WELD).sum()
|
|
||||||
ne_joint = (m.eq_type == EqType.JOINT).sum()
|
|
||||||
ne = ne_connect * 3 + ne_weld * 6 + ne_joint
|
|
||||||
|
|
||||||
nf = 0
|
|
||||||
|
|
||||||
if m.opt.disableflags & DisableBit.LIMIT:
|
|
||||||
nl = 0
|
|
||||||
else:
|
|
||||||
nl = int(m.jnt_limited.sum())
|
|
||||||
|
|
||||||
if d is None:
|
|
||||||
nc = collision_driver.ncon(m) * 4
|
|
||||||
else:
|
|
||||||
nc = d.efc_J.shape[-2] - ne - nf - nl
|
|
||||||
|
|
||||||
return ne, nf, nl, nc
|
return ne, nf, nl, nc
|
||||||
|
|
||||||
|
|
||||||
|
def make_efc_type(
|
||||||
|
m: Union[Model, mujoco.MjModel], dim: Optional[np.ndarray] = None
|
||||||
|
) -> np.ndarray:
|
||||||
|
"""Returns efc_type that outlines the type of each constraint row."""
|
||||||
|
if m.opt.disableflags & DisableBit.CONSTRAINT:
|
||||||
|
return np.empty(0, dtype=int)
|
||||||
|
|
||||||
|
dim = collision_driver.make_condim(m) if dim is None else dim
|
||||||
|
efc_types = []
|
||||||
|
|
||||||
|
if not m.opt.disableflags & DisableBit.EQUALITY:
|
||||||
|
num_rows = (m.eq_type == EqType.CONNECT).sum() * 3
|
||||||
|
num_rows += (m.eq_type == EqType.WELD).sum() * 6
|
||||||
|
num_rows += (m.eq_type == EqType.JOINT).sum()
|
||||||
|
efc_types.extend([ConstraintType.EQUALITY] * num_rows)
|
||||||
|
|
||||||
|
if not m.opt.disableflags & DisableBit.LIMIT:
|
||||||
|
efc_types.extend([ConstraintType.LIMIT_JOINT] * m.jnt_limited.sum())
|
||||||
|
|
||||||
|
if not m.opt.disableflags & DisableBit.CONTACT:
|
||||||
|
num_rows = sum(_CONDIM_EFC_COUNT[d] for d in dim)
|
||||||
|
efc_types.extend([ConstraintType.CONTACT_PYRAMIDAL] * num_rows)
|
||||||
|
|
||||||
|
return np.array(efc_types)
|
||||||
|
|
||||||
|
|
||||||
|
def make_efc_address(efc_type: np.ndarray, dim: np.ndarray) -> np.ndarray:
|
||||||
|
"""Returns efc_address that maps contacts to constraint row address."""
|
||||||
|
nc = (efc_type == ConstraintType.CONTACT_PYRAMIDAL).sum()
|
||||||
|
nc_start = efc_type.size - nc
|
||||||
|
offsets = np.cumsum([0] + [_CONDIM_EFC_COUNT[d] for d in dim])[:-1]
|
||||||
|
|
||||||
|
return nc_start + offsets
|
||||||
|
|
||||||
|
|
||||||
def make_constraint(m: Model, d: Data) -> Data:
|
def make_constraint(m: Model, d: Data) -> Data:
|
||||||
"""Creates constraint jacobians and other supporting data."""
|
"""Creates constraint jacobians and other supporting data."""
|
||||||
|
|
||||||
@@ -366,7 +406,7 @@ def make_constraint(m: Model, d: Data) -> Data:
|
|||||||
d = d.replace(efc_D=z, efc_aref=z, efc_frictionloss=z)
|
d = d.replace(efc_D=z, efc_aref=z, efc_frictionloss=z)
|
||||||
return d
|
return d
|
||||||
|
|
||||||
efc = jax.tree_map(lambda *x: jp.concatenate(x), *efcs)
|
efc = jax.tree_util.tree_map(lambda *x: jp.concatenate(x), *efcs)
|
||||||
|
|
||||||
@jax.vmap
|
@jax.vmap
|
||||||
def fn(efc):
|
def fn(efc):
|
||||||
|
|||||||
@@ -48,13 +48,31 @@ class ConstraintTest(absltest.TestCase):
|
|||||||
mujoco.mj_forward(m, d)
|
mujoco.mj_forward(m, d)
|
||||||
mx = mjx.put_model(m)
|
mx = mjx.put_model(m)
|
||||||
dx = mjx.put_data(m, d)
|
dx = mjx.put_data(m, d)
|
||||||
|
|
||||||
dx = mjx.make_constraint(mx, dx)
|
dx = mjx.make_constraint(mx, dx)
|
||||||
nnz = dx.efc_J.any(axis=1)
|
d_efc_j = d.efc_J.reshape((-1, m.nv))
|
||||||
_assert_eq(d.efc_J, dx.efc_J[nnz].reshape(-1), 'efc_J')
|
|
||||||
_assert_eq(d.efc_D, dx.efc_D[nnz], 'efc_D')
|
# ne, nf, nl order matches
|
||||||
_assert_eq(d.efc_aref, dx.efc_aref[nnz], 'efc_aref')
|
efl = d.ne + d.nf + d.nl
|
||||||
_assert_eq(d.efc_frictionloss, dx.efc_frictionloss[nnz], 'efc_frictionloss')
|
_assert_eq(d_efc_j[:efl], dx.efc_J[:efl], 'efc_J')
|
||||||
|
_assert_eq(d.efc_D[:efl], dx.efc_D[:efl], 'efc_D')
|
||||||
|
_assert_eq(d.efc_aref[:efl], dx.efc_aref[:efl], 'efc_aref')
|
||||||
|
_assert_eq(dx.efc_frictionloss, 0, 'efc_frictionloss')
|
||||||
|
|
||||||
|
# contact order might not match, so check efcs contact by contact
|
||||||
|
for i in range(d.ncon):
|
||||||
|
geom_match = (dx.contact.geom == d.contact.geom[i]).all(axis=-1)
|
||||||
|
geom_match &= (dx.contact.pos == d.contact.pos[i]).all(axis=-1)
|
||||||
|
self.assertTrue(geom_match.any(), f'contact {i} not found in MJX contact')
|
||||||
|
j = np.nonzero(geom_match)[0][0]
|
||||||
|
self.assertEqual(d.contact.dim[i], dx.contact.dim[j])
|
||||||
|
nc = max(1, (d.contact.dim[i] - 1) * 2)
|
||||||
|
d_beg, dx_beg = d.contact.efc_address[i], dx.contact.efc_address[j]
|
||||||
|
d_end, dx_end = d_beg + nc, dx_beg + nc
|
||||||
|
_assert_eq(d_efc_j[d_beg:d_end], dx.efc_J[dx_beg:dx_end], 'efc_J')
|
||||||
|
_assert_eq(d.efc_D[d_beg:d_end], dx.efc_D[dx_beg:dx_end], 'efc_D')
|
||||||
|
d_efc_aref = d.efc_aref[d_beg:d_end]
|
||||||
|
dx_efc_aref = dx.efc_aref[dx_beg:dx_end]
|
||||||
|
_assert_eq(d_efc_aref, dx_efc_aref, 'efc_aref')
|
||||||
|
|
||||||
def test_disable_refsafe(self):
|
def test_disable_refsafe(self):
|
||||||
m = test_util.load_test_file('constraints.xml')
|
m = test_util.load_test_file('constraints.xml')
|
||||||
@@ -65,14 +83,14 @@ class ConstraintTest(absltest.TestCase):
|
|||||||
pos = jp.ones(3)
|
pos = jp.ones(3)
|
||||||
|
|
||||||
m.opt.disableflags = m.opt.disableflags | mjx.DisableBit.REFSAFE
|
m.opt.disableflags = m.opt.disableflags | mjx.DisableBit.REFSAFE
|
||||||
mx = mjx.device_put(m)
|
mx = mjx.put_model(m)
|
||||||
k, *_ = constraint._kbi(mx, solimp, solref, pos)
|
k, *_ = constraint._kbi(mx, solimp, solref, pos)
|
||||||
self.assertEqual(k, 1 / (0.99**2 * timeconst**2))
|
self.assertEqual(k, 1 / (0.99**2 * timeconst**2))
|
||||||
|
|
||||||
def test_disable_constraint(self):
|
def test_disable_constraint(self):
|
||||||
m = test_util.load_test_file('constraints.xml')
|
m = test_util.load_test_file('constraints.xml')
|
||||||
m.opt.disableflags = m.opt.disableflags | mjx.DisableBit.CONSTRAINT
|
m.opt.disableflags = m.opt.disableflags | mjx.DisableBit.CONSTRAINT
|
||||||
ne, nf, nl, nc = mjx.count_constraints(m)
|
ne, nf, nl, nc = constraint.counts(constraint.make_efc_type(m))
|
||||||
self.assertEqual(ne, 0)
|
self.assertEqual(ne, 0)
|
||||||
self.assertEqual(nf, 0)
|
self.assertEqual(nf, 0)
|
||||||
self.assertEqual(nl, 0)
|
self.assertEqual(nl, 0)
|
||||||
@@ -83,18 +101,18 @@ class ConstraintTest(absltest.TestCase):
|
|||||||
def test_disable_equality(self):
|
def test_disable_equality(self):
|
||||||
m = test_util.load_test_file('constraints.xml')
|
m = test_util.load_test_file('constraints.xml')
|
||||||
m.opt.disableflags = m.opt.disableflags | mjx.DisableBit.EQUALITY
|
m.opt.disableflags = m.opt.disableflags | mjx.DisableBit.EQUALITY
|
||||||
ne, nf, nl, nc = mjx.count_constraints(m)
|
ne, nf, nl, nc = constraint.counts(constraint.make_efc_type(m))
|
||||||
self.assertEqual(ne, 0)
|
self.assertEqual(ne, 0)
|
||||||
self.assertEqual(nf, 0)
|
self.assertEqual(nf, 0)
|
||||||
self.assertEqual(nl, 2)
|
self.assertEqual(nl, 2)
|
||||||
self.assertEqual(nc, 64)
|
self.assertEqual(nc, 148)
|
||||||
dx = constraint.make_constraint(mjx.put_model(m), mjx.make_data(m))
|
dx = constraint.make_constraint(mjx.put_model(m), mjx.make_data(m))
|
||||||
self.assertEqual(dx.efc_J.shape[0], 66) # only joint range, contact
|
self.assertEqual(dx.efc_J.shape[0], 150) # only joint range, contact
|
||||||
|
|
||||||
def test_disable_contact(self):
|
def test_disable_contact(self):
|
||||||
m = test_util.load_test_file('constraints.xml')
|
m = test_util.load_test_file('constraints.xml')
|
||||||
m.opt.disableflags = m.opt.disableflags | mjx.DisableBit.CONTACT
|
m.opt.disableflags = m.opt.disableflags | mjx.DisableBit.CONTACT
|
||||||
ne, nf, nl, nc = mjx.count_constraints(m)
|
ne, nf, nl, nc = constraint.counts(constraint.make_efc_type(m))
|
||||||
self.assertEqual(ne, 10)
|
self.assertEqual(ne, 10)
|
||||||
self.assertEqual(nf, 0)
|
self.assertEqual(nf, 0)
|
||||||
self.assertEqual(nl, 2)
|
self.assertEqual(nl, 2)
|
||||||
|
|||||||
@@ -18,7 +18,7 @@ import copy
|
|||||||
import dataclasses
|
import dataclasses
|
||||||
|
|
||||||
import typing
|
import typing
|
||||||
from typing import Any, Dict, Optional, Sequence, TypeVar, Union
|
from typing import Dict, Optional, Sequence, Tuple, TypeVar, Union
|
||||||
import jax
|
import jax
|
||||||
import numpy as np
|
import numpy as np
|
||||||
|
|
||||||
@@ -122,7 +122,7 @@ class PyTreeNode:
|
|||||||
raise NotImplementedError
|
raise NotImplementedError
|
||||||
|
|
||||||
@classmethod
|
@classmethod
|
||||||
def fields(cls) -> tuple[dataclasses.Field[Any], ...]:
|
def fields(cls) -> Tuple[dataclasses.Field, ...]: # pylint: disable=g-bare-generic
|
||||||
return dataclasses.fields(cls)
|
return dataclasses.fields(cls)
|
||||||
|
|
||||||
def tree_replace(
|
def tree_replace(
|
||||||
|
|||||||
@@ -1,318 +0,0 @@
|
|||||||
# Copyright 2023 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.
|
|
||||||
# ==============================================================================
|
|
||||||
"""Get and put mujoco data on/off device."""
|
|
||||||
|
|
||||||
import copy
|
|
||||||
import dataclasses
|
|
||||||
from typing import Any, Dict, Iterable, List, Union, overload
|
|
||||||
import warnings
|
|
||||||
|
|
||||||
import jax
|
|
||||||
from jax import numpy as jp
|
|
||||||
import mujoco
|
|
||||||
from mujoco.mjx._src import collision_driver
|
|
||||||
from mujoco.mjx._src import mesh
|
|
||||||
from mujoco.mjx._src import types
|
|
||||||
import numpy as np
|
|
||||||
|
|
||||||
_MJ_TYPE_ATTR = {
|
|
||||||
mujoco.mjtBias: (mujoco.MjModel.actuator_biastype,),
|
|
||||||
mujoco.mjtDyn: (mujoco.MjModel.actuator_dyntype,),
|
|
||||||
mujoco.mjtEq: (mujoco.MjModel.eq_type,),
|
|
||||||
mujoco.mjtGain: (mujoco.MjModel.actuator_gaintype,),
|
|
||||||
mujoco.mjtTrn: (mujoco.MjModel.actuator_trntype,),
|
|
||||||
mujoco.mjtCone: (
|
|
||||||
mujoco.MjModel.opt,
|
|
||||||
mujoco.MjOption.cone,
|
|
||||||
),
|
|
||||||
mujoco.mjtIntegrator: (
|
|
||||||
mujoco.MjModel.opt,
|
|
||||||
mujoco.MjOption.integrator,
|
|
||||||
),
|
|
||||||
mujoco.mjtSolver: (
|
|
||||||
mujoco.MjModel.opt,
|
|
||||||
mujoco.MjOption.solver,
|
|
||||||
),
|
|
||||||
}
|
|
||||||
|
|
||||||
_TYPE_MAP = {
|
|
||||||
mujoco._structs._MjContactList: types.Contact, # pylint: disable=protected-access
|
|
||||||
mujoco.MjData: types.Data,
|
|
||||||
mujoco.MjModel: types.Model,
|
|
||||||
mujoco.MjOption: types.Option,
|
|
||||||
mujoco.MjStatistic: types.Statistic,
|
|
||||||
mujoco.mjtBias: types.BiasType,
|
|
||||||
mujoco.mjtCone: types.ConeType,
|
|
||||||
mujoco.mjtDisableBit: types.DisableBit,
|
|
||||||
mujoco.mjtDyn: types.DynType,
|
|
||||||
mujoco.mjtEq: types.EqType,
|
|
||||||
mujoco.mjtGain: types.GainType,
|
|
||||||
mujoco.mjtIntegrator: types.IntegratorType,
|
|
||||||
mujoco.mjtSolver: types.SolverType,
|
|
||||||
mujoco.mjtTrn: types.TrnType,
|
|
||||||
}
|
|
||||||
|
|
||||||
_TRANSFORMS = {
|
|
||||||
(types.Data, 'ximat'): lambda x: x.reshape(x.shape[:-1] + (3, 3)),
|
|
||||||
(types.Data, 'xmat'): lambda x: x.reshape(x.shape[:-1] + (3, 3)),
|
|
||||||
(types.Data, 'geom_xmat'): lambda x: x.reshape(x.shape[:-1] + (3, 3)),
|
|
||||||
(types.Data, 'site_xmat'): lambda x: x.reshape(x.shape[:-1] + (3, 3)),
|
|
||||||
(types.Data, 'cam_xmat'): lambda x: x.reshape(x.shape[:-1] + (3, 3)),
|
|
||||||
(types.Model, 'cam_mat0'): lambda x: x.reshape(x.shape[:-1] + (3, 3)),
|
|
||||||
(types.Contact, 'frame'): (
|
|
||||||
lambda x: x.reshape(x.shape[:-1] + (3, 3)) # pylint: disable=g-long-lambda
|
|
||||||
if x is not None and x.shape[0]
|
|
||||||
else jp.zeros((0, 3, 3))
|
|
||||||
),
|
|
||||||
}
|
|
||||||
|
|
||||||
_INVERSE_TRANSFORMS = {
|
|
||||||
(types.Data, 'ximat'): lambda x: x.reshape(x.shape[:-2] + (9,)),
|
|
||||||
(types.Data, 'xmat'): lambda x: x.reshape(x.shape[:-2] + (9,)),
|
|
||||||
(types.Data, 'geom_xmat'): lambda x: x.reshape(x.shape[:-2] + (9,)),
|
|
||||||
(types.Data, 'site_xmat'): lambda x: x.reshape(x.shape[:-2] + (9,)),
|
|
||||||
(types.Data, 'cam_xmat'): lambda x: x.reshape(x.shape[:-2] + (9,)),
|
|
||||||
(types.Model, 'cam_mat0'): lambda x: x.reshape(x.shape[:-2] + (9,)),
|
|
||||||
(types.Contact, 'frame'): (
|
|
||||||
lambda x: x.reshape(x.shape[:-2] + (9,)) # pylint: disable=g-long-lambda
|
|
||||||
if x is not None and x.shape[0]
|
|
||||||
else jp.zeros((0, 9))
|
|
||||||
),
|
|
||||||
}
|
|
||||||
|
|
||||||
_DERIVED = mesh.DERIVED.union(
|
|
||||||
# efc_J is dense in MJX, sparse in MJ. ignore for now.
|
|
||||||
{(types.Data, 'efc_J'), (types.Option, 'has_fluid_params')}
|
|
||||||
)
|
|
||||||
|
|
||||||
|
|
||||||
def _model_derived(value: mujoco.MjModel) -> Dict[str, Any]:
|
|
||||||
return {k: jax.device_put(v) for k, v in mesh.get(value).items()}
|
|
||||||
|
|
||||||
|
|
||||||
def _data_derived(value: mujoco.MjData) -> Dict[str, Any]:
|
|
||||||
return {'efc_J': jax.device_put(value.efc_J)}
|
|
||||||
|
|
||||||
|
|
||||||
def _option_derived(value: types.Option) -> Dict[str, Any]:
|
|
||||||
has_fluid = (
|
|
||||||
value.density > 0 or value.viscosity > 0 or (value.wind != 0.0).any()
|
|
||||||
)
|
|
||||||
return {'has_fluid_params': has_fluid}
|
|
||||||
|
|
||||||
|
|
||||||
def _validate(m: mujoco.MjModel):
|
|
||||||
"""Validates that an mjModel is compatible with MJX."""
|
|
||||||
|
|
||||||
# check enum types
|
|
||||||
for mj_type, attrs in _MJ_TYPE_ATTR.items():
|
|
||||||
val = m
|
|
||||||
for attr in attrs:
|
|
||||||
val = attr.fget(val) # pytype: disable=attribute-error
|
|
||||||
|
|
||||||
typs = set(val) if isinstance(val, Iterable) else {val}
|
|
||||||
unsupported_typs = typs - set(_TYPE_MAP[mj_type])
|
|
||||||
unsupported = [mj_type(t) for t in unsupported_typs] # pylint: disable=too-many-function-args
|
|
||||||
if unsupported:
|
|
||||||
raise NotImplementedError(f'{unsupported} not implemented.')
|
|
||||||
|
|
||||||
# check condim
|
|
||||||
if any(dim != 3 for dim in m.geom_condim) or any(
|
|
||||||
dim != 3 for dim in m.pair_dim
|
|
||||||
):
|
|
||||||
raise NotImplementedError('Only condim=3 is supported.')
|
|
||||||
|
|
||||||
if m.ntendon:
|
|
||||||
raise NotImplementedError('Tendons are not supported.')
|
|
||||||
|
|
||||||
# check collision geom types
|
|
||||||
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,
|
|
||||||
mujoco.mjtGeom.mjGEOM_HFIELD,
|
|
||||||
):
|
|
||||||
# MuJoCo does not collide planes with other planes or hfields
|
|
||||||
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.
|
|
||||||
|
|
||||||
# mjNDISABLE is not a DisableBit flag, so must be explicitly ignored
|
|
||||||
disablebit_members = set(mujoco.mjtDisableBit.__members__.values()) - {
|
|
||||||
mujoco.mjtDisableBit.mjNDISABLE}
|
|
||||||
unsupported_disable = disablebit_members - {
|
|
||||||
mujoco.mjtDisableBit(t.value) for t in types.DisableBit
|
|
||||||
}
|
|
||||||
for f in unsupported_disable:
|
|
||||||
if f & m.opt.disableflags:
|
|
||||||
warnings.warn(f'Ignoring disable flag {f.name}.')
|
|
||||||
|
|
||||||
# mjNENABLE is not an EnableBit flag, so must be explicitly ignored
|
|
||||||
unsupported_enable = set(mujoco.mjtEnableBit.__members__.values()) - {
|
|
||||||
mujoco.mjtEnableBit.mjNENABLE
|
|
||||||
}
|
|
||||||
for f in unsupported_enable:
|
|
||||||
if f & m.opt.enableflags:
|
|
||||||
warnings.warn(f'Ignoring enable flag {f.name}.')
|
|
||||||
|
|
||||||
if not np.allclose(m.dof_frictionloss, 0):
|
|
||||||
raise NotImplementedError('dof_frictionloss is not implemented.')
|
|
||||||
|
|
||||||
|
|
||||||
@overload
|
|
||||||
def device_put(value: mujoco.MjData) -> types.Data:
|
|
||||||
...
|
|
||||||
|
|
||||||
|
|
||||||
@overload
|
|
||||||
def device_put(value: mujoco.MjModel) -> types.Model:
|
|
||||||
...
|
|
||||||
|
|
||||||
|
|
||||||
def device_put(value):
|
|
||||||
"""Places mujoco data onto a device.
|
|
||||||
|
|
||||||
Args:
|
|
||||||
value: a mujoco struct to transfer
|
|
||||||
|
|
||||||
Returns:
|
|
||||||
on-device MJX struct reflecting the input value
|
|
||||||
"""
|
|
||||||
warnings.warn(
|
|
||||||
'device_put is deprecated, use put_model and put_data instead',
|
|
||||||
category=DeprecationWarning,
|
|
||||||
)
|
|
||||||
|
|
||||||
clz = _TYPE_MAP.get(type(value))
|
|
||||||
if clz is None:
|
|
||||||
raise NotImplementedError(f'{type(value)} is not supported for device_put.')
|
|
||||||
|
|
||||||
if isinstance(value, mujoco.MjModel):
|
|
||||||
_validate(value) # type: ignore
|
|
||||||
|
|
||||||
init_kwargs = {}
|
|
||||||
for f in dataclasses.fields(clz): # type: ignore
|
|
||||||
if (clz, f.name) in _DERIVED:
|
|
||||||
continue
|
|
||||||
|
|
||||||
field_value = getattr(value, f.name)
|
|
||||||
if (clz, f.name) in _TRANSFORMS:
|
|
||||||
field_value = _TRANSFORMS[(clz, f.name)](field_value)
|
|
||||||
|
|
||||||
if f.type is jax.Array:
|
|
||||||
field_value = jax.device_put(field_value)
|
|
||||||
elif type(field_value) in _TYPE_MAP.keys():
|
|
||||||
field_value = device_put(field_value)
|
|
||||||
|
|
||||||
init_kwargs[f.name] = copy.copy(field_value)
|
|
||||||
|
|
||||||
derived_kwargs = {}
|
|
||||||
if isinstance(value, mujoco.MjModel):
|
|
||||||
derived_kwargs = _model_derived(value)
|
|
||||||
elif isinstance(value, mujoco.MjData):
|
|
||||||
derived_kwargs = _data_derived(value)
|
|
||||||
elif isinstance(value, mujoco.MjOption):
|
|
||||||
derived_kwargs = _option_derived(value)
|
|
||||||
|
|
||||||
return clz(**init_kwargs, **derived_kwargs) # type: ignore
|
|
||||||
|
|
||||||
|
|
||||||
@overload
|
|
||||||
def device_get_into(
|
|
||||||
result: Union[mujoco.MjData, List[mujoco.MjData]], value: types.Data
|
|
||||||
):
|
|
||||||
...
|
|
||||||
|
|
||||||
|
|
||||||
def device_get_into(result, value):
|
|
||||||
"""Transfers data off device into a mujoco MjData.
|
|
||||||
|
|
||||||
Data on device often has a batch dimension which adds (N,) to the beginning
|
|
||||||
of each array shape where N = batch size.
|
|
||||||
|
|
||||||
If result is a single MjData, arrays are copied over with the batch dimension
|
|
||||||
intact. If result is a list, the list must be length N and will be populated
|
|
||||||
with distinct MjData structs where the batch dimension is stripped.
|
|
||||||
|
|
||||||
Args:
|
|
||||||
result: struct (or list of structs) to transfer into
|
|
||||||
value: device value to transfer
|
|
||||||
|
|
||||||
Raises:
|
|
||||||
RuntimeError: if result length doesn't match data batch size
|
|
||||||
"""
|
|
||||||
warnings.warn(
|
|
||||||
'device_get_into is deprecated, use get_data instead',
|
|
||||||
category=DeprecationWarning,
|
|
||||||
)
|
|
||||||
|
|
||||||
value = jax.device_get(value)
|
|
||||||
|
|
||||||
if isinstance(result, list):
|
|
||||||
array_shapes = [s.shape for s in jax.tree_util.tree_flatten(value)[0]]
|
|
||||||
|
|
||||||
if any(len(s) < 1 or s[0] != array_shapes[0][0] for s in array_shapes):
|
|
||||||
raise ValueError('unrecognizable batch dimension in value')
|
|
||||||
|
|
||||||
batch_size = array_shapes[0][0]
|
|
||||||
|
|
||||||
if len(result) != batch_size:
|
|
||||||
raise ValueError(
|
|
||||||
f"result length ({len(result)}) doesn't match value batch size"
|
|
||||||
f' ({batch_size})'
|
|
||||||
)
|
|
||||||
|
|
||||||
for i in range(batch_size):
|
|
||||||
value_i = jax.tree_map(lambda x, i=i: x[i], value)
|
|
||||||
device_get_into(result[i], value_i)
|
|
||||||
|
|
||||||
else:
|
|
||||||
if isinstance(result, mujoco.MjData):
|
|
||||||
ncon = value.contact.dist.shape[0]
|
|
||||||
nefc = value.efc_J.shape[0]
|
|
||||||
mujoco._functions._realloc_con_efc( # pylint: disable=protected-access
|
|
||||||
result, ncon=ncon, nefc=nefc
|
|
||||||
)
|
|
||||||
result.ncon = ncon
|
|
||||||
result.nefc = nefc
|
|
||||||
efc_start = nefc - ncon * 4
|
|
||||||
result.contact.efc_address[:] = np.arange(efc_start, nefc, 4)
|
|
||||||
result.contact.dim[:] = 3
|
|
||||||
|
|
||||||
for f in dataclasses.fields(value): # type: ignore
|
|
||||||
if (type(value), f.name) in _DERIVED:
|
|
||||||
continue
|
|
||||||
|
|
||||||
field_value = getattr(value, f.name)
|
|
||||||
|
|
||||||
if (type(value), f.name) in _INVERSE_TRANSFORMS:
|
|
||||||
field_value = _INVERSE_TRANSFORMS[(type(value), f.name)](field_value)
|
|
||||||
|
|
||||||
if type(field_value) in _TYPE_MAP.values():
|
|
||||||
device_get_into(getattr(result, f.name), field_value)
|
|
||||||
continue
|
|
||||||
|
|
||||||
try:
|
|
||||||
setattr(result, f.name, field_value)
|
|
||||||
except AttributeError:
|
|
||||||
getattr(result, f.name)[:] = field_value
|
|
||||||
@@ -1,203 +0,0 @@
|
|||||||
# Copyright 2023 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 moving mujoco structs on and off device."""
|
|
||||||
|
|
||||||
import dataclasses
|
|
||||||
|
|
||||||
from absl.testing import absltest
|
|
||||||
from absl.testing import parameterized
|
|
||||||
import jax
|
|
||||||
from jax import numpy as jp
|
|
||||||
import mujoco
|
|
||||||
from mujoco import mjx
|
|
||||||
from mujoco.mjx._src import device
|
|
||||||
from mujoco.mjx._src import test_util
|
|
||||||
from mujoco.mjx._src import types
|
|
||||||
# pylint: disable=g-importing-member
|
|
||||||
from mujoco.mjx._src.dataclasses import PyTreeNode
|
|
||||||
# pylint: enable=g-importing-member
|
|
||||||
import numpy as np
|
|
||||||
|
|
||||||
|
|
||||||
def _assert_eq(testcase, a, b, attr=None, name=None):
|
|
||||||
if (type(a), attr) in device._DERIVED:
|
|
||||||
return
|
|
||||||
|
|
||||||
if attr:
|
|
||||||
a, b = getattr(a, attr), getattr(b, attr)
|
|
||||||
|
|
||||||
if isinstance(a, PyTreeNode):
|
|
||||||
for field in dataclasses.fields(a):
|
|
||||||
_assert_eq(testcase, a, b, field.name, type(a).__name__)
|
|
||||||
return
|
|
||||||
|
|
||||||
typ = {'Model': types.Model, 'Data': types.Data,
|
|
||||||
'Contact': types.Contact}.get(name)
|
|
||||||
if (typ, attr) in device._TRANSFORMS:
|
|
||||||
b = device._TRANSFORMS[(typ, attr)](b)
|
|
||||||
|
|
||||||
err_msg = f'mismatch: {attr} in {name}'
|
|
||||||
if not hasattr(b, 'shape') or not b.shape:
|
|
||||||
testcase.assertEqual(a, b, err_msg)
|
|
||||||
return
|
|
||||||
|
|
||||||
a, b = np.array(a), np.array(b)
|
|
||||||
np.testing.assert_allclose(a, b, err_msg=err_msg, atol=1e-8)
|
|
||||||
|
|
||||||
|
|
||||||
class DeviceTest(parameterized.TestCase):
|
|
||||||
|
|
||||||
@parameterized.parameters('constraints.xml', 'pendula.xml')
|
|
||||||
def testdevice_put(self, fname):
|
|
||||||
"""Test putting MjData and MjModel on device."""
|
|
||||||
m = test_util.load_test_file(fname)
|
|
||||||
# advance state to ensure non-zero fields
|
|
||||||
d = mujoco.MjData(m)
|
|
||||||
for _ in range(10):
|
|
||||||
mujoco.mj_step(m, d)
|
|
||||||
|
|
||||||
_assert_eq(self, mjx.device_put(d), d)
|
|
||||||
_assert_eq(self, mjx.device_put(m), m)
|
|
||||||
|
|
||||||
@parameterized.parameters('constraints.xml', 'pendula.xml')
|
|
||||||
def testdevice_get(self, fname):
|
|
||||||
"""Test getting MjData from a device."""
|
|
||||||
m = test_util.load_test_file(fname)
|
|
||||||
m.opt.jacobian = mujoco.mjtJacobian.mjJAC_SPARSE # force sparse for testing
|
|
||||||
mx = device.device_put(m)
|
|
||||||
dx = mjx.make_data(mx)
|
|
||||||
d = mujoco.MjData(m)
|
|
||||||
device.device_get_into(d, dx)
|
|
||||||
_assert_eq(self, dx, d)
|
|
||||||
|
|
||||||
@parameterized.parameters('constraints.xml', 'pendula.xml')
|
|
||||||
def testdevice_get_batched(self, fname):
|
|
||||||
"""Test getting MjData from a device."""
|
|
||||||
m = test_util.load_test_file(fname)
|
|
||||||
m.opt.jacobian = mujoco.mjtJacobian.mjJAC_SPARSE # force sparse for testing
|
|
||||||
mx = device.device_put(m)
|
|
||||||
batch_size = 32
|
|
||||||
|
|
||||||
# create mjx_data and batch it
|
|
||||||
dx = mjx.make_data(mx)
|
|
||||||
dx = jax.tree_map(
|
|
||||||
lambda x: jp.repeat(x, batch_size).reshape((batch_size,) + x.shape),
|
|
||||||
dx,
|
|
||||||
)
|
|
||||||
ds = [mujoco.MjData(m) for _ in range(batch_size - 1)]
|
|
||||||
|
|
||||||
with self.assertRaises(ValueError):
|
|
||||||
device.device_get_into(ds, dx)
|
|
||||||
|
|
||||||
ds = [mujoco.MjData(m) for _ in range(batch_size)]
|
|
||||||
device.device_get_into(ds, dx)
|
|
||||||
dx = jax.device_get(dx) # faster indexing for testing
|
|
||||||
for i in range(batch_size):
|
|
||||||
_assert_eq(self, jax.tree_map(lambda x, i=i: x[i], dx), ds[i])
|
|
||||||
|
|
||||||
|
|
||||||
class ValidateInputTest(absltest.TestCase):
|
|
||||||
|
|
||||||
def test_solver(self):
|
|
||||||
m = mujoco.MjModel.from_xml_string(
|
|
||||||
'<mujoco><option solver="PGS"/><worldbody/></mujoco>'
|
|
||||||
)
|
|
||||||
with self.assertRaises(NotImplementedError):
|
|
||||||
mjx.device_put(m)
|
|
||||||
|
|
||||||
def test_integrator(self):
|
|
||||||
m = mujoco.MjModel.from_xml_string(
|
|
||||||
'<mujoco><option integrator="implicit"/><worldbody/></mujoco>'
|
|
||||||
)
|
|
||||||
with self.assertRaises(NotImplementedError):
|
|
||||||
mjx.device_put(m)
|
|
||||||
|
|
||||||
def test_cone(self):
|
|
||||||
m = mujoco.MjModel.from_xml_string(
|
|
||||||
'<mujoco><option cone="elliptic"/><worldbody/></mujoco>'
|
|
||||||
)
|
|
||||||
with self.assertRaises(NotImplementedError):
|
|
||||||
mjx.device_put(m)
|
|
||||||
|
|
||||||
def test_dyn(self):
|
|
||||||
m = test_util.load_test_file('pendula.xml')
|
|
||||||
m.actuator_dyntype[0] = mujoco.mjtDyn.mjDYN_MUSCLE
|
|
||||||
with self.assertRaises(NotImplementedError):
|
|
||||||
mjx.device_put(m)
|
|
||||||
|
|
||||||
def test_gain(self):
|
|
||||||
m = test_util.load_test_file('pendula.xml')
|
|
||||||
m.actuator_gaintype[0] = mujoco.mjtGain.mjGAIN_MUSCLE
|
|
||||||
with self.assertRaises(NotImplementedError):
|
|
||||||
mjx.device_put(m)
|
|
||||||
|
|
||||||
def test_bias(self):
|
|
||||||
m = test_util.load_test_file('pendula.xml')
|
|
||||||
m.actuator_gaintype[0] = mujoco.mjtGain.mjGAIN_MUSCLE
|
|
||||||
with self.assertRaises(NotImplementedError):
|
|
||||||
mjx.device_put(m)
|
|
||||||
|
|
||||||
def test_condim(self):
|
|
||||||
m = test_util.load_test_file('constraints.xml')
|
|
||||||
for i in [1, 4, 6]:
|
|
||||||
m.geom_condim[0] = i
|
|
||||||
with self.assertRaises(NotImplementedError):
|
|
||||||
mjx.device_put(m)
|
|
||||||
|
|
||||||
def test_geoms(self):
|
|
||||||
m = mujoco.MjModel.from_xml_string("""
|
|
||||||
<mujoco>
|
|
||||||
<worldbody>
|
|
||||||
<body>
|
|
||||||
<joint axis="1 0 0" type="free"/>
|
|
||||||
<geom size="0.2 0.2 0.2" type="box"/>
|
|
||||||
</body>
|
|
||||||
<body>
|
|
||||||
<joint axis="1 0 0" type="free"/>
|
|
||||||
<geom size="0.1 0.1" type="cylinder"/>
|
|
||||||
</body>
|
|
||||||
</worldbody>
|
|
||||||
</mujoco>
|
|
||||||
""")
|
|
||||||
with self.assertRaises(NotImplementedError):
|
|
||||||
mjx.device_put(m)
|
|
||||||
|
|
||||||
def test_tendon(self):
|
|
||||||
m = mujoco.MjModel.from_xml_string("""
|
|
||||||
<mujoco>
|
|
||||||
<worldbody>
|
|
||||||
<body name="left_thigh" pos="0 0.1 -0.04">
|
|
||||||
<joint axis="0 1 0" name="left_hip_y" type="hinge"/>
|
|
||||||
<geom fromto="0 0 0 0 -0.01 -.34" name="left_thigh1" size="0.06" type="capsule"/>
|
|
||||||
<body name="left_shin" pos="0 -0.01 -0.403">
|
|
||||||
<joint axis="0 -1 0" name="left_knee" pos="0 0 .02" range="-160 -2" type="hinge"/>
|
|
||||||
<geom fromto="0 0 0 0 0 -.3" name="left_shin1" size="0.049" type="capsule"/>
|
|
||||||
</body>
|
|
||||||
</body>
|
|
||||||
</worldbody>
|
|
||||||
<tendon>
|
|
||||||
<fixed name="left_hipknee">
|
|
||||||
<joint coef="-1" joint="left_hip_y"/>
|
|
||||||
<joint coef="1" joint="left_knee"/>
|
|
||||||
</fixed>
|
|
||||||
</tendon>
|
|
||||||
</mujoco>
|
|
||||||
""")
|
|
||||||
with self.assertRaises(NotImplementedError):
|
|
||||||
mjx.device_put(m)
|
|
||||||
|
|
||||||
|
|
||||||
if __name__ == '__main__':
|
|
||||||
absltest.main()
|
|
||||||
@@ -174,16 +174,17 @@ def fwd_actuation(m: Model, d: Data) -> Data:
|
|||||||
|
|
||||||
qfrc_actuator = d.actuator_moment.T @ force
|
qfrc_actuator = d.actuator_moment.T @ force
|
||||||
|
|
||||||
|
if m.ngravcomp:
|
||||||
|
# actuator-level gravity compensation, skip if added as passive force
|
||||||
|
qfrc_actuator += d.qfrc_gravcomp * m.jnt_actgravcomp[m.dof_jntid]
|
||||||
|
|
||||||
# clamp qfrc_actuator
|
# clamp qfrc_actuator
|
||||||
actfrcrange = jp.where(
|
actfrcrange = jp.where(
|
||||||
m.jnt_actfrclimited[:, None],
|
m.jnt_actfrclimited[:, None],
|
||||||
m.jnt_actfrcrange,
|
m.jnt_actfrcrange,
|
||||||
jp.array([-jp.inf, jp.inf]),
|
jp.array([-jp.inf, jp.inf]),
|
||||||
)
|
)
|
||||||
ids = sum(
|
actfrcrange = actfrcrange[m.dof_jntid]
|
||||||
([i] * JointType(j).dof_width() for i, j in enumerate(m.jnt_type)), []
|
|
||||||
)
|
|
||||||
actfrcrange = jp.take(actfrcrange, jp.array(ids), axis=0)
|
|
||||||
qfrc_actuator = jp.clip(qfrc_actuator, actfrcrange[:, 0], actfrcrange[:, 1])
|
qfrc_actuator = jp.clip(qfrc_actuator, actfrcrange[:, 0], actfrcrange[:, 1])
|
||||||
|
|
||||||
d = d.replace(act_dot=act_dot, qfrc_actuator=qfrc_actuator)
|
d = d.replace(act_dot=act_dot, qfrc_actuator=qfrc_actuator)
|
||||||
@@ -310,7 +311,7 @@ def rungekutta4(m: Model, d: Data) -> Data:
|
|||||||
|
|
||||||
kqvel = d.qvel # intermediate RK solution
|
kqvel = d.qvel # intermediate RK solution
|
||||||
# RK solutions sum
|
# RK solutions sum
|
||||||
qvel, qacc, act_dot = jax.tree_map(
|
qvel, qacc, act_dot = jax.tree_util.tree_map(
|
||||||
lambda k: B[0] * k, (kqvel, d.qacc, d.act_dot)
|
lambda k: B[0] * k, (kqvel, d.qacc, d.act_dot)
|
||||||
)
|
)
|
||||||
integrate_fn = lambda *args: _integrate_pos(*args, dt=m.opt.timestep)
|
integrate_fn = lambda *args: _integrate_pos(*args, dt=m.opt.timestep)
|
||||||
@@ -318,7 +319,7 @@ def rungekutta4(m: Model, d: Data) -> Data:
|
|||||||
def f(carry, x):
|
def f(carry, x):
|
||||||
qvel, qacc, act_dot, kqvel, d = carry
|
qvel, qacc, act_dot, kqvel, d = carry
|
||||||
a, b, t = x # tableau numbers
|
a, b, t = x # tableau numbers
|
||||||
dqvel, dqacc, dact_dot = jax.tree_map(
|
dqvel, dqacc, dact_dot = jax.tree_util.tree_map(
|
||||||
lambda k: a * k, (kqvel, d.qacc, d.act_dot)
|
lambda k: a * k, (kqvel, d.qacc, d.act_dot)
|
||||||
)
|
)
|
||||||
# get intermediate RK solutions
|
# get intermediate RK solutions
|
||||||
|
|||||||
+123
-134
@@ -22,15 +22,14 @@ from jax import numpy as jp
|
|||||||
import mujoco
|
import mujoco
|
||||||
from mujoco.mjx._src import collision_driver
|
from mujoco.mjx._src import collision_driver
|
||||||
from mujoco.mjx._src import constraint
|
from mujoco.mjx._src import constraint
|
||||||
from mujoco.mjx._src import mesh
|
|
||||||
from mujoco.mjx._src import support
|
from mujoco.mjx._src import support
|
||||||
from mujoco.mjx._src import types
|
from mujoco.mjx._src import types
|
||||||
import numpy as np
|
import numpy as np
|
||||||
import scipy
|
import scipy
|
||||||
|
|
||||||
|
|
||||||
def _put_option(o: mujoco.MjOption, device=None) -> types.Option:
|
def _make_option(o: mujoco.MjOption) -> types.Option:
|
||||||
"""Puts mujoco.MjOption onto a device, resulting in mjx.Option."""
|
"""Returns mjx.Option given mujoco.MjOption."""
|
||||||
if o.integrator not in set(types.IntegratorType):
|
if o.integrator not in set(types.IntegratorType):
|
||||||
raise NotImplementedError(f'{mujoco.mjtIntegrator(o.integrator)}')
|
raise NotImplementedError(f'{mujoco.mjtIntegrator(o.integrator)}')
|
||||||
|
|
||||||
@@ -47,38 +46,20 @@ def _put_option(o: mujoco.MjOption, device=None) -> types.Option:
|
|||||||
if o.enableflags & 2**i:
|
if o.enableflags & 2**i:
|
||||||
raise NotImplementedError(f'{mujoco.mjtEnableBit(2 ** i)}')
|
raise NotImplementedError(f'{mujoco.mjtEnableBit(2 ** i)}')
|
||||||
|
|
||||||
static_fields = {
|
fields = {f.name: getattr(o, f.name, None) for f in types.Option.fields()}
|
||||||
f.name: copy.copy(getattr(o, f.name))
|
fields['integrator'] = types.IntegratorType(o.integrator)
|
||||||
for f in types.Option.fields()
|
fields['cone'] = types.ConeType(o.cone)
|
||||||
if f.type in (int, bytes, np.ndarray)
|
fields['jacobian'] = types.JacobianType(o.jacobian)
|
||||||
}
|
fields['solver'] = types.SolverType(o.solver)
|
||||||
static_fields['integrator'] = types.IntegratorType(o.integrator)
|
fields['disableflags'] = types.DisableBit(o.disableflags)
|
||||||
static_fields['cone'] = types.ConeType(o.cone)
|
fields['has_fluid_params'] = o.density > 0 or o.viscosity > 0 or o.wind.any()
|
||||||
static_fields['jacobian'] = types.JacobianType(o.jacobian)
|
|
||||||
static_fields['solver'] = types.SolverType(o.solver)
|
|
||||||
static_fields['disableflags'] = types.DisableBit(o.disableflags)
|
|
||||||
|
|
||||||
device_fields = {
|
return types.Option(**fields)
|
||||||
f.name: copy.copy(getattr(o, f.name))
|
|
||||||
for f in types.Option.fields()
|
|
||||||
if f.type is jax.Array
|
|
||||||
}
|
|
||||||
device_fields = jax.device_put(device_fields, device=device)
|
|
||||||
|
|
||||||
has_fluid_params = o.density > 0 or o.viscosity > 0 or o.wind.any()
|
|
||||||
|
|
||||||
return types.Option(
|
|
||||||
has_fluid_params=has_fluid_params,
|
|
||||||
**static_fields,
|
|
||||||
**device_fields,
|
|
||||||
)
|
|
||||||
|
|
||||||
|
|
||||||
def _put_statistic(s: mujoco.MjStatistic, device=None) -> types.Statistic:
|
def _make_statistic(s: mujoco.MjStatistic) -> types.Statistic:
|
||||||
"""Puts mujoco.MjStatistic onto a device, resulting in mjx.Statistic."""
|
"""Puts mujoco.MjStatistic onto a device, resulting in mjx.Statistic."""
|
||||||
return types.Statistic(
|
return types.Statistic(meaninertia=s.meaninertia)
|
||||||
meaninertia=jax.device_put(s.meaninertia, device=device)
|
|
||||||
)
|
|
||||||
|
|
||||||
|
|
||||||
def put_model(m: mujoco.MjModel, device=None) -> types.Model:
|
def put_model(m: mujoco.MjModel, device=None) -> types.Model:
|
||||||
@@ -87,20 +68,21 @@ def put_model(m: mujoco.MjModel, device=None) -> types.Model:
|
|||||||
if m.ntendon:
|
if m.ntendon:
|
||||||
raise NotImplementedError('tendons are not supported')
|
raise NotImplementedError('tendons are not supported')
|
||||||
|
|
||||||
if (m.geom_condim != 3).any() or (m.pair_dim != 3).any():
|
for g1, g2, ip in collision_driver.geom_pairs(m):
|
||||||
raise NotImplementedError('only condim=3 is supported')
|
t1, t2 = m.geom_type[[g1, g2]]
|
||||||
|
# check collision function exists for type pair
|
||||||
# check collision geom types
|
if not collision_driver.has_collision_fn(t1, t2):
|
||||||
for (g1, g2, *_), c in collision_driver.collision_candidates(m).items():
|
t1, t2 = mujoco.mjtGeom(t1), mujoco.mjtGeom(t2)
|
||||||
g1, g2 = mujoco.mjtGeom(g1), mujoco.mjtGeom(g2)
|
raise NotImplementedError(f'({t1}, {t2}) collisions not implemented.')
|
||||||
if collision_driver.get_collision_fn((g1, g2)) is None:
|
# margin/gap not supported for geoms
|
||||||
raise NotImplementedError(f'({g1}, {g2}) has no collision function')
|
if mujoco.mjtGeom.mjGEOM_MESH in (t1, t2):
|
||||||
*_, params = collision_driver.get_params(m, c)
|
if ip != -1:
|
||||||
margin_gap = not np.allclose(np.concatenate([params.margin, params.gap]), 0)
|
margin = m.pair_margin[ip]
|
||||||
if mujoco.mjtGeom.mjGEOM_MESH in (g1, g2) and margin_gap:
|
else:
|
||||||
raise NotImplementedError(
|
margin = m.geom_margin[g1] + m.geom_margin[g2]
|
||||||
f'Margin and gap not implemented for ({g1}, {g2})'
|
if margin.any():
|
||||||
)
|
t1, t2 = mujoco.mjtGeom(t1), mujoco.mjtGeom(t2)
|
||||||
|
raise NotImplementedError(f'({t1}, {t2}) margin/gap not implemented.')
|
||||||
|
|
||||||
for enum_field, enum_type, mj_type in (
|
for enum_field, enum_type, mj_type in (
|
||||||
(m.actuator_biastype, types.BiasType, mujoco.mjtBias),
|
(m.actuator_biastype, types.BiasType, mujoco.mjtBias),
|
||||||
@@ -118,40 +100,24 @@ def put_model(m: mujoco.MjModel, device=None) -> types.Model:
|
|||||||
if not np.allclose(m.dof_frictionloss, 0):
|
if not np.allclose(m.dof_frictionloss, 0):
|
||||||
raise NotImplementedError('dof_frictionloss is not implemented.')
|
raise NotImplementedError('dof_frictionloss is not implemented.')
|
||||||
|
|
||||||
opt = _put_option(m.opt, device=device)
|
fields = {f.name: getattr(m, f.name) for f in types.Model.fields()}
|
||||||
stat = _put_statistic(m.stat, device=device)
|
fields['geom_rgba'] = fields['geom_rgba'].reshape((-1, 4))
|
||||||
|
fields['mat_rgba'] = fields['mat_rgba'].reshape((-1, 4))
|
||||||
|
fields['cam_mat0'] = fields['cam_mat0'].reshape((-1, 3, 3))
|
||||||
|
fields['opt'] = _make_option(m.opt)
|
||||||
|
fields['stat'] = _make_statistic(m.stat)
|
||||||
|
model = types.Model(**{k: copy.copy(v) for k, v in fields.items()})
|
||||||
|
|
||||||
static_fields = {
|
return jax.device_put(model, device=device)
|
||||||
f.name: getattr(m, f.name)
|
|
||||||
for f in types.Model.fields()
|
|
||||||
if f.type in (int, bytes, np.ndarray)
|
|
||||||
}
|
|
||||||
static_fields['geom_rgba'] = static_fields['geom_rgba'].reshape((-1, 4))
|
|
||||||
static_fields['mat_rgba'] = static_fields['mat_rgba'].reshape((-1, 4))
|
|
||||||
|
|
||||||
device_fields = {
|
|
||||||
f.name: copy.copy(getattr(m, f.name)) # copy because device_put is async
|
|
||||||
for f in types.Model.fields()
|
|
||||||
if f.type is jax.Array
|
|
||||||
}
|
|
||||||
device_fields['cam_mat0'] = device_fields['cam_mat0'].reshape((-1, 3, 3))
|
|
||||||
device_fields.update(mesh.get(m))
|
|
||||||
device_fields = jax.device_put(device_fields, device=device)
|
|
||||||
|
|
||||||
return types.Model(
|
|
||||||
opt=opt,
|
|
||||||
stat=stat,
|
|
||||||
**static_fields,
|
|
||||||
**device_fields,
|
|
||||||
)
|
|
||||||
|
|
||||||
|
|
||||||
def make_data(m: Union[types.Model, mujoco.MjModel]) -> types.Data:
|
def make_data(m: Union[types.Model, mujoco.MjModel]) -> types.Data:
|
||||||
"""Allocate and initialize Data."""
|
"""Allocate and initialize Data."""
|
||||||
|
dim = collision_driver.make_condim(m)
|
||||||
ncon = collision_driver.ncon(m)
|
efc_type = constraint.make_efc_type(m, dim)
|
||||||
ne, nf, nl, nc = constraint.count_constraints(m)
|
efc_address = constraint.make_efc_address(efc_type, dim)
|
||||||
nefc = ne + nf + nl + nc
|
ne, nf, nl, nc = constraint.counts(efc_type)
|
||||||
|
ncon, nefc = dim.size, ne + nf + nl + nc
|
||||||
|
|
||||||
zero_0 = jp.zeros(0, dtype=float)
|
zero_0 = jp.zeros(0, dtype=float)
|
||||||
zero_nv = jp.zeros(m.nv, dtype=float)
|
zero_nv = jp.zeros(m.nv, dtype=float)
|
||||||
@@ -167,8 +133,28 @@ def make_data(m: Union[types.Model, mujoco.MjModel]) -> types.Data:
|
|||||||
zero_njnt_3 = jp.zeros((m.njnt, 3), dtype=float)
|
zero_njnt_3 = jp.zeros((m.njnt, 3), dtype=float)
|
||||||
zero_nm = jp.zeros(m.nM, dtype=float)
|
zero_nm = jp.zeros(m.nM, dtype=float)
|
||||||
|
|
||||||
# create first d to get num contacts and nc
|
contact = types.Contact(
|
||||||
|
dist=jp.zeros(ncon),
|
||||||
|
pos=jp.zeros((ncon, 3)),
|
||||||
|
frame=jp.zeros((ncon, 3, 3)),
|
||||||
|
includemargin=jp.zeros(ncon),
|
||||||
|
friction=jp.zeros((ncon, 5)),
|
||||||
|
solref=jp.zeros((ncon, mujoco.mjNREF)),
|
||||||
|
solreffriction=jp.zeros((ncon, mujoco.mjNREF)),
|
||||||
|
solimp=jp.zeros((ncon, mujoco.mjNIMP)),
|
||||||
|
dim=dim,
|
||||||
|
geom1=jp.zeros(ncon, dtype=int) - 1,
|
||||||
|
geom2=jp.zeros(ncon, dtype=int) - 1,
|
||||||
|
geom=jp.zeros((ncon, 2), dtype=int) - 1,
|
||||||
|
efc_address=efc_address,
|
||||||
|
)
|
||||||
|
|
||||||
d = types.Data(
|
d = types.Data(
|
||||||
|
ne=ne,
|
||||||
|
nf=nf,
|
||||||
|
nl=nl,
|
||||||
|
nefc=nefc,
|
||||||
|
ncon=ncon,
|
||||||
solver_niter=jp.array(0, dtype=int),
|
solver_niter=jp.array(0, dtype=int),
|
||||||
time=jp.array(0.0),
|
time=jp.array(0.0),
|
||||||
qpos=jp.array(m.qpos0),
|
qpos=jp.array(m.qpos0),
|
||||||
@@ -203,7 +189,8 @@ def make_data(m: Union[types.Model, mujoco.MjModel]) -> types.Data:
|
|||||||
qM=zero_nm if support.is_sparse(m) else zero_nv_nv,
|
qM=zero_nm if support.is_sparse(m) else zero_nv_nv,
|
||||||
qLD=zero_nm if support.is_sparse(m) else zero_nv_nv,
|
qLD=zero_nm if support.is_sparse(m) else zero_nv_nv,
|
||||||
qLDiagInv=zero_nv if support.is_sparse(m) else zero_0,
|
qLDiagInv=zero_nv if support.is_sparse(m) else zero_0,
|
||||||
contact=types.Contact.zero(ncon),
|
contact=contact,
|
||||||
|
efc_type=efc_type,
|
||||||
efc_J=jp.zeros((nefc, m.nv), dtype=float),
|
efc_J=jp.zeros((nefc, m.nv), dtype=float),
|
||||||
efc_frictionloss=zero_nefc,
|
efc_frictionloss=zero_nefc,
|
||||||
efc_D=zero_nefc,
|
efc_D=zero_nefc,
|
||||||
@@ -211,6 +198,7 @@ def make_data(m: Union[types.Model, mujoco.MjModel]) -> types.Data:
|
|||||||
cvel=zero_nbody_6,
|
cvel=zero_nbody_6,
|
||||||
cdof_dot=zero_nv_6,
|
cdof_dot=zero_nv_6,
|
||||||
qfrc_bias=zero_nv,
|
qfrc_bias=zero_nv,
|
||||||
|
qfrc_gravcomp=zero_nv,
|
||||||
qfrc_passive=zero_nv,
|
qfrc_passive=zero_nv,
|
||||||
efc_aref=zero_nefc,
|
efc_aref=zero_nefc,
|
||||||
qfrc_actuator=zero_nv,
|
qfrc_actuator=zero_nv,
|
||||||
@@ -219,16 +207,13 @@ def make_data(m: Union[types.Model, mujoco.MjModel]) -> types.Data:
|
|||||||
qfrc_constraint=zero_nv,
|
qfrc_constraint=zero_nv,
|
||||||
qfrc_inverse=zero_nv,
|
qfrc_inverse=zero_nv,
|
||||||
efc_force=zero_nefc,
|
efc_force=zero_nefc,
|
||||||
|
userdata=jp.zeros(m.nuserdata, dtype=float),
|
||||||
)
|
)
|
||||||
|
|
||||||
return d
|
return d
|
||||||
|
|
||||||
|
|
||||||
def _get_contact(
|
def _get_contact(c: mujoco._structs._MjContactList, cx: types.Contact):
|
||||||
c: mujoco._structs._MjContactList,
|
|
||||||
cx: types.Contact,
|
|
||||||
efc_start: int,
|
|
||||||
):
|
|
||||||
"""Converts mjx.Contact to mujoco._structs._MjContactList."""
|
"""Converts mjx.Contact to mujoco._structs._MjContactList."""
|
||||||
con_id = np.nonzero(cx.dist <= 0)[0]
|
con_id = np.nonzero(cx.dist <= 0)[0]
|
||||||
for field in types.Contact.fields():
|
for field in types.Contact.fields():
|
||||||
@@ -237,10 +222,6 @@ def _get_contact(
|
|||||||
value = value.reshape((-1, 9))
|
value = value.reshape((-1, 9))
|
||||||
getattr(c, field.name)[:] = value
|
getattr(c, field.name)[:] = value
|
||||||
|
|
||||||
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(
|
def get_data(
|
||||||
m: mujoco.MjModel, d: types.Data
|
m: mujoco.MjModel, d: types.Data
|
||||||
@@ -274,13 +255,6 @@ def get_data_into(
|
|||||||
d = jax.device_get(d)
|
d = jax.device_get(d)
|
||||||
|
|
||||||
batch_size = d.qpos.shape[0] if batched else 1
|
batch_size = d.qpos.shape[0] if batched else 1
|
||||||
ne, nf, nl, nc = constraint.count_constraints(m, d)
|
|
||||||
efc_type = np.array([
|
|
||||||
mujoco.mjtConstraint.mjCNSTR_EQUALITY,
|
|
||||||
mujoco.mjtConstraint.mjCNSTR_FRICTION_DOF,
|
|
||||||
mujoco.mjtConstraint.mjCNSTR_LIMIT_JOINT,
|
|
||||||
mujoco.mjtConstraint.mjCNSTR_CONTACT_PYRAMIDAL,
|
|
||||||
]).repeat([ne, nf, nl, nc])
|
|
||||||
|
|
||||||
dof_i, dof_j = [], []
|
dof_i, dof_j = [], []
|
||||||
for i in range(m.nv):
|
for i in range(m.nv):
|
||||||
@@ -291,12 +265,11 @@ def get_data_into(
|
|||||||
j = m.dof_parentid[j]
|
j = m.dof_parentid[j]
|
||||||
|
|
||||||
for i in range(batch_size):
|
for i in range(batch_size):
|
||||||
d_i = jax.tree_map(lambda x, i=i: x[i], d) if batched else d
|
d_i = jax.tree_util.tree_map(lambda x, i=i: x[i], d) if batched else d
|
||||||
result_i = result[i] if batched else result
|
result_i = result[i] if batched else result
|
||||||
ncon = (d_i.contact.dist <= 0).sum()
|
ncon = (d_i.contact.dist <= 0).sum()
|
||||||
efc_active = (d_i.efc_J != 0).any(axis=1)
|
efc_active = (d_i.efc_J != 0).any(axis=1)
|
||||||
efc_con = efc_type == mujoco.mjtConstraint.mjCNSTR_CONTACT_PYRAMIDAL
|
nefc = int(efc_active.sum())
|
||||||
nefc, nc = int(efc_active.sum()), int((efc_active & efc_con).sum())
|
|
||||||
result_i.nnzJ = nefc * m.nv
|
result_i.nnzJ = nefc * m.nv
|
||||||
if ncon != result_i.ncon or nefc != result_i.nefc:
|
if ncon != result_i.ncon or nefc != result_i.nefc:
|
||||||
mujoco._functions._realloc_con_efc(result_i, ncon=ncon, nefc=nefc) # pylint: disable=protected-access
|
mujoco._functions._realloc_con_efc(result_i, ncon=ncon, nefc=nefc) # pylint: disable=protected-access
|
||||||
@@ -306,61 +279,77 @@ def get_data_into(
|
|||||||
|
|
||||||
for field in types.Data.fields():
|
for field in types.Data.fields():
|
||||||
if field.name == 'contact':
|
if field.name == 'contact':
|
||||||
_get_contact(result_i.contact, d_i.contact, nefc - nc)
|
_get_contact(result_i.contact, d_i.contact)
|
||||||
|
# efc_address must be updated because rows were deleted above:
|
||||||
|
efc_map = np.cumsum(efc_active) - 1
|
||||||
|
result_i.contact.efc_address[:] = efc_map[result_i.contact.efc_address]
|
||||||
continue
|
continue
|
||||||
|
|
||||||
value = getattr(d_i, field.name)
|
value = getattr(d_i, field.name)
|
||||||
|
|
||||||
if field.name in ('xmat', 'ximat', 'geom_xmat', 'site_xmat', 'cam_xmat'):
|
if field.name in ('nefc', 'ncon'):
|
||||||
|
value = {'nefc': nefc, 'ncon': ncon}[field.name]
|
||||||
|
elif field.name.endswith('xmat') or field.name == 'ximat':
|
||||||
value = value.reshape((-1, 9))
|
value = value.reshape((-1, 9))
|
||||||
|
elif field.name.startswith('efc_'):
|
||||||
if field.name in ('efc_frictionloss', 'efc_D', 'efc_aref', 'efc_force'):
|
|
||||||
value = value[efc_active]
|
value = value[efc_active]
|
||||||
|
|
||||||
if field.name == 'efc_J':
|
if field.name == 'efc_J':
|
||||||
value = value[efc_active].reshape(-1)
|
value = value.reshape(-1)
|
||||||
|
elif field.name == 'qM' and not support.is_sparse(m):
|
||||||
if field.name == 'qM' and not support.is_sparse(m):
|
|
||||||
value = value[dof_i, dof_j]
|
value = value[dof_i, dof_j]
|
||||||
|
elif field.name == 'qLD' and not support.is_sparse(m):
|
||||||
if field.name == 'qLD' and not support.is_sparse(m):
|
|
||||||
value = value[dof_i, dof_j]
|
value = value[dof_i, dof_j]
|
||||||
|
elif field.name == 'qLDiagInv' and not support.is_sparse(m):
|
||||||
if field.name == 'qLDiagInv' and not support.is_sparse(m):
|
|
||||||
value = np.ones(m.nv)
|
value = np.ones(m.nv)
|
||||||
|
|
||||||
if value.shape:
|
if isinstance(value, np.ndarray) and value.shape:
|
||||||
getattr(result_i, field.name)[:] = value
|
getattr(result_i, field.name)[:] = value
|
||||||
else:
|
else:
|
||||||
setattr(result_i, field.name, value)
|
setattr(result_i, field.name, value)
|
||||||
|
|
||||||
result_i.efc_type[:] = efc_type[efc_active]
|
|
||||||
|
|
||||||
|
def _make_contact(
|
||||||
def _put_contact(
|
c: mujoco._structs._MjContactList,
|
||||||
c: mujoco._structs._MjContactList, ncon: int, device=None
|
dim: np.ndarray,
|
||||||
|
efc_address: np.ndarray,
|
||||||
) -> types.Contact:
|
) -> types.Contact:
|
||||||
"""Puts mujoco.structs._MjContactList onto a device, resulting in mjx.Contact."""
|
"""Converts mujoco.structs._MjContactList into mjx.Contact."""
|
||||||
fields = {
|
fields = {f.name: getattr(c, f.name) for f in types.Contact.fields()}
|
||||||
f.name: copy.copy(getattr(c, f.name)) for f in types.Contact.fields()
|
|
||||||
}
|
|
||||||
fields['frame'] = fields['frame'].reshape((-1, 3, 3))
|
fields['frame'] = fields['frame'].reshape((-1, 3, 3))
|
||||||
pad_size = ncon - c.dist.shape[0]
|
# reorder contacts so that their condims match those specified in dim.
|
||||||
pad_fn = lambda x: np.concatenate(
|
# if we have fewer Contacts for a condim range, pad the range with zeros
|
||||||
(x, np.zeros((pad_size,) + x.shape[1:], dtype=x.dtype))
|
|
||||||
|
# build a map for where to find a dim-matching contact, or -1 if none
|
||||||
|
contact_map = np.zeros_like(dim) - 1
|
||||||
|
for i, di in enumerate(fields['dim']):
|
||||||
|
space = [j for j, dj in enumerate(dim) if di == dj and contact_map[j] == -1]
|
||||||
|
if not space:
|
||||||
|
# this can happen if max_geom_pairs or max_contact_points is too low
|
||||||
|
raise ValueError(f'unable to place Contact[{i}], no space in condim {di}')
|
||||||
|
contact_map[space[0]] = i
|
||||||
|
|
||||||
|
if contact_map.size > 0:
|
||||||
|
# reorganize contact according, with a zero contact at the end for -1
|
||||||
|
zero = jax.tree_util.tree_map(
|
||||||
|
lambda x: np.zeros((1,) + x.shape[1:], dtype=x.dtype), fields
|
||||||
)
|
)
|
||||||
fields = jax.tree_map(pad_fn, fields)
|
zero['dist'][:] = np.finfo(float).max
|
||||||
fields['dist'][-pad_size:] = np.inf
|
fields = jax.tree_util.tree_map(lambda *x: np.concatenate(x), fields, zero)
|
||||||
fields = jax.device_put(fields, device=device)
|
fields = jax.tree_util.tree_map(lambda x: x[contact_map], fields)
|
||||||
|
|
||||||
|
fields['dim'] = dim
|
||||||
|
fields['efc_address'] = efc_address
|
||||||
|
|
||||||
return types.Contact(**fields)
|
return types.Contact(**fields)
|
||||||
|
|
||||||
|
|
||||||
def put_data(m: mujoco.MjModel, d: mujoco.MjData, device=None) -> types.Data:
|
def put_data(m: mujoco.MjModel, d: mujoco.MjData, device=None) -> types.Data:
|
||||||
"""Puts mujoco.MjData onto a device, resulting in mjx.Data."""
|
"""Puts mujoco.MjData onto a device, resulting in mjx.Data."""
|
||||||
ncon = collision_driver.ncon(m)
|
dim = collision_driver.make_condim(m)
|
||||||
ne, nf, nl, nc = constraint.count_constraints(m)
|
efc_type = constraint.make_efc_type(m, dim)
|
||||||
nefc = ne + nf + nl + nc
|
efc_address = constraint.make_efc_address(efc_type, dim)
|
||||||
|
ne, nf, nl, nc = constraint.counts(efc_type)
|
||||||
|
ncon, nefc = dim.size, ne + nf + nl + nc
|
||||||
|
|
||||||
for d_val, val, name in (
|
for d_val, val, name in (
|
||||||
(d.ncon, ncon, 'ncon'),
|
(d.ncon, ncon, 'ncon'),
|
||||||
@@ -372,12 +361,9 @@ def put_data(m: mujoco.MjModel, d: mujoco.MjData, device=None) -> types.Data:
|
|||||||
if d_val > val:
|
if d_val > val:
|
||||||
raise ValueError(f'd.{name} too high, d.{name} = {d_val}, model = {val}')
|
raise ValueError(f'd.{name} too high, d.{name} = {d_val}, model = {val}')
|
||||||
|
|
||||||
fields = {
|
fields = {f.name: getattr(d, f.name) for f in types.Data.fields()}
|
||||||
f.name: copy.copy(getattr(d, f.name)) # copy because device_put is async
|
|
||||||
for f in types.Data.fields()
|
|
||||||
if f.type is jax.Array
|
|
||||||
}
|
|
||||||
|
|
||||||
|
# MJX prefers square matrices for these fields:
|
||||||
for fname in ('xmat', 'ximat', 'geom_xmat', 'site_xmat', 'cam_xmat'):
|
for fname in ('xmat', 'ximat', 'geom_xmat', 'site_xmat', 'cam_xmat'):
|
||||||
fields[fname] = fields[fname].reshape((-1, 3, 3))
|
fields[fname] = fields[fname].reshape((-1, 3, 3))
|
||||||
|
|
||||||
@@ -405,7 +391,7 @@ def put_data(m: mujoco.MjModel, d: mujoco.MjData, device=None) -> types.Data:
|
|||||||
value_beg = sum([ne, nf, nl][:i])
|
value_beg = sum([ne, nf, nl][:i])
|
||||||
d_beg = sum([d.ne, d.nf, d.nl][:i])
|
d_beg = sum([d.ne, d.nf, d.nl][:i])
|
||||||
size = [d.ne, d.nf, d.nl, d.nefc - d.nl - d.nf - d.ne][i]
|
size = [d.ne, d.nf, d.nl, d.nefc - d.nl - d.nf - d.ne][i]
|
||||||
value[value_beg:value_beg+size] = fields[fname][d_beg:d_beg+size]
|
value[value_beg : value_beg + size] = fields[fname][d_beg : d_beg + size]
|
||||||
fields[fname] = value
|
fields[fname] = value
|
||||||
|
|
||||||
# convert qM and qLD if jacobian is dense
|
# convert qM and qLD if jacobian is dense
|
||||||
@@ -420,7 +406,10 @@ def put_data(m: mujoco.MjModel, d: mujoco.MjData, device=None) -> types.Data:
|
|||||||
fields['qLD'] = np.zeros((m.nv, m.nv))
|
fields['qLD'] = np.zeros((m.nv, m.nv))
|
||||||
fields['qLDiagInv'] = np.zeros(0)
|
fields['qLDiagInv'] = np.zeros(0)
|
||||||
|
|
||||||
fields = jax.device_put(fields, device=device)
|
fields['contact'] = _make_contact(d.contact, dim, efc_address)
|
||||||
fields['contact'] = _put_contact(d.contact, ncon, device=device)
|
fields.update(ne=ne, nf=nf, nl=nl, nefc=nefc, ncon=ncon, efc_type=efc_type)
|
||||||
|
|
||||||
return types.Data(**fields)
|
# copy because device_put is async:
|
||||||
|
data = types.Data(**{k: copy.copy(v) for k, v in fields.items()})
|
||||||
|
|
||||||
|
return jax.device_put(data, device=device)
|
||||||
|
|||||||
@@ -63,7 +63,7 @@ _MULTIPLE_CONVEX_OBJECTS = """
|
|||||||
_MULTIPLE_CONSTRAINTS = """
|
_MULTIPLE_CONSTRAINTS = """
|
||||||
<mujoco>
|
<mujoco>
|
||||||
<worldbody>
|
<worldbody>
|
||||||
<geom type="plane" size="3 3 .01"/>
|
<geom type="plane" size="3 3 .01" condim="6"/>
|
||||||
<body name="cap1" pos="-.3 -.3 .2">
|
<body name="cap1" pos="-.3 -.3 .2">
|
||||||
<freejoint/>
|
<freejoint/>
|
||||||
<geom type="capsule" size=".2 .05"/>
|
<geom type="capsule" size=".2 .05"/>
|
||||||
@@ -108,10 +108,6 @@ class ModelIOTest(parameterized.TestCase):
|
|||||||
np.testing.assert_allclose(mx.geom_bodyid, m.geom_bodyid)
|
np.testing.assert_allclose(mx.geom_bodyid, m.geom_bodyid)
|
||||||
np.testing.assert_almost_equal(mx.geom_solref, m.geom_solref)
|
np.testing.assert_almost_equal(mx.geom_solref, m.geom_solref)
|
||||||
np.testing.assert_almost_equal(mx.geom_pos, m.geom_pos)
|
np.testing.assert_almost_equal(mx.geom_pos, m.geom_pos)
|
||||||
self.assertLen(mx.geom_convex_face, 6)
|
|
||||||
self.assertLen(mx.geom_convex_vert, 6)
|
|
||||||
self.assertLen(mx.geom_convex_edge_dir, 6)
|
|
||||||
self.assertLen(mx.geom_convex_facenormal, 6)
|
|
||||||
|
|
||||||
np.testing.assert_allclose(mx.jnt_type, m.jnt_type)
|
np.testing.assert_allclose(mx.jnt_type, m.jnt_type)
|
||||||
np.testing.assert_allclose(mx.jnt_dofadr, m.jnt_dofadr)
|
np.testing.assert_allclose(mx.jnt_dofadr, m.jnt_dofadr)
|
||||||
@@ -177,22 +173,6 @@ class ModelIOTest(parameterized.TestCase):
|
|||||||
</tendon>
|
</tendon>
|
||||||
</mujoco>"""))
|
</mujoco>"""))
|
||||||
|
|
||||||
def test_condim_not_implemented(self):
|
|
||||||
with self.assertRaises(NotImplementedError):
|
|
||||||
mjx.put_model(mujoco.MjModel.from_xml_string("""
|
|
||||||
<mujoco>
|
|
||||||
<worldbody>
|
|
||||||
<body>
|
|
||||||
<freejoint/>
|
|
||||||
<geom size="0.05" condim="1"/>
|
|
||||||
</body>
|
|
||||||
<body>
|
|
||||||
<freejoint/>
|
|
||||||
<geom size="0.05" condim="1"/>
|
|
||||||
</body>
|
|
||||||
</worldbody>
|
|
||||||
</mujoco>"""))
|
|
||||||
|
|
||||||
def test_cylinder_not_implemented(self):
|
def test_cylinder_not_implemented(self):
|
||||||
with self.assertRaises(NotImplementedError):
|
with self.assertRaises(NotImplementedError):
|
||||||
mjx.put_model(mujoco.MjModel.from_xml_string("""
|
mjx.put_model(mujoco.MjModel.from_xml_string("""
|
||||||
@@ -244,6 +224,7 @@ class DataIOTest(parameterized.TestCase):
|
|||||||
nv = 19
|
nv = 19
|
||||||
nefc = 185
|
nefc = 185
|
||||||
|
|
||||||
|
self.assertEqual(d.nefc, nefc)
|
||||||
self.assertEqual(d.qpos.shape, (nq,))
|
self.assertEqual(d.qpos.shape, (nq,))
|
||||||
self.assertEqual(d.qvel.shape, (nv,))
|
self.assertEqual(d.qvel.shape, (nv,))
|
||||||
self.assertEqual(d.act.shape, (0,))
|
self.assertEqual(d.act.shape, (0,))
|
||||||
@@ -336,21 +317,21 @@ class DataIOTest(parameterized.TestCase):
|
|||||||
np.testing.assert_allclose(dx.site_xmat.reshape((1, 9)), d.site_xmat)
|
np.testing.assert_allclose(dx.site_xmat.reshape((1, 9)), d.site_xmat)
|
||||||
|
|
||||||
# efc_ are also shape transformed and padded
|
# efc_ are also shape transformed and padded
|
||||||
self.assertEqual(dx.efc_J.shape, (21, 8)) # nefc, nv
|
self.assertEqual(dx.efc_J.shape, (45, 8)) # nefc, nv
|
||||||
d_efc_j = d.efc_J.reshape((-1, 8))
|
d_efc_j = d.efc_J.reshape((-1, 8))
|
||||||
np.testing.assert_allclose(dx.efc_J[:3], d_efc_j[:3]) # connect eq
|
np.testing.assert_allclose(dx.efc_J[:3], d_efc_j[:3]) # connect eq
|
||||||
np.testing.assert_allclose(dx.efc_J[3], d_efc_j[3]) # one active limit
|
np.testing.assert_allclose(dx.efc_J[3], d_efc_j[3]) # one active limit
|
||||||
np.testing.assert_allclose(dx.efc_J[4], 0) # one inactive limit
|
np.testing.assert_allclose(dx.efc_J[4], 0) # one inactive limit
|
||||||
np.testing.assert_allclose(dx.efc_J[5:9], d_efc_j[4:8]) # contact
|
np.testing.assert_allclose(dx.efc_J[5:15], d_efc_j[4:14]) # contact
|
||||||
np.testing.assert_allclose(dx.efc_J[9:], 0) # no contact
|
np.testing.assert_allclose(dx.efc_J[15:], 0) # no contact
|
||||||
|
|
||||||
# check another efc_ too
|
# check another efc_ too
|
||||||
self.assertEqual(dx.efc_aref.shape, (21,)) # nefc
|
self.assertEqual(dx.efc_aref.shape, (45,)) # nefc
|
||||||
np.testing.assert_allclose(dx.efc_aref[:3], d.efc_aref[:3])
|
np.testing.assert_allclose(dx.efc_aref[:3], d.efc_aref[:3])
|
||||||
np.testing.assert_allclose(dx.efc_aref[3], d.efc_aref[3])
|
np.testing.assert_allclose(dx.efc_aref[3], d.efc_aref[3])
|
||||||
np.testing.assert_allclose(dx.efc_aref[4], 0)
|
np.testing.assert_allclose(dx.efc_aref[4], 0)
|
||||||
np.testing.assert_allclose(dx.efc_aref[5:9], d.efc_aref[4:8])
|
np.testing.assert_allclose(dx.efc_aref[5:15], d.efc_aref[4:14])
|
||||||
np.testing.assert_allclose(dx.efc_aref[9:], 0)
|
np.testing.assert_allclose(dx.efc_aref[15:], 0)
|
||||||
|
|
||||||
# check sparse transform is correct
|
# check sparse transform is correct
|
||||||
m.opt.jacobian = mujoco.mjtJacobian.mjJAC_SPARSE
|
m.opt.jacobian = mujoco.mjtJacobian.mjJAC_SPARSE
|
||||||
@@ -407,12 +388,11 @@ class DataIOTest(parameterized.TestCase):
|
|||||||
np.testing.assert_allclose(d_2.site_xmat, d.site_xmat)
|
np.testing.assert_allclose(d_2.site_xmat, d.site_xmat)
|
||||||
|
|
||||||
# efc_* are also shape transformed and filtered
|
# efc_* are also shape transformed and filtered
|
||||||
self.assertEqual(d_2.efc_J.shape, (64,)) # nefc * nv
|
self.assertEqual(d_2.nefc, 14)
|
||||||
|
self.assertEqual(d_2.efc_J.shape, (112,)) # nefc * nv
|
||||||
np.testing.assert_allclose(d_2.efc_J, d.efc_J)
|
np.testing.assert_allclose(d_2.efc_J, d.efc_J)
|
||||||
self.assertEqual(d_2.efc_aref.shape, (8,)) # nefc
|
self.assertEqual(d_2.efc_aref.shape, (14,)) # nefc
|
||||||
np.testing.assert_allclose(d_2.efc_aref, d.efc_aref)
|
np.testing.assert_allclose(d_2.efc_aref, d.efc_aref)
|
||||||
|
|
||||||
# efc_address is created on demand
|
|
||||||
np.testing.assert_allclose(d_2.contact.efc_address, d.contact.efc_address)
|
np.testing.assert_allclose(d_2.contact.efc_address, d.contact.efc_address)
|
||||||
|
|
||||||
def test_get_data_batched(self):
|
def test_get_data_batched(self):
|
||||||
@@ -423,7 +403,7 @@ class DataIOTest(parameterized.TestCase):
|
|||||||
mujoco.mj_step(m, d, 2)
|
mujoco.mj_step(m, d, 2)
|
||||||
dx = mjx.put_data(m, d)
|
dx = mjx.put_data(m, d)
|
||||||
# second data in batch has contact dist > 0, disables contact
|
# second data in batch has contact dist > 0, disables contact
|
||||||
dx_b = jax.tree_map(lambda x: jp.stack((x, x + 0.05)), dx)
|
dx_b = jax.tree_util.tree_map(lambda x: jp.stack((x, x + 0.05)), dx)
|
||||||
ds = mjx.get_data(m, dx_b)
|
ds = mjx.get_data(m, dx_b)
|
||||||
self.assertLen(ds, 2)
|
self.assertLen(ds, 2)
|
||||||
np.testing.assert_allclose(ds[0].qpos, d.qpos)
|
np.testing.assert_allclose(ds[0].qpos, d.qpos)
|
||||||
|
|||||||
@@ -245,6 +245,11 @@ def inert_mul(i: jax.Array, v: jax.Array) -> jax.Array:
|
|||||||
return jp.concatenate((ang, vel))
|
return jp.concatenate((ang, vel))
|
||||||
|
|
||||||
|
|
||||||
|
def sign(x: jax.Array) -> jax.Array:
|
||||||
|
"""Returns the sign of x in the set {-1, 1}."""
|
||||||
|
return jp.where(x < 0, -1, 1)
|
||||||
|
|
||||||
|
|
||||||
def transform_motion(vel: jax.Array, offset: jax.Array, rotmat: jax.Array):
|
def transform_motion(vel: jax.Array, offset: jax.Array, rotmat: jax.Array):
|
||||||
"""Transform spatial motion.
|
"""Transform spatial motion.
|
||||||
|
|
||||||
|
|||||||
+91
-134
@@ -15,14 +15,15 @@
|
|||||||
"""Mesh processing."""
|
"""Mesh processing."""
|
||||||
|
|
||||||
import collections
|
import collections
|
||||||
import dataclasses
|
|
||||||
import itertools
|
import itertools
|
||||||
from typing import Dict, List, Optional, Sequence, Tuple
|
from typing import Tuple
|
||||||
import warnings
|
import warnings
|
||||||
|
|
||||||
import mujoco
|
import jax
|
||||||
|
from jax import numpy as jp
|
||||||
# pylint: disable=g-importing-member
|
# pylint: disable=g-importing-member
|
||||||
from mujoco.mjx._src.types import GeomType
|
from mujoco.mjx._src.collision_types import ConvexInfo
|
||||||
|
from mujoco.mjx._src.collision_types import GeomInfo
|
||||||
from mujoco.mjx._src.types import Model
|
from mujoco.mjx._src.types import Model
|
||||||
# pylint: enable=g-importing-member
|
# pylint: enable=g-importing-member
|
||||||
import numpy as np
|
import numpy as np
|
||||||
@@ -30,37 +31,7 @@ from scipy import spatial
|
|||||||
import trimesh
|
import trimesh
|
||||||
|
|
||||||
|
|
||||||
_BOX_CORNERS = list(itertools.product((-1, 1), (-1, 1), (-1, 1)))
|
|
||||||
# pyformat: disable
|
|
||||||
# Rectangular box faces using a counter-clockwise winding order convention.
|
|
||||||
_BOX_FACES = [
|
|
||||||
0, 4, 5, 1, # left
|
|
||||||
0, 2, 6, 4, # bottom
|
|
||||||
6, 7, 5, 4, # front
|
|
||||||
2, 3, 7, 6, # right
|
|
||||||
1, 5, 7, 3, # top
|
|
||||||
0, 1, 3, 2, # back
|
|
||||||
]
|
|
||||||
# pyformat: enable
|
|
||||||
_MAX_HULL_FACE_VERTICES = 20
|
_MAX_HULL_FACE_VERTICES = 20
|
||||||
_CONVEX_CACHE: Dict[Tuple[int, int], Dict[str, np.ndarray]] = {}
|
|
||||||
_DERIVED_ARGS = [
|
|
||||||
'geom_convex_face',
|
|
||||||
'geom_convex_vert',
|
|
||||||
'geom_convex_edge_dir',
|
|
||||||
'geom_convex_facenormal',
|
|
||||||
'geom_convex_edge',
|
|
||||||
'geom_convex_edge_face_normal',
|
|
||||||
]
|
|
||||||
DERIVED = {(Model, d) for d in _DERIVED_ARGS}
|
|
||||||
|
|
||||||
|
|
||||||
def _box(size: np.ndarray):
|
|
||||||
"""Creates a mesh for a box with rectangular faces."""
|
|
||||||
box_corners = np.array(_BOX_CORNERS)
|
|
||||||
vert = box_corners * size.reshape(-1, 3)
|
|
||||||
face = np.array([_BOX_FACES]).reshape(-1, 4)
|
|
||||||
return vert, face
|
|
||||||
|
|
||||||
|
|
||||||
def _get_face_norm(vert: np.ndarray, face: np.ndarray) -> np.ndarray:
|
def _get_face_norm(vert: np.ndarray, face: np.ndarray) -> np.ndarray:
|
||||||
@@ -170,16 +141,7 @@ def _convex_hull_2d(points: np.ndarray, normal: np.ndarray) -> np.ndarray:
|
|||||||
return hull_point_idx
|
return hull_point_idx
|
||||||
|
|
||||||
|
|
||||||
@dataclasses.dataclass
|
def _merge_coplanar(m: Model, tm: trimesh.Trimesh, meshid: int) -> np.ndarray:
|
||||||
class MeshInfo:
|
|
||||||
name: str
|
|
||||||
vert: np.ndarray
|
|
||||||
face: np.ndarray
|
|
||||||
convex_vert: Optional[np.ndarray]
|
|
||||||
convex_face: Optional[np.ndarray]
|
|
||||||
|
|
||||||
|
|
||||||
def _merge_coplanar(tm: trimesh.Trimesh, mesh_info: MeshInfo) -> np.ndarray:
|
|
||||||
"""Merges coplanar facets."""
|
"""Merges coplanar facets."""
|
||||||
if not tm.facets:
|
if not tm.facets:
|
||||||
return tm.faces.copy() # no facets
|
return tm.faces.copy() # no facets
|
||||||
@@ -204,9 +166,11 @@ def _merge_coplanar(tm: trimesh.Trimesh, mesh_info: MeshInfo) -> np.ndarray:
|
|||||||
|
|
||||||
# resize faces that exceed max polygon vertices
|
# resize faces that exceed max polygon vertices
|
||||||
if face.shape[0] > _MAX_HULL_FACE_VERTICES:
|
if face.shape[0] > _MAX_HULL_FACE_VERTICES:
|
||||||
|
name = m.names[m.name_meshadr[meshid]:]
|
||||||
|
name = name[:name.find(b'\x00')].decode('utf-8')
|
||||||
warnings.warn(
|
warnings.warn(
|
||||||
f'Mesh "{mesh_info.name}" has a coplanar face with more than'
|
f'Mesh "{name}" has a coplanar face with more than '
|
||||||
f' {_MAX_HULL_FACE_VERTICES} vertices. This may lead to performance '
|
f'{_MAX_HULL_FACE_VERTICES} vertices. This may lead to performance '
|
||||||
'issues and inaccuracies in collision detection. Consider '
|
'issues and inaccuracies in collision detection. Consider '
|
||||||
'decimating the mesh.'
|
'decimating the mesh.'
|
||||||
)
|
)
|
||||||
@@ -231,106 +195,99 @@ def _merge_coplanar(tm: trimesh.Trimesh, mesh_info: MeshInfo) -> np.ndarray:
|
|||||||
return np.concatenate([faces, facets])
|
return np.concatenate([faces, facets])
|
||||||
|
|
||||||
|
|
||||||
def _mesh_info(
|
def box(info: GeomInfo) -> ConvexInfo:
|
||||||
m: mujoco.MjModel,
|
"""Creates a box with rectangular faces."""
|
||||||
) -> List[MeshInfo]:
|
vert = np.array(
|
||||||
"""Extracts mesh info from MjModel."""
|
list(itertools.product((-1, 1), (-1, 1), (-1, 1))), dtype=float
|
||||||
mesh_infos = []
|
)
|
||||||
for i in range(m.nmesh):
|
# pyformat: disable
|
||||||
name = mujoco.mj_id2name(m, mujoco.mjtObj.mjOBJ_MESH.value, i)
|
# rectangular box faces using a counter-clockwise winding order convention:
|
||||||
|
face = np.array(
|
||||||
|
[
|
||||||
|
0, 4, 5, 1, # left
|
||||||
|
0, 2, 6, 4, # bottom
|
||||||
|
6, 7, 5, 4, # front
|
||||||
|
2, 3, 7, 6, # right
|
||||||
|
1, 5, 7, 3, # top
|
||||||
|
0, 1, 3, 2, # back
|
||||||
|
]
|
||||||
|
).reshape((-1, 4))
|
||||||
|
# pyformat: enable
|
||||||
|
face_normal = _get_face_norm(vert, face)
|
||||||
|
edge, edge_face_normal = _get_edge_normals(face, face_normal)
|
||||||
|
edge_dir = _get_unique_edge_dir(vert, face)
|
||||||
|
face = vert[face] # materialize full nface x nvert matrix
|
||||||
|
|
||||||
last = (i + 1) >= m.nmesh
|
c = ConvexInfo(
|
||||||
face_start = m.mesh_faceadr[i]
|
info.pos,
|
||||||
face_end = m.mesh_faceadr[i + 1] if not last else m.mesh_face.shape[0]
|
info.mat,
|
||||||
face = m.mesh_face[face_start:face_end]
|
vert,
|
||||||
|
face,
|
||||||
|
face_normal,
|
||||||
|
edge,
|
||||||
|
edge_face_normal,
|
||||||
|
edge_dir,
|
||||||
|
)
|
||||||
|
c = jax.tree_util.tree_map(jp.array, c)
|
||||||
|
vert = jax.vmap(jp.multiply, in_axes=(None, 0))(c.vert, info.size)
|
||||||
|
face = jax.vmap(jp.multiply, in_axes=(None, 0))(c.face, info.size)
|
||||||
|
c = c.replace(vert=vert, face=face)
|
||||||
|
|
||||||
vert_start = m.mesh_vertadr[i]
|
return c
|
||||||
vert_end = m.mesh_vertadr[i + 1] if not last else m.mesh_vert.shape[0]
|
|
||||||
vert = m.mesh_vert[vert_start:vert_end]
|
|
||||||
|
|
||||||
graphadr = m.mesh_graphadr[i]
|
|
||||||
if graphadr < 0:
|
|
||||||
mesh_infos.append(MeshInfo(name, vert, face, None, None))
|
|
||||||
continue
|
|
||||||
|
|
||||||
|
def convex(m: Model, mesh_id: int, info: GeomInfo) -> ConvexInfo:
|
||||||
|
"""Processes a mesh for use in convex collision algorithms.
|
||||||
|
|
||||||
|
Args:
|
||||||
|
m: an MJX model
|
||||||
|
mesh_id: the mesh id to process
|
||||||
|
info: pos, mat, size of this geom
|
||||||
|
|
||||||
|
Returns:
|
||||||
|
a convex mesh info
|
||||||
|
"""
|
||||||
|
vert_beg = m.mesh_vertadr[mesh_id]
|
||||||
|
vert_end = m.mesh_vertadr[mesh_id + 1] if mesh_id < m.nmesh - 1 else None
|
||||||
|
vert = m.mesh_vert[vert_beg:vert_end]
|
||||||
|
|
||||||
|
graphadr = m.mesh_graphadr[mesh_id]
|
||||||
graph = m.mesh_graph[graphadr:]
|
graph = m.mesh_graph[graphadr:]
|
||||||
|
graph_idx = 0
|
||||||
|
|
||||||
numvert, numface = graph[0], graph[1]
|
numvert, numface = graph[0], graph[1]
|
||||||
|
graph_idx += 2
|
||||||
|
|
||||||
# unused vert_edgeadr
|
# skip vert_edgeadr (numvert,)
|
||||||
# vert_edgeadr = graph[2 : numvert + 2]
|
graph_idx += numvert
|
||||||
last_idx = numvert + 2
|
vert_globalid = graph[graph_idx : graph_idx + numvert]
|
||||||
|
graph_idx += numvert
|
||||||
|
|
||||||
vert_globalid = graph[last_idx : last_idx + numvert]
|
# skip edge_localid (numvert, 3)
|
||||||
last_idx += numvert
|
graph_idx += numvert + 3 * numface
|
||||||
|
face_globalid = graph[graph_idx : graph_idx + 3 * numface].reshape((-1, 3))
|
||||||
|
|
||||||
# unused edge_localid
|
vert = vert[vert_globalid]
|
||||||
# edge_localid = graph[last_idx : last_idx + numvert + 3 * numface]
|
|
||||||
last_idx += numvert + 3 * numface
|
|
||||||
|
|
||||||
face_globalid = graph[last_idx : last_idx + 3 * numface]
|
|
||||||
face_globalid = face_globalid.reshape((numface, 3))
|
|
||||||
|
|
||||||
convex_vert = vert[vert_globalid]
|
|
||||||
vertex_map = dict(zip(vert_globalid, np.arange(vert_globalid.shape[0])))
|
vertex_map = dict(zip(vert_globalid, np.arange(vert_globalid.shape[0])))
|
||||||
convex_face = np.vectorize(vertex_map.get)(face_globalid)
|
face = np.vectorize(vertex_map.get)(face_globalid)
|
||||||
mesh_infos.append(MeshInfo(name, vert, face, convex_vert, convex_face))
|
|
||||||
|
|
||||||
return mesh_infos
|
tm_convex = trimesh.Trimesh(vertices=vert, faces=face)
|
||||||
|
|
||||||
|
|
||||||
def _geom_mesh_kwargs(
|
|
||||||
mesh_info: MeshInfo,
|
|
||||||
) -> Dict[str, np.ndarray]:
|
|
||||||
"""Generates convex mesh attributes for mjx.Model."""
|
|
||||||
tm_convex = trimesh.Trimesh(
|
|
||||||
vertices=mesh_info.convex_vert, faces=mesh_info.convex_face
|
|
||||||
)
|
|
||||||
vert = np.array(tm_convex.vertices)
|
vert = np.array(tm_convex.vertices)
|
||||||
face = _merge_coplanar(tm_convex, mesh_info)
|
face = _merge_coplanar(m, tm_convex, mesh_id)
|
||||||
facenormal = _get_face_norm(vert, face)
|
face_normal = _get_face_norm(vert, face)
|
||||||
edge, edge_face_normal = _get_edge_normals(face, facenormal)
|
edge, edge_face_normal = _get_edge_normals(face, face_normal)
|
||||||
return {
|
edge_dir = _get_unique_edge_dir(vert, face)
|
||||||
'geom_convex_face': vert[face],
|
face = vert[face] # materialize full nface x nvert matrix
|
||||||
'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_edge': edge,
|
|
||||||
'geom_convex_edge_face_normal': edge_face_normal,
|
|
||||||
}
|
|
||||||
|
|
||||||
|
c = ConvexInfo(
|
||||||
def get(m: mujoco.MjModel) -> Dict[str, Sequence[Optional[np.ndarray]]]:
|
info.pos,
|
||||||
"""Derives geom mesh attributes for mjx.Model from MjModel."""
|
info.mat,
|
||||||
kwargs = {k: [] for k in _DERIVED_ARGS}
|
vert,
|
||||||
mesh_infos = _mesh_info(m)
|
face,
|
||||||
geom_con = m.geom_conaffinity | m.geom_contype
|
face_normal,
|
||||||
for geomid in range(m.ngeom):
|
edge,
|
||||||
mesh_info = None
|
edge_face_normal,
|
||||||
dataid = m.geom_dataid[geomid]
|
edge_dir,
|
||||||
if not geom_con[geomid]:
|
|
||||||
# ignore visual-only meshes
|
|
||||||
kwargs = {k: kwargs[k] + [None] for k in _DERIVED_ARGS}
|
|
||||||
continue
|
|
||||||
elif m.geom_type[geomid] == GeomType.BOX:
|
|
||||||
vert, face = _box(m.geom_size[geomid])
|
|
||||||
mesh_info = MeshInfo(
|
|
||||||
name='box',
|
|
||||||
vert=vert,
|
|
||||||
face=face,
|
|
||||||
convex_vert=vert,
|
|
||||||
convex_face=face,
|
|
||||||
)
|
)
|
||||||
elif dataid < 0:
|
|
||||||
kwargs = {k: kwargs[k] + [None] for k in _DERIVED_ARGS}
|
|
||||||
continue
|
|
||||||
|
|
||||||
mesh_info = mesh_info or mesh_infos[dataid]
|
return jax.tree_util.tree_map(jp.array, c)
|
||||||
vert, face = mesh_info.vert, mesh_info.face
|
|
||||||
key = (hash(vert.data.tobytes()), hash(face.data.tobytes()))
|
|
||||||
if key not in _CONVEX_CACHE:
|
|
||||||
_CONVEX_CACHE[key] = _geom_mesh_kwargs(mesh_info)
|
|
||||||
|
|
||||||
kwargs = {k: kwargs[k] + [_CONVEX_CACHE[key][k]] for k in _DERIVED_ARGS}
|
|
||||||
|
|
||||||
return kwargs
|
|
||||||
|
|||||||
@@ -20,7 +20,7 @@ import numpy as np
|
|||||||
import trimesh
|
import trimesh
|
||||||
|
|
||||||
|
|
||||||
class GeomMeshKwargsTest(absltest.TestCase):
|
class MeshTest(absltest.TestCase):
|
||||||
|
|
||||||
def test_pyramid(self):
|
def test_pyramid(self):
|
||||||
"""Tests that a triangulated pyramid converts to merged coplanar faces."""
|
"""Tests that a triangulated pyramid converts to merged coplanar faces."""
|
||||||
@@ -37,29 +37,21 @@ class GeomMeshKwargsTest(absltest.TestCase):
|
|||||||
tm = trimesh.Trimesh(vertices=vert, faces=face)
|
tm = trimesh.Trimesh(vertices=vert, faces=face)
|
||||||
tm_convex = trimesh.convex.convex_hull(tm)
|
tm_convex = trimesh.convex.convex_hull(tm)
|
||||||
convex_vert = np.array(tm_convex.vertices)
|
convex_vert = np.array(tm_convex.vertices)
|
||||||
convex_face = np.array(tm_convex.faces)
|
convex_face = mesh._merge_coplanar(None, tm_convex, 0)
|
||||||
mesh_info = mesh.MeshInfo(
|
|
||||||
name='test',
|
|
||||||
vert=vert,
|
|
||||||
face=face,
|
|
||||||
convex_vert=convex_vert,
|
|
||||||
convex_face=convex_face,
|
|
||||||
)
|
|
||||||
h = mesh._geom_mesh_kwargs(mesh_info)
|
|
||||||
|
|
||||||
# get index of vertices in h['geom_convex_vert'] for vertices in vert
|
# get index of vertices in h['geom_convex_vert'] for vertices in vert
|
||||||
dist = np.repeat(vert, vert.shape[0], axis=0) - np.tile(
|
dist = np.repeat(vert, vert.shape[0], axis=0) - np.tile(
|
||||||
h['geom_convex_vert'], (vert.shape[0], 1)
|
convex_vert, (vert.shape[0], 1)
|
||||||
)
|
)
|
||||||
dist = (dist**2).sum(axis=1).reshape((vert.shape[0], -1))
|
dist = (dist**2).sum(axis=1).reshape((vert.shape[0], -1))
|
||||||
vidx = np.argmin(dist, axis=0)
|
vidx = np.argmin(dist, axis=0)
|
||||||
|
|
||||||
# check verts
|
# check verts
|
||||||
np.testing.assert_array_equal(h['geom_convex_vert'], vert[vidx])
|
np.testing.assert_array_equal(convex_vert, vert[vidx])
|
||||||
|
|
||||||
# check face vertices
|
# check face vertices
|
||||||
map_ = {v: k for k, v in enumerate(vidx)}
|
map_ = {v: k for k, v in enumerate(vidx)}
|
||||||
h_face = np.vectorize(map_.get)(h['geom_convex_face_vert_idx'])
|
h_face = np.vectorize(map_.get)(convex_face)
|
||||||
face_verts = sorted([tuple(sorted(set(s))) for s in h_face.tolist()])
|
face_verts = sorted([tuple(sorted(set(s))) for s in h_face.tolist()])
|
||||||
expected_face_verts = sorted([
|
expected_face_verts = sorted([
|
||||||
(0, 3, 4), (1, 3, 4), (0, 2, 4), (0, 1, 2, 3), (1, 2, 4)])
|
(0, 3, 4), (1, 3, 4), (0, 2, 4), (0, 1, 2, 3), (1, 2, 4)])
|
||||||
@@ -69,7 +61,8 @@ class GeomMeshKwargsTest(absltest.TestCase):
|
|||||||
)
|
)
|
||||||
|
|
||||||
# check edges
|
# check edges
|
||||||
unique_edge = np.vectorize(map_.get)(h['geom_convex_edge_dir'])
|
edge_dir = mesh._get_unique_edge_dir(convex_vert, convex_face)
|
||||||
|
unique_edge = np.vectorize(map_.get)(edge_dir)
|
||||||
unique_edge = np.array(sorted(unique_edge.tolist()))
|
unique_edge = np.array(sorted(unique_edge.tolist()))
|
||||||
np.testing.assert_array_equal(
|
np.testing.assert_array_equal(
|
||||||
unique_edge,
|
unique_edge,
|
||||||
@@ -77,10 +70,11 @@ class GeomMeshKwargsTest(absltest.TestCase):
|
|||||||
)
|
)
|
||||||
|
|
||||||
# face normals
|
# face normals
|
||||||
self.assertEqual(h['geom_convex_facenormal'].shape, (5, 3))
|
face_normal = mesh._get_face_norm(convex_vert, convex_face)
|
||||||
|
self.assertEqual(face_normal.shape, (5, 3))
|
||||||
|
|
||||||
# face edges
|
# face edges
|
||||||
edges = h['geom_convex_edge']
|
edges, edge_normal = mesh._get_edge_normals(convex_face, face_normal)
|
||||||
edges = np.vectorize(map_.get)(edges)
|
edges = np.vectorize(map_.get)(edges)
|
||||||
mask = edges[:, 0] != edges[:, 1]
|
mask = edges[:, 0] != edges[:, 1]
|
||||||
edges = edges[mask]
|
edges = edges[mask]
|
||||||
@@ -103,7 +97,6 @@ class GeomMeshKwargsTest(absltest.TestCase):
|
|||||||
)
|
)
|
||||||
|
|
||||||
# face edge normals
|
# face edge normals
|
||||||
edge_normal = h['geom_convex_edge_face_normal']
|
|
||||||
edge_normal = edge_normal[mask]
|
edge_normal = edge_normal[mask]
|
||||||
edge_normal = np.take_along_axis(
|
edge_normal = np.take_along_axis(
|
||||||
edge_normal, sort_col_idx[..., None], axis=1
|
edge_normal, sort_col_idx[..., None], axis=1
|
||||||
|
|||||||
@@ -29,6 +29,100 @@ from mujoco.mjx._src.types import Model
|
|||||||
# pylint: enable=g-importing-member
|
# pylint: enable=g-importing-member
|
||||||
|
|
||||||
|
|
||||||
|
def _spring_damper(m: Model, d: Data) -> jax.Array:
|
||||||
|
"""Applies joint level spring and damping forces."""
|
||||||
|
def fn(jnt_typs, stiffness, qpos_spring, qpos):
|
||||||
|
qpos_i = 0
|
||||||
|
qfrcs = []
|
||||||
|
for i in range(len(jnt_typs)):
|
||||||
|
jnt_typ = JointType(jnt_typs[i])
|
||||||
|
q = qpos[qpos_i : qpos_i + jnt_typ.qpos_width()]
|
||||||
|
qs = qpos_spring[qpos_i : qpos_i + jnt_typ.qpos_width()]
|
||||||
|
qfrc = jp.zeros(jnt_typ.dof_width())
|
||||||
|
if jnt_typ == JointType.FREE:
|
||||||
|
qfrc = qfrc.at[:3].set(-stiffness[i] * (q[:3] - qs[:3]))
|
||||||
|
qfrc = qfrc.at[3:6].set(-stiffness[i] * math.quat_sub(q[3:7], qs[3:7]))
|
||||||
|
elif jnt_typ == JointType.BALL:
|
||||||
|
qfrc = -stiffness[i] * math.quat_sub(q, qs)
|
||||||
|
elif jnt_typ in (
|
||||||
|
JointType.SLIDE,
|
||||||
|
JointType.HINGE,
|
||||||
|
):
|
||||||
|
qfrc = -stiffness[i] * (q - qs)
|
||||||
|
else:
|
||||||
|
raise RuntimeError(f'unrecognized joint type: {jnt_typ}')
|
||||||
|
qfrcs.append(qfrc)
|
||||||
|
qpos_i += jnt_typ.qpos_width()
|
||||||
|
return jp.concatenate(qfrcs)
|
||||||
|
|
||||||
|
# dof-level springs
|
||||||
|
qfrc = scan.flat(
|
||||||
|
m,
|
||||||
|
fn,
|
||||||
|
'jjqq',
|
||||||
|
'v',
|
||||||
|
m.jnt_type,
|
||||||
|
m.jnt_stiffness,
|
||||||
|
m.qpos_spring,
|
||||||
|
d.qpos,
|
||||||
|
)
|
||||||
|
|
||||||
|
# dof-level dampers
|
||||||
|
qfrc -= m.dof_damping * d.qvel
|
||||||
|
|
||||||
|
return qfrc
|
||||||
|
|
||||||
|
|
||||||
|
def _gravcomp(m: Model, d: Data) -> jax.Array:
|
||||||
|
"""Applies body-level gravity compensation."""
|
||||||
|
force = -m.opt.gravity * (m.body_mass * m.body_gravcomp)[:, None]
|
||||||
|
|
||||||
|
apply_f = lambda f, pos, body_id: support.jac(m, d, pos, body_id)[0] @ f
|
||||||
|
qfrc = jax.vmap(apply_f)(force, d.xipos, jp.arange(m.nbody)).sum(axis=0)
|
||||||
|
|
||||||
|
return qfrc
|
||||||
|
|
||||||
|
|
||||||
|
def _fluid(m: Model, d: Data) -> jax.Array:
|
||||||
|
"""Applies body-level viscosity, lift and drag."""
|
||||||
|
force, torque = jax.vmap(
|
||||||
|
_inertia_box_fluid_model, in_axes=(None, 0, 0, 0, 0, 0, 0)
|
||||||
|
)(
|
||||||
|
m,
|
||||||
|
m.body_inertia,
|
||||||
|
m.body_mass,
|
||||||
|
d.subtree_com[jp.array(m.body_rootid)],
|
||||||
|
d.xipos,
|
||||||
|
d.ximat,
|
||||||
|
d.cvel,
|
||||||
|
)
|
||||||
|
qfrc = jax.vmap(support.apply_ft, in_axes=(None, None, 0, 0, 0, 0))(
|
||||||
|
m, d, force, torque, d.xipos, jp.arange(m.nbody)
|
||||||
|
)
|
||||||
|
|
||||||
|
return jp.sum(qfrc, axis=0)
|
||||||
|
|
||||||
|
|
||||||
|
def passive(m: Model, d: Data) -> Data:
|
||||||
|
"""Adds all passive forces."""
|
||||||
|
if m.opt.disableflags & DisableBit.PASSIVE:
|
||||||
|
return d.replace(qfrc_passive=jp.zeros(m.nv), qfrc_gravcomp=jp.zeros(m.nv))
|
||||||
|
|
||||||
|
qfrc_passive = _spring_damper(m, d)
|
||||||
|
qfrc_gravcomp = jp.zeros(m.nv)
|
||||||
|
|
||||||
|
if m.ngravcomp and not m.opt.disableflags & DisableBit.GRAVITY:
|
||||||
|
qfrc_gravcomp = _gravcomp(m, d)
|
||||||
|
# add gravcomp unless added via actuators
|
||||||
|
qfrc_passive += qfrc_gravcomp * (1 - m.jnt_actgravcomp[m.dof_jntid])
|
||||||
|
|
||||||
|
if m.opt.has_fluid_params:
|
||||||
|
qfrc_passive += _fluid(m, d)
|
||||||
|
|
||||||
|
d = d.replace(qfrc_passive=qfrc_passive, qfrc_gravcomp=qfrc_gravcomp)
|
||||||
|
return d
|
||||||
|
|
||||||
|
|
||||||
def _inertia_box_fluid_model(
|
def _inertia_box_fluid_model(
|
||||||
m: Model,
|
m: Model,
|
||||||
inertia: jax.Array,
|
inertia: jax.Array,
|
||||||
@@ -71,71 +165,3 @@ def _inertia_box_fluid_model(
|
|||||||
force, torque = ximat @ lfrc_vel, ximat @ lfrc_ang
|
force, torque = ximat @ lfrc_vel, ximat @ lfrc_ang
|
||||||
|
|
||||||
return force, torque
|
return force, torque
|
||||||
|
|
||||||
|
|
||||||
def passive(m: Model, d: Data) -> Data:
|
|
||||||
"""Adds all passive forces."""
|
|
||||||
if m.opt.disableflags & DisableBit.PASSIVE:
|
|
||||||
return d.replace(qfrc_passive=jp.zeros(m.nv))
|
|
||||||
|
|
||||||
# joint-level springs
|
|
||||||
def fn(jnt_typs, stiffness, qpos_spring, qpos):
|
|
||||||
qpos_i = 0
|
|
||||||
qfrcs = []
|
|
||||||
for i in range(len(jnt_typs)):
|
|
||||||
jnt_typ = JointType(jnt_typs[i])
|
|
||||||
q = qpos[qpos_i : qpos_i + jnt_typ.qpos_width()]
|
|
||||||
qs = qpos_spring[qpos_i : qpos_i + jnt_typ.qpos_width()]
|
|
||||||
qfrc = jp.zeros(jnt_typ.dof_width())
|
|
||||||
if jnt_typ == JointType.FREE:
|
|
||||||
qfrc = qfrc.at[:3].set(-stiffness[i] * (q[:3] - qs[:3]))
|
|
||||||
qfrc = qfrc.at[3:6].set(-stiffness[i] * math.quat_sub(q[3:7], qs[3:7]))
|
|
||||||
elif jnt_typ == JointType.BALL:
|
|
||||||
qfrc = -stiffness[i] * math.quat_sub(q, qs)
|
|
||||||
elif jnt_typ in (
|
|
||||||
JointType.SLIDE,
|
|
||||||
JointType.HINGE,
|
|
||||||
):
|
|
||||||
qfrc = -stiffness[i] * (q - qs)
|
|
||||||
else:
|
|
||||||
raise RuntimeError(f'unrecognized joint type: {jnt_typ}')
|
|
||||||
qfrcs.append(qfrc)
|
|
||||||
qpos_i += jnt_typ.qpos_width()
|
|
||||||
return jp.concatenate(qfrcs)
|
|
||||||
|
|
||||||
qfrc_passive = scan.flat(
|
|
||||||
m,
|
|
||||||
fn,
|
|
||||||
'jjqq',
|
|
||||||
'v',
|
|
||||||
m.jnt_type,
|
|
||||||
m.jnt_stiffness,
|
|
||||||
m.qpos_spring,
|
|
||||||
d.qpos,
|
|
||||||
)
|
|
||||||
|
|
||||||
# dof-level dampers
|
|
||||||
qfrc_passive -= m.dof_damping * d.qvel
|
|
||||||
|
|
||||||
# TODO(robotics-simulation): body-level gravity compensation
|
|
||||||
|
|
||||||
# body-level viscosity, lift and drag
|
|
||||||
if m.opt.has_fluid_params:
|
|
||||||
force, torque = jax.vmap(
|
|
||||||
_inertia_box_fluid_model, in_axes=(None, 0, 0, 0, 0, 0, 0)
|
|
||||||
)(
|
|
||||||
m,
|
|
||||||
m.body_inertia,
|
|
||||||
m.body_mass,
|
|
||||||
d.subtree_com[jp.array(m.body_rootid)],
|
|
||||||
d.xipos,
|
|
||||||
d.ximat,
|
|
||||||
d.cvel,
|
|
||||||
)
|
|
||||||
qfrc_target = jax.vmap(support.apply_ft, in_axes=(None, None, 0, 0, 0, 0))(
|
|
||||||
m, d, force, torque, d.xipos, jp.arange(m.nbody)
|
|
||||||
)
|
|
||||||
qfrc_passive += jp.sum(qfrc_target, axis=0)
|
|
||||||
|
|
||||||
d = d.replace(qfrc_passive=qfrc_passive)
|
|
||||||
return d
|
|
||||||
|
|||||||
@@ -42,13 +42,14 @@ class PassiveTest(absltest.TestCase):
|
|||||||
m = test_util.load_test_file('pendula.xml')
|
m = test_util.load_test_file('pendula.xml')
|
||||||
d = mujoco.MjData(m)
|
d = mujoco.MjData(m)
|
||||||
# give the system a little kick to ensure we have non-identity rotations
|
# give the system a little kick to ensure we have non-identity rotations
|
||||||
d.ctrl = np.array([0.1, -0.1, 0.2, 0.3, -0.4])
|
d.ctrl = np.array([0.1, -0.1, 0.2, 0.3, -0.4, 0.5, -0.6, 0.1])
|
||||||
mujoco.mj_step(m, d, 10) # let dynamics get state significantly non-zero
|
mujoco.mj_step(m, d, 10) # let dynamics get state significantly non-zero
|
||||||
mujoco.mj_forward(m, d)
|
mujoco.mj_forward(m, d)
|
||||||
mx = mjx.put_model(m)
|
mx = mjx.put_model(m)
|
||||||
|
|
||||||
dx = jax.jit(mjx.passive)(mx, mjx.put_data(m, d))
|
dx = jax.jit(mjx.passive)(mx, mjx.put_data(m, d))
|
||||||
_assert_attr_eq(d, dx, 'qfrc_passive')
|
_assert_attr_eq(d, dx, 'qfrc_passive')
|
||||||
|
_assert_attr_eq(d, dx, 'qfrc_gravcomp')
|
||||||
|
|
||||||
# test with fluid forces
|
# test with fluid forces
|
||||||
m.opt.density = 0.01
|
m.opt.density = 0.01
|
||||||
@@ -56,18 +57,21 @@ class PassiveTest(absltest.TestCase):
|
|||||||
mx = mjx.put_model(m)
|
mx = mjx.put_model(m)
|
||||||
dx = jax.jit(mjx.passive)(mx, mjx.put_data(m, d))
|
dx = jax.jit(mjx.passive)(mx, mjx.put_data(m, d))
|
||||||
_assert_attr_eq(d, dx, 'qfrc_passive')
|
_assert_attr_eq(d, dx, 'qfrc_passive')
|
||||||
|
_assert_attr_eq(d, dx, 'qfrc_gravcomp')
|
||||||
|
|
||||||
m.opt.viscosity = 0.02
|
m.opt.viscosity = 0.02
|
||||||
mujoco.mj_forward(m, d)
|
mujoco.mj_forward(m, d)
|
||||||
mx = mjx.put_model(m)
|
mx = mjx.put_model(m)
|
||||||
dx = jax.jit(mjx.passive)(mx, mjx.put_data(m, d))
|
dx = jax.jit(mjx.passive)(mx, mjx.put_data(m, d))
|
||||||
_assert_attr_eq(d, dx, 'qfrc_passive')
|
_assert_attr_eq(d, dx, 'qfrc_passive')
|
||||||
|
_assert_attr_eq(d, dx, 'qfrc_gravcomp')
|
||||||
|
|
||||||
m.opt.wind = np.array([0.03, 0.04, 0.05])
|
m.opt.wind = np.array([0.03, 0.04, 0.05])
|
||||||
mujoco.mj_forward(m, d)
|
mujoco.mj_forward(m, d)
|
||||||
mx = mjx.put_model(m)
|
mx = mjx.put_model(m)
|
||||||
dx = jax.jit(mjx.passive)(mx, mjx.put_data(m, d))
|
dx = jax.jit(mjx.passive)(mx, mjx.put_data(m, d))
|
||||||
_assert_attr_eq(d, dx, 'qfrc_passive')
|
_assert_attr_eq(d, dx, 'qfrc_passive')
|
||||||
|
_assert_attr_eq(d, dx, 'qfrc_gravcomp')
|
||||||
|
|
||||||
# test disable passive
|
# test disable passive
|
||||||
mx = mx.tree_replace({'opt.disableflags': mjx.DisableBit.PASSIVE})
|
mx = mx.tree_replace({'opt.disableflags': mjx.DisableBit.PASSIVE})
|
||||||
|
|||||||
+19
-28
@@ -134,32 +134,27 @@ def _ray_triangle(
|
|||||||
vert: jax.Array,
|
vert: jax.Array,
|
||||||
pnt: jax.Array,
|
pnt: jax.Array,
|
||||||
vec: jax.Array,
|
vec: jax.Array,
|
||||||
b0: jax.Array,
|
basis: jax.Array,
|
||||||
b1: jax.Array,
|
|
||||||
) -> jax.Array:
|
) -> jax.Array:
|
||||||
"""Returns the distance at which a ray intersects with a triangle."""
|
"""Returns the distance at which a ray intersects with a triangle."""
|
||||||
# project difference vectors in ray normal plane
|
# project difference vectors in ray normal plane
|
||||||
planar = jp.dot(jp.array([b0, b1]), (vert - pnt).T)
|
planar = jp.dot(vert - pnt, basis)
|
||||||
|
|
||||||
# determine if origin is inside planar projection of triangle
|
# determine if origin is inside planar projection of triangle
|
||||||
# A = (p0-p2, p1-p2), b = -p2, solve A*t = b
|
# A = (p0-p2, p1-p2), b = -p2, solve A*t = b
|
||||||
A = jp.array( # pylint: disable=invalid-name
|
A = planar[0:2] - planar[2] # pylint: disable=invalid-name
|
||||||
[planar[:, 0] - planar[:, 2], planar[:, 1] - planar[:, 2]]
|
b = -planar[2]
|
||||||
).T.flatten()
|
det = A[0, 0] * A[1, 1] - A[1, 0] * A[0, 1]
|
||||||
b = -planar[:, 2]
|
|
||||||
det = A[0] * A[3] - A[1] * A[2]
|
|
||||||
valid = jp.abs(det) >= mujoco.mjMINVAL
|
|
||||||
|
|
||||||
t0 = (A[3] * b[0] - A[1] * b[1]) / det
|
t0 = (A[1, 1] * b[0] - A[1, 0] * b[1]) / det
|
||||||
t1 = (-A[2] * b[0] + A[0] * b[1]) / det
|
t1 = (-A[0, 1] * b[0] + A[0, 0] * b[1]) / det
|
||||||
valid &= (t0 >= 0) & (t1 >= 0) & (t0 + t1 <= 1)
|
valid = (t0 >= 0) & (t1 >= 0) & (t0 + t1 <= 1)
|
||||||
|
|
||||||
# intersect ray with plane of triangle
|
# intersect ray with plane of triangle
|
||||||
nrm = jp.cross(vert[0] - vert[2], vert[1] - vert[2])
|
nrm = jp.cross(vert[0] - vert[2], vert[1] - vert[2])
|
||||||
denom = jp.dot(vec, nrm)
|
dist = jp.dot(vert[2] - pnt, nrm) / jp.dot(vec, nrm)
|
||||||
valid &= jp.abs(denom) >= mujoco.mjMINVAL
|
valid &= dist >= 0
|
||||||
|
dist = jp.where(valid, dist, jp.inf)
|
||||||
dist = jp.where(valid, -jp.dot(pnt - vert[2], nrm) / denom, jp.inf)
|
|
||||||
|
|
||||||
return dist
|
return dist
|
||||||
|
|
||||||
@@ -174,33 +169,29 @@ def _ray_mesh(
|
|||||||
"""Returns the best distance and geom_id for ray mesh intersections."""
|
"""Returns the best distance and geom_id for ray mesh intersections."""
|
||||||
data_id = m.geom_dataid[geom_id]
|
data_id = m.geom_dataid[geom_id]
|
||||||
|
|
||||||
ray_basis = lambda x: math.orthogonals(math.normalize(x))
|
ray_basis = lambda x: jp.array(math.orthogonals(math.normalize(x))).T
|
||||||
b0, b1 = jax.vmap(ray_basis)(vec)
|
basis = jax.vmap(ray_basis)(vec)
|
||||||
|
|
||||||
faceadr = np.append(m.mesh_faceadr, m.nmeshface)
|
faceadr = np.append(m.mesh_faceadr, m.nmeshface)
|
||||||
vertadr = np.append(m.mesh_vertadr, m.nmeshvert)
|
vertadr = np.append(m.mesh_vertadr, m.nmeshvert)
|
||||||
|
|
||||||
dists = []
|
dists, geom_ids = [], []
|
||||||
for i, id_ in enumerate(data_id):
|
for i, id_ in enumerate(data_id):
|
||||||
face = m.mesh_face[faceadr[id_] : faceadr[id_ + 1]]
|
face = m.mesh_face[faceadr[id_] : faceadr[id_ + 1]]
|
||||||
vert = m.mesh_vert[vertadr[id_] : vertadr[id_ + 1]]
|
vert = m.mesh_vert[vertadr[id_] : vertadr[id_ + 1]]
|
||||||
dist = jax.vmap(_ray_triangle, in_axes=(0, None, None, None, None))(
|
vert = jp.array(vert[face])
|
||||||
vert[face], pnt[i], vec[i], b0[i], b1[i]
|
dist = jax.vmap(_ray_triangle, in_axes=(0, None, None, None))(
|
||||||
|
vert, pnt[i], vec[i], basis[i]
|
||||||
)
|
)
|
||||||
dists.append(dist)
|
dists.append(dist)
|
||||||
|
geom_ids.append(np.repeat(geom_id[i], dist.size))
|
||||||
# map the triangle id to data id
|
|
||||||
tri_id = np.append(0, (faceadr[data_id + 1] - faceadr[data_id]).cumsum())
|
|
||||||
tri_data_id = np.zeros(tri_id[-1], dtype=np.int32)
|
|
||||||
tri_data_id[tri_id[:-1]] = 1
|
|
||||||
tri_data_id = tri_data_id.cumsum() - 1
|
|
||||||
|
|
||||||
dists = jp.concatenate(dists)
|
dists = jp.concatenate(dists)
|
||||||
min_id = jp.argmin(dists)
|
min_id = jp.argmin(dists)
|
||||||
# Grab the best distance amongst all meshes, bypassing the argmin in `ray`.
|
# Grab the best distance amongst all meshes, bypassing the argmin in `ray`.
|
||||||
# This avoids having to compute the best distance per mesh.
|
# This avoids having to compute the best distance per mesh.
|
||||||
dist = dists[min_id, None]
|
dist = dists[min_id, None]
|
||||||
id_ = jp.array(geom_id)[jp.array(tri_data_id)[min_id], None]
|
id_ = jp.array(np.concatenate(geom_ids))[min_id, None]
|
||||||
|
|
||||||
return dist, id_
|
return dist, id_
|
||||||
|
|
||||||
|
|||||||
@@ -152,11 +152,7 @@ class RayTest(absltest.TestCase):
|
|||||||
mx, dx = mjx.put_model(m), mjx.put_data(m, d)
|
mx, dx = mjx.put_model(m), mjx.put_data(m, d)
|
||||||
|
|
||||||
# look at the tetrahedron
|
# look at the tetrahedron
|
||||||
pnt, vec = jp.array([2.0, 2.0, 2.0]), -jp.array([
|
pnt, vec = jp.array([2.0, 2.0, 2.0]), -jp.array([1.0, 1.0, 1.0])
|
||||||
1.0,
|
|
||||||
1.0,
|
|
||||||
1.0,
|
|
||||||
])
|
|
||||||
vec /= jp.linalg.norm(vec)
|
vec /= jp.linalg.norm(vec)
|
||||||
dist, geomid = jax.jit(mjx.ray)(mx, dx, pnt, vec)
|
dist, geomid = jax.jit(mjx.ray)(mx, dx, pnt, vec)
|
||||||
_assert_eq(geomid, 4, 'geom_id')
|
_assert_eq(geomid, 4, 'geom_id')
|
||||||
@@ -166,12 +162,14 @@ class RayTest(absltest.TestCase):
|
|||||||
_assert_eq(geomid, 4, 'geom_id')
|
_assert_eq(geomid, 4, 'geom_id')
|
||||||
_assert_eq(dist, mj_dist, 'dist-tetrahedron')
|
_assert_eq(dist, mj_dist, 'dist-tetrahedron')
|
||||||
|
|
||||||
|
# look away from the dodecahedron
|
||||||
|
pnt, vec = jp.array([4.0, 2.0, 2.0]), jp.array([2.0, 1.0, 1.0])
|
||||||
|
vec /= jp.linalg.norm(vec)
|
||||||
|
_, geomid = jax.jit(mjx.ray)(mx, dx, pnt, vec)
|
||||||
|
_assert_eq(geomid, -1, 'geom_id')
|
||||||
|
|
||||||
# look at the dodecahedron
|
# look at the dodecahedron
|
||||||
pnt, vec = jp.array([4.0, 2.0, 2.0]), -jp.array([
|
pnt, vec = jp.array([4.0, 2.0, 2.0]), -jp.array([2.0, 1.0, 1.0])
|
||||||
2.0,
|
|
||||||
1.0,
|
|
||||||
1.0,
|
|
||||||
])
|
|
||||||
vec /= jp.linalg.norm(vec)
|
vec /= jp.linalg.norm(vec)
|
||||||
dist, geomid = jax.jit(mjx.ray)(mx, dx, pnt, vec)
|
dist, geomid = jax.jit(mjx.ray)(mx, dx, pnt, vec)
|
||||||
_assert_eq(geomid, 5, 'geom_id')
|
_assert_eq(geomid, 5, 'geom_id')
|
||||||
|
|||||||
@@ -62,7 +62,7 @@ def _take(obj: Y, idx: np.ndarray) -> Y:
|
|||||||
x = x.take(jp.array(idx), axis=0, mode='wrap')
|
x = x.take(jp.array(idx), axis=0, mode='wrap')
|
||||||
return x
|
return x
|
||||||
|
|
||||||
return jax.tree_map(take, obj)
|
return jax.tree_util.tree_map(take, obj)
|
||||||
|
|
||||||
|
|
||||||
def _q_bodyid(m: Model) -> np.ndarray:
|
def _q_bodyid(m: Model) -> np.ndarray:
|
||||||
@@ -120,7 +120,7 @@ def _nvmap(f: Callable[..., Y], *args) -> Y:
|
|||||||
args = [a if n is None else None for n, a in zip(np_args, args)]
|
args = [a if n is None else None for n, a in zip(np_args, args)]
|
||||||
|
|
||||||
# remove empty args that we should not vmap over
|
# remove empty args that we should not vmap over
|
||||||
args = jax.tree_map(lambda a: a if a.shape[0] else None, args)
|
args = jax.tree_util.tree_map(lambda a: a if a.shape[0] else None, args)
|
||||||
in_axes = [None if a is None else 0 for a in args]
|
in_axes = [None if a is None else 0 for a in args]
|
||||||
|
|
||||||
def outer_f(*args, np_args=np_args):
|
def outer_f(*args, np_args=np_args):
|
||||||
@@ -322,7 +322,7 @@ def flat(
|
|||||||
[v if typ in flat_ else jp.concatenate(v) for v, typ in zip(y, out_types)]
|
[v if typ in flat_ else jp.concatenate(v) for v, typ in zip(y, out_types)]
|
||||||
for y in ys
|
for y in ys
|
||||||
]
|
]
|
||||||
ys = jax.tree_map(lambda *x: jp.concatenate(x), *ys)
|
ys = jax.tree_util.tree_map(lambda *x: jp.concatenate(x), *ys)
|
||||||
|
|
||||||
# put concatenated results back in order
|
# put concatenated results back in order
|
||||||
reordered_ys = []
|
reordered_ys = []
|
||||||
@@ -465,15 +465,15 @@ def body_tree(
|
|||||||
def index_sum(x, i=id_map, s=body_ids.size):
|
def index_sum(x, i=id_map, s=body_ids.size):
|
||||||
return jax.ops.segment_sum(x, i, s)
|
return jax.ops.segment_sum(x, i, s)
|
||||||
|
|
||||||
y = jax.tree_map(index_sum, y)
|
y = jax.tree_util.tree_map(index_sum, y)
|
||||||
carry = y if carry is None else jax.tree_map(jp.add, carry, y)
|
carry = y if carry is None else jax.tree_util.tree_map(jp.add, carry, y)
|
||||||
elif key in key_parents:
|
elif key in key_parents:
|
||||||
ys = [key_y[p] for p in key_parents[key]]
|
ys = [key_y[p] for p in key_parents[key]]
|
||||||
y = jax.tree_map(lambda *x: jp.concatenate(x), *ys)
|
y = jax.tree_util.tree_map(lambda *x: jp.concatenate(x), *ys)
|
||||||
body_ids = np.concatenate([key_body_ids[p] for p in key_parents[key]])
|
body_ids = np.concatenate([key_body_ids[p] for p in key_parents[key]])
|
||||||
parent_ids = m.body_parentid[key_body_ids[key]]
|
parent_ids = m.body_parentid[key_body_ids[key]]
|
||||||
take_fn = lambda x, i=_index(body_ids, parent_ids): _take(x, i)
|
take_fn = lambda x, i=_index(body_ids, parent_ids): _take(x, i)
|
||||||
carry = jax.tree_map(take_fn, y)
|
carry = jax.tree_util.tree_map(take_fn, y)
|
||||||
|
|
||||||
f_args = [_take(arg, ids) for arg, ids in zip(args, key_in_take[key])]
|
f_args = [_take(arg, ids) for arg, ids in zip(args, key_in_take[key])]
|
||||||
key_y[key] = _nvmap(f, carry, *f_args)
|
key_y[key] = _nvmap(f, carry, *f_args)
|
||||||
@@ -488,8 +488,8 @@ def body_tree(
|
|||||||
if len(out_types) > 1:
|
if len(out_types) > 1:
|
||||||
y_typ = [y_[i] for y_ in y_typ]
|
y_typ = [y_[i] for y_ in y_typ]
|
||||||
if typ != 'b':
|
if typ != 'b':
|
||||||
y_typ = jax.tree_map(jp.concatenate, y_typ)
|
y_typ = jax.tree_util.tree_map(jp.concatenate, y_typ)
|
||||||
y_typ = jax.tree_map(lambda *x: jp.concatenate(x), *y_typ)
|
y_typ = jax.tree_util.tree_map(lambda *x: jp.concatenate(x), *y_typ)
|
||||||
y_take = np.argsort(np.concatenate([key_y_take[key][i] for key in keys]))
|
y_take = np.argsort(np.concatenate([key_y_take[key][i] for key in keys]))
|
||||||
_check_output(y_typ, y_take, typ, i)
|
_check_output(y_typ, y_take, typ, i)
|
||||||
y.append(_take(y_typ, y_take))
|
y.append(_take(y_typ, y_take))
|
||||||
|
|||||||
@@ -55,7 +55,7 @@ class ScanTest(absltest.TestCase):
|
|||||||
<worldbody/>
|
<worldbody/>
|
||||||
</mujoco>
|
</mujoco>
|
||||||
""")
|
""")
|
||||||
m = mjx.device_put(m)
|
m = mjx.put_model(m)
|
||||||
|
|
||||||
def fn(body_id):
|
def fn(body_id):
|
||||||
return body_id + 1
|
return body_id + 1
|
||||||
@@ -69,7 +69,7 @@ class ScanTest(absltest.TestCase):
|
|||||||
def test_flat_joints(self):
|
def test_flat_joints(self):
|
||||||
"""Tests scanning over bodies with joints of different types."""
|
"""Tests scanning over bodies with joints of different types."""
|
||||||
m = mujoco.MjModel.from_xml_string(self._MULTI_DOF_XML)
|
m = mujoco.MjModel.from_xml_string(self._MULTI_DOF_XML)
|
||||||
m = mjx.device_put(m)
|
m = mjx.put_model(m)
|
||||||
|
|
||||||
# we will test two functions:
|
# we will test two functions:
|
||||||
# 1) j_fn receives jnt_types as a jp array
|
# 1) j_fn receives jnt_types as a jp array
|
||||||
@@ -105,7 +105,7 @@ class ScanTest(absltest.TestCase):
|
|||||||
def test_body_tree(self):
|
def test_body_tree(self):
|
||||||
"""Tests tree scanning over bodies with different joint counts."""
|
"""Tests tree scanning over bodies with different joint counts."""
|
||||||
m = mujoco.MjModel.from_xml_string(self._MULTI_DOF_XML)
|
m = mujoco.MjModel.from_xml_string(self._MULTI_DOF_XML)
|
||||||
m = mjx.device_put(m)
|
m = mjx.put_model(m)
|
||||||
|
|
||||||
# we will test two functions:
|
# we will test two functions:
|
||||||
# 1) j_fn receives jnt_pos which is a jp array
|
# 1) j_fn receives jnt_pos which is a jp array
|
||||||
@@ -196,7 +196,7 @@ class ScanTest(absltest.TestCase):
|
|||||||
def test_scan_actuators(self):
|
def test_scan_actuators(self):
|
||||||
"""Tests scanning over actuators."""
|
"""Tests scanning over actuators."""
|
||||||
m = mujoco.MjModel.from_xml_string(self._MULTI_ACT_XML)
|
m = mujoco.MjModel.from_xml_string(self._MULTI_ACT_XML)
|
||||||
m = mjx.device_put(m)
|
m = mjx.put_model(m)
|
||||||
|
|
||||||
fn = lambda *args: args
|
fn = lambda *args: args
|
||||||
args = (
|
args = (
|
||||||
|
|||||||
@@ -17,7 +17,6 @@
|
|||||||
import jax
|
import jax
|
||||||
from jax import numpy as jp
|
from jax import numpy as jp
|
||||||
import mujoco
|
import mujoco
|
||||||
from mujoco.mjx._src import constraint
|
|
||||||
from mujoco.mjx._src import math
|
from mujoco.mjx._src import math
|
||||||
from mujoco.mjx._src import smooth
|
from mujoco.mjx._src import smooth
|
||||||
from mujoco.mjx._src import support
|
from mujoco.mjx._src import support
|
||||||
@@ -81,7 +80,7 @@ class _Context(PyTreeNode):
|
|||||||
prev_cost=0.0,
|
prev_cost=0.0,
|
||||||
solver_niter=0,
|
solver_niter=0,
|
||||||
)
|
)
|
||||||
ctx = _update_constraint(m, d, ctx)
|
ctx = _update_constraint(d, ctx)
|
||||||
if grad:
|
if grad:
|
||||||
ctx = _update_gradient(m, d, ctx)
|
ctx = _update_gradient(m, d, ctx)
|
||||||
ctx = ctx.replace(search=-ctx.Mgrad) # start with preconditioned gradient
|
ctx = ctx.replace(search=-ctx.Mgrad) # start with preconditioned gradient
|
||||||
@@ -107,7 +106,7 @@ class _LSPoint(PyTreeNode):
|
|||||||
@classmethod
|
@classmethod
|
||||||
def create(
|
def create(
|
||||||
cls,
|
cls,
|
||||||
m: Model,
|
d: Data,
|
||||||
ctx: _Context,
|
ctx: _Context,
|
||||||
alpha: jax.Array,
|
alpha: jax.Array,
|
||||||
jv: jax.Array,
|
jv: jax.Array,
|
||||||
@@ -118,8 +117,7 @@ class _LSPoint(PyTreeNode):
|
|||||||
# roughly corresponds to CGEval in mujoco/src/engine/engine_solver.c
|
# roughly corresponds to CGEval in mujoco/src/engine/engine_solver.c
|
||||||
|
|
||||||
# TODO(robotics-team): change this to support friction constraints
|
# TODO(robotics-team): change this to support friction constraints
|
||||||
ne, nf, *_ = constraint.count_constraints(m)
|
active = ((ctx.Jaref + alpha * jv) < 0).at[:d.ne + d.nf].set(True)
|
||||||
active = ((ctx.Jaref + alpha * jv) < 0).at[:ne + nf].set(True)
|
|
||||||
quad = jax.vmap(jp.multiply)(quad, active) # only active
|
quad = jax.vmap(jp.multiply)(quad, active) # only active
|
||||||
quad_total = quad_gauss + jp.sum(quad, axis=0)
|
quad_total = quad_gauss + jp.sum(quad, axis=0)
|
||||||
|
|
||||||
@@ -161,13 +159,12 @@ def _while_loop_scan(cond_fun, body_fun, init_val, max_iter):
|
|||||||
return jax.lax.scan(_fun, init, None, length=max_iter)[0][0]
|
return jax.lax.scan(_fun, init, None, length=max_iter)[0][0]
|
||||||
|
|
||||||
|
|
||||||
def _update_constraint(m: Model, d: Data, ctx: _Context) -> _Context:
|
def _update_constraint(d: Data, ctx: _Context) -> _Context:
|
||||||
"""Updates constraint force and resulting cost given latst solver iteration.
|
"""Updates constraint force and resulting cost given latst solver iteration.
|
||||||
|
|
||||||
Corresponds to CGupdateConstraint in mujoco/src/engine/engine_solver.c
|
Corresponds to CGupdateConstraint in mujoco/src/engine/engine_solver.c
|
||||||
|
|
||||||
Args:
|
Args:
|
||||||
m: model defining constraints
|
|
||||||
d: data which contains latest qacc and smooth terms
|
d: data which contains latest qacc and smooth terms
|
||||||
ctx: current solver context
|
ctx: current solver context
|
||||||
|
|
||||||
@@ -177,8 +174,7 @@ def _update_constraint(m: Model, d: Data, ctx: _Context) -> _Context:
|
|||||||
# TODO(robotics-team): add friction constraints
|
# TODO(robotics-team): add friction constraints
|
||||||
|
|
||||||
# only count active constraints
|
# only count active constraints
|
||||||
ne, nf, *_ = constraint.count_constraints(m)
|
active = (ctx.Jaref < 0).at[:d.ne + d.nf].set(True)
|
||||||
active = (ctx.Jaref < 0).at[:ne + nf].set(True)
|
|
||||||
|
|
||||||
efc_force = d.efc_D * -ctx.Jaref * active
|
efc_force = d.efc_D * -ctx.Jaref * active
|
||||||
qfrc_constraint = d.efc_J.T @ efc_force
|
qfrc_constraint = d.efc_J.T @ efc_force
|
||||||
@@ -217,8 +213,7 @@ def _update_gradient(m: Model, d: Data, ctx: _Context) -> _Context:
|
|||||||
if m.opt.solver == SolverType.CG:
|
if m.opt.solver == SolverType.CG:
|
||||||
mgrad = smooth.solve_m(m, d, grad)
|
mgrad = smooth.solve_m(m, d, grad)
|
||||||
elif m.opt.solver == SolverType.NEWTON:
|
elif m.opt.solver == SolverType.NEWTON:
|
||||||
ne, nf, *_ = constraint.count_constraints(m)
|
active = (ctx.Jaref < 0).at[: d.ne + d.nf].set(True)
|
||||||
active = (ctx.Jaref < 0).at[: ne + nf].set(True)
|
|
||||||
h = (d.efc_J.T * d.efc_D * active) @ d.efc_J
|
h = (d.efc_J.T * d.efc_D * active) @ d.efc_J
|
||||||
h = support.full_m(m, d) + h
|
h = support.full_m(m, d) + h
|
||||||
h_ = jax.scipy.linalg.cho_factor(h)
|
h_ = jax.scipy.linalg.cho_factor(h)
|
||||||
@@ -262,7 +257,7 @@ def _linesearch(m: Model, d: Data, ctx: _Context) -> _Context:
|
|||||||
quad = jp.stack((0.5 * ctx.Jaref * ctx.Jaref, jv * ctx.Jaref, 0.5 * jv * jv))
|
quad = jp.stack((0.5 * ctx.Jaref * ctx.Jaref, jv * ctx.Jaref, 0.5 * jv * jv))
|
||||||
quad = (quad * d.efc_D).T
|
quad = (quad * d.efc_D).T
|
||||||
|
|
||||||
point_fn = lambda a: _LSPoint.create(m, ctx, a, jv, quad, quad_gauss)
|
point_fn = lambda a: _LSPoint.create(d, ctx, a, jv, quad, quad_gauss)
|
||||||
|
|
||||||
def cond(ctx: _LSContext) -> jax.Array:
|
def cond(ctx: _LSContext) -> jax.Array:
|
||||||
done = ctx.ls_iter >= m.opt.ls_iterations
|
done = ctx.ls_iter >= m.opt.ls_iterations
|
||||||
@@ -283,14 +278,14 @@ def _linesearch(m: Model, d: Data, ctx: _Context) -> _Context:
|
|||||||
# 1) they are not correctly at a bracket boundary (e.g. lo.deriv_0 > 0), OR
|
# 1) they are not correctly at a bracket boundary (e.g. lo.deriv_0 > 0), OR
|
||||||
# 2) if moving to next or mid narrows the bracket
|
# 2) if moving to next or mid narrows the bracket
|
||||||
swap_lo_next = (lo.deriv_0 > 0) | (lo.deriv_0 < lo_next.deriv_0)
|
swap_lo_next = (lo.deriv_0 > 0) | (lo.deriv_0 < lo_next.deriv_0)
|
||||||
lo = jax.tree_map(lambda x, y: jp.where(swap_lo_next, y, x), lo, lo_next)
|
lo = jax.tree_util.tree_map(lambda x, y: jp.where(swap_lo_next, y, x), lo, lo_next)
|
||||||
swap_lo_mid = (mid.deriv_0 < 0) & (lo.deriv_0 < mid.deriv_0)
|
swap_lo_mid = (mid.deriv_0 < 0) & (lo.deriv_0 < mid.deriv_0)
|
||||||
lo = jax.tree_map(lambda x, y: jp.where(swap_lo_mid, y, x), lo, mid)
|
lo = jax.tree_util.tree_map(lambda x, y: jp.where(swap_lo_mid, y, x), lo, mid)
|
||||||
|
|
||||||
swap_hi_next = (hi.deriv_0 < 0) | (hi.deriv_0 > hi_next.deriv_0)
|
swap_hi_next = (hi.deriv_0 < 0) | (hi.deriv_0 > hi_next.deriv_0)
|
||||||
hi = jax.tree_map(lambda x, y: jp.where(swap_hi_next, y, x), hi, hi_next)
|
hi = jax.tree_util.tree_map(lambda x, y: jp.where(swap_hi_next, y, x), hi, hi_next)
|
||||||
swap_hi_mid = (mid.deriv_0 > 0) & (hi.deriv_0 > mid.deriv_0)
|
swap_hi_mid = (mid.deriv_0 > 0) & (hi.deriv_0 > mid.deriv_0)
|
||||||
hi = jax.tree_map(lambda x, y: jp.where(swap_hi_mid, y, x), hi, mid)
|
hi = jax.tree_util.tree_map(lambda x, y: jp.where(swap_hi_mid, y, x), hi, mid)
|
||||||
|
|
||||||
swap = swap_lo_next | swap_lo_mid | swap_hi_next | swap_hi_mid
|
swap = swap_lo_next | swap_lo_mid | swap_hi_next | swap_hi_mid
|
||||||
|
|
||||||
@@ -302,8 +297,8 @@ def _linesearch(m: Model, d: Data, ctx: _Context) -> _Context:
|
|||||||
p0 = point_fn(jp.array(0.0))
|
p0 = point_fn(jp.array(0.0))
|
||||||
lo = point_fn(p0.alpha - p0.deriv_0 / p0.deriv_1)
|
lo = point_fn(p0.alpha - p0.deriv_0 / p0.deriv_1)
|
||||||
lesser_fn = lambda x, y: jp.where(lo.deriv_0 < p0.deriv_0, x, y)
|
lesser_fn = lambda x, y: jp.where(lo.deriv_0 < p0.deriv_0, x, y)
|
||||||
hi = jax.tree_map(lesser_fn, p0, lo)
|
hi = jax.tree_util.tree_map(lesser_fn, p0, lo)
|
||||||
lo = jax.tree_map(lesser_fn, lo, p0)
|
lo = jax.tree_util.tree_map(lesser_fn, lo, p0)
|
||||||
ls_ctx = _LSContext(lo=lo, hi=hi, swap=jp.array(True), ls_iter=0)
|
ls_ctx = _LSContext(lo=lo, hi=hi, swap=jp.array(True), ls_iter=0)
|
||||||
ls_ctx = _while_loop_scan(cond, body, ls_ctx, m.opt.ls_iterations)
|
ls_ctx = _while_loop_scan(cond, body, ls_ctx, m.opt.ls_iterations)
|
||||||
|
|
||||||
@@ -336,7 +331,7 @@ def solve(m: Model, d: Data) -> Data:
|
|||||||
def body(ctx: _Context) -> _Context:
|
def body(ctx: _Context) -> _Context:
|
||||||
ctx = _linesearch(m, d, ctx)
|
ctx = _linesearch(m, d, ctx)
|
||||||
prev_grad, prev_Mgrad = ctx.grad, ctx.Mgrad # pylint: disable=invalid-name
|
prev_grad, prev_Mgrad = ctx.grad, ctx.Mgrad # pylint: disable=invalid-name
|
||||||
ctx = _update_constraint(m, d, ctx)
|
ctx = _update_constraint(d, ctx)
|
||||||
ctx = _update_gradient(m, d, ctx)
|
ctx = _update_gradient(m, d, ctx)
|
||||||
|
|
||||||
# polak-ribiere:
|
# polak-ribiere:
|
||||||
|
|||||||
@@ -25,6 +25,7 @@ from mujoco.mjx._src.types import Data
|
|||||||
from mujoco.mjx._src.types import JacobianType
|
from mujoco.mjx._src.types import JacobianType
|
||||||
from mujoco.mjx._src.types import Model
|
from mujoco.mjx._src.types import Model
|
||||||
# pylint: enable=g-importing-member
|
# pylint: enable=g-importing-member
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
|
|
||||||
def is_sparse(m: Union[mujoco.MjModel, Model]) -> bool:
|
def is_sparse(m: Union[mujoco.MjModel, Model]) -> bool:
|
||||||
@@ -149,19 +150,6 @@ def jac(
|
|||||||
return jacp, jacr
|
return jacp, jacr
|
||||||
|
|
||||||
|
|
||||||
def jac_dif_pair(
|
|
||||||
m: Model,
|
|
||||||
d: Data,
|
|
||||||
pos: jax.Array,
|
|
||||||
body_1: jax.Array,
|
|
||||||
body_2: jax.Array,
|
|
||||||
) -> jax.Array:
|
|
||||||
"""Compute Jacobian difference for two body points."""
|
|
||||||
jacp2, _ = jac(m, d, pos, body_2)
|
|
||||||
jacp1, _ = jac(m, d, pos, body_1)
|
|
||||||
return jacp2 - jacp1
|
|
||||||
|
|
||||||
|
|
||||||
def apply_ft(
|
def apply_ft(
|
||||||
m: Model,
|
m: Model,
|
||||||
d: Data,
|
d: Data,
|
||||||
@@ -200,11 +188,92 @@ def local_to_global(
|
|||||||
return pos, mat
|
return pos, mat
|
||||||
|
|
||||||
|
|
||||||
def get_custom_numeric(m: Union[Model, mujoco.MjModel], name: str) -> float:
|
def _getnum(m: Union[Model, mujoco.MjModel], obj: mujoco._enums.mjtObj) -> int:
|
||||||
"""Returns a custom numeric given an MjModel or mjx.Model."""
|
"""Gets the number of objects for the given object type."""
|
||||||
for i in range(m.nnumeric):
|
return {
|
||||||
name_ = m.names[m.name_numericadr[i] :].decode('utf-8').split('\x00', 1)[0]
|
mujoco.mjtObj.mjOBJ_BODY: m.nbody,
|
||||||
if name_ == name:
|
mujoco.mjtObj.mjOBJ_JOINT: m.njnt,
|
||||||
return m.numeric_data[m.numeric_adr[i]]
|
mujoco.mjtObj.mjOBJ_GEOM: m.ngeom,
|
||||||
|
mujoco.mjtObj.mjOBJ_SITE: m.nsite,
|
||||||
|
mujoco.mjtObj.mjOBJ_CAMERA: m.ncam,
|
||||||
|
mujoco.mjtObj.mjOBJ_MESH: m.nmesh,
|
||||||
|
mujoco.mjtObj.mjOBJ_PAIR: m.npair,
|
||||||
|
mujoco.mjtObj.mjOBJ_EQUALITY: m.neq,
|
||||||
|
mujoco.mjtObj.mjOBJ_ACTUATOR: m.nu,
|
||||||
|
mujoco.mjtObj.mjOBJ_SENSOR: m.nsensor,
|
||||||
|
mujoco.mjtObj.mjOBJ_NUMERIC: m.nnumeric,
|
||||||
|
mujoco.mjtObj.mjOBJ_TUPLE: m.ntuple,
|
||||||
|
mujoco.mjtObj.mjOBJ_KEY: m.nkey,
|
||||||
|
}.get(obj, 0)
|
||||||
|
|
||||||
return -1
|
|
||||||
|
def _getadr(
|
||||||
|
m: Union[Model, mujoco.MjModel], obj: mujoco._enums.mjtObj
|
||||||
|
) -> np.ndarray:
|
||||||
|
"""Gets the name addresses for the given object type."""
|
||||||
|
return {
|
||||||
|
mujoco.mjtObj.mjOBJ_BODY: m.name_bodyadr,
|
||||||
|
mujoco.mjtObj.mjOBJ_JOINT: m.name_jntadr,
|
||||||
|
mujoco.mjtObj.mjOBJ_GEOM: m.name_geomadr,
|
||||||
|
mujoco.mjtObj.mjOBJ_SITE: m.name_siteadr,
|
||||||
|
mujoco.mjtObj.mjOBJ_CAMERA: m.name_camadr,
|
||||||
|
mujoco.mjtObj.mjOBJ_MESH: m.name_meshadr,
|
||||||
|
mujoco.mjtObj.mjOBJ_PAIR: m.name_pairadr,
|
||||||
|
mujoco.mjtObj.mjOBJ_EQUALITY: m.name_eqadr,
|
||||||
|
mujoco.mjtObj.mjOBJ_ACTUATOR: m.name_actuatoradr,
|
||||||
|
mujoco.mjtObj.mjOBJ_SENSOR: m.name_sensoradr,
|
||||||
|
mujoco.mjtObj.mjOBJ_NUMERIC: m.name_numericadr,
|
||||||
|
mujoco.mjtObj.mjOBJ_TUPLE: m.name_tupleadr,
|
||||||
|
mujoco.mjtObj.mjOBJ_KEY: m.name_keyadr,
|
||||||
|
}[obj]
|
||||||
|
|
||||||
|
|
||||||
|
def id2name(
|
||||||
|
m: Union[Model, mujoco.MjModel], typ: mujoco._enums.mjtObj, i: int
|
||||||
|
) -> Optional[str]:
|
||||||
|
"""Gets the name of an object with the specified mjtObj type and id.
|
||||||
|
|
||||||
|
See mujoco.id2name for more info.
|
||||||
|
|
||||||
|
Args:
|
||||||
|
m: mujoco.MjModel or mjx.Model
|
||||||
|
typ: mujoco.mjtObj type
|
||||||
|
i: the id
|
||||||
|
|
||||||
|
Returns:
|
||||||
|
the name string, or None if not found
|
||||||
|
"""
|
||||||
|
num = _getnum(m, typ)
|
||||||
|
if i < 0 or i >= num:
|
||||||
|
return None
|
||||||
|
|
||||||
|
adr = _getadr(m, typ)
|
||||||
|
name = m.names[adr[i] :].decode('utf-8').split('\x00', 1)[0]
|
||||||
|
return name or None
|
||||||
|
|
||||||
|
|
||||||
|
def name2id(
|
||||||
|
m: Union[Model, mujoco.MjModel], typ: mujoco._enums.mjtObj, name: str
|
||||||
|
) -> int:
|
||||||
|
"""Gets the id of an object with the specified mjtObj type and name.
|
||||||
|
|
||||||
|
See mujoco.mj_name2id for more info.
|
||||||
|
|
||||||
|
Args:
|
||||||
|
m: mujoco.MjModel or mjx.Model
|
||||||
|
typ: mujoco.mjtObj type
|
||||||
|
name: the name of the object
|
||||||
|
|
||||||
|
Returns:
|
||||||
|
the id, or -1 if not found
|
||||||
|
"""
|
||||||
|
num = _getnum(m, typ)
|
||||||
|
adr = _getadr(m, typ)
|
||||||
|
|
||||||
|
# TODO: consider using MjModel.names_map instead
|
||||||
|
names_map = {
|
||||||
|
m.names[adr[i] :].decode('utf-8').split('\x00', 1)[0]: i
|
||||||
|
for i in range(num)
|
||||||
|
}
|
||||||
|
|
||||||
|
return names_map.get(name, -1)
|
||||||
|
|||||||
@@ -116,7 +116,7 @@ class SupportTest(parameterized.TestCase):
|
|||||||
|
|
||||||
np.testing.assert_almost_equal(qfrc, qfrc_expected, 6)
|
np.testing.assert_almost_equal(qfrc, qfrc_expected, 6)
|
||||||
|
|
||||||
def test_custom_numeric(self):
|
def test_custom(self):
|
||||||
xml = """
|
xml = """
|
||||||
<mujoco model="right_shadow_hand">
|
<mujoco model="right_shadow_hand">
|
||||||
<custom>
|
<custom>
|
||||||
@@ -126,9 +126,36 @@ class SupportTest(parameterized.TestCase):
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
"""
|
"""
|
||||||
m = mujoco.MjModel.from_xml_string(xml)
|
m = mujoco.MjModel.from_xml_string(xml)
|
||||||
self.assertEqual(support.get_custom_numeric(m, 'something'), -1)
|
|
||||||
self.assertEqual(support.get_custom_numeric(m, 'max_contact_points'), 15)
|
def _get_numeric(m, name):
|
||||||
self.assertEqual(support.get_custom_numeric(m, 'max_geom_pairs'), 42)
|
id_ = support.name2id(m, mujoco.mjtObj.mjOBJ_NUMERIC, name)
|
||||||
|
return int(m.numeric_data[id_]) if id_ >= 0 else -1
|
||||||
|
|
||||||
|
self.assertEqual(_get_numeric(m, 'something'), -1)
|
||||||
|
self.assertEqual(_get_numeric(m, 'max_contact_points'), 15)
|
||||||
|
self.assertEqual(_get_numeric(m, 'max_geom_pairs'), 42)
|
||||||
|
|
||||||
|
mx = mjx.put_model(m)
|
||||||
|
self.assertEqual(_get_numeric(mx, 'something'), -1)
|
||||||
|
self.assertEqual(_get_numeric(mx, 'max_contact_points'), 15)
|
||||||
|
self.assertEqual(_get_numeric(mx, 'max_geom_pairs'), 42)
|
||||||
|
|
||||||
|
def test_names_and_ids(self):
|
||||||
|
m = test_util.load_test_file('pendula.xml')
|
||||||
|
mx = mjx.put_model(m)
|
||||||
|
|
||||||
|
nums = {
|
||||||
|
mujoco.mjtObj.mjOBJ_JOINT: m.njnt,
|
||||||
|
mujoco.mjtObj.mjOBJ_GEOM: m.ngeom,
|
||||||
|
mujoco.mjtObj.mjOBJ_BODY: m.nbody,
|
||||||
|
}
|
||||||
|
|
||||||
|
for obj in nums:
|
||||||
|
names = [mujoco.mj_id2name(m, obj.value, i) for i in range(nums[obj])]
|
||||||
|
for i, n in enumerate(names):
|
||||||
|
self.assertEqual(support.id2name(mx, obj, i), n)
|
||||||
|
i = i if n is not None else -1
|
||||||
|
self.assertEqual(support.name2id(mx, obj, n), i)
|
||||||
|
|
||||||
|
|
||||||
if __name__ == '__main__':
|
if __name__ == '__main__':
|
||||||
|
|||||||
@@ -17,7 +17,7 @@
|
|||||||
import os
|
import os
|
||||||
import sys
|
import sys
|
||||||
import time
|
import time
|
||||||
from typing import Dict, Tuple
|
from typing import Dict, Optional, Tuple
|
||||||
from xml.etree import ElementTree as ET
|
from xml.etree import ElementTree as ET
|
||||||
|
|
||||||
from etils import epath
|
from etils import epath
|
||||||
@@ -152,6 +152,7 @@ def _make_joint(joint_type: str, name: str) -> Dict[str, str]:
|
|||||||
joint_attr['damping'] = '{:.2f}'.format(np.random.uniform() * 20)
|
joint_attr['damping'] = '{:.2f}'.format(np.random.uniform() * 20)
|
||||||
joint_attr['stiffness'] = '{:.2f}'.format(np.random.uniform() * 20)
|
joint_attr['stiffness'] = '{:.2f}'.format(np.random.uniform() * 20)
|
||||||
|
|
||||||
|
joint_attr['actuatorgravcomp'] = np.random.choice(['true', 'false'])
|
||||||
return joint_attr
|
return joint_attr
|
||||||
|
|
||||||
|
|
||||||
@@ -201,9 +202,9 @@ def _make_geom(
|
|||||||
|
|
||||||
def _make_actuator(
|
def _make_actuator(
|
||||||
actuator_type: str,
|
actuator_type: str,
|
||||||
joint: str | None = None,
|
joint: Optional[str] = None,
|
||||||
site: str | None = None,
|
site: Optional[str] = None,
|
||||||
refsite: str | None = None,
|
refsite: Optional[str] = None,
|
||||||
) -> Dict[str, str]:
|
) -> Dict[str, str]:
|
||||||
"""Returns attributes for an actuator."""
|
"""Returns attributes for an actuator."""
|
||||||
if joint:
|
if joint:
|
||||||
@@ -334,7 +335,16 @@ def create_mjcf(
|
|||||||
z_pos = np.random.uniform(low=-1, high=1) * 0.01 # small jitter
|
z_pos = np.random.uniform(low=-1, high=1) * 0.01 # small jitter
|
||||||
pos = f'{body_pos[0]:.3f} {body_pos[1]:.3f} {body_pos[2] + z_pos:.3f}'
|
pos = f'{body_pos[0]:.3f} {body_pos[1]:.3f} {body_pos[2] + z_pos:.3f}'
|
||||||
n_bodies = len(list(mjcf.iter('body')))
|
n_bodies = len(list(mjcf.iter('body')))
|
||||||
child = ET.SubElement(body, 'body', {'pos': pos, 'name': f'body{n_bodies}'})
|
gravcomp = np.random.uniform() * p(50)
|
||||||
|
child = ET.SubElement(
|
||||||
|
body,
|
||||||
|
'body',
|
||||||
|
{
|
||||||
|
'pos': pos,
|
||||||
|
'name': f'body{n_bodies}',
|
||||||
|
'gravcomp': f'{gravcomp:.3f}',
|
||||||
|
},
|
||||||
|
)
|
||||||
ET.SubElement(child, 'site', {'name': f'site{n_bodies}'})
|
ET.SubElement(child, 'site', {'name': f'site{n_bodies}'})
|
||||||
|
|
||||||
n_joints = len(list(mjcf.iter('joint')))
|
n_joints = len(list(mjcf.iter('joint')))
|
||||||
|
|||||||
+109
-41
@@ -15,10 +15,8 @@
|
|||||||
"""Base types used in MJX."""
|
"""Base types used in MJX."""
|
||||||
|
|
||||||
import enum
|
import enum
|
||||||
from typing import List, Optional
|
|
||||||
|
|
||||||
import jax
|
import jax
|
||||||
import jax.numpy as jp
|
|
||||||
import mujoco
|
import mujoco
|
||||||
from mujoco.mjx._src.dataclasses import PyTreeNode # pylint: disable=g-importing-member
|
from mujoco.mjx._src.dataclasses import PyTreeNode # pylint: disable=g-importing-member
|
||||||
import numpy as np
|
import numpy as np
|
||||||
@@ -100,6 +98,7 @@ class GeomType(enum.IntEnum):
|
|||||||
CYLINDER: cylinder
|
CYLINDER: cylinder
|
||||||
BOX: box
|
BOX: box
|
||||||
MESH: mesh
|
MESH: mesh
|
||||||
|
SDF: signed distance field
|
||||||
"""
|
"""
|
||||||
|
|
||||||
PLANE = mujoco.mjtGeom.mjGEOM_PLANE
|
PLANE = mujoco.mjtGeom.mjGEOM_PLANE
|
||||||
@@ -213,6 +212,24 @@ class BiasType(enum.IntEnum):
|
|||||||
# unsupported: MUSCLE, USER
|
# unsupported: MUSCLE, USER
|
||||||
|
|
||||||
|
|
||||||
|
class ConstraintType(enum.IntEnum):
|
||||||
|
"""Type of constraint.
|
||||||
|
|
||||||
|
Attributes:
|
||||||
|
EQUALITY: equality constraint
|
||||||
|
LIMIT_JOINT: joint limit
|
||||||
|
CONTACT_FRICTIONLESS: frictionless contact
|
||||||
|
CONTACT_PYRAMIDAL: frictional contact, pyramidal friction cone
|
||||||
|
"""
|
||||||
|
EQUALITY = mujoco.mjtConstraint.mjCNSTR_EQUALITY
|
||||||
|
# unsupported: FRICTION_DOF, FRICTION_TENDON
|
||||||
|
LIMIT_JOINT = mujoco.mjtConstraint.mjCNSTR_LIMIT_JOINT
|
||||||
|
# unsupported: LIMIT_TENDON
|
||||||
|
CONTACT_FRICTIONLESS = mujoco.mjtConstraint.mjCNSTR_CONTACT_FRICTIONLESS
|
||||||
|
CONTACT_PYRAMIDAL = mujoco.mjtConstraint.mjCNSTR_CONTACT_PYRAMIDAL
|
||||||
|
# unsupported: CONTACT_ELLIPTIC
|
||||||
|
|
||||||
|
|
||||||
class CamLightType(enum.IntEnum):
|
class CamLightType(enum.IntEnum):
|
||||||
"""Type of camera light.
|
"""Type of camera light.
|
||||||
|
|
||||||
@@ -309,7 +326,12 @@ class Model(PyTreeNode):
|
|||||||
npair: number of predefined geom pairs
|
npair: number of predefined geom pairs
|
||||||
nexclude: number of excluded geom pairs
|
nexclude: number of excluded geom pairs
|
||||||
neq: number of equality constraints
|
neq: number of equality constraints
|
||||||
|
ngravcomp: number of bodies with nonzero gravcomp
|
||||||
nnumeric: number of numeric custom fields
|
nnumeric: number of numeric custom fields
|
||||||
|
ntuple: number of tuple custom fields
|
||||||
|
nsensor: number of sensors
|
||||||
|
nkey: number of keyframes
|
||||||
|
nuserdata: size of userdata array
|
||||||
nM: number of non-zeros in sparse inertia matrix
|
nM: number of non-zeros in sparse inertia matrix
|
||||||
opt: physics options
|
opt: physics options
|
||||||
stat: model statistics
|
stat: model statistics
|
||||||
@@ -331,6 +353,7 @@ class Model(PyTreeNode):
|
|||||||
body_mass: mass (nbody,)
|
body_mass: mass (nbody,)
|
||||||
body_subtreemass: mass of subtree starting at this body (nbody,)
|
body_subtreemass: mass of subtree starting at this body (nbody,)
|
||||||
body_inertia: diagonal inertia in ipos/iquat frame (nbody, 3)
|
body_inertia: diagonal inertia in ipos/iquat frame (nbody, 3)
|
||||||
|
body_gravcomp: antigravity force, units of body weight (nbody,)
|
||||||
body_invweight0: mean inv inert in qpos0 (trn, rot) (nbody, 2)
|
body_invweight0: mean inv inert in qpos0 (trn, rot) (nbody, 2)
|
||||||
jnt_type: type of joint (mjtJoint) (njnt,)
|
jnt_type: type of joint (mjtJoint) (njnt,)
|
||||||
jnt_qposadr: start addr in 'qpos' for joint's data (njnt,)
|
jnt_qposadr: start addr in 'qpos' for joint's data (njnt,)
|
||||||
@@ -338,6 +361,8 @@ class Model(PyTreeNode):
|
|||||||
jnt_bodyid: id of joint's body (njnt,)
|
jnt_bodyid: id of joint's body (njnt,)
|
||||||
jnt_group: group for visibility (njnt,)
|
jnt_group: group for visibility (njnt,)
|
||||||
jnt_limited: does joint have limits (njnt,)
|
jnt_limited: does joint have limits (njnt,)
|
||||||
|
jnt_actfrclimited: does joint have actuator force limits (njnt,)
|
||||||
|
jnt_actgravcomp: is gravcomp force applied via actuators (njnt,)
|
||||||
jnt_solref: constraint solver reference: limit (njnt, mjNREF)
|
jnt_solref: constraint solver reference: limit (njnt, mjNREF)
|
||||||
jnt_solimp: constraint solver impedance: limit (njnt, mjNIMP)
|
jnt_solimp: constraint solver impedance: limit (njnt, mjNIMP)
|
||||||
jnt_pos: local anchor position (njnt, 3)
|
jnt_pos: local anchor position (njnt, 3)
|
||||||
@@ -370,6 +395,7 @@ class Model(PyTreeNode):
|
|||||||
geom_solref: constraint solver reference: contact (ngeom, mjNREF)
|
geom_solref: constraint solver reference: contact (ngeom, mjNREF)
|
||||||
geom_solimp: constraint solver impedance: contact (ngeom, mjNIMP)
|
geom_solimp: constraint solver impedance: contact (ngeom, mjNIMP)
|
||||||
geom_size: geom-specific size parameters (ngeom, 3)
|
geom_size: geom-specific size parameters (ngeom, 3)
|
||||||
|
geom_rbound: radius of bounding sphere (ngeom,)
|
||||||
geom_pos: local position offset rel. to body (ngeom, 3)
|
geom_pos: local position offset rel. to body (ngeom, 3)
|
||||||
geom_quat: local orientation offset rel. to body (ngeom, 4)
|
geom_quat: local orientation offset rel. to body (ngeom, 4)
|
||||||
geom_friction: friction for (slide, spin, roll) (ngeom, 3)
|
geom_friction: friction for (slide, spin, roll) (ngeom, 3)
|
||||||
@@ -387,17 +413,13 @@ class Model(PyTreeNode):
|
|||||||
cam_poscom0: global position rel. to sub-com in qpos0 (ncam, 3)
|
cam_poscom0: global position rel. to sub-com in qpos0 (ncam, 3)
|
||||||
cam_pos0: global position rel. to body in qpos0 (ncam, 3)
|
cam_pos0: global position rel. to body in qpos0 (ncam, 3)
|
||||||
cam_mat0: global orientation in qpos0 (ncam, 9)
|
cam_mat0: global orientation in qpos0 (ncam, 9)
|
||||||
mat_rgba: rgba (nmat, 4)
|
mesh_vertadr: first vertex address (nmesh,)
|
||||||
mesh_vertadr: first vertex address (nmesh x 1)
|
mesh_faceadr: first face address (nmesh,)
|
||||||
mesh_faceadr: first face address (nmesh x 1)
|
mesh_graphadr: graph data address; -1: no graph (nmesh,)
|
||||||
mesh_vert: vertex positions for all meshes (nmeshvert, 3)
|
mesh_vert: vertex positions for all meshes (nmeshvert, 3)
|
||||||
mesh_face: vertex face data (nmeshface, 3)
|
mesh_face: vertex face data (nmeshface, 3)
|
||||||
geom_convex_face: vertex face data, MJX only (ngeom,)
|
mesh_graph: convex graph data (nmeshgraph,)
|
||||||
geom_convex_vert: vertex data, MJX only (ngeom,)
|
mat_rgba: rgba (nmat, 4)
|
||||||
geom_convex_edge_dir: unique edge direction, MJX only (ngeom,)
|
|
||||||
geom_convex_facenormal: normal face data, MJX only (ngeom,)
|
|
||||||
geom_convex_face_edge: edges for each face (ngeom,)
|
|
||||||
geom_convex_face_edge_normal: edge normals for each face (ngeom,)
|
|
||||||
pair_dim: contact dimensionality (npair,)
|
pair_dim: contact dimensionality (npair,)
|
||||||
pair_geom1: id of geom1 (npair,)
|
pair_geom1: id of geom1 (npair,)
|
||||||
pair_geom2: id of geom2 (npair,)
|
pair_geom2: id of geom2 (npair,)
|
||||||
@@ -434,7 +456,24 @@ class Model(PyTreeNode):
|
|||||||
actuator_gear: scale length and transmitted force (nu, 6)
|
actuator_gear: scale length and transmitted force (nu, 6)
|
||||||
numeric_adr: address of field in numeric_data (nnumeric,)
|
numeric_adr: address of field in numeric_data (nnumeric,)
|
||||||
numeric_data: array of all numeric fields (nnumericdata,)
|
numeric_data: array of all numeric fields (nnumericdata,)
|
||||||
|
tuple_adr: address of text in text_data (ntuple,)
|
||||||
|
tuple_size: number of objects in tuple (ntuple,)
|
||||||
|
tuple_objtype: array of object types in all tuples (ntupledata,)
|
||||||
|
tuple_objid: array of object ids in all tuples (ntupledata,)
|
||||||
|
tuple_objprm: array of object params in all tuples (ntupledata,)
|
||||||
|
name_bodyadr: body name pointers (nbody,)
|
||||||
|
name_jntadr: joint name pointers (njnt,)
|
||||||
|
name_geomadr: geom name pointers (ngeom,)
|
||||||
|
name_siteadr: site name pointers (nsite,)
|
||||||
|
name_camadr: camera name pointers (ncam,)
|
||||||
|
name_meshadr: mesh name pointers (nmesh,)
|
||||||
|
name_pairadr: geom pair name pointers (npair,)
|
||||||
|
name_eqadr: equality constraint name pointers (neq,)
|
||||||
|
name_actuatoradr: actuator name pointers (nu,)
|
||||||
|
name_sensoradr: sensor name pointers (nsensor,)
|
||||||
name_numericadr: numeric name pointers (nnumeric,)
|
name_numericadr: numeric name pointers (nnumeric,)
|
||||||
|
name_tupleadr: tuple name pointers (ntuple,)
|
||||||
|
name_keyadr: keyframe name pointers (nkey,)
|
||||||
names: names of all objects, 0-terminated (nnames,)
|
names: names of all objects, 0-terminated (nnames,)
|
||||||
"""
|
"""
|
||||||
nq: int
|
nq: int
|
||||||
@@ -453,7 +492,12 @@ class Model(PyTreeNode):
|
|||||||
npair: int
|
npair: int
|
||||||
nexclude: int
|
nexclude: int
|
||||||
neq: int
|
neq: int
|
||||||
|
ngravcomp: int
|
||||||
nnumeric: int
|
nnumeric: int
|
||||||
|
nuserdata: int
|
||||||
|
ntuple: int
|
||||||
|
nsensor: int
|
||||||
|
nkey: int
|
||||||
nM: int # pylint:disable=invalid-name
|
nM: int # pylint:disable=invalid-name
|
||||||
opt: Option
|
opt: Option
|
||||||
stat: Statistic
|
stat: Statistic
|
||||||
@@ -475,6 +519,7 @@ class Model(PyTreeNode):
|
|||||||
body_mass: jax.Array
|
body_mass: jax.Array
|
||||||
body_subtreemass: jax.Array
|
body_subtreemass: jax.Array
|
||||||
body_inertia: jax.Array
|
body_inertia: jax.Array
|
||||||
|
body_gravcomp: jax.Array
|
||||||
body_invweight0: jax.Array
|
body_invweight0: jax.Array
|
||||||
jnt_type: np.ndarray
|
jnt_type: np.ndarray
|
||||||
jnt_qposadr: np.ndarray
|
jnt_qposadr: np.ndarray
|
||||||
@@ -482,6 +527,7 @@ class Model(PyTreeNode):
|
|||||||
jnt_bodyid: np.ndarray
|
jnt_bodyid: np.ndarray
|
||||||
jnt_limited: np.ndarray
|
jnt_limited: np.ndarray
|
||||||
jnt_actfrclimited: np.ndarray
|
jnt_actfrclimited: np.ndarray
|
||||||
|
jnt_actgravcomp: np.ndarray
|
||||||
jnt_solref: jax.Array
|
jnt_solref: jax.Array
|
||||||
jnt_solimp: jax.Array
|
jnt_solimp: jax.Array
|
||||||
jnt_pos: jax.Array
|
jnt_pos: jax.Array
|
||||||
@@ -514,6 +560,7 @@ class Model(PyTreeNode):
|
|||||||
geom_solref: jax.Array
|
geom_solref: jax.Array
|
||||||
geom_solimp: jax.Array
|
geom_solimp: jax.Array
|
||||||
geom_size: jax.Array
|
geom_size: jax.Array
|
||||||
|
geom_rbound: jax.Array
|
||||||
geom_pos: jax.Array
|
geom_pos: jax.Array
|
||||||
geom_quat: jax.Array
|
geom_quat: jax.Array
|
||||||
geom_friction: jax.Array
|
geom_friction: jax.Array
|
||||||
@@ -533,18 +580,14 @@ class Model(PyTreeNode):
|
|||||||
cam_mat0: jax.Array
|
cam_mat0: jax.Array
|
||||||
mesh_vertadr: np.ndarray
|
mesh_vertadr: np.ndarray
|
||||||
mesh_faceadr: np.ndarray
|
mesh_faceadr: np.ndarray
|
||||||
|
mesh_graphadr: np.ndarray
|
||||||
mesh_vert: np.ndarray
|
mesh_vert: np.ndarray
|
||||||
mesh_face: np.ndarray
|
mesh_face: np.ndarray
|
||||||
|
mesh_graph: np.ndarray
|
||||||
mat_rgba: np.ndarray
|
mat_rgba: np.ndarray
|
||||||
pair_dim: np.ndarray
|
pair_dim: np.ndarray
|
||||||
pair_geom1: np.ndarray
|
pair_geom1: np.ndarray
|
||||||
pair_geom2: np.ndarray
|
pair_geom2: np.ndarray
|
||||||
geom_convex_face: List[Optional[jax.Array]]
|
|
||||||
geom_convex_vert: List[Optional[jax.Array]]
|
|
||||||
geom_convex_edge_dir: List[Optional[jax.Array]]
|
|
||||||
geom_convex_facenormal: 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_solref: jax.Array
|
||||||
pair_solreffriction: jax.Array
|
pair_solreffriction: jax.Array
|
||||||
pair_solimp: jax.Array
|
pair_solimp: jax.Array
|
||||||
@@ -578,7 +621,24 @@ class Model(PyTreeNode):
|
|||||||
actuator_gear: jax.Array
|
actuator_gear: jax.Array
|
||||||
numeric_adr: np.ndarray
|
numeric_adr: np.ndarray
|
||||||
numeric_data: np.ndarray
|
numeric_data: np.ndarray
|
||||||
|
tuple_adr: np.ndarray
|
||||||
|
tuple_size: np.ndarray
|
||||||
|
tuple_objtype: np.ndarray
|
||||||
|
tuple_objid: np.ndarray
|
||||||
|
tuple_objprm: np.ndarray
|
||||||
|
name_bodyadr: np.ndarray
|
||||||
|
name_jntadr: np.ndarray
|
||||||
|
name_geomadr: np.ndarray
|
||||||
|
name_siteadr: np.ndarray
|
||||||
|
name_camadr: np.ndarray
|
||||||
|
name_meshadr: np.ndarray
|
||||||
|
name_pairadr: np.ndarray
|
||||||
|
name_eqadr: np.ndarray
|
||||||
|
name_actuatoradr: np.ndarray
|
||||||
|
name_sensoradr: np.ndarray
|
||||||
name_numericadr: np.ndarray
|
name_numericadr: np.ndarray
|
||||||
|
name_tupleadr: np.ndarray
|
||||||
|
name_keyadr: np.ndarray
|
||||||
names: bytes
|
names: bytes
|
||||||
|
|
||||||
|
|
||||||
@@ -594,8 +654,11 @@ class Contact(PyTreeNode):
|
|||||||
solref: constraint solver reference, normal direction (mjNREF,)
|
solref: constraint solver reference, normal direction (mjNREF,)
|
||||||
solreffriction: constraint solver reference, friction directions (mjNREF,)
|
solreffriction: constraint solver reference, friction directions (mjNREF,)
|
||||||
solimp: constraint solver impedance (mjNIMP,)
|
solimp: constraint solver impedance (mjNIMP,)
|
||||||
geom1: id of geom 1
|
dim: contact space dimensionality: 1, 3, 4, or 6
|
||||||
geom2: id of geom 2
|
geom1: id of geom 1; deprecated, use geom[0]
|
||||||
|
geom2: id of geom 2; deprecated, use geom[1]
|
||||||
|
geom: geom ids (2,)
|
||||||
|
efc_address: address in efc; -1: not included
|
||||||
"""
|
"""
|
||||||
dist: jax.Array
|
dist: jax.Array
|
||||||
pos: jax.Array
|
pos: jax.Array
|
||||||
@@ -605,33 +668,25 @@ class Contact(PyTreeNode):
|
|||||||
solref: jax.Array
|
solref: jax.Array
|
||||||
solreffriction: jax.Array
|
solreffriction: jax.Array
|
||||||
solimp: jax.Array
|
solimp: jax.Array
|
||||||
# unsupported: mu, H, dim
|
# unsupported: mu, H
|
||||||
|
dim: np.ndarray
|
||||||
geom1: jax.Array
|
geom1: jax.Array
|
||||||
geom2: jax.Array
|
geom2: jax.Array
|
||||||
# unsupported: efc_address, exclude
|
geom: jax.Array
|
||||||
|
# unsupported: flex, elem, vert, exclude
|
||||||
@classmethod
|
efc_address: np.ndarray
|
||||||
def zero(cls, ncon: int = 0) -> 'Contact':
|
|
||||||
"""Returns a contact filled with zeros."""
|
|
||||||
return Contact(
|
|
||||||
dist=jp.zeros(ncon),
|
|
||||||
pos=jp.zeros((ncon, 3,)),
|
|
||||||
frame=jp.zeros((ncon, 3, 3)),
|
|
||||||
includemargin=jp.zeros(ncon),
|
|
||||||
friction=jp.zeros((ncon, 5)),
|
|
||||||
solref=jp.zeros((ncon, mujoco.mjNREF)),
|
|
||||||
solreffriction=jp.zeros((ncon, mujoco.mjNREF)),
|
|
||||||
solimp=jp.zeros((ncon, mujoco.mjNIMP,)),
|
|
||||||
geom1=jp.zeros(ncon, dtype=int),
|
|
||||||
geom2=jp.zeros(ncon, dtype=int),
|
|
||||||
)
|
|
||||||
|
|
||||||
|
|
||||||
class Data(PyTreeNode):
|
class Data(PyTreeNode):
|
||||||
r"""Dynamic state that updates each step.\
|
r"""\Dynamic state that updates each step.
|
||||||
|
|
||||||
Attributes:
|
Attributes:
|
||||||
solver_niter: number of solver iterations, per island (mjNISLAND,)
|
ne: number of equality constraints
|
||||||
|
nf: number of friction constraints
|
||||||
|
nl: number of limit constraints
|
||||||
|
nefc: number of constraints
|
||||||
|
ncon: number of contacts
|
||||||
|
solver_niter: number of solver iterations
|
||||||
time: simulation time
|
time: simulation time
|
||||||
qpos: position (nq,)
|
qpos: position (nq,)
|
||||||
qvel: velocity (nv,)
|
qvel: velocity (nv,)
|
||||||
@@ -661,14 +716,15 @@ class Data(PyTreeNode):
|
|||||||
cinert: com-based body inertia and mass (nbody, 10)
|
cinert: com-based body inertia and mass (nbody, 10)
|
||||||
actuator_length: actuator lengths (nu,)
|
actuator_length: actuator lengths (nu,)
|
||||||
actuator_moment: actuator moments (nu, nv)
|
actuator_moment: actuator moments (nu, nv)
|
||||||
crb: com-based composite inertia and mass (nbody, 10)
|
crb: com-based composite inertia and mass (nbody, 10) \
|
||||||
qM: total inertia if sparse: (nM,)
|
qM: total inertia if sparse: (nM,)
|
||||||
if dense: (nv, nv)
|
if dense: (nv, nv)
|
||||||
qLD: L'*D*L (or Cholesky) factorization of M. if sparse: (nM,)
|
qLD: L'*D*L (or Cholesky) factorization of M. if sparse: (nM,)
|
||||||
if dense: (nv, nv)
|
if dense: (nv, nv)
|
||||||
qLDiagInv: 1/diag(D) if sparse: (nv,)
|
qLDiagInv: 1/diag(D) if sparse: (nv,)
|
||||||
if dense: (0,)
|
if dense: (0,)
|
||||||
contact: list of all detected contacts (ncon,)
|
contact: all detected contacts (ncon,)
|
||||||
|
efc_type: constraint type (nefc,)
|
||||||
efc_J: constraint Jacobian (nefc, nv)
|
efc_J: constraint Jacobian (nefc, nv)
|
||||||
efc_frictionloss: frictionloss (friction) (nefc,)
|
efc_frictionloss: frictionloss (friction) (nefc,)
|
||||||
efc_D: constraint mass (nefc,)
|
efc_D: constraint mass (nefc,)
|
||||||
@@ -676,6 +732,7 @@ class Data(PyTreeNode):
|
|||||||
cvel: com-based velocity [3D rot; 3D tran] (nbody, 6)
|
cvel: com-based velocity [3D rot; 3D tran] (nbody, 6)
|
||||||
cdof_dot: time-derivative of cdof (nv, 6)
|
cdof_dot: time-derivative of cdof (nv, 6)
|
||||||
qfrc_bias: C(qpos,qvel) (nv,)
|
qfrc_bias: C(qpos,qvel) (nv,)
|
||||||
|
qfrc_gravcomp: passive gravity compensation force (nv,)
|
||||||
qfrc_passive: passive force (nv,)
|
qfrc_passive: passive force (nv,)
|
||||||
efc_aref: reference pseudo-acceleration (nefc,)
|
efc_aref: reference pseudo-acceleration (nefc,)
|
||||||
qfrc_actuator: actuator force (nv,)
|
qfrc_actuator: actuator force (nv,)
|
||||||
@@ -685,7 +742,14 @@ class Data(PyTreeNode):
|
|||||||
qfrc_inverse: net external force; should equal: (nv,)
|
qfrc_inverse: net external force; should equal: (nv,)
|
||||||
qfrc_applied + J'*xfrc_applied + qfrc_actuator
|
qfrc_applied + J'*xfrc_applied + qfrc_actuator
|
||||||
efc_force: constraint force in constraint space (nefc,)
|
efc_force: constraint force in constraint space (nefc,)
|
||||||
|
userdata: user data, not touched by engine (nuserdata,)
|
||||||
"""
|
"""
|
||||||
|
# constant sizes:
|
||||||
|
ne: int
|
||||||
|
nf: int
|
||||||
|
nl: int
|
||||||
|
nefc: int
|
||||||
|
ncon: int
|
||||||
# solver statistics:
|
# solver statistics:
|
||||||
solver_niter: jax.Array
|
solver_niter: jax.Array
|
||||||
# global properties:
|
# global properties:
|
||||||
@@ -703,6 +767,8 @@ class Data(PyTreeNode):
|
|||||||
# dynamics:
|
# dynamics:
|
||||||
qacc: jax.Array
|
qacc: jax.Array
|
||||||
act_dot: jax.Array
|
act_dot: jax.Array
|
||||||
|
# user data:
|
||||||
|
userdata: jax.Array
|
||||||
# position dependent:
|
# position dependent:
|
||||||
xpos: jax.Array
|
xpos: jax.Array
|
||||||
xquat: jax.Array
|
xquat: jax.Array
|
||||||
@@ -727,6 +793,7 @@ class Data(PyTreeNode):
|
|||||||
qLD: jax.Array # pylint:disable=invalid-name
|
qLD: jax.Array # pylint:disable=invalid-name
|
||||||
qLDiagInv: jax.Array # pylint:disable=invalid-name
|
qLDiagInv: jax.Array # pylint:disable=invalid-name
|
||||||
contact: Contact
|
contact: Contact
|
||||||
|
efc_type: np.ndarray
|
||||||
efc_J: jax.Array # pylint:disable=invalid-name
|
efc_J: jax.Array # pylint:disable=invalid-name
|
||||||
efc_frictionloss: jax.Array
|
efc_frictionloss: jax.Array
|
||||||
efc_D: jax.Array # pylint:disable=invalid-name
|
efc_D: jax.Array # pylint:disable=invalid-name
|
||||||
@@ -736,6 +803,7 @@ class Data(PyTreeNode):
|
|||||||
cdof_dot: jax.Array
|
cdof_dot: jax.Array
|
||||||
qfrc_bias: jax.Array
|
qfrc_bias: jax.Array
|
||||||
qfrc_passive: jax.Array
|
qfrc_passive: jax.Array
|
||||||
|
qfrc_gravcomp: jax.Array
|
||||||
efc_aref: jax.Array
|
efc_aref: jax.Array
|
||||||
# position, velcoity, control & acceleration dependent:
|
# position, velcoity, control & acceleration dependent:
|
||||||
qfrc_actuator: jax.Array
|
qfrc_actuator: jax.Array
|
||||||
|
|||||||
@@ -30,7 +30,10 @@ import numpy as np
|
|||||||
|
|
||||||
def _assert_attr_eq(mjx_d, mj_d, attr, name, atol):
|
def _assert_attr_eq(mjx_d, mj_d, attr, name, atol):
|
||||||
if attr == 'efc_address':
|
if attr == 'efc_address':
|
||||||
# we do not test efc_address since it gets set in constraint logic
|
# contact order not guaranteed to match
|
||||||
|
np.testing.assert_array_equal(
|
||||||
|
np.sort(mjx_d.efc_address), np.sort(mj_d.efc_address)
|
||||||
|
)
|
||||||
return
|
return
|
||||||
err_msg = f'mismatch: {attr} in run: {name}'
|
err_msg = f'mismatch: {attr} in run: {name}'
|
||||||
mjx_d, mj_d = getattr(mjx_d, attr), getattr(mj_d, attr)
|
mjx_d, mj_d = getattr(mjx_d, attr), getattr(mj_d, attr)
|
||||||
@@ -79,9 +82,12 @@ class CollisionDriverIntegrationTest(parameterized.TestCase):
|
|||||||
self.assertSequenceEqual(set(idx_mjx), set(idx_mj))
|
self.assertSequenceEqual(set(idx_mjx), set(idx_mj))
|
||||||
idx = sorted(range(len(idx_mj)), key=lambda x: idx_mj.index(idx_mjx[x]))
|
idx = sorted(range(len(idx_mj)), key=lambda x: idx_mj.index(idx_mjx[x]))
|
||||||
|
|
||||||
mjx_contact = jax.tree_map(
|
mjx_contact = jax.tree_util.tree_map(
|
||||||
lambda x: x.take(np.array(idx), axis=0), dx.contact
|
lambda x: x.take(np.array(idx), axis=0), dx.contact
|
||||||
)
|
)
|
||||||
|
mjx_contact = mjx_contact.replace(
|
||||||
|
dim=mjx_contact.dim[idx], efc_address=mjx_contact.efc_address[idx]
|
||||||
|
)
|
||||||
for field in dataclasses.fields(Contact):
|
for field in dataclasses.fields(Contact):
|
||||||
_assert_attr_eq(mjx_contact, d.contact, field.name, seed, 1e-7)
|
_assert_attr_eq(mjx_contact, d.contact, field.name, seed, 1e-7)
|
||||||
|
|
||||||
|
|||||||
@@ -68,9 +68,9 @@ class TransmissionIntegrationTest(parameterized.TestCase):
|
|||||||
mujoco.mj_transmission(m, d)
|
mujoco.mj_transmission(m, d)
|
||||||
dx = transmission_jit_fn(mx, dx)
|
dx = transmission_jit_fn(mx, dx)
|
||||||
|
|
||||||
_assert_attr_eq(d, dx, 'actuator_length', seed, f'transmission{seed}')
|
for field in ['actuator_length', 'actuator_moment']:
|
||||||
_assert_attr_eq(
|
_assert_attr_eq(
|
||||||
d, dx, 'actuator_moment', seed, f'transmission{seed}', atol=1e-4
|
d, dx, field, seed, f'transmission{seed}', atol=1e-4
|
||||||
)
|
)
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
@@ -14,7 +14,7 @@
|
|||||||
</default>
|
</default>
|
||||||
|
|
||||||
<worldbody>
|
<worldbody>
|
||||||
<geom pos="0 0 -1" type="plane" size="10 10 .01"/>
|
<geom pos="0 0 -1" type="plane" size="20 20 .01" condim="1"/>
|
||||||
|
|
||||||
<body name="anchor1" pos="-3 0 0"/>
|
<body name="anchor1" pos="-3 0 0"/>
|
||||||
<body name="beam1" pos="-3 0 0">
|
<body name="beam1" pos="-3 0 0">
|
||||||
@@ -37,6 +37,26 @@
|
|||||||
<joint name="joint4" axis="1 0 0" type="hinge" damping="1.0"/> <!-- tests no joint range -->
|
<joint name="joint4" axis="1 0 0" type="hinge" damping="1.0"/> <!-- tests no joint range -->
|
||||||
<geom class="box"/>
|
<geom class="box"/>
|
||||||
</body>
|
</body>
|
||||||
|
|
||||||
|
<body name="box_condim1" pos="4 0 0">
|
||||||
|
<freejoint/>
|
||||||
|
<geom class="box" condim="1"/>
|
||||||
|
</body>
|
||||||
|
|
||||||
|
<body name="box_condim3" pos="5 0 0">
|
||||||
|
<freejoint/>
|
||||||
|
<geom class="box" condim="3"/>
|
||||||
|
</body>
|
||||||
|
|
||||||
|
<body name="box_condim4" pos="6 0 0">
|
||||||
|
<freejoint/>
|
||||||
|
<geom class="box" condim="4"/>
|
||||||
|
</body>
|
||||||
|
|
||||||
|
<body name="box_condim6" pos="6 0 0">
|
||||||
|
<freejoint/>
|
||||||
|
<geom class="box" condim="6"/>
|
||||||
|
</body>
|
||||||
</worldbody>
|
</worldbody>
|
||||||
|
|
||||||
<equality>
|
<equality>
|
||||||
|
|||||||
@@ -110,6 +110,20 @@
|
|||||||
</body>
|
</body>
|
||||||
</body>
|
</body>
|
||||||
</body>
|
</body>
|
||||||
|
|
||||||
|
<!-- triple pendulum of hinges with gravcomp -->
|
||||||
|
<body pos="3.0 0 0" gravcomp="1">
|
||||||
|
<joint name="joint15" axis="0.1 0.2 0.3" type="hinge"/>
|
||||||
|
<geom/>
|
||||||
|
<body pos="0 0 -0.8" gravcomp="2">
|
||||||
|
<joint name="joint16" axis="0.4 0.5 0.6" type="hinge" armature="0.02" range="-20 20"/>
|
||||||
|
<geom/>
|
||||||
|
<body pos="0 -0.7 0" gravcomp="3">
|
||||||
|
<joint name="joint17" axis="0.7 0.8 0.9" type="hinge" damping="0.75" range="-30 30" actuatorgravcomp="true"/>
|
||||||
|
<geom/>
|
||||||
|
</body>
|
||||||
|
</body>
|
||||||
|
</body>
|
||||||
</worldbody>
|
</worldbody>
|
||||||
|
|
||||||
<actuator>
|
<actuator>
|
||||||
@@ -118,5 +132,8 @@
|
|||||||
<motor gear="0 0 300" joint="joint1" name="act3"/>
|
<motor gear="0 0 300" joint="joint1" name="act3"/>
|
||||||
<motor gear="275" joint="joint2" name="act4"/>
|
<motor gear="275" joint="joint2" name="act4"/>
|
||||||
<motor gear="275" joint="joint3" name="act5"/>
|
<motor gear="275" joint="joint3" name="act5"/>
|
||||||
|
<motor gear="150" joint="joint15" name="act6"/>
|
||||||
|
<motor gear="150" joint="joint16" name="act7"/>
|
||||||
|
<motor gear="150" joint="joint17" name="act8"/>
|
||||||
</actuator>
|
</actuator>
|
||||||
</mujoco>
|
</mujoco>
|
||||||
|
|||||||
@@ -47,8 +47,9 @@ _OUTPUT = flags.DEFINE_enum(
|
|||||||
|
|
||||||
def _main(argv: Sequence[str]):
|
def _main(argv: Sequence[str]):
|
||||||
"""Runs testpeed function."""
|
"""Runs testpeed function."""
|
||||||
base_path = _BASE_PATH.value or epath.resource_path('mujoco.mjx')
|
path = epath.resource_path('mujoco.mjx') / 'test_data'
|
||||||
f = base_path / 'test_data' / _MJCF.value
|
path = _BASE_PATH.value or path
|
||||||
|
f = epath.Path(path) / _MJCF.value
|
||||||
m = mujoco.MjModel.from_xml_path(f.as_posix())
|
m = mujoco.MjModel.from_xml_path(f.as_posix())
|
||||||
|
|
||||||
print(f'Rolling out {_NSTEP.value} steps at dt = {m.opt.timestep:.3f}...')
|
print(f'Rolling out {_NSTEP.value} steps at dt = {m.opt.timestep:.3f}...')
|
||||||
|
|||||||
@@ -53,7 +53,7 @@ def _main(argv: Sequence[str]) -> None:
|
|||||||
start = time.time()
|
start = time.time()
|
||||||
|
|
||||||
# TODO(robotics-simulation): recompile when changing disable flags, etc.
|
# TODO(robotics-simulation): recompile when changing disable flags, etc.
|
||||||
dx = dx.replace(ctrl=d.ctrl, xfrc_applied=d.xfrc_applied)
|
dx = dx.replace(ctrl=d.ctrl, act=d.act, xfrc_applied=d.xfrc_applied)
|
||||||
dx = dx.replace(qpos=d.qpos, qvel=d.qvel, time=d.time) # handle resets
|
dx = dx.replace(qpos=d.qpos, qvel=d.qvel, time=d.time) # handle resets
|
||||||
mx = mx.tree_replace({
|
mx = mx.tree_replace({
|
||||||
'opt.gravity': m.opt.gravity,
|
'opt.gravity': m.opt.gravity,
|
||||||
|
|||||||
+4
-4
@@ -4,7 +4,7 @@ build-backend = "setuptools.build_meta"
|
|||||||
|
|
||||||
[project]
|
[project]
|
||||||
name="mujoco-mjx"
|
name="mujoco-mjx"
|
||||||
version = "3.1.4"
|
version = "3.1.5"
|
||||||
authors = [
|
authors = [
|
||||||
{name = "Google DeepMind", email = "mujoco@deepmind.com"},
|
{name = "Google DeepMind", email = "mujoco@deepmind.com"},
|
||||||
]
|
]
|
||||||
@@ -31,7 +31,7 @@ dependencies = [
|
|||||||
"etils[epath]",
|
"etils[epath]",
|
||||||
"jax",
|
"jax",
|
||||||
"jaxlib",
|
"jaxlib",
|
||||||
"mujoco>=3.1.4.dev0",
|
"mujoco>=3.1.5.dev0",
|
||||||
"scipy",
|
"scipy",
|
||||||
"trimesh",
|
"trimesh",
|
||||||
]
|
]
|
||||||
@@ -42,6 +42,6 @@ mjx-viewer = "mujoco.mjx.viewer:main"
|
|||||||
|
|
||||||
[project.urls]
|
[project.urls]
|
||||||
Homepage = "https://github.com/google-deepmind/mujoco/tree/main/mjx"
|
Homepage = "https://github.com/google-deepmind/mujoco/tree/main/mjx"
|
||||||
Documentation = "https://mujoco.readthedocs.io/en/3.1.4"
|
Documentation = "https://mujoco.readthedocs.io/en/3.1.5"
|
||||||
Repository = "https://github.com/google-deepmind/mujoco/tree/main/mjx"
|
Repository = "https://github.com/google-deepmind/mujoco/tree/main/mjx"
|
||||||
Changelog = "https://mujoco.readthedocs.io/en/3.1.4/changelog.html"
|
Changelog = "https://mujoco.readthedocs.io/en/3.1.5/changelog.html"
|
||||||
|
|||||||
File diff suppressed because one or more lines are too long
+1
-1
@@ -829,7 +829,7 @@
|
|||||||
"\n",
|
"\n",
|
||||||
" friction, gain, bias = rand(rng)\n",
|
" friction, gain, bias = rand(rng)\n",
|
||||||
"\n",
|
"\n",
|
||||||
" in_axes = jax.tree_map(lambda x: None, sys)\n",
|
" in_axes = jax.tree_util.tree_map(lambda x: None, sys)\n",
|
||||||
" in_axes = in_axes.tree_replace({\n",
|
" in_axes = in_axes.tree_replace({\n",
|
||||||
" 'geom_friction': 0,\n",
|
" 'geom_friction': 0,\n",
|
||||||
" 'actuator_gainprm': 0,\n",
|
" 'actuator_gainprm': 0,\n",
|
||||||
|
|||||||
+175
-2223
File diff suppressed because it is too large
Load Diff
@@ -27,20 +27,20 @@
|
|||||||
|
|
||||||
<default>
|
<default>
|
||||||
<geom solimp=".9 .9 .01"/>
|
<geom solimp=".9 .9 .01"/>
|
||||||
<default class="object1">
|
<default class="capsule">
|
||||||
<geom type="capsule" material="object1" size="0.1 0.05"/>
|
<geom type="capsule" material="capsule" size="0.1 0.05"/>
|
||||||
</default>
|
</default>
|
||||||
<default class="object2">
|
<default class="ellipsoid">
|
||||||
<geom type="ellipsoid" material="object2" size="0.15 0.1 0.07"/>
|
<geom type="ellipsoid" material="ellipsoid" size="0.15 0.1 0.07"/>
|
||||||
</default>
|
</default>
|
||||||
<default class="object3">
|
<default class="box">
|
||||||
<geom type="box" material="object3" size="0.15 0.1 0.05"/>
|
<geom type="box" material="box" size="0.15 0.1 0.05"/>
|
||||||
</default>
|
</default>
|
||||||
<default class="object4">
|
<default class="cylinder">
|
||||||
<geom type="cylinder" material="object4" size="0.1 0.05" condim="4" friction="1 .01 .01"/>
|
<geom type="cylinder" material="cylinder" size="0.1 0.05" condim="4" friction="1 .01 .01"/>
|
||||||
</default>
|
</default>
|
||||||
<default class="object5">
|
<default class="sphere">
|
||||||
<geom type="sphere" material="object5" size="0.1"/>
|
<geom type="sphere" material="sphere" size="0.1"/>
|
||||||
</default>
|
</default>
|
||||||
<default class="border">
|
<default class="border">
|
||||||
<geom type="capsule" size="0.4" rgba=".4 .4 .4 1"/>
|
<geom type="capsule" size="0.4" rgba=".4 .4 .4 1"/>
|
||||||
@@ -56,13 +56,13 @@
|
|||||||
rgb1="0.6 0.6 0.6" rgb2="0.6 0.6 0.6" markrgb="1 1 1"/>
|
rgb1="0.6 0.6 0.6" rgb2="0.6 0.6 0.6" markrgb="1 1 1"/>
|
||||||
<texture name="texplane" type="2d" builtin="checker" rgb1=".4 .4 .4" rgb2=".6 .6 .6"
|
<texture name="texplane" type="2d" builtin="checker" rgb1=".4 .4 .4" rgb2=".6 .6 .6"
|
||||||
width="512" height="512"/>
|
width="512" height="512"/>
|
||||||
<material name='MatPlane' reflectance='0.3' texture="texplane" texrepeat="1 1" texuniform="true"
|
<material name="MatPlane" reflectance="0.3" texture="texplane" texrepeat="1 1" texuniform="true"
|
||||||
rgba=".7 .7 .7 1"/>
|
rgba=".7 .7 .7 1"/>
|
||||||
<material name='object1' texture="texgeom" texuniform="true" rgba=".4 .9 .6 1" />
|
<material name="capsule" texture="texgeom" texuniform="true" rgba=".4 .9 .6 1" />
|
||||||
<material name='object2' texture="texgeom" texuniform="true" rgba=".4 .6 .9 1" />
|
<material name="ellipsoid" texture="texgeom" texuniform="true" rgba=".4 .6 .9 1" />
|
||||||
<material name='object3' texture="texgeom" texuniform="true" rgba=".4 .9 .9 1" />
|
<material name="box" texture="texgeom" texuniform="true" rgba=".4 .9 .9 1" />
|
||||||
<material name='object4' texture="texgeom" texuniform="true" rgba=".8 .6 .8 1" />
|
<material name="cylinder" texture="texgeom" texuniform="true" rgba=".8 .6 .8 1" />
|
||||||
<material name='object5' texture="texgeom" texuniform="true" rgba=".9 .1 .1 1" />
|
<material name="sphere" texture="texgeom" texuniform="true" rgba=".9 .1 .1 1" />
|
||||||
</asset>
|
</asset>
|
||||||
|
|
||||||
<visual>
|
<visual>
|
||||||
@@ -84,430 +84,70 @@
|
|||||||
<geom class="borderpost" pos="3 -3 0"/>
|
<geom class="borderpost" pos="3 -3 0"/>
|
||||||
<geom class="borderpost" pos="-3 -3 0"/>
|
<geom class="borderpost" pos="-3 -3 0"/>
|
||||||
|
|
||||||
<body pos="-2 -2 1" quat="-0.632456 -0.632456 0.316228 0.316228">
|
<frame pos="-2 -2 2.5">
|
||||||
|
<replicate count="5" offset="0 1 0" euler="0 180 0">
|
||||||
|
<frame pos="0 0 -1.5">
|
||||||
|
<replicate count="4" offset="0 0 1" euler="0 0 60">
|
||||||
|
<body euler="30 40 0">
|
||||||
<freejoint/>
|
<freejoint/>
|
||||||
<geom class="object1"/>
|
<geom class="capsule"/>
|
||||||
</body>
|
|
||||||
<body pos="-2 -2 2" quat="-0.5547 -0.5547 0.5547 0.27735">
|
|
||||||
<freejoint/>
|
|
||||||
<geom class="object1"/>
|
|
||||||
</body>
|
|
||||||
<body pos="-2 -2 3" quat="-0.471405 -0.471405 0.707107 0.235702">
|
|
||||||
<freejoint/>
|
|
||||||
<geom class="object1"/>
|
|
||||||
</body>
|
|
||||||
<body pos="-2 -2 4" quat="-0.471405 -0.471405 0.707107 0.235702">
|
|
||||||
<freejoint/>
|
|
||||||
<geom class="object1"/>
|
|
||||||
</body>
|
</body>
|
||||||
|
</replicate>
|
||||||
|
</frame>
|
||||||
|
</replicate>
|
||||||
|
</frame>
|
||||||
|
|
||||||
<body pos="-2 -1 1" quat="-0.755929 -0.377964 0.377964 0.377964">
|
<frame pos="-1 -2 2.5">
|
||||||
|
<replicate count="5" offset="0 1 0" euler="0 180 0">
|
||||||
|
<frame pos="0 0 -1.5">
|
||||||
|
<replicate count="4" offset="0 0 1" euler="0 0 60">
|
||||||
|
<body euler="20 40 60">
|
||||||
<freejoint/>
|
<freejoint/>
|
||||||
<geom class="object1"/>
|
<geom class="ellipsoid"/>
|
||||||
</body>
|
|
||||||
<body pos="-2 -1 2" quat="-0.632456 -0.316228 0.632456 0.316228">
|
|
||||||
<freejoint/>
|
|
||||||
<geom class="object1"/>
|
|
||||||
</body>
|
|
||||||
<body pos="-2 -1 3" quat="-0.516398 -0.258199 0.774597 0.258199">
|
|
||||||
<freejoint/>
|
|
||||||
<geom class="object1"/>
|
|
||||||
</body>
|
|
||||||
<body pos="-2 -1 4" quat="-0.516398 -0.258199 0.774597 0.258199">
|
|
||||||
<freejoint/>
|
|
||||||
<geom class="object1"/>
|
|
||||||
</body>
|
</body>
|
||||||
|
</replicate>
|
||||||
|
</frame>
|
||||||
|
</replicate>
|
||||||
|
</frame>
|
||||||
|
|
||||||
<body pos="-2 0 1" quat="-0.816497 0 0.408248 0.408248">
|
<frame pos="0 -2 3.5">
|
||||||
|
<replicate count="5" offset="0 1 0" euler="0 180 0">
|
||||||
|
<frame pos="0 0 -1.5">
|
||||||
|
<replicate count="4" offset="0 0 1" euler="0 0 60">
|
||||||
|
<body euler="30 70 110">
|
||||||
<freejoint/>
|
<freejoint/>
|
||||||
<geom class="object1"/>
|
<geom class="box"/>
|
||||||
</body>
|
|
||||||
<body pos="-2 0 2" quat="-0.666667 0 0.666667 0.333333">
|
|
||||||
<freejoint/>
|
|
||||||
<geom class="object1"/>
|
|
||||||
</body>
|
|
||||||
<body pos="-2 0 3" quat="-0.534522 0 0.801784 0.267261">
|
|
||||||
<freejoint/>
|
|
||||||
<geom class="object1"/>
|
|
||||||
</body>
|
|
||||||
<body pos="-2 0 4" quat="-0.534522 0 0.801784 0.267261">
|
|
||||||
<freejoint/>
|
|
||||||
<geom class="object1"/>
|
|
||||||
</body>
|
</body>
|
||||||
|
</replicate>
|
||||||
|
</frame>
|
||||||
|
</replicate>
|
||||||
|
</frame>
|
||||||
|
|
||||||
<body pos="-2 1 1" quat="-0.755929 0.377964 0.377964 0.377964">
|
<frame pos="1 -2 2.5">
|
||||||
|
<replicate count="5" offset="0 1 0" euler="0 180 0">
|
||||||
|
<frame pos="0 0 -1.5">
|
||||||
|
<replicate count="4" offset="0 0 1" euler="0 0 60">
|
||||||
|
<body euler="60 30 0">
|
||||||
<freejoint/>
|
<freejoint/>
|
||||||
<geom class="object1"/>
|
<geom class="cylinder"/>
|
||||||
</body>
|
|
||||||
<body pos="-2 1 2" quat="-0.632456 0.316228 0.632456 0.316228">
|
|
||||||
<freejoint/>
|
|
||||||
<geom class="object1"/>
|
|
||||||
</body>
|
|
||||||
<body pos="-2 1 3" quat="-0.516398 0.258199 0.774597 0.258199">
|
|
||||||
<freejoint/>
|
|
||||||
<geom class="object1"/>
|
|
||||||
</body>
|
|
||||||
<body pos="-2 1 4" quat="-0.516398 0.258199 0.774597 0.258199">
|
|
||||||
<freejoint/>
|
|
||||||
<geom class="object1"/>
|
|
||||||
</body>
|
</body>
|
||||||
|
</replicate>
|
||||||
|
</frame>
|
||||||
|
</replicate>
|
||||||
|
</frame>
|
||||||
|
|
||||||
<body pos="-2 2 1" quat="-0.632456 0.632456 0.316228 0.316228">
|
<frame pos="2 -2 2.5">
|
||||||
<freejoint/>
|
<replicate count="5" offset="0 1 0">
|
||||||
<geom class="object1"/>
|
<frame pos="0 0 -1.5">
|
||||||
</body>
|
<replicate count="4" offset="0 0 1">
|
||||||
<body pos="-2 2 2" quat="-0.5547 0.5547 0.5547 0.27735">
|
<body>
|
||||||
<freejoint/>
|
<freejoint/>
|
||||||
<geom class="object1"/>
|
<geom class="sphere"/>
|
||||||
</body>
|
</body>
|
||||||
<body pos="-2 2 3" quat="-0.471405 0.471405 0.707107 0.235702">
|
</replicate>
|
||||||
<freejoint/>
|
</frame>
|
||||||
<geom class="object1"/>
|
</replicate>
|
||||||
</body>
|
</frame>
|
||||||
<body pos="-2 2 4" quat="-0.471405 0.471405 0.707107 0.235702">
|
|
||||||
<freejoint/>
|
|
||||||
<geom class="object1"/>
|
|
||||||
</body>
|
|
||||||
|
|
||||||
<body pos="-1 -2 1" quat="-0.377964 -0.755929 0.377964 0.377964">
|
|
||||||
<freejoint/>
|
|
||||||
<geom class="object2"/>
|
|
||||||
</body>
|
|
||||||
<body pos="-1 -2 2" quat="-0.316228 -0.632456 0.632456 0.316228">
|
|
||||||
<freejoint/>
|
|
||||||
<geom class="object2"/>
|
|
||||||
</body>
|
|
||||||
<body pos="-1 -2 3" quat="-0.258199 -0.516398 0.774597 0.258199">
|
|
||||||
<freejoint/>
|
|
||||||
<geom class="object2"/>
|
|
||||||
</body>
|
|
||||||
<body pos="-1 -2 4" quat="-0.258199 -0.516398 0.774597 0.258199">
|
|
||||||
<freejoint/>
|
|
||||||
<geom class="object2"/>
|
|
||||||
</body>
|
|
||||||
|
|
||||||
<body pos="-1 -1 1" quat="-0.5 -0.5 0.5 0.5">
|
|
||||||
<freejoint/>
|
|
||||||
<geom class="object2"/>
|
|
||||||
</body>
|
|
||||||
<body pos="-1 -1 2" quat="-0.377964 -0.377964 0.755929 0.377964">
|
|
||||||
<freejoint/>
|
|
||||||
<geom class="object2"/>
|
|
||||||
</body>
|
|
||||||
<body pos="-1 -1 3" quat="-0.288675 -0.288675 0.866025 0.288675">
|
|
||||||
<freejoint/>
|
|
||||||
<geom class="object2"/>
|
|
||||||
</body>
|
|
||||||
<body pos="-1 -1 4" quat="-0.288675 -0.288675 0.866025 0.288675">
|
|
||||||
<freejoint/>
|
|
||||||
<geom class="object2"/>
|
|
||||||
</body>
|
|
||||||
|
|
||||||
<body pos="-1 0 1" quat="-0.57735 0 0.57735 0.57735">
|
|
||||||
<freejoint/>
|
|
||||||
<geom class="object2"/>
|
|
||||||
</body>
|
|
||||||
<body pos="-1 0 2" quat="-0.408248 0 0.816497 0.408248">
|
|
||||||
<freejoint/>
|
|
||||||
<geom class="object2"/>
|
|
||||||
</body>
|
|
||||||
<body pos="-1 0 3" quat="-0.301511 0 0.904534 0.301511">
|
|
||||||
<freejoint/>
|
|
||||||
<geom class="object2"/>
|
|
||||||
</body>
|
|
||||||
<body pos="-1 0 4" quat="-0.301511 0 0.904534 0.301511">
|
|
||||||
<freejoint/>
|
|
||||||
<geom class="object2"/>
|
|
||||||
</body>
|
|
||||||
|
|
||||||
<body pos="-1 1 1" quat="-0.5 0.5 0.5 0.5">
|
|
||||||
<freejoint/>
|
|
||||||
<geom class="object2"/>
|
|
||||||
</body>
|
|
||||||
<body pos="-1 1 2" quat="-0.377964 0.377964 0.755929 0.377964">
|
|
||||||
<freejoint/>
|
|
||||||
<geom class="object2"/>
|
|
||||||
</body>
|
|
||||||
<body pos="-1 1 3" quat="-0.288675 0.288675 0.866025 0.288675">
|
|
||||||
<freejoint/>
|
|
||||||
<geom class="object2"/>
|
|
||||||
</body>
|
|
||||||
<body pos="-1 1 4" quat="-0.288675 0.288675 0.866025 0.288675">
|
|
||||||
<freejoint/>
|
|
||||||
<geom class="object2"/>
|
|
||||||
</body>
|
|
||||||
|
|
||||||
<body pos="-1 2 1" quat="-0.377964 0.755929 0.377964 0.377964">
|
|
||||||
<freejoint/>
|
|
||||||
<geom class="object2"/>
|
|
||||||
</body>
|
|
||||||
<body pos="-1 2 2" quat="-0.316228 0.632456 0.632456 0.316228">
|
|
||||||
<freejoint/>
|
|
||||||
<geom class="object2"/>
|
|
||||||
</body>
|
|
||||||
<body pos="-1 2 3" quat="-0.258199 0.516398 0.774597 0.258199">
|
|
||||||
<freejoint/>
|
|
||||||
<geom class="object2"/>
|
|
||||||
</body>
|
|
||||||
<body pos="-1 2 4" quat="-0.258199 0.516398 0.774597 0.258199">
|
|
||||||
<freejoint/>
|
|
||||||
<geom class="object2"/>
|
|
||||||
</body>
|
|
||||||
|
|
||||||
<body pos="0 -2 1" quat="0 -0.816497 0.408248 0.408248">
|
|
||||||
<freejoint/>
|
|
||||||
<geom class="object3"/>
|
|
||||||
</body>
|
|
||||||
<body pos="0 -2 2" quat="0 -0.666667 0.666667 0.333333">
|
|
||||||
<freejoint/>
|
|
||||||
<geom class="object3"/>
|
|
||||||
</body>
|
|
||||||
<body pos="0 -2 3" quat="0 -0.534522 0.801784 0.267261">
|
|
||||||
<freejoint/>
|
|
||||||
<geom class="object3"/>
|
|
||||||
</body>
|
|
||||||
<body pos="0 -2 4" quat="0 -0.534522 0.801784 0.267261">
|
|
||||||
<freejoint/>
|
|
||||||
<geom class="object3"/>
|
|
||||||
</body>
|
|
||||||
|
|
||||||
<body pos="0 -1 1" quat="0 -0.57735 0.57735 0.57735">
|
|
||||||
<freejoint/>
|
|
||||||
<geom class="object3"/>
|
|
||||||
</body>
|
|
||||||
<body pos="0 -1 2" quat="0 -0.408248 0.816497 0.408248">
|
|
||||||
<freejoint/>
|
|
||||||
<geom class="object3"/>
|
|
||||||
</body>
|
|
||||||
<body pos="0 -1 3" quat="0 -0.301511 0.904534 0.301511">
|
|
||||||
<freejoint/>
|
|
||||||
<geom class="object3"/>
|
|
||||||
</body>
|
|
||||||
<body pos="0 -1 4" quat="0 -0.301511 0.904534 0.301511">
|
|
||||||
<freejoint/>
|
|
||||||
<geom class="object3"/>
|
|
||||||
</body>
|
|
||||||
|
|
||||||
<body pos="0.3 0 1" quat="0 0 0.707107 0.707107">
|
|
||||||
<freejoint/>
|
|
||||||
<geom class="object3"/>
|
|
||||||
</body>
|
|
||||||
<body pos="0 0 2" quat="0 0 0.894427 0.447214">
|
|
||||||
<freejoint/>
|
|
||||||
<geom class="object3"/>
|
|
||||||
</body>
|
|
||||||
<body pos="0 0 3" quat="0 0 0.948683 0.316228">
|
|
||||||
<freejoint/>
|
|
||||||
<geom class="object3"/>
|
|
||||||
</body>
|
|
||||||
<body pos="0 0 4" quat="0 0 0.948683 0.316228">
|
|
||||||
<freejoint/>
|
|
||||||
<geom class="object3"/>
|
|
||||||
</body>
|
|
||||||
|
|
||||||
<body pos="0 1 1" quat="0 0.57735 0.57735 0.57735">
|
|
||||||
<freejoint/>
|
|
||||||
<geom class="object3"/>
|
|
||||||
</body>
|
|
||||||
<body pos="0 1 2" quat="0 0.408248 0.816497 0.408248">
|
|
||||||
<freejoint/>
|
|
||||||
<geom class="object3"/>
|
|
||||||
</body>
|
|
||||||
<body pos="0 1 3" quat="0 0.301511 0.904534 0.301511">
|
|
||||||
<freejoint/>
|
|
||||||
<geom class="object3"/>
|
|
||||||
</body>
|
|
||||||
<body pos="0 1 4" quat="0 0.301511 0.904534 0.301511">
|
|
||||||
<freejoint/>
|
|
||||||
<geom class="object3"/>
|
|
||||||
</body>
|
|
||||||
|
|
||||||
<body pos="0 2 1" quat="0 0.816497 0.408248 0.408248">
|
|
||||||
<freejoint/>
|
|
||||||
<geom class="object3"/>
|
|
||||||
</body>
|
|
||||||
<body pos="0 2 2" quat="0 0.666667 0.666667 0.333333">
|
|
||||||
<freejoint/>
|
|
||||||
<geom class="object3"/>
|
|
||||||
</body>
|
|
||||||
<body pos="0 2 3" quat="0 0.534522 0.801784 0.267261">
|
|
||||||
<freejoint/>
|
|
||||||
<geom class="object3"/>
|
|
||||||
</body>
|
|
||||||
<body pos="0 2 4" quat="0 0.534522 0.801784 0.267261">
|
|
||||||
<freejoint/>
|
|
||||||
<geom class="object3"/>
|
|
||||||
</body>
|
|
||||||
|
|
||||||
<body pos="1 -2 1" quat="0.377964 -0.755929 0.377964 0.377964">
|
|
||||||
<freejoint/>
|
|
||||||
<geom class="object4"/>
|
|
||||||
</body>
|
|
||||||
<body pos="1 -2 2" quat="0.316228 -0.632456 0.632456 0.316228">
|
|
||||||
<freejoint/>
|
|
||||||
<geom class="object4"/>
|
|
||||||
</body>
|
|
||||||
<body pos="1 -2 3" quat="0.258199 -0.516398 0.774597 0.258199">
|
|
||||||
<freejoint/>
|
|
||||||
<geom class="object4"/>
|
|
||||||
</body>
|
|
||||||
<body pos="1 -2 4" quat="0.258199 -0.516398 0.774597 0.258199">
|
|
||||||
<freejoint/>
|
|
||||||
<geom class="object4"/>
|
|
||||||
</body>
|
|
||||||
|
|
||||||
<body pos="1 -1 1" quat="0.5 -0.5 0.5 0.5">
|
|
||||||
<freejoint/>
|
|
||||||
<geom class="object4"/>
|
|
||||||
</body>
|
|
||||||
<body pos="1 -1 2" quat="0.377964 -0.377964 0.755929 0.377964">
|
|
||||||
<freejoint/>
|
|
||||||
<geom class="object4"/>
|
|
||||||
</body>
|
|
||||||
<body pos="1 -1 3" quat="0.288675 -0.288675 0.866025 0.288675">
|
|
||||||
<freejoint/>
|
|
||||||
<geom class="object4"/>
|
|
||||||
</body>
|
|
||||||
<body pos="1 -1 4" quat="0.288675 -0.288675 0.866025 0.288675">
|
|
||||||
<freejoint/>
|
|
||||||
<geom class="object4"/>
|
|
||||||
</body>
|
|
||||||
|
|
||||||
<body pos="1 0 1" quat="0.57735 0 0.57735 0.57735">
|
|
||||||
<freejoint/>
|
|
||||||
<geom class="object4"/>
|
|
||||||
</body>
|
|
||||||
<body pos="1 0 2" quat="0.408248 0 0.816497 0.408248">
|
|
||||||
<freejoint/>
|
|
||||||
<geom class="object4"/>
|
|
||||||
</body>
|
|
||||||
<body pos="1 0 3" quat="0.301511 0 0.904534 0.301511">
|
|
||||||
<freejoint/>
|
|
||||||
<geom class="object4"/>
|
|
||||||
</body>
|
|
||||||
<body pos="1 0 4" quat="0.301511 0 0.904534 0.301511">
|
|
||||||
<freejoint/>
|
|
||||||
<geom class="object4"/>
|
|
||||||
</body>
|
|
||||||
|
|
||||||
<body pos="1 1 1" quat="0.5 0.5 0.5 0.5">
|
|
||||||
<freejoint/>
|
|
||||||
<geom class="object4"/>
|
|
||||||
</body>
|
|
||||||
<body pos="1 1 2" quat="0.377964 0.377964 0.755929 0.377964">
|
|
||||||
<freejoint/>
|
|
||||||
<geom class="object4"/>
|
|
||||||
</body>
|
|
||||||
<body pos="1 1 3" quat="0.288675 0.288675 0.866025 0.288675">
|
|
||||||
<freejoint/>
|
|
||||||
<geom class="object4"/>
|
|
||||||
</body>
|
|
||||||
<body pos="1 1 4" quat="0.288675 0.288675 0.866025 0.288675">
|
|
||||||
<freejoint/>
|
|
||||||
<geom class="object4"/>
|
|
||||||
</body>
|
|
||||||
|
|
||||||
<body pos="1 2 1" quat="0.377964 0.755929 0.377964 0.377964">
|
|
||||||
<freejoint/>
|
|
||||||
<geom class="object4"/>
|
|
||||||
</body>
|
|
||||||
<body pos="1 2 2" quat="0.316228 0.632456 0.632456 0.316228">
|
|
||||||
<freejoint/>
|
|
||||||
<geom class="object4"/>
|
|
||||||
</body>
|
|
||||||
<body pos="1 2 3" quat="0.258199 0.516398 0.774597 0.258199">
|
|
||||||
<freejoint/>
|
|
||||||
<geom class="object4"/>
|
|
||||||
</body>
|
|
||||||
<body pos="1 2 4" quat="0.258199 0.516398 0.774597 0.258199">
|
|
||||||
<freejoint/>
|
|
||||||
<geom class="object4"/>
|
|
||||||
</body>
|
|
||||||
|
|
||||||
<body pos="2 -2 1" quat="0.632456 -0.632456 0.316228 0.316228">
|
|
||||||
<freejoint/>
|
|
||||||
<geom class="object5"/>
|
|
||||||
</body>
|
|
||||||
<body pos="2 -2 2" quat="0.5547 -0.5547 0.5547 0.27735">
|
|
||||||
<freejoint/>
|
|
||||||
<geom class="object5"/>
|
|
||||||
</body>
|
|
||||||
<body pos="2 -2 3" quat="0.471405 -0.471405 0.707107 0.235702">
|
|
||||||
<freejoint/>
|
|
||||||
<geom class="object5"/>
|
|
||||||
</body>
|
|
||||||
<body pos="2 -2 4" quat="0.471405 -0.471405 0.707107 0.235702">
|
|
||||||
<freejoint/>
|
|
||||||
<geom class="object5"/>
|
|
||||||
</body>
|
|
||||||
|
|
||||||
<body pos="2 -1 1" quat="0.755929 -0.377964 0.377964 0.377964">
|
|
||||||
<freejoint/>
|
|
||||||
<geom class="object5"/>
|
|
||||||
</body>
|
|
||||||
<body pos="2 -1 2" quat="0.632456 -0.316228 0.632456 0.316228">
|
|
||||||
<freejoint/>
|
|
||||||
<geom class="object5"/>
|
|
||||||
</body>
|
|
||||||
<body pos="2 -1 3" quat="0.516398 -0.258199 0.774597 0.258199">
|
|
||||||
<freejoint/>
|
|
||||||
<geom class="object5"/>
|
|
||||||
</body>
|
|
||||||
<body pos="2 -1 4" quat="0.516398 -0.258199 0.774597 0.258199">
|
|
||||||
<freejoint/>
|
|
||||||
<geom class="object5"/>
|
|
||||||
</body>
|
|
||||||
|
|
||||||
<body pos="2 0 1" quat="0.816497 0 0.408248 0.408248">
|
|
||||||
<freejoint/>
|
|
||||||
<geom class="object5"/>
|
|
||||||
</body>
|
|
||||||
<body pos="2 0 2" quat="0.666667 0 0.666667 0.333333">
|
|
||||||
<freejoint/>
|
|
||||||
<geom class="object5"/>
|
|
||||||
</body>
|
|
||||||
<body pos="2 0 3" quat="0.534522 0 0.801784 0.267261">
|
|
||||||
<freejoint/>
|
|
||||||
<geom class="object5"/>
|
|
||||||
</body>
|
|
||||||
<body pos="2 0 4" quat="0.534522 0 0.801784 0.267261">
|
|
||||||
<freejoint/>
|
|
||||||
<geom class="object5"/>
|
|
||||||
</body>
|
|
||||||
|
|
||||||
<body pos="2 1 1" quat="0.755929 0.377964 0.377964 0.377964">
|
|
||||||
<freejoint/>
|
|
||||||
<geom class="object5"/>
|
|
||||||
</body>
|
|
||||||
<body pos="2 1 2" quat="0.632456 0.316228 0.632456 0.316228">
|
|
||||||
<freejoint/>
|
|
||||||
<geom class="object5"/>
|
|
||||||
</body>
|
|
||||||
<body pos="2 1 3" quat="0.516398 0.258199 0.774597 0.258199">
|
|
||||||
<freejoint/>
|
|
||||||
<geom class="object5"/>
|
|
||||||
</body>
|
|
||||||
<body pos="2 1 4" quat="0.516398 0.258199 0.774597 0.258199">
|
|
||||||
<freejoint/>
|
|
||||||
<geom class="object5"/>
|
|
||||||
</body>
|
|
||||||
|
|
||||||
<body pos="2 2 1" quat="0.632456 0.632456 0.316228 0.316228">
|
|
||||||
<freejoint/>
|
|
||||||
<geom class="object5"/>
|
|
||||||
</body>
|
|
||||||
<body pos="2 2 2" quat="0.5547 0.5547 0.5547 0.27735">
|
|
||||||
<freejoint/>
|
|
||||||
<geom class="object5"/>
|
|
||||||
</body>
|
|
||||||
<body pos="2 2 3" quat="0.471405 0.471405 0.707107 0.235702">
|
|
||||||
<freejoint/>
|
|
||||||
<geom class="object5"/>
|
|
||||||
</body>
|
|
||||||
<body pos="2 2 4" quat="0.471405 0.471405 0.707107 0.235702">
|
|
||||||
<freejoint/>
|
|
||||||
<geom class="object5"/>
|
|
||||||
</body>
|
|
||||||
|
|
||||||
</worldbody>
|
</worldbody>
|
||||||
</mujoco>
|
</mujoco>
|
||||||
|
|||||||
@@ -0,0 +1,27 @@
|
|||||||
|
# Replicate
|
||||||
|
|
||||||
|
Models in this directory provide usage examples of the `<replicate>` meta-element, see
|
||||||
|
[documentation](https://mujoco.readthedocs.io/en/stable/XMLreference.html#replicate-r).
|
||||||
|
|
||||||
|
The models contain several categories:
|
||||||
|
|
||||||
|
1. Constructing static geometries:
|
||||||
|
- cylinder.xml
|
||||||
|
- bowl.xml
|
||||||
|
- helix.xml
|
||||||
|
- container.xml
|
||||||
|
|
||||||
|
2. Replicating moving bodies:
|
||||||
|
- particle.xml
|
||||||
|
- bunnies.xml
|
||||||
|
- leaves.xml
|
||||||
|
- stonehenge.xml
|
||||||
|
|
||||||
|
3. Replicas that include tendons:
|
||||||
|
- newton_cradle.xml
|
||||||
|
- tendon.xml
|
||||||
|
|
||||||
|
4. Replicas with multiple referencing elements:
|
||||||
|
- references.xml
|
||||||
|
|
||||||
|
[](https://www.youtube.com/watch?v=5k0_wsIRAFc)
|
||||||
@@ -0,0 +1,46 @@
|
|||||||
|
<mujoco model="Bowl">
|
||||||
|
<statistic meansize=".05"/>
|
||||||
|
|
||||||
|
<worldbody>
|
||||||
|
<light pos=".1 0 .6"/>
|
||||||
|
<light pos="-.1 0 .6"/>
|
||||||
|
<geom type="plane" size="1 1 .01"/>
|
||||||
|
|
||||||
|
<body pos="-.1 0 .04" euler="20 10 0">
|
||||||
|
<freejoint/>
|
||||||
|
<geom type="cylinder" size="0.03 0.002" rgba=".8 0 0 1"/>
|
||||||
|
<replicate count="20" euler="0 0 18">
|
||||||
|
<frame pos="-.025 0 .03" euler="0 15 0">
|
||||||
|
<replicate count="6" euler="0 15 0">
|
||||||
|
<geom type="box" size=".0044 .009 .002" pos="0 0 -.03" rgba=".8 0 0 1"/>
|
||||||
|
</replicate>
|
||||||
|
</frame>
|
||||||
|
</replicate>
|
||||||
|
</body>
|
||||||
|
|
||||||
|
<body pos="0 0 .02" euler="20 20 0">
|
||||||
|
<freejoint/>
|
||||||
|
<geom type="cylinder" size="0.01 0.0015" rgba="0 .8 0 1"/>
|
||||||
|
<replicate count="20" euler="0 0 18">
|
||||||
|
<frame pos="-.005 0 .03" euler="0 15 0">
|
||||||
|
<replicate count="6" euler="0 15 0">
|
||||||
|
<geom type="box" size=".0044 .006 .0015" pos="0 0 -.03" rgba="0 .8 0 1"/>
|
||||||
|
</replicate>
|
||||||
|
</frame>
|
||||||
|
</replicate>
|
||||||
|
</body>
|
||||||
|
|
||||||
|
<body pos=".09 0 .04" euler="30 30 0">
|
||||||
|
<freejoint/>
|
||||||
|
<geom type="cylinder" size="0.03 0.002" rgba="0 0 .8 1"/>
|
||||||
|
<replicate count="20" euler="0 0 18">
|
||||||
|
<frame pos="-.025 0 .03" euler="0 15 0">
|
||||||
|
<replicate count="4" euler="0 15 0">
|
||||||
|
<geom type="box" size=".0044 .009 .002" pos="0 0 -.03" rgba="0 0 .8 1"/>
|
||||||
|
</replicate>
|
||||||
|
</frame>
|
||||||
|
</replicate>
|
||||||
|
</body>
|
||||||
|
|
||||||
|
</worldbody>
|
||||||
|
</mujoco>
|
||||||
@@ -0,0 +1,25 @@
|
|||||||
|
<mujoco model="Bunnies">
|
||||||
|
|
||||||
|
<include file="container.xml"/>
|
||||||
|
|
||||||
|
<size memory="1G"/>
|
||||||
|
|
||||||
|
<option solver="CG" tolerance="1e-6" timestep=".01"/>
|
||||||
|
|
||||||
|
<asset>
|
||||||
|
<mesh file="bunny.obj"/>
|
||||||
|
</asset>
|
||||||
|
|
||||||
|
<worldbody>
|
||||||
|
<replicate count="5" offset=".17 0 0">
|
||||||
|
<replicate count="5" offset="0 .17 0">
|
||||||
|
<replicate count="5" offset="0 0 .17">
|
||||||
|
<body pos="-0.35 -0.35 0.65">
|
||||||
|
<freejoint/>
|
||||||
|
<geom type="mesh" mesh="bunny" rgba="0.8 0.2 0.1 1"/>
|
||||||
|
</body>
|
||||||
|
</replicate>
|
||||||
|
</replicate>
|
||||||
|
</replicate>
|
||||||
|
</worldbody>
|
||||||
|
</mujoco>
|
||||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,31 @@
|
|||||||
|
<mujoco model="Container">
|
||||||
|
<statistic extent="1.5" meansize=".05"/>
|
||||||
|
|
||||||
|
<visual>
|
||||||
|
<rgba haze="0.15 0.25 0.35 1"/>
|
||||||
|
<map shadowscale="0.5" zfar="40"/>
|
||||||
|
</visual>
|
||||||
|
|
||||||
|
<asset>
|
||||||
|
<texture type="skybox" builtin="gradient" rgb1="0.3 0.5 0.7" rgb2="0 0 0" width="64" height="64"/>
|
||||||
|
<texture name="texplane" type="2d" builtin="checker" rgb1=".2 .3 .4" rgb2=".1 0.15 0.2" width="512" height="512" mark="cross" markrgb=".8 .8 .8"/>
|
||||||
|
<material name="matplane" reflectance="0.3" texture="texplane" texrepeat="1 1" texuniform="true"/>
|
||||||
|
</asset>
|
||||||
|
|
||||||
|
<worldbody>
|
||||||
|
<light directional="true" diffuse=".4 .4 .4" specular="0.1 0.1 0.1" pos="0 0 5" castshadow="false"/>
|
||||||
|
<light diffuse=".6 .6 .6" specular="0.2 0.2 0.2" pos="0 0 4"/>
|
||||||
|
|
||||||
|
<geom name="ground" type="plane" size="0 0 1" pos="0 0 0" quat="1 0 0 0" material="matplane" condim="1"/>
|
||||||
|
|
||||||
|
<body mocap="true" pos="-.1 .05 0" zaxis=".5 0 1">
|
||||||
|
<geom type="capsule" size=".1 .1" group="1" condim="1"/>
|
||||||
|
</body>
|
||||||
|
</worldbody>
|
||||||
|
|
||||||
|
<worldbody>
|
||||||
|
<replicate count="4" euler="0 0 90">
|
||||||
|
<geom type="plane" size=".5 .5 .05" zaxis="1 0 0" pos="-.5 0 -.25"/>
|
||||||
|
</replicate>
|
||||||
|
</worldbody>
|
||||||
|
</mujoco>
|
||||||
@@ -0,0 +1,31 @@
|
|||||||
|
<mujoco model="Cylinder">
|
||||||
|
<statistic meansize=".05"/>
|
||||||
|
|
||||||
|
<worldbody>
|
||||||
|
<light pos=".5 0 3"/>
|
||||||
|
<light pos="-.5 0 3"/>
|
||||||
|
<geom type="plane" size="2 2 .01"/>
|
||||||
|
|
||||||
|
<body pos="-.3 0 .3" euler="80 10 0">
|
||||||
|
<freejoint/>
|
||||||
|
<replicate count="30" euler="0 0 12">
|
||||||
|
<geom type="box" pos="0 -.12 0" size=".01315 .005 .1" rgba=".8 0 0 1"/>
|
||||||
|
</replicate>
|
||||||
|
</body>
|
||||||
|
|
||||||
|
<body pos="0 0 .4" euler="20 20 0">
|
||||||
|
<freejoint/>
|
||||||
|
<replicate count="10" euler="0 0 36">
|
||||||
|
<geom type="box" pos="0 -0.067 0" size=".025 .01 .1" rgba="0 .8 0 1"/>
|
||||||
|
</replicate>
|
||||||
|
</body>
|
||||||
|
|
||||||
|
<body pos=".3 0 .3" euler="30 30 0">
|
||||||
|
<freejoint/>
|
||||||
|
<replicate count="4" euler="0 0 90">
|
||||||
|
<geom type="box" pos="0.005 -0.05 0" size=".05 .005 .08" rgba="0 0 .8 1"/>
|
||||||
|
</replicate>
|
||||||
|
</body>
|
||||||
|
|
||||||
|
</worldbody>
|
||||||
|
</mujoco>
|
||||||
@@ -0,0 +1,25 @@
|
|||||||
|
<mujoco model="Helix">
|
||||||
|
<statistic meansize=".05"/>
|
||||||
|
|
||||||
|
<worldbody>
|
||||||
|
<light pos=".5 0 3"/>
|
||||||
|
<light pos="-.5 0 3"/>
|
||||||
|
<geom type="plane" size="2 2 .01"/>
|
||||||
|
|
||||||
|
<body pos="0 0 .3">
|
||||||
|
<freejoint/>
|
||||||
|
<replicate count="50" offset="0 0 0.01" euler="0 0 25">
|
||||||
|
<geom type="capsule" size=".01 .05" pos="0 -.23 -.2" euler="0 84.5 0" rgba="0 0 1 1"/>
|
||||||
|
</replicate>
|
||||||
|
</body>
|
||||||
|
|
||||||
|
|
||||||
|
<body pos=".6 0 .3" euler="0 0 90">
|
||||||
|
<freejoint/>
|
||||||
|
<replicate count="50" offset="0 0 0.01" euler="0 0 15">
|
||||||
|
<geom type="box" pos=".025 -.2 -.2" size=".032 .04 .005" euler="-24 -12 2" rgba=".8 0 0 1"/>
|
||||||
|
</replicate>
|
||||||
|
</body>
|
||||||
|
|
||||||
|
</worldbody>
|
||||||
|
</mujoco>
|
||||||
@@ -0,0 +1,50 @@
|
|||||||
|
<mujoco model="Leaves">
|
||||||
|
|
||||||
|
<statistic extent="1.5"/>
|
||||||
|
|
||||||
|
<visual>
|
||||||
|
<rgba haze="0.15 0.25 0.35 1"/>
|
||||||
|
<map shadowscale="0.5" zfar="40"/>
|
||||||
|
</visual>
|
||||||
|
|
||||||
|
<asset>
|
||||||
|
<texture type="skybox" builtin="gradient" rgb1="0.3 0.5 0.7" rgb2="0 0 0" width="64" height="64"/>
|
||||||
|
<texture name="texplane" type="2d" builtin="checker" rgb1=".2 .3 .4" rgb2=".1 0.15 0.2" width="512" height="512" mark="cross" markrgb=".8 .8 .8"/>
|
||||||
|
<material name="matplane" reflectance="0.3" texture="texplane" texrepeat="1 1" texuniform="true"/>
|
||||||
|
</asset>
|
||||||
|
|
||||||
|
<size memory="1G"/>
|
||||||
|
|
||||||
|
<option solver="CG" tolerance="1e-6" timestep=".003" density="1.225" viscosity="1.8e-5" integrator="implicitfast"/>
|
||||||
|
|
||||||
|
<visual>
|
||||||
|
<global realtime="0.2"/>
|
||||||
|
</visual>
|
||||||
|
|
||||||
|
<default>
|
||||||
|
<default class="leaf">
|
||||||
|
<geom type="ellipsoid" size=".025 .01 0.001" mass="1e-4" fluidshape="ellipsoid" rgba="0 .4 0 1"/>
|
||||||
|
</default>
|
||||||
|
</default>
|
||||||
|
|
||||||
|
<worldbody>
|
||||||
|
<camera name="Cinematic" pos="0 -1.6 4.5" xyaxes="1 0 0 0 0.6 0.8" mode="trackcom"/>
|
||||||
|
|
||||||
|
<light diffuse="1 1 1" attenuation=".4 .2 0" pos="-1 -1 6" mode="trackcom" dir="1 1 -3"/>
|
||||||
|
<light diffuse="1 1 1" attenuation=".4 .2 0" pos="-.98 -1.02 6" mode="trackcom" dir="1 1 -3"/>
|
||||||
|
<geom name="ground" type="plane" size="5 5 .01" pos="0 0 0" quat="1 0 0 0" material="matplane" condim="1"/>
|
||||||
|
<replicate count="10" offset="0 0 0.07" euler="0 0 30">
|
||||||
|
<replicate count="6" euler="-3 0 0">
|
||||||
|
<frame pos="-.3 -.3 3">
|
||||||
|
<replicate count="5" offset=".05 0 0" euler="50 0 0">
|
||||||
|
<body euler="50 50 0">
|
||||||
|
<freejoint/>
|
||||||
|
<geom class="leaf" pos=".025 0 0" euler="10 0 0"/>
|
||||||
|
<geom class="leaf" pos="-.025 0 0" euler="-14 0 0" mass="1.2e-4"/>
|
||||||
|
</body>
|
||||||
|
</replicate>
|
||||||
|
</frame>
|
||||||
|
</replicate>
|
||||||
|
</replicate>
|
||||||
|
</worldbody>
|
||||||
|
</mujoco>
|
||||||
@@ -0,0 +1,69 @@
|
|||||||
|
<mujoco model="Newton's Cradle">
|
||||||
|
<option timestep="1e-4"/>
|
||||||
|
|
||||||
|
<visual>
|
||||||
|
<global realtime="0.2"/>
|
||||||
|
</visual>
|
||||||
|
|
||||||
|
<asset>
|
||||||
|
<texture type="skybox" builtin="gradient" rgb1="0.3 0.5 0.7" rgb2="0 0 0" width="64" height="64"/>
|
||||||
|
<texture name="texplane" type="2d" builtin="checker" rgb1=".2 .3 .4" rgb2=".1 0.15 0.2" width="512" height="512" mark="cross" markrgb=".8 .8 .8"/>
|
||||||
|
<material name="matplane" reflectance="0.5" texture="texplane" texrepeat="3 3" texuniform="true"/>
|
||||||
|
</asset>
|
||||||
|
|
||||||
|
<default>
|
||||||
|
<geom friction=".5" solref="-1e8 -0" solimp="0 .01 1e-3"/>
|
||||||
|
<tendon limited="true" range="0 0.146" width="0.001" solreflimit="1e-3 1" rgba="0 0 .7 1"/>
|
||||||
|
<default class="steel ball">
|
||||||
|
<geom size=".02" rgba=".7 .7 0 1" density="8000" friction=".4" solref="-1e8 -0" solimp="0 .01 1e-3"/>
|
||||||
|
</default>
|
||||||
|
</default>
|
||||||
|
|
||||||
|
<worldbody>
|
||||||
|
<light pos=".5 0 1" dir="-1 0 -2"/>
|
||||||
|
<light pos="-.5 0 1" dir="1 0 -2"/>
|
||||||
|
<geom type="plane" size="4 4 .1" material="matplane"/>
|
||||||
|
|
||||||
|
<replicate count="5" offset="-.04 0 0">
|
||||||
|
<site name="a" pos="0 -.04 .2"/>
|
||||||
|
<site name="c" pos="0 .04 .2"/>
|
||||||
|
<body name="equilibrium" pos="0 0 .04">
|
||||||
|
<freejoint/>
|
||||||
|
<geom class="steel ball"/>
|
||||||
|
<site name="b" pos="0 -.008 .018"/>
|
||||||
|
<site name="d" pos="0 .008 .018"/>
|
||||||
|
</body>
|
||||||
|
</replicate>
|
||||||
|
|
||||||
|
<replicate count="2" offset=".04 0 0">
|
||||||
|
<site name="e" pos=".04 -.04 .2"/>
|
||||||
|
<site name="g" pos=".04 .04 .2"/>
|
||||||
|
<body name="perturbed" pos=".12 0 .061" euler="0 -34 0">
|
||||||
|
<freejoint/>
|
||||||
|
<geom class="steel ball"/>
|
||||||
|
<site name="f" pos="0 -.008 .018"/>
|
||||||
|
<site name="h" pos="0 .008 .018"/>
|
||||||
|
</body>
|
||||||
|
</replicate>
|
||||||
|
</worldbody>
|
||||||
|
|
||||||
|
<tendon>
|
||||||
|
<spatial>
|
||||||
|
<site site="a"/>
|
||||||
|
<site site="b"/>
|
||||||
|
</spatial>
|
||||||
|
<spatial>
|
||||||
|
<site site="c"/>
|
||||||
|
<site site="d"/>
|
||||||
|
</spatial>
|
||||||
|
<spatial>
|
||||||
|
<site site="e"/>
|
||||||
|
<site site="f"/>
|
||||||
|
</spatial>
|
||||||
|
<spatial>
|
||||||
|
<site site="g"/>
|
||||||
|
<site site="h"/>
|
||||||
|
</spatial>
|
||||||
|
</tendon>
|
||||||
|
|
||||||
|
</mujoco>
|
||||||
@@ -0,0 +1,23 @@
|
|||||||
|
<mujoco model="Particle">
|
||||||
|
|
||||||
|
<include file="container.xml"/>
|
||||||
|
|
||||||
|
<size memory="1G"/>
|
||||||
|
|
||||||
|
<option solver="CG" tolerance="1e-6" timestep=".01"/>
|
||||||
|
|
||||||
|
<worldbody>
|
||||||
|
<replicate count="10" offset=".07 0 0">
|
||||||
|
<replicate count="10" offset="0 .07 0">
|
||||||
|
<replicate count="10" offset="0 0 .07">
|
||||||
|
<body pos="-0.35 -0.35 0.65">
|
||||||
|
<joint type="slide" axis="1 0 0" group="3"/>
|
||||||
|
<joint type="slide" axis="0 1 0" group="3"/>
|
||||||
|
<joint type="slide" axis="0 0 1" group="3"/>
|
||||||
|
<geom size="0.025" condim="1" priority="1" rgba="0.8 0.2 0.1 1"/>
|
||||||
|
</body>
|
||||||
|
</replicate>
|
||||||
|
</replicate>
|
||||||
|
</replicate>
|
||||||
|
</worldbody>
|
||||||
|
</mujoco>
|
||||||
@@ -0,0 +1,40 @@
|
|||||||
|
<mujoco model="Stonehenge">
|
||||||
|
<statistic meansize="1"/>
|
||||||
|
<worldbody>
|
||||||
|
<light directional="true" pos="-50 -20 10" dir="5 2 -1" diffuse="1 1 1"/>
|
||||||
|
<geom type="plane" size="30 30 .1" rgba="0 1 0 1"/>
|
||||||
|
<geom type="plane" pos="38 0 5" size="10 30 .1" euler="0 -30 0" rgba="0 .8 0 1"/>
|
||||||
|
|
||||||
|
<body name="wrecking ball" pos="45 0 16">
|
||||||
|
<freejoint/>
|
||||||
|
<geom size="5.5"/>
|
||||||
|
</body>
|
||||||
|
|
||||||
|
<replicate count="30" euler="0 0 12">
|
||||||
|
<body pos="0 -15 2">
|
||||||
|
<freejoint/>
|
||||||
|
<geom type="box" size="1 .5 2"/>
|
||||||
|
</body>
|
||||||
|
<body pos="1.5 -15 4.4" euler="0 0 6">
|
||||||
|
<freejoint/>
|
||||||
|
<geom type="box" size="1.5 .5 .4"/>
|
||||||
|
</body>
|
||||||
|
</replicate>
|
||||||
|
|
||||||
|
<replicate count="5" euler="0 0 45">
|
||||||
|
<body pos="-1.2 -10 3">
|
||||||
|
<freejoint/>
|
||||||
|
<geom type="box" size=".9 .5 3"/>
|
||||||
|
</body>
|
||||||
|
<body pos="1.2 -10 3">
|
||||||
|
<freejoint/>
|
||||||
|
<geom type="box" size=".9 .5 3"/>
|
||||||
|
</body>
|
||||||
|
<body pos="0 -10 6.4">
|
||||||
|
<freejoint/>
|
||||||
|
<geom type="box" size="2.4 .5 .4"/>
|
||||||
|
</body>
|
||||||
|
</replicate>
|
||||||
|
|
||||||
|
</worldbody>
|
||||||
|
</mujoco>
|
||||||
@@ -0,0 +1,47 @@
|
|||||||
|
<mujoco model="Tendon">
|
||||||
|
<option solver="PGS"/>
|
||||||
|
|
||||||
|
<default>
|
||||||
|
<site size=".02" rgba="0 .8 0 1"/>
|
||||||
|
<geom condim="1"/>
|
||||||
|
</default>
|
||||||
|
|
||||||
|
<worldbody>
|
||||||
|
<light pos="1 0 4" dir="-1 0 -4"/>
|
||||||
|
<light pos="-1 0 4" dir="1 0 -4"/>
|
||||||
|
<geom type="plane" size="2 2 .01"/>
|
||||||
|
<site name="ceiling" pos="0.01 0.02 2.4"/>
|
||||||
|
|
||||||
|
<body pos="0 0 1.5">
|
||||||
|
<freejoint/>
|
||||||
|
<site name="hook" pos="0 0 .3"/>
|
||||||
|
<site name="bulb"/>
|
||||||
|
<geom name="bulb" size=".3" rgba="0 0 .8 1" density="100" contype="0" conaffinity="0"/>
|
||||||
|
</body>
|
||||||
|
|
||||||
|
<replicate count="18" euler="0 0 20">
|
||||||
|
<body pos=".2 0 1.5">
|
||||||
|
<freejoint/>
|
||||||
|
<site name="a"/>
|
||||||
|
<geom rgba=".8 0 .4 1" size=".03"/>
|
||||||
|
</body>
|
||||||
|
<body pos="1.2 0 1.5">
|
||||||
|
<freejoint/>
|
||||||
|
<site name="b"/>
|
||||||
|
<geom rgba=".8 .4 0 1" size=".1"/>
|
||||||
|
</body>
|
||||||
|
</replicate>
|
||||||
|
</worldbody>
|
||||||
|
|
||||||
|
<tendon>
|
||||||
|
<spatial springlength="0 0.5" stiffness="10000">
|
||||||
|
<site site="ceiling"/>
|
||||||
|
<site site="hook"/>
|
||||||
|
</spatial>
|
||||||
|
<spatial range="0 0.5">
|
||||||
|
<site site="a"/>
|
||||||
|
<geom geom="bulb" sidesite="bulb"/>
|
||||||
|
<site site="b"/>
|
||||||
|
</spatial>
|
||||||
|
</tendon>
|
||||||
|
</mujoco>
|
||||||
@@ -84,7 +84,7 @@ if(NOT TARGET mujoco)
|
|||||||
if(MUJOCO_FRAMEWORK)
|
if(MUJOCO_FRAMEWORK)
|
||||||
message("MuJoCo framework is at ${MUJOCO_FRAMEWORK}/mujoco.framework")
|
message("MuJoCo framework is at ${MUJOCO_FRAMEWORK}/mujoco.framework")
|
||||||
set(MUJOCO_LIBRARY
|
set(MUJOCO_LIBRARY
|
||||||
${MUJOCO_FRAMEWORK}/mujoco.framework/Versions/A/libmujoco.3.1.4.dylib
|
${MUJOCO_FRAMEWORK}/mujoco.framework/Versions/A/libmujoco.3.1.5.dylib
|
||||||
)
|
)
|
||||||
target_compile_options(mujoco INTERFACE -F${MUJOCO_FRAMEWORK})
|
target_compile_options(mujoco INTERFACE -F${MUJOCO_FRAMEWORK})
|
||||||
endif()
|
endif()
|
||||||
@@ -92,7 +92,7 @@ if(NOT TARGET mujoco)
|
|||||||
|
|
||||||
if(NOT MUJOCO_FRAMEWORK)
|
if(NOT MUJOCO_FRAMEWORK)
|
||||||
find_library(
|
find_library(
|
||||||
MUJOCO_LIBRARY mujoco mujoco.3.1.4 HINTS ${MUJOCO_LIBRARY_DIR} REQUIRED
|
MUJOCO_LIBRARY mujoco mujoco.3.1.5 HINTS ${MUJOCO_LIBRARY_DIR} REQUIRED
|
||||||
)
|
)
|
||||||
find_path(MUJOCO_INCLUDE mujoco/mujoco.h HINTS ${MUJOCO_INCLUDE_DIR} REQUIRED)
|
find_path(MUJOCO_INCLUDE mujoco/mujoco.h HINTS ${MUJOCO_INCLUDE_DIR} REQUIRED)
|
||||||
message("MuJoCo is at ${MUJOCO_LIBRARY}")
|
message("MuJoCo is at ${MUJOCO_LIBRARY}")
|
||||||
@@ -191,7 +191,7 @@ findorfetch(
|
|||||||
GIT_REPO
|
GIT_REPO
|
||||||
https://github.com/pybind/pybind11
|
https://github.com/pybind/pybind11
|
||||||
GIT_TAG
|
GIT_TAG
|
||||||
8a099e44b3d5f85b20f05828d919d2332a8de841 # v2.11.1
|
3e9dfa2866941655c56877882565e7577de6fc7b # v2.12.0
|
||||||
TARGETS
|
TARGETS
|
||||||
pybind11::pybind11_headers
|
pybind11::pybind11_headers
|
||||||
EXCLUDE_FROM_ALL
|
EXCLUDE_FROM_ALL
|
||||||
|
|||||||
@@ -1303,6 +1303,23 @@ Euler integrator, semi-implicit in velocity.
|
|||||||
mat = np.array([[1., 2., 3.], [4., 5., 6.], [7., 8., 9.]])
|
mat = np.array([[1., 2., 3.], [4., 5., 6.], [7., 8., 9.]])
|
||||||
self.assertEqual(mujoco.mju_mulVecMatVec(vec1, mat, vec2), 204.)
|
self.assertEqual(mujoco.mju_mulVecMatVec(vec1, mat, vec2), 204.)
|
||||||
|
|
||||||
|
def test_mju_euler_to_quat(self):
|
||||||
|
quat = np.zeros(4)
|
||||||
|
euler = np.array([0, np.pi/2, 0])
|
||||||
|
seq = 'xyz'
|
||||||
|
mujoco.mju_euler2Quat(quat, euler, seq)
|
||||||
|
expected_quat = np.array([np.sqrt(0.5), 0, np.sqrt(0.5), 0.])
|
||||||
|
np.testing.assert_almost_equal(quat, expected_quat)
|
||||||
|
|
||||||
|
error = 'mju_euler2Quat: seq must contain exactly 3 characters'
|
||||||
|
with self.assertRaisesWithLiteralMatch(mujoco.FatalError, error):
|
||||||
|
mujoco.mju_euler2Quat(quat, euler, 'xy')
|
||||||
|
with self.assertRaisesWithLiteralMatch(mujoco.FatalError, error):
|
||||||
|
mujoco.mju_euler2Quat(quat, euler, 'xyzy')
|
||||||
|
error = 'mju_euler2Quat: seq[2] is \'p\', should be one of x, y, z, X, Y, Z'
|
||||||
|
with self.assertRaisesWithLiteralMatch(mujoco.FatalError, error):
|
||||||
|
mujoco.mju_euler2Quat(quat, euler, 'xYp')
|
||||||
|
|
||||||
@parameterized.product(flg_html=(False, True), flg_pad=(False, True))
|
@parameterized.product(flg_html=(False, True), flg_pad=(False, True))
|
||||||
def test_mj_printSchema(self, flg_html, flg_pad): # pylint: disable=invalid-name
|
def test_mj_printSchema(self, flg_html, flg_pad): # pylint: disable=invalid-name
|
||||||
# Make sure that mj_printSchema doesn't raise an exception
|
# Make sure that mj_printSchema doesn't raise an exception
|
||||||
|
|||||||
@@ -106,11 +106,11 @@ PYBIND11_MODULE(_functions, pymodule) {
|
|||||||
Def<traits::mj_defaultOption>(pymodule);
|
Def<traits::mj_defaultOption>(pymodule);
|
||||||
Def<traits::mj_defaultVisual>(pymodule);
|
Def<traits::mj_defaultVisual>(pymodule);
|
||||||
// Skipped: mj_copyModel (have MjModel.__copy__, memory managed by MjModel)
|
// Skipped: mj_copyModel (have MjModel.__copy__, memory managed by MjModel)
|
||||||
DEF_WITH_OMITTED_PY_ARGS(traits::mj_saveModel, "buffer_sz")(
|
pymodule.def(
|
||||||
pymodule,
|
"mj_saveModel",
|
||||||
[](const raw::MjModel* m, const std::optional<std::string>& filename,
|
[](const MjModelWrapper& m, const std::optional<std::string>& filename = std::nullopt,
|
||||||
std::optional<
|
std::optional<
|
||||||
Eigen::Ref<Eigen::Vector<std::uint8_t, Eigen::Dynamic>>> buffer) {
|
Eigen::Ref<Eigen::Vector<std::uint8_t, Eigen::Dynamic>>> buffer = std::nullopt) {
|
||||||
void* buffer_ptr = nullptr;
|
void* buffer_ptr = nullptr;
|
||||||
int buffer_sz = 0;
|
int buffer_sz = 0;
|
||||||
if (buffer.has_value()) {
|
if (buffer.has_value()) {
|
||||||
@@ -118,9 +118,13 @@ PYBIND11_MODULE(_functions, pymodule) {
|
|||||||
buffer_sz = buffer->size();
|
buffer_sz = buffer->size();
|
||||||
}
|
}
|
||||||
return InterceptMjErrors(::mj_saveModel)(
|
return InterceptMjErrors(::mj_saveModel)(
|
||||||
m, filename.has_value() ? filename->c_str() : nullptr,
|
m.get(), filename.has_value() ? filename->c_str() : nullptr,
|
||||||
buffer_ptr, buffer_sz);
|
buffer_ptr, buffer_sz);
|
||||||
});
|
},
|
||||||
|
py::arg("m"), py::arg_v("filename", std::nullopt),
|
||||||
|
py::arg_v("buffer", std::nullopt),
|
||||||
|
py::doc(traits::mj_saveModel::doc),
|
||||||
|
py::call_guard<py::gil_scoped_release>());
|
||||||
// Skipped: mj_loadModel (have MjModel.from_binary_path)
|
// Skipped: mj_loadModel (have MjModel.from_binary_path)
|
||||||
// Skipped: mj_deleteModel (have MjModel.__del__)
|
// Skipped: mj_deleteModel (have MjModel.__del__)
|
||||||
Def<traits::mj_sizeModel>(pymodule);
|
Def<traits::mj_sizeModel>(pymodule);
|
||||||
@@ -995,6 +999,7 @@ PYBIND11_MODULE(_functions, pymodule) {
|
|||||||
Def<traits::mju_derivQuat>(pymodule);
|
Def<traits::mju_derivQuat>(pymodule);
|
||||||
Def<traits::mju_quatIntegrate>(pymodule);
|
Def<traits::mju_quatIntegrate>(pymodule);
|
||||||
Def<traits::mju_quatZ2Vec>(pymodule);
|
Def<traits::mju_quatZ2Vec>(pymodule);
|
||||||
|
Def<traits::mju_euler2Quat>(pymodule);
|
||||||
|
|
||||||
// Poses
|
// Poses
|
||||||
Def<traits::mju_mulPose>(pymodule);
|
Def<traits::mju_mulPose>(pymodule);
|
||||||
|
|||||||
@@ -7,13 +7,13 @@
|
|||||||
<key>CFBundleIdentifier</key>
|
<key>CFBundleIdentifier</key>
|
||||||
<string>org.mujoco.mjpython</string>
|
<string>org.mujoco.mjpython</string>
|
||||||
<key>CFBundleVersion</key>
|
<key>CFBundleVersion</key>
|
||||||
<string>3.1.4</string>
|
<string>3.1.5</string>
|
||||||
<key>CFBundleGetInfoString</key>
|
<key>CFBundleGetInfoString</key>
|
||||||
<string>3.1.4</string>
|
<string>3.1.5</string>
|
||||||
<key>CFBundleLongVersionString</key>
|
<key>CFBundleLongVersionString</key>
|
||||||
<string>3.1.4</string>
|
<string>3.1.5</string>
|
||||||
<key>CFBundleShortVersionString</key>
|
<key>CFBundleShortVersionString</key>
|
||||||
<string>3.1.4</string>
|
<string>3.1.5</string>
|
||||||
<key>CFBundleExecutable</key>
|
<key>CFBundleExecutable</key>
|
||||||
<string>mjpython</string>
|
<string>mjpython</string>
|
||||||
<key>CFBundleIconFile</key>
|
<key>CFBundleIconFile</key>
|
||||||
|
|||||||
@@ -78,7 +78,10 @@ the clause:
|
|||||||
|
|
||||||
# Create render contexts.
|
# Create render contexts.
|
||||||
# TODO(nimrod): Figure out why pytype doesn't like gl_context.GLContext
|
# TODO(nimrod): Figure out why pytype doesn't like gl_context.GLContext
|
||||||
self._gl_context = gl_context.GLContext(width, height) # type: ignore
|
self._gl_context = None # type: ignore
|
||||||
|
if gl_context.GLContext is not None:
|
||||||
|
self._gl_context = gl_context.GLContext(width, height)
|
||||||
|
if self._gl_context:
|
||||||
self._gl_context.make_current()
|
self._gl_context.make_current()
|
||||||
self._mjr_context = _render.MjrContext(
|
self._mjr_context = _render.MjrContext(
|
||||||
model, _enums.mjtFontScale.mjFONTSCALE_150.value
|
model, _enums.mjtFontScale.mjFONTSCALE_150.value
|
||||||
@@ -148,8 +151,10 @@ the clause:
|
|||||||
self._scene.flags[_enums.mjtRndFlag.mjRND_SEGMENT] = True
|
self._scene.flags[_enums.mjtRndFlag.mjRND_SEGMENT] = True
|
||||||
self._scene.flags[_enums.mjtRndFlag.mjRND_IDCOLOR] = True
|
self._scene.flags[_enums.mjtRndFlag.mjRND_IDCOLOR] = True
|
||||||
|
|
||||||
if self._gl_context is None:
|
if self._mjr_context is None:
|
||||||
raise RuntimeError('render cannot be called after close.')
|
raise RuntimeError('render cannot be called after close.')
|
||||||
|
|
||||||
|
if self._gl_context:
|
||||||
self._gl_context.make_current()
|
self._gl_context.make_current()
|
||||||
|
|
||||||
if self._depth_rendering:
|
if self._depth_rendering:
|
||||||
|
|||||||
@@ -2226,6 +2226,7 @@ This is useful for example when the MJB is not available as a file on disk.)"));
|
|||||||
X(headlight);
|
X(headlight);
|
||||||
X(directional);
|
X(directional);
|
||||||
X(castshadow);
|
X(castshadow);
|
||||||
|
X(bulbradius);
|
||||||
#undef X
|
#undef X
|
||||||
|
|
||||||
#define X(var) DefinePyArray(mjvLight, #var, &MjvLightWrapper::var)
|
#define X(var) DefinePyArray(mjvLight, #var, &MjvLightWrapper::var)
|
||||||
|
|||||||
@@ -4,7 +4,7 @@ build-backend = "setuptools.build_meta"
|
|||||||
|
|
||||||
[project]
|
[project]
|
||||||
name = "mujoco"
|
name = "mujoco"
|
||||||
version = "3.1.4"
|
version = "3.1.5"
|
||||||
authors = [
|
authors = [
|
||||||
{name = "Google DeepMind", email = "mujoco@deepmind.com"},
|
{name = "Google DeepMind", email = "mujoco@deepmind.com"},
|
||||||
]
|
]
|
||||||
@@ -36,9 +36,9 @@ dynamic = ["readme", "scripts"]
|
|||||||
|
|
||||||
[project.urls]
|
[project.urls]
|
||||||
Homepage = "https://github.com/google-deepmind/mujoco"
|
Homepage = "https://github.com/google-deepmind/mujoco"
|
||||||
Documentation = "https://mujoco.readthedocs.io/en/3.1.4"
|
Documentation = "https://mujoco.readthedocs.io/en/3.1.5"
|
||||||
Repository = "https://github.com/google-deepmind/mujoco"
|
Repository = "https://github.com/google-deepmind/mujoco"
|
||||||
Changelog = "https://mujoco.readthedocs.io/en/3.1.4/changelog.html"
|
Changelog = "https://mujoco.readthedocs.io/en/3.1.5/changelog.html"
|
||||||
|
|
||||||
[tool.setuptools]
|
[tool.setuptools]
|
||||||
include-package-data = false
|
include-package-data = false
|
||||||
|
|||||||
@@ -24,7 +24,7 @@ set(MSVC_INCREMENTAL_DEFAULT ON)
|
|||||||
|
|
||||||
project(
|
project(
|
||||||
mujoco_samples
|
mujoco_samples
|
||||||
VERSION 3.1.4
|
VERSION 3.1.5
|
||||||
DESCRIPTION "MuJoCo samples binaries"
|
DESCRIPTION "MuJoCo samples binaries"
|
||||||
HOMEPAGE_URL "https://mujoco.org"
|
HOMEPAGE_URL "https://mujoco.org"
|
||||||
)
|
)
|
||||||
|
|||||||
+2
-1
@@ -89,7 +89,8 @@ int filetype(const char* filename) {
|
|||||||
|
|
||||||
|
|
||||||
// main function
|
// main function
|
||||||
int main(int argc, const char** argv) {
|
int main(int argc, char** argv) {
|
||||||
|
|
||||||
// model and error
|
// model and error
|
||||||
mjModel* m = 0;
|
mjModel* m = 0;
|
||||||
char error[1000];
|
char error[1000];
|
||||||
|
|||||||
@@ -29,7 +29,7 @@ set(MUJOCO_DEP_VERSION_lodepng
|
|||||||
|
|
||||||
project(
|
project(
|
||||||
mujoco_simulate
|
mujoco_simulate
|
||||||
VERSION 3.1.4
|
VERSION 3.1.5
|
||||||
DESCRIPTION "MuJoCo simulate binaries"
|
DESCRIPTION "MuJoCo simulate binaries"
|
||||||
HOMEPAGE_URL "https://mujoco.org"
|
HOMEPAGE_URL "https://mujoco.org"
|
||||||
)
|
)
|
||||||
|
|||||||
@@ -163,6 +163,7 @@ void mjccd_support(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *vec) {
|
|||||||
break;
|
break;
|
||||||
|
|
||||||
case mjGEOM_MESH:
|
case mjGEOM_MESH:
|
||||||
|
case mjGEOM_SDF:
|
||||||
// init search
|
// init search
|
||||||
vertdata = m->mesh_vert + 3*m->mesh_vertadr[m->geom_dataid[g]];
|
vertdata = m->mesh_vert + 3*m->mesh_vertadr[m->geom_dataid[g]];
|
||||||
tmp = -1E+10;
|
tmp = -1E+10;
|
||||||
|
|||||||
@@ -40,7 +40,7 @@
|
|||||||
// table of pair-wise collision functions
|
// table of pair-wise collision functions
|
||||||
mjfCollision mjCOLLISIONFUNC[mjNGEOMTYPES][mjNGEOMTYPES] = {
|
mjfCollision mjCOLLISIONFUNC[mjNGEOMTYPES][mjNGEOMTYPES] = {
|
||||||
/* PLANE HFIELD SPHERE CAPSULE ELLIPSOID CYLINDER BOX MESH SDF */
|
/* PLANE HFIELD SPHERE CAPSULE ELLIPSOID CYLINDER BOX MESH SDF */
|
||||||
/*PLANE */ {0, 0, mjc_PlaneSphere, mjc_PlaneCapsule, mjc_PlaneConvex, mjc_PlaneCylinder, mjc_PlaneBox, mjc_PlaneConvex, mjc_SDF},
|
/*PLANE */ {0, 0, mjc_PlaneSphere, mjc_PlaneCapsule, mjc_PlaneConvex, mjc_PlaneCylinder, mjc_PlaneBox, mjc_PlaneConvex, mjc_PlaneConvex},
|
||||||
/*HFIELD */ {0, 0, mjc_ConvexHField, mjc_ConvexHField, mjc_ConvexHField, mjc_ConvexHField, mjc_ConvexHField, mjc_ConvexHField, mjc_HFieldSDF},
|
/*HFIELD */ {0, 0, mjc_ConvexHField, mjc_ConvexHField, mjc_ConvexHField, mjc_ConvexHField, mjc_ConvexHField, mjc_ConvexHField, mjc_HFieldSDF},
|
||||||
/*SPHERE */ {0, 0, mjc_SphereSphere, mjc_SphereCapsule, mjc_Convex, mjc_SphereCylinder, mjc_SphereBox, mjc_Convex, mjc_SDF},
|
/*SPHERE */ {0, 0, mjc_SphereSphere, mjc_SphereCapsule, mjc_Convex, mjc_SphereCylinder, mjc_SphereBox, mjc_Convex, mjc_SDF},
|
||||||
/*CAPSULE */ {0, 0, 0, mjc_CapsuleCapsule, mjc_Convex, mjc_Convex, mjc_CapsuleBox, mjc_Convex, mjc_SDF},
|
/*CAPSULE */ {0, 0, 0, mjc_CapsuleCapsule, mjc_Convex, mjc_Convex, mjc_CapsuleBox, mjc_Convex, mjc_SDF},
|
||||||
|
|||||||
+18
-21
@@ -256,6 +256,21 @@ static void clampVec(mjtNum* vec, const mjtNum* range, const mjtByte* limited, i
|
|||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
// return number of dofs given joint type
|
||||||
|
static int jnt_dofnum(mjtJoint type) {
|
||||||
|
if (type == mjJNT_FREE) {
|
||||||
|
return 6;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (type == mjJNT_BALL) {
|
||||||
|
return 3;
|
||||||
|
}
|
||||||
|
|
||||||
|
return 1;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
// (qpos, qvel, ctrl, act) => (qfrc_actuator, actuator_force, act_dot)
|
// (qpos, qvel, ctrl, act) => (qfrc_actuator, actuator_force, act_dot)
|
||||||
void mj_fwdActuation(const mjModel* m, mjData* d) {
|
void mj_fwdActuation(const mjModel* m, mjData* d) {
|
||||||
TM_START;
|
TM_START;
|
||||||
@@ -469,7 +484,7 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
|
|||||||
mju_mulMatTVec(d->qfrc_actuator, moment, force, nu, nv);
|
mju_mulMatTVec(d->qfrc_actuator, moment, force, nu, nv);
|
||||||
|
|
||||||
// actuator-level gravity compensation
|
// actuator-level gravity compensation
|
||||||
if (!mjDISABLED(mjDSBL_GRAVITY) && mju_norm3(m->opt.gravity)) {
|
if (m->ngravcomp && !mjDISABLED(mjDSBL_GRAVITY) && mju_norm3(m->opt.gravity)) {
|
||||||
int njnt = m->njnt;
|
int njnt = m->njnt;
|
||||||
for (int i=0; i < njnt; i++) {
|
for (int i=0; i < njnt; i++) {
|
||||||
// skip if gravcomp added as passive force
|
// skip if gravcomp added as passive force
|
||||||
@@ -477,28 +492,10 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
|
|||||||
continue;
|
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
|
// add gravcomp force
|
||||||
|
int dofnum = jnt_dofnum(m->jnt_type[i]);
|
||||||
int dofadr = m->jnt_dofadr[i];
|
int dofadr = m->jnt_dofadr[i];
|
||||||
for (int j=0; j < dofnum; j++) {
|
mju_addTo(d->qfrc_actuator + dofadr, d->qfrc_gravcomp + dofadr, dofnum);
|
||||||
d->qfrc_actuator[dofadr+j] += d->qfrc_gravcomp[dofadr+j];
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
+17
-7
@@ -534,11 +534,15 @@ mjModel* mj_makeModel(
|
|||||||
m->nuser_actuator = nuser_actuator;
|
m->nuser_actuator = nuser_actuator;
|
||||||
m->nuser_sensor = nuser_sensor;
|
m->nuser_sensor = nuser_sensor;
|
||||||
m->nnames = nnames;
|
m->nnames = nnames;
|
||||||
m->nnames_map = mjLOAD_MULTIPLE
|
long nnames_map = (long)nbody + njnt + ngeom + nsite + ncam + nlight + nflex + nmesh + nskin +
|
||||||
* (nbody + njnt + ngeom + nsite + ncam + nlight + nflex + nmesh
|
nhfield + ntex + nmat + npair + nexclude + neq + ntendon + nu + nsensor +
|
||||||
+ nskin + nhfield + ntex + nmat + npair + nexclude + neq
|
nnumeric + ntext + ntuple + nkey + nplugin;
|
||||||
+ ntendon + nu + nsensor + nnumeric + ntext + ntuple
|
if (nnames_map >= INT_MAX / mjLOAD_MULTIPLE) {
|
||||||
+ nkey + nplugin);
|
mju_free(m);
|
||||||
|
mju_warning("Invalid model: size of nnames_map is larger than INT_MAX");
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
m->nnames_map = mjLOAD_MULTIPLE * nnames_map;
|
||||||
m->npaths = npaths;
|
m->npaths = npaths;
|
||||||
|
|
||||||
#define X(name) \
|
#define X(name) \
|
||||||
@@ -1520,14 +1524,20 @@ void* mj_stackAllocByte(mjData* d, size_t bytes, size_t alignment) {
|
|||||||
|
|
||||||
|
|
||||||
// allocate mjtNums on the stack
|
// allocate mjtNums on the stack
|
||||||
mjtNum* mj_stackAllocNum(mjData* d, int size) {
|
mjtNum* mj_stackAllocNum(mjData* d, size_t size) {
|
||||||
|
if (mjUNLIKELY(size >= SIZE_MAX / sizeof(mjtNum))) {
|
||||||
|
mjERROR("requested size is too large.");
|
||||||
|
}
|
||||||
return (mjtNum*) stackalloc(d, size * sizeof(mjtNum), _Alignof(mjtNum));
|
return (mjtNum*) stackalloc(d, size * sizeof(mjtNum), _Alignof(mjtNum));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
// allocate ints on the stack
|
// allocate ints on the stack
|
||||||
int* mj_stackAllocInt(mjData* d, int size) {
|
int* mj_stackAllocInt(mjData* d, size_t size) {
|
||||||
|
if (mjUNLIKELY(size >= SIZE_MAX / sizeof(int))) {
|
||||||
|
mjERROR("requested size is too large.");
|
||||||
|
}
|
||||||
return (int*) stackalloc(d, size * sizeof(int), _Alignof(int));
|
return (int*) stackalloc(d, size * sizeof(int), _Alignof(int));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -129,10 +129,10 @@ void mj__freeStack(mjData* d) __attribute__((noinline));
|
|||||||
MJAPI void* mj_stackAllocByte(mjData* d, size_t bytes, size_t alignment);
|
MJAPI void* mj_stackAllocByte(mjData* d, size_t bytes, size_t alignment);
|
||||||
|
|
||||||
// mjData stack allocate for array of mjtNums
|
// mjData stack allocate for array of mjtNums
|
||||||
MJAPI mjtNum* mj_stackAllocNum(mjData* d, int size);
|
MJAPI mjtNum* mj_stackAllocNum(mjData* d, size_t size);
|
||||||
|
|
||||||
// mjData stack allocate for array of ints
|
// mjData stack allocate for array of ints
|
||||||
MJAPI int* mj_stackAllocInt(mjData* d, int size);
|
MJAPI int* mj_stackAllocInt(mjData* d, size_t size);
|
||||||
|
|
||||||
// de-allocate data
|
// de-allocate data
|
||||||
MJAPI void mj_deleteData(mjData* d);
|
MJAPI void mj_deleteData(mjData* d);
|
||||||
|
|||||||
@@ -175,7 +175,7 @@ static void mj_springdamper(const mjModel* m, mjData* d) {
|
|||||||
|
|
||||||
// body-level gravity compensation, return 1 if any, 0 otherwise
|
// body-level gravity compensation, return 1 if any, 0 otherwise
|
||||||
static int mj_gravcomp(const mjModel* m, mjData* d) {
|
static int mj_gravcomp(const mjModel* m, mjData* d) {
|
||||||
if (mjDISABLED(mjDSBL_GRAVITY) || mju_norm3(m->opt.gravity) == 0) {
|
if (!m->ngravcomp || mjDISABLED(mjDSBL_GRAVITY) || mju_norm3(m->opt.gravity) == 0) {
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -1033,11 +1033,31 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename,
|
|||||||
fprintf(fp, "CONTACT\n");
|
fprintf(fp, "CONTACT\n");
|
||||||
for (int i=0; i < d->ncon; i++) {
|
for (int i=0; i < d->ncon; i++) {
|
||||||
fprintf(fp, " %d:\n dim %d\n", i, d->contact[i].dim);
|
fprintf(fp, " %d:\n dim %d\n", i, d->contact[i].dim);
|
||||||
|
int g1 = d->contact[i].geom[0];
|
||||||
|
int g2 = d->contact[i].geom[1];
|
||||||
|
|
||||||
|
// special case for geom-geom contacts
|
||||||
|
if (g1 > -1 && g2 > -1) {
|
||||||
|
fprintf(fp, " geoms ");
|
||||||
|
const char* geom1 = mj_id2name(m, mjOBJ_GEOM, g1);
|
||||||
|
const char* geom2 = mj_id2name(m, mjOBJ_GEOM, g2);
|
||||||
|
if (geom1) {
|
||||||
|
fprintf(fp, "%s : ", geom1);
|
||||||
|
} else {
|
||||||
|
fprintf(fp, "%d : ", g1);
|
||||||
|
}
|
||||||
|
if (geom2) {
|
||||||
|
fprintf(fp, "%s\n", geom2);
|
||||||
|
} else {
|
||||||
|
fprintf(fp, "%d\n", g2);
|
||||||
|
}
|
||||||
|
} else {
|
||||||
fprintf(fp, " gfev %d %d %d %d : %d %d %d %d\n",
|
fprintf(fp, " gfev %d %d %d %d : %d %d %d %d\n",
|
||||||
d->contact[i].geom[0], d->contact[i].flex[0],
|
d->contact[i].geom[0], d->contact[i].flex[0],
|
||||||
d->contact[i].elem[0], d->contact[i].vert[0],
|
d->contact[i].elem[0], d->contact[i].vert[0],
|
||||||
d->contact[i].geom[1], d->contact[i].flex[1],
|
d->contact[i].geom[1], d->contact[i].flex[1],
|
||||||
d->contact[i].elem[1], d->contact[i].vert[1]);
|
d->contact[i].elem[1], d->contact[i].vert[1]);
|
||||||
|
}
|
||||||
fprintf(fp, " exclude %d\n efc_address %d\n",
|
fprintf(fp, " exclude %d\n efc_address %d\n",
|
||||||
d->contact[i].exclude, d->contact[i].efc_address);
|
d->contact[i].exclude, d->contact[i].efc_address);
|
||||||
printVector(" solref ", d->contact[i].solref, mjNREF, fp, float_format);
|
printVector(" solref ", d->contact[i].solref, mjNREF, fp, float_format);
|
||||||
|
|||||||
@@ -15,6 +15,7 @@
|
|||||||
#include "engine/engine_setconst.h"
|
#include "engine/engine_setconst.h"
|
||||||
|
|
||||||
#include <stdio.h>
|
#include <stdio.h>
|
||||||
|
#include <string.h>
|
||||||
|
|
||||||
#include <mujoco/mjdata.h>
|
#include <mujoco/mjdata.h>
|
||||||
#include <mujoco/mjmacro.h>
|
#include <mujoco/mjmacro.h>
|
||||||
@@ -92,6 +93,11 @@ static void set0(mjModel* m, mjData* d) {
|
|||||||
// compute dof_M0 for CRB algorithm
|
// compute dof_M0 for CRB algorithm
|
||||||
mj_setM0(m, d);
|
mj_setM0(m, d);
|
||||||
|
|
||||||
|
// save flex_rigid, temporarily make all flexes non-rigid
|
||||||
|
mjtByte* rigid = mju_malloc(m->nflex);
|
||||||
|
memcpy(rigid, m->flex_rigid, m->nflex);
|
||||||
|
memset(m->flex_rigid, 0, m->nflex);
|
||||||
|
|
||||||
// run remaining computations
|
// run remaining computations
|
||||||
mj_crb(m, d);
|
mj_crb(m, d);
|
||||||
mj_factorM(m, d);
|
mj_factorM(m, d);
|
||||||
@@ -99,6 +105,10 @@ static void set0(mjModel* m, mjData* d) {
|
|||||||
mj_tendon(m, d);
|
mj_tendon(m, d);
|
||||||
mj_transmission(m, d);
|
mj_transmission(m, d);
|
||||||
|
|
||||||
|
// restore flex rigidity
|
||||||
|
memcpy(m->flex_rigid, rigid, m->nflex);
|
||||||
|
mju_free(rigid);
|
||||||
|
|
||||||
// restore camera and light mode
|
// restore camera and light mode
|
||||||
for (int i=0; i < m->ncam; i++) {
|
for (int i=0; i < m->ncam; i++) {
|
||||||
m->cam_mode[i] = cammode[i];
|
m->cam_mode[i] = cammode[i];
|
||||||
|
|||||||
@@ -38,8 +38,8 @@
|
|||||||
|
|
||||||
//-------------------------- Constants -------------------------------------------------------------
|
//-------------------------- Constants -------------------------------------------------------------
|
||||||
|
|
||||||
#define mjVERSION 314
|
#define mjVERSION 315
|
||||||
#define mjVERSIONSTRING "3.1.4"
|
#define mjVERSIONSTRING "3.1.5"
|
||||||
|
|
||||||
// names of disable flags
|
// names of disable flags
|
||||||
const char* mjDISABLESTRING[mjNDISABLE] = {
|
const char* mjDISABLESTRING[mjNDISABLE] = {
|
||||||
|
|||||||
@@ -511,3 +511,41 @@ void mju_makeFrame(mjtNum frame[9]) {
|
|||||||
// zaxis = cross(xaxis, yaxis)
|
// zaxis = cross(xaxis, yaxis)
|
||||||
mju_cross(frame+6, frame, frame+3);
|
mju_cross(frame+6, frame, frame+3);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
// convert sequence of Euler angles (radians) to quaternion
|
||||||
|
// seq[0,1,2] must be in 'xyzXYZ', lower/upper-case mean intrinsic/extrinsic rotations
|
||||||
|
void mju_euler2Quat(mjtNum quat[4], const mjtNum euler[3], const char* seq) {
|
||||||
|
if (strnlen(seq, 4) != 3) {
|
||||||
|
mjERROR("seq must contain exactly 3 characters");
|
||||||
|
}
|
||||||
|
|
||||||
|
// init
|
||||||
|
double tmp[4] = {1, 0, 0, 0};
|
||||||
|
|
||||||
|
// loop over euler angles, accumulate rotations
|
||||||
|
for (int i=0; i<3; i++) {
|
||||||
|
// construct quaternion rotation
|
||||||
|
mjtNum rot[4] = {cos(euler[i]/2), 0, 0, 0};
|
||||||
|
mjtNum sa = sin(euler[i]/2);
|
||||||
|
if (seq[i]=='x' || seq[i]=='X') {
|
||||||
|
rot[1] = sa;
|
||||||
|
} else if (seq[i]=='y' || seq[i]=='Y') {
|
||||||
|
rot[2] = sa;
|
||||||
|
} else if (seq[i]=='z' || seq[i]=='Z') {
|
||||||
|
rot[3] = sa;
|
||||||
|
} else {
|
||||||
|
mjERROR("seq[%d] is '%c', should be one of x, y, z, X, Y, Z", i, seq[i]);
|
||||||
|
}
|
||||||
|
|
||||||
|
// accumulate rotation
|
||||||
|
if (seq[i]=='x' || seq[i]=='y' || seq[i]=='z') {
|
||||||
|
mju_mulQuat(tmp, tmp, rot); // moving axes: post-multiply
|
||||||
|
} else {
|
||||||
|
mju_mulQuat(tmp, rot, tmp); // fixed axes: pre-multiply
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
mju_copy4(quat, tmp);
|
||||||
|
}
|
||||||
|
|||||||
@@ -75,6 +75,9 @@ MJAPI void mju_negPose(mjtNum posres[3], mjtNum quatres[4],
|
|||||||
MJAPI void mju_trnVecPose(mjtNum res[3], const mjtNum pos[3], const mjtNum quat[4],
|
MJAPI void mju_trnVecPose(mjtNum res[3], const mjtNum pos[3], const mjtNum quat[4],
|
||||||
const mjtNum vec[3]);
|
const mjtNum vec[3]);
|
||||||
|
|
||||||
|
// convert sequence of Euler angles (radians) to quaternion
|
||||||
|
// seq[0,1,2] must be in 'xyzXYZ', lower/upper-case mean intrinsic/extrinsic rotations
|
||||||
|
MJAPI void mju_euler2Quat(mjtNum quat[4], const mjtNum euler[3], const char* seq);
|
||||||
|
|
||||||
//------------------------------ spatial algebra ---------------------------------------------------
|
//------------------------------ spatial algebra ---------------------------------------------------
|
||||||
|
|
||||||
|
|||||||
@@ -2091,6 +2091,7 @@ void mjv_makeLights(const mjModel* m, mjData* d, mjvScene* scn) {
|
|||||||
memset(thislight, 0, sizeof(mjvLight));
|
memset(thislight, 0, sizeof(mjvLight));
|
||||||
thislight->directional = m->light_directional[i];
|
thislight->directional = m->light_directional[i];
|
||||||
thislight->castshadow = m->light_castshadow[i];
|
thislight->castshadow = m->light_castshadow[i];
|
||||||
|
thislight->bulbradius = m->light_bulbradius[i];
|
||||||
if (!thislight->directional) {
|
if (!thislight->directional) {
|
||||||
f2f(thislight->attenuation, m->light_attenuation+3*i, 3);
|
f2f(thislight->attenuation, m->light_attenuation+3*i, 3);
|
||||||
thislight->exponent = m->light_exponent[i];
|
thislight->exponent = m->light_exponent[i];
|
||||||
|
|||||||
@@ -21,7 +21,7 @@ set(MUJOCO_USER_SRCS
|
|||||||
user_composite.h
|
user_composite.h
|
||||||
user_flexcomp.cc
|
user_flexcomp.cc
|
||||||
user_flexcomp.h
|
user_flexcomp.h
|
||||||
user_init.cc
|
user_init.c
|
||||||
user_mesh.cc
|
user_mesh.cc
|
||||||
user_model.cc
|
user_model.cc
|
||||||
user_model.h
|
user_model.h
|
||||||
|
|||||||
+34
-3
@@ -91,6 +91,17 @@ int mjs_attachBody(mjsFrame* parent, const mjsBody* child,
|
|||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
// attach frame to a parent body
|
||||||
|
int mjs_attachFrame(mjsBody* parent, const mjsFrame* child,
|
||||||
|
const char* prefix, const char* suffix) {
|
||||||
|
mjCBody* body_parent = static_cast<mjCBody*>(parent->element);
|
||||||
|
mjCFrame* child_frame = static_cast<mjCFrame*>(child->element);
|
||||||
|
*body_parent += std::string(prefix) + *child_frame + std::string(suffix);
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
// get error message from model
|
// get error message from model
|
||||||
const char* mjs_getError(mjSpec* s) {
|
const char* mjs_getError(mjSpec* s) {
|
||||||
mjCModel* modelC = static_cast<mjCModel*>(s->element);
|
mjCModel* modelC = static_cast<mjCModel*>(s->element);
|
||||||
@@ -99,6 +110,17 @@ const char* mjs_getError(mjSpec* s) {
|
|||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
// Detach body from mjSpec, return 0 if success.
|
||||||
|
int mjs_detachBody(mjSpec* s, mjsBody* b) {
|
||||||
|
mjCModel* model = static_cast<mjCModel*>(s->element);
|
||||||
|
mjCBody* body = static_cast<mjCBody*>(b->element);
|
||||||
|
*model -= *body;
|
||||||
|
mjs_deleteBody(b);
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
// check if model has warnings
|
// check if model has warnings
|
||||||
int mjs_isWarning(mjSpec* s) {
|
int mjs_isWarning(mjSpec* s) {
|
||||||
mjCModel* modelC = static_cast<mjCModel*>(s->element);
|
mjCModel* modelC = static_cast<mjCModel*>(s->element);
|
||||||
@@ -115,6 +137,14 @@ void mjs_deleteSpec(mjSpec* s) {
|
|||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
// delete body
|
||||||
|
void mjs_deleteBody(mjsBody* b) {
|
||||||
|
mjCBody* body = static_cast<mjCBody*>(b->element);
|
||||||
|
delete body;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
// add child body to body, return child spec
|
// add child body to body, return child spec
|
||||||
mjsBody* mjs_addBody(mjsBody* bodyspec, mjsDefault* defspec) {
|
mjsBody* mjs_addBody(mjsBody* bodyspec, mjsDefault* defspec) {
|
||||||
mjCDef* def = defspec ? static_cast<mjCDef*>(defspec->element) : 0;
|
mjCDef* def = defspec ? static_cast<mjCDef*>(defspec->element) : 0;
|
||||||
@@ -650,11 +680,12 @@ void mjs_setActivePlugins(mjSpec* s, void* activeplugins) {
|
|||||||
|
|
||||||
|
|
||||||
// compute full inertia
|
// compute full inertia
|
||||||
const char* mjs_setFullInertia(mjsBody* bodyspec, double quat[4], double inertia[3]) {
|
const char* mjs_fullInertia(double quat[4], double inertia[3], const double fullinertia[6]) {
|
||||||
mjCBody* body = static_cast<mjCBody*>(bodyspec->element);
|
return FullInertia(quat, inertia, fullinertia);
|
||||||
return body->FullInertia(quat, inertia);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
// -------------------------- GLOBAL ASSET CACHE -------------------------------
|
// -------------------------- GLOBAL ASSET CACHE -------------------------------
|
||||||
|
|
||||||
void mj_setCacheSize(mjCache cache, std::size_t size) {
|
void mj_setCacheSize(mjCache cache, std::size_t size) {
|
||||||
|
|||||||
+190
-139
@@ -15,6 +15,7 @@
|
|||||||
#ifndef MUJOCO_SRC_USER_USER_API_H_
|
#ifndef MUJOCO_SRC_USER_USER_API_H_
|
||||||
#define MUJOCO_SRC_USER_USER_API_H_
|
#define MUJOCO_SRC_USER_USER_API_H_
|
||||||
|
|
||||||
|
#include <math.h>
|
||||||
#include <stddef.h>
|
#include <stddef.h>
|
||||||
#include <mujoco/mjexport.h>
|
#include <mujoco/mjexport.h>
|
||||||
#include <mujoco/mjmodel.h>
|
#include <mujoco/mjmodel.h>
|
||||||
@@ -26,6 +27,8 @@
|
|||||||
extern "C" {
|
extern "C" {
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
#define mjNAN NAN // used to mark undefined fields
|
||||||
|
|
||||||
|
|
||||||
//---------------------------------- handles to internal objects -----------------------------------
|
//---------------------------------- handles to internal objects -----------------------------------
|
||||||
|
|
||||||
@@ -69,13 +72,22 @@ typedef enum _mjtLimited { // type of limit specification
|
|||||||
} mjtLimited;
|
} mjtLimited;
|
||||||
|
|
||||||
|
|
||||||
typedef enum _mjtInertiaFromGeom {
|
typedef enum _mjtInertiaFromGeom { // whether to infer body inertias from child geoms
|
||||||
mjINERTIAFROMGEOM_FALSE = 0, // do not use; inertial element required
|
mjINERTIAFROMGEOM_FALSE = 0, // do not use; inertial element required
|
||||||
mjINERTIAFROMGEOM_TRUE, // always use; overwrite inertial element
|
mjINERTIAFROMGEOM_TRUE, // always use; overwrite inertial element
|
||||||
mjINERTIAFROMGEOM_AUTO // use only if inertial element is missing
|
mjINERTIAFROMGEOM_AUTO // use only if inertial element is missing
|
||||||
} mjtInertiaFromGeom;
|
} mjtInertiaFromGeom;
|
||||||
|
|
||||||
|
|
||||||
|
typedef enum _mjtOrientation { // type of orientation specifier
|
||||||
|
mjORIENTATION_QUAT = 0, // quaternion
|
||||||
|
mjORIENTATION_AXISANGLE, // axis and angle
|
||||||
|
mjORIENTATION_XYAXES, // x and y axes
|
||||||
|
mjORIENTATION_ZAXIS, // z axis (minimal rotation)
|
||||||
|
mjORIENTATION_EULER, // Euler angles
|
||||||
|
} mjtOrientation;
|
||||||
|
|
||||||
|
|
||||||
//---------------------------------- attribute structs (mjs) ---------------------------------------
|
//---------------------------------- attribute structs (mjs) ---------------------------------------
|
||||||
|
|
||||||
typedef struct _mjElement { // element type, do not modify
|
typedef struct _mjElement { // element type, do not modify
|
||||||
@@ -84,8 +96,8 @@ typedef struct _mjElement { // element type, do not modify
|
|||||||
|
|
||||||
|
|
||||||
typedef struct _mjSpec { // model specification
|
typedef struct _mjSpec { // model specification
|
||||||
mjElement* element; // object type
|
mjElement* element; // element type
|
||||||
mjStatistic stat; // statistics override (if defined)
|
mjString modelname; // model name
|
||||||
|
|
||||||
// compiler settings
|
// compiler settings
|
||||||
mjtByte autolimits; // infer "limited" attribute based on range
|
mjtByte autolimits; // infer "limited" attribute based on range
|
||||||
@@ -109,15 +121,14 @@ typedef struct _mjSpec { // model specification
|
|||||||
mjLROpt LRopt; // options for lengthrange computation
|
mjLROpt LRopt; // options for lengthrange computation
|
||||||
|
|
||||||
// engine data
|
// engine data
|
||||||
mjString modelname; // model name
|
mjOption option; // physics options
|
||||||
mjOption option; // options
|
|
||||||
mjVisual visual; // visual options
|
mjVisual visual; // visual options
|
||||||
size_t memory; // size of arena+stack memory in bytes
|
mjStatistic stat; // statistics override (if defined)
|
||||||
|
|
||||||
|
// sizes
|
||||||
|
size_t memory; // number of bytes in arena+stack memory
|
||||||
int nemax; // max number of equality constraints
|
int nemax; // max number of equality constraints
|
||||||
int njmax; // max number of constraints (Jacobian rows)
|
int nuserdata; // number of mjtNums in userdata
|
||||||
int nconmax; // max number of detected contacts (mjContact array size)
|
|
||||||
size_t nstack; // (deprecated) number of fields in mjData stack
|
|
||||||
int nuserdata; // number extra fields in mjData
|
|
||||||
int nuser_body; // number of mjtNums in body_user
|
int nuser_body; // number of mjtNums in body_user
|
||||||
int nuser_jnt; // number of mjtNums in jnt_user
|
int nuser_jnt; // number of mjtNums in jnt_user
|
||||||
int nuser_geom; // number of mjtNums in geom_user
|
int nuser_geom; // number of mjtNums in geom_user
|
||||||
@@ -126,29 +137,31 @@ typedef struct _mjSpec { // model specification
|
|||||||
int nuser_tendon; // number of mjtNums in tendon_user
|
int nuser_tendon; // number of mjtNums in tendon_user
|
||||||
int nuser_actuator; // number of mjtNums in actuator_user
|
int nuser_actuator; // number of mjtNums in actuator_user
|
||||||
int nuser_sensor; // number of mjtNums in sensor_user
|
int nuser_sensor; // number of mjtNums in sensor_user
|
||||||
|
|
||||||
// sizes
|
|
||||||
int nkey; // number of keyframes
|
int nkey; // number of keyframes
|
||||||
|
int njmax; // (deprecated) max number of constraints
|
||||||
|
int nconmax; // (deprecated) max number of detected contacts
|
||||||
|
size_t nstack; // (deprecated) number of mjtNums in mjData stack
|
||||||
|
|
||||||
// global data
|
// global data
|
||||||
mjString comment; // comment at top of XML
|
mjString comment; // comment at top of XML
|
||||||
mjString modelfiledir; // path to model file
|
mjString modelfiledir; // path to model file
|
||||||
|
|
||||||
// other
|
// other
|
||||||
bool hasImplicitPluginElem; // already encountered an implicit plugin sensor/actuator
|
mjtByte hasImplicitPluginElem; // already encountered an implicit plugin sensor/actuator
|
||||||
} mjSpec;
|
} mjSpec;
|
||||||
|
|
||||||
|
|
||||||
typedef struct _mjsOrientation { // alternative orientation specifiers
|
typedef struct _mjsOrientation { // alternative orientation specifiers
|
||||||
double axisangle[4]; // rotation axis and angle
|
mjtOrientation type; // active orientation specifier
|
||||||
|
double axisangle[4]; // axis and angle
|
||||||
double xyaxes[6]; // x and y axes
|
double xyaxes[6]; // x and y axes
|
||||||
double zaxis[3]; // z axis (use minimal rotation)
|
double zaxis[3]; // z axis (minimal rotation)
|
||||||
double euler[3]; // euler angles
|
double euler[3]; // Euler angles
|
||||||
} mjsOrientation;
|
} mjsOrientation;
|
||||||
|
|
||||||
|
|
||||||
typedef struct _mjsPlugin { // plugin specification
|
typedef struct _mjsPlugin { // plugin specification
|
||||||
mjElement* instance; // object type
|
mjElement* instance; // element type
|
||||||
mjString name; // name
|
mjString name; // name
|
||||||
mjString instance_name; // instance name
|
mjString instance_name; // instance name
|
||||||
int plugin_slot; // global registered slot number of the plugin
|
int plugin_slot; // global registered slot number of the plugin
|
||||||
@@ -158,7 +171,7 @@ typedef struct _mjsPlugin { // plugin specification
|
|||||||
|
|
||||||
|
|
||||||
typedef struct _mjsBody { // body specification
|
typedef struct _mjsBody { // body specification
|
||||||
mjElement* element; // object type
|
mjElement* element; // element type
|
||||||
mjString name; // name
|
mjString name; // name
|
||||||
mjString childclass; // childclass name
|
mjString childclass; // childclass name
|
||||||
|
|
||||||
@@ -186,7 +199,7 @@ typedef struct _mjsBody { // body specification
|
|||||||
|
|
||||||
|
|
||||||
typedef struct _mjsFrame { // frame specification
|
typedef struct _mjsFrame { // frame specification
|
||||||
mjElement* element; // object type
|
mjElement* element; // element type
|
||||||
mjString name; // name
|
mjString name; // name
|
||||||
mjString childclass; // childclass name
|
mjString childclass; // childclass name
|
||||||
double pos[3]; // position
|
double pos[3]; // position
|
||||||
@@ -197,7 +210,7 @@ typedef struct _mjsFrame { // frame specification
|
|||||||
|
|
||||||
|
|
||||||
typedef struct _mjsJoint { // joint specification
|
typedef struct _mjsJoint { // joint specification
|
||||||
mjElement* element; // object type
|
mjElement* element; // element type
|
||||||
mjString name; // name
|
mjString name; // name
|
||||||
mjString classname; // class name
|
mjString classname; // class name
|
||||||
mjtJoint type; // joint type
|
mjtJoint type; // joint type
|
||||||
@@ -238,7 +251,7 @@ typedef struct _mjsJoint { // joint specification
|
|||||||
|
|
||||||
|
|
||||||
typedef struct _mjsGeom { // geom specification
|
typedef struct _mjsGeom { // geom specification
|
||||||
mjElement* element; // object type
|
mjElement* element; // element type
|
||||||
mjString name; // name
|
mjString name; // name
|
||||||
mjString classname; // classname
|
mjString classname; // classname
|
||||||
mjtGeom type; // geom type
|
mjtGeom type; // geom type
|
||||||
@@ -287,7 +300,7 @@ typedef struct _mjsGeom { // geom specification
|
|||||||
|
|
||||||
|
|
||||||
typedef struct _mjsSite { // site specification
|
typedef struct _mjsSite { // site specification
|
||||||
mjElement* element; // object type
|
mjElement* element; // element type
|
||||||
mjString name; // name
|
mjString name; // name
|
||||||
mjString classname; // class name
|
mjString classname; // class name
|
||||||
|
|
||||||
@@ -311,7 +324,7 @@ typedef struct _mjsSite { // site specification
|
|||||||
|
|
||||||
|
|
||||||
typedef struct _mjsCamera { // camera specification
|
typedef struct _mjsCamera { // camera specification
|
||||||
mjElement* element; // object type
|
mjElement* element; // element type
|
||||||
mjString name; // name
|
mjString name; // name
|
||||||
mjString classname; // class name
|
mjString classname; // class name
|
||||||
|
|
||||||
@@ -340,7 +353,7 @@ typedef struct _mjsCamera { // camera specification
|
|||||||
|
|
||||||
|
|
||||||
typedef struct _mjsLight { // light specification
|
typedef struct _mjsLight { // light specification
|
||||||
mjElement* element; // object type
|
mjElement* element; // element type
|
||||||
mjString name; // name
|
mjString name; // name
|
||||||
mjString classname; // class name
|
mjString classname; // class name
|
||||||
|
|
||||||
@@ -354,6 +367,7 @@ typedef struct _mjsLight { // light specification
|
|||||||
mjtByte active; // is light active
|
mjtByte active; // is light active
|
||||||
mjtByte directional; // is light directional or spot
|
mjtByte directional; // is light directional or spot
|
||||||
mjtByte castshadow; // does light cast shadows
|
mjtByte castshadow; // does light cast shadows
|
||||||
|
double bulbradius; // bulb radius, for soft shadows
|
||||||
float attenuation[3]; // OpenGL attenuation (quadratic model)
|
float attenuation[3]; // OpenGL attenuation (quadratic model)
|
||||||
float cutoff; // OpenGL cutoff
|
float cutoff; // OpenGL cutoff
|
||||||
float exponent; // OpenGL exponent
|
float exponent; // OpenGL exponent
|
||||||
@@ -367,7 +381,7 @@ typedef struct _mjsLight { // light specification
|
|||||||
|
|
||||||
|
|
||||||
typedef struct _mjsFlex {
|
typedef struct _mjsFlex {
|
||||||
mjElement* element; // object type
|
mjElement* element; // element type
|
||||||
mjString name; // name
|
mjString name; // name
|
||||||
mjString classname; // class name
|
mjString classname; // class name
|
||||||
|
|
||||||
@@ -408,7 +422,7 @@ typedef struct _mjsFlex {
|
|||||||
|
|
||||||
|
|
||||||
typedef struct _mjsMesh { // mesh specification
|
typedef struct _mjsMesh { // mesh specification
|
||||||
mjElement* element; // object type
|
mjElement* element; // element type
|
||||||
mjString name; // name
|
mjString name; // name
|
||||||
mjString classname; // class name
|
mjString classname; // class name
|
||||||
mjString content_type; // content type of file
|
mjString content_type; // content type of file
|
||||||
@@ -429,7 +443,7 @@ typedef struct _mjsMesh { // mesh specification
|
|||||||
|
|
||||||
|
|
||||||
typedef struct _mjsHField { // height field specification
|
typedef struct _mjsHField { // height field specification
|
||||||
mjElement* element; // object type
|
mjElement* element; // element type
|
||||||
mjString name; // name
|
mjString name; // name
|
||||||
mjString content_type; // content type of file
|
mjString content_type; // content type of file
|
||||||
mjString file; // file: (nrow, ncol, [elevation data])
|
mjString file; // file: (nrow, ncol, [elevation data])
|
||||||
@@ -443,7 +457,7 @@ typedef struct _mjsHField { // height field specification
|
|||||||
|
|
||||||
|
|
||||||
typedef struct _mjsSkin { // skin specification
|
typedef struct _mjsSkin { // skin specification
|
||||||
mjElement* element; // object type
|
mjElement* element; // element type
|
||||||
mjString name; // name
|
mjString name; // name
|
||||||
mjString classname; // class name
|
mjString classname; // class name
|
||||||
mjString file; // skin file
|
mjString file; // skin file
|
||||||
@@ -470,7 +484,7 @@ typedef struct _mjsSkin { // skin specification
|
|||||||
|
|
||||||
|
|
||||||
typedef struct _mjsTexture { // texture specification
|
typedef struct _mjsTexture { // texture specification
|
||||||
mjElement* element; // object type
|
mjElement* element; // element type
|
||||||
mjString name; // name
|
mjString name; // name
|
||||||
mjString classname; // class name
|
mjString classname; // class name
|
||||||
mjtTexture type; // texture type
|
mjtTexture type; // texture type
|
||||||
@@ -504,7 +518,7 @@ typedef struct _mjsTexture { // texture specification
|
|||||||
|
|
||||||
|
|
||||||
typedef struct _mjsMaterial { // material specification
|
typedef struct _mjsMaterial { // material specification
|
||||||
mjElement* element; // object type
|
mjElement* element; // element type
|
||||||
mjString name; // name
|
mjString name; // name
|
||||||
mjString classname; // class name
|
mjString classname; // class name
|
||||||
mjString texture; // name of texture (empty: none)
|
mjString texture; // name of texture (empty: none)
|
||||||
@@ -514,13 +528,15 @@ typedef struct _mjsMaterial { // material specification
|
|||||||
float specular; // specular
|
float specular; // specular
|
||||||
float shininess; // shininess
|
float shininess; // shininess
|
||||||
float reflectance; // reflectance
|
float reflectance; // reflectance
|
||||||
|
float metallic; // metallic
|
||||||
|
float roughness; // roughness
|
||||||
float rgba[4]; // rgba
|
float rgba[4]; // rgba
|
||||||
mjString info; // message appended to compiler errors
|
mjString info; // message appended to compiler errors
|
||||||
} mjsMaterial;
|
} mjsMaterial;
|
||||||
|
|
||||||
|
|
||||||
typedef struct _mjsPair {
|
typedef struct _mjsPair {
|
||||||
mjElement* element; // object type
|
mjElement* element; // element type
|
||||||
mjString name; // name
|
mjString name; // name
|
||||||
mjString classname; // class name
|
mjString classname; // class name
|
||||||
mjString geomname1; // name of geom 1
|
mjString geomname1; // name of geom 1
|
||||||
@@ -539,7 +555,7 @@ typedef struct _mjsPair {
|
|||||||
|
|
||||||
|
|
||||||
typedef struct _mjsExclude {
|
typedef struct _mjsExclude {
|
||||||
mjElement* element; // object type
|
mjElement* element; // element type
|
||||||
mjString name; // name
|
mjString name; // name
|
||||||
mjString bodyname1; // name of geom 1
|
mjString bodyname1; // name of geom 1
|
||||||
mjString bodyname2; // name of geom 2
|
mjString bodyname2; // name of geom 2
|
||||||
@@ -548,7 +564,7 @@ typedef struct _mjsExclude {
|
|||||||
|
|
||||||
|
|
||||||
typedef struct _mjsEquality { // equality specification
|
typedef struct _mjsEquality { // equality specification
|
||||||
mjElement* element; // object type
|
mjElement* element; // element type
|
||||||
mjString name; // name
|
mjString name; // name
|
||||||
mjString classname; // class name
|
mjString classname; // class name
|
||||||
mjtEq type; // constraint type
|
mjtEq type; // constraint type
|
||||||
@@ -563,7 +579,7 @@ typedef struct _mjsEquality { // equality specification
|
|||||||
|
|
||||||
|
|
||||||
typedef struct _mjsTendon { // tendon specification
|
typedef struct _mjsTendon { // tendon specification
|
||||||
mjElement* element; // object type
|
mjElement* element; // element type
|
||||||
mjString name; // name
|
mjString name; // name
|
||||||
mjString classname; // class name
|
mjString classname; // class name
|
||||||
|
|
||||||
@@ -595,13 +611,13 @@ typedef struct _mjsTendon { // tendon specification
|
|||||||
|
|
||||||
|
|
||||||
typedef struct _mjsWrap { // wrapping object specification
|
typedef struct _mjsWrap { // wrapping object specification
|
||||||
mjElement* element; // object type
|
mjElement* element; // element type
|
||||||
mjString info; // message appended to errors
|
mjString info; // message appended to errors
|
||||||
} mjsWrap;
|
} mjsWrap;
|
||||||
|
|
||||||
|
|
||||||
typedef struct _mjsActuator { // actuator specification
|
typedef struct _mjsActuator { // actuator specification
|
||||||
mjElement* element; // object type
|
mjElement* element; // element type
|
||||||
mjString name; // name
|
mjString name; // name
|
||||||
mjString classname; // class name
|
mjString classname; // class name
|
||||||
|
|
||||||
@@ -645,7 +661,7 @@ typedef struct _mjsActuator { // actuator specification
|
|||||||
|
|
||||||
|
|
||||||
typedef struct _mjsSensor { // sensor specification
|
typedef struct _mjsSensor { // sensor specification
|
||||||
mjElement* element; // object type
|
mjElement* element; // element type
|
||||||
mjString name; // name
|
mjString name; // name
|
||||||
mjString classname; // class name
|
mjString classname; // class name
|
||||||
|
|
||||||
@@ -673,7 +689,7 @@ typedef struct _mjsSensor { // sensor specification
|
|||||||
|
|
||||||
|
|
||||||
typedef struct _mjsNumeric { // custom numeric field specification
|
typedef struct _mjsNumeric { // custom numeric field specification
|
||||||
mjElement* element; // object type
|
mjElement* element; // element type
|
||||||
mjString name; // name
|
mjString name; // name
|
||||||
mjDoubleVec data; // initialization data
|
mjDoubleVec data; // initialization data
|
||||||
int size; // array size, can be bigger than data size
|
int size; // array size, can be bigger than data size
|
||||||
@@ -682,7 +698,7 @@ typedef struct _mjsNumeric { // custom numeric field specification
|
|||||||
|
|
||||||
|
|
||||||
typedef struct _mjsText { // custom text specification
|
typedef struct _mjsText { // custom text specification
|
||||||
mjElement* element; // object type
|
mjElement* element; // element type
|
||||||
mjString name; // name
|
mjString name; // name
|
||||||
mjString data; // text string
|
mjString data; // text string
|
||||||
mjString info; // message appended to compiler errors
|
mjString info; // message appended to compiler errors
|
||||||
@@ -690,7 +706,7 @@ typedef struct _mjsText { // custom text specification
|
|||||||
|
|
||||||
|
|
||||||
typedef struct _mjsTuple { // tuple specification
|
typedef struct _mjsTuple { // tuple specification
|
||||||
mjElement* element; // object type
|
mjElement* element; // element type
|
||||||
mjString name; // name
|
mjString name; // name
|
||||||
mjIntVec objtype; // object types
|
mjIntVec objtype; // object types
|
||||||
mjStringVec objname; // object names
|
mjStringVec objname; // object names
|
||||||
@@ -700,7 +716,7 @@ typedef struct _mjsTuple { // tuple specification
|
|||||||
|
|
||||||
|
|
||||||
typedef struct _mjsKey { // keyframe specification
|
typedef struct _mjsKey { // keyframe specification
|
||||||
mjElement* element; // object type
|
mjElement* element; // element type
|
||||||
mjString name; // name
|
mjString name; // name
|
||||||
double time; // time
|
double time; // time
|
||||||
mjDoubleVec qpos; // qpos
|
mjDoubleVec qpos; // qpos
|
||||||
@@ -714,8 +730,8 @@ typedef struct _mjsKey { // keyframe specification
|
|||||||
|
|
||||||
|
|
||||||
typedef struct _mjsDefault { // default specification
|
typedef struct _mjsDefault { // default specification
|
||||||
mjString name; // name
|
mjElement* element; // element type
|
||||||
mjElement* element; // object type
|
mjString name; // class name
|
||||||
mjsJoint* joint; // joint defaults
|
mjsJoint* joint; // joint defaults
|
||||||
mjsGeom* geom; // geom defaults
|
mjsGeom* geom; // geom defaults
|
||||||
mjsSite* site; // site defaults
|
mjsSite* site; // site defaults
|
||||||
@@ -731,34 +747,47 @@ typedef struct _mjsDefault { // default specification
|
|||||||
} mjsDefault;
|
} mjsDefault;
|
||||||
|
|
||||||
|
|
||||||
//---------------------------------- API functions -------------------------------------------------
|
//---------------------------------- Top-level spec manipulation -----------------------------------
|
||||||
|
|
||||||
// Create model.
|
// Create spec.
|
||||||
MJAPI mjSpec* mjs_createSpec();
|
MJAPI mjSpec* mjs_createSpec(void);
|
||||||
|
|
||||||
// Copy model.
|
// Compile spec to model.
|
||||||
MJAPI mjSpec* mjs_copySpec(const mjSpec* s);
|
|
||||||
|
|
||||||
// Copy back model.
|
|
||||||
MJAPI void mjs_copyBack(mjSpec* s, const mjModel* m);
|
|
||||||
|
|
||||||
// Compile model.
|
|
||||||
MJAPI mjModel* mjs_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
|
// Copy spec.
|
||||||
|
MJAPI mjSpec* mjs_copySpec(const mjSpec* s);
|
||||||
|
|
||||||
|
// Get compiler error message from spec.
|
||||||
|
MJAPI const char* mjs_getError(mjSpec* s);
|
||||||
|
|
||||||
|
// Return 1 if compiler error is a warning.
|
||||||
|
MJAPI int mjs_isWarning(mjSpec* s);
|
||||||
|
|
||||||
|
// Copy model fields back into spec.
|
||||||
|
MJAPI void mjs_copyBack(mjSpec* s, const mjModel* m);
|
||||||
|
|
||||||
|
// Delete spec.
|
||||||
|
MJAPI void mjs_deleteSpec(mjSpec* s);
|
||||||
|
|
||||||
|
|
||||||
|
//---------------------------------- Attachment ----------------------------------------------------
|
||||||
|
|
||||||
|
// Attach child body to a parent frame, return 0 on success.
|
||||||
MJAPI int mjs_attachBody(mjsFrame* parent, const mjsBody* child,
|
MJAPI int mjs_attachBody(mjsFrame* parent, const mjsBody* child,
|
||||||
const char* prefix, const char* suffix);
|
const char* prefix, const char* suffix);
|
||||||
|
|
||||||
// Get error message from model.
|
// Attach child frame to a parent body, return 0 on success.
|
||||||
MJAPI const char* mjs_getError(mjSpec* s);
|
MJAPI int mjs_attachFrame(mjsBody* parent, const mjsFrame* child,
|
||||||
|
const char* prefix, const char* suffix);
|
||||||
|
|
||||||
// Return 1 if model has warnings.
|
// Detach body from mjSpec, remove all references and delete the body, return 0 on success.
|
||||||
MJAPI int mjs_isWarning(mjSpec* s);
|
MJAPI int mjs_detachBody(mjSpec* s, mjsBody* b);
|
||||||
|
|
||||||
// Delete model.
|
|
||||||
MJAPI void mjs_deleteSpec(mjSpec* s);
|
|
||||||
|
|
||||||
// Add child body to body, return child spec.
|
//---------------------------------- Add tree elements ---------------------------------------------
|
||||||
|
|
||||||
|
// Add child body to body, return child.
|
||||||
MJAPI mjsBody* mjs_addBody(mjsBody* body, mjsDefault* def);
|
MJAPI mjsBody* mjs_addBody(mjsBody* body, mjsDefault* def);
|
||||||
|
|
||||||
// Add site to body, return site spec.
|
// Add site to body, return site spec.
|
||||||
@@ -782,34 +811,31 @@ MJAPI mjsLight* mjs_addLight(mjsBody* body, mjsDefault* def);
|
|||||||
// Add frame to body.
|
// Add frame to body.
|
||||||
MJAPI mjsFrame* mjs_addFrame(mjsBody* body, mjsFrame* parentframe);
|
MJAPI mjsFrame* mjs_addFrame(mjsBody* body, mjsFrame* parentframe);
|
||||||
|
|
||||||
// Add flex to model.
|
// Delete body. TODO: make this a general mjs_deleteElement function
|
||||||
|
MJAPI void mjs_deleteBody(mjsBody* b);
|
||||||
|
|
||||||
|
|
||||||
|
//---------------------------------- Add non-tree elements -----------------------------------------
|
||||||
|
|
||||||
|
// Add actuator.
|
||||||
|
MJAPI mjsActuator* mjs_addActuator(mjSpec* s, mjsDefault* def);
|
||||||
|
|
||||||
|
// Add sensor.
|
||||||
|
MJAPI mjsSensor* mjs_addSensor(mjSpec* s);
|
||||||
|
|
||||||
|
// Add flex.
|
||||||
MJAPI mjsFlex* mjs_addFlex(mjSpec* s);
|
MJAPI mjsFlex* mjs_addFlex(mjSpec* s);
|
||||||
|
|
||||||
// Add mesh to model.
|
// Add contact pair.
|
||||||
MJAPI mjsMesh* mjs_addMesh(mjSpec* s, mjsDefault* def);
|
|
||||||
|
|
||||||
// Add height field to model.
|
|
||||||
MJAPI mjsHField* mjs_addHField(mjSpec* s);
|
|
||||||
|
|
||||||
// Add skin to model.
|
|
||||||
MJAPI mjsSkin* mjs_addSkin(mjSpec* s);
|
|
||||||
|
|
||||||
// Add texture to model.
|
|
||||||
MJAPI mjsTexture* mjs_addTexture(mjSpec* s);
|
|
||||||
|
|
||||||
// Add material to model.
|
|
||||||
MJAPI mjsMaterial* mjs_addMaterial(mjSpec* s, mjsDefault* def);
|
|
||||||
|
|
||||||
// Add pair to model.
|
|
||||||
MJAPI mjsPair* mjs_addPair(mjSpec* s, mjsDefault* def);
|
MJAPI mjsPair* mjs_addPair(mjSpec* s, mjsDefault* def);
|
||||||
|
|
||||||
// Add excluded body pair to model.
|
// Add excluded body pair.
|
||||||
MJAPI mjsExclude* mjs_addExclude(mjSpec* s);
|
MJAPI mjsExclude* mjs_addExclude(mjSpec* s);
|
||||||
|
|
||||||
// Add equality to model.
|
// Add equality.
|
||||||
MJAPI mjsEquality* mjs_addEquality(mjSpec* s, mjsDefault* def);
|
MJAPI mjsEquality* mjs_addEquality(mjSpec* s, mjsDefault* def);
|
||||||
|
|
||||||
// Add tendon to model.
|
// Add tendon.
|
||||||
MJAPI mjsTendon* mjs_addTendon(mjSpec* s, mjsDefault* def);
|
MJAPI mjsTendon* mjs_addTendon(mjSpec* s, mjsDefault* def);
|
||||||
|
|
||||||
// Wrap site using tendon.
|
// Wrap site using tendon.
|
||||||
@@ -824,42 +850,48 @@ MJAPI mjsWrap* mjs_wrapJoint(mjsTendon* tendon, const char* name, double coef);
|
|||||||
// Wrap pulley using tendon.
|
// Wrap pulley using tendon.
|
||||||
MJAPI mjsWrap* mjs_wrapPulley(mjsTendon* tendon, double divisor);
|
MJAPI mjsWrap* mjs_wrapPulley(mjsTendon* tendon, double divisor);
|
||||||
|
|
||||||
// Add actuator to model.
|
// Add numeric.
|
||||||
MJAPI mjsActuator* mjs_addActuator(mjSpec* s, mjsDefault* def);
|
|
||||||
|
|
||||||
// Add sensor to model.
|
|
||||||
MJAPI mjsSensor* mjs_addSensor(mjSpec* s);
|
|
||||||
|
|
||||||
// Add numeric to model.
|
|
||||||
MJAPI mjsNumeric* mjs_addNumeric(mjSpec* s);
|
MJAPI mjsNumeric* mjs_addNumeric(mjSpec* s);
|
||||||
|
|
||||||
// Add text to model.
|
// Add text.
|
||||||
MJAPI mjsText* mjs_addText(mjSpec* s);
|
MJAPI mjsText* mjs_addText(mjSpec* s);
|
||||||
|
|
||||||
// Add tuple to model.
|
// Add tuple.
|
||||||
MJAPI mjsTuple* mjs_addTuple(mjSpec* s);
|
MJAPI mjsTuple* mjs_addTuple(mjSpec* s);
|
||||||
|
|
||||||
// Add keyframe to model.
|
// Add keyframe.
|
||||||
MJAPI mjsKey* mjs_addKey(mjSpec* s);
|
MJAPI mjsKey* mjs_addKey(mjSpec* s);
|
||||||
|
|
||||||
// Add plugin to model.
|
// Add plugin.
|
||||||
MJAPI mjsPlugin* mjs_addPlugin(mjSpec* s);
|
MJAPI mjsPlugin* mjs_addPlugin(mjSpec* s);
|
||||||
|
|
||||||
// Add default to model.
|
// Add default.
|
||||||
MJAPI mjsDefault* mjs_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.
|
|
||||||
|
//---------------------------------- Add assets ----------------------------------------------------
|
||||||
|
|
||||||
|
// Add mesh.
|
||||||
|
MJAPI mjsMesh* mjs_addMesh(mjSpec* s, mjsDefault* def);
|
||||||
|
|
||||||
|
// Add height field.
|
||||||
|
MJAPI mjsHField* mjs_addHField(mjSpec* s);
|
||||||
|
|
||||||
|
// Add skin.
|
||||||
|
MJAPI mjsSkin* mjs_addSkin(mjSpec* s);
|
||||||
|
|
||||||
|
// Add texture.
|
||||||
|
MJAPI mjsTexture* mjs_addTexture(mjSpec* s);
|
||||||
|
|
||||||
|
// Add material.
|
||||||
|
MJAPI mjsMaterial* mjs_addMaterial(mjSpec* s, mjsDefault* def);
|
||||||
|
|
||||||
|
|
||||||
|
//---------------------------------- Find/get utilities --------------------------------------------
|
||||||
|
|
||||||
|
// Get spec from body.
|
||||||
MJAPI mjSpec* mjs_getSpec(mjsBody* body);
|
MJAPI mjSpec* mjs_getSpec(mjsBody* body);
|
||||||
|
|
||||||
// Get default corresponding to an mjElement.
|
|
||||||
MJAPI mjsDefault* mjs_getDefault(mjElement* element);
|
|
||||||
|
|
||||||
// Find default in model by class name.
|
|
||||||
MJAPI mjsDefault* mjs_findDefault(mjSpec* s, const char* classname);
|
|
||||||
|
|
||||||
// Get global default from model.
|
|
||||||
MJAPI mjsDefault* mjs_getSpecDefault(mjSpec* s);
|
|
||||||
|
|
||||||
// Find body in model by name.
|
// Find body in model by name.
|
||||||
MJAPI mjsBody* mjs_findBody(mjSpec* s, const char* name);
|
MJAPI mjsBody* mjs_findBody(mjSpec* s, const char* name);
|
||||||
|
|
||||||
@@ -872,9 +904,21 @@ MJAPI mjsMesh* mjs_findMesh(mjSpec* s, const char* name);
|
|||||||
// Find frame by name.
|
// Find frame by name.
|
||||||
MJAPI mjsFrame* mjs_findFrame(mjSpec* s, const char* name);
|
MJAPI mjsFrame* mjs_findFrame(mjSpec* s, const char* name);
|
||||||
|
|
||||||
|
// Get default corresponding to an element.
|
||||||
|
MJAPI mjsDefault* mjs_getDefault(mjElement* element);
|
||||||
|
|
||||||
|
// Find default in model by class name.
|
||||||
|
MJAPI mjsDefault* mjs_findDefault(mjSpec* s, const char* classname);
|
||||||
|
|
||||||
|
// Get global default from model.
|
||||||
|
MJAPI mjsDefault* mjs_getSpecDefault(mjSpec* s);
|
||||||
|
|
||||||
// Get element id.
|
// Get element id.
|
||||||
MJAPI int mjs_getId(mjElement* element);
|
MJAPI int mjs_getId(mjElement* element);
|
||||||
|
|
||||||
|
|
||||||
|
//---------------------------------- Attribute setters ---------------------------------------------
|
||||||
|
|
||||||
// Copy text to string.
|
// Copy text to string.
|
||||||
MJAPI void mjs_setString(mjString dest, const char* text);
|
MJAPI void mjs_setString(mjString dest, const char* text);
|
||||||
|
|
||||||
@@ -902,110 +946,117 @@ MJAPI void mjs_appendFloatVec(mjFloatVecVec dest, const float* array, int size);
|
|||||||
// Copy double array to vector.
|
// Copy double array to vector.
|
||||||
MJAPI void mjs_setDouble(mjDoubleVec dest, const double* array, int size);
|
MJAPI void mjs_setDouble(mjDoubleVec dest, const double* array, int size);
|
||||||
|
|
||||||
|
// Set plugin attributes.
|
||||||
|
MJAPI void mjs_setPluginAttributes(mjsPlugin* plugin, void* attributes);
|
||||||
|
|
||||||
|
|
||||||
|
//---------------------------------- Attribute getters ---------------------------------------------
|
||||||
|
|
||||||
// Get string contents.
|
// Get string contents.
|
||||||
MJAPI const char* mjs_getString(mjString source);
|
MJAPI const char* mjs_getString(mjString source);
|
||||||
|
|
||||||
// Get double array contents and optionally its size.
|
// Get double array contents and optionally its size.
|
||||||
MJAPI const double* mjs_getDouble(mjDoubleVec source, int* size);
|
MJAPI const double* mjs_getDouble(mjDoubleVec source, int* size);
|
||||||
|
|
||||||
// Set plugin attributes.
|
|
||||||
MJAPI void mjs_setPluginAttributes(mjsPlugin* plugin, void* attributes);
|
//---------------------------------- Other utilities -----------------------------------------------
|
||||||
|
|
||||||
// Set active plugins.
|
// Set active plugins.
|
||||||
MJAPI void mjs_setActivePlugins(mjSpec* s, void* activeplugins);
|
MJAPI void mjs_setActivePlugins(mjSpec* s, void* activeplugins);
|
||||||
|
|
||||||
// Set default.
|
// Set element's default.
|
||||||
MJAPI void mjs_setDefault(mjElement* element, mjsDefault* def);
|
MJAPI void mjs_setDefault(mjElement* element, mjsDefault* def);
|
||||||
|
|
||||||
// Set frame.
|
// Set element's enlcosing frame.
|
||||||
MJAPI void mjs_setFrame(mjElement* dest, mjsFrame* frame);
|
MJAPI void mjs_setFrame(mjElement* dest, mjsFrame* frame);
|
||||||
|
|
||||||
// Resolve alternative orientations to quat.
|
// Resolve alternative orientations to quat, return error if any.
|
||||||
MJAPI const char* mjs_resolveOrientation(double quat[4], mjtByte degree, const char* sequence,
|
MJAPI const char* mjs_resolveOrientation(double quat[4], mjtByte degree, const char* sequence,
|
||||||
const mjsOrientation* orientation);
|
const mjsOrientation* orientation);
|
||||||
|
|
||||||
// Compute quat and inertia from body->fullinertia.
|
// Compute quat and diag inertia from full inertia matrix, return error if any.
|
||||||
MJAPI const char* mjs_setFullInertia(mjsBody* body, double quat[4], double inertia[3]);
|
MJAPI const char* mjs_fullInertia(double quat[4], double inertia[3], const double fullinertia[6]);
|
||||||
|
|
||||||
|
|
||||||
//---------------------------------- Initialization functions --------------------------------------
|
//---------------------------------- Initialization -----------------------------------------------
|
||||||
|
|
||||||
// Default model attributes.
|
// Default spec attributes.
|
||||||
MJAPI void mjs_defaultSpec(mjSpec& model);
|
MJAPI void mjs_defaultSpec(mjSpec* spec);
|
||||||
|
|
||||||
// Default orientation attributes.
|
// Default orientation attributes.
|
||||||
MJAPI void mjs_defaultOrientation(mjsOrientation& orient);
|
MJAPI void mjs_defaultOrientation(mjsOrientation* orient);
|
||||||
|
|
||||||
// Default body attributes.
|
// Default body attributes.
|
||||||
MJAPI void mjs_defaultBody(mjsBody& body);
|
MJAPI void mjs_defaultBody(mjsBody* body);
|
||||||
|
|
||||||
// Default frame attributes.
|
// Default frame attributes.
|
||||||
MJAPI void mjs_defaultFrame(mjsFrame& frame);
|
MJAPI void mjs_defaultFrame(mjsFrame* frame);
|
||||||
|
|
||||||
// Default joint attributes.
|
// Default joint attributes.
|
||||||
MJAPI void mjs_defaultJoint(mjsJoint& joint);
|
MJAPI void mjs_defaultJoint(mjsJoint* joint);
|
||||||
|
|
||||||
// Default geom attributes.
|
// Default geom attributes.
|
||||||
MJAPI void mjs_defaultGeom(mjsGeom& geom);
|
MJAPI void mjs_defaultGeom(mjsGeom* geom);
|
||||||
|
|
||||||
// Default site attributes.
|
// Default site attributes.
|
||||||
MJAPI void mjs_defaultSite(mjsSite& site);
|
MJAPI void mjs_defaultSite(mjsSite* site);
|
||||||
|
|
||||||
// Default camera attributes.
|
// Default camera attributes.
|
||||||
MJAPI void mjs_defaultCamera(mjsCamera& camera);
|
MJAPI void mjs_defaultCamera(mjsCamera* camera);
|
||||||
|
|
||||||
// Default light attributes.
|
// Default light attributes.
|
||||||
MJAPI void mjs_defaultLight(mjsLight& light);
|
MJAPI void mjs_defaultLight(mjsLight* light);
|
||||||
|
|
||||||
// Default flex attributes.
|
// Default flex attributes.
|
||||||
MJAPI void mjs_defaultFlex(mjsFlex& flex);
|
MJAPI void mjs_defaultFlex(mjsFlex* flex);
|
||||||
|
|
||||||
// Default mesh attributes.
|
// Default mesh attributes.
|
||||||
MJAPI void mjs_defaultMesh(mjsMesh& mesh);
|
MJAPI void mjs_defaultMesh(mjsMesh* mesh);
|
||||||
|
|
||||||
// Default height field attributes.
|
// Default height field attributes.
|
||||||
MJAPI void mjs_defaultHField(mjsHField& hfield);
|
MJAPI void mjs_defaultHField(mjsHField* hfield);
|
||||||
|
|
||||||
// Default skin attributes.
|
// Default skin attributes.
|
||||||
MJAPI void mjs_defaultSkin(mjsSkin& skin);
|
MJAPI void mjs_defaultSkin(mjsSkin* skin);
|
||||||
|
|
||||||
// Default texture attributes.
|
// Default texture attributes.
|
||||||
MJAPI void mjs_defaultTexture(mjsTexture& texture);
|
MJAPI void mjs_defaultTexture(mjsTexture* texture);
|
||||||
|
|
||||||
// Default material attributes.
|
// Default material attributes.
|
||||||
MJAPI void mjs_defaultMaterial(mjsMaterial& material);
|
MJAPI void mjs_defaultMaterial(mjsMaterial* material);
|
||||||
|
|
||||||
// Default pair attributes.
|
// Default pair attributes.
|
||||||
MJAPI void mjs_defaultPair(mjsPair& pair);
|
MJAPI void mjs_defaultPair(mjsPair* pair);
|
||||||
|
|
||||||
// Default equality attributes.
|
// Default equality attributes.
|
||||||
MJAPI void mjs_defaultEquality(mjsEquality& equality);
|
MJAPI void mjs_defaultEquality(mjsEquality* equality);
|
||||||
|
|
||||||
// Default tendon attributes.
|
// Default tendon attributes.
|
||||||
MJAPI void mjs_defaultTendon(mjsTendon& tendon);
|
MJAPI void mjs_defaultTendon(mjsTendon* tendon);
|
||||||
|
|
||||||
// Default actuator attributes.
|
// Default actuator attributes.
|
||||||
MJAPI void mjs_defaultActuator(mjsActuator& actuator);
|
MJAPI void mjs_defaultActuator(mjsActuator* actuator);
|
||||||
|
|
||||||
// Default sensor attributes.
|
// Default sensor attributes.
|
||||||
MJAPI void mjs_defaultSensor(mjsSensor& sensor);
|
MJAPI void mjs_defaultSensor(mjsSensor* sensor);
|
||||||
|
|
||||||
// Default numeric attributes.
|
// Default numeric attributes.
|
||||||
MJAPI void mjs_defaultNumeric(mjsNumeric& numeric);
|
MJAPI void mjs_defaultNumeric(mjsNumeric* numeric);
|
||||||
|
|
||||||
// Default text attributes.
|
// Default text attributes.
|
||||||
MJAPI void mjs_defaultText(mjsText& text);
|
MJAPI void mjs_defaultText(mjsText* text);
|
||||||
|
|
||||||
// Default tuple attributes.
|
// Default tuple attributes.
|
||||||
MJAPI void mjs_defaultTuple(mjsTuple& tuple);
|
MJAPI void mjs_defaultTuple(mjsTuple* tuple);
|
||||||
|
|
||||||
// Default keyframe attributes.
|
// Default keyframe attributes.
|
||||||
MJAPI void mjs_defaultKey(mjsKey& key);
|
MJAPI void mjs_defaultKey(mjsKey* key);
|
||||||
|
|
||||||
// Default plugin attributes.
|
// Default plugin attributes.
|
||||||
MJAPI void mjs_defaultPlugin(mjsPlugin& plugin);
|
MJAPI void mjs_defaultPlugin(mjsPlugin* plugin);
|
||||||
|
|
||||||
//------------------------- Cache functions ------------------------------------
|
|
||||||
|
//---------------------------------- Compiler cache ------------------------------------------------
|
||||||
|
|
||||||
typedef struct _mjCache* mjCache;
|
typedef struct _mjCache* mjCache;
|
||||||
|
|
||||||
@@ -1013,7 +1064,7 @@ typedef struct _mjCache* mjCache;
|
|||||||
MJAPI void mj_setCacheSize(mjCache cache, size_t size);
|
MJAPI void mj_setCacheSize(mjCache cache, size_t size);
|
||||||
|
|
||||||
// Get internal global cache context.
|
// Get internal global cache context.
|
||||||
MJAPI mjCache mj_globalCache();
|
MJAPI mjCache mj_globalCache(void);
|
||||||
|
|
||||||
#ifdef __cplusplus
|
#ifdef __cplusplus
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -50,6 +50,8 @@ template std::size_t mjCAsset::Add(const std::string& name,
|
|||||||
const uint8_t* data, std::size_t n);
|
const uint8_t* data, std::size_t n);
|
||||||
template std::size_t mjCAsset::Add(const std::string& name,
|
template std::size_t mjCAsset::Add(const std::string& name,
|
||||||
const int* data, std::size_t n);
|
const int* data, std::size_t n);
|
||||||
|
template std::size_t mjCAsset::Add(const std::string& name,
|
||||||
|
const unsigned int* data, std::size_t n);
|
||||||
template std::size_t mjCAsset::Add(const std::string& name,
|
template std::size_t mjCAsset::Add(const std::string& name,
|
||||||
const float* data, std::size_t n);
|
const float* data, std::size_t n);
|
||||||
template std::size_t mjCAsset::Add(const std::string& name,
|
template std::size_t mjCAsset::Add(const std::string& name,
|
||||||
@@ -67,6 +69,8 @@ template std::size_t mjCAsset::AddVector(const std::string& name,
|
|||||||
const std::vector<uint8_t>& v);
|
const std::vector<uint8_t>& v);
|
||||||
template std::size_t mjCAsset::AddVector(const std::string& name,
|
template std::size_t mjCAsset::AddVector(const std::string& name,
|
||||||
const std::vector<int>& v);
|
const std::vector<int>& v);
|
||||||
|
template std::size_t mjCAsset::AddVector(const std::string& name,
|
||||||
|
const std::vector<unsigned int>& v);
|
||||||
template std::size_t mjCAsset::AddVector(const std::string& name,
|
template std::size_t mjCAsset::AddVector(const std::string& name,
|
||||||
const std::vector<float>& v);
|
const std::vector<float>& v);
|
||||||
template std::size_t mjCAsset::AddVector(const std::string& name,
|
template std::size_t mjCAsset::AddVector(const std::string& name,
|
||||||
@@ -101,6 +105,8 @@ const uint8_t* mjCAsset::Get(const std::string& name, std::size_t* n) const;
|
|||||||
template
|
template
|
||||||
const int* mjCAsset::Get(const std::string& name, std::size_t* n) const;
|
const int* mjCAsset::Get(const std::string& name, std::size_t* n) const;
|
||||||
template
|
template
|
||||||
|
const unsigned int* mjCAsset::Get(const std::string& name, std::size_t* n) const;
|
||||||
|
template
|
||||||
const float* mjCAsset::Get(const std::string& name, std::size_t* n) const;
|
const float* mjCAsset::Get(const std::string& name, std::size_t* n) const;
|
||||||
template
|
template
|
||||||
const double* mjCAsset::Get(const std::string& name, std::size_t* n) const;
|
const double* mjCAsset::Get(const std::string& name, std::size_t* n) const;
|
||||||
@@ -122,6 +128,8 @@ template std::optional<std::vector<uint8_t>>
|
|||||||
mjCAsset::GetVector(const std::string& name) const;
|
mjCAsset::GetVector(const std::string& name) const;
|
||||||
template std::optional<std::vector<int>>
|
template std::optional<std::vector<int>>
|
||||||
mjCAsset::GetVector(const std::string& name) const;
|
mjCAsset::GetVector(const std::string& name) const;
|
||||||
|
template std::optional<std::vector<unsigned int>>
|
||||||
|
mjCAsset::GetVector(const std::string& name) const;
|
||||||
template std::optional<std::vector<float>>
|
template std::optional<std::vector<float>>
|
||||||
mjCAsset::GetVector(const std::string& name) const;
|
mjCAsset::GetVector(const std::string& name) const;
|
||||||
template std::optional<std::vector<double>>
|
template std::optional<std::vector<double>>
|
||||||
|
|||||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user