Merge branch 'google-deepmind:main' into main
This commit is contained in:
Abhishek Joshi
@@ -250,7 +250,7 @@ jobs:
|
||||
shell: bash
|
||||
run:
|
||||
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
|
||||
shell: bash
|
||||
env:
|
||||
|
||||
+1
-1
@@ -28,7 +28,7 @@ set(MSVC_INCREMENTAL_DEFAULT ON)
|
||||
|
||||
project(
|
||||
mujoco
|
||||
VERSION 3.1.4
|
||||
VERSION 3.1.5
|
||||
DESCRIPTION "MuJoCo Physics Simulator"
|
||||
HOMEPAGE_URL "https://mujoco.org"
|
||||
)
|
||||
|
||||
Vendored
+4
-4
@@ -1,6 +1,6 @@
|
||||
1 VERSIONINFO
|
||||
FILEVERSION 3,1,4,0
|
||||
PRODUCTVERSION 3,1,4,0
|
||||
FILEVERSION 3,1,5,0
|
||||
PRODUCTVERSION 3,1,5,0
|
||||
FILEOS 0x4
|
||||
FILETYPE 0x1
|
||||
{
|
||||
@@ -9,9 +9,9 @@ FILETYPE 0x1
|
||||
BLOCK "040904b0"
|
||||
{
|
||||
VALUE "ProductName", "MuJoCo"
|
||||
VALUE "ProductVersion", "3.1.4"
|
||||
VALUE "ProductVersion", "3.1.5"
|
||||
VALUE "FileDescription", "MuJoCo"
|
||||
VALUE "FileVersion", "3.1.4"
|
||||
VALUE "FileVersion", "3.1.5"
|
||||
VALUE "InternalName", "mujoco.dll"
|
||||
VALUE "OriginalFilename", "mujoco.dll"
|
||||
VALUE "CompanyName", "Google DeepMind"
|
||||
|
||||
Vendored
+4
-4
@@ -1,8 +1,8 @@
|
||||
MUJOCO ICON "mujoco.ico"
|
||||
|
||||
1 VERSIONINFO
|
||||
FILEVERSION 3,1,4,0
|
||||
PRODUCTVERSION 3,1,4,0
|
||||
FILEVERSION 3,1,5,0
|
||||
PRODUCTVERSION 3,1,5,0
|
||||
FILEOS 0x4
|
||||
FILETYPE 0x1
|
||||
{
|
||||
@@ -11,9 +11,9 @@ FILETYPE 0x1
|
||||
BLOCK "040904b0"
|
||||
{
|
||||
VALUE "ProductName", "MuJoCo"
|
||||
VALUE "ProductVersion", "3.1.4"
|
||||
VALUE "ProductVersion", "3.1.5"
|
||||
VALUE "FileDescription", "MuJoCo"
|
||||
VALUE "FileVersion", "3.1.4"
|
||||
VALUE "FileVersion", "3.1.5"
|
||||
VALUE "InternalName", "simulate.exe"
|
||||
VALUE "OriginalFilename", "simulate.exe"
|
||||
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.
|
||||
Defined in `mjui.h <https://github.com/google-deepmind/mujoco/blob/main/include/mujoco/mjui.h>`_.
|
||||
* - ``mjVERSION_HEADER``
|
||||
- 314
|
||||
- 315
|
||||
- The version of the MuJoCo headers; changes with every release. This is an integer equal to 100x the software
|
||||
version, so 210 corresponds to version 2.1. Defined in mujoco.h. The API function :ref:`mj_version` returns a
|
||||
number with the same meaning but for the compiled library.
|
||||
|
||||
@@ -2840,6 +2840,16 @@ mju_quatZ2Vec
|
||||
|
||||
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
|
||||
|
||||
+977
-876
File diff suppressed because it is too large
Load Diff
+125
-121
@@ -7,6 +7,42 @@
|
||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
|
||||
| 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 |
|
||||
| <compiler>` | | |
|
||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||
@@ -55,83 +91,6 @@
|
||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
|
||||
| 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>` | | |
|
||||
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
|
||||
| |_| 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:`reflectance<asset-material-reflectance>` | :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>` | | | | |
|
||||
| | | | :ref:`reflectance<asset-material-reflectance>` | :ref:`metallic<asset-material-metallic>` | :ref:`roughness<asset-material-roughness>` | :ref:`rgba<asset-material-rgba>` | |
|
||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
|
||||
| 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:`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:: |
|
||||
@@ -1287,6 +1210,83 @@
|
||||
| | | | :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:: |
|
||||
| :ref:`default | R | :class: mjcf-attributes |
|
||||
| <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:`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:: |
|
||||
@@ -1378,11 +1380,13 @@
|
||||
| :ref:`light | ? | :class: mjcf-attributes |
|
||||
| <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:: |
|
||||
|
||||
+75
-13
@@ -5,6 +5,64 @@ Changelog
|
||||
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
|
||||
^^^^^^^
|
||||
.. 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
|
||||
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
|
||||
non-reproducible results.
|
||||
- This feature was seen as overreach by the engine. Adding noise should be the user's responsibility.
|
||||
@@ -27,21 +85,25 @@ General
|
||||
model
|
||||
`refsite.xml <https://github.com/google-deepmind/mujoco/blob/main/test/engine/testdata/actuation/refsite.xml>`__,
|
||||
which demostrates Cartesian actuation of an arm, has been updated to use this attribute.
|
||||
3. Added support for gmsh format 2.2, as generated by e.g. `fTetwild <https://github.com/wildmeshing/fTetWild>`__.
|
||||
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
|
||||
^^^
|
||||
4. Improved performance of SAT for convex collisions.
|
||||
5. Fixed bug for sphere/capsule-convex deep penetration.
|
||||
6. Fixed bug where ``mjx.Data`` produced by ``mjx.put_data`` had different treedef than ``mjx.make_data``.
|
||||
7. Throw an error for margin/gap for convex mesh collisions, since they are not supported.
|
||||
5. Improved performance of SAT for convex collisions.
|
||||
6. Fixed bug for sphere/capsule-convex deep penetration.
|
||||
7. Fixed bug where ``mjx.Data`` produced by ``mjx.put_data`` had different treedef than ``mjx.make_data``.
|
||||
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
|
||||
^^^^^^^^
|
||||
8. Fixed bug in order of enable flag strings. Before this change, using the simulate UI to toggle the
|
||||
:ref:`invdiscrete<option-flag-invdiscrete>` or the (now removed) ``sensornoise`` flags would actually toggle the
|
||||
other flag.
|
||||
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
|
||||
other flag.
|
||||
|
||||
Python bindings
|
||||
^^^^^^^^^^^^^^^
|
||||
@@ -50,10 +112,10 @@ Python bindings
|
||||
:align: right
|
||||
:width: 240px
|
||||
|
||||
9. Added the ``mujoco.minimize`` Python module for nonlinear least-squares, designed for System Identification (sysID).
|
||||
The sysID tutorial is work in progress, but a pedagogical colab notebook with examples, including Inverse
|
||||
Kinematics, is available here: |ls_colab|
|
||||
|br| The video on the right shows example clips from the tutorial.
|
||||
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
|
||||
Kinematics, is available here: |ls_colab|
|
||||
|br| The video on the right shows example clips from the tutorial.
|
||||
|
||||
.. |ls_colab| image:: https://colab.research.google.com/assets/colab-badge.svg
|
||||
:target: https://colab.research.google.com/github/google-deepmind/mujoco/blob/main/python/least_squares.ipynb
|
||||
|
||||
@@ -22,8 +22,8 @@ been described elsewhere.
|
||||
|
||||
.. _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
|
||||
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>`__
|
||||
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.
|
||||
- :ref:`mj_forward` invokes only stages **2-22**, computing the continuous-time forward dynamics, ending with the
|
||||
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
|
||||
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.
|
||||
- :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
|
||||
is detected, the state is automatically reset and the corresponding warning is raised:
|
||||
: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:
|
||||
:ref:`mj_collision`
|
||||
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`
|
||||
11. Compute the tendon lengths and moment arms. This includes the computation of minimal-length paths for spatial
|
||||
10. Compute the tendon lengths and moment arms. This includes the computation of minimal-length paths for spatial
|
||||
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`,
|
||||
:ref:`mj_energyPos`
|
||||
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
|
||||
<https://en.wikipedia.org/wiki/Lyapunov_exponent>`__; this is a property of any rigid-body simulator (and indeed of
|
||||
`real-world physics <https://en.wikipedia.org/wiki/Roulette>`__) and is not MuJoCo-specific.
|
||||
- Exact reproducibillity is only guaranteed within a **single version**. Small numerical differences are quite common
|
||||
between versioned releases, for example due to code optimizations. This means that when saving an initial state and an
|
||||
open-loop control sequence, the resulting rolled-out trajectory will be identical within the same version but will
|
||||
likely be different between MuJoCo versions.
|
||||
- Exact reproducibility is only guaranteed within a **single version**, on the **same architecture**. Small numerical
|
||||
differences are quite common between versioned releases, for example due to code optimizations. This means that when
|
||||
saving an initial state and an open-loop control sequence, the resulting rolled-out trajectory will be identical
|
||||
within the same version, but will likely be different between MuJoCo versions or different operating systems.
|
||||
|
||||
.. _derivatives:
|
||||
|
||||
|
||||
Binary file not shown.
|
After Width: | Height: | Size: 59 KiB |
+24
-14
@@ -922,6 +922,7 @@ struct mjModel_ {
|
||||
int nD; // number of non-zeros in sparse dof-dof matrix
|
||||
int nB; // number of non-zeros in sparse body-dof matrix
|
||||
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 njmax; // number of available rows in constraint Jacobian
|
||||
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)
|
||||
mjtByte* light_directional; // directional light (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)
|
||||
mjtNum* light_pos; // position 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_facetexcoord; // texture face data (nmeshface x 3)
|
||||
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_quat; // rotation applied to asset vertices (nmesh x 4)
|
||||
int* mesh_pathadr; // address of asset path for mesh; -1: none (nmesh x 1)
|
||||
@@ -1194,20 +1197,20 @@ struct mjModel_ {
|
||||
int* skin_pathadr; // address of asset path for skin; -1: none (nskin x 1)
|
||||
|
||||
// height fields
|
||||
mjtNum* hfield_size; // (x, y, z_top, z_bottom) (nhfield x 4)
|
||||
int* hfield_nrow; // number of rows 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)
|
||||
float* hfield_data; // elevation data (nhfielddata x 1)
|
||||
int* hfield_pathadr; // address of asset path for hfield; -1: none (nhfield x 1)
|
||||
mjtNum* hfield_size; // (x, y, z_top, z_bottom) (nhfield x 4)
|
||||
int* hfield_nrow; // number of rows 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)
|
||||
float* hfield_data; // elevation data (nhfielddata x 1)
|
||||
int* hfield_pathadr; // address of hfield asset path; -1: none (nhfield x 1)
|
||||
|
||||
// textures
|
||||
int* tex_type; // texture type (mjtTexture) (ntex x 1)
|
||||
int* tex_height; // number of rows 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)
|
||||
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_type; // texture type (mjtTexture) (ntex x 1)
|
||||
int* tex_height; // number of rows 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)
|
||||
mjtByte* tex_rgb; // rgb (alpha = 1) (ntexdata x 1)
|
||||
int* tex_pathadr; // address of texture asset path; -1: none (ntex x 1)
|
||||
|
||||
// materials
|
||||
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_shininess; // shininess coef (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)
|
||||
|
||||
// predefined geom pairs for collision detection; has precedence over exclude
|
||||
@@ -2020,6 +2025,7 @@ struct mjvLight_ { // OpenGL light
|
||||
mjtByte headlight; // headlight
|
||||
mjtByte directional; // directional light
|
||||
mjtByte castshadow; // does light cast shadows
|
||||
float bulbradius; // bulb radius for soft shadows
|
||||
};
|
||||
typedef struct mjvLight_ mjvLight;
|
||||
struct mjvOption_ { // abstract visualization options
|
||||
@@ -2234,6 +2240,7 @@ struct mjvSceneState_ {
|
||||
|
||||
mjtByte* light_directional;
|
||||
mjtByte* light_castshadow;
|
||||
float* light_bulbradius;
|
||||
mjtByte* light_active;
|
||||
float* light_attenuation;
|
||||
float* light_cutoff;
|
||||
@@ -2301,6 +2308,8 @@ struct mjvSceneState_ {
|
||||
float* mat_specular;
|
||||
float* mat_shininess;
|
||||
float* mat_reflectance;
|
||||
float* mat_metallic;
|
||||
float* mat_roughness;
|
||||
float* mat_rgba;
|
||||
|
||||
int* eq_type;
|
||||
@@ -2445,8 +2454,8 @@ void mj_resetDataKeyframe(const mjModel* m, mjData* d, int key);
|
||||
void mj_markStack(mjData* d);
|
||||
void mj_freeStack(mjData* d);
|
||||
void* mj_stackAllocByte(mjData* d, size_t bytes, size_t alignment);
|
||||
mjtNum* mj_stackAllocNum(mjData* d, int size);
|
||||
int* mj_stackAllocInt(mjData* d, int size);
|
||||
mjtNum* mj_stackAllocNum(mjData* d, size_t size);
|
||||
int* mj_stackAllocInt(mjData* d, size_t size);
|
||||
void mj_deleteData(mjData* d);
|
||||
void mj_resetCallbacks(void);
|
||||
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_quatIntegrate(mjtNum quat[4], const mjtNum vel[3], mjtNum scale);
|
||||
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],
|
||||
const mjtNum pos1[3], const mjtNum quat1[4],
|
||||
const mjtNum pos2[3], const mjtNum quat2[4]);
|
||||
|
||||
+17
-12
@@ -66,25 +66,32 @@ directly from the top-level ``mjx`` module.
|
||||
Structs
|
||||
-------
|
||||
|
||||
Before running MJX functions on an accelerator device, structs must be copied onto the device via the ``mjx.device_put``
|
||||
function. Placing an :ref:`mjModel` on device yields an ``mjx.Model``. Placing an :ref:`mjData` on device yields
|
||||
Before running MJX functions on an accelerator device, structs must be copied onto the device via the ``mjx.put_model`` and ``mjx.put_data``
|
||||
functions. Placing an :ref:`mjModel` on device yields an ``mjx.Model``. Placing an :ref:`mjData` on device yields
|
||||
an ``mjx.Data``:
|
||||
|
||||
.. code-block:: python
|
||||
|
||||
model = mujoco.MjModel.from_xml_string("...")
|
||||
data = mujoco.MjData(model)
|
||||
mjx_model = mjx.device_put(model)
|
||||
mjx_data = mjx.device_put(data)
|
||||
mjx_model = mjx.put_model(model)
|
||||
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
|
||||
: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
|
||||
(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
|
||||
@@ -191,7 +198,7 @@ The following features are **fully supported** in MJX:
|
||||
* - :ref:`Geom <mjtGeom>`
|
||||
- ``PLANE``, ``SPHERE``, ``CAPSULE``, ``BOX``, ``MESH``
|
||||
* - :ref:`Constraint <mjtConstraint>`
|
||||
- ``EQUALITY``, ``LIMIT_JOINT``, ``CONTACT_PYRAMIDAL``
|
||||
- ``EQUALITY``, ``LIMIT_JOINT``, ``CONTACT_FRICTIONLESS``, ``CONTACT_PYRAMIDAL``
|
||||
* - :ref:`Equality <mjtEq>`
|
||||
- ``CONNECT``, ``WELD``, ``JOINT``
|
||||
* - :ref:`Integrator <mjtIntegrator>`
|
||||
@@ -199,7 +206,7 @@ The following features are **fully supported** in MJX:
|
||||
* - :ref:`Cone <mjtCone>`
|
||||
- ``PYRAMIDAL``
|
||||
* - :ref:`Condim <coContact>`
|
||||
- 3
|
||||
- 1, 3, 4, 6
|
||||
* - :ref:`Solver <mjtSolver>`
|
||||
- ``CG``, ``NEWTON``
|
||||
* - Fluid Model
|
||||
@@ -217,10 +224,8 @@ The following features are **in development** and coming soon:
|
||||
- Feature
|
||||
* - :ref:`Geom <mjtGeom>`
|
||||
- ``SDF``, ``HFIELD``, ``ELLIPSOID``, ``CYLINDER``
|
||||
* - :ref:`Condim <coContact>`
|
||||
- 1, 4, 6
|
||||
* - :ref:`Constraint <mjtConstraint>`
|
||||
- :ref:`Frictionloss <coFriction>`, ``CONTACT_FRICTIONLESS``, ``CONTACT_ELLIPTIC``, ``FRICTION_DOF``
|
||||
- :ref:`Frictionloss <coFriction>`, ``CONTACT_ELLIPTIC``, ``FRICTION_DOF``
|
||||
* - :ref:`Integrator <mjtIntegrator>`
|
||||
- ``IMPLICIT``, ``IMPLICITFAST``
|
||||
* - Dynamics
|
||||
|
||||
@@ -129,3 +129,17 @@ Drones
|
||||
- Preview
|
||||
* - `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
|
||||
* - `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
|
||||
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``.
|
||||
|
||||
Linux
|
||||
_____
|
||||
|
||||
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
|
||||
_______
|
||||
|
||||
+17
-12
@@ -637,6 +637,7 @@ struct mjModel_ {
|
||||
int nD; // number of non-zeros in sparse dof-dof matrix
|
||||
int nB; // number of non-zeros in sparse body-dof matrix
|
||||
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 njmax; // number of available rows in constraint Jacobian
|
||||
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)
|
||||
mjtByte* light_directional; // directional light (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)
|
||||
mjtNum* light_pos; // position 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_facetexcoord; // texture face data (nmeshface x 3)
|
||||
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_quat; // rotation applied to asset vertices (nmesh x 4)
|
||||
int* mesh_pathadr; // address of asset path for mesh; -1: none (nmesh x 1)
|
||||
@@ -909,20 +912,20 @@ struct mjModel_ {
|
||||
int* skin_pathadr; // address of asset path for skin; -1: none (nskin x 1)
|
||||
|
||||
// height fields
|
||||
mjtNum* hfield_size; // (x, y, z_top, z_bottom) (nhfield x 4)
|
||||
int* hfield_nrow; // number of rows 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)
|
||||
float* hfield_data; // elevation data (nhfielddata x 1)
|
||||
int* hfield_pathadr; // address of asset path for hfield; -1: none (nhfield x 1)
|
||||
mjtNum* hfield_size; // (x, y, z_top, z_bottom) (nhfield x 4)
|
||||
int* hfield_nrow; // number of rows 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)
|
||||
float* hfield_data; // elevation data (nhfielddata x 1)
|
||||
int* hfield_pathadr; // address of hfield asset path; -1: none (nhfield x 1)
|
||||
|
||||
// textures
|
||||
int* tex_type; // texture type (mjtTexture) (ntex x 1)
|
||||
int* tex_height; // number of rows 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)
|
||||
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_type; // texture type (mjtTexture) (ntex x 1)
|
||||
int* tex_height; // number of rows 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)
|
||||
mjtByte* tex_rgb; // rgb (alpha = 1) (ntexdata x 1)
|
||||
int* tex_pathadr; // address of texture asset path; -1: none (ntex x 1)
|
||||
|
||||
// materials
|
||||
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_shininess; // shininess coef (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)
|
||||
|
||||
// predefined geom pairs for collision detection; has precedence over exclude
|
||||
|
||||
@@ -263,6 +263,7 @@ struct mjvLight_ { // OpenGL light
|
||||
mjtByte headlight; // headlight
|
||||
mjtByte directional; // directional light
|
||||
mjtByte castshadow; // does light cast shadows
|
||||
float bulbradius; // bulb radius for soft shadows
|
||||
};
|
||||
typedef struct mjvLight_ mjvLight;
|
||||
|
||||
@@ -493,6 +494,7 @@ struct mjvSceneState_ {
|
||||
|
||||
mjtByte* light_directional;
|
||||
mjtByte* light_castshadow;
|
||||
float* light_bulbradius;
|
||||
mjtByte* light_active;
|
||||
float* light_attenuation;
|
||||
float* light_cutoff;
|
||||
@@ -560,6 +562,8 @@ struct mjvSceneState_ {
|
||||
float* mat_specular;
|
||||
float* mat_shininess;
|
||||
float* mat_reflectance;
|
||||
float* mat_metallic;
|
||||
float* mat_roughness;
|
||||
float* mat_rgba;
|
||||
|
||||
int* eq_type;
|
||||
|
||||
@@ -139,6 +139,7 @@
|
||||
X ( njmax ) \
|
||||
X ( nconmax ) \
|
||||
XMJV( ntree ) \
|
||||
X ( ngravcomp ) \
|
||||
X ( nuserdata ) \
|
||||
XMJV( nsensordata ) \
|
||||
X ( npluginstate ) \
|
||||
@@ -292,6 +293,7 @@
|
||||
X ( int, light_targetbodyid, nlight, 1 ) \
|
||||
XMJV( mjtByte, light_directional, nlight, 1 ) \
|
||||
XMJV( mjtByte, light_castshadow, nlight, 1 ) \
|
||||
XMJV( float, light_bulbradius, nlight, 1 ) \
|
||||
XMJV( mjtByte, light_active, nlight, 1 ) \
|
||||
X ( mjtNum, light_pos, nlight, 3 ) \
|
||||
X ( mjtNum, light_dir, nlight, 3 ) \
|
||||
@@ -365,6 +367,7 @@
|
||||
XMJV( int, mesh_bvhadr, nmesh, 1 ) \
|
||||
XMJV( int, mesh_bvhnum, nmesh, 1 ) \
|
||||
XMJV( int, mesh_graphadr, nmesh, 1 ) \
|
||||
X ( mjtNum, mesh_scale, nmesh, 3 ) \
|
||||
X ( mjtNum, mesh_pos, nmesh, 3 ) \
|
||||
X ( mjtNum, mesh_quat, nmesh, 4 ) \
|
||||
X ( float, mesh_vert, nmeshvert, 3 ) \
|
||||
@@ -416,6 +419,8 @@
|
||||
XMJV( float, mat_specular, nmat, 1 ) \
|
||||
XMJV( float, mat_shininess, 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 ) \
|
||||
X ( int, pair_dim, npair, 1 ) \
|
||||
X ( int, pair_geom1, npair, 1 ) \
|
||||
|
||||
@@ -24,7 +24,7 @@ extern "C" {
|
||||
#endif
|
||||
|
||||
// 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
|
||||
#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);
|
||||
|
||||
// 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.
|
||||
MJAPI int* mj_stackAllocInt(mjData* d, int size);
|
||||
MJAPI int* mj_stackAllocInt(mjData* d, size_t size);
|
||||
|
||||
// Free memory allocation in mjData.
|
||||
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 rotnew2old[9]);
|
||||
|
||||
|
||||
//---------------------------------- Quaternions ---------------------------------------------------
|
||||
|
||||
// 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.
|
||||
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 ---------------------------------------------------------
|
||||
|
||||
|
||||
+30
-2
@@ -774,7 +774,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([
|
||||
),
|
||||
FunctionParameterDecl(
|
||||
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
|
||||
@@ -794,7 +794,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([
|
||||
),
|
||||
FunctionParameterDecl(
|
||||
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
|
||||
@@ -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
|
||||
)),
|
||||
('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',
|
||||
FunctionDecl(
|
||||
name='mju_mulPose',
|
||||
|
||||
+71
-12
@@ -1200,6 +1200,11 @@ STRUCTS: Mapping[str, StructDecl] = dict([
|
||||
type=ValueType(name='int'),
|
||||
doc='number of kinematic trees under world body',
|
||||
),
|
||||
StructFieldDecl(
|
||||
name='ngravcomp',
|
||||
type=ValueType(name='int'),
|
||||
doc='number of bodies with nonzero gravcomp',
|
||||
),
|
||||
StructFieldDecl(
|
||||
name='nemax',
|
||||
type=ValueType(name='int'),
|
||||
@@ -2095,6 +2100,13 @@ STRUCTS: Mapping[str, StructDecl] = dict([
|
||||
),
|
||||
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(
|
||||
name='light_active',
|
||||
type=PointerType(
|
||||
@@ -2655,6 +2667,13 @@ STRUCTS: Mapping[str, StructDecl] = dict([
|
||||
),
|
||||
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(
|
||||
name='mesh_pos',
|
||||
type=PointerType(
|
||||
@@ -2835,84 +2854,84 @@ STRUCTS: Mapping[str, StructDecl] = dict([
|
||||
type=PointerType(
|
||||
inner_type=ValueType(name='mjtNum'),
|
||||
),
|
||||
doc='(x, y, z_top, z_bottom) (nhfield x 4)',
|
||||
doc='(x, y, z_top, z_bottom) (nhfield x 4)',
|
||||
),
|
||||
StructFieldDecl(
|
||||
name='hfield_nrow',
|
||||
type=PointerType(
|
||||
inner_type=ValueType(name='int'),
|
||||
),
|
||||
doc='number of rows in grid (nhfield x 1)',
|
||||
doc='number of rows in grid (nhfield x 1)',
|
||||
),
|
||||
StructFieldDecl(
|
||||
name='hfield_ncol',
|
||||
type=PointerType(
|
||||
inner_type=ValueType(name='int'),
|
||||
),
|
||||
doc='number of columns in grid (nhfield x 1)',
|
||||
doc='number of columns in grid (nhfield x 1)',
|
||||
),
|
||||
StructFieldDecl(
|
||||
name='hfield_adr',
|
||||
type=PointerType(
|
||||
inner_type=ValueType(name='int'),
|
||||
),
|
||||
doc='address in hfield_data (nhfield x 1)',
|
||||
doc='address in hfield_data (nhfield x 1)',
|
||||
),
|
||||
StructFieldDecl(
|
||||
name='hfield_data',
|
||||
type=PointerType(
|
||||
inner_type=ValueType(name='float'),
|
||||
),
|
||||
doc='elevation data (nhfielddata x 1)', # pylint: disable=line-too-long
|
||||
doc='elevation data (nhfielddata x 1)', # pylint: disable=line-too-long
|
||||
),
|
||||
StructFieldDecl(
|
||||
name='hfield_pathadr',
|
||||
type=PointerType(
|
||||
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(
|
||||
name='tex_type',
|
||||
type=PointerType(
|
||||
inner_type=ValueType(name='int'),
|
||||
),
|
||||
doc='texture type (mjtTexture) (ntex x 1)',
|
||||
doc='texture type (mjtTexture) (ntex x 1)',
|
||||
),
|
||||
StructFieldDecl(
|
||||
name='tex_height',
|
||||
type=PointerType(
|
||||
inner_type=ValueType(name='int'),
|
||||
),
|
||||
doc='number of rows in texture image (ntex x 1)',
|
||||
doc='number of rows in texture image (ntex x 1)',
|
||||
),
|
||||
StructFieldDecl(
|
||||
name='tex_width',
|
||||
type=PointerType(
|
||||
inner_type=ValueType(name='int'),
|
||||
),
|
||||
doc='number of columns in texture image (ntex x 1)',
|
||||
doc='number of columns in texture image (ntex x 1)',
|
||||
),
|
||||
StructFieldDecl(
|
||||
name='tex_adr',
|
||||
type=PointerType(
|
||||
inner_type=ValueType(name='int'),
|
||||
),
|
||||
doc='address in rgb (ntex x 1)',
|
||||
doc='address in rgb (ntex x 1)',
|
||||
),
|
||||
StructFieldDecl(
|
||||
name='tex_rgb',
|
||||
type=PointerType(
|
||||
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(
|
||||
name='tex_pathadr',
|
||||
type=PointerType(
|
||||
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(
|
||||
name='mat_texid',
|
||||
@@ -2963,6 +2982,20 @@ STRUCTS: Mapping[str, StructDecl] = dict([
|
||||
),
|
||||
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(
|
||||
name='mat_rgba',
|
||||
type=PointerType(
|
||||
@@ -5610,6 +5643,11 @@ STRUCTS: Mapping[str, StructDecl] = dict([
|
||||
type=ValueType(name='mjtByte'),
|
||||
doc='does light cast shadows',
|
||||
),
|
||||
StructFieldDecl(
|
||||
name='bulbradius',
|
||||
type=ValueType(name='float'),
|
||||
doc='bulb radius for soft shadows',
|
||||
),
|
||||
),
|
||||
)),
|
||||
('mjvOption',
|
||||
@@ -6705,6 +6743,13 @@ STRUCTS: Mapping[str, StructDecl] = dict([
|
||||
),
|
||||
doc='',
|
||||
),
|
||||
StructFieldDecl(
|
||||
name='light_bulbradius',
|
||||
type=PointerType(
|
||||
inner_type=ValueType(name='float'),
|
||||
),
|
||||
doc='',
|
||||
),
|
||||
StructFieldDecl(
|
||||
name='light_active',
|
||||
type=PointerType(
|
||||
@@ -7132,6 +7177,20 @@ STRUCTS: Mapping[str, StructDecl] = dict([
|
||||
),
|
||||
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(
|
||||
name='mat_rgba',
|
||||
type=PointerType(
|
||||
|
||||
@@ -16,12 +16,7 @@
|
||||
|
||||
# pylint:disable=g-importing-member
|
||||
from mujoco.mjx._src.collision_driver import collision
|
||||
from mujoco.mjx._src.collision_driver import get_params
|
||||
from mujoco.mjx._src.collision_driver import ncon
|
||||
from mujoco.mjx._src.constraint import count_constraints
|
||||
from mujoco.mjx._src.constraint import make_constraint
|
||||
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 forward
|
||||
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.solver import solve
|
||||
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 mul_m
|
||||
from mujoco.mjx._src.support import name2id
|
||||
from mujoco.mjx._src.test_util import benchmark
|
||||
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
|
||||
from jax import numpy as jp
|
||||
from mujoco.mjx._src import math
|
||||
from mujoco.mjx._src import mesh
|
||||
# pylint: disable=g-importing-member
|
||||
from mujoco.mjx._src.collision_base import Contact
|
||||
from mujoco.mjx._src.collision_base import GeomInfo
|
||||
from mujoco.mjx._src.collision_types import Collision
|
||||
from mujoco.mjx._src.collision_types import ConvexInfo
|
||||
from mujoco.mjx._src.collision_types import FunctionKey
|
||||
from mujoco.mjx._src.collision_types import GeomInfo
|
||||
from mujoco.mjx._src.types import Data
|
||||
from mujoco.mjx._src.types import GeomType
|
||||
from mujoco.mjx._src.types import Model
|
||||
# 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(
|
||||
a: jax.Array, b: jax.Array, p0: jax.Array, plane_normal: jax.Array
|
||||
) -> jax.Array:
|
||||
@@ -178,7 +219,8 @@ def _manifold_points(
|
||||
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."""
|
||||
vert = convex.vert
|
||||
|
||||
@@ -186,7 +228,7 @@ def plane_convex(plane: GeomInfo, convex: GeomInfo) -> Contact:
|
||||
plane_pos = convex.mat.T @ (plane.pos - convex.pos)
|
||||
n = convex.mat.T @ plane.mat[:, 2]
|
||||
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]
|
||||
|
||||
# convert to world frame
|
||||
@@ -200,10 +242,11 @@ def plane_convex(plane: GeomInfo, convex: GeomInfo) -> Contact:
|
||||
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."""
|
||||
faces = convex.face
|
||||
normals = convex.facenorm
|
||||
normals = convex.face_normal
|
||||
|
||||
# Put sphere in convex frame.
|
||||
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
|
||||
pos = convex.mat @ pos + convex.pos
|
||||
|
||||
return jax.tree_map(
|
||||
lambda x: jp.expand_dims(x, axis=0), (dist, pos, math.make_frame(n))
|
||||
)
|
||||
return 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."""
|
||||
# Get convex transformed normals, faces, and vertices.
|
||||
faces = convex.face
|
||||
normals = convex.facenorm
|
||||
normals = convex.face_normal
|
||||
|
||||
# Put capsule in convex frame.
|
||||
cap_pos = convex.mat.T @ (cap.pos - convex.pos)
|
||||
@@ -347,7 +389,7 @@ def capsule_convex(cap: GeomInfo, convex: GeomInfo) -> Contact:
|
||||
degenerate_edge_dir,
|
||||
edge_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_voronoi_front = ((edge_face_normals @ edge_axis) < 0).all()
|
||||
@@ -869,10 +911,9 @@ def _sat_gaussmap(
|
||||
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."""
|
||||
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
|
||||
# face has shape (n_face, n_vert, 3)
|
||||
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
|
||||
|
||||
faces1 = to_local_pos + faces1 @ to_local_mat.T
|
||||
normals1 = c1.facenorm @ to_local_mat.T
|
||||
normals2 = c2.facenorm
|
||||
normals1 = c1.face_normal @ to_local_mat.T
|
||||
normals2 = c2.face_normal
|
||||
|
||||
vertices1 = to_local_pos + c1.vert @ to_local_mat.T
|
||||
vertices2 = c2.vert
|
||||
|
||||
unique_edges1 = jp.take(vertices1, c1.edge, axis=0)
|
||||
unique_edges2 = jp.take(vertices2, c2.edge, axis=0)
|
||||
unique_edges1 = jp.take(vertices1, c1.edge_dir, axis=0)
|
||||
unique_edges2 = jp.take(vertices2, c2.edge_dir, axis=0)
|
||||
|
||||
edges1 = jp.take(vertices1, c1.edge, axis=0)
|
||||
edges2 = jp.take(vertices2, c2.edge, axis=0)
|
||||
@@ -944,13 +985,6 @@ def convex_convex(c1: GeomInfo, c2: GeomInfo) -> Contact:
|
||||
pos = c2.pos + pos @ c2.mat.T
|
||||
normal = normal @ c2.mat.T
|
||||
normal = -normal if swapped else normal
|
||||
|
||||
frame = jax.vmap(math.make_frame)(normal)
|
||||
|
||||
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
|
||||
# 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
|
||||
from jax import numpy as jp
|
||||
import mujoco
|
||||
from mujoco.mjx._src import collision_base
|
||||
from mujoco.mjx._src import mesh
|
||||
from mujoco.mjx._src import support
|
||||
# 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 convex_convex
|
||||
from mujoco.mjx._src.collision_convex import plane_convex
|
||||
from mujoco.mjx._src.collision_convex import sphere_convex
|
||||
from mujoco.mjx._src.collision_primitive import capsule_capsule
|
||||
from mujoco.mjx._src.collision_primitive import plane_capsule
|
||||
from mujoco.mjx._src.collision_primitive import plane_cylinder
|
||||
from mujoco.mjx._src.collision_primitive import plane_ellipsoid
|
||||
from mujoco.mjx._src.collision_primitive import plane_sphere
|
||||
from mujoco.mjx._src.collision_primitive import sphere_capsule
|
||||
from mujoco.mjx._src.collision_primitive import sphere_sphere
|
||||
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 Data
|
||||
from mujoco.mjx._src.types import DisableBit
|
||||
from mujoco.mjx._src.types import GeomType
|
||||
from mujoco.mjx._src.types import Model
|
||||
# pylint: enable=g-importing-member
|
||||
import numpy as np
|
||||
|
||||
# pair-wise collision functions
|
||||
_COLLISION_FUNC = {
|
||||
@@ -50,6 +76,7 @@ _COLLISION_FUNC = {
|
||||
(GeomType.PLANE, GeomType.CAPSULE): plane_capsule,
|
||||
(GeomType.PLANE, GeomType.BOX): plane_convex,
|
||||
(GeomType.PLANE, GeomType.ELLIPSOID): plane_ellipsoid,
|
||||
(GeomType.PLANE, GeomType.CYLINDER): plane_cylinder,
|
||||
(GeomType.PLANE, GeomType.MESH): plane_convex,
|
||||
(GeomType.SPHERE, GeomType.SPHERE): sphere_sphere,
|
||||
(GeomType.SPHERE, GeomType.CAPSULE): sphere_capsule,
|
||||
@@ -57,378 +84,315 @@ _COLLISION_FUNC = {
|
||||
(GeomType.SPHERE, GeomType.MESH): sphere_convex,
|
||||
(GeomType.CAPSULE, GeomType.CAPSULE): capsule_capsule,
|
||||
(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.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.MESH): convex_convex,
|
||||
(GeomType.MESH, GeomType.MESH): convex_convex,
|
||||
}
|
||||
|
||||
|
||||
def get_collision_fn(
|
||||
key: Tuple[Union[GeomType, mujoco.mjtGeom], Union[GeomType, mujoco.mjtGeom]]
|
||||
) -> Optional[Callable[[GeomInfo, GeomInfo], collision_base.Contact]]:
|
||||
"""Returns a collision function given a pair of geom types."""
|
||||
return _COLLISION_FUNC.get(key, None)
|
||||
# geoms for which we ignore broadphase
|
||||
_GEOM_NO_BROADPHASE = {GeomType.HFIELD, GeomType.PLANE}
|
||||
|
||||
|
||||
def _add_candidate(
|
||||
result: CandidateSet,
|
||||
def has_collision_fn(t1: GeomType, t2: GeomType) -> bool:
|
||||
"""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],
|
||||
g1: int,
|
||||
g2: int,
|
||||
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
|
||||
) -> Iterator[Tuple[int, int, int]]:
|
||||
"""Yields geom pairs to check for collisions.
|
||||
|
||||
# MuJoCo does not collide planes with other planes or hfields
|
||||
if t1 == GeomType.PLANE and t2 == GeomType.PLANE:
|
||||
return
|
||||
if t1 == GeomType.PLANE and t2 == GeomType.HFIELD:
|
||||
return
|
||||
Args:
|
||||
m: a MuJoCo or MJX model
|
||||
|
||||
def mesh_key(i):
|
||||
convex_data = [[None] * m.ngeom] * 3
|
||||
if isinstance(m, Model):
|
||||
convex_data = [
|
||||
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
|
||||
Yields:
|
||||
geom1, geom2, and pair index if defined in <pair> (else -1)
|
||||
"""
|
||||
pairs = set()
|
||||
|
||||
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)
|
||||
geom_con = m.geom_contype | m.geom_conaffinity
|
||||
filterparent = not (m.opt.disableflags & DisableBit.FILTERPARENT)
|
||||
b_start = m.body_geomadr
|
||||
b_end = b_start + m.body_geomnum
|
||||
|
||||
for b1 in range(m.nbody):
|
||||
if not geom_con[b_start[b1]:b_end[b1]].any():
|
||||
continue
|
||||
w1 = m.body_weldid[b1]
|
||||
w1_p = m.body_weldid[m.body_parentid[w1]]
|
||||
|
||||
for b2 in range(b1, m.nbody):
|
||||
if not geom_con[b_start[b2]:b_end[b2]].any():
|
||||
continue
|
||||
signature = (b1 << 16) + (b2)
|
||||
if signature in exclude_signature:
|
||||
continue
|
||||
if _body_pair_filter(m, b1, b2):
|
||||
w2 = m.body_weldid[b2]
|
||||
# ignore self-collisions
|
||||
if w1 == w2:
|
||||
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 in range(b_start[b1], b_end[b1]):
|
||||
if not geom_con[g1]:
|
||||
continue
|
||||
for g2 in range(b_start[b2], b_end[b2]):
|
||||
if not geom_con[g2]:
|
||||
for g1, g2 in itertools.product(g1_range, g2_range):
|
||||
t1, t2 = m.geom_type[g1], m.geom_type[g2]
|
||||
# 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
|
||||
if (t1, t2) == (GeomType.PLANE, GeomType.HFIELD):
|
||||
continue
|
||||
# geoms must match contype and conaffinity on some bit
|
||||
mask = m.geom_contype[g1] & m.geom_conaffinity[g2]
|
||||
mask |= m.geom_contype[g2] & m.geom_conaffinity[g1]
|
||||
if mask != 0:
|
||||
_add_candidate(candidate_set, m, g1, g2)
|
||||
if not mask:
|
||||
continue
|
||||
|
||||
return candidate_set
|
||||
if (g1, g2) not in pairs:
|
||||
pairs.add((g1, g2))
|
||||
yield g1, g2, -1
|
||||
|
||||
|
||||
def ncon(m: Union[Model, mujoco.MjModel]) -> int:
|
||||
"""Returns the number of contacts computed in MJX given a model."""
|
||||
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 0
|
||||
return np.empty(0, dtype=int)
|
||||
|
||||
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'))
|
||||
group_counts = {k: len(v) for k, v in _geom_groups(m).items()}
|
||||
|
||||
count = 0
|
||||
for k, v in candidates.items():
|
||||
fn = get_collision_fn(k[0:2])
|
||||
if fn is None:
|
||||
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
|
||||
# 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')
|
||||
|
||||
return min(max_count, count) if max_count > -1 else count
|
||||
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:
|
||||
"""Collides geometries."""
|
||||
if ncon(m) == 0:
|
||||
return d.replace(contact=Contact.zero())
|
||||
if d.ncon == 0:
|
||||
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 = []
|
||||
for key, candidates in candidate_set.items():
|
||||
geom_types = key[0:2]
|
||||
contacts.append(_collide_geoms(m, d, geom_types, candidates))
|
||||
# run collision functions on groups
|
||||
for key, contact in groups.items():
|
||||
# determine which contacts we'll use for collision testing by running a
|
||||
# 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:
|
||||
raise RuntimeError('No contacts found.')
|
||||
# run the collision function specified by the grouping key
|
||||
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'))
|
||||
if max_contact_points > -1 and contact.dist.shape[0] > max_contact_points:
|
||||
# get top-k contacts
|
||||
_, 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)
|
||||
# limit the number of contacts per condim group if requested
|
||||
if max_contact_points > -1:
|
||||
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)
|
||||
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)
|
||||
|
||||
@@ -220,6 +220,73 @@ class EllipsoidCollisionTest(parameterized.TestCase):
|
||||
_assert_attr_eq(
|
||||
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):
|
||||
_CAP_PLANE = """
|
||||
@@ -358,7 +425,8 @@ class CapsuleCollisionTest(parameterized.TestCase):
|
||||
self.assertEqual(c.pos.shape[0], 2)
|
||||
self.assertGreater(c.dist[1], 0)
|
||||
# extract the contact point with penetration
|
||||
c = jax.tree_map(lambda x: jp.take(x, 0, axis=0)[None], dx.contact)
|
||||
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):
|
||||
_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.assertGreater(c.dist[1], 0)
|
||||
# extract the contact point with penetration
|
||||
c = jax.tree_map(lambda x: jp.take(x, 0, axis=0)[None], dx.contact)
|
||||
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):
|
||||
_assert_attr_eq(c, d.contact, field.name, 'edge_shallow_tip1', 1e-4)
|
||||
np.testing.assert_array_almost_equal(
|
||||
@@ -413,7 +482,8 @@ class CapsuleCollisionTest(parameterized.TestCase):
|
||||
self.assertEqual(c.pos.shape[0], 2)
|
||||
self.assertGreater(c.dist[1], 0)
|
||||
# extract the contact point with penetration
|
||||
c = jax.tree_map(lambda x: jp.take(x, 0, axis=0)[None], dx.contact)
|
||||
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):
|
||||
_assert_attr_eq(c, d.contact, field.name, 'edge_shallow_tip2', 1e-4)
|
||||
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):
|
||||
"""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)
|
||||
# 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):
|
||||
_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)
|
||||
# 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):
|
||||
_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)
|
||||
|
||||
|
||||
class NconTest(parameterized.TestCase):
|
||||
"""Tests ncon."""
|
||||
class DimTest(parameterized.TestCase):
|
||||
"""Tests contact dim."""
|
||||
|
||||
def test_ncon(self):
|
||||
m = test_util.load_test_file('constraints.xml')
|
||||
ncon = collision_driver.ncon(m)
|
||||
self.assertEqual(ncon, 16)
|
||||
dim = collision_driver.make_condim(m)
|
||||
expected = [1] * 4 + [3] * 20 + [4] * 4 + [6] * 4
|
||||
np.testing.assert_array_equal(dim, np.array(expected))
|
||||
|
||||
def test_disable_contact(self):
|
||||
m = test_util.load_test_file('constraints.xml')
|
||||
m.opt.disableflags |= DisableBit.CONTACT
|
||||
ncon = collision_driver.ncon(m)
|
||||
self.assertEqual(ncon, 0)
|
||||
dim = collision_driver.make_condim(m)
|
||||
self.assertEqual(dim.size, 0)
|
||||
|
||||
def test_ncon_meshes(self):
|
||||
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)
|
||||
|
||||
mx = mjx.put_model(m)
|
||||
ncon = collision_driver.ncon(mx)
|
||||
ncon = collision_driver.make_condim(mx).size
|
||||
self.assertEqual(ncon, 15)
|
||||
|
||||
# get rid of max_contact_points, test only max_geom_pairs
|
||||
@@ -707,11 +825,11 @@ class NconTest(parameterized.TestCase):
|
||||
if name_ == 'max_contact_points':
|
||||
m.numeric_data[m.numeric_adr[i]] = -1
|
||||
|
||||
ncon = collision_driver.ncon(m)
|
||||
ncon = collision_driver.make_condim(m).size
|
||||
self.assertEqual(ncon, 98)
|
||||
|
||||
mx = mjx.put_model(m)
|
||||
ncon = collision_driver.ncon(mx)
|
||||
ncon = collision_driver.make_condim(mx).size
|
||||
self.assertEqual(ncon, 98)
|
||||
|
||||
|
||||
|
||||
@@ -20,34 +20,53 @@ import jax
|
||||
from jax import numpy as jp
|
||||
from mujoco.mjx._src import math
|
||||
# pylint: disable=g-importing-member
|
||||
from mujoco.mjx._src.collision_base import Contact
|
||||
from mujoco.mjx._src.collision_base import GeomInfo
|
||||
from mujoco.mjx._src.collision_types import Collision
|
||||
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
|
||||
|
||||
|
||||
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(
|
||||
plane_normal: jax.Array,
|
||||
plane_pos: jax.Array,
|
||||
sphere_pos: jax.Array,
|
||||
radius: jax.Array,
|
||||
sphere_radius: jax.Array,
|
||||
) -> Tuple[jax.Array, jax.Array]:
|
||||
"""Returns the penetration and contact point between a plane and sphere."""
|
||||
cdist = jp.dot(sphere_pos - plane_pos, plane_normal)
|
||||
dist = cdist - radius
|
||||
pos = sphere_pos - plane_normal * (radius + 0.5 * dist)
|
||||
"""Returns the distance and contact point between a plane and sphere."""
|
||||
dist = jp.dot(sphere_pos - plane_pos, plane_normal) - sphere_radius
|
||||
pos = sphere_pos - plane_normal * (sphere_radius + 0.5 * dist)
|
||||
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."""
|
||||
n = plane.mat[:, 2]
|
||||
dist, pos = _plane_sphere(n, plane.pos, sphere.pos, sphere.size[0])
|
||||
return jax.tree_map(
|
||||
lambda x: jp.expand_dims(x, axis=0), (dist, pos, math.make_frame(n))
|
||||
)
|
||||
return 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."""
|
||||
n, axis = plane.mat[:, 2], cap.mat[:, 2]
|
||||
# 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)
|
||||
frame = jp.array([[n, b, jp.cross(n, b)]])
|
||||
segment = axis * cap.size[1]
|
||||
contacts = []
|
||||
collisions = []
|
||||
for offset in [segment, -segment]:
|
||||
dist, pos = _plane_sphere(n, plane.pos, cap.pos + offset, cap.size[0])
|
||||
dist = jp.expand_dims(dist, axis=0)
|
||||
pos = jp.expand_dims(pos, axis=0)
|
||||
contacts.append((dist, pos, frame))
|
||||
return jax.tree_map(lambda *x: jp.concatenate(x), *contacts)
|
||||
collisions.append((dist, pos, frame))
|
||||
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."""
|
||||
n = plane.mat[:, 2]
|
||||
size = ellipsoid.size
|
||||
@@ -73,14 +93,72 @@ def plane_ellipsoid(plane: GeomInfo, ellipsoid: GeomInfo) -> Contact:
|
||||
pos = ellipsoid.pos + ellipsoid.mat @ (sphere_support * size)
|
||||
dist = jp.dot(n, pos - plane.pos)
|
||||
pos = pos - n * dist * 0.5
|
||||
return jax.tree_map(
|
||||
lambda x: jp.expand_dims(x, axis=0), (dist, pos, math.make_frame(n))
|
||||
return 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(
|
||||
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."""
|
||||
n, dist = math.normalize_with_norm(pos2 - pos1)
|
||||
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
|
||||
|
||||
|
||||
def sphere_sphere(s1: GeomInfo, s2: GeomInfo) -> Contact:
|
||||
@collider(ncon=1)
|
||||
def sphere_sphere(s1: GeomInfo, s2: GeomInfo) -> Collision:
|
||||
"""Calculates contact between two spheres."""
|
||||
dist, pos, n = _sphere_sphere(s1.pos, s1.size[0], s2.pos, s2.size[0])
|
||||
return jax.tree_map(
|
||||
lambda x: jp.expand_dims(x, axis=0), (dist, pos, math.make_frame(n))
|
||||
)
|
||||
return 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."""
|
||||
axis, length = cap.mat[:, 2], cap.size[1]
|
||||
segment = axis * length
|
||||
@@ -105,19 +183,14 @@ def sphere_capsule(sphere: GeomInfo, cap: GeomInfo) -> Contact:
|
||||
cap.pos - segment, cap.pos + segment, sphere.pos
|
||||
)
|
||||
dist, pos, n = _sphere_sphere(sphere.pos, sphere.size[0], pt, cap.size[0])
|
||||
return jax.tree_map(
|
||||
lambda x: jp.expand_dims(x, axis=0), (dist, pos, math.make_frame(n))
|
||||
)
|
||||
return 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."""
|
||||
axis1, length1, axis2, length2 = (
|
||||
cap1.mat[:, 2],
|
||||
cap1.size[1],
|
||||
cap2.mat[:, 2],
|
||||
cap2.size[1],
|
||||
)
|
||||
axis1, length1 = cap1.mat[:, 2], cap1.size[1]
|
||||
axis2, length2 = cap2.mat[:, 2], cap2.size[1]
|
||||
seg1, seg2 = axis1 * length1, axis2 * length2
|
||||
pt1, pt2 = math.closest_segment_to_segment_points(
|
||||
cap1.pos - seg1,
|
||||
@@ -127,14 +200,4 @@ def capsule_capsule(cap1: GeomInfo, cap2: GeomInfo) -> Contact:
|
||||
)
|
||||
radius1, radius2 = cap1.size[0], cap2.size[0]
|
||||
dist, pos, n = _sphere_sphere(pt1, radius1, pt2, radius2)
|
||||
return jax.tree_map(
|
||||
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
|
||||
return dist, pos, math.make_frame(n)
|
||||
|
||||
@@ -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
|
||||
# pylint: disable=g-importing-member
|
||||
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 Data
|
||||
from mujoco.mjx._src.types import DisableBit
|
||||
@@ -34,6 +35,9 @@ from mujoco.mjx._src.types import Model
|
||||
import numpy as np
|
||||
|
||||
|
||||
_CONDIM_EFC_COUNT = {1: 1, 3: 4, 4: 6, 6: 10}
|
||||
|
||||
|
||||
class _Efc(PyTreeNode):
|
||||
"""Support data for creating constraint matrices."""
|
||||
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)
|
||||
|
||||
# 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 = jp.repeat(invweight, 3)
|
||||
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)
|
||||
|
||||
# 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 = jp.repeat(invweight, 3)
|
||||
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]:
|
||||
"""Calculates constraint rows for contacts."""
|
||||
|
||||
if collision_driver.ncon(m) == 0:
|
||||
if d.ncon == 0:
|
||||
return None
|
||||
|
||||
@jax.vmap
|
||||
def fn(c: Contact):
|
||||
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]
|
||||
def contact_efc(c: Contact, condim: int):
|
||||
|
||||
# rotate Jacobian differences to contact frame
|
||||
diff_con = c.frame @ diff.T
|
||||
@jax.vmap
|
||||
def fn(c: Contact):
|
||||
dist = c.dist - c.includemargin
|
||||
active = dist < 0
|
||||
body1, body2 = jp.array(m.geom_bodyid)[c.geom]
|
||||
jac1p, jac1r = support.jac(m, d, c.pos, body1)
|
||||
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]
|
||||
|
||||
# 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)
|
||||
if condim == 1:
|
||||
return diff[0] * active, tran, dist * active, c.solref, c.solimp
|
||||
|
||||
active = dist < 0
|
||||
j, invweight = jp.stack(js) * active, jp.stack(invweights)
|
||||
pos = jp.repeat(dist, 4) * active
|
||||
solref, solimp = jp.tile(c.solref, (4, 1)), jp.tile(c.solimp, (4, 1))
|
||||
# a pair of opposing pyramid edges per friction dimension
|
||||
# repeat friction directions with positive and negative sign
|
||||
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 j, invweight, pos, solref, solimp
|
||||
return fn(c)
|
||||
|
||||
res = fn(d.contact)
|
||||
# remove contact grouping dimension:
|
||||
j, invweight, pos, solref, solimp = jax.tree_map(jp.concatenate, res)
|
||||
# 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)
|
||||
|
||||
return _Efc(j, pos, pos, invweight, solref, solimp, frictionloss)
|
||||
|
||||
|
||||
def count_constraints(
|
||||
m: Union[Model, mujoco.MjModel], d: Optional[Data] = None
|
||||
) -> Tuple[int, int, int, int]:
|
||||
def counts(efc_type: np.ndarray) -> Tuple[int, int, int, int]:
|
||||
"""Returns equality, friction, limit, and contact constraint counts."""
|
||||
if m.opt.disableflags & DisableBit.CONSTRAINT:
|
||||
return 0, 0, 0, 0
|
||||
|
||||
if m.opt.disableflags & DisableBit.EQUALITY:
|
||||
ne = 0
|
||||
else:
|
||||
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
|
||||
ne = (efc_type == ConstraintType.EQUALITY).sum()
|
||||
nf = 0 # no support for friction loss yet
|
||||
nl = (efc_type == ConstraintType.LIMIT_JOINT).sum()
|
||||
nc_f = (efc_type == ConstraintType.CONTACT_FRICTIONLESS).sum()
|
||||
nc_p = (efc_type == ConstraintType.CONTACT_PYRAMIDAL).sum()
|
||||
nc = nc_f + nc_p
|
||||
|
||||
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:
|
||||
"""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)
|
||||
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
|
||||
def fn(efc):
|
||||
|
||||
@@ -48,13 +48,31 @@ class ConstraintTest(absltest.TestCase):
|
||||
mujoco.mj_forward(m, d)
|
||||
mx = mjx.put_model(m)
|
||||
dx = mjx.put_data(m, d)
|
||||
|
||||
dx = mjx.make_constraint(mx, dx)
|
||||
nnz = dx.efc_J.any(axis=1)
|
||||
_assert_eq(d.efc_J, dx.efc_J[nnz].reshape(-1), 'efc_J')
|
||||
_assert_eq(d.efc_D, dx.efc_D[nnz], 'efc_D')
|
||||
_assert_eq(d.efc_aref, dx.efc_aref[nnz], 'efc_aref')
|
||||
_assert_eq(d.efc_frictionloss, dx.efc_frictionloss[nnz], 'efc_frictionloss')
|
||||
d_efc_j = d.efc_J.reshape((-1, m.nv))
|
||||
|
||||
# ne, nf, nl order matches
|
||||
efl = d.ne + d.nf + d.nl
|
||||
_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):
|
||||
m = test_util.load_test_file('constraints.xml')
|
||||
@@ -65,14 +83,14 @@ class ConstraintTest(absltest.TestCase):
|
||||
pos = jp.ones(3)
|
||||
|
||||
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)
|
||||
self.assertEqual(k, 1 / (0.99**2 * timeconst**2))
|
||||
|
||||
def test_disable_constraint(self):
|
||||
m = test_util.load_test_file('constraints.xml')
|
||||
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(nf, 0)
|
||||
self.assertEqual(nl, 0)
|
||||
@@ -83,18 +101,18 @@ class ConstraintTest(absltest.TestCase):
|
||||
def test_disable_equality(self):
|
||||
m = test_util.load_test_file('constraints.xml')
|
||||
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(nf, 0)
|
||||
self.assertEqual(nl, 2)
|
||||
self.assertEqual(nc, 64)
|
||||
self.assertEqual(nc, 148)
|
||||
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):
|
||||
m = test_util.load_test_file('constraints.xml')
|
||||
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(nf, 0)
|
||||
self.assertEqual(nl, 2)
|
||||
|
||||
@@ -18,7 +18,7 @@ import copy
|
||||
import dataclasses
|
||||
|
||||
import typing
|
||||
from typing import Any, Dict, Optional, Sequence, TypeVar, Union
|
||||
from typing import Dict, Optional, Sequence, Tuple, TypeVar, Union
|
||||
import jax
|
||||
import numpy as np
|
||||
|
||||
@@ -122,7 +122,7 @@ class PyTreeNode:
|
||||
raise NotImplementedError
|
||||
|
||||
@classmethod
|
||||
def fields(cls) -> tuple[dataclasses.Field[Any], ...]:
|
||||
def fields(cls) -> Tuple[dataclasses.Field, ...]: # pylint: disable=g-bare-generic
|
||||
return dataclasses.fields(cls)
|
||||
|
||||
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
|
||||
|
||||
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
|
||||
actfrcrange = jp.where(
|
||||
m.jnt_actfrclimited[:, None],
|
||||
m.jnt_actfrcrange,
|
||||
jp.array([-jp.inf, jp.inf]),
|
||||
)
|
||||
ids = sum(
|
||||
([i] * JointType(j).dof_width() for i, j in enumerate(m.jnt_type)), []
|
||||
)
|
||||
actfrcrange = jp.take(actfrcrange, jp.array(ids), axis=0)
|
||||
actfrcrange = actfrcrange[m.dof_jntid]
|
||||
qfrc_actuator = jp.clip(qfrc_actuator, actfrcrange[:, 0], actfrcrange[:, 1])
|
||||
|
||||
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
|
||||
# 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)
|
||||
)
|
||||
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):
|
||||
qvel, qacc, act_dot, kqvel, d = carry
|
||||
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)
|
||||
)
|
||||
# get intermediate RK solutions
|
||||
|
||||
+125
-136
@@ -22,15 +22,14 @@ from jax import numpy as jp
|
||||
import mujoco
|
||||
from mujoco.mjx._src import collision_driver
|
||||
from mujoco.mjx._src import constraint
|
||||
from mujoco.mjx._src import mesh
|
||||
from mujoco.mjx._src import support
|
||||
from mujoco.mjx._src import types
|
||||
import numpy as np
|
||||
import scipy
|
||||
|
||||
|
||||
def _put_option(o: mujoco.MjOption, device=None) -> types.Option:
|
||||
"""Puts mujoco.MjOption onto a device, resulting in mjx.Option."""
|
||||
def _make_option(o: mujoco.MjOption) -> types.Option:
|
||||
"""Returns mjx.Option given mujoco.MjOption."""
|
||||
if o.integrator not in set(types.IntegratorType):
|
||||
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:
|
||||
raise NotImplementedError(f'{mujoco.mjtEnableBit(2 ** i)}')
|
||||
|
||||
static_fields = {
|
||||
f.name: copy.copy(getattr(o, f.name))
|
||||
for f in types.Option.fields()
|
||||
if f.type in (int, bytes, np.ndarray)
|
||||
}
|
||||
static_fields['integrator'] = types.IntegratorType(o.integrator)
|
||||
static_fields['cone'] = types.ConeType(o.cone)
|
||||
static_fields['jacobian'] = types.JacobianType(o.jacobian)
|
||||
static_fields['solver'] = types.SolverType(o.solver)
|
||||
static_fields['disableflags'] = types.DisableBit(o.disableflags)
|
||||
fields = {f.name: getattr(o, f.name, None) for f in types.Option.fields()}
|
||||
fields['integrator'] = types.IntegratorType(o.integrator)
|
||||
fields['cone'] = types.ConeType(o.cone)
|
||||
fields['jacobian'] = types.JacobianType(o.jacobian)
|
||||
fields['solver'] = types.SolverType(o.solver)
|
||||
fields['disableflags'] = types.DisableBit(o.disableflags)
|
||||
fields['has_fluid_params'] = o.density > 0 or o.viscosity > 0 or o.wind.any()
|
||||
|
||||
device_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,
|
||||
)
|
||||
return types.Option(**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."""
|
||||
return types.Statistic(
|
||||
meaninertia=jax.device_put(s.meaninertia, device=device)
|
||||
)
|
||||
return types.Statistic(meaninertia=s.meaninertia)
|
||||
|
||||
|
||||
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:
|
||||
raise NotImplementedError('tendons are not supported')
|
||||
|
||||
if (m.geom_condim != 3).any() or (m.pair_dim != 3).any():
|
||||
raise NotImplementedError('only condim=3 is 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 collision_driver.get_collision_fn((g1, g2)) is None:
|
||||
raise NotImplementedError(f'({g1}, {g2}) has no collision function')
|
||||
*_, params = collision_driver.get_params(m, c)
|
||||
margin_gap = not np.allclose(np.concatenate([params.margin, params.gap]), 0)
|
||||
if mujoco.mjtGeom.mjGEOM_MESH in (g1, g2) and margin_gap:
|
||||
raise NotImplementedError(
|
||||
f'Margin and gap not implemented for ({g1}, {g2})'
|
||||
)
|
||||
for g1, g2, ip in collision_driver.geom_pairs(m):
|
||||
t1, t2 = m.geom_type[[g1, g2]]
|
||||
# check collision function exists for type pair
|
||||
if not collision_driver.has_collision_fn(t1, t2):
|
||||
t1, t2 = mujoco.mjtGeom(t1), mujoco.mjtGeom(t2)
|
||||
raise NotImplementedError(f'({t1}, {t2}) collisions not implemented.')
|
||||
# margin/gap not supported for geoms
|
||||
if mujoco.mjtGeom.mjGEOM_MESH in (t1, t2):
|
||||
if ip != -1:
|
||||
margin = m.pair_margin[ip]
|
||||
else:
|
||||
margin = m.geom_margin[g1] + m.geom_margin[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 (
|
||||
(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):
|
||||
raise NotImplementedError('dof_frictionloss is not implemented.')
|
||||
|
||||
opt = _put_option(m.opt, device=device)
|
||||
stat = _put_statistic(m.stat, device=device)
|
||||
fields = {f.name: getattr(m, f.name) for f in types.Model.fields()}
|
||||
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 = {
|
||||
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,
|
||||
)
|
||||
return jax.device_put(model, device=device)
|
||||
|
||||
|
||||
def make_data(m: Union[types.Model, mujoco.MjModel]) -> types.Data:
|
||||
"""Allocate and initialize Data."""
|
||||
|
||||
ncon = collision_driver.ncon(m)
|
||||
ne, nf, nl, nc = constraint.count_constraints(m)
|
||||
nefc = ne + nf + nl + nc
|
||||
dim = collision_driver.make_condim(m)
|
||||
efc_type = constraint.make_efc_type(m, dim)
|
||||
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
|
||||
|
||||
zero_0 = jp.zeros(0, 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_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(
|
||||
ne=ne,
|
||||
nf=nf,
|
||||
nl=nl,
|
||||
nefc=nefc,
|
||||
ncon=ncon,
|
||||
solver_niter=jp.array(0, dtype=int),
|
||||
time=jp.array(0.0),
|
||||
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,
|
||||
qLD=zero_nm if support.is_sparse(m) else zero_nv_nv,
|
||||
qLDiagInv=zero_nv if support.is_sparse(m) else zero_0,
|
||||
contact=types.Contact.zero(ncon),
|
||||
contact=contact,
|
||||
efc_type=efc_type,
|
||||
efc_J=jp.zeros((nefc, m.nv), dtype=float),
|
||||
efc_frictionloss=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,
|
||||
cdof_dot=zero_nv_6,
|
||||
qfrc_bias=zero_nv,
|
||||
qfrc_gravcomp=zero_nv,
|
||||
qfrc_passive=zero_nv,
|
||||
efc_aref=zero_nefc,
|
||||
qfrc_actuator=zero_nv,
|
||||
@@ -219,16 +207,13 @@ def make_data(m: Union[types.Model, mujoco.MjModel]) -> types.Data:
|
||||
qfrc_constraint=zero_nv,
|
||||
qfrc_inverse=zero_nv,
|
||||
efc_force=zero_nefc,
|
||||
userdata=jp.zeros(m.nuserdata, dtype=float),
|
||||
)
|
||||
|
||||
return d
|
||||
|
||||
|
||||
def _get_contact(
|
||||
c: mujoco._structs._MjContactList,
|
||||
cx: types.Contact,
|
||||
efc_start: int,
|
||||
):
|
||||
def _get_contact(c: mujoco._structs._MjContactList, cx: types.Contact):
|
||||
"""Converts mjx.Contact to mujoco._structs._MjContactList."""
|
||||
con_id = np.nonzero(cx.dist <= 0)[0]
|
||||
for field in types.Contact.fields():
|
||||
@@ -237,10 +222,6 @@ def _get_contact(
|
||||
value = value.reshape((-1, 9))
|
||||
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(
|
||||
m: mujoco.MjModel, d: types.Data
|
||||
@@ -274,13 +255,6 @@ def get_data_into(
|
||||
d = jax.device_get(d)
|
||||
|
||||
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 = [], []
|
||||
for i in range(m.nv):
|
||||
@@ -291,12 +265,11 @@ def get_data_into(
|
||||
j = m.dof_parentid[j]
|
||||
|
||||
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
|
||||
ncon = (d_i.contact.dist <= 0).sum()
|
||||
efc_active = (d_i.efc_J != 0).any(axis=1)
|
||||
efc_con = efc_type == mujoco.mjtConstraint.mjCNSTR_CONTACT_PYRAMIDAL
|
||||
nefc, nc = int(efc_active.sum()), int((efc_active & efc_con).sum())
|
||||
nefc = int(efc_active.sum())
|
||||
result_i.nnzJ = nefc * m.nv
|
||||
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
|
||||
@@ -306,61 +279,77 @@ def get_data_into(
|
||||
|
||||
for field in types.Data.fields():
|
||||
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
|
||||
|
||||
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))
|
||||
|
||||
if field.name in ('efc_frictionloss', 'efc_D', 'efc_aref', 'efc_force'):
|
||||
elif field.name.startswith('efc_'):
|
||||
value = value[efc_active]
|
||||
|
||||
if field.name == 'efc_J':
|
||||
value = value[efc_active].reshape(-1)
|
||||
|
||||
if field.name == 'qM' and not support.is_sparse(m):
|
||||
if field.name == 'efc_J':
|
||||
value = value.reshape(-1)
|
||||
elif field.name == 'qM' and not support.is_sparse(m):
|
||||
value = value[dof_i, dof_j]
|
||||
|
||||
if field.name == 'qLD' and not support.is_sparse(m):
|
||||
elif field.name == 'qLD' and not support.is_sparse(m):
|
||||
value = value[dof_i, dof_j]
|
||||
|
||||
if field.name == 'qLDiagInv' and not support.is_sparse(m):
|
||||
elif field.name == 'qLDiagInv' and not support.is_sparse(m):
|
||||
value = np.ones(m.nv)
|
||||
|
||||
if value.shape:
|
||||
if isinstance(value, np.ndarray) and value.shape:
|
||||
getattr(result_i, field.name)[:] = value
|
||||
else:
|
||||
setattr(result_i, field.name, value)
|
||||
|
||||
result_i.efc_type[:] = efc_type[efc_active]
|
||||
|
||||
|
||||
def _put_contact(
|
||||
c: mujoco._structs._MjContactList, ncon: int, device=None
|
||||
def _make_contact(
|
||||
c: mujoco._structs._MjContactList,
|
||||
dim: np.ndarray,
|
||||
efc_address: np.ndarray,
|
||||
) -> types.Contact:
|
||||
"""Puts mujoco.structs._MjContactList onto a device, resulting in mjx.Contact."""
|
||||
fields = {
|
||||
f.name: copy.copy(getattr(c, f.name)) for f in types.Contact.fields()
|
||||
}
|
||||
"""Converts mujoco.structs._MjContactList into mjx.Contact."""
|
||||
fields = {f.name: getattr(c, f.name) for f in types.Contact.fields()}
|
||||
fields['frame'] = fields['frame'].reshape((-1, 3, 3))
|
||||
pad_size = ncon - c.dist.shape[0]
|
||||
pad_fn = lambda x: np.concatenate(
|
||||
(x, np.zeros((pad_size,) + x.shape[1:], dtype=x.dtype))
|
||||
)
|
||||
fields = jax.tree_map(pad_fn, fields)
|
||||
fields['dist'][-pad_size:] = np.inf
|
||||
fields = jax.device_put(fields, device=device)
|
||||
# reorder contacts so that their condims match those specified in dim.
|
||||
# if we have fewer Contacts for a condim range, pad the range with zeros
|
||||
|
||||
# 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
|
||||
)
|
||||
zero['dist'][:] = np.finfo(float).max
|
||||
fields = jax.tree_util.tree_map(lambda *x: np.concatenate(x), fields, zero)
|
||||
fields = jax.tree_util.tree_map(lambda x: x[contact_map], fields)
|
||||
|
||||
fields['dim'] = dim
|
||||
fields['efc_address'] = efc_address
|
||||
|
||||
return types.Contact(**fields)
|
||||
|
||||
|
||||
def put_data(m: mujoco.MjModel, d: mujoco.MjData, device=None) -> types.Data:
|
||||
"""Puts mujoco.MjData onto a device, resulting in mjx.Data."""
|
||||
ncon = collision_driver.ncon(m)
|
||||
ne, nf, nl, nc = constraint.count_constraints(m)
|
||||
nefc = ne + nf + nl + nc
|
||||
dim = collision_driver.make_condim(m)
|
||||
efc_type = constraint.make_efc_type(m, dim)
|
||||
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 (
|
||||
(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:
|
||||
raise ValueError(f'd.{name} too high, d.{name} = {d_val}, model = {val}')
|
||||
|
||||
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
|
||||
}
|
||||
fields = {f.name: getattr(d, f.name) for f in types.Data.fields()}
|
||||
|
||||
# MJX prefers square matrices for these fields:
|
||||
for fname in ('xmat', 'ximat', 'geom_xmat', 'site_xmat', 'cam_xmat'):
|
||||
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])
|
||||
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]
|
||||
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
|
||||
|
||||
# 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['qLDiagInv'] = np.zeros(0)
|
||||
|
||||
fields = jax.device_put(fields, device=device)
|
||||
fields['contact'] = _put_contact(d.contact, ncon, device=device)
|
||||
fields['contact'] = _make_contact(d.contact, dim, efc_address)
|
||||
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 = """
|
||||
<mujoco>
|
||||
<worldbody>
|
||||
<geom type="plane" size="3 3 .01"/>
|
||||
<geom type="plane" size="3 3 .01" condim="6"/>
|
||||
<body name="cap1" pos="-.3 -.3 .2">
|
||||
<freejoint/>
|
||||
<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_almost_equal(mx.geom_solref, m.geom_solref)
|
||||
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_dofadr, m.jnt_dofadr)
|
||||
@@ -177,22 +173,6 @@ class ModelIOTest(parameterized.TestCase):
|
||||
</tendon>
|
||||
</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):
|
||||
with self.assertRaises(NotImplementedError):
|
||||
mjx.put_model(mujoco.MjModel.from_xml_string("""
|
||||
@@ -244,6 +224,7 @@ class DataIOTest(parameterized.TestCase):
|
||||
nv = 19
|
||||
nefc = 185
|
||||
|
||||
self.assertEqual(d.nefc, nefc)
|
||||
self.assertEqual(d.qpos.shape, (nq,))
|
||||
self.assertEqual(d.qvel.shape, (nv,))
|
||||
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)
|
||||
|
||||
# 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))
|
||||
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[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[9:], 0) # no contact
|
||||
np.testing.assert_allclose(dx.efc_J[5:15], d_efc_j[4:14]) # contact
|
||||
np.testing.assert_allclose(dx.efc_J[15:], 0) # no contact
|
||||
|
||||
# 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[4], 0)
|
||||
np.testing.assert_allclose(dx.efc_aref[5:9], d.efc_aref[4:8])
|
||||
np.testing.assert_allclose(dx.efc_aref[9:], 0)
|
||||
np.testing.assert_allclose(dx.efc_aref[5:15], d.efc_aref[4:14])
|
||||
np.testing.assert_allclose(dx.efc_aref[15:], 0)
|
||||
|
||||
# check sparse transform is correct
|
||||
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)
|
||||
|
||||
# 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)
|
||||
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)
|
||||
|
||||
# efc_address is created on demand
|
||||
np.testing.assert_allclose(d_2.contact.efc_address, d.contact.efc_address)
|
||||
|
||||
def test_get_data_batched(self):
|
||||
@@ -423,7 +403,7 @@ class DataIOTest(parameterized.TestCase):
|
||||
mujoco.mj_step(m, d, 2)
|
||||
dx = mjx.put_data(m, d)
|
||||
# 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)
|
||||
self.assertLen(ds, 2)
|
||||
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))
|
||||
|
||||
|
||||
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):
|
||||
"""Transform spatial motion.
|
||||
|
||||
|
||||
+102
-145
@@ -15,14 +15,15 @@
|
||||
"""Mesh processing."""
|
||||
|
||||
import collections
|
||||
import dataclasses
|
||||
import itertools
|
||||
from typing import Dict, List, Optional, Sequence, Tuple
|
||||
from typing import Tuple
|
||||
import warnings
|
||||
|
||||
import mujoco
|
||||
import jax
|
||||
from jax import numpy as jp
|
||||
# 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
|
||||
# pylint: enable=g-importing-member
|
||||
import numpy as np
|
||||
@@ -30,37 +31,7 @@ from scipy import spatial
|
||||
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
|
||||
_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:
|
||||
@@ -170,16 +141,7 @@ def _convex_hull_2d(points: np.ndarray, normal: np.ndarray) -> np.ndarray:
|
||||
return hull_point_idx
|
||||
|
||||
|
||||
@dataclasses.dataclass
|
||||
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:
|
||||
def _merge_coplanar(m: Model, tm: trimesh.Trimesh, meshid: int) -> np.ndarray:
|
||||
"""Merges coplanar facets."""
|
||||
if not tm.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
|
||||
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(
|
||||
f'Mesh "{mesh_info.name}" has a coplanar face with more than'
|
||||
f' {_MAX_HULL_FACE_VERTICES} vertices. This may lead to performance '
|
||||
f'Mesh "{name}" has a coplanar face with more than '
|
||||
f'{_MAX_HULL_FACE_VERTICES} vertices. This may lead to performance '
|
||||
'issues and inaccuracies in collision detection. Consider '
|
||||
'decimating the mesh.'
|
||||
)
|
||||
@@ -231,106 +195,99 @@ def _merge_coplanar(tm: trimesh.Trimesh, mesh_info: MeshInfo) -> np.ndarray:
|
||||
return np.concatenate([faces, facets])
|
||||
|
||||
|
||||
def _mesh_info(
|
||||
m: mujoco.MjModel,
|
||||
) -> List[MeshInfo]:
|
||||
"""Extracts mesh info from MjModel."""
|
||||
mesh_infos = []
|
||||
for i in range(m.nmesh):
|
||||
name = mujoco.mj_id2name(m, mujoco.mjtObj.mjOBJ_MESH.value, i)
|
||||
|
||||
last = (i + 1) >= m.nmesh
|
||||
face_start = m.mesh_faceadr[i]
|
||||
face_end = m.mesh_faceadr[i + 1] if not last else m.mesh_face.shape[0]
|
||||
face = m.mesh_face[face_start:face_end]
|
||||
|
||||
vert_start = m.mesh_vertadr[i]
|
||||
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
|
||||
|
||||
graph = m.mesh_graph[graphadr:]
|
||||
numvert, numface = graph[0], graph[1]
|
||||
|
||||
# unused vert_edgeadr
|
||||
# vert_edgeadr = graph[2 : numvert + 2]
|
||||
last_idx = numvert + 2
|
||||
|
||||
vert_globalid = graph[last_idx : last_idx + numvert]
|
||||
last_idx += numvert
|
||||
|
||||
# unused edge_localid
|
||||
# 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])))
|
||||
convex_face = np.vectorize(vertex_map.get)(face_globalid)
|
||||
mesh_infos.append(MeshInfo(name, vert, face, convex_vert, convex_face))
|
||||
|
||||
return mesh_infos
|
||||
|
||||
|
||||
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
|
||||
def box(info: GeomInfo) -> ConvexInfo:
|
||||
"""Creates a box with rectangular faces."""
|
||||
vert = np.array(
|
||||
list(itertools.product((-1, 1), (-1, 1), (-1, 1))), dtype=float
|
||||
)
|
||||
# pyformat: disable
|
||||
# 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
|
||||
|
||||
c = ConvexInfo(
|
||||
info.pos,
|
||||
info.mat,
|
||||
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)
|
||||
|
||||
return c
|
||||
|
||||
|
||||
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_idx = 0
|
||||
|
||||
numvert, numface = graph[0], graph[1]
|
||||
graph_idx += 2
|
||||
|
||||
# skip vert_edgeadr (numvert,)
|
||||
graph_idx += numvert
|
||||
vert_globalid = graph[graph_idx : graph_idx + numvert]
|
||||
graph_idx += numvert
|
||||
|
||||
# skip edge_localid (numvert, 3)
|
||||
graph_idx += numvert + 3 * numface
|
||||
face_globalid = graph[graph_idx : graph_idx + 3 * numface].reshape((-1, 3))
|
||||
|
||||
vert = vert[vert_globalid]
|
||||
vertex_map = dict(zip(vert_globalid, np.arange(vert_globalid.shape[0])))
|
||||
face = np.vectorize(vertex_map.get)(face_globalid)
|
||||
|
||||
tm_convex = trimesh.Trimesh(vertices=vert, faces=face)
|
||||
vert = np.array(tm_convex.vertices)
|
||||
face = _merge_coplanar(tm_convex, mesh_info)
|
||||
facenormal = _get_face_norm(vert, face)
|
||||
edge, edge_face_normal = _get_edge_normals(face, facenormal)
|
||||
return {
|
||||
'geom_convex_face': vert[face],
|
||||
'geom_convex_face_vert_idx': face,
|
||||
'geom_convex_vert': vert,
|
||||
'geom_convex_edge_dir': _get_unique_edge_dir(vert, face),
|
||||
'geom_convex_facenormal': facenormal,
|
||||
'geom_convex_edge': edge,
|
||||
'geom_convex_edge_face_normal': edge_face_normal,
|
||||
}
|
||||
face = _merge_coplanar(m, tm_convex, mesh_id)
|
||||
face_normal = _get_face_norm(vert, face)
|
||||
edge, edge_face_normal = _get_edge_normals(face, face_normal)
|
||||
edge_dir = _get_unique_edge_dir(vert, face)
|
||||
face = vert[face] # materialize full nface x nvert matrix
|
||||
|
||||
c = ConvexInfo(
|
||||
info.pos,
|
||||
info.mat,
|
||||
vert,
|
||||
face,
|
||||
face_normal,
|
||||
edge,
|
||||
edge_face_normal,
|
||||
edge_dir,
|
||||
)
|
||||
|
||||
def get(m: mujoco.MjModel) -> Dict[str, Sequence[Optional[np.ndarray]]]:
|
||||
"""Derives geom mesh attributes for mjx.Model from MjModel."""
|
||||
kwargs = {k: [] for k in _DERIVED_ARGS}
|
||||
mesh_infos = _mesh_info(m)
|
||||
geom_con = m.geom_conaffinity | m.geom_contype
|
||||
for geomid in range(m.ngeom):
|
||||
mesh_info = None
|
||||
dataid = m.geom_dataid[geomid]
|
||||
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]
|
||||
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
|
||||
return jax.tree_util.tree_map(jp.array, c)
|
||||
|
||||
@@ -20,7 +20,7 @@ import numpy as np
|
||||
import trimesh
|
||||
|
||||
|
||||
class GeomMeshKwargsTest(absltest.TestCase):
|
||||
class MeshTest(absltest.TestCase):
|
||||
|
||||
def test_pyramid(self):
|
||||
"""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_convex = trimesh.convex.convex_hull(tm)
|
||||
convex_vert = np.array(tm_convex.vertices)
|
||||
convex_face = np.array(tm_convex.faces)
|
||||
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)
|
||||
convex_face = mesh._merge_coplanar(None, tm_convex, 0)
|
||||
|
||||
# get index of vertices in h['geom_convex_vert'] for vertices in vert
|
||||
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))
|
||||
vidx = np.argmin(dist, axis=0)
|
||||
|
||||
# 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
|
||||
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()])
|
||||
expected_face_verts = sorted([
|
||||
(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
|
||||
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()))
|
||||
np.testing.assert_array_equal(
|
||||
unique_edge,
|
||||
@@ -77,10 +70,11 @@ class GeomMeshKwargsTest(absltest.TestCase):
|
||||
)
|
||||
|
||||
# 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
|
||||
edges = h['geom_convex_edge']
|
||||
edges, edge_normal = mesh._get_edge_normals(convex_face, face_normal)
|
||||
edges = np.vectorize(map_.get)(edges)
|
||||
mask = edges[:, 0] != edges[:, 1]
|
||||
edges = edges[mask]
|
||||
@@ -103,7 +97,6 @@ class GeomMeshKwargsTest(absltest.TestCase):
|
||||
)
|
||||
|
||||
# face edge normals
|
||||
edge_normal = h['geom_convex_edge_face_normal']
|
||||
edge_normal = edge_normal[mask]
|
||||
edge_normal = np.take_along_axis(
|
||||
edge_normal, sort_col_idx[..., None], axis=1
|
||||
|
||||
@@ -29,6 +29,100 @@ from mujoco.mjx._src.types import Model
|
||||
# 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(
|
||||
m: Model,
|
||||
inertia: jax.Array,
|
||||
@@ -71,71 +165,3 @@ def _inertia_box_fluid_model(
|
||||
force, torque = ximat @ lfrc_vel, ximat @ lfrc_ang
|
||||
|
||||
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')
|
||||
d = mujoco.MjData(m)
|
||||
# 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_forward(m, d)
|
||||
mx = mjx.put_model(m)
|
||||
|
||||
dx = jax.jit(mjx.passive)(mx, mjx.put_data(m, d))
|
||||
_assert_attr_eq(d, dx, 'qfrc_passive')
|
||||
_assert_attr_eq(d, dx, 'qfrc_gravcomp')
|
||||
|
||||
# test with fluid forces
|
||||
m.opt.density = 0.01
|
||||
@@ -56,18 +57,21 @@ class PassiveTest(absltest.TestCase):
|
||||
mx = mjx.put_model(m)
|
||||
dx = jax.jit(mjx.passive)(mx, mjx.put_data(m, d))
|
||||
_assert_attr_eq(d, dx, 'qfrc_passive')
|
||||
_assert_attr_eq(d, dx, 'qfrc_gravcomp')
|
||||
|
||||
m.opt.viscosity = 0.02
|
||||
mujoco.mj_forward(m, d)
|
||||
mx = mjx.put_model(m)
|
||||
dx = jax.jit(mjx.passive)(mx, mjx.put_data(m, d))
|
||||
_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])
|
||||
mujoco.mj_forward(m, d)
|
||||
mx = mjx.put_model(m)
|
||||
dx = jax.jit(mjx.passive)(mx, mjx.put_data(m, d))
|
||||
_assert_attr_eq(d, dx, 'qfrc_passive')
|
||||
_assert_attr_eq(d, dx, 'qfrc_gravcomp')
|
||||
|
||||
# test disable passive
|
||||
mx = mx.tree_replace({'opt.disableflags': mjx.DisableBit.PASSIVE})
|
||||
|
||||
+19
-28
@@ -134,32 +134,27 @@ def _ray_triangle(
|
||||
vert: jax.Array,
|
||||
pnt: jax.Array,
|
||||
vec: jax.Array,
|
||||
b0: jax.Array,
|
||||
b1: jax.Array,
|
||||
basis: jax.Array,
|
||||
) -> jax.Array:
|
||||
"""Returns the distance at which a ray intersects with a triangle."""
|
||||
# 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
|
||||
# A = (p0-p2, p1-p2), b = -p2, solve A*t = b
|
||||
A = jp.array( # pylint: disable=invalid-name
|
||||
[planar[:, 0] - planar[:, 2], planar[:, 1] - planar[:, 2]]
|
||||
).T.flatten()
|
||||
b = -planar[:, 2]
|
||||
det = A[0] * A[3] - A[1] * A[2]
|
||||
valid = jp.abs(det) >= mujoco.mjMINVAL
|
||||
A = planar[0:2] - planar[2] # pylint: disable=invalid-name
|
||||
b = -planar[2]
|
||||
det = A[0, 0] * A[1, 1] - A[1, 0] * A[0, 1]
|
||||
|
||||
t0 = (A[3] * b[0] - A[1] * b[1]) / det
|
||||
t1 = (-A[2] * b[0] + A[0] * b[1]) / det
|
||||
valid &= (t0 >= 0) & (t1 >= 0) & (t0 + t1 <= 1)
|
||||
t0 = (A[1, 1] * b[0] - A[1, 0] * b[1]) / det
|
||||
t1 = (-A[0, 1] * b[0] + A[0, 0] * b[1]) / det
|
||||
valid = (t0 >= 0) & (t1 >= 0) & (t0 + t1 <= 1)
|
||||
|
||||
# intersect ray with plane of triangle
|
||||
nrm = jp.cross(vert[0] - vert[2], vert[1] - vert[2])
|
||||
denom = jp.dot(vec, nrm)
|
||||
valid &= jp.abs(denom) >= mujoco.mjMINVAL
|
||||
|
||||
dist = jp.where(valid, -jp.dot(pnt - vert[2], nrm) / denom, jp.inf)
|
||||
dist = jp.dot(vert[2] - pnt, nrm) / jp.dot(vec, nrm)
|
||||
valid &= dist >= 0
|
||||
dist = jp.where(valid, dist, jp.inf)
|
||||
|
||||
return dist
|
||||
|
||||
@@ -174,33 +169,29 @@ def _ray_mesh(
|
||||
"""Returns the best distance and geom_id for ray mesh intersections."""
|
||||
data_id = m.geom_dataid[geom_id]
|
||||
|
||||
ray_basis = lambda x: math.orthogonals(math.normalize(x))
|
||||
b0, b1 = jax.vmap(ray_basis)(vec)
|
||||
ray_basis = lambda x: jp.array(math.orthogonals(math.normalize(x))).T
|
||||
basis = jax.vmap(ray_basis)(vec)
|
||||
|
||||
faceadr = np.append(m.mesh_faceadr, m.nmeshface)
|
||||
vertadr = np.append(m.mesh_vertadr, m.nmeshvert)
|
||||
|
||||
dists = []
|
||||
dists, geom_ids = [], []
|
||||
for i, id_ in enumerate(data_id):
|
||||
face = m.mesh_face[faceadr[id_] : faceadr[id_ + 1]]
|
||||
vert = m.mesh_vert[vertadr[id_] : vertadr[id_ + 1]]
|
||||
dist = jax.vmap(_ray_triangle, in_axes=(0, None, None, None, None))(
|
||||
vert[face], pnt[i], vec[i], b0[i], b1[i]
|
||||
vert = jp.array(vert[face])
|
||||
dist = jax.vmap(_ray_triangle, in_axes=(0, None, None, None))(
|
||||
vert, pnt[i], vec[i], basis[i]
|
||||
)
|
||||
dists.append(dist)
|
||||
|
||||
# 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
|
||||
geom_ids.append(np.repeat(geom_id[i], dist.size))
|
||||
|
||||
dists = jp.concatenate(dists)
|
||||
min_id = jp.argmin(dists)
|
||||
# Grab the best distance amongst all meshes, bypassing the argmin in `ray`.
|
||||
# This avoids having to compute the best distance per mesh.
|
||||
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_
|
||||
|
||||
|
||||
@@ -152,11 +152,7 @@ class RayTest(absltest.TestCase):
|
||||
mx, dx = mjx.put_model(m), mjx.put_data(m, d)
|
||||
|
||||
# look at the tetrahedron
|
||||
pnt, vec = jp.array([2.0, 2.0, 2.0]), -jp.array([
|
||||
1.0,
|
||||
1.0,
|
||||
1.0,
|
||||
])
|
||||
pnt, vec = jp.array([2.0, 2.0, 2.0]), -jp.array([1.0, 1.0, 1.0])
|
||||
vec /= jp.linalg.norm(vec)
|
||||
dist, geomid = jax.jit(mjx.ray)(mx, dx, pnt, vec)
|
||||
_assert_eq(geomid, 4, 'geom_id')
|
||||
@@ -166,12 +162,14 @@ class RayTest(absltest.TestCase):
|
||||
_assert_eq(geomid, 4, 'geom_id')
|
||||
_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
|
||||
pnt, vec = jp.array([4.0, 2.0, 2.0]), -jp.array([
|
||||
2.0,
|
||||
1.0,
|
||||
1.0,
|
||||
])
|
||||
pnt, vec = jp.array([4.0, 2.0, 2.0]), -jp.array([2.0, 1.0, 1.0])
|
||||
vec /= jp.linalg.norm(vec)
|
||||
dist, geomid = jax.jit(mjx.ray)(mx, dx, pnt, vec)
|
||||
_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')
|
||||
return x
|
||||
|
||||
return jax.tree_map(take, obj)
|
||||
return jax.tree_util.tree_map(take, obj)
|
||||
|
||||
|
||||
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)]
|
||||
|
||||
# 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]
|
||||
|
||||
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)]
|
||||
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
|
||||
reordered_ys = []
|
||||
@@ -465,15 +465,15 @@ def body_tree(
|
||||
def index_sum(x, i=id_map, s=body_ids.size):
|
||||
return jax.ops.segment_sum(x, i, s)
|
||||
|
||||
y = jax.tree_map(index_sum, y)
|
||||
carry = y if carry is None else jax.tree_map(jp.add, carry, y)
|
||||
y = jax.tree_util.tree_map(index_sum, y)
|
||||
carry = y if carry is None else jax.tree_util.tree_map(jp.add, carry, y)
|
||||
elif key in key_parents:
|
||||
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]])
|
||||
parent_ids = m.body_parentid[key_body_ids[key]]
|
||||
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])]
|
||||
key_y[key] = _nvmap(f, carry, *f_args)
|
||||
@@ -488,8 +488,8 @@ def body_tree(
|
||||
if len(out_types) > 1:
|
||||
y_typ = [y_[i] for y_ in y_typ]
|
||||
if typ != 'b':
|
||||
y_typ = jax.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(jp.concatenate, 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]))
|
||||
_check_output(y_typ, y_take, typ, i)
|
||||
y.append(_take(y_typ, y_take))
|
||||
|
||||
@@ -55,7 +55,7 @@ class ScanTest(absltest.TestCase):
|
||||
<worldbody/>
|
||||
</mujoco>
|
||||
""")
|
||||
m = mjx.device_put(m)
|
||||
m = mjx.put_model(m)
|
||||
|
||||
def fn(body_id):
|
||||
return body_id + 1
|
||||
@@ -69,7 +69,7 @@ class ScanTest(absltest.TestCase):
|
||||
def test_flat_joints(self):
|
||||
"""Tests scanning over bodies with joints of different types."""
|
||||
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:
|
||||
# 1) j_fn receives jnt_types as a jp array
|
||||
@@ -105,7 +105,7 @@ class ScanTest(absltest.TestCase):
|
||||
def test_body_tree(self):
|
||||
"""Tests tree scanning over bodies with different joint counts."""
|
||||
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:
|
||||
# 1) j_fn receives jnt_pos which is a jp array
|
||||
@@ -196,7 +196,7 @@ class ScanTest(absltest.TestCase):
|
||||
def test_scan_actuators(self):
|
||||
"""Tests scanning over actuators."""
|
||||
m = mujoco.MjModel.from_xml_string(self._MULTI_ACT_XML)
|
||||
m = mjx.device_put(m)
|
||||
m = mjx.put_model(m)
|
||||
|
||||
fn = lambda *args: args
|
||||
args = (
|
||||
|
||||
@@ -17,7 +17,6 @@
|
||||
import jax
|
||||
from jax import numpy as jp
|
||||
import mujoco
|
||||
from mujoco.mjx._src import constraint
|
||||
from mujoco.mjx._src import math
|
||||
from mujoco.mjx._src import smooth
|
||||
from mujoco.mjx._src import support
|
||||
@@ -81,7 +80,7 @@ class _Context(PyTreeNode):
|
||||
prev_cost=0.0,
|
||||
solver_niter=0,
|
||||
)
|
||||
ctx = _update_constraint(m, d, ctx)
|
||||
ctx = _update_constraint(d, ctx)
|
||||
if grad:
|
||||
ctx = _update_gradient(m, d, ctx)
|
||||
ctx = ctx.replace(search=-ctx.Mgrad) # start with preconditioned gradient
|
||||
@@ -107,7 +106,7 @@ class _LSPoint(PyTreeNode):
|
||||
@classmethod
|
||||
def create(
|
||||
cls,
|
||||
m: Model,
|
||||
d: Data,
|
||||
ctx: _Context,
|
||||
alpha: jax.Array,
|
||||
jv: jax.Array,
|
||||
@@ -118,8 +117,7 @@ class _LSPoint(PyTreeNode):
|
||||
# roughly corresponds to CGEval in mujoco/src/engine/engine_solver.c
|
||||
|
||||
# TODO(robotics-team): change this to support friction constraints
|
||||
ne, nf, *_ = constraint.count_constraints(m)
|
||||
active = ((ctx.Jaref + alpha * jv) < 0).at[:ne + nf].set(True)
|
||||
active = ((ctx.Jaref + alpha * jv) < 0).at[:d.ne + d.nf].set(True)
|
||||
quad = jax.vmap(jp.multiply)(quad, active) # only active
|
||||
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]
|
||||
|
||||
|
||||
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.
|
||||
|
||||
Corresponds to CGupdateConstraint in mujoco/src/engine/engine_solver.c
|
||||
|
||||
Args:
|
||||
m: model defining constraints
|
||||
d: data which contains latest qacc and smooth terms
|
||||
ctx: current solver context
|
||||
|
||||
@@ -177,8 +174,7 @@ def _update_constraint(m: Model, d: Data, ctx: _Context) -> _Context:
|
||||
# TODO(robotics-team): add friction constraints
|
||||
|
||||
# only count active constraints
|
||||
ne, nf, *_ = constraint.count_constraints(m)
|
||||
active = (ctx.Jaref < 0).at[:ne + nf].set(True)
|
||||
active = (ctx.Jaref < 0).at[:d.ne + d.nf].set(True)
|
||||
|
||||
efc_force = d.efc_D * -ctx.Jaref * active
|
||||
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:
|
||||
mgrad = smooth.solve_m(m, d, grad)
|
||||
elif m.opt.solver == SolverType.NEWTON:
|
||||
ne, nf, *_ = constraint.count_constraints(m)
|
||||
active = (ctx.Jaref < 0).at[: ne + nf].set(True)
|
||||
active = (ctx.Jaref < 0).at[: d.ne + d.nf].set(True)
|
||||
h = (d.efc_J.T * d.efc_D * active) @ d.efc_J
|
||||
h = support.full_m(m, d) + h
|
||||
h_ = jax.scipy.linalg.cho_factor(h)
|
||||
@@ -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 = (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:
|
||||
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
|
||||
# 2) if moving to next or mid narrows the bracket
|
||||
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)
|
||||
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)
|
||||
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)
|
||||
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
|
||||
|
||||
@@ -302,8 +297,8 @@ def _linesearch(m: Model, d: Data, ctx: _Context) -> _Context:
|
||||
p0 = point_fn(jp.array(0.0))
|
||||
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)
|
||||
hi = jax.tree_map(lesser_fn, p0, lo)
|
||||
lo = jax.tree_map(lesser_fn, lo, p0)
|
||||
hi = jax.tree_util.tree_map(lesser_fn, p0, lo)
|
||||
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 = _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:
|
||||
ctx = _linesearch(m, d, ctx)
|
||||
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)
|
||||
|
||||
# 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 Model
|
||||
# pylint: enable=g-importing-member
|
||||
import numpy as np
|
||||
|
||||
|
||||
def is_sparse(m: Union[mujoco.MjModel, Model]) -> bool:
|
||||
@@ -149,19 +150,6 @@ def jac(
|
||||
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(
|
||||
m: Model,
|
||||
d: Data,
|
||||
@@ -200,11 +188,92 @@ def local_to_global(
|
||||
return pos, mat
|
||||
|
||||
|
||||
def get_custom_numeric(m: Union[Model, mujoco.MjModel], name: str) -> float:
|
||||
"""Returns a custom numeric given an MjModel or mjx.Model."""
|
||||
for i in range(m.nnumeric):
|
||||
name_ = m.names[m.name_numericadr[i] :].decode('utf-8').split('\x00', 1)[0]
|
||||
if name_ == name:
|
||||
return m.numeric_data[m.numeric_adr[i]]
|
||||
def _getnum(m: Union[Model, mujoco.MjModel], obj: mujoco._enums.mjtObj) -> int:
|
||||
"""Gets the number of objects for the given object type."""
|
||||
return {
|
||||
mujoco.mjtObj.mjOBJ_BODY: m.nbody,
|
||||
mujoco.mjtObj.mjOBJ_JOINT: m.njnt,
|
||||
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)
|
||||
|
||||
def test_custom_numeric(self):
|
||||
def test_custom(self):
|
||||
xml = """
|
||||
<mujoco model="right_shadow_hand">
|
||||
<custom>
|
||||
@@ -126,9 +126,36 @@ class SupportTest(parameterized.TestCase):
|
||||
</mujoco>
|
||||
"""
|
||||
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)
|
||||
self.assertEqual(support.get_custom_numeric(m, 'max_geom_pairs'), 42)
|
||||
|
||||
def _get_numeric(m, name):
|
||||
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__':
|
||||
|
||||
@@ -17,7 +17,7 @@
|
||||
import os
|
||||
import sys
|
||||
import time
|
||||
from typing import Dict, Tuple
|
||||
from typing import Dict, Optional, Tuple
|
||||
from xml.etree import ElementTree as ET
|
||||
|
||||
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['stiffness'] = '{:.2f}'.format(np.random.uniform() * 20)
|
||||
|
||||
joint_attr['actuatorgravcomp'] = np.random.choice(['true', 'false'])
|
||||
return joint_attr
|
||||
|
||||
|
||||
@@ -201,9 +202,9 @@ def _make_geom(
|
||||
|
||||
def _make_actuator(
|
||||
actuator_type: str,
|
||||
joint: str | None = None,
|
||||
site: str | None = None,
|
||||
refsite: str | None = None,
|
||||
joint: Optional[str] = None,
|
||||
site: Optional[str] = None,
|
||||
refsite: Optional[str] = None,
|
||||
) -> Dict[str, str]:
|
||||
"""Returns attributes for an actuator."""
|
||||
if joint:
|
||||
@@ -334,7 +335,16 @@ def create_mjcf(
|
||||
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}'
|
||||
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}'})
|
||||
|
||||
n_joints = len(list(mjcf.iter('joint')))
|
||||
|
||||
+109
-41
@@ -15,10 +15,8 @@
|
||||
"""Base types used in MJX."""
|
||||
|
||||
import enum
|
||||
from typing import List, Optional
|
||||
|
||||
import jax
|
||||
import jax.numpy as jp
|
||||
import mujoco
|
||||
from mujoco.mjx._src.dataclasses import PyTreeNode # pylint: disable=g-importing-member
|
||||
import numpy as np
|
||||
@@ -100,6 +98,7 @@ class GeomType(enum.IntEnum):
|
||||
CYLINDER: cylinder
|
||||
BOX: box
|
||||
MESH: mesh
|
||||
SDF: signed distance field
|
||||
"""
|
||||
|
||||
PLANE = mujoco.mjtGeom.mjGEOM_PLANE
|
||||
@@ -213,6 +212,24 @@ class BiasType(enum.IntEnum):
|
||||
# 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):
|
||||
"""Type of camera light.
|
||||
|
||||
@@ -309,7 +326,12 @@ class Model(PyTreeNode):
|
||||
npair: number of predefined geom pairs
|
||||
nexclude: number of excluded geom pairs
|
||||
neq: number of equality constraints
|
||||
ngravcomp: number of bodies with nonzero gravcomp
|
||||
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
|
||||
opt: physics options
|
||||
stat: model statistics
|
||||
@@ -331,6 +353,7 @@ class Model(PyTreeNode):
|
||||
body_mass: mass (nbody,)
|
||||
body_subtreemass: mass of subtree starting at this body (nbody,)
|
||||
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)
|
||||
jnt_type: type of joint (mjtJoint) (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_group: group for visibility (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_solimp: constraint solver impedance: limit (njnt, mjNIMP)
|
||||
jnt_pos: local anchor position (njnt, 3)
|
||||
@@ -370,6 +395,7 @@ class Model(PyTreeNode):
|
||||
geom_solref: constraint solver reference: contact (ngeom, mjNREF)
|
||||
geom_solimp: constraint solver impedance: contact (ngeom, mjNIMP)
|
||||
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_quat: local orientation offset rel. to body (ngeom, 4)
|
||||
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_pos0: global position rel. to body in qpos0 (ncam, 3)
|
||||
cam_mat0: global orientation in qpos0 (ncam, 9)
|
||||
mat_rgba: rgba (nmat, 4)
|
||||
mesh_vertadr: first vertex address (nmesh x 1)
|
||||
mesh_faceadr: first face address (nmesh x 1)
|
||||
mesh_vertadr: first vertex address (nmesh,)
|
||||
mesh_faceadr: first face address (nmesh,)
|
||||
mesh_graphadr: graph data address; -1: no graph (nmesh,)
|
||||
mesh_vert: vertex positions for all meshes (nmeshvert, 3)
|
||||
mesh_face: vertex face data (nmeshface, 3)
|
||||
geom_convex_face: vertex face data, MJX only (ngeom,)
|
||||
geom_convex_vert: vertex data, MJX only (ngeom,)
|
||||
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,)
|
||||
mesh_graph: convex graph data (nmeshgraph,)
|
||||
mat_rgba: rgba (nmat, 4)
|
||||
pair_dim: contact dimensionality (npair,)
|
||||
pair_geom1: id of geom1 (npair,)
|
||||
pair_geom2: id of geom2 (npair,)
|
||||
@@ -434,7 +456,24 @@ class Model(PyTreeNode):
|
||||
actuator_gear: scale length and transmitted force (nu, 6)
|
||||
numeric_adr: address of field in numeric_data (nnumeric,)
|
||||
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_tupleadr: tuple name pointers (ntuple,)
|
||||
name_keyadr: keyframe name pointers (nkey,)
|
||||
names: names of all objects, 0-terminated (nnames,)
|
||||
"""
|
||||
nq: int
|
||||
@@ -453,7 +492,12 @@ class Model(PyTreeNode):
|
||||
npair: int
|
||||
nexclude: int
|
||||
neq: int
|
||||
ngravcomp: int
|
||||
nnumeric: int
|
||||
nuserdata: int
|
||||
ntuple: int
|
||||
nsensor: int
|
||||
nkey: int
|
||||
nM: int # pylint:disable=invalid-name
|
||||
opt: Option
|
||||
stat: Statistic
|
||||
@@ -475,6 +519,7 @@ class Model(PyTreeNode):
|
||||
body_mass: jax.Array
|
||||
body_subtreemass: jax.Array
|
||||
body_inertia: jax.Array
|
||||
body_gravcomp: jax.Array
|
||||
body_invweight0: jax.Array
|
||||
jnt_type: np.ndarray
|
||||
jnt_qposadr: np.ndarray
|
||||
@@ -482,6 +527,7 @@ class Model(PyTreeNode):
|
||||
jnt_bodyid: np.ndarray
|
||||
jnt_limited: np.ndarray
|
||||
jnt_actfrclimited: np.ndarray
|
||||
jnt_actgravcomp: np.ndarray
|
||||
jnt_solref: jax.Array
|
||||
jnt_solimp: jax.Array
|
||||
jnt_pos: jax.Array
|
||||
@@ -514,6 +560,7 @@ class Model(PyTreeNode):
|
||||
geom_solref: jax.Array
|
||||
geom_solimp: jax.Array
|
||||
geom_size: jax.Array
|
||||
geom_rbound: jax.Array
|
||||
geom_pos: jax.Array
|
||||
geom_quat: jax.Array
|
||||
geom_friction: jax.Array
|
||||
@@ -533,18 +580,14 @@ class Model(PyTreeNode):
|
||||
cam_mat0: jax.Array
|
||||
mesh_vertadr: np.ndarray
|
||||
mesh_faceadr: np.ndarray
|
||||
mesh_graphadr: np.ndarray
|
||||
mesh_vert: np.ndarray
|
||||
mesh_face: np.ndarray
|
||||
mesh_graph: np.ndarray
|
||||
mat_rgba: np.ndarray
|
||||
pair_dim: np.ndarray
|
||||
pair_geom1: 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_solreffriction: jax.Array
|
||||
pair_solimp: jax.Array
|
||||
@@ -578,7 +621,24 @@ class Model(PyTreeNode):
|
||||
actuator_gear: jax.Array
|
||||
numeric_adr: 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_tupleadr: np.ndarray
|
||||
name_keyadr: np.ndarray
|
||||
names: bytes
|
||||
|
||||
|
||||
@@ -594,8 +654,11 @@ class Contact(PyTreeNode):
|
||||
solref: constraint solver reference, normal direction (mjNREF,)
|
||||
solreffriction: constraint solver reference, friction directions (mjNREF,)
|
||||
solimp: constraint solver impedance (mjNIMP,)
|
||||
geom1: id of geom 1
|
||||
geom2: id of geom 2
|
||||
dim: contact space dimensionality: 1, 3, 4, or 6
|
||||
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
|
||||
pos: jax.Array
|
||||
@@ -605,33 +668,25 @@ class Contact(PyTreeNode):
|
||||
solref: jax.Array
|
||||
solreffriction: jax.Array
|
||||
solimp: jax.Array
|
||||
# unsupported: mu, H, dim
|
||||
# unsupported: mu, H
|
||||
dim: np.ndarray
|
||||
geom1: jax.Array
|
||||
geom2: jax.Array
|
||||
# unsupported: efc_address, exclude
|
||||
|
||||
@classmethod
|
||||
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),
|
||||
)
|
||||
geom: jax.Array
|
||||
# unsupported: flex, elem, vert, exclude
|
||||
efc_address: np.ndarray
|
||||
|
||||
|
||||
class Data(PyTreeNode):
|
||||
r"""Dynamic state that updates each step.\
|
||||
r"""\Dynamic state that updates each step.
|
||||
|
||||
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
|
||||
qpos: position (nq,)
|
||||
qvel: velocity (nv,)
|
||||
@@ -661,14 +716,15 @@ class Data(PyTreeNode):
|
||||
cinert: com-based body inertia and mass (nbody, 10)
|
||||
actuator_length: actuator lengths (nu,)
|
||||
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,)
|
||||
if dense: (nv, nv)
|
||||
qLD: L'*D*L (or Cholesky) factorization of M. if sparse: (nM,)
|
||||
if dense: (nv, nv)
|
||||
qLDiagInv: 1/diag(D) if sparse: (nv,)
|
||||
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_frictionloss: frictionloss (friction) (nefc,)
|
||||
efc_D: constraint mass (nefc,)
|
||||
@@ -676,6 +732,7 @@ class Data(PyTreeNode):
|
||||
cvel: com-based velocity [3D rot; 3D tran] (nbody, 6)
|
||||
cdof_dot: time-derivative of cdof (nv, 6)
|
||||
qfrc_bias: C(qpos,qvel) (nv,)
|
||||
qfrc_gravcomp: passive gravity compensation force (nv,)
|
||||
qfrc_passive: passive force (nv,)
|
||||
efc_aref: reference pseudo-acceleration (nefc,)
|
||||
qfrc_actuator: actuator force (nv,)
|
||||
@@ -685,7 +742,14 @@ class Data(PyTreeNode):
|
||||
qfrc_inverse: net external force; should equal: (nv,)
|
||||
qfrc_applied + J'*xfrc_applied + qfrc_actuator
|
||||
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_niter: jax.Array
|
||||
# global properties:
|
||||
@@ -703,6 +767,8 @@ class Data(PyTreeNode):
|
||||
# dynamics:
|
||||
qacc: jax.Array
|
||||
act_dot: jax.Array
|
||||
# user data:
|
||||
userdata: jax.Array
|
||||
# position dependent:
|
||||
xpos: jax.Array
|
||||
xquat: jax.Array
|
||||
@@ -727,6 +793,7 @@ class Data(PyTreeNode):
|
||||
qLD: jax.Array # pylint:disable=invalid-name
|
||||
qLDiagInv: jax.Array # pylint:disable=invalid-name
|
||||
contact: Contact
|
||||
efc_type: np.ndarray
|
||||
efc_J: jax.Array # pylint:disable=invalid-name
|
||||
efc_frictionloss: jax.Array
|
||||
efc_D: jax.Array # pylint:disable=invalid-name
|
||||
@@ -736,6 +803,7 @@ class Data(PyTreeNode):
|
||||
cdof_dot: jax.Array
|
||||
qfrc_bias: jax.Array
|
||||
qfrc_passive: jax.Array
|
||||
qfrc_gravcomp: jax.Array
|
||||
efc_aref: jax.Array
|
||||
# position, velcoity, control & acceleration dependent:
|
||||
qfrc_actuator: jax.Array
|
||||
|
||||
@@ -30,7 +30,10 @@ import numpy as np
|
||||
|
||||
def _assert_attr_eq(mjx_d, mj_d, attr, name, atol):
|
||||
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
|
||||
err_msg = f'mismatch: {attr} in run: {name}'
|
||||
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))
|
||||
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
|
||||
)
|
||||
mjx_contact = mjx_contact.replace(
|
||||
dim=mjx_contact.dim[idx], efc_address=mjx_contact.efc_address[idx]
|
||||
)
|
||||
for field in dataclasses.fields(Contact):
|
||||
_assert_attr_eq(mjx_contact, d.contact, field.name, seed, 1e-7)
|
||||
|
||||
|
||||
@@ -68,10 +68,10 @@ class TransmissionIntegrationTest(parameterized.TestCase):
|
||||
mujoco.mj_transmission(m, d)
|
||||
dx = transmission_jit_fn(mx, dx)
|
||||
|
||||
_assert_attr_eq(d, dx, 'actuator_length', seed, f'transmission{seed}')
|
||||
_assert_attr_eq(
|
||||
d, dx, 'actuator_moment', seed, f'transmission{seed}', atol=1e-4
|
||||
)
|
||||
for field in ['actuator_length', 'actuator_moment']:
|
||||
_assert_attr_eq(
|
||||
d, dx, field, seed, f'transmission{seed}', atol=1e-4
|
||||
)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
|
||||
@@ -14,7 +14,7 @@
|
||||
</default>
|
||||
|
||||
<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="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 -->
|
||||
<geom class="box"/>
|
||||
</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>
|
||||
|
||||
<equality>
|
||||
|
||||
@@ -110,6 +110,20 @@
|
||||
</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>
|
||||
|
||||
<actuator>
|
||||
@@ -118,5 +132,8 @@
|
||||
<motor gear="0 0 300" joint="joint1" name="act3"/>
|
||||
<motor gear="275" joint="joint2" name="act4"/>
|
||||
<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>
|
||||
</mujoco>
|
||||
|
||||
@@ -47,8 +47,9 @@ _OUTPUT = flags.DEFINE_enum(
|
||||
|
||||
def _main(argv: Sequence[str]):
|
||||
"""Runs testpeed function."""
|
||||
base_path = _BASE_PATH.value or epath.resource_path('mujoco.mjx')
|
||||
f = base_path / 'test_data' / _MJCF.value
|
||||
path = epath.resource_path('mujoco.mjx') / 'test_data'
|
||||
path = _BASE_PATH.value or path
|
||||
f = epath.Path(path) / _MJCF.value
|
||||
m = mujoco.MjModel.from_xml_path(f.as_posix())
|
||||
|
||||
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()
|
||||
|
||||
# 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
|
||||
mx = mx.tree_replace({
|
||||
'opt.gravity': m.opt.gravity,
|
||||
|
||||
+4
-4
@@ -4,7 +4,7 @@ build-backend = "setuptools.build_meta"
|
||||
|
||||
[project]
|
||||
name="mujoco-mjx"
|
||||
version = "3.1.4"
|
||||
version = "3.1.5"
|
||||
authors = [
|
||||
{name = "Google DeepMind", email = "mujoco@deepmind.com"},
|
||||
]
|
||||
@@ -31,7 +31,7 @@ dependencies = [
|
||||
"etils[epath]",
|
||||
"jax",
|
||||
"jaxlib",
|
||||
"mujoco>=3.1.4.dev0",
|
||||
"mujoco>=3.1.5.dev0",
|
||||
"scipy",
|
||||
"trimesh",
|
||||
]
|
||||
@@ -42,6 +42,6 @@ mjx-viewer = "mujoco.mjx.viewer:main"
|
||||
|
||||
[project.urls]
|
||||
Homepage = "https://github.com/google-deepmind/mujoco/tree/main/mjx"
|
||||
Documentation = "https://mujoco.readthedocs.io/en/3.1.4"
|
||||
Documentation = "https://mujoco.readthedocs.io/en/3.1.5"
|
||||
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",
|
||||
" friction, gain, bias = rand(rng)\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",
|
||||
" 'geom_friction': 0,\n",
|
||||
" 'actuator_gainprm': 0,\n",
|
||||
|
||||
+163
-2211
File diff suppressed because it is too large
Load Diff
@@ -27,20 +27,20 @@
|
||||
|
||||
<default>
|
||||
<geom solimp=".9 .9 .01"/>
|
||||
<default class="object1">
|
||||
<geom type="capsule" material="object1" size="0.1 0.05"/>
|
||||
<default class="capsule">
|
||||
<geom type="capsule" material="capsule" size="0.1 0.05"/>
|
||||
</default>
|
||||
<default class="object2">
|
||||
<geom type="ellipsoid" material="object2" size="0.15 0.1 0.07"/>
|
||||
<default class="ellipsoid">
|
||||
<geom type="ellipsoid" material="ellipsoid" size="0.15 0.1 0.07"/>
|
||||
</default>
|
||||
<default class="object3">
|
||||
<geom type="box" material="object3" size="0.15 0.1 0.05"/>
|
||||
<default class="box">
|
||||
<geom type="box" material="box" size="0.15 0.1 0.05"/>
|
||||
</default>
|
||||
<default class="object4">
|
||||
<geom type="cylinder" material="object4" size="0.1 0.05" condim="4" friction="1 .01 .01"/>
|
||||
<default class="cylinder">
|
||||
<geom type="cylinder" material="cylinder" size="0.1 0.05" condim="4" friction="1 .01 .01"/>
|
||||
</default>
|
||||
<default class="object5">
|
||||
<geom type="sphere" material="object5" size="0.1"/>
|
||||
<default class="sphere">
|
||||
<geom type="sphere" material="sphere" size="0.1"/>
|
||||
</default>
|
||||
<default class="border">
|
||||
<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"/>
|
||||
<texture name="texplane" type="2d" builtin="checker" rgb1=".4 .4 .4" rgb2=".6 .6 .6"
|
||||
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"/>
|
||||
<material name='object1' texture="texgeom" texuniform="true" rgba=".4 .9 .6 1" />
|
||||
<material name='object2' texture="texgeom" texuniform="true" rgba=".4 .6 .9 1" />
|
||||
<material name='object3' texture="texgeom" texuniform="true" rgba=".4 .9 .9 1" />
|
||||
<material name='object4' texture="texgeom" texuniform="true" rgba=".8 .6 .8 1" />
|
||||
<material name='object5' texture="texgeom" texuniform="true" rgba=".9 .1 .1 1" />
|
||||
<material name="capsule" texture="texgeom" texuniform="true" rgba=".4 .9 .6 1" />
|
||||
<material name="ellipsoid" texture="texgeom" texuniform="true" rgba=".4 .6 .9 1" />
|
||||
<material name="box" texture="texgeom" texuniform="true" rgba=".4 .9 .9 1" />
|
||||
<material name="cylinder" texture="texgeom" texuniform="true" rgba=".8 .6 .8 1" />
|
||||
<material name="sphere" texture="texgeom" texuniform="true" rgba=".9 .1 .1 1" />
|
||||
</asset>
|
||||
|
||||
<visual>
|
||||
@@ -84,430 +84,70 @@
|
||||
<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">
|
||||
<freejoint/>
|
||||
<geom class="object1"/>
|
||||
</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>
|
||||
<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/>
|
||||
<geom class="capsule"/>
|
||||
</body>
|
||||
</replicate>
|
||||
</frame>
|
||||
</replicate>
|
||||
</frame>
|
||||
|
||||
<body pos="-2 -1 1" quat="-0.755929 -0.377964 0.377964 0.377964">
|
||||
<freejoint/>
|
||||
<geom class="object1"/>
|
||||
</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>
|
||||
<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/>
|
||||
<geom class="ellipsoid"/>
|
||||
</body>
|
||||
</replicate>
|
||||
</frame>
|
||||
</replicate>
|
||||
</frame>
|
||||
|
||||
<body pos="-2 0 1" quat="-0.816497 0 0.408248 0.408248">
|
||||
<freejoint/>
|
||||
<geom class="object1"/>
|
||||
</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>
|
||||
<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/>
|
||||
<geom class="box"/>
|
||||
</body>
|
||||
</replicate>
|
||||
</frame>
|
||||
</replicate>
|
||||
</frame>
|
||||
|
||||
<body pos="-2 1 1" quat="-0.755929 0.377964 0.377964 0.377964">
|
||||
<freejoint/>
|
||||
<geom class="object1"/>
|
||||
</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>
|
||||
<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/>
|
||||
<geom class="cylinder"/>
|
||||
</body>
|
||||
</replicate>
|
||||
</frame>
|
||||
</replicate>
|
||||
</frame>
|
||||
|
||||
<body pos="-2 2 1" quat="-0.632456 0.632456 0.316228 0.316228">
|
||||
<freejoint/>
|
||||
<geom class="object1"/>
|
||||
</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 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>
|
||||
<frame pos="2 -2 2.5">
|
||||
<replicate count="5" offset="0 1 0">
|
||||
<frame pos="0 0 -1.5">
|
||||
<replicate count="4" offset="0 0 1">
|
||||
<body>
|
||||
<freejoint/>
|
||||
<geom class="sphere"/>
|
||||
</body>
|
||||
</replicate>
|
||||
</frame>
|
||||
</replicate>
|
||||
</frame>
|
||||
|
||||
</worldbody>
|
||||
</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)
|
||||
message("MuJoCo framework is at ${MUJOCO_FRAMEWORK}/mujoco.framework")
|
||||
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})
|
||||
endif()
|
||||
@@ -92,7 +92,7 @@ if(NOT TARGET mujoco)
|
||||
|
||||
if(NOT MUJOCO_FRAMEWORK)
|
||||
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)
|
||||
message("MuJoCo is at ${MUJOCO_LIBRARY}")
|
||||
@@ -191,7 +191,7 @@ findorfetch(
|
||||
GIT_REPO
|
||||
https://github.com/pybind/pybind11
|
||||
GIT_TAG
|
||||
8a099e44b3d5f85b20f05828d919d2332a8de841 # v2.11.1
|
||||
3e9dfa2866941655c56877882565e7577de6fc7b # v2.12.0
|
||||
TARGETS
|
||||
pybind11::pybind11_headers
|
||||
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.]])
|
||||
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))
|
||||
def test_mj_printSchema(self, flg_html, flg_pad): # pylint: disable=invalid-name
|
||||
# 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_defaultVisual>(pymodule);
|
||||
// Skipped: mj_copyModel (have MjModel.__copy__, memory managed by MjModel)
|
||||
DEF_WITH_OMITTED_PY_ARGS(traits::mj_saveModel, "buffer_sz")(
|
||||
pymodule,
|
||||
[](const raw::MjModel* m, const std::optional<std::string>& filename,
|
||||
pymodule.def(
|
||||
"mj_saveModel",
|
||||
[](const MjModelWrapper& m, const std::optional<std::string>& filename = std::nullopt,
|
||||
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;
|
||||
int buffer_sz = 0;
|
||||
if (buffer.has_value()) {
|
||||
@@ -118,9 +118,13 @@ PYBIND11_MODULE(_functions, pymodule) {
|
||||
buffer_sz = buffer->size();
|
||||
}
|
||||
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);
|
||||
});
|
||||
},
|
||||
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_deleteModel (have MjModel.__del__)
|
||||
Def<traits::mj_sizeModel>(pymodule);
|
||||
@@ -995,6 +999,7 @@ PYBIND11_MODULE(_functions, pymodule) {
|
||||
Def<traits::mju_derivQuat>(pymodule);
|
||||
Def<traits::mju_quatIntegrate>(pymodule);
|
||||
Def<traits::mju_quatZ2Vec>(pymodule);
|
||||
Def<traits::mju_euler2Quat>(pymodule);
|
||||
|
||||
// Poses
|
||||
Def<traits::mju_mulPose>(pymodule);
|
||||
|
||||
@@ -7,13 +7,13 @@
|
||||
<key>CFBundleIdentifier</key>
|
||||
<string>org.mujoco.mjpython</string>
|
||||
<key>CFBundleVersion</key>
|
||||
<string>3.1.4</string>
|
||||
<string>3.1.5</string>
|
||||
<key>CFBundleGetInfoString</key>
|
||||
<string>3.1.4</string>
|
||||
<string>3.1.5</string>
|
||||
<key>CFBundleLongVersionString</key>
|
||||
<string>3.1.4</string>
|
||||
<string>3.1.5</string>
|
||||
<key>CFBundleShortVersionString</key>
|
||||
<string>3.1.4</string>
|
||||
<string>3.1.5</string>
|
||||
<key>CFBundleExecutable</key>
|
||||
<string>mjpython</string>
|
||||
<key>CFBundleIconFile</key>
|
||||
|
||||
@@ -78,8 +78,11 @@ the clause:
|
||||
|
||||
# Create render contexts.
|
||||
# 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.make_current()
|
||||
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._mjr_context = _render.MjrContext(
|
||||
model, _enums.mjtFontScale.mjFONTSCALE_150.value
|
||||
)
|
||||
@@ -148,9 +151,11 @@ the clause:
|
||||
self._scene.flags[_enums.mjtRndFlag.mjRND_SEGMENT] = 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.')
|
||||
self._gl_context.make_current()
|
||||
|
||||
if self._gl_context:
|
||||
self._gl_context.make_current()
|
||||
|
||||
if self._depth_rendering:
|
||||
out_shape = (self._height, self._width)
|
||||
|
||||
@@ -2226,6 +2226,7 @@ This is useful for example when the MJB is not available as a file on disk.)"));
|
||||
X(headlight);
|
||||
X(directional);
|
||||
X(castshadow);
|
||||
X(bulbradius);
|
||||
#undef X
|
||||
|
||||
#define X(var) DefinePyArray(mjvLight, #var, &MjvLightWrapper::var)
|
||||
|
||||
@@ -4,7 +4,7 @@ build-backend = "setuptools.build_meta"
|
||||
|
||||
[project]
|
||||
name = "mujoco"
|
||||
version = "3.1.4"
|
||||
version = "3.1.5"
|
||||
authors = [
|
||||
{name = "Google DeepMind", email = "mujoco@deepmind.com"},
|
||||
]
|
||||
@@ -36,9 +36,9 @@ dynamic = ["readme", "scripts"]
|
||||
|
||||
[project.urls]
|
||||
Homepage = "https://github.com/google-deepmind/mujoco"
|
||||
Documentation = "https://mujoco.readthedocs.io/en/3.1.4"
|
||||
Documentation = "https://mujoco.readthedocs.io/en/3.1.5"
|
||||
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]
|
||||
include-package-data = false
|
||||
|
||||
@@ -24,7 +24,7 @@ set(MSVC_INCREMENTAL_DEFAULT ON)
|
||||
|
||||
project(
|
||||
mujoco_samples
|
||||
VERSION 3.1.4
|
||||
VERSION 3.1.5
|
||||
DESCRIPTION "MuJoCo samples binaries"
|
||||
HOMEPAGE_URL "https://mujoco.org"
|
||||
)
|
||||
|
||||
+2
-1
@@ -89,7 +89,8 @@ int filetype(const char* filename) {
|
||||
|
||||
|
||||
// main function
|
||||
int main(int argc, const char** argv) {
|
||||
int main(int argc, char** argv) {
|
||||
|
||||
// model and error
|
||||
mjModel* m = 0;
|
||||
char error[1000];
|
||||
|
||||
@@ -29,7 +29,7 @@ set(MUJOCO_DEP_VERSION_lodepng
|
||||
|
||||
project(
|
||||
mujoco_simulate
|
||||
VERSION 3.1.4
|
||||
VERSION 3.1.5
|
||||
DESCRIPTION "MuJoCo simulate binaries"
|
||||
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;
|
||||
|
||||
case mjGEOM_MESH:
|
||||
case mjGEOM_SDF:
|
||||
// init search
|
||||
vertdata = m->mesh_vert + 3*m->mesh_vertadr[m->geom_dataid[g]];
|
||||
tmp = -1E+10;
|
||||
|
||||
@@ -40,7 +40,7 @@
|
||||
// table of pair-wise collision functions
|
||||
mjfCollision mjCOLLISIONFUNC[mjNGEOMTYPES][mjNGEOMTYPES] = {
|
||||
/* 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},
|
||||
/*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},
|
||||
|
||||
+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)
|
||||
void mj_fwdActuation(const mjModel* m, mjData* d) {
|
||||
TM_START;
|
||||
@@ -469,7 +484,7 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
|
||||
mju_mulMatTVec(d->qfrc_actuator, moment, force, nu, nv);
|
||||
|
||||
// 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;
|
||||
for (int i=0; i < njnt; i++) {
|
||||
// skip if gravcomp added as passive force
|
||||
@@ -477,28 +492,10 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
|
||||
continue;
|
||||
}
|
||||
|
||||
// get number of dofs for this joint
|
||||
int dofnum;
|
||||
switch (m->jnt_type[i]) {
|
||||
case mjJNT_HINGE:
|
||||
case mjJNT_SLIDE:
|
||||
dofnum = 1;
|
||||
break;
|
||||
|
||||
case mjJNT_BALL:
|
||||
dofnum = 3;
|
||||
break;
|
||||
|
||||
case mjJNT_FREE:
|
||||
dofnum = 6;
|
||||
break;
|
||||
}
|
||||
|
||||
// add gravcomp force
|
||||
int dofnum = jnt_dofnum(m->jnt_type[i]);
|
||||
int dofadr = m->jnt_dofadr[i];
|
||||
for (int j=0; j < dofnum; j++) {
|
||||
d->qfrc_actuator[dofadr+j] += d->qfrc_gravcomp[dofadr+j];
|
||||
}
|
||||
mju_addTo(d->qfrc_actuator + dofadr, d->qfrc_gravcomp + dofadr, dofnum);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
+17
-7
@@ -534,11 +534,15 @@ mjModel* mj_makeModel(
|
||||
m->nuser_actuator = nuser_actuator;
|
||||
m->nuser_sensor = nuser_sensor;
|
||||
m->nnames = nnames;
|
||||
m->nnames_map = mjLOAD_MULTIPLE
|
||||
* (nbody + njnt + ngeom + nsite + ncam + nlight + nflex + nmesh
|
||||
+ nskin + nhfield + ntex + nmat + npair + nexclude + neq
|
||||
+ ntendon + nu + nsensor + nnumeric + ntext + ntuple
|
||||
+ nkey + nplugin);
|
||||
long nnames_map = (long)nbody + njnt + ngeom + nsite + ncam + nlight + nflex + nmesh + nskin +
|
||||
nhfield + ntex + nmat + npair + nexclude + neq + ntendon + nu + nsensor +
|
||||
nnumeric + ntext + ntuple + nkey + nplugin;
|
||||
if (nnames_map >= INT_MAX / mjLOAD_MULTIPLE) {
|
||||
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;
|
||||
|
||||
#define X(name) \
|
||||
@@ -1520,14 +1524,20 @@ void* mj_stackAllocByte(mjData* d, size_t bytes, size_t alignment) {
|
||||
|
||||
|
||||
// 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));
|
||||
}
|
||||
|
||||
|
||||
|
||||
// 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));
|
||||
}
|
||||
|
||||
|
||||
@@ -129,10 +129,10 @@ void mj__freeStack(mjData* d) __attribute__((noinline));
|
||||
MJAPI void* mj_stackAllocByte(mjData* d, size_t bytes, size_t alignment);
|
||||
|
||||
// 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
|
||||
MJAPI int* mj_stackAllocInt(mjData* d, int size);
|
||||
MJAPI int* mj_stackAllocInt(mjData* d, size_t size);
|
||||
|
||||
// de-allocate data
|
||||
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
|
||||
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;
|
||||
}
|
||||
|
||||
|
||||
@@ -1033,11 +1033,31 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename,
|
||||
fprintf(fp, "CONTACT\n");
|
||||
for (int i=0; i < d->ncon; i++) {
|
||||
fprintf(fp, " %d:\n dim %d\n", i, d->contact[i].dim);
|
||||
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].elem[0], d->contact[i].vert[0],
|
||||
d->contact[i].geom[1], d->contact[i].flex[1],
|
||||
d->contact[i].elem[1], d->contact[i].vert[1]);
|
||||
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",
|
||||
d->contact[i].geom[0], d->contact[i].flex[0],
|
||||
d->contact[i].elem[0], d->contact[i].vert[0],
|
||||
d->contact[i].geom[1], d->contact[i].flex[1],
|
||||
d->contact[i].elem[1], d->contact[i].vert[1]);
|
||||
}
|
||||
fprintf(fp, " exclude %d\n efc_address %d\n",
|
||||
d->contact[i].exclude, d->contact[i].efc_address);
|
||||
printVector(" solref ", d->contact[i].solref, mjNREF, fp, float_format);
|
||||
|
||||
@@ -15,6 +15,7 @@
|
||||
#include "engine/engine_setconst.h"
|
||||
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
|
||||
#include <mujoco/mjdata.h>
|
||||
#include <mujoco/mjmacro.h>
|
||||
@@ -92,6 +93,11 @@ static void set0(mjModel* m, mjData* d) {
|
||||
// compute dof_M0 for CRB algorithm
|
||||
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
|
||||
mj_crb(m, d);
|
||||
mj_factorM(m, d);
|
||||
@@ -99,6 +105,10 @@ static void set0(mjModel* m, mjData* d) {
|
||||
mj_tendon(m, d);
|
||||
mj_transmission(m, d);
|
||||
|
||||
// restore flex rigidity
|
||||
memcpy(m->flex_rigid, rigid, m->nflex);
|
||||
mju_free(rigid);
|
||||
|
||||
// restore camera and light mode
|
||||
for (int i=0; i < m->ncam; i++) {
|
||||
m->cam_mode[i] = cammode[i];
|
||||
|
||||
@@ -38,8 +38,8 @@
|
||||
|
||||
//-------------------------- Constants -------------------------------------------------------------
|
||||
|
||||
#define mjVERSION 314
|
||||
#define mjVERSIONSTRING "3.1.4"
|
||||
#define mjVERSION 315
|
||||
#define mjVERSIONSTRING "3.1.5"
|
||||
|
||||
// names of disable flags
|
||||
const char* mjDISABLESTRING[mjNDISABLE] = {
|
||||
|
||||
@@ -511,3 +511,41 @@ void mju_makeFrame(mjtNum frame[9]) {
|
||||
// zaxis = cross(xaxis, yaxis)
|
||||
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],
|
||||
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 ---------------------------------------------------
|
||||
|
||||
|
||||
@@ -2091,6 +2091,7 @@ void mjv_makeLights(const mjModel* m, mjData* d, mjvScene* scn) {
|
||||
memset(thislight, 0, sizeof(mjvLight));
|
||||
thislight->directional = m->light_directional[i];
|
||||
thislight->castshadow = m->light_castshadow[i];
|
||||
thislight->bulbradius = m->light_bulbradius[i];
|
||||
if (!thislight->directional) {
|
||||
f2f(thislight->attenuation, m->light_attenuation+3*i, 3);
|
||||
thislight->exponent = m->light_exponent[i];
|
||||
|
||||
@@ -21,7 +21,7 @@ set(MUJOCO_USER_SRCS
|
||||
user_composite.h
|
||||
user_flexcomp.cc
|
||||
user_flexcomp.h
|
||||
user_init.cc
|
||||
user_init.c
|
||||
user_mesh.cc
|
||||
user_model.cc
|
||||
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
|
||||
const char* mjs_getError(mjSpec* s) {
|
||||
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
|
||||
int mjs_isWarning(mjSpec* s) {
|
||||
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
|
||||
mjsBody* mjs_addBody(mjsBody* bodyspec, mjsDefault* defspec) {
|
||||
mjCDef* def = defspec ? static_cast<mjCDef*>(defspec->element) : 0;
|
||||
@@ -650,11 +680,12 @@ void mjs_setActivePlugins(mjSpec* s, void* activeplugins) {
|
||||
|
||||
|
||||
// compute full inertia
|
||||
const char* mjs_setFullInertia(mjsBody* bodyspec, double quat[4], double inertia[3]) {
|
||||
mjCBody* body = static_cast<mjCBody*>(bodyspec->element);
|
||||
return body->FullInertia(quat, inertia);
|
||||
const char* mjs_fullInertia(double quat[4], double inertia[3], const double fullinertia[6]) {
|
||||
return FullInertia(quat, inertia, fullinertia);
|
||||
}
|
||||
|
||||
|
||||
|
||||
// -------------------------- GLOBAL ASSET CACHE -------------------------------
|
||||
|
||||
void mj_setCacheSize(mjCache cache, std::size_t size) {
|
||||
|
||||
+221
-170
@@ -15,6 +15,7 @@
|
||||
#ifndef MUJOCO_SRC_USER_USER_API_H_
|
||||
#define MUJOCO_SRC_USER_USER_API_H_
|
||||
|
||||
#include <math.h>
|
||||
#include <stddef.h>
|
||||
#include <mujoco/mjexport.h>
|
||||
#include <mujoco/mjmodel.h>
|
||||
@@ -26,6 +27,8 @@
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
#define mjNAN NAN // used to mark undefined fields
|
||||
|
||||
|
||||
//---------------------------------- handles to internal objects -----------------------------------
|
||||
|
||||
@@ -40,42 +43,51 @@ typedef struct _mjDoubleVec* mjDoubleVec;
|
||||
|
||||
//---------------------------------- enum types (mjt) ----------------------------------------------
|
||||
|
||||
typedef enum _mjtGeomInertia { // type of inertia inference
|
||||
mjINERTIA_VOLUME, // mass distributed in the volume
|
||||
mjINERTIA_SHELL, // mass distributed on the surface
|
||||
typedef enum _mjtGeomInertia { // type of inertia inference
|
||||
mjINERTIA_VOLUME, // mass distributed in the volume
|
||||
mjINERTIA_SHELL, // mass distributed on the surface
|
||||
} mjtGeomInertia;
|
||||
|
||||
|
||||
typedef enum _mjtBuiltin { // type of built-in procedural texture
|
||||
mjBUILTIN_NONE = 0, // no built-in texture
|
||||
mjBUILTIN_GRADIENT, // gradient: rgb1->rgb2
|
||||
mjBUILTIN_CHECKER, // checker pattern: rgb1, rgb2
|
||||
mjBUILTIN_FLAT // 2d: rgb1; cube: rgb1-up, rgb2-side, rgb3-down
|
||||
typedef enum _mjtBuiltin { // type of built-in procedural texture
|
||||
mjBUILTIN_NONE = 0, // no built-in texture
|
||||
mjBUILTIN_GRADIENT, // gradient: rgb1->rgb2
|
||||
mjBUILTIN_CHECKER, // checker pattern: rgb1, rgb2
|
||||
mjBUILTIN_FLAT // 2d: rgb1; cube: rgb1-up, rgb2-side, rgb3-down
|
||||
} mjtBuiltin;
|
||||
|
||||
|
||||
typedef enum _mjtMark { // mark type for procedural textures
|
||||
mjMARK_NONE = 0, // no mark
|
||||
mjMARK_EDGE, // edges
|
||||
mjMARK_CROSS, // cross
|
||||
mjMARK_RANDOM // random dots
|
||||
typedef enum _mjtMark { // mark type for procedural textures
|
||||
mjMARK_NONE = 0, // no mark
|
||||
mjMARK_EDGE, // edges
|
||||
mjMARK_CROSS, // cross
|
||||
mjMARK_RANDOM // random dots
|
||||
} mjtMark;
|
||||
|
||||
|
||||
typedef enum _mjtLimited { // type of limit specification
|
||||
mjLIMITED_FALSE = 0, // not limited
|
||||
mjLIMITED_TRUE, // limited
|
||||
mjLIMITED_AUTO, // limited inferred from presence of range
|
||||
typedef enum _mjtLimited { // type of limit specification
|
||||
mjLIMITED_FALSE = 0, // not limited
|
||||
mjLIMITED_TRUE, // limited
|
||||
mjLIMITED_AUTO, // limited inferred from presence of range
|
||||
} mjtLimited;
|
||||
|
||||
|
||||
typedef enum _mjtInertiaFromGeom {
|
||||
mjINERTIAFROMGEOM_FALSE = 0, // do not use; inertial element required
|
||||
mjINERTIAFROMGEOM_TRUE, // always use; overwrite inertial element
|
||||
mjINERTIAFROMGEOM_AUTO // use only if inertial element is missing
|
||||
typedef enum _mjtInertiaFromGeom { // whether to infer body inertias from child geoms
|
||||
mjINERTIAFROMGEOM_FALSE = 0, // do not use; inertial element required
|
||||
mjINERTIAFROMGEOM_TRUE, // always use; overwrite inertial element
|
||||
mjINERTIAFROMGEOM_AUTO // use only if inertial element is missing
|
||||
} mjtInertiaFromGeom;
|
||||
|
||||
|
||||
typedef 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) ---------------------------------------
|
||||
|
||||
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
|
||||
mjElement* element; // object type
|
||||
mjStatistic stat; // statistics override (if defined)
|
||||
mjElement* element; // element type
|
||||
mjString modelname; // model name
|
||||
|
||||
// compiler settings
|
||||
mjtByte autolimits; // infer "limited" attribute based on range
|
||||
@@ -109,46 +121,47 @@ typedef struct _mjSpec { // model specification
|
||||
mjLROpt LRopt; // options for lengthrange computation
|
||||
|
||||
// engine data
|
||||
mjString modelname; // model name
|
||||
mjOption option; // options
|
||||
mjVisual visual; // visual options
|
||||
size_t memory; // size of arena+stack memory in bytes
|
||||
int nemax; // max number of equality constraints
|
||||
int njmax; // max number of constraints (Jacobian rows)
|
||||
int nconmax; // max number of detected contacts (mjContact array size)
|
||||
size_t nstack; // (deprecated) number of fields in mjData stack
|
||||
int nuserdata; // number extra fields in mjData
|
||||
int nuser_body; // number of mjtNums in body_user
|
||||
int nuser_jnt; // number of mjtNums in jnt_user
|
||||
int nuser_geom; // number of mjtNums in geom_user
|
||||
int nuser_site; // number of mjtNums in site_user
|
||||
int nuser_cam; // number of mjtNums in cam_user
|
||||
int nuser_tendon; // number of mjtNums in tendon_user
|
||||
int nuser_actuator; // number of mjtNums in actuator_user
|
||||
int nuser_sensor; // number of mjtNums in sensor_user
|
||||
mjOption option; // physics options
|
||||
mjVisual visual; // visual options
|
||||
mjStatistic stat; // statistics override (if defined)
|
||||
|
||||
// sizes
|
||||
int nkey; // number of keyframes
|
||||
size_t memory; // number of bytes in arena+stack memory
|
||||
int nemax; // max number of equality constraints
|
||||
int nuserdata; // number of mjtNums in userdata
|
||||
int nuser_body; // number of mjtNums in body_user
|
||||
int nuser_jnt; // number of mjtNums in jnt_user
|
||||
int nuser_geom; // number of mjtNums in geom_user
|
||||
int nuser_site; // number of mjtNums in site_user
|
||||
int nuser_cam; // number of mjtNums in cam_user
|
||||
int nuser_tendon; // number of mjtNums in tendon_user
|
||||
int nuser_actuator; // number of mjtNums in actuator_user
|
||||
int nuser_sensor; // number of mjtNums in sensor_user
|
||||
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
|
||||
mjString comment; // comment at top of XML
|
||||
mjString modelfiledir; // path to model file
|
||||
mjString comment; // comment at top of XML
|
||||
mjString modelfiledir; // path to model file
|
||||
|
||||
// other
|
||||
bool hasImplicitPluginElem; // already encountered an implicit plugin sensor/actuator
|
||||
mjtByte hasImplicitPluginElem; // already encountered an implicit plugin sensor/actuator
|
||||
} mjSpec;
|
||||
|
||||
|
||||
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 zaxis[3]; // z axis (use minimal rotation)
|
||||
double euler[3]; // euler angles
|
||||
double zaxis[3]; // z axis (minimal rotation)
|
||||
double euler[3]; // Euler angles
|
||||
} mjsOrientation;
|
||||
|
||||
|
||||
typedef struct _mjsPlugin { // plugin specification
|
||||
mjElement* instance; // object type
|
||||
mjElement* instance; // element type
|
||||
mjString name; // name
|
||||
mjString instance_name; // instance name
|
||||
int plugin_slot; // global registered slot number of the plugin
|
||||
@@ -158,7 +171,7 @@ typedef struct _mjsPlugin { // plugin specification
|
||||
|
||||
|
||||
typedef struct _mjsBody { // body specification
|
||||
mjElement* element; // object type
|
||||
mjElement* element; // element type
|
||||
mjString name; // name
|
||||
mjString childclass; // childclass name
|
||||
|
||||
@@ -186,7 +199,7 @@ typedef struct _mjsBody { // body specification
|
||||
|
||||
|
||||
typedef struct _mjsFrame { // frame specification
|
||||
mjElement* element; // object type
|
||||
mjElement* element; // element type
|
||||
mjString name; // name
|
||||
mjString childclass; // childclass name
|
||||
double pos[3]; // position
|
||||
@@ -197,7 +210,7 @@ typedef struct _mjsFrame { // frame specification
|
||||
|
||||
|
||||
typedef struct _mjsJoint { // joint specification
|
||||
mjElement* element; // object type
|
||||
mjElement* element; // element type
|
||||
mjString name; // name
|
||||
mjString classname; // class name
|
||||
mjtJoint type; // joint type
|
||||
@@ -238,7 +251,7 @@ typedef struct _mjsJoint { // joint specification
|
||||
|
||||
|
||||
typedef struct _mjsGeom { // geom specification
|
||||
mjElement* element; // object type
|
||||
mjElement* element; // element type
|
||||
mjString name; // name
|
||||
mjString classname; // classname
|
||||
mjtGeom type; // geom type
|
||||
@@ -287,7 +300,7 @@ typedef struct _mjsGeom { // geom specification
|
||||
|
||||
|
||||
typedef struct _mjsSite { // site specification
|
||||
mjElement* element; // object type
|
||||
mjElement* element; // element type
|
||||
mjString name; // name
|
||||
mjString classname; // class name
|
||||
|
||||
@@ -311,7 +324,7 @@ typedef struct _mjsSite { // site specification
|
||||
|
||||
|
||||
typedef struct _mjsCamera { // camera specification
|
||||
mjElement* element; // object type
|
||||
mjElement* element; // element type
|
||||
mjString name; // name
|
||||
mjString classname; // class name
|
||||
|
||||
@@ -340,7 +353,7 @@ typedef struct _mjsCamera { // camera specification
|
||||
|
||||
|
||||
typedef struct _mjsLight { // light specification
|
||||
mjElement* element; // object type
|
||||
mjElement* element; // element type
|
||||
mjString name; // name
|
||||
mjString classname; // class name
|
||||
|
||||
@@ -354,6 +367,7 @@ typedef struct _mjsLight { // light specification
|
||||
mjtByte active; // is light active
|
||||
mjtByte directional; // is light directional or spot
|
||||
mjtByte castshadow; // does light cast shadows
|
||||
double bulbradius; // bulb radius, for soft shadows
|
||||
float attenuation[3]; // OpenGL attenuation (quadratic model)
|
||||
float cutoff; // OpenGL cutoff
|
||||
float exponent; // OpenGL exponent
|
||||
@@ -367,7 +381,7 @@ typedef struct _mjsLight { // light specification
|
||||
|
||||
|
||||
typedef struct _mjsFlex {
|
||||
mjElement* element; // object type
|
||||
mjElement* element; // element type
|
||||
mjString name; // name
|
||||
mjString classname; // class name
|
||||
|
||||
@@ -408,7 +422,7 @@ typedef struct _mjsFlex {
|
||||
|
||||
|
||||
typedef struct _mjsMesh { // mesh specification
|
||||
mjElement* element; // object type
|
||||
mjElement* element; // element type
|
||||
mjString name; // name
|
||||
mjString classname; // class name
|
||||
mjString content_type; // content type of file
|
||||
@@ -429,7 +443,7 @@ typedef struct _mjsMesh { // mesh specification
|
||||
|
||||
|
||||
typedef struct _mjsHField { // height field specification
|
||||
mjElement* element; // object type
|
||||
mjElement* element; // element type
|
||||
mjString name; // name
|
||||
mjString content_type; // content type of file
|
||||
mjString file; // file: (nrow, ncol, [elevation data])
|
||||
@@ -443,7 +457,7 @@ typedef struct _mjsHField { // height field specification
|
||||
|
||||
|
||||
typedef struct _mjsSkin { // skin specification
|
||||
mjElement* element; // object type
|
||||
mjElement* element; // element type
|
||||
mjString name; // name
|
||||
mjString classname; // class name
|
||||
mjString file; // skin file
|
||||
@@ -470,7 +484,7 @@ typedef struct _mjsSkin { // skin specification
|
||||
|
||||
|
||||
typedef struct _mjsTexture { // texture specification
|
||||
mjElement* element; // object type
|
||||
mjElement* element; // element type
|
||||
mjString name; // name
|
||||
mjString classname; // class name
|
||||
mjtTexture type; // texture type
|
||||
@@ -504,7 +518,7 @@ typedef struct _mjsTexture { // texture specification
|
||||
|
||||
|
||||
typedef struct _mjsMaterial { // material specification
|
||||
mjElement* element; // object type
|
||||
mjElement* element; // element type
|
||||
mjString name; // name
|
||||
mjString classname; // class name
|
||||
mjString texture; // name of texture (empty: none)
|
||||
@@ -514,13 +528,15 @@ typedef struct _mjsMaterial { // material specification
|
||||
float specular; // specular
|
||||
float shininess; // shininess
|
||||
float reflectance; // reflectance
|
||||
float metallic; // metallic
|
||||
float roughness; // roughness
|
||||
float rgba[4]; // rgba
|
||||
mjString info; // message appended to compiler errors
|
||||
} mjsMaterial;
|
||||
|
||||
|
||||
typedef struct _mjsPair {
|
||||
mjElement* element; // object type
|
||||
mjElement* element; // element type
|
||||
mjString name; // name
|
||||
mjString classname; // class name
|
||||
mjString geomname1; // name of geom 1
|
||||
@@ -539,7 +555,7 @@ typedef struct _mjsPair {
|
||||
|
||||
|
||||
typedef struct _mjsExclude {
|
||||
mjElement* element; // object type
|
||||
mjElement* element; // element type
|
||||
mjString name; // name
|
||||
mjString bodyname1; // name of geom 1
|
||||
mjString bodyname2; // name of geom 2
|
||||
@@ -548,7 +564,7 @@ typedef struct _mjsExclude {
|
||||
|
||||
|
||||
typedef struct _mjsEquality { // equality specification
|
||||
mjElement* element; // object type
|
||||
mjElement* element; // element type
|
||||
mjString name; // name
|
||||
mjString classname; // class name
|
||||
mjtEq type; // constraint type
|
||||
@@ -563,7 +579,7 @@ typedef struct _mjsEquality { // equality specification
|
||||
|
||||
|
||||
typedef struct _mjsTendon { // tendon specification
|
||||
mjElement* element; // object type
|
||||
mjElement* element; // element type
|
||||
mjString name; // name
|
||||
mjString classname; // class name
|
||||
|
||||
@@ -595,13 +611,13 @@ typedef struct _mjsTendon { // tendon specification
|
||||
|
||||
|
||||
typedef struct _mjsWrap { // wrapping object specification
|
||||
mjElement* element; // object type
|
||||
mjElement* element; // element type
|
||||
mjString info; // message appended to errors
|
||||
} mjsWrap;
|
||||
|
||||
|
||||
typedef struct _mjsActuator { // actuator specification
|
||||
mjElement* element; // object type
|
||||
mjElement* element; // element type
|
||||
mjString name; // name
|
||||
mjString classname; // class name
|
||||
|
||||
@@ -645,7 +661,7 @@ typedef struct _mjsActuator { // actuator specification
|
||||
|
||||
|
||||
typedef struct _mjsSensor { // sensor specification
|
||||
mjElement* element; // object type
|
||||
mjElement* element; // element type
|
||||
mjString name; // name
|
||||
mjString classname; // class name
|
||||
|
||||
@@ -673,7 +689,7 @@ typedef struct _mjsSensor { // sensor specification
|
||||
|
||||
|
||||
typedef struct _mjsNumeric { // custom numeric field specification
|
||||
mjElement* element; // object type
|
||||
mjElement* element; // element type
|
||||
mjString name; // name
|
||||
mjDoubleVec data; // initialization data
|
||||
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
|
||||
mjElement* element; // object type
|
||||
mjElement* element; // element type
|
||||
mjString name; // name
|
||||
mjString data; // text string
|
||||
mjString info; // message appended to compiler errors
|
||||
@@ -690,7 +706,7 @@ typedef struct _mjsText { // custom text specification
|
||||
|
||||
|
||||
typedef struct _mjsTuple { // tuple specification
|
||||
mjElement* element; // object type
|
||||
mjElement* element; // element type
|
||||
mjString name; // name
|
||||
mjIntVec objtype; // object types
|
||||
mjStringVec objname; // object names
|
||||
@@ -700,7 +716,7 @@ typedef struct _mjsTuple { // tuple specification
|
||||
|
||||
|
||||
typedef struct _mjsKey { // keyframe specification
|
||||
mjElement* element; // object type
|
||||
mjElement* element; // element type
|
||||
mjString name; // name
|
||||
double time; // time
|
||||
mjDoubleVec qpos; // qpos
|
||||
@@ -714,8 +730,8 @@ typedef struct _mjsKey { // keyframe specification
|
||||
|
||||
|
||||
typedef struct _mjsDefault { // default specification
|
||||
mjString name; // name
|
||||
mjElement* element; // object type
|
||||
mjElement* element; // element type
|
||||
mjString name; // class name
|
||||
mjsJoint* joint; // joint defaults
|
||||
mjsGeom* geom; // geom defaults
|
||||
mjsSite* site; // site defaults
|
||||
@@ -731,34 +747,47 @@ typedef struct _mjsDefault { // default specification
|
||||
} mjsDefault;
|
||||
|
||||
|
||||
//---------------------------------- API functions -------------------------------------------------
|
||||
//---------------------------------- Top-level spec manipulation -----------------------------------
|
||||
|
||||
// Create model.
|
||||
MJAPI mjSpec* mjs_createSpec();
|
||||
// Create spec.
|
||||
MJAPI mjSpec* mjs_createSpec(void);
|
||||
|
||||
// Copy model.
|
||||
MJAPI mjSpec* mjs_copySpec(const mjSpec* s);
|
||||
|
||||
// Copy back model.
|
||||
MJAPI void mjs_copyBack(mjSpec* s, const mjModel* m);
|
||||
|
||||
// Compile model.
|
||||
// Compile spec to model.
|
||||
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,
|
||||
const char* prefix, const char* suffix);
|
||||
|
||||
// Get error message from model.
|
||||
MJAPI const char* mjs_getError(mjSpec* s);
|
||||
// Attach child frame to a parent body, return 0 on success.
|
||||
MJAPI int mjs_attachFrame(mjsBody* parent, const mjsFrame* child,
|
||||
const char* prefix, const char* suffix);
|
||||
|
||||
// Return 1 if model has warnings.
|
||||
MJAPI int mjs_isWarning(mjSpec* s);
|
||||
// Detach body from mjSpec, remove all references and delete the body, return 0 on success.
|
||||
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);
|
||||
|
||||
// Add site to body, return site spec.
|
||||
@@ -782,34 +811,31 @@ MJAPI mjsLight* mjs_addLight(mjsBody* body, mjsDefault* def);
|
||||
// Add frame to body.
|
||||
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);
|
||||
|
||||
// Add mesh to model.
|
||||
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.
|
||||
// Add contact pair.
|
||||
MJAPI mjsPair* mjs_addPair(mjSpec* s, mjsDefault* def);
|
||||
|
||||
// Add excluded body pair to model.
|
||||
// Add excluded body pair.
|
||||
MJAPI mjsExclude* mjs_addExclude(mjSpec* s);
|
||||
|
||||
// Add equality to model.
|
||||
// Add equality.
|
||||
MJAPI mjsEquality* mjs_addEquality(mjSpec* s, mjsDefault* def);
|
||||
|
||||
// Add tendon to model.
|
||||
// Add tendon.
|
||||
MJAPI mjsTendon* mjs_addTendon(mjSpec* s, mjsDefault* def);
|
||||
|
||||
// Wrap site using tendon.
|
||||
@@ -824,42 +850,48 @@ MJAPI mjsWrap* mjs_wrapJoint(mjsTendon* tendon, const char* name, double coef);
|
||||
// Wrap pulley using tendon.
|
||||
MJAPI mjsWrap* mjs_wrapPulley(mjsTendon* tendon, double divisor);
|
||||
|
||||
// Add actuator to model.
|
||||
MJAPI mjsActuator* mjs_addActuator(mjSpec* s, mjsDefault* def);
|
||||
|
||||
// Add sensor to model.
|
||||
MJAPI mjsSensor* mjs_addSensor(mjSpec* s);
|
||||
|
||||
// Add numeric to model.
|
||||
// Add numeric.
|
||||
MJAPI mjsNumeric* mjs_addNumeric(mjSpec* s);
|
||||
|
||||
// Add text to model.
|
||||
// Add text.
|
||||
MJAPI mjsText* mjs_addText(mjSpec* s);
|
||||
|
||||
// Add tuple to model.
|
||||
// Add tuple.
|
||||
MJAPI mjsTuple* mjs_addTuple(mjSpec* s);
|
||||
|
||||
// Add keyframe to model.
|
||||
// Add keyframe.
|
||||
MJAPI mjsKey* mjs_addKey(mjSpec* s);
|
||||
|
||||
// Add plugin to model.
|
||||
// Add plugin.
|
||||
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);
|
||||
|
||||
// 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);
|
||||
|
||||
// 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.
|
||||
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.
|
||||
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.
|
||||
MJAPI int mjs_getId(mjElement* element);
|
||||
|
||||
|
||||
//---------------------------------- Attribute setters ---------------------------------------------
|
||||
|
||||
// Copy text to string.
|
||||
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.
|
||||
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.
|
||||
MJAPI const char* mjs_getString(mjString source);
|
||||
|
||||
// Get double array contents and optionally its 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.
|
||||
MJAPI void mjs_setActivePlugins(mjSpec* s, void* activeplugins);
|
||||
|
||||
// Set default.
|
||||
// Set element's default.
|
||||
MJAPI void mjs_setDefault(mjElement* element, mjsDefault* def);
|
||||
|
||||
// Set frame.
|
||||
// Set element's enlcosing 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,
|
||||
const mjsOrientation* orientation);
|
||||
|
||||
// Compute quat and inertia from body->fullinertia.
|
||||
MJAPI const char* mjs_setFullInertia(mjsBody* body, double quat[4], double inertia[3]);
|
||||
// Compute quat and diag inertia from full inertia matrix, return error if any.
|
||||
MJAPI const char* mjs_fullInertia(double quat[4], double inertia[3], const double fullinertia[6]);
|
||||
|
||||
|
||||
//---------------------------------- Initialization functions --------------------------------------
|
||||
//---------------------------------- Initialization -----------------------------------------------
|
||||
|
||||
// Default model attributes.
|
||||
MJAPI void mjs_defaultSpec(mjSpec& model);
|
||||
// Default spec attributes.
|
||||
MJAPI void mjs_defaultSpec(mjSpec* spec);
|
||||
|
||||
// Default orientation attributes.
|
||||
MJAPI void mjs_defaultOrientation(mjsOrientation& orient);
|
||||
MJAPI void mjs_defaultOrientation(mjsOrientation* orient);
|
||||
|
||||
// Default body attributes.
|
||||
MJAPI void mjs_defaultBody(mjsBody& body);
|
||||
MJAPI void mjs_defaultBody(mjsBody* body);
|
||||
|
||||
// Default frame attributes.
|
||||
MJAPI void mjs_defaultFrame(mjsFrame& frame);
|
||||
MJAPI void mjs_defaultFrame(mjsFrame* frame);
|
||||
|
||||
// Default joint attributes.
|
||||
MJAPI void mjs_defaultJoint(mjsJoint& joint);
|
||||
MJAPI void mjs_defaultJoint(mjsJoint* joint);
|
||||
|
||||
// Default geom attributes.
|
||||
MJAPI void mjs_defaultGeom(mjsGeom& geom);
|
||||
MJAPI void mjs_defaultGeom(mjsGeom* geom);
|
||||
|
||||
// Default site attributes.
|
||||
MJAPI void mjs_defaultSite(mjsSite& site);
|
||||
MJAPI void mjs_defaultSite(mjsSite* site);
|
||||
|
||||
// Default camera attributes.
|
||||
MJAPI void mjs_defaultCamera(mjsCamera& camera);
|
||||
MJAPI void mjs_defaultCamera(mjsCamera* camera);
|
||||
|
||||
// Default light attributes.
|
||||
MJAPI void mjs_defaultLight(mjsLight& light);
|
||||
MJAPI void mjs_defaultLight(mjsLight* light);
|
||||
|
||||
// Default flex attributes.
|
||||
MJAPI void mjs_defaultFlex(mjsFlex& flex);
|
||||
MJAPI void mjs_defaultFlex(mjsFlex* flex);
|
||||
|
||||
// Default mesh attributes.
|
||||
MJAPI void mjs_defaultMesh(mjsMesh& mesh);
|
||||
MJAPI void mjs_defaultMesh(mjsMesh* mesh);
|
||||
|
||||
// Default height field attributes.
|
||||
MJAPI void mjs_defaultHField(mjsHField& hfield);
|
||||
MJAPI void mjs_defaultHField(mjsHField* hfield);
|
||||
|
||||
// Default skin attributes.
|
||||
MJAPI void mjs_defaultSkin(mjsSkin& skin);
|
||||
MJAPI void mjs_defaultSkin(mjsSkin* skin);
|
||||
|
||||
// Default texture attributes.
|
||||
MJAPI void mjs_defaultTexture(mjsTexture& texture);
|
||||
MJAPI void mjs_defaultTexture(mjsTexture* texture);
|
||||
|
||||
// Default material attributes.
|
||||
MJAPI void mjs_defaultMaterial(mjsMaterial& material);
|
||||
MJAPI void mjs_defaultMaterial(mjsMaterial* material);
|
||||
|
||||
// Default pair attributes.
|
||||
MJAPI void mjs_defaultPair(mjsPair& pair);
|
||||
MJAPI void mjs_defaultPair(mjsPair* pair);
|
||||
|
||||
// Default equality attributes.
|
||||
MJAPI void mjs_defaultEquality(mjsEquality& equality);
|
||||
MJAPI void mjs_defaultEquality(mjsEquality* equality);
|
||||
|
||||
// Default tendon attributes.
|
||||
MJAPI void mjs_defaultTendon(mjsTendon& tendon);
|
||||
MJAPI void mjs_defaultTendon(mjsTendon* tendon);
|
||||
|
||||
// Default actuator attributes.
|
||||
MJAPI void mjs_defaultActuator(mjsActuator& actuator);
|
||||
MJAPI void mjs_defaultActuator(mjsActuator* actuator);
|
||||
|
||||
// Default sensor attributes.
|
||||
MJAPI void mjs_defaultSensor(mjsSensor& sensor);
|
||||
MJAPI void mjs_defaultSensor(mjsSensor* sensor);
|
||||
|
||||
// Default numeric attributes.
|
||||
MJAPI void mjs_defaultNumeric(mjsNumeric& numeric);
|
||||
MJAPI void mjs_defaultNumeric(mjsNumeric* numeric);
|
||||
|
||||
// Default text attributes.
|
||||
MJAPI void mjs_defaultText(mjsText& text);
|
||||
MJAPI void mjs_defaultText(mjsText* text);
|
||||
|
||||
// Default tuple attributes.
|
||||
MJAPI void mjs_defaultTuple(mjsTuple& tuple);
|
||||
MJAPI void mjs_defaultTuple(mjsTuple* tuple);
|
||||
|
||||
// Default keyframe attributes.
|
||||
MJAPI void mjs_defaultKey(mjsKey& key);
|
||||
MJAPI void mjs_defaultKey(mjsKey* key);
|
||||
|
||||
// Default plugin attributes.
|
||||
MJAPI void mjs_defaultPlugin(mjsPlugin& plugin);
|
||||
MJAPI void mjs_defaultPlugin(mjsPlugin* plugin);
|
||||
|
||||
//------------------------- Cache functions ------------------------------------
|
||||
|
||||
//---------------------------------- Compiler cache ------------------------------------------------
|
||||
|
||||
typedef struct _mjCache* mjCache;
|
||||
|
||||
@@ -1013,7 +1064,7 @@ typedef struct _mjCache* mjCache;
|
||||
MJAPI void mj_setCacheSize(mjCache cache, size_t size);
|
||||
|
||||
// Get internal global cache context.
|
||||
MJAPI mjCache mj_globalCache();
|
||||
MJAPI mjCache mj_globalCache(void);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
|
||||
@@ -50,6 +50,8 @@ template std::size_t mjCAsset::Add(const std::string& name,
|
||||
const uint8_t* data, std::size_t n);
|
||||
template std::size_t mjCAsset::Add(const std::string& name,
|
||||
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,
|
||||
const float* data, std::size_t n);
|
||||
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);
|
||||
template std::size_t mjCAsset::AddVector(const std::string& name,
|
||||
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,
|
||||
const std::vector<float>& v);
|
||||
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
|
||||
const int* mjCAsset::Get(const std::string& name, std::size_t* n) const;
|
||||
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;
|
||||
template
|
||||
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;
|
||||
template std::optional<std::vector<int>>
|
||||
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>>
|
||||
mjCAsset::GetVector(const std::string& name) const;
|
||||
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