diff --git a/CMakeLists.txt b/CMakeLists.txt index c9723a29..57b9ddd3 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -28,7 +28,7 @@ set(MSVC_INCREMENTAL_DEFAULT ON) project( mujoco - VERSION 3.0.2 + VERSION 3.1.2 DESCRIPTION "MuJoCo Physics Simulator" HOMEPAGE_URL "https://mujoco.org" ) diff --git a/README.md b/README.md index 21a3cb81..cb19a639 100644 --- a/README.md +++ b/README.md @@ -164,7 +164,7 @@ These packages give users of various languages access to MuJoCo functionality: by [Manoj Velmurugan](https://github.com/vmanoj1996). - **Swift**: [swift-mujoco](https://github.com/liuliu/swift-mujoco) - **Java**: [mujoco-java](https://github.com/CommonWealthRobotics/mujoco-java) -- **Julia**: [Lyceum](https://github.com/Lyceum/MuJoCo.jl) (unmaintained) +- **Julia**: [MuJoCo.jl](https://github.com/JamieMair/MuJoCo.jl) ### Converters diff --git a/cmake/MujocoDependencies.cmake b/cmake/MujocoDependencies.cmake index 87bb13a7..86c860a2 100644 --- a/cmake/MujocoDependencies.cmake +++ b/cmake/MujocoDependencies.cmake @@ -39,7 +39,7 @@ set(MUJOCO_DEP_VERSION_qhull CACHE STRING "Version of `qhull` to be fetched." ) set(MUJOCO_DEP_VERSION_Eigen3 - aa6964bf3a34fd607837dd8123bc42465185c4f8 + 454f89af9d6f3525b1df5f9ef9c86df58bf2d4d3 CACHE STRING "Version of `Eigen3` to be fetched." ) @@ -54,7 +54,7 @@ set(MUJOCO_DEP_VERSION_gtest ) set(MUJOCO_DEP_VERSION_benchmark - 344117638c8ff7e239044fd0fa7085839fc03021 # v1.8.3 + e45585a4b8e75c28479fa4107182c28172799640 # v1.8.3 CACHE STRING "Version of `benchmark` to be fetched." ) diff --git a/dist/mujoco.rc b/dist/mujoco.rc index e52e3530..2c2d58c6 100644 --- a/dist/mujoco.rc +++ b/dist/mujoco.rc @@ -1,6 +1,6 @@ 1 VERSIONINFO -FILEVERSION 3,0,2,0 -PRODUCTVERSION 3,0,2,0 +FILEVERSION 3,1,2,0 +PRODUCTVERSION 3,1,2,0 FILEOS 0x4 FILETYPE 0x1 { @@ -9,9 +9,9 @@ FILETYPE 0x1 BLOCK "040904b0" { VALUE "ProductName", "MuJoCo" - VALUE "ProductVersion", "3.0.2" + VALUE "ProductVersion", "3.1.2" VALUE "FileDescription", "MuJoCo" - VALUE "FileVersion", "3.0.2" + VALUE "FileVersion", "3.1.2" VALUE "InternalName", "mujoco.dll" VALUE "OriginalFilename", "mujoco.dll" VALUE "CompanyName", "Google DeepMind" diff --git a/dist/simulate.rc b/dist/simulate.rc index 4835c979..255cb59c 100644 --- a/dist/simulate.rc +++ b/dist/simulate.rc @@ -1,8 +1,8 @@ MUJOCO ICON "mujoco.ico" 1 VERSIONINFO -FILEVERSION 3,0,2,0 -PRODUCTVERSION 3,0,2,0 +FILEVERSION 3,1,2,0 +PRODUCTVERSION 3,1,2,0 FILEOS 0x4 FILETYPE 0x1 { @@ -11,9 +11,9 @@ FILETYPE 0x1 BLOCK "040904b0" { VALUE "ProductName", "MuJoCo" - VALUE "ProductVersion", "3.0.2" + VALUE "ProductVersion", "3.1.2" VALUE "FileDescription", "MuJoCo" - VALUE "FileVersion", "3.0.2" + VALUE "FileVersion", "3.1.2" VALUE "InternalName", "simulate.exe" VALUE "OriginalFilename", "simulate.exe" VALUE "CompanyName", "Google DeepMind" diff --git a/doc/APIreference/APIglobals.rst b/doc/APIreference/APIglobals.rst index 33c5401c..483383c4 100644 --- a/doc/APIreference/APIglobals.rst +++ b/doc/APIreference/APIglobals.rst @@ -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 `_. * - ``mjVERSION_HEADER`` - - 302 + - 312 - 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. diff --git a/doc/APIreference/APItypes.rst b/doc/APIreference/APItypes.rst index 684e810b..ee8de505 100644 --- a/doc/APIreference/APItypes.rst +++ b/doc/APIreference/APItypes.rst @@ -1219,9 +1219,9 @@ a frame at the center-of-mass of the local kinematic subtree (``mjData.subtree_c This choice increases the precision of kinematic computations for mechanisms that are distant from the global origin. ``cdof``: - These 6D motion vectors describe the instantaneous axis of a degree-of-freedom and are used by all Jacobian functions. - Therefore, the minimal computation required for analytic Jacobians is :ref:`mj_kinematics` followed by - :ref:`mj_comPos`. + These 6D motion vectors (3 rotation, 3 translation) describe the instantaneous axis of a degree-of-freedom and are + used by all Jacobian functions. The minimal computation required for analytic Jacobians is :ref:`mj_kinematics` + followed by :ref:`mj_comPos`. ``cinert``: These 10-vectors describe the inertial properties of a body in the c-frame and are used by the Composite Rigid Body diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index 2f423abf..8fb1edd3 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -172,7 +172,7 @@ This element does not strictly belong to MJCF. Instead it is a meta-element, use files in a single document object model (DOM) before parsing. The included file must be a valid XML file with a unique top-level element. This top-level element is removed by the parser, and the elements below it are inserted at the location of the :el:`include` element. At least one element must be inserted as a result of this procedure. The -:el:`include` element can be used where ever an XML element is expected in the MJFC file. Nested includes are allowed, +:el:`include` element can be used where ever an XML element is expected in the MJCF file. Nested includes are allowed, however a given XML file can be included at most once in the entire model. After all the included XML files have been assembled into a single DOM, it must correspond to a valid MJCF model. Other than that, it is up to the user to decide how to use includes and how to modularize large files if desired. @@ -1378,9 +1378,9 @@ also known as terrain map, is a 2D matrix of elevation data. The data can be spe | For collision detection, a height field is treated as a union of triangular prisms. Collisions between height fields and other geoms (except for planes and other height fields which are not supported) are computed by first selecting the sub-grid of prisms that could collide with the geom based on its bounding box, and then using the general convex - collider. The number of possible contacts between a height field and a geom is limited to 9; any contacts beyond that - are discarded. To avoid penetration due to discarded contacts, the spatial features of the height field should be - large compared to the geoms it collides with. + collider. The number of possible contacts between a height field and a geom is limited to 50 + (:ref:`mjMAXCONPAIR `); any contacts beyond that are discarded. To avoid penetration due to discarded + contacts, the spatial features of the height field should be large compared to the geoms it collides with. .. _asset-hfield-name: diff --git a/doc/changelog.rst b/doc/changelog.rst index 56904ed8..52592511 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -5,45 +5,67 @@ Changelog Upcoming version (not yet released) ----------------------------------- +MJX +^^^ + +1. Add :ref:`dyntype` ``filterexact``. +2. Add :at:`site` transmission. + + +Version 3.1.1 (December 18, 2023) +----------------------------------- + +Bug fixes +^^^^^^^^^ +1. Fixed a bug (introduced in 3.1.0) where box-box collisions produced no contacts if one box was deeply embedded in the other. +2. Fixed a bug in :ref:`simulate` where the "LOADING..." message was not showing correctly. +3. Fixed a crash in the Python :ref:`passive viewer`, when used with models containing Flex objects. +4. Fixed a bug in MJX where ``site_xmat`` was ignored in ``get_data`` and ``put_data`` +5. Fixed a bug in MJX where ``efc_address`` was sometimes incorrectly calculated in ``get_data``. + + +Version 3.1.0 (December 12, 2023) +--------------------------------- + General ^^^^^^^ -- Improved convergence of Signed Distance Function (SDF) collisions by using line search and a new objective function - for the optimization. This allows to decrease the number of initial points needed for finding the contacts and is more - robust for very small or large geom sizes. -- Added :ref:`frame` to MJCF, a :ref:`meta-element` which defines a pure coordinate transformation - on its direct children, without requiring a :ref:`body`. -- Added the :at:`kv` attribute to the :ref:`position` and :ref:`intvelocity` - actuators, for specifying actuator-applied damping. This can be used to implement a PD controller with 0 reference - velocity. When using this attribute, it is recommended to use the implicitfast or implicit - :ref:`integrators`. +1. Improved convergence of Signed Distance Function (SDF) collisions by using line search and a new objective function + for the optimization. This allows to decrease the number of initial points needed for finding the contacts and is more + robust for very small or large geom sizes. +2. Added :ref:`frame` to MJCF, a :ref:`meta-element` which defines a pure coordinate transformation + on its direct children, without requiring a :ref:`body`. +3. Added the :at:`kv` attribute to the :ref:`position` and :ref:`intvelocity` + actuators, for specifying actuator-applied damping. This can be used to implement a PD controller with 0 reference + velocity. When using this attribute, it is recommended to use the implicitfast or implicit + :ref:`integrators`. Plugins ^^^^^^^ -- Allow actuator plugins to use activation variables in ``mjData.act`` as their internal state, rather than - ``mjData.plugin_state``. Actuator plugins can now specify :ref:`callbacks` that compute activation - variables, and they can be used with built-in :ref:`dyntype` actuator dynamics. +4. Allow actuator plugins to use activation variables in ``mjData.act`` as their internal state, rather than + ``mjData.plugin_state``. Actuator plugins can now specify :ref:`callbacks` that compute activation + variables, and they can be used with built-in :ref:`dyntype` actuator dynamics. -- Added the `pid `__ actuator plugin, a - configurable PID controller that implements the Integral term, which is not available with native MuJoCo actuators. +5. Added the `pid `__ actuator plugin, a + configurable PID controller that implements the Integral term, which is not available with native MuJoCo actuators. MJX ^^^ -- Added ``site_xpos`` and ``site_xmat`` to MJX. -- Added ``put_data``, ``put_model``, ``get_data`` to replace ``device_put`` and ``device_get_into``, which will be - deprecated. These new functions correctly translate fields that are the result of intermediate calculations such as - ``efc_J``. +6. Added ``site_xpos`` and ``site_xmat`` to MJX. +7. Added ``put_data``, ``put_model``, ``get_data`` to replace ``device_put`` and ``device_get_into``, which will be + deprecated. These new functions correctly translate fields that are the result of intermediate calculations such as + ``efc_J``. Bug fixes ^^^^^^^^^ -- Fix bug in Cartesian actuation with movable refsite, as when using body-centric Cartesian actuators on a quadruped. - Before this fix such actuators could lead to non-conservation of momentum. -- Fix bug that prevented using flex with the :ref:`passive viewer`. -- Fix bug that prevented the use of elasticity plugins in combination with pinned flex vertices. -- Release Python wheels targeting macOS 10.16 to support x86_64 systems where SYSTEM_VERSION_COMPAT is set. The minimum - supported version is still 11.0, but we release these wheels to fix compatibility for those users. See - :github:issue:`1213`. +8. Fix bug in Cartesian actuation with movable refsite, as when using body-centric Cartesian actuators on a quadruped. + Before this fix such actuators could lead to non-conservation of momentum. +9. Fix bug that prevented using flex with :ref:`simulate`. +10. Fix bug that prevented the use of elasticity plugins in combination with pinned flex vertices. +11. Release Python wheels targeting macOS 10.16 to support x86_64 systems where SYSTEM_VERSION_COMPAT is set. The minimum + supported version is still 11.0, but we release these wheels to fix compatibility for those users. See + :github:issue:`1213`. Version 3.0.1 (November 15, 2023) --------------------------------- diff --git a/doc/computation/index.rst b/doc/computation/index.rst index 8afdb7cf..190be5ac 100644 --- a/doc/computation/index.rst +++ b/doc/computation/index.rst @@ -300,19 +300,22 @@ is attached; the possible attachment object types are :at:`joint`, :at:`tendon`, Slider-cranks can also be modeled explicitly by creating MuJoCo bodies and coupling them with equality constraints to the rest of the system, but that would be less efficient. -:at:`site` - :at:`site` transmission (without a :at:`refsite`, see below) and :at:`body` transmission targets have a fixed zero - length :math:`l_i(q) = 0`. They can therefore not be used to maintain a desired length, but can be used to apply - forces. Site transmissions correspond to applying a Cartsian force/torque at the site, and are useful for modeling - jets and propellors. :el:`body` transmissions correspond to applying forces at contact points belonging to a body, in +:at:`body` + :el:`body` transmission corresponds to applying forces at contact points belonging to a body, in order to model vacuum grippers and biomechanical adhesive appendages. For more information about adhesion, see the - :ref:`adhesion` actuator documentation. + :ref:`adhesion` actuator documentation. These transmission targets have a fixed zero length + :math:`l_i(q) = 0`. - If a :at:`site` transmission target is defined with the optional :at:`refsite` attribute, forces and torques are - applied in the frame of the reference site rather than the site's own frame. If a reference site is defined then - the length of the actuator is nonzero and corresponds to the pose difference of the two sites. This length can then - be controlled with a :el:`position` actuator, enabling Cartesian end-effector control. See the - :ref:`refsite` documentation for more details. +:at:`site` + Site transmissions correspond to applying a Cartsian force/torque in the frame of a site. When a :at:`refsite` is not + defined (see below), these targets have a fixed zero length :math:`l_i(q) = 0` and are useful for modeling jets and + propellors: forces and torques which are fixed to the site frame. + + If a :at:`site` transmission is defined with the optional :at:`refsite` attribute, forces and torques are applied in + the frame of the reference site rather than the site's own frame. If a reference site is defined, the length of the + actuator is nonzero and corresponds to the pose difference of the two sites, projected onto a chosen direction in the + reference frame. This length can then be controlled with a :el:`position` actuator, allowing for Cartesian + end-effector control. See the :ref:`refsite` documentation for more details. .. _geActivation: @@ -1455,48 +1458,45 @@ others can be pruned quickly without a detailed check. MuJoCo has flexible mecha checked in detail. The decision process involves two stages: generation and filtering. Generation - First we generate a list of candidate geom pairs in one of two ways: "pair" or "dynamic". The user can also specify - "all" which merges both sources (and is the default). This is done via the setting ``mjModel.opt.collision``. "Pair" - refers to an explicit list of geom pairs defined with the :ref:`pair ` element in MJCF. It gives the - user full control, however it is a static mechanism (independent of the spatial arrangement of the geoms at runtime) - and can be tedious for large models. It is normally used to supplement the output of the "dynamic" mechanism. Dynamic - generation works with bodies rather than geoms; when a body pair is included this means that all geoms attached to - one body can collide with all geoms attached to the other body. + First we generate a list of candidate geom pairs by merging from two sources: pairs of bodies that might contain + colliding geoms and the explicit list of geom pairs defined with the :ref:`pair ` element in MJCF. The body pairs are generated via broad-phase collision detection based on a modified sweep-and-prune algorithm. The modification is that the axis for sorting is chosen as the principal eigenvector of the covariance matrix of all geom - centers - which maximizes the spread. Then, for each body pair, a mid-phase collision detection using a static - bounding volume hierarchy (a BVH binary tree) of axis-aligned bounding boxes (AABB) is performed. Each body is - equipped with an AABB tree of its geoms, aligned with the body inertial or geom frames for all inner or leaf nodes, + centers -- which maximizes the spread. Then, for each body pair, mid-phase collision detection is performed using a + static bounding volume hierarchy (a BVH binary tree) of axis-aligned bounding boxes (AABB). Each body is equipped + with an AABB tree of its geoms, aligned with the body inertial or geom frames for all inner or leaf nodes, respectively. - Finally, the user can explicitly exclude certain body pairs using the :ref:`exclude ` element - in MJCF. Exclusion is applied when "dynamic" or "all" are selected, but not when "pair" is selected. At the end of - this step we have a list of geoms pairs that is typically much smaller than :math:`n (n-1)/2`, but can still be - pruned further before detailed collision checking. + Finally, the user can explicitly exclude certain body pairs using the :ref:`exclude ` element in + MJCF. At the end of this step we have a list of geoms pairs that is typically much smaller than :math:`n (n-1)/2`, + but can still be pruned further before detailed collision checking. Filtering Next we apply four filters to the list generated in the previous step. Filters 1 and 2 are applied to all geom pairs. - Filters 3 and 4 are applied only to pairs generated by the "dynamic" mechanism, thereby allowing the user to bypass + Filters 3 and 4 are applied only to pairs generated by the body-pair mechanism, thereby allowing the user to bypass those filters by specifying geom pairs explicitly. - #. The types of the two geoms must correspond to a collision function that is capable of performing the detailed + 1. The types of the two geoms must correspond to a collision function that is capable of performing the detailed check. This is usually the case but there are exceptions (for example plane-plane collisions are not supported), and furthermore the user may override the default table of collision functions with NULL pointers, effectively disabling collisions between certain geom types. - #. A bounding sphere test is applied, taking into account the contact margin. If one of the geoms in the pair is a + 2. A bounding sphere test is applied, taking into account the contact margin. If one of the geoms in the pair is a plane, this becomes a plane-sphere test. - #. The two geoms cannot belong to the same body. Furthermore, they cannot belong to a parent and a child body, unless + 3. The two geoms cannot belong to the same body. Furthermore, they cannot belong to a parent and a child body, unless the parent is the world body. The motivation is to avoid permanent contacts within bodies and joints. Note that if several bodies are welded together in the sense that there are no joints between them, they are treated as a single body for the purposes of this test. The parent-filter test can be disabled by the user, while the same-body test cannot be disabled. - #. The two geoms must be "compatible" in the following sense. Each geom has integer parameters ``contype`` and + 4. The two geoms must be "compatible" in the following sense. Each geom has integer parameters ``contype`` and ``conaffinity``. The boolean expression below must be true for the test to pass: - ``(contype1 & conaffinity2) || (contype2 & conaffinity1)`` This requires the ``contype`` of one geom and the - ``conaffinity`` of the other geom to have a common bit set to 1. This is a powerful mechanism borrowed from the - Open Dynamics Engine. The default setting for all geoms is ``contype = conaffinity = 1`` which always passes the - test, so the user can ignore this mechanism if it is confusing at first. + + ``(contype1 & conaffinity2) || (contype2 & conaffinity1)`` + + This requires the ``contype`` of one geom and the ``conaffinity`` of the other geom to have a common bit set to 1. + This is a powerful mechanism borrowed from Open Dynamics Engine. The default setting for all geoms is + ``contype = conaffinity = 1`` which always passes the test, so the user can ignore this mechanism if it is + confusing at first. .. _coChecking: diff --git a/doc/includes/references.h b/doc/includes/references.h index f4aedde9..1c788150 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -237,7 +237,7 @@ struct mjData_ { // computed by mj_fwdPosition/mj_comPos mjtNum* subtree_com; // center of mass of each subtree (nbody x 3) - mjtNum* cdof; // com-based motion axis of each dof (nv x 6) + mjtNum* cdof; // com-based motion axis of each dof (rot:lin) (nv x 6) mjtNum* cinert; // com-based body inertia and mass (nbody x 10) // computed by mj_fwdPosition/mj_flex @@ -285,8 +285,8 @@ struct mjData_ { mjtNum* actuator_velocity; // actuator velocities (nu x 1) // computed by mj_fwdVelocity/mj_comVel - mjtNum* cvel; // com-based velocity [3D rot; 3D tran] (nbody x 6) - mjtNum* cdof_dot; // time-derivative of cdof (nv x 6) + mjtNum* cvel; // com-based velocity (rot:lin) (nbody x 6) + mjtNum* cdof_dot; // time-derivative of cdof (rot:lin) (nv x 6) // computed by mj_fwdVelocity/mj_rne (without acceleration) mjtNum* qfrc_bias; // C(qpos,qvel) (nv x 1) @@ -459,10 +459,11 @@ typedef enum mjtGeom_ { // type of geometric shape mjGEOM_ARROW1, // arrow without wedges mjGEOM_ARROW2, // arrow in both directions mjGEOM_LINE, // line + mjGEOM_LINEBOX, // box with line edges mjGEOM_FLEX, // flex mjGEOM_SKIN, // skin mjGEOM_LABEL, // text label - mjGEOM_TRIANGLE, // triangle connecting a frame + mjGEOM_TRIANGLE, // triangle mjGEOM_NONE = 1001 // missing geom type } mjtGeom; @@ -570,7 +571,9 @@ typedef enum mjtObj_ { // type of MujoCo object mjOBJ_TEXT, // text mjOBJ_TUPLE, // tuple mjOBJ_KEY, // keyframe - mjOBJ_PLUGIN // plugin instance + mjOBJ_PLUGIN, // plugin instance + + mjNOBJECT // number of object types } mjtObj; typedef enum mjtConstraint_ { // type of constraint mjCNSTR_EQUALITY = 0, // equality constraint @@ -2167,6 +2170,7 @@ struct mjvSceneState_ { int nnames; int npaths; int nsensordata; + int narena; mjOption opt; mjVisual vis; @@ -2380,6 +2384,7 @@ struct mjvSceneState_ { mjtNum* ten_length; mjtNum* wrap_xpos; + mjtNum* bvh_aabb_dyn; mjtByte* bvh_active; int* island_dofadr; int* island_dofind; @@ -2391,6 +2396,7 @@ struct mjvSceneState_ { mjContact* contact; mjtNum* efc_force; + void* arena; } data; }; typedef struct mjvSceneState_ mjvSceneState; diff --git a/doc/mjx.rst b/doc/mjx.rst index 92a709b6..c9263fa9 100644 --- a/doc/mjx.rst +++ b/doc/mjx.rst @@ -181,9 +181,9 @@ The following features are **fully supported** in MJX: * - :ref:`Joint ` - ``FREE``, ``BALL``, ``SLIDE``, ``HINGE`` * - :ref:`Transmission ` - - ``TRN_JOINT`` + - ``TRN_JOINT``, ``TRN_SITE`` * - :ref:`Actuator Dynamics ` - - ``NONE``, ``INTEGRATOR``, ``FILTER`` + - ``NONE``, ``INTEGRATOR``, ``FILTER``, ``FILTEREXACT`` * - :ref:`Actuator Gain ` - ``FIXED``, ``AFFINE`` * - :ref:`Actuator Bias ` @@ -257,9 +257,9 @@ The following features are **unsupported**: * - Category - Feature * - :ref:`Transmission ` - - ``TRN_JOINTINPARENT``, ``TRN_SLIDERCRANK``, ``TRN_SITE``, ``TRN_BODY`` + - ``TRN_JOINTINPARENT``, ``TRN_SLIDERCRANK``, ``TRN_BODY`` * - :ref:`Actuator Dynamics ` - - ``FILTEREXACT``, ``USER`` + - ``USER`` * - :ref:`Actuator Gain ` - ``USER`` * - :ref:`Actuator Bias ` diff --git a/doc/modeling.rst b/doc/modeling.rst index c43b9a04..681534cd 100644 --- a/doc/modeling.rst +++ b/doc/modeling.rst @@ -403,7 +403,7 @@ and the damping ratio is ignored. Equivalently, in the direct format, the :math: .. math:: \begin{aligned} b &= \text{damping} / d_\text{width} \\ - k &= \text{stiffness} / d_\text{width}^2 \\ + k &= \text{stiffness} \cdot d(r) / d_\text{width}^2 \\ \end{aligned} .. tip:: diff --git a/doc/unity.rst b/doc/unity.rst index 457c2cd7..4e443f40 100644 --- a/doc/unity.rst +++ b/doc/unity.rst @@ -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.0.2.dylib`` (it can be +``/Applications/MuJoCo.app/Contents/Frameworks/mujoco.framework/Versions/Current/libmujoco.3.1.2.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.0.2/lib/libmujoco.so.3.0.2`` and rename it as ``libmujoco.so``. +``~/.mujoco/mujoco-3.1.2/lib/libmujoco.so.3.1.2`` and rename it as ``libmujoco.so``. Windows _______ diff --git a/include/mujoco/mjdata.h b/include/mujoco/mjdata.h index a3609b8b..1fc6fe19 100644 --- a/include/mujoco/mjdata.h +++ b/include/mujoco/mjdata.h @@ -265,7 +265,7 @@ struct mjData_ { // computed by mj_fwdPosition/mj_comPos mjtNum* subtree_com; // center of mass of each subtree (nbody x 3) - mjtNum* cdof; // com-based motion axis of each dof (nv x 6) + mjtNum* cdof; // com-based motion axis of each dof (rot:lin) (nv x 6) mjtNum* cinert; // com-based body inertia and mass (nbody x 10) // computed by mj_fwdPosition/mj_flex @@ -313,8 +313,8 @@ struct mjData_ { mjtNum* actuator_velocity; // actuator velocities (nu x 1) // computed by mj_fwdVelocity/mj_comVel - mjtNum* cvel; // com-based velocity [3D rot; 3D tran] (nbody x 6) - mjtNum* cdof_dot; // time-derivative of cdof (nv x 6) + mjtNum* cvel; // com-based velocity (rot:lin) (nbody x 6) + mjtNum* cdof_dot; // time-derivative of cdof (rot:lin) (nv x 6) // computed by mj_fwdVelocity/mj_rne (without acceleration) mjtNum* qfrc_bias; // C(qpos,qvel) (nv x 1) diff --git a/include/mujoco/mjmodel.h b/include/mujoco/mjmodel.h index 663e191f..d3417683 100644 --- a/include/mujoco/mjmodel.h +++ b/include/mujoco/mjmodel.h @@ -109,10 +109,11 @@ typedef enum mjtGeom_ { // type of geometric shape mjGEOM_ARROW1, // arrow without wedges mjGEOM_ARROW2, // arrow in both directions mjGEOM_LINE, // line + mjGEOM_LINEBOX, // box with line edges mjGEOM_FLEX, // flex mjGEOM_SKIN, // skin mjGEOM_LABEL, // text label - mjGEOM_TRIANGLE, // triangle connecting a frame + mjGEOM_TRIANGLE, // triangle mjGEOM_NONE = 1001 // missing geom type } mjtGeom; @@ -246,7 +247,9 @@ typedef enum mjtObj_ { // type of MujoCo object mjOBJ_TEXT, // text mjOBJ_TUPLE, // tuple mjOBJ_KEY, // keyframe - mjOBJ_PLUGIN // plugin instance + mjOBJ_PLUGIN, // plugin instance + + mjNOBJECT // number of object types } mjtObj; diff --git a/include/mujoco/mjvisualize.h b/include/mujoco/mjvisualize.h index d693df9c..dc8a4bc6 100644 --- a/include/mujoco/mjvisualize.h +++ b/include/mujoco/mjvisualize.h @@ -436,6 +436,7 @@ struct mjvSceneState_ { int nnames; int npaths; int nsensordata; + int narena; mjOption opt; mjVisual vis; @@ -649,6 +650,7 @@ struct mjvSceneState_ { mjtNum* ten_length; mjtNum* wrap_xpos; + mjtNum* bvh_aabb_dyn; mjtByte* bvh_active; int* island_dofadr; int* island_dofind; @@ -660,6 +662,7 @@ struct mjvSceneState_ { mjContact* contact; mjtNum* efc_force; + void* arena; } data; }; typedef struct mjvSceneState_ mjvSceneState; diff --git a/include/mujoco/mjxmacro.h b/include/mujoco/mjxmacro.h index f4088314..86f61e0c 100644 --- a/include/mujoco/mjxmacro.h +++ b/include/mujoco/mjxmacro.h @@ -614,7 +614,7 @@ X ( mjtNum, qLD, nM, 1 ) \ X ( mjtNum, qLDiagInv, nv, 1 ) \ X ( mjtNum, qLDiagSqrtInv, nv, 1 ) \ - X ( mjtNum, bvh_aabb_dyn, nbvhdynamic, 6 ) \ + XMJV( mjtNum, bvh_aabb_dyn, nbvhdynamic, 6 ) \ XMJV( mjtByte, bvh_active, nbvh, 1 ) \ X ( mjtNum, flexedge_velocity, nflexedge, 1 ) \ X ( mjtNum, ten_velocity, ntendon, 1 ) \ diff --git a/include/mujoco/mujoco.h b/include/mujoco/mujoco.h index 5f32cbc7..5ab8a363 100644 --- a/include/mujoco/mujoco.h +++ b/include/mujoco/mujoco.h @@ -24,7 +24,7 @@ extern "C" { #endif // header version; should match the library version as returned by mj_version() -#define mjVERSION_HEADER 302 +#define mjVERSION_HEADER 312 // needed to define size_t, fabs and log10 #include diff --git a/introspect/enums.py b/introspect/enums.py index bb039f65..8502dbdb 100644 --- a/introspect/enums.py +++ b/introspect/enums.py @@ -90,10 +90,11 @@ ENUMS: Mapping[str, EnumDecl] = dict([ ('mjGEOM_ARROW1', 101), ('mjGEOM_ARROW2', 102), ('mjGEOM_LINE', 103), - ('mjGEOM_FLEX', 104), - ('mjGEOM_SKIN', 105), - ('mjGEOM_LABEL', 106), - ('mjGEOM_TRIANGLE', 107), + ('mjGEOM_LINEBOX', 104), + ('mjGEOM_FLEX', 105), + ('mjGEOM_SKIN', 106), + ('mjGEOM_LABEL', 107), + ('mjGEOM_TRIANGLE', 108), ('mjGEOM_NONE', 1001), ]), )), @@ -265,6 +266,7 @@ ENUMS: Mapping[str, EnumDecl] = dict([ ('mjOBJ_TUPLE', 23), ('mjOBJ_KEY', 24), ('mjOBJ_PLUGIN', 25), + ('mjNOBJECT', 26), ]), )), ('mjtConstraint', diff --git a/introspect/enums_test.py b/introspect/enums_test.py index b4144420..3580d4c1 100644 --- a/introspect/enums_test.py +++ b/introspect/enums_test.py @@ -61,7 +61,7 @@ class EnumsTest(absltest.TestCase): self.assertEqual(enum_decl.values['mjGEOM_ARROW'], 100) self.assertEqual(enum_decl.values['mjGEOM_ARROW1'], 101) self.assertEqual(enum_decl.values['mjGEOM_ARROW2'], 102) - self.assertEqual(enum_decl.values['mjGEOM_TRIANGLE'], 107) + self.assertEqual(enum_decl.values['mjGEOM_TRIANGLE'], 108) # Skip a few... self.assertEqual(enum_decl.values['mjGEOM_NONE'], 1001) diff --git a/introspect/structs.py b/introspect/structs.py index a1fa3159..43e7cdc9 100644 --- a/introspect/structs.py +++ b/introspect/structs.py @@ -4493,7 +4493,7 @@ STRUCTS: Mapping[str, StructDecl] = dict([ type=PointerType( inner_type=ValueType(name='mjtNum'), ), - doc='com-based motion axis of each dof (nv x 6)', # pylint: disable=line-too-long + doc='com-based motion axis of each dof (rot:lin) (nv x 6)', # pylint: disable=line-too-long ), StructFieldDecl( name='cinert', @@ -4703,14 +4703,14 @@ STRUCTS: Mapping[str, StructDecl] = dict([ type=PointerType( inner_type=ValueType(name='mjtNum'), ), - doc='com-based velocity [3D rot; 3D tran] (nbody x 6)', # pylint: disable=line-too-long + doc='com-based velocity (rot:lin) (nbody x 6)', # pylint: disable=line-too-long ), StructFieldDecl( name='cdof_dot', type=PointerType( inner_type=ValueType(name='mjtNum'), ), - doc='time-derivative of cdof (nv x 6)', # pylint: disable=line-too-long + doc='time-derivative of cdof (rot:lin) (nv x 6)', # pylint: disable=line-too-long ), StructFieldDecl( name='qfrc_bias', @@ -6334,6 +6334,11 @@ STRUCTS: Mapping[str, StructDecl] = dict([ type=ValueType(name='int'), doc='', ), + StructFieldDecl( + name='narena', + type=ValueType(name='int'), + doc='', + ), StructFieldDecl( name='opt', type=ValueType(name='mjOption'), @@ -7596,6 +7601,13 @@ STRUCTS: Mapping[str, StructDecl] = dict([ ), doc='', ), + StructFieldDecl( + name='bvh_aabb_dyn', + type=PointerType( + inner_type=ValueType(name='mjtNum'), + ), + doc='', + ), StructFieldDecl( name='bvh_active', type=PointerType( @@ -7659,6 +7671,13 @@ STRUCTS: Mapping[str, StructDecl] = dict([ ), doc='', ), + StructFieldDecl( + name='arena', + type=PointerType( + inner_type=ValueType(name='void'), + ), + doc='', + ), ), ), doc='', diff --git a/mjx/mujoco/mjx/_src/device_test.py b/mjx/mujoco/mjx/_src/device_test.py index 9f518d91..b4b9a0cd 100644 --- a/mjx/mujoco/mjx/_src/device_test.py +++ b/mjx/mujoco/mjx/_src/device_test.py @@ -129,12 +129,6 @@ class ValidateInputTest(absltest.TestCase): with self.assertRaises(NotImplementedError): mjx.device_put(m) - def test_trn(self): - m = test_util.load_test_file('pendula.xml') - m.actuator_trntype[0] = mujoco.mjtTrn.mjTRN_SITE - 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 diff --git a/mjx/mujoco/mjx/_src/forward.py b/mjx/mujoco/mjx/_src/forward.py index 827c8f31..1ff268e7 100644 --- a/mjx/mujoco/mjx/_src/forward.py +++ b/mjx/mujoco/mjx/_src/forward.py @@ -107,7 +107,7 @@ def fwd_actuation(m: Model, d: Data) -> Data: act_dot = jp.array(0.0) elif dyn_typ == DynType.INTEGRATOR: act_dot = ctrl - elif dyn_typ == DynType.FILTER: + elif dyn_typ in (DynType.FILTER, DynType.FILTEREXACT): act_dot = (ctrl - act) / jp.clip(dyn_prm[0], mujoco.mjMINVAL) else: raise NotImplementedError(f'dyntype {dyn_typ.name} not implemented.') @@ -228,6 +228,34 @@ def _integrate_pos( return jp.concatenate(qs) if qs else jp.empty((0,)) +def _next_activation(m: Model, d: Data, act_dot: jax.Array) -> jax.Array: + """Returns the next act given the current act_dot, after clamping.""" + act = d.act + + if not m.na: + return act + + actrange = jp.where( + m.actuator_actlimited[:, None], + m.actuator_actrange, + jp.array([-jp.inf, jp.inf]), + ) + + def fn(dyntype, dynprm, act, act_dot, actrange): + if dyntype == DynType.FILTEREXACT: + tau = jp.clip(dynprm[0], a_min=mujoco.mjMINVAL) + act = act + act_dot * tau * (1 - jp.exp(-m.opt.timestep / tau)) + else: + act = act + act_dot * m.opt.timestep + act = jp.clip(act, actrange[0], actrange[1]) + return act + + args = (m.actuator_dyntype, m.actuator_dynprm, act, act_dot, actrange) + act = scan.flat(m, fn, 'uuaau', 'a', *args, group_by='u') + + return act.reshape(m.na) + + @named_scope def _advance( m: Model, @@ -237,16 +265,7 @@ def _advance( qvel: Optional[jax.Array] = None, ) -> Data: """Advance state and time given activation derivatives and acceleration.""" - act = d.act - if m.na: - act = d.act + act_dot * m.opt.timestep - actrange = jp.where( - m.actuator_actlimited[:, None], - m.actuator_actrange, - jp.array([-jp.inf, jp.inf]), - ) - fn = lambda act, actrange: jp.clip(act, actrange[0], actrange[1]) - act = scan.flat(m, fn, 'au', 'a', act, actrange, group_by='u') + act = _next_activation(m, d, act_dot) # advance velocities d = d.replace(qvel=d.qvel + qacc * m.opt.timestep) diff --git a/mjx/mujoco/mjx/_src/forward_test.py b/mjx/mujoco/mjx/_src/forward_test.py index fbe28c9d..cc767ae8 100644 --- a/mjx/mujoco/mjx/_src/forward_test.py +++ b/mjx/mujoco/mjx/_src/forward_test.py @@ -134,5 +134,45 @@ class ForwardTest(absltest.TestCase): np.testing.assert_allclose(dx.qvel, 1 + m.opt.timestep) +class ActuatorTest(absltest.TestCase): + _DYN_XML = """ + + + + + + + + + + + + + + + + + + + """ + + def test_dyntype(self): + m = mujoco.MjModel.from_xml_string(self._DYN_XML) + d = mujoco.MjData(m) + d.ctrl = np.array([1.5, 1.5, 1.5, 1.5]) + d.act = np.array([0.5, 0.5, 0.5]) + + mx = mjx.put_model(m) + dx = mjx.put_data(m, d) + + mujoco.mj_fwdActuation(m, d) + dx = jax.jit(mjx.fwd_actuation)(mx, dx) + _assert_attr_eq(d, dx, 'act_dot') + + mujoco.mj_Euler(m, d) + dx = jax.jit(mjx.euler)(mx, dx) + _assert_attr_eq(d, dx, 'act') + + if __name__ == '__main__': absltest.main() diff --git a/mjx/mujoco/mjx/_src/io.py b/mjx/mujoco/mjx/_src/io.py index 9658ad53..ab255cfa 100644 --- a/mjx/mujoco/mjx/_src/io.py +++ b/mjx/mujoco/mjx/_src/io.py @@ -103,6 +103,10 @@ def put_model(m: mujoco.MjModel, device=None) -> types.Model: f'{[mj_type(m) for m in missing]} not supported' ) + # TODO: implement reference sites. + if any(m.actuator_trnid[:, 1] != -1): + raise NotImplementedError('refsite is not supported') + opt = _put_option(m.opt, device=device) stat = _put_statistic(m.stat, device=device) @@ -217,7 +221,7 @@ def _get_contact( value = value.reshape((-1, 9)) getattr(c, field.name)[:] = value - ncon = con_id.shape[0] + ncon = cx.dist.shape[0] c.efc_address[:] = np.arange(efc_start, efc_start + ncon * 4, 4)[con_id] @@ -257,7 +261,7 @@ def get_data( value = getattr(dx_i, field.name) - if field.name in ('xmat', 'ximat', 'geom_xmat'): + if field.name in ('xmat', 'ximat', 'geom_xmat', 'site_xmat'): value = value.reshape((-1, 9)) if field.name in ('efc_frictionloss', 'efc_D', 'efc_aref', 'efc_force'): @@ -318,7 +322,7 @@ def put_data(m: mujoco.MjModel, d: mujoco.MjData, device=None) -> types.Data: if f.type is jax.Array } - for fname in ('xmat', 'ximat', 'geom_xmat'): + for fname in ('xmat', 'ximat', 'geom_xmat', 'site_xmat'): fields[fname] = fields[fname].reshape((-1, 3, 3)) # pad efc fields: MuJoCo efc arrays are sparse for inactive constraints. diff --git a/mjx/mujoco/mjx/_src/io_test.py b/mjx/mujoco/mjx/_src/io_test.py index 4c7b0065..08dec57a 100644 --- a/mjx/mujoco/mjx/_src/io_test.py +++ b/mjx/mujoco/mjx/_src/io_test.py @@ -71,6 +71,7 @@ _MULTIPLE_CONSTRAINTS = """ + @@ -81,7 +82,8 @@ _MULTIPLE_CONSTRAINTS = """ """ -class IoTest(parameterized.TestCase): +class ModelIOTest(parameterized.TestCase): + """IO tests for mjx.Model.""" def test_put_model(self): m = mujoco.MjModel.from_xml_string(_MULTIPLE_CONVEX_OBJECTS) @@ -132,7 +134,7 @@ class IoTest(parameterized.TestCase): ) self.assertTrue(m.opt.has_fluid_params) - def test_put_model_implicit_not_implemented(self): + def test_implicit_not_implemented(self): """Test that MJX guards against models with unimplemented features.""" with self.assertRaises(NotImplementedError): @@ -142,7 +144,7 @@ class IoTest(parameterized.TestCase): ) ) - def test_put_model_cone_not_implemented(self): + def test_cone_not_implemented(self): with self.assertRaises(NotImplementedError): mjx.put_model( mujoco.MjModel.from_xml_string( @@ -150,7 +152,7 @@ class IoTest(parameterized.TestCase): ) ) - def test_put_model_pgs_not_implemented(self): + def test_pgs_not_implemented(self): with self.assertRaises(NotImplementedError): mjx.put_model( mujoco.MjModel.from_xml_string( @@ -158,7 +160,7 @@ class IoTest(parameterized.TestCase): ) ) - def test_put_model_site_actuator_not_implemented(self): + def test_site_actuator_not_implemented(self): with self.assertRaises(NotImplementedError): mjx.put_model(mujoco.MjModel.from_xml_string(""" @@ -173,7 +175,7 @@ class IoTest(parameterized.TestCase): """)) - def test_put_model_tendon_not_implemented(self): + def test_tendon_not_implemented(self): with self.assertRaises(NotImplementedError): mjx.put_model(mujoco.MjModel.from_xml_string(""" @@ -190,7 +192,7 @@ class IoTest(parameterized.TestCase): """)) - def test_put_model_condim_not_implemented(self): + def test_condim_not_implemented(self): with self.assertRaises(NotImplementedError): mjx.put_model(mujoco.MjModel.from_xml_string(""" @@ -206,7 +208,7 @@ class IoTest(parameterized.TestCase): """)) - def test_put_model_cylinder_not_implemented(self): + def test_cylinder_not_implemented(self): with self.assertRaises(NotImplementedError): mjx.put_model(mujoco.MjModel.from_xml_string(""" @@ -222,6 +224,29 @@ class IoTest(parameterized.TestCase): """)) + def test_refsite_not_implemented(self): + """Tests that site transmissions with refsites are not implemented.""" + with self.assertRaises(NotImplementedError): + mjx.put_model(mujoco.MjModel.from_xml_string(""" + + + + + + + + + + + + + + """)) + + +class DataIOTest(parameterized.TestCase): + """IO tests for mjx.Data.""" + def test_make_data(self): """Test that make_data returns the correct shapes.""" @@ -318,9 +343,11 @@ class IoTest(parameterized.TestCase): self.assertEqual(dx.xmat.shape, (3, 3, 3)) self.assertEqual(dx.ximat.shape, (3, 3, 3)) self.assertEqual(dx.geom_xmat.shape, (3, 3, 3)) + self.assertEqual(dx.site_xmat.shape, (1, 3, 3)) np.testing.assert_allclose(dx.xmat.reshape((3, 9)), d.xmat) np.testing.assert_allclose(dx.ximat.reshape((3, 9)), d.ximat) np.testing.assert_allclose(dx.geom_xmat.reshape((3, 9)), d.geom_xmat) + 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 @@ -369,13 +396,15 @@ class IoTest(parameterized.TestCase): self.assertEqual(d_2.contact.frame.shape, (1, 9)) np.testing.assert_allclose(d_2.contact.frame, d.contact.frame) - # xmat, ximat, geom_xmat are all shape transformed + # xmat, ximat, geom_xmat, site_xmat are all shape transformed self.assertEqual(d_2.xmat.shape, (3, 9)) self.assertEqual(d_2.ximat.shape, (3, 9)) self.assertEqual(d_2.geom_xmat.shape, (3, 9)) + self.assertEqual(d_2.site_xmat.shape, (1, 9)) np.testing.assert_allclose(d_2.xmat, d.xmat) np.testing.assert_allclose(d_2.ximat, d.ximat) np.testing.assert_allclose(d_2.geom_xmat, d.geom_xmat) + 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 @@ -402,5 +431,6 @@ class IoTest(parameterized.TestCase): self.assertEqual(ds[0].ncon, 1) self.assertEqual(ds[1].ncon, 0) + if __name__ == '__main__': absltest.main() diff --git a/mjx/mujoco/mjx/_src/scan.py b/mjx/mujoco/mjx/_src/scan.py index ae0eeb30..9035c98e 100644 --- a/mjx/mujoco/mjx/_src/scan.py +++ b/mjx/mujoco/mjx/_src/scan.py @@ -49,7 +49,9 @@ def _take(obj: Y, idx: np.ndarray) -> Y: def take(x): # TODO(erikfrey): if this helps perf, add support for striding too - if ( + if not x.shape[0]: + return x + elif ( len(idx.shape) == 1 and idx.size > 0 and (idx == np.arange(idx[0], idx[0] + idx.size)).all() @@ -113,8 +115,12 @@ def _nvmap(f: Callable[..., Y], *args) -> Y: if isinstance(arg, np.ndarray) and not np.all(arg == arg[0]): raise RuntimeError(f'numpy arg elements do not match: {arg}') + # split out numpy and jax args np_args = [a[0] if isinstance(a, np.ndarray) else None for a in args] args = [a if n is None else None for n, a in zip(np_args, args)] + + # remove empty args that we should not vmap over + args = jax.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): @@ -126,7 +132,15 @@ def _nvmap(f: Callable[..., Y], *args) -> Y: def _check_input(m: Model, args: Any, in_types: str) -> None: """Checks that scan input has the right shape.""" - size = {'b': m.nbody, 'j': m.njnt, 'q': m.nq, 'v': m.nv, 'u': m.nu, 'a': m.na} + size = { + 'b': m.nbody, + 'j': m.njnt, + 'q': m.nq, + 'v': m.nv, + 'u': m.nu, + 'a': m.na, + 's': m.nsite, + } for idx, (arg, typ) in enumerate(zip(args, in_types)): if len(arg) != size[typ]: raise IndexError( @@ -162,7 +176,7 @@ def flat( ) -> Y: r"""Scan a function across bodies or actuators. - Scan group data according to type and batch shape then calls vmap(f) on it. + Scan group data according to type and batch shape then calls vmap(f) on it.\ Args: m: an mjx model @@ -223,14 +237,22 @@ def flat( 'j': ( m.actuator_trnid[i, 0] if m.actuator_trntype[i] == TrnType.JOINT - else np.array(-1) + else -1 + ), + 's': ( + m.actuator_trnid[i, 0] + if m.actuator_trntype[i] == TrnType.SITE + else -1 ), } - # v/q associated with joint transmissions - typ_ids.update({ - 'v': np.nonzero(m.dof_jntid == typ_ids['j'])[0], - 'q': np.nonzero(_q_jointid(m) == typ_ids['j'])[0], - }) + v, q = np.array([-1]), np.array([-1]) + if m.actuator_trntype[i] == TrnType.JOINT: + # v/q are associated with the joint transmissions only + v = np.nonzero(m.dof_jntid == typ_ids['j'])[0] + q = np.nonzero(_q_jointid(m) == typ_ids['j'])[0] + + typ_ids.update({'v': v, 'q': q}) + return typ_ids # build up a grouping of type take-ids in body/actuator order diff --git a/mjx/mujoco/mjx/_src/smooth.py b/mjx/mujoco/mjx/_src/smooth.py index ba470bed..81c53213 100644 --- a/mjx/mujoco/mjx/_src/smooth.py +++ b/mjx/mujoco/mjx/_src/smooth.py @@ -19,11 +19,13 @@ from jax import numpy as jp import mujoco from mujoco.mjx._src import math from mujoco.mjx._src import scan +from mujoco.mjx._src import support # pylint: disable=g-importing-member from mujoco.mjx._src.types import Data from mujoco.mjx._src.types import DisableBit from mujoco.mjx._src.types import JointType from mujoco.mjx._src.types import Model +from mujoco.mjx._src.types import TrnType # pylint: enable=g-importing-member @@ -432,38 +434,55 @@ def rne(m: Model, d: Data) -> Data: def transmission(m: Model, d: Data) -> Data: """Computes actuator/transmission lengths and moments.""" + # TODO: consider combining transmission calculation into fwd_actuation. if not m.nu: return d - def fn(gear, jnt_typ, m_j, qpos): - # handles joint transmissions only - if jnt_typ == JointType.FREE: + def fn(trntype, trnid, gear, jnt_typ, m_j, qpos, site_xpos, site_xmat): + if trntype == TrnType.JOINT: + if jnt_typ == JointType.FREE: + length = jp.zeros(1) + moment = gear + m_j = m_j + jp.arange(6) + elif jnt_typ == JointType.BALL: + axis, angle = math.quat_to_axis_angle(qpos) + length = jp.dot(axis * angle, gear[:3])[None] + moment = gear[:3] + m_j = m_j + jp.arange(3) + elif jnt_typ in (JointType.SLIDE, JointType.HINGE): + length = qpos * gear[0] + moment = gear[:1] + m_j = m_j[None] + else: + raise RuntimeError(f'unrecognized joint type: {JointType(jnt_typ)}') + + moment = jp.zeros((m.nv,)).at[m_j].set(moment) + elif trntype == TrnType.SITE: length = jp.zeros(1) - moment = gear - m_j = m_j + jp.arange(6) - elif jnt_typ == JointType.BALL: - axis, angle = math.quat_to_axis_angle(qpos) - length = jp.dot(axis * angle, gear[:3])[None] - moment = gear[:3] - m_j = m_j + jp.arange(3) - elif jnt_typ in (JointType.SLIDE, JointType.HINGE): - length = qpos * gear[0] - moment = gear[:1] - m_j = m_j[None] + jacp, jacr = support.jac( + m, d, site_xpos, jp.array(m.site_bodyid)[trnid[0]] + ) + jac = jp.concatenate((jacp, jacr), axis=1) + wrench = jp.concatenate((site_xmat @ gear[:3], site_xmat @ gear[3:])) + moment = jac @ wrench else: - raise RuntimeError(f'unrecognized joint type: {jnt_typ}') - moment = jp.zeros((m.nv,)).at[m_j].set(moment) + raise RuntimeError(f'unrecognized trntype: {TrnType(trntype)}') + return length, moment length, moment = scan.flat( m, fn, - 'ujjq', - 'uuuu', + 'uuujjqss', + 'uu', + m.actuator_trntype, + jp.array(m.actuator_trnid), m.actuator_gear, m.jnt_type, jp.array(m.jnt_dofadr), d.qpos, + d.site_xpos, + d.site_xmat, group_by='u', ) length = length.reshape((m.nu,)) diff --git a/mjx/mujoco/mjx/_src/smooth_test.py b/mjx/mujoco/mjx/_src/smooth_test.py index 203cb654..e83779ad 100644 --- a/mjx/mujoco/mjx/_src/smooth_test.py +++ b/mjx/mujoco/mjx/_src/smooth_test.py @@ -132,5 +132,40 @@ class SmoothTest(absltest.TestCase): dx = jax.jit(mjx.rne)(mx, dx) np.testing.assert_allclose(dx.qfrc_bias, 0) + def test_site_transmission(self): + m = mujoco.MjModel.from_xml_string(""" + + + + + + + + + + + + + + + + + + + + + + """) + d = mujoco.MjData(m) + mujoco.mj_forward(m, d) + mx = mjx.put_model(m) + dx = mjx.put_data(m, d) + + mujoco.mj_transmission(m, d) + dx = mjx.transmission(mx, dx) + _assert_attr_eq(d, dx, 'actuator_length') + _assert_attr_eq(d, dx, 'actuator_moment') + + if __name__ == '__main__': absltest.main() diff --git a/mjx/mujoco/mjx/_src/test_util.py b/mjx/mujoco/mjx/_src/test_util.py index 870621f7..66bab2f7 100644 --- a/mjx/mujoco/mjx/_src/test_util.py +++ b/mjx/mujoco/mjx/_src/test_util.py @@ -29,6 +29,8 @@ TEST_FILES: List[str] = [ ] _ACTUATOR_TYPES = ['motor', 'velocity', 'position', 'general', 'intvelocity'] +_DYN_TYPES = ['none', 'integrator', 'filter', 'filterexact'] +_DYN_PRMS = ['0.189', '2.1'] _JOINT_TYPES = ['free', 'hinge', 'slide', 'ball'] _JOINT_AXES = ['1 0 0', '0 1 0', '0 0 1'] _FRICTIONS = ['1.2 0.003 0.0002', '0.2 0.0001 0.0005'] @@ -45,7 +47,7 @@ _SOLIMPS = [ _DIMS = ['3'] _MARGINS = ['0.0', '0.01', '0.02'] _GAPS = ['0.0', '0.005'] -_GEARS = ['20', '50', '100'] +_GEARS = ['2.1 0.0 3.3 0 2.3 0', '5.0 3.1 0 2.3 0.0 1.1'] def p(pct: int) -> bool: @@ -121,12 +123,21 @@ def _make_geom( return attr -def _make_actuator(actuator_type: str, joint: str) -> Dict[str, str]: +def _make_actuator( + actuator_type: str, joint: str | None = None, site: str | None = None +) -> Dict[str, str]: """Returns attributes for an actuator.""" - attr = {'joint': joint} - if actuator_type == 'motor': - attr['gear'] = np.random.choice(_GEARS) - elif actuator_type == 'position': + if joint: + attr = {'joint': joint} + elif site: + attr = {'site': site} + else: + raise ValueError('must provide a joint or site name') + + attr['gear'] = np.random.choice(_GEARS) + + # set actuator type + if actuator_type == 'position': attr['kp'] = np.random.choice(_KP_POS) elif actuator_type == 'general': attr['biastype'] = 'affine' @@ -139,10 +150,18 @@ def _make_actuator(actuator_type: str, joint: str) -> Dict[str, str]: elif actuator_type == 'velocity': attr['kv'] = np.random.choice(_KV_VEL) + # set dyntype + if actuator_type == 'general': + attr['dyntype'] = np.random.choice(_DYN_TYPES) + if attr['dyntype'] != 'none': + attr['dynprm'] = np.random.choice(_DYN_PRMS) + + # ctrlrange if p(50) and actuator_type != 'intvelocity': lb, ub = -np.random.uniform(), np.random.uniform() attr['ctrlrange'] = f'{lb:.2f} {ub:.2f}' + # forcerange if p(50): lb, ub = -np.random.uniform(), np.random.uniform() attr['forcerange'] = f'{lb*10:.2f} {ub*10:.2f}' @@ -233,6 +252,7 @@ def create_mjcf( 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}'}) + ET.SubElement(child, 'site', {'name': f'site{n_bodies}'}) n_joints = len(list(mjcf.iter('joint'))) for nj in range(np.random.randint(1, max_stacked_joints + 1)): @@ -270,17 +290,28 @@ def create_mjcf( for _ in range(num_trees): make_tree(world, 0) + bodies = list(mjcf.iter('body')) + n_bodies = len(bodies) + # actuators if add_actuators: actuator = ET.SubElement(mjcf, 'actuator') n_joints = len(list(mjcf.iter('joint'))) nu = np.random.randint(1, n_joints + 1) actuators = [] + + # joint transmission for i in range(nu): actuator_type = np.random.choice(_ACTUATOR_TYPES) attr = _make_actuator(actuator_type, joint=f'joint{i}') actuators.append((actuator_type, attr)) + # site transmission + for i in range(np.random.randint(0, n_bodies)): + actuator_type = np.random.choice(_ACTUATOR_TYPES) + attr = _make_actuator(actuator_type, site=f'site{i}') + actuators.append((actuator_type, attr)) + np.random.shuffle(actuators) for typ, attr in actuators: ET.SubElement(actuator, typ, attr) @@ -308,9 +339,7 @@ def create_mjcf( ET.SubElement(contact, 'pair', attr) # exclude contacts - bodies = list(mjcf.iter('body')) body_names = [b.get('name') for b in bodies] - n_bodies = len(bodies) for _ in range(min(max_contact_excludes, (n_bodies * (n_bodies - 1) // 2))): if p(50): continue diff --git a/mjx/mujoco/mjx/_src/types.py b/mjx/mujoco/mjx/_src/types.py index 19b37630..ece81623 100644 --- a/mjx/mujoco/mjx/_src/types.py +++ b/mjx/mujoco/mjx/_src/types.py @@ -20,10 +20,7 @@ from typing import Sequence import jax import jax.numpy as jp import mujoco -# pylint: disable=g-importing-member -from mujoco.mjx._src import dataclasses -from mujoco.mjx._src.dataclasses import PyTreeNode -# pylint: enable=g-importing-member +from mujoco.mjx._src.dataclasses import PyTreeNode # pylint: disable=g-importing-member import numpy as np @@ -156,9 +153,11 @@ class TrnType(enum.IntEnum): Attributes: JOINT: force on joint + SITE: force on site """ JOINT = mujoco.mjtTrn.mjTRN_JOINT - # unsupported: JOINTINPARENT, SLIDERCRANK, TENDON, SITE, BODY + SITE = mujoco.mjtTrn.mjTRN_SITE + # unsupported: JOINTINPARENT, SLIDERCRANK, TENDON, BODY class DynType(enum.IntEnum): @@ -167,11 +166,14 @@ class DynType(enum.IntEnum): Attributes: NONE: no internal dynamics; ctrl specifies force INTEGRATOR: integrator: da/dt = u + FILTER: linear filter: da/dt = (u-a) / tau + FILTEREXACT: linear filter: da/dt = (u-a) / tau, with exact integration """ NONE = mujoco.mjtDyn.mjDYN_NONE INTEGRATOR = mujoco.mjtDyn.mjDYN_INTEGRATOR FILTER = mujoco.mjtDyn.mjDYN_FILTER - # unsupported: FILTEREXACT, MUSCLE, USER + FILTEREXACT = mujoco.mjtDyn.mjDYN_FILTEREXACT + # unsupported: MUSCLE, USER class GainType(enum.IntEnum): diff --git a/mjx/pyproject.toml b/mjx/pyproject.toml index a385e9af..b0672ead 100644 --- a/mjx/pyproject.toml +++ b/mjx/pyproject.toml @@ -4,7 +4,7 @@ build-backend = "setuptools.build_meta" [project] name="mujoco-mjx" -version = "3.0.2" +version = "3.1.2" authors = [ {name = "Google DeepMind", email = "mujoco@deepmind.com"}, ] @@ -31,13 +31,13 @@ dependencies = [ "etils[epath]", "jax", "jaxlib", - "mujoco>=3.0.2.dev0", + "mujoco>=3.1.2.dev0", "scipy", "trimesh", ] [project.urls] Homepage = "https://github.com/google-deepmind/mujoco/tree/main/mjx" -Documentation = "https://mujoco.readthedocs.io/en/3.0.2" +Documentation = "https://mujoco.readthedocs.io/en/3.1.2" Repository = "https://github.com/google-deepmind/mujoco/tree/main/mjx" -Changelog = "https://mujoco.readthedocs.io/en/3.0.2/changelog.html" +Changelog = "https://mujoco.readthedocs.io/en/3.1.2/changelog.html" diff --git a/mjx/tutorial.ipynb b/mjx/tutorial.ipynb index 16aa9c32..f27461c6 100644 --- a/mjx/tutorial.ipynb +++ b/mjx/tutorial.ipynb @@ -416,10 +416,8 @@ " forward_reward = self._forward_reward_weight * velocity[0]\n", "\n", " min_z, max_z = self._healthy_z_range\n", - " is_healthy = jp.where(data.qpos[2] \u003c min_z, x=0.0, y=1.0)\n", - " is_healthy = jp.where(\n", - " data.qpos[2] \u003e max_z, x=0.0, y=is_healthy\n", - " )\n", + " is_healthy = jp.where(data.qpos[2] \u003c min_z, 0.0, 1.0)\n", + " is_healthy = jp.where(data.qpos[2] \u003e max_z, 0.0, is_healthy)\n", " if self._terminate_when_unhealthy:\n", " healthy_reward = self._healthy_reward\n", " else:\n", @@ -923,7 +921,9 @@ " 'physics_steps_per_control_step', physics_steps_per_control_step)\n", " super().__init__(mj_model=mj_model, **kwargs)\n", "\n", - " self.torso_idx = 1\n", + " self.torso_idx = mujoco.mj_name2id(\n", + " mj_model, mujoco.mjtObj.mjOBJ_BODY.value, 'torso'\n", + " )\n", " self._action_scale = action_scale\n", " self._obs_noise = obs_noise\n", " self._reset_horizon = 500\n", @@ -940,6 +940,7 @@ " self.reward_config = get_config()\n", " self.lowers = self._default_ap_pose - jp.array([0.2, 0.8, 0.8] * 4)\n", " self.uppers = self._default_ap_pose + jp.array([0.2, 0.8, 0.8] * 4)\n", + " self._foot_radius = 0.014\n", "\n", " def sample_command(self, rng: jax.Array) -\u003e jax.Array:\n", " lin_vel_x = [-0.6, 1.0] # min max [m/s]\n", @@ -1023,12 +1024,15 @@ " joint_vel = qvel[6:]\n", "\n", " # foot contact data based on z-position\n", - " foot_contact = 0.017 - self._get_feet_pos_vel(x, xd)[0][:, 2]\n", - " contact = foot_contact \u003e -1e-3 # a mm or less off the floor\n", + " foot_contact_pos = (\n", + " self._get_feet_pos_vel(x, xd)[0][:, 2]\n", + " - self._foot_radius\n", + " )\n", + " contact = foot_contact_pos \u003c 1e-3 # a mm or less off the floor\n", " contact_filt_mm = jp.logical_or(contact, state.info['last_contact'])\n", " contact_filt_cm = jp.logical_or(\n", - " foot_contact \u003e -1e-2, state.info['last_contact']\n", - " )\n", + " foot_contact_pos \u003c 3e-2, state.info['last_contact']\n", + " ) # 3cm or less off the floor\n", " first_contact = (state.info['feet_air_time'] \u003e 0) * (contact_filt_mm)\n", " state.info['feet_air_time'] += self.dt\n", "\n", @@ -1100,29 +1104,26 @@ " state.info.update(rng=rng)\n", "\n", " # resetting logic if joint limits are reached or robot is falling\n", - " done = 0.0\n", " up = jp.array([0.0, 0.0, 1.0])\n", - " done = jp.where(jp.dot(math.rotate(up, x.rot[0]), up) \u003c 0, 1.0, done)\n", - " done = jp.where(jp.logical_or(\n", - " jp.any(joint_angles \u003c .98 * self.lowers),\n", - " jp.any(joint_angles \u003e .98 * self.uppers)), 1.0, done)\n", - " done = jp.where(x.pos[self.torso_idx, 2] \u003c 0.18, 1.0, done)\n", + " done = jp.dot(math.rotate(up, x.rot[0]), up) \u003c 0\n", + " done |= jp.any(joint_angles \u003c 0.98 * self.lowers)\n", + " done |= jp.any(joint_angles \u003e 0.98 * self.uppers)\n", + " done |= x.pos[0, 2] \u003c 0.18\n", "\n", " # termination reward\n", - " reward += jp.where(\n", - " (done == 1.0) \u0026 (state.info['step'] \u003c self._reset_horizon),\n", - " self.reward_config.rewards.scales.termination,\n", - " 0.0,\n", + " reward += (\n", + " done * (state.info['step'] \u003c self._reset_horizon) *\n", + " self.reward_config.rewards.scales.termination\n", " )\n", "\n", " # when done, sample new command if more than _reset_horizon timesteps\n", " # achieved\n", " state.info['command'] = jp.where(\n", - " (done == 1.0) \u0026 (state.info['step'] \u003e self._reset_horizon),\n", + " done \u0026 (state.info['step'] \u003e self._reset_horizon),\n", " self.sample_command(cmd_rng), state.info['command'])\n", " # reset the step counter when done\n", " state.info['step'] = jp.where(\n", - " (done == 1.0) | (state.info['step'] \u003e self._reset_horizon), 0,\n", + " done | (state.info['step'] \u003e self._reset_horizon), 0,\n", " state.info['step']\n", " )\n", "\n", @@ -1133,7 +1134,7 @@ "\n", " state = state.replace(\n", " pipeline_state=data, obs=obs + obs_noise, reward=reward,\n", - " done=done)\n", + " done=done * 1.0)\n", " return state\n", "\n", " def _get_obs(self, qpos: jax.Array, x: Transform, xd: Motion,\n", @@ -1233,7 +1234,8 @@ " self, x: Transform, xd: Motion) -\u003e Tuple[jax.Array, jax.Array]:\n", " offset = Transform.create(pos=self._feet_pos)\n", " pos = x.take(self._feet_index).vmap().do(offset).pos\n", - " vel = offset.vmap().do(xd.take(self._feet_index)).vel\n", + " world_offset = Transform.create(pos=pos - x.take(self._feet_index).pos)\n", + " vel = world_offset.vmap().do(xd.take(self._feet_index)).vel\n", " return pos, vel\n", "\n", " def _reward_foot_slip(\n", @@ -1405,8 +1407,8 @@ "private_outputs": true, "provenance": [ { - "file_id": "1brcF4_qCRS2ASc-QQw1rsEwl5IjzGvq2", - "timestamp": 1697763780236 + "file_id": "1QsuS7EJhdPEHxxAu9XwozvA7eb4ZnlAb", + "timestamp": 1701993737024 } ], "toc_visible": true diff --git a/plugin/actuator/README.md b/plugin/actuator/README.md index 80676921..9139aed1 100644 --- a/plugin/actuator/README.md +++ b/plugin/actuator/README.md @@ -38,16 +38,10 @@ You can use it like: The available options are: -|Attribute|Default |Meaning | -|---------|--------|-------------------------------------------------------------------------------------------------------------------------------------| -|`kp` |0 |**P** gain for the controller. | -|`ki` |0 |**I** gain for the controller. | -: : : : -: : :If nonzero, one activation variable will be added to `mjData.act`, containing the current I term (in units of force). : -|`kd` |0 |**D** gain for the controller. | -|`imax` |Optional|If specified, the force produced by the I term will be clipped to the range `[-imax, -imax]`. | -|`slewmax`|Optional|The maximum rate at which the setpoint for the PID controller can change. | -: : : : -: : :If a bigger change is requested between two timesteps, it will be clipped to the range `[ctrl - slewmax * dt, ctrl + slewmax * dt]` : -: : : : -: : :If specified, one activation variable will be added to `mjData.act` containing the previous value of `ctrl`. : +|Attribute | Default | Meaning | +|----------|---------|---------| +|`kp` | 0 | **P** gain for the controller. | +|`ki` | 0 | **I** gain for the controller.

If nonzero, one activation variable will be added to `mjData.act`, containing the current I term (in units of force). | +|`kd` | 0 | **D** gain for the controller. | +|`imax` | Optional | If specified, the force produced by the I term will be clipped to the range `[-imax, -imax]`. | +|`slewmax` | Optional | The maximum rate at which the setpoint for the PID controller can change.

If a bigger change is requested between two timesteps, it will be clipped to the range `[ctrl - slewmax * dt, ctrl + slewmax * dt]`

If specified, one activation variable will be added to `mjData.act` containing the previous value of `ctrl`. | diff --git a/python/mujoco/CMakeLists.txt b/python/mujoco/CMakeLists.txt index 256adb10..ed90bd7d 100644 --- a/python/mujoco/CMakeLists.txt +++ b/python/mujoco/CMakeLists.txt @@ -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.0.2.dylib + ${MUJOCO_FRAMEWORK}/mujoco.framework/Versions/A/libmujoco.3.1.2.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.0.2 HINTS ${MUJOCO_LIBRARY_DIR} REQUIRED + MUJOCO_LIBRARY mujoco mujoco.3.1.2 HINTS ${MUJOCO_LIBRARY_DIR} REQUIRED ) find_path(MUJOCO_INCLUDE mujoco/mujoco.h HINTS ${MUJOCO_INCLUDE_DIR} REQUIRED) message("MuJoCo is at ${MUJOCO_LIBRARY}") @@ -173,7 +173,7 @@ findorfetch( GIT_REPO https://gitlab.com/libeigen/eigen GIT_TAG - aa6964bf3a34fd607837dd8123bc42465185c4f8 + 454f89af9d6f3525b1df5f9ef9c86df58bf2d4d3 TARGETS Eigen3::Eigen EXCLUDE_FROM_ALL diff --git a/python/mujoco/bindings_test.py b/python/mujoco/bindings_test.py index 1222569c..3d2e0ac6 100644 --- a/python/mujoco/bindings_test.py +++ b/python/mujoco/bindings_test.py @@ -840,7 +840,7 @@ Euler integrator, semi-implicit in velocity. self.assertEqual(mujoco.mjtGeom.mjGEOM_ARROW, 100) self.assertEqual(mujoco.mjtGeom.mjGEOM_ARROW1, 101) self.assertEqual(mujoco.mjtGeom.mjGEOM_ARROW2, 102) - self.assertEqual(mujoco.mjtGeom.mjGEOM_TRIANGLE, 107) + self.assertEqual(mujoco.mjtGeom.mjGEOM_TRIANGLE, 108) self.assertEqual(mujoco.mjtGeom.mjGEOM_NONE, 1001) def test_enum_from_int(self): diff --git a/python/mujoco/cgl/__init__.py b/python/mujoco/cgl/__init__.py index ba1e36a6..36037116 100644 --- a/python/mujoco/cgl/__init__.py +++ b/python/mujoco/cgl/__init__.py @@ -57,15 +57,18 @@ class GLContext: def free(self): """Frees resources associated with this context.""" - if self._context: - cgl.CGLUnlockContext(self._context) - cgl.CGLSetCurrentContext(None) - cgl.CGLReleaseContext(self._context) - self._context = None + try: + if self._context: + cgl.CGLUnlockContext(self._context) + cgl.CGLSetCurrentContext(None) + cgl.CGLReleaseContext(self._context) + self._context = None - if self._pix: - cgl.CGLReleasePixelFormat(self._pix) - self._context = None + if self._pix: + cgl.CGLReleasePixelFormat(self._pix) + self._pix = None + except Exception: # pylint: disable=broad-exception-caught + pass def __del__(self): self.free() diff --git a/python/mujoco/glfw/__init__.py b/python/mujoco/glfw/__init__.py index a5693bec..0fd8bd9a 100644 --- a/python/mujoco/glfw/__init__.py +++ b/python/mujoco/glfw/__init__.py @@ -35,7 +35,7 @@ class GLContext: if glfw.get_current_context() == self._context: glfw.make_context_current(None) glfw.destroy_window(self._context) - self._context = None + self._context = None def __del__(self): self.free() diff --git a/python/mujoco/mjpython/Info.plist b/python/mujoco/mjpython/Info.plist index 6c6d68b2..e4cb8947 100644 --- a/python/mujoco/mjpython/Info.plist +++ b/python/mujoco/mjpython/Info.plist @@ -7,13 +7,13 @@ CFBundleIdentifier org.mujoco.mjpython CFBundleVersion - 3.0.2 + 3.1.2 CFBundleGetInfoString - 3.0.2 + 3.1.2 CFBundleLongVersionString - 3.0.2 + 3.1.2 CFBundleShortVersionString - 3.0.2 + 3.1.2 CFBundleExecutable mjpython CFBundleIconFile diff --git a/python/mujoco/renderer.py b/python/mujoco/renderer.py index f34c8243..dfb387c6 100644 --- a/python/mujoco/renderer.py +++ b/python/mujoco/renderer.py @@ -136,6 +136,9 @@ the clause: A new numpy array holding the pixels with shape `(H, W)` or `(H, W, 3)`, depending on the value of `self._depth_rendering` unless `out is None`, in which case a reference to `out` is returned. + + Raises: + RuntimeError: if this method is called after the close method. """ original_flags = self._scene.flags.copy() @@ -145,6 +148,8 @@ the clause: self._scene.flags[_enums.mjtRndFlag.mjRND_SEGMENT] = True self._scene.flags[_enums.mjtRndFlag.mjRND_IDCOLOR] = True + if self._gl_context is None: + raise RuntimeError('render cannot be called after close.') self._gl_context.make_current() if self._depth_rendering: @@ -288,3 +293,35 @@ the clause: camera, _enums.mjtCatBit.mjCAT_ALL.value, self._scene, ) + + def close(self) -> None: + """Frees the resources used by the renderer. + + This method can be used directly: + + ```python + renderer = Renderer(...) + # Use renderer. + renderer.close() + ``` + + or via a context manager: + + ```python + with Renderer(...) as renderer: + # Use renderer. + ``` + """ + if self._gl_context: + self._gl_context.free() + self._gl_context = None + + def __enter__(self): + return self + + def __exit__(self, exc_type, exc_value, traceback): + del exc_type, exc_value, traceback # Unused. + self.close() + + def __del__(self) -> None: + self.close() diff --git a/python/mujoco/renderer_test.py b/python/mujoco/renderer_test.py index 8aa44a97..e8022006 100644 --- a/python/mujoco/renderer_test.py +++ b/python/mujoco/renderer_test.py @@ -33,10 +33,10 @@ class MuJoCoRendererTest(parameterized.TestCase): """ model = mujoco.MjModel.from_xml_string(xml) data = mujoco.MjData(model) - renderer = mujoco.Renderer(model, 50, 50) - mujoco.mj_forward(model, data) - with self.assertRaisesRegex(ValueError, r'camera "b" does not exist'): - renderer.update_scene(data, 'b') + with mujoco.Renderer(model, 50, 50) as renderer: + mujoco.mj_forward(model, data) + with self.assertRaisesRegex(ValueError, r'camera "b" does not exist'): + renderer.update_scene(data, 'b') def test_renderer_camera_under_range(self): xml = """ @@ -48,10 +48,10 @@ class MuJoCoRendererTest(parameterized.TestCase): """ model = mujoco.MjModel.from_xml_string(xml) data = mujoco.MjData(model) - renderer = mujoco.Renderer(model, 50, 50) - mujoco.mj_forward(model, data) - with self.assertRaisesRegex(ValueError, '-2 is out of range'): - renderer.update_scene(data, -2) + with mujoco.Renderer(model, 50, 50) as renderer: + mujoco.mj_forward(model, data) + with self.assertRaisesRegex(ValueError, '-2 is out of range'): + renderer.update_scene(data, -2) def test_renderer_camera_over_range(self): xml = """ @@ -63,10 +63,10 @@ class MuJoCoRendererTest(parameterized.TestCase): """ model = mujoco.MjModel.from_xml_string(xml) data = mujoco.MjData(model) - renderer = mujoco.Renderer(model, 50, 50) - mujoco.mj_forward(model, data) - with self.assertRaisesRegex(ValueError, '1 is out of range'): - renderer.update_scene(data, 1) + with mujoco.Renderer(model, 50, 50) as renderer: + mujoco.mj_forward(model, data) + with self.assertRaisesRegex(ValueError, '1 is out of range'): + renderer.update_scene(data, 1) def test_renderer_renders_scene(self): xml = """ @@ -79,19 +79,19 @@ class MuJoCoRendererTest(parameterized.TestCase): """ model = mujoco.MjModel.from_xml_string(xml) data = mujoco.MjData(model) - renderer = mujoco.Renderer(model, 50, 50) - mujoco.mj_forward(model, data) - renderer.update_scene(data, 'closeup') + with mujoco.Renderer(model, 50, 50) as renderer: + mujoco.mj_forward(model, data) + renderer.update_scene(data, 'closeup') - pixels = renderer.render().flatten() - not_all_black = False + pixels = renderer.render().flatten() + not_all_black = False - # Pixels should all be a neutral color. - for pixel in pixels: - if pixel > 0: - not_all_black = True - break - self.assertTrue(not_all_black) + # Pixels should all be a neutral color. + for pixel in pixels: + if pixel > 0: + not_all_black = True + break + self.assertTrue(not_all_black) def test_renderer_output_without_out(self): xml = """ @@ -105,25 +105,25 @@ class MuJoCoRendererTest(parameterized.TestCase): model = mujoco.MjModel.from_xml_string(xml) data = mujoco.MjData(model) mujoco.mj_forward(model, data) - renderer = mujoco.Renderer(model, 50, 50) - renderer.update_scene(data, 'closeup') - pixels = [renderer.render()] - - colors = ( - (1.0, 0.0, 0.0, 1.0), - (0.0, 1.0, 0.0, 1.0), - (0.0, 0.0, 1.0, 1.0), - ) - - for i, color in enumerate(colors): - model.geom_rgba[0, :] = color - mujoco.mj_forward(model, data) + with mujoco.Renderer(model, 50, 50) as renderer: renderer.update_scene(data, 'closeup') - pixels.append(renderer.render()) - self.assertIsNot(pixels[-2], pixels[-1]) + pixels = [renderer.render()] - # Pixels should change over steps. - self.assertFalse((pixels[i + 1] == pixels[i]).all()) + colors = ( + (1.0, 0.0, 0.0, 1.0), + (0.0, 1.0, 0.0, 1.0), + (0.0, 0.0, 1.0, 1.0), + ) + + for i, color in enumerate(colors): + model.geom_rgba[0, :] = color + mujoco.mj_forward(model, data) + renderer.update_scene(data, 'closeup') + pixels.append(renderer.render()) + self.assertIsNot(pixels[-2], pixels[-1]) + + # Pixels should change over steps. + self.assertFalse((pixels[i + 1] == pixels[i]).all()) def test_renderer_output_with_out(self): xml = """ @@ -139,23 +139,21 @@ class MuJoCoRendererTest(parameterized.TestCase): model = mujoco.MjModel.from_xml_string(xml) data = mujoco.MjData(model) mujoco.mj_forward(model, data) - renderer = mujoco.Renderer(model, *render_size) - renderer.update_scene(data, 'closeup') + with mujoco.Renderer(model, *render_size) as renderer: + renderer.update_scene(data, 'closeup') - self.assertTrue(np.all(render_out == 0)) + self.assertTrue(np.all(render_out == 0)) - pixels = renderer.render(out=render_out) + pixels = renderer.render(out=render_out) - # Pixels should always refer to the same `render_out` array. - self.assertIs(pixels, render_out) - self.assertFalse(np.all(render_out == 0)) + # Pixels should always refer to the same `render_out` array. + self.assertIs(pixels, render_out) + self.assertFalse(np.all(render_out == 0)) - failing_render_size = (10, 10) - self.assertNotEqual(failing_render_size, render_size) - with self.assertRaises(ValueError): - pixels = renderer.render( - out=np.zeros((*failing_render_size, 3), np.uint8) - ) + failing_render_size = (10, 10) + self.assertNotEqual(failing_render_size, render_size) + with self.assertRaises(ValueError): + renderer.render(out=np.zeros((*failing_render_size, 3), np.uint8)) if __name__ == '__main__': diff --git a/python/pyproject.toml b/python/pyproject.toml index 81b7da8e..e8a31471 100644 --- a/python/pyproject.toml +++ b/python/pyproject.toml @@ -4,7 +4,7 @@ build-backend = "setuptools.build_meta" [project] name = "mujoco" -version = "3.0.2" +version = "3.1.2" 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.0.2" +Documentation = "https://mujoco.readthedocs.io/en/3.1.2" Repository = "https://github.com/google-deepmind/mujoco" -Changelog = "https://mujoco.readthedocs.io/en/3.0.2/changelog.html" +Changelog = "https://mujoco.readthedocs.io/en/3.1.2/changelog.html" [tool.setuptools] include-package-data = false diff --git a/sample/CMakeLists.txt b/sample/CMakeLists.txt index 3dceb85c..3df0c6ce 100644 --- a/sample/CMakeLists.txt +++ b/sample/CMakeLists.txt @@ -24,7 +24,7 @@ set(MSVC_INCREMENTAL_DEFAULT ON) project( mujoco_samples - VERSION 3.0.2 + VERSION 3.1.2 DESCRIPTION "MuJoCo samples binaries" HOMEPAGE_URL "https://mujoco.org" ) diff --git a/simulate/CMakeLists.txt b/simulate/CMakeLists.txt index 464fc4c5..670aa6e2 100644 --- a/simulate/CMakeLists.txt +++ b/simulate/CMakeLists.txt @@ -29,7 +29,7 @@ set(MUJOCO_DEP_VERSION_lodepng project( mujoco_simulate - VERSION 3.0.2 + VERSION 3.1.2 DESCRIPTION "MuJoCo simulate binaries" HOMEPAGE_URL "https://mujoco.org" ) diff --git a/simulate/simulate.cc b/simulate/simulate.cc index 0b2a150a..226e290d 100644 --- a/simulate/simulate.cc +++ b/simulate/simulate.cc @@ -1817,6 +1817,9 @@ void Simulate::Sync() { if (!m_) { return; } + if (this->exitrequest.load()) { + return; + } bool update_profiler = this->profiler && (this->pause_update || this->run); bool update_sensor = this->sensor && (this->pause_update || this->run); @@ -2476,7 +2479,14 @@ void Simulate::Render() { // show pause/loading label if (!this->run || this->loadrequest) { - const char* label = this->loadrequest ? "LOADING..." : "PAUSE"; + char label[30] = {'\0'}; + if (this->loadrequest) { + std::snprintf(label, sizeof(label), "LOADING..."); + } else if (this->scrub_index == 0) { + std::snprintf(label, sizeof(label), "PAUSE"); + } else { + std::snprintf(label, sizeof(label), "PAUSE (%d)", this->scrub_index); + } mjr_overlay(mjFONT_BIG, mjGRID_TOP, smallrect, label, nullptr, &this->platform_ui->mjr_context()); } @@ -2707,9 +2717,9 @@ void Simulate::RenderLoop() { } } - if (!is_passive_){ - mjv_freeScene(&this->scn); - } else { + const MutexLock lock(this->mtx); + mjv_freeScene(&this->scn); + if (is_passive_) { mjv_freeSceneState(&scnstate_); } diff --git a/src/engine/engine_collision_driver.c b/src/engine/engine_collision_driver.c index 10920162..f6132b74 100644 --- a/src/engine/engine_collision_driver.c +++ b/src/engine/engine_collision_driver.c @@ -1524,16 +1524,49 @@ void mj_collideGeoms(const mjModel* m, mjData* d, int g1, int g2) { mjERROR("too many contacts returned by collision function"); } - // remove repeated contacts in box-box + // remove bad and repeated contacts in box-box if (type1 == mjGEOM_BOX && type2 == mjGEOM_BOX) { // use dim field to mark: -1: bad, 0: good for (int i=0; i < num; i++) { con[i].dim = 0; } - // find bad + // get box info + const mjtNum* pos1 = d->geom_xpos + 3 * g1; + const mjtNum* mat1 = d->geom_xmat + 9 * g1; + const mjtNum* size1 = m->geom_size + 3 * g1; + const mjtNum* pos2 = d->geom_xpos + 3 * g2; + const mjtNum* mat2 = d->geom_xmat + 9 * g2; + const mjtNum* size2 = m->geom_size + 3 * g2; + + // find bad: contacts outside one of the boxes + for (int i=0; i < num; i++) { + // box sizes with margin + mjtNum sz1[3] = {size1[0] + margin, size1[1] + margin, size1[2] + margin}; + mjtNum sz2[3] = {size2[0] + margin, size2[1] + margin, size2[2] + margin}; + + // relative distance from surface (1%) outside of which box-box contacts are removed + static mjtNum kRemoveRatio = 1.01; + + // is the contact outside: 1, inside: -1, within the removal width: 0 + int out1 = mju_outsideBox(con[i].pos, pos1, mat1, sz1, kRemoveRatio); + int out2 = mju_outsideBox(con[i].pos, pos2, mat2, sz2, kRemoveRatio); + + // mark as bad if outside one box and not inside the other box + if ((out1 == 1 && out2 != -1) || (out2 == 1 && out1 != -1)) { + con[i].dim = -1; + } + } + + // find duplicates for (int i=0; i < num-1; i++) { + if (con[i].dim == -1) { + continue; // already marked bad: skip + } for (int j=i+1; j < num; j++) { + if (con[j].dim == -1) { + continue; // already marked bad: skip + } if (con[i].pos[0] == con[j].pos[0] && con[i].pos[1] == con[j].pos[1] && con[i].pos[2] == con[j].pos[2]) { @@ -1546,6 +1579,7 @@ void mj_collideGeoms(const mjModel* m, mjData* d, int g1, int g2) { // consolidate good int i = 0; for (int j=0; j < num; j++) { + // good: maybe copy if (con[j].dim == 0) { // different: copy if (i < j) { diff --git a/src/engine/engine_collision_primitive.h b/src/engine/engine_collision_primitive.h index 49946707..8156905e 100644 --- a/src/engine/engine_collision_primitive.h +++ b/src/engine/engine_collision_primitive.h @@ -16,6 +16,7 @@ #define MUJOCO_SRC_ENGINE_ENGINE_COLLISION_PRIMITIVE_H_ #include +#include #include // define and extract geom info @@ -47,32 +48,32 @@ int mjraw_SphereTriangle(mjContact* con, mjtNum margin, const mjtNum* t1, const mjtNum* t2, const mjtNum* t3, mjtNum rt); // plane collisions -int mjc_PlaneSphere (const mjModel* m, const mjData* d, - mjContact* con, int g1, int g2, mjtNum margin); -int mjc_PlaneCapsule (const mjModel* m, const mjData* d, - mjContact* con, int g1, int g2, mjtNum margin); -int mjc_PlaneCylinder (const mjModel* m, const mjData* d, - mjContact* con, int g1, int g2, mjtNum margin); -int mjc_PlaneBox (const mjModel* m, const mjData* d, - mjContact* con, int g1, int g2, mjtNum margin); +MJAPI int mjc_PlaneSphere (const mjModel* m, const mjData* d, + mjContact* con, int g1, int g2, mjtNum margin); +MJAPI int mjc_PlaneCapsule (const mjModel* m, const mjData* d, + mjContact* con, int g1, int g2, mjtNum margin); +MJAPI int mjc_PlaneCylinder (const mjModel* m, const mjData* d, + mjContact* con, int g1, int g2, mjtNum margin); +MJAPI int mjc_PlaneBox (const mjModel* m, const mjData* d, + mjContact* con, int g1, int g2, mjtNum margin); // sphere and capsule collisions -int mjc_SphereSphere (const mjModel* m, const mjData* d, - mjContact* con, int g1, int g2, mjtNum margin); -int mjc_SphereCapsule (const mjModel* m, const mjData* d, - mjContact* con, int g1, int g2, mjtNum margin); -int mjc_SphereCylinder (const mjModel* m, const mjData* d, - mjContact* con, int g1, int g2, mjtNum margin); -int mjc_CapsuleCapsule (const mjModel* m, const mjData* d, - mjContact* con, int g1, int g2, mjtNum margin); +MJAPI int mjc_SphereSphere (const mjModel* m, const mjData* d, + mjContact* con, int g1, int g2, mjtNum margin); +MJAPI int mjc_SphereCapsule (const mjModel* m, const mjData* d, + mjContact* con, int g1, int g2, mjtNum margin); +MJAPI int mjc_SphereCylinder (const mjModel* m, const mjData* d, + mjContact* con, int g1, int g2, mjtNum margin); +MJAPI int mjc_CapsuleCapsule (const mjModel* m, const mjData* d, + mjContact* con, int g1, int g2, mjtNum margin); // box collisions: from engine_collision_box.c -int mjc_CapsuleBox (const mjModel* m, const mjData* d, - mjContact* con, int g1, int g2, mjtNum margin); -int mjc_SphereBox (const mjModel* m, const mjData* d, - mjContact* con, int g1, int g2, mjtNum margin); -int mjc_BoxBox (const mjModel* m, const mjData* d, - mjContact* con, int g1, int g2, mjtNum margin); +MJAPI int mjc_CapsuleBox (const mjModel* m, const mjData* d, + mjContact* con, int g1, int g2, mjtNum margin); +MJAPI int mjc_SphereBox (const mjModel* m, const mjData* d, + mjContact* con, int g1, int g2, mjtNum margin); +MJAPI int mjc_BoxBox (const mjModel* m, const mjData* d, + mjContact* con, int g1, int g2, mjtNum margin); #ifdef __cplusplus } diff --git a/src/engine/engine_core_constraint.c b/src/engine/engine_core_constraint.c index d1cb4a6c..fbb8ff9a 100644 --- a/src/engine/engine_core_constraint.c +++ b/src/engine/engine_core_constraint.c @@ -1325,6 +1325,19 @@ static void getposdim(const mjModel* m, const mjData* d, int i, mjtNum* pos, int +// return a to the power of b, quick return for powers 1 and 2 +// solimp[4] == 2 is the default, so these branches are common +static mjtNum power(mjtNum a, mjtNum b) { + if (b == 1) { + return a; + } else if (b == 2) { + return a*a; + } + return mju_pow(a, b); +} + + + // compute impedance and derivative for one constraint static void getimpedance(const mjtNum* solimp, mjtNum pos, mjtNum margin, mjtNum* imp, mjtNum* impP) { @@ -1359,16 +1372,16 @@ static void getimpedance(const mjtNum* solimp, mjtNum pos, mjtNum margin, // y(x) = a*x^p if x<=midpoint else if (x <= solimp[3]) { - mjtNum a = 1/mju_pow(solimp[3], solimp[4]-1); - y = a*mju_pow(x, solimp[4]); - yP = solimp[4] * a*mju_pow(x, solimp[4]-1); + mjtNum a = 1/power(solimp[3], solimp[4]-1); + y = a*power(x, solimp[4]); + yP = solimp[4] * a*power(x, solimp[4]-1); } // y(x) = 1-b*(1-x)^p is x>midpoint else { - mjtNum b = 1/mju_pow(1-solimp[3], solimp[4]-1); - y = 1-b*mju_pow(1-x, solimp[4]); - yP = solimp[4] * b*mju_pow(1-x, solimp[4]-1); + mjtNum b = 1/power(1-solimp[3], solimp[4]-1); + y = 1-b*power(1-x, solimp[4]); + yP = solimp[4] * b*power(1-x, solimp[4]-1); } // scale diff --git a/src/engine/engine_io.c b/src/engine/engine_io.c index 32994e9c..ca55358a 100644 --- a/src/engine/engine_io.c +++ b/src/engine/engine_io.c @@ -1858,6 +1858,8 @@ static int numObjects(const mjModel* m, mjtObj objtype) { return m->nkey; case mjOBJ_PLUGIN: return m->nplugin; + case mjNOBJECT: + return -2; } return -2; } diff --git a/src/engine/engine_support.c b/src/engine/engine_support.c index b223a3e2..dfa91d0a 100644 --- a/src/engine/engine_support.c +++ b/src/engine/engine_support.c @@ -38,8 +38,8 @@ //-------------------------- Constants ------------------------------------------------------------- - #define mjVERSION 302 -#define mjVERSIONSTRING "3.0.2" + #define mjVERSION 312 +#define mjVERSIONSTRING "3.1.2" // names of disable flags const char* mjDISABLESTRING[mjNDISABLE] = { diff --git a/src/engine/engine_util_misc.c b/src/engine/engine_util_misc.c index 30eb0d2a..37eca9e4 100644 --- a/src/engine/engine_util_misc.c +++ b/src/engine/engine_util_misc.c @@ -837,6 +837,52 @@ mjtNum mju_springDamper(mjtNum pos0, mjtNum vel0, mjtNum k, mjtNum b, mjtNum t) +// return 1 if point is outside box given by pos, mat, size * inflate +// return -1 if point is inside box given by pos, mat, size / inflate +// return 0 if point is between the inflated and deflated boxes +int mju_outsideBox(const mjtNum point[3], const mjtNum pos[3], const mjtNum mat[9], + const mjtNum size[3], mjtNum inflate) { + // check inflation coefficient + if (inflate < 1) { + mjERROR("inflation coefficient must be >= 1") + } + + // vector from pos to point, projected to box frame + mjtNum vec[3] = {point[0]-pos[0], point[1]-pos[1], point[2]-pos[2]}; + mju_rotVecMatT(vec, vec, mat); + + // big: inflated box + mjtNum big[3] = {size[0], size[1], size[2]}; + if (inflate > 1) { + mju_scl3(big, big, inflate); + } + + // check if outside big box + if (vec[0] > big[0] || vec[0] < -big[0] || + vec[1] > big[1] || vec[1] < -big[1] || + vec[2] > big[2] || vec[2] < -big[2]) { + return 1; + } + + // quick return if no inflation + if (inflate == 1) { + return -1; + } + + // check if inside small (deflated) box + mjtNum small[3] = {size[0]/inflate, size[1]/inflate, size[2]/inflate}; + if (vec[0] < small[0] && vec[0] > -small[0] && + vec[1] < small[1] && vec[1] > -small[1] && + vec[2] < small[2] && vec[2] > -small[2]) { + return -1; + } + + // within margin between small and big box + return 0; +} + + + // print matrix to screen void mju_printMat(const mjtNum* mat, int nr, int nc) { for (int r=0; r < nr; r++) { diff --git a/src/engine/engine_util_misc.h b/src/engine/engine_util_misc.h index 32417f2c..cb317258 100644 --- a/src/engine/engine_util_misc.h +++ b/src/engine/engine_util_misc.h @@ -78,6 +78,12 @@ MJAPI void mju_decodePyramid(mjtNum* force, const mjtNum* pyramid, // integrate spring-damper analytically, return pos(dt) MJAPI mjtNum mju_springDamper(mjtNum pos0, mjtNum vel0, mjtNum Kp, mjtNum Kv, mjtNum dt); +// return 1 if point is outside box given by pos, mat, size * inflate +// return -1 if point is inside box given by pos, mat, size / inflate +// return 0 if point is between the inflated and deflated boxes +MJAPI int mju_outsideBox(const mjtNum point[3], const mjtNum pos[3], const mjtNum mat[9], + const mjtNum size[3], mjtNum inflate); + // print matrix MJAPI void mju_printMat(const mjtNum* mat, int nr, int nc); diff --git a/src/engine/engine_vis_state.c b/src/engine/engine_vis_state.c index 5ef3c274..55d35497 100644 --- a/src/engine/engine_vis_state.c +++ b/src/engine/engine_vis_state.c @@ -86,6 +86,11 @@ void mjv_makeSceneState(const mjModel* m, const mjData* d, mjvSceneState* scnsta #undef XMJV #undef X + // create an arena in the scnstate, to allow visualization code to use the stack. + // TODO: Consider allocating way less than narena, since stack allocations in + // visualization code are much smaller than the arena space required by the model, + // typically. + scnstate->nbuffer += roundUpToCacheLine(m->narena); // buffer space required for contacts int condimmax = mj_isPyramidal(m) ? 10 : 6; scnstate->nbuffer += roundUpToCacheLine(sizeof(*d->contact) * maxgeom); @@ -118,6 +123,10 @@ void mjv_makeSceneState(const mjModel* m, const mjData* d, mjvSceneState* scnsta #undef XMJV #undef X + scnstate->model.narena = m->narena; + scnstate->data.arena = (void*)ptr; + ptr += roundUpToCacheLine(m->narena); + scnstate->data.contact = (mjContact*)ptr; ptr += roundUpToCacheLine(sizeof(*scnstate->data.contact) * scnstate->maxgeom); @@ -177,6 +186,7 @@ void mjv_assignFromSceneState(const mjvSceneState* scnstate, mjModel* m, mjData* m->opt = scnstate->model.opt; m->vis = scnstate->model.vis; m->stat = scnstate->model.stat; + m->narena = scnstate->model.narena; #define X(dtype, var, dim0, dim1) #define XMJV(dtype, var, dim0, dim1) m->var = scnstate->model.var; @@ -194,10 +204,16 @@ void mjv_assignFromSceneState(const mjvSceneState* scnstate, mjModel* m, mjData* #endif memcpy(d->warning, scnstate->data.warning, sizeof(d->warning)); + d->threadpool = 0; d->nefc = scnstate->data.nefc; d->ncon = scnstate->data.ncon; d->nisland = scnstate->data.nisland; d->time = scnstate->data.time; + d->narena = scnstate->model.narena; + d->arena = scnstate->data.arena; + d->parena = 0; + d->pbase = 0; + d->pstack = 0; #define X(dtype, var, dim0, dim1) #define XMJV(dtype, var, dim0, dim1) d->var = scnstate->data.var; diff --git a/src/engine/engine_vis_visualize.c b/src/engine/engine_vis_visualize.c index 9154fa4d..d2e2de20 100644 --- a/src/engine/engine_vis_visualize.c +++ b/src/engine/engine_vis_visualize.c @@ -62,7 +62,7 @@ static void makeLabel(const mjModel* m, mjtObj type, int id, char* label) { } // copy result into label - strncpy(label, txt, 99); + strncpy(label, txt, 100); label[99] = '\0'; } @@ -509,56 +509,6 @@ static int bodycategory(const mjModel* m, int bodyid) { -// draw bounding box -static void drawBoundingBox(mjData* d, mjvScene* scn, - const mjtNum aabb[6], const mjtNum xpos[3], - const mjtNum xmat[9], const float rgba[4]) { - mjtNum x[3]; - mjtNum dist[3][3]; - mjvGeom* thisgeom; - int category = mjCAT_DECOR; - int objtype = mjOBJ_UNKNOWN; - int i = -1; - - if (xmat != NULL) { - mju_rotVecMat(x, aabb, xmat); - mju_addTo3(x, xpos); - for (int j=0; j < 3; j++) { - for (int k=0; k < 3; k++) { - dist[k][j] = aabb[k+3] * xmat[3*j+k]; - } - } - } else { - mju_copy3(x, aabb); - mju_addTo3(x, xpos); - for (int j=0; j < 3; j++) { - mju_zero3(dist[j]); - dist[j][j] = aabb[j+3]; - } - } - - int split[3] = {1, 2, 4}; - for (int v=0; v < 8; v++) { - mjtNum from[3] = {x[0], x[1], x[2]}; - for (int k=0; k < 3; k++) { - mju_addToScl3(from, dist[k], v&split[k] ? 1 : -1); - } - - mjtNum to[3]; - for (int k=0; k < 3; k++) { - mju_addScl3(to, from, dist[k], 2); - if (!(v&split[k])) { - START - mjv_connector(thisgeom, mjGEOM_LINE, 2, from, to); - f2f(thisgeom->rgba, rgba, 4); - FINISH - } - } - } -} - - - // computes the camera frustum static void getFrustum(float zver[2], float zhor[2], float znear, const float K[4], const float sensorsize[2]) { @@ -683,6 +633,8 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, } // body BVH + category = mjCAT_DECOR; + objtype = mjOBJ_UNKNOWN; if (vopt->flags[mjVIS_BODYBVH]) { float rgba[] = {1, 0, 0, 1}; for (int i = 0; i < m->nbvhstatic; i++) { @@ -708,20 +660,30 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, break; } - // compute transformation - mjtNum *aabb = isleaf ? m->geom_aabb + 6*geomid : m->bvh_aabb + 6*i; - + // get xpos, xmat, size const mjtNum* xpos = isleaf ? d->geom_xpos + 3 * geomid : d->xipos + 3 * bodyid; const mjtNum* xmat = isleaf ? d->geom_xmat + 9 * geomid : d->ximat + 9 * bodyid; + const mjtNum *size = isleaf ? m->geom_aabb + 6*geomid + 3 : m->bvh_aabb + 6*i + 3; + + // offset xpos with aabb center (not always at frame origin) + const mjtNum *center = isleaf ? m->geom_aabb + 6*geomid : m->bvh_aabb + 6*i; + mjtNum pos[3]; + mju_rotVecMat(pos, center, xmat); + mju_addTo3(pos, xpos); rgba[0] = d->bvh_active[i] ? 1 : 0; rgba[1] = d->bvh_active[i] ? 0 : 1; - drawBoundingBox(d, scn, aabb, xpos, xmat, rgba); + START + mjv_initGeom(thisgeom, mjGEOM_LINEBOX, size, pos, xmat, rgba); + FINISH + } } // flex BVH + category = mjCAT_DECOR; + objtype = mjOBJ_UNKNOWN; if (vopt->flags[mjVIS_FLEXBVH]) { float rgba[] = {1, 0, 0, 0.1}; for (int f=0; f < m->nflex; f++) { @@ -741,10 +703,8 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, rgba[0] = d->bvh_active[i] ? 1 : 0; rgba[1] = d->bvh_active[i] ? 0 : 1; - // b/304453879 : add LINEBOX geom for bounding box visualization - START - mjv_initGeom(thisgeom, mjGEOM_BOX, aabb+3, aabb, NULL, rgba); + mjv_initGeom(thisgeom, mjGEOM_LINEBOX, aabb+3, aabb, NULL, rgba); FINISH } } @@ -752,6 +712,8 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, } // mesh BVH + category = mjCAT_DECOR; + objtype = mjOBJ_UNKNOWN; if (vopt->flags[mjVIS_MESHBVH]) { float rgba[] = {1, 0, 0, 1}; for (int geomid = 0; geomid < m->ngeom; geomid++) { @@ -770,11 +732,6 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, } } - // compute transformation - const mjtNum *aabb = m->bvh_aabb + 6*i; - const mjtNum* xpos = d->geom_xpos + 3 * geomid; - const mjtNum* xmat = d->geom_xmat + 9 * geomid; - if (!d->bvh_active[i]) { continue; } @@ -782,7 +739,20 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, rgba[0] = d->bvh_active[i] ? 1 : 0; rgba[1] = d->bvh_active[i] ? 0 : 1; - drawBoundingBox(d, scn, aabb, xpos, xmat, rgba); + // get xpos, xmat, size + const mjtNum* xpos = d->geom_xpos + 3 * geomid; + const mjtNum* xmat = d->geom_xmat + 9 * geomid; + const mjtNum *size = m->bvh_aabb + 6*i + 3; + + // offset xpos with aabb center (not always at geom origin) + const mjtNum *center = m->bvh_aabb + 6*i; + mjtNum pos[3]; + mju_rotVecMat(pos, center, xmat); + mju_addTo3(pos, xpos); + + START + mjv_initGeom(thisgeom, mjGEOM_LINEBOX, size, pos, xmat, rgba); + FINISH } } } diff --git a/src/render/render_context.c b/src/render/render_context.c index afb1377f..ec349b2a 100644 --- a/src/render/render_context.c +++ b/src/render/render_context.c @@ -1675,8 +1675,17 @@ void mjr_addAux(int index, int width, int height, int samples, mjrContext* con) // check max size int maxSize = 0; glGetIntegerv(GL_MAX_RENDERBUFFER_SIZE, &maxSize); - if (width > maxSize || height > maxSize) { - mju_error("Auxiliary buffer size exceeds maximum allowed by OpenGL implementation"); + if (width > maxSize) { + mju_error( + "Auxiliary buffer width exceeds maximum allowed by OpenGL " + "implementation: %d > %d", + width, maxSize); + } + if (height > maxSize) { + mju_error( + "Auxiliary buffer height exceeds maximum allowed by OpenGL " + "implementation: %d > %d", + height, maxSize); } // clamp samples request diff --git a/src/render/render_gl3.c b/src/render/render_gl3.c index 4e108c0d..6535874a 100644 --- a/src/render/render_gl3.c +++ b/src/render/render_gl3.c @@ -404,6 +404,41 @@ static void renderGeom(const mjvGeom* geom, int mode, const float* headpos, } break; + case mjGEOM_LINEBOX: // box with line edges + glLineWidth(1.5*con->lineWidth); + lighting = glIsEnabled(GL_LIGHTING); + glDisable(GL_LIGHTING); + // bottom face + glBegin(GL_LINE_LOOP); + glVertex3f(-size[0], -size[1], -size[2]); + glVertex3f( size[0], -size[1], -size[2]); + glVertex3f( size[0], size[1], -size[2]); + glVertex3f(-size[0], size[1], -size[2]); + glEnd(); + // top face + glBegin(GL_LINE_LOOP); + glVertex3f(-size[0], -size[1], size[2]); + glVertex3f( size[0], -size[1], size[2]); + glVertex3f( size[0], size[1], size[2]); + glVertex3f(-size[0], size[1], size[2]); + glEnd(); + // vertical edges + glBegin(GL_LINES); + glVertex3f(-size[0], -size[1], -size[2]); + glVertex3f(-size[0], -size[1], size[2]); + glVertex3f( size[0], -size[1], -size[2]); + glVertex3f( size[0], -size[1], size[2]); + glVertex3f( size[0], size[1], -size[2]); + glVertex3f( size[0], size[1], size[2]); + glVertex3f(-size[0], size[1], -size[2]); + glVertex3f(-size[0], size[1], size[2]); + glEnd(); + glLineWidth(con->lineWidth); + if (lighting) { + glEnable(GL_LIGHTING); + } + break; + case mjGEOM_TRIANGLE: // triangle glBegin(GL_TRIANGLES); glVertex3f(0, 0, 0); diff --git a/src/user/user_mesh.cc b/src/user/user_mesh.cc index cc2536bf..edd36263 100644 --- a/src/user/user_mesh.cc +++ b/src/user/user_mesh.cc @@ -296,6 +296,11 @@ void mjCMesh::LoadSDF() { name.c_str(), id); } + if (scale_[0] != 1 || scale_[1] != 1 || scale_[2] != 1) { + throw mjCError(this, "attribute scale is not compatible with SDFs in mesh '%s', (id = %d)", + name.c_str(), id); + } + model->ResolvePlugin(this, plugin_name, plugin_instance_name, &plugin_instance); const mjpPlugin* plugin = mjp_getPluginAtSlot(plugin_instance->plugin_slot); if (!(plugin->capabilityflags & mjPLUGIN_SDF)) { diff --git a/src/user/user_model.cc b/src/user/user_model.cc index 693287fa..a4f5a2dd 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -19,7 +19,9 @@ #include #include #include +#include #include +#include #include #include @@ -112,6 +114,16 @@ mjCModel::mjCModel() { center[0] = mjNAN; center[1] = center[2] = 0; + //------------------------ auto-computed statistics +#ifndef MEMORY_SANITIZER + // initializing as best practice, but want MSAN to catch unintialized use + meaninertia_auto = 0; + meanmass_auto = 0; + meansize_auto = 0; + extent_auto = 0; + center_auto[0] = center_auto[1] = center_auto[2] = 0; +#endif + //------------------------ engine data modelname = "MuJoCo Model"; mj_defaultOption(&option); @@ -170,6 +182,35 @@ mjCModel::mjCModel() { world->name = "world"; world->def = defaults[0]; bodies.push_back(world); + + for (int i = 0; i < mjNOBJECT; ++i) { + object_lists[i] = nullptr; + } + + object_lists[mjOBJ_BODY] = (std::vector*) &bodies; + object_lists[mjOBJ_XBODY] = (std::vector*) &bodies; + object_lists[mjOBJ_JOINT] = (std::vector*) &joints; + object_lists[mjOBJ_GEOM] = (std::vector*) &geoms; + object_lists[mjOBJ_SITE] = (std::vector*) &sites; + object_lists[mjOBJ_CAMERA] = (std::vector*) &cameras; + object_lists[mjOBJ_LIGHT] = (std::vector*) &lights; + object_lists[mjOBJ_FLEX] = (std::vector*) &flexes; + object_lists[mjOBJ_MESH] = (std::vector*) &meshes; + object_lists[mjOBJ_SKIN] = (std::vector*) &skins; + object_lists[mjOBJ_HFIELD] = (std::vector*) &hfields; + object_lists[mjOBJ_TEXTURE] = (std::vector*) &textures; + object_lists[mjOBJ_MATERIAL] = (std::vector*) &materials; + object_lists[mjOBJ_PAIR] = (std::vector*) &pairs; + object_lists[mjOBJ_EXCLUDE] = (std::vector*) &excludes; + object_lists[mjOBJ_EQUALITY] = (std::vector*) &equalities; + object_lists[mjOBJ_TENDON] = (std::vector*) &tendons; + object_lists[mjOBJ_ACTUATOR] = (std::vector*) &actuators; + object_lists[mjOBJ_SENSOR] = (std::vector*) &sensors; + object_lists[mjOBJ_NUMERIC] = (std::vector*) &numerics; + object_lists[mjOBJ_TEXT] = (std::vector*) &texts; + object_lists[mjOBJ_TUPLE] = (std::vector*) &tuples; + object_lists[mjOBJ_KEY] = (std::vector*) &keys; + object_lists[mjOBJ_PLUGIN] = (std::vector*) &plugins; } @@ -442,118 +483,20 @@ mjCPlugin* mjCModel::AddPlugin(void) { // get number of objects of specified type int mjCModel::NumObjects(mjtObj type) { - switch (type) { - case mjOBJ_BODY: - case mjOBJ_XBODY: - return (int)bodies.size(); - case mjOBJ_JOINT: - return (int)joints.size(); - case mjOBJ_GEOM: - return (int)geoms.size(); - case mjOBJ_SITE: - return (int)sites.size(); - case mjOBJ_CAMERA: - return (int)cameras.size(); - case mjOBJ_LIGHT: - return (int)lights.size(); - case mjOBJ_FLEX: - return (int)flexes.size(); - case mjOBJ_MESH: - return (int)meshes.size(); - case mjOBJ_SKIN: - return (int)skins.size(); - case mjOBJ_HFIELD: - return (int)hfields.size(); - case mjOBJ_TEXTURE: - return (int)textures.size(); - case mjOBJ_MATERIAL: - return (int)materials.size(); - case mjOBJ_PAIR: - return (int)pairs.size(); - case mjOBJ_EXCLUDE: - return (int)excludes.size(); - case mjOBJ_EQUALITY: - return (int)equalities.size(); - case mjOBJ_TENDON: - return (int)tendons.size(); - case mjOBJ_ACTUATOR: - return (int)actuators.size(); - case mjOBJ_SENSOR: - return (int)sensors.size(); - case mjOBJ_NUMERIC: - return (int)numerics.size(); - case mjOBJ_TEXT: - return (int)texts.size(); - case mjOBJ_TUPLE: - return (int)tuples.size(); - case mjOBJ_KEY: - return (int)keys.size(); - case mjOBJ_PLUGIN: - return (int)plugins.size(); - default: + if (!object_lists[type]) { return 0; } + return (int) object_lists[type]->size(); } // get pointer to specified object mjCBase* mjCModel::GetObject(mjtObj type, int id) { - if (id>=0 && id= NumObjects(type)) { + return nullptr; } - - return 0; + return (*object_lists[type])[id]; } @@ -631,67 +574,31 @@ mjCDef* mjCModel::AddDef(string name, int parentid) { // find object by name in given list template -static T* findobject(string name, vector& list) { - for (unsigned int i=0; iname == name) { - return list[i]; +static T* findobject(std::string_view name, const vector& list, const mjKeyMap& ids) { + // this can occur in the URDF parser + if (ids.empty()) { + for (unsigned int i=0; iname == name) { + return list[i]; + } } + return nullptr; } - return 0; + // during model compilation + auto id = ids.find(name); + if (id == ids.end()) { + return nullptr; + } + return list[id->second]; } // find object in global lists given string type and name mjCBase* mjCModel::FindObject(mjtObj type, string name) { - switch (type) { - case mjOBJ_BODY: - case mjOBJ_XBODY: - return findobject(name, bodies); - case mjOBJ_JOINT: - return findobject(name, joints); - case mjOBJ_GEOM: - return findobject(name, geoms); - case mjOBJ_SITE: - return findobject(name, sites); - case mjOBJ_CAMERA: - return findobject(name, cameras); - case mjOBJ_LIGHT: - return findobject(name, lights); - case mjOBJ_FLEX: - return findobject(name, flexes); - case mjOBJ_MESH: - return findobject(name, meshes); - case mjOBJ_SKIN: - return findobject(name, skins); - case mjOBJ_HFIELD: - return findobject(name, hfields); - case mjOBJ_TEXTURE: - return findobject(name, textures); - case mjOBJ_MATERIAL: - return findobject(name, materials); - case mjOBJ_PAIR: - return findobject(name, pairs); - case mjOBJ_EXCLUDE: - return findobject(name, excludes); - case mjOBJ_EQUALITY: - return findobject(name, equalities); - case mjOBJ_TENDON: - return findobject(name, tendons); - case mjOBJ_ACTUATOR: - return findobject(name, actuators); - case mjOBJ_SENSOR: - return findobject(name, sensors); - case mjOBJ_NUMERIC: - return findobject(name, numerics); - case mjOBJ_TEXT: - return findobject(name, texts); - case mjOBJ_TUPLE: - return findobject(name, tuples); - case mjOBJ_PLUGIN: - return findobject(name, plugins); - default: - return 0; + if (!object_lists[type]) { + return nullptr; } + return findobject(name, *object_lists[type], ids[type]); } @@ -842,57 +749,56 @@ void mjCModel::IndexAssets(void) { // if asset name is missing, set to filename -void mjCModel::SetDefaultNames(void) { +template +void mjCModel::SetDefaultNames(std::vector& assets) { string stripped; + std::map> names; - // meshes - for (int i=0; iname.empty()) { - stripped = mjuu_strippath(meshes[i]->file()); - meshes[i]->name = mjuu_stripext(stripped); - - // name cannot be empty - if (meshes[i]->name.empty()) { - throw mjCError(meshes[i], "empty name in mesh"); - } + // use filename if name is missing + for (int i=0; iname.empty()) { + stripped = mjuu_strippath(assets[i]->get_file()); + assets[i]->name = mjuu_stripext(stripped); + names[assets[i]->name].push_back(i); } } - // skins - for (int i=0; iname.empty()) { - stripped = mjuu_strippath(skins[i]->file); - skins[i]->name = mjuu_stripext(stripped); + // add suffix if duplicates + for (auto const& [name, indices] : names) { + if (indices.size() > 1) { + for (int i=0; iname += "_" + std::to_string(i); + } + } + } +} + + + +// throw error if a name is missing +void mjCModel::CheckEmptyNames(void) { + // meshes + for (int i=0; iname.empty()) { + throw mjCError(meshes[i], "empty name in mesh"); } } // hfields for (int i=0; iname.empty()) { - stripped = mjuu_strippath(hfields[i]->file); - hfields[i]->name = mjuu_stripext(stripped); - - // name cannot be empty - if (hfields[i]->name.empty()) { - throw mjCError(hfields[i], "empty name in height field"); - } + throw mjCError(hfields[i], "empty name in height field"); } } // textures for (int i=0; iname.empty()) { - stripped = mjuu_strippath(textures[i]->file); - textures[i]->name = mjuu_stripext(stripped); - - // name cannot be empty, except for skybox - if (textures[i]->name.empty() && textures[i]->type!=mjTEXTURE_SKYBOX) { - throw mjCError(textures[i], "empty name in texture"); - } + if (textures[i]->name.empty() && textures[i]->type!=mjTEXTURE_SKYBOX) { + throw mjCError(textures[i], "empty name in texture"); } } - // materials: name check only + // materials for (int i=0; iname.empty()) { throw mjCError(materials[i], "empty name in material"); @@ -2357,6 +2263,14 @@ void mjCModel::CopyObjects(mjModel* m) { //------------------------------- FUSE STATIC ------------------------------------------------------ +template +static void makelistid(std::vector& dest, std::vector& source) { + for (int i=0; iid = (int)dest.size(); + dest.push_back(source[i]); + } +} + // change frame to parent body static void changeframe(double childpos[3], double childquat[4], const double bodypos[3], const double bodyquat[4]) { @@ -2379,23 +2293,9 @@ void mjCModel::FuseReindex(mjCBody* body) { body->bodies[i]->id : body->weldid); } - // joints - for (int i=0; ijoints.size(); i++) { - body->joints[i]->id = (int)joints.size(); - joints.push_back(body->joints[i]); - } - - // geoms - for (int i=0; igeoms.size(); i++) { - body->geoms[i]->id = (int)geoms.size(); - geoms.push_back(body->geoms[i]); - } - - // sites - for (int i=0; isites.size(); i++) { - body->sites[i]->id = (int)sites.size(); - sites.push_back(body->sites[i]); - } + makelistid(joints, body->joints); + makelistid(geoms, body->geoms); + makelistid(sites, body->sites); // process children recursively for (int i=0; ibodies.size(); i++) { @@ -2632,34 +2532,38 @@ static void reassignid(vector& list) { // set ids, check for repeated names template -static void processlist(vector& list, string defname, bool checkrepeat=true) { +static void processlist(mjListKeyMap& ids, vector& list, + mjtObj type, bool checkrepeat = true) { // loop over list elements - for (int i=0; i<(int)list.size(); i++) { + for (size_t i=0; i < list.size(); i++) { // check for incompatible id setting; SHOULD NOT OCCUR if (list[i]->id!=-1 && list[i]->id!=i) { - throw mjCError(list[i], "incompatible id in %s array, position %d", defname.c_str(), i); + throw mjCError(list[i], "incompatible id in %s array, position %d", mju_type2Str(type), i); } // id equals position in array list[i]->id = i; + + // add to ids map + ids[type][list[i]->name] = i; } // check for repeated names if (checkrepeat) { // created vectors with all names vector allnames; - for (int i=0; i<(int)list.size(); i++) { + for (size_t i=0; i < list.size(); i++) { if (!list[i]->name.empty()) { allnames.push_back(list[i]->name); } } // sort and check for duplicates - if (allnames.size()>1) { + if (allnames.size() > 1) { std::sort(allnames.begin(), allnames.end()); auto adjacent = std::adjacent_find(allnames.begin(), allnames.end()); - if (adjacent!=allnames.end()) { - string msg = "repeated name '" + *adjacent + "' in " + defname; + if (adjacent != allnames.end()) { + string msg = "repeated name '" + *adjacent + "' in " + mju_type2Str(type); throw mjCError(NULL, msg.c_str()); } } @@ -2785,33 +2689,21 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) { // make lists of objects created in kinematic tree MakeLists(bodies[0]); - // set object ids and default names, check for repeated names - processlist(bodies, "body"); - processlist(joints, "joint"); - processlist(geoms, "geom"); - processlist(sites, "site"); - processlist(cameras, "camera"); - processlist(lights, "light"); - processlist(flexes, "flex"); - processlist(meshes, "mesh"); - processlist(skins, "skin"); - processlist(hfields, "hfield"); - processlist(textures, "texture"); - processlist(materials, "material"); - processlist(pairs, "pair"); - processlist(excludes, "exclude"); - processlist(equalities, "equality"); - processlist(tendons, "tendon"); - processlist(actuators, "actuator"); - processlist(sensors, "sensor"); - processlist(numerics, "numeric"); - processlist(texts, "text"); - processlist(tuples, "tuple"); - processlist(keys, "key"); - processlist(plugins, "plugin"); + // fill missing names and check that they are all filled + SetDefaultNames(meshes); + SetDefaultNames(skins); + SetDefaultNames(hfields); + SetDefaultNames(textures); + CheckEmptyNames(); - // set default names, convert names into indices - SetDefaultNames(); + // set object ids, check for repeated names + for (int i = 0; i < mjNOBJECT; i++) { + if (i != mjOBJ_XBODY && object_lists[i]) { + processlist(ids, *object_lists[i], (mjtObj) i); + } + } + + // convert names into indices IndexAssets(); // mark meshes that need convex hull @@ -3141,6 +3033,13 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) { // actuator lengthrange computation LengthRange(m, d); + // save automatically-computed statistics, to disambiguate when saving + extent_auto = m->stat.extent; + meaninertia_auto = m->stat.meaninertia; + meanmass_auto = m->stat.meanmass; + meansize_auto = m->stat.meansize; + copyvec(center_auto, m->stat.center, 3); + // override model statistics if defined by user if (mjuu_defined(extent)) m->stat.extent = (mjtNum)extent; if (mjuu_defined(meaninertia)) m->stat.meaninertia = (mjtNum)meaninertia; @@ -3217,10 +3116,16 @@ bool mjCModel::CopyBack(const mjModel* m) { option = m->opt; visual = m->vis; - // runtime-modifiable members of mjStatistic - meansize = m->stat.meansize; - extent = m->stat.extent; - mju_copy3(center, m->stat.center); + // runtime-modifiable members of mjStatistic, if different from computed values + if (m->stat.meaninertia != meaninertia_auto) meaninertia = m->stat.meaninertia; + if (m->stat.meanmass != meanmass_auto) meanmass = m->stat.meanmass; + if (m->stat.meansize != meansize_auto) meansize = m->stat.meansize; + if (m->stat.extent != extent_auto) extent = m->stat.extent; + if (m->stat.center[0] != center_auto[0] || + m->stat.center[1] != center_auto[1] || + m->stat.center[2] != center_auto[2]) { + mju_copy3(center, m->stat.center); + } // qpos0, qpos_spring for (int i=0; i +#include #include #include #include @@ -30,6 +32,9 @@ typedef enum _mjtInertiaFromGeom { mjINERTIAFROMGEOM_AUTO // use only if inertial element is missing } mjtInertiaFromGeom; +typedef std::map > mjKeyMap; +typedef std::array mjListKeyMap; + //---------------------------------- class mjCModel ------------------------------------------------ @@ -176,10 +181,13 @@ class mjCModel { template // add object of any type, with def parameter T* AddObjectDef(std::vector& list, std::string type, mjCDef* def); + template // if asset name is missing, set to filename + void SetDefaultNames(std::vector& assets); + //------------------------ compile phases void MakeLists(mjCBody* body); // make lists of bodies, geoms, joints, sites void IndexAssets(void); // convert asset names into indices - void SetDefaultNames(void); // if mesh or hfield name is missing, set to filename + void CheckEmptyNames(void); // check empty names void SetSizes(void); // compute sizes void AutoSpringDamper(mjModel*);// automatic stiffness and damping computation void LengthRange(mjModel*, mjData*); // compute actuator lengthrange @@ -284,6 +292,20 @@ class mjCModel { std::vector lights; // list of lights //------------------------ internal variables + + // array of pointers to each object list (enumerated by type) + std::array*, mjNOBJECT> object_lists; + + // statistics, as computed by mj_setConst + double meaninertia_auto; // mean diagonal inertia, as computed by mj_setConst + double meanmass_auto; // mean body mass, as computed by mj_setConst + double meansize_auto; // mean body size, as computed by mj_setConst + double extent_auto; // spatial extent, as computed by mj_setConst + double center_auto[3]; // center of model, as computed by mj_setConst + + // map from object names to ids + mjListKeyMap ids; + bool hasImplicitPluginElem; // already encountered an implicit plugin sensor/actuator bool compiled; // already compiled flag (cannot be compiled again) mjCError errInfo; // last error info diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index 009ecebc..84a9c8c8 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -993,7 +993,7 @@ void mjCBody::Compile(void) { // frame if (frame) { - mjuu_frameaccum(pos, quat, frame->pos, frame->quat); + mjuu_frameaccumChild(frame->pos, frame->quat, pos, quat); } // accumulate rbound, contype, conaffinity over geoms @@ -1101,7 +1101,7 @@ void mjCFrame::Compile() { // compile parents and accumulate result if (frame) { frame->Compile(); - mjuu_frameaccum(pos, quat, frame->pos, frame->quat); + mjuu_frameaccumChild(frame->pos, frame->quat, pos, quat); } mjuu_normvec(quat, 4); @@ -1263,7 +1263,7 @@ int mjCJoint::Compile(void) { mjuu_zerovec(pos, 3); } else if (frame) { double qunit[4] = {1, 0, 0, 0}; - mjuu_frameaccum(pos, qunit, frame->pos, frame->quat); + mjuu_frameaccumChild(frame->pos, frame->quat, pos, qunit); } // convert reference angles to radians for hinge joints @@ -1878,7 +1878,7 @@ void mjCGeom::Compile(void) { // frame if (frame) { - mjuu_frameaccum(pos, quat, frame->pos, frame->quat); + mjuu_frameaccumChild(frame->pos, frame->quat, pos, quat); } } @@ -1990,7 +1990,7 @@ void mjCSite::Compile(void) { // frame if (frame) { - mjuu_frameaccum(pos, quat, frame->pos, frame->quat); + mjuu_frameaccumChild(frame->pos, frame->quat, pos, quat); } // normalize quaternion @@ -2055,7 +2055,7 @@ void mjCCamera::Compile(void) { // frame if (frame) { - mjuu_frameaccum(pos, quat, frame->pos, frame->quat); + mjuu_frameaccumChild(frame->pos, frame->quat, pos, quat); } // normalize quaternion @@ -2159,7 +2159,7 @@ void mjCLight::Compile(void) { // frame if (frame) { - mjuu_frameaccum(pos, quat, frame->pos, frame->quat); + mjuu_frameaccumChild(frame->pos, frame->quat, pos, quat); } // normalize direction, make sure it is not zero diff --git a/src/user/user_util.cc b/src/user/user_util.cc index 4c75ccce..35e5568d 100644 --- a/src/user/user_util.cc +++ b/src/user/user_util.cc @@ -367,8 +367,8 @@ void mjuu_frame2quat(double* quat, const double* x, const double* y, const doubl // invert frame transformation -void mjuu_frameinvert(double* newpos, double* newquat, - const double* oldpos, const double* oldquat) { +void mjuu_frameinvert(double newpos[3], double newquat[4], + const double oldpos[3], const double oldquat[4]) { // position mjuu_localaxis(newpos, oldpos, oldquat); newpos[0] = -newpos[0]; @@ -384,28 +384,39 @@ void mjuu_frameinvert(double* newpos, double* newquat, // accumulate frame transformations (forward kinematics) -void mjuu_frameaccum(double* pos, double* quat, - const double* addpos, const double* addquat) { +void mjuu_frameaccum(double pos[3], double quat[4], + const double childpos[3], const double childquat[4]) { double mat[9], vec[3], qtmp[4]; mjuu_quat2mat(mat, quat); - mjuu_mulvecmat(vec, addpos, mat); + mjuu_mulvecmat(vec, childpos, mat); pos[0] += vec[0]; pos[1] += vec[1]; pos[2] += vec[2]; - mjuu_mulquat(qtmp, quat, addquat); + mjuu_mulquat(qtmp, quat, childquat); mjuu_copyvec(quat, qtmp, 4); } +// accumulate frame transformation in second frame +void mjuu_frameaccumChild(const double pos[3], const double quat[4], + double childpos[3], double childquat[4]) { + double p[] = {pos[0], pos[1], pos[2]}; + double q[] = {quat[0], quat[1], quat[2], quat[3]}; + mjuu_frameaccum(p, q, childpos, childquat); + mjuu_copyvec(childpos, p, 3); + mjuu_copyvec(childquat, q, 4); +} + + // invert frame accumulation -void mjuu_frameaccuminv(double* pos, double* quat, - const double* addpos, const double* addquat) { +void mjuu_frameaccuminv(double pos[3], double quat[4], + const double childpos[3], const double childquat[4]) { double mat[9], vec[3], qtmp[4]; - double qneg[4] = {addquat[0], -addquat[1], -addquat[2], -addquat[3]}; + double qneg[4] = {childquat[0], -childquat[1], -childquat[2], -childquat[3]}; mjuu_mulquat(qtmp, quat, qneg); mjuu_copyvec(quat, qtmp, 4); mjuu_quat2mat(mat, quat); - mjuu_mulvecmat(vec, addpos, mat); + mjuu_mulvecmat(vec, childpos, mat); pos[0] -= vec[0]; pos[1] -= vec[1]; pos[2] -= vec[2]; diff --git a/src/user/user_util.h b/src/user/user_util.h index fa230137..2cede571 100644 --- a/src/user/user_util.h +++ b/src/user/user_util.h @@ -104,16 +104,20 @@ void mjuu_z2quat(double* quat, const double* vec); void mjuu_frame2quat(double* quat, const double* x, const double* y, const double* z); // invert frame transformation -void mjuu_frameinvert(double* newpos, double* newquat, - const double* oldpos, const double* oldquat); +void mjuu_frameinvert(double newpos[3], double newquat[4], + const double oldpos[3], const double oldquat[4]); -// accumulate frame transformations -void mjuu_frameaccum(double* pos, double* quat, - const double* addpos, const double* addquat); +// accumulate frame transformation into parent frame +void mjuu_frameaccum(double pos[3], double quat[4], + const double childpos[3], const double childquat[4]); + +// accumulate frame transformation into child frame +void mjuu_frameaccumChild(const double pos[3], const double quat[4], + double childpos[3], double childquat[4]); // invert frame accumulation -void mjuu_frameaccuminv(double* pos, double* quat, - const double* addpos, const double* addquat); +void mjuu_frameaccuminv(double pos[3], double quat[4], + const double childpos[3], const double childquat[4]); // convert local_inertia[3] to global_inertia[6] void mjuu_globalinertia(double* global, const double* local, const double* quat); diff --git a/src/xml/xml_util.cc b/src/xml/xml_util.cc index 0d1a7f75..d7a09afc 100644 --- a/src/xml/xml_util.cc +++ b/src/xml/xml_util.cc @@ -14,6 +14,7 @@ #include #include +#include #include #include #include @@ -1004,7 +1005,8 @@ void mjXUtil::WriteAttr(XMLElement* elem, string name, int n, const T* data, con } // append number - if (isint(data[i])) { + double doubledata = static_cast(data[i]); + if (doubledata < INT_MAX && doubledata > -INT_MAX && isint(data[i])) { stream << Round(data[i]); } else { stream << data[i]; diff --git a/test/engine/CMakeLists.txt b/test/engine/CMakeLists.txt index 1e083b36..727cf955 100644 --- a/test/engine/CMakeLists.txt +++ b/test/engine/CMakeLists.txt @@ -12,6 +12,9 @@ # See the License for the specific language governing permissions and # limitations under the License. +mujoco_test(engine_collision_box_test) +target_link_libraries(engine_collision_box_test fixture gmock) + mujoco_test(engine_collision_convex_test) target_link_libraries(engine_collision_convex_test fixture gmock) @@ -96,5 +99,10 @@ target_link_libraries(engine_util_spatial_test fixture gmock) mujoco_test(engine_vfs_test) target_link_libraries(engine_vfs_test fixture gmock) -mujoco_test(engine_vis_state_test) +mujoco_test( + engine_vis_state_test + PROPERTIES + ENVIRONMENT + "MUJOCO_PLUGIN_DIR=$" +) target_link_libraries(engine_vis_state_test fixture gmock) diff --git a/test/engine/engine_collision_box_test.cc b/test/engine/engine_collision_box_test.cc new file mode 100644 index 00000000..5477189d --- /dev/null +++ b/test/engine/engine_collision_box_test.cc @@ -0,0 +1,248 @@ +// 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 engine/engine_collision_box.c. + +#include + +#include +#include +#include +#include +#include "test/fixture.h" +#include "src/engine/engine_collision_primitive.h" +#include "src/engine/engine_util_misc.h" + + +namespace mujoco { +namespace { + +using MjCollisionBoxTest = MujocoTest; +using ::testing::NotNull; + +static const char* const kBad0FilePath = + "engine/testdata/collision_box/boxbox_bad0.xml"; +static const char* const kBad1FilePath = + "engine/testdata/collision_box/boxbox_bad1.xml"; + +TEST_F(MjCollisionBoxTest, BadContacts) { + for (const char* local_path : {kBad0FilePath, kBad1FilePath}) { + const std::string xml_path = GetTestDataFilePath(local_path); + mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, 0, 0); + ASSERT_THAT(model, NotNull()); + mjData* data = mj_makeData(model); + mj_forward(model, data); + + // allocate contact array and matching arrays + mj_markStack(data); + mjContact* con_raw = (mjContact*) mj_stackAllocByte( + data, mjMAXCONPAIR * sizeof(mjContact), alignof(mjContact)); + int* match_raw = mj_stackAllocInt(data, mjMAXCONPAIR); + int* match = mj_stackAllocInt(data, data->ncon); + + int g1 = -1; + int g2 = -1; + for (int c = 0; c < data->ncon; c++) { + mjContact* con = data->contact + c; + int g1new = con->geom[0]; + int g2new = con->geom[1]; + + // not box-box: skip + if (model->geom_type[g1new] != mjGEOM_BOX || + model->geom_type[g2new] != mjGEOM_BOX) { + continue; + } + + // same geom pair: skip + if (g1 == g1new && g2 == g2new) { + continue; + } + + g1 = g1new; + g2 = g2new; + + // call low-level box-box collider + int num = mjc_BoxBox(model, data, con_raw, g1, g2, con->includemargin); + + // allocate and clear arrays marking already matched contacts + mju_zeroInt(match_raw, num); + mju_zeroInt(match, data->ncon); + + // loop over raw contacts, match with contact array using pos + int nmatched = 0; + for (int i = 0; i < num; i++) { + for (int j = 0; j < data->ncon; j++) { + if (!match[j] && + con_raw[i].pos[0] == data->contact[j].pos[0] && + con_raw[i].pos[1] == data->contact[j].pos[1] && + con_raw[i].pos[2] == data->contact[j].pos[2]) { + match_raw[i] = match[j] = 1; + nmatched++; + } + } + } + + // expect some contacts to have been removed + EXPECT_LT(nmatched, num) << local_path; + + // get box info + const mjtNum* pos1 = data->geom_xpos + 3 * g1; + const mjtNum* mat1 = data->geom_xmat + 9 * g1; + const mjtNum* size1 = model->geom_size + 3 * g1; + const mjtNum* pos2 = data->geom_xpos + 3 * g2; + const mjtNum* mat2 = data->geom_xmat + 9 * g2; + const mjtNum* size2 = model->geom_size + 3 * g2; + mjtNum margin = mju_max(model->geom_margin[g1], model->geom_margin[g2]); + + // loop over raw contacts, find removed + for (int i = 0; i < num; i++) { + if (!match_raw[i]) { + // === check if outside + + mjtNum sz1[3] = {size1[0]+margin, size1[1]+margin, size1[2]+margin}; + mjtNum sz2[3] = {size2[0]+margin, size2[1]+margin, size2[2]+margin}; + + // relative distance (1%) outside of which contacts are removed + static mjtNum kRatio = 1.01; + + // is the contact outside: 1, inside: -1, within the removal width: 0 + int out1 = mju_outsideBox(con_raw[i].pos, pos1, mat1, sz1, kRatio); + int out2 = mju_outsideBox(con_raw[i].pos, pos2, mat2, sz2, kRatio); + + // mark as bad if outside one box and not inside the other box + bool outside = (out1 == 1 && out2 != -1) || (out2 == 1 && out1 != -1); + + // expect that removed contact was outside + EXPECT_TRUE(outside); + } + } + } + + mj_freeStack(data); + mj_deleteData(data); + mj_deleteModel(model); + } +} + +static const char* const kDuplicateFilePath = + "engine/testdata/collision_box/boxbox_duplicate.xml"; + +TEST_F(MjCollisionBoxTest, DuplicateContacts) { + const std::string xml_path = GetTestDataFilePath(kDuplicateFilePath); + mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, 0, 0); + ASSERT_THAT(model, NotNull()); + mjData* data = mj_makeData(model); + mj_forward(model, data); + + // allocate contact array and matching arrays + mj_markStack(data); + mjContact* con_raw = (mjContact*) mj_stackAllocByte( + data, mjMAXCONPAIR * sizeof(mjContact), alignof(mjContact)); + int* match_raw = mj_stackAllocInt(data, mjMAXCONPAIR); + int* match = mj_stackAllocInt(data, data->ncon); + + + int g1 = -1; + int g2 = -1; + for (int c = 0; c < data->ncon; c++) { + mjContact* con = data->contact + c; + int g1new = con->geom[0]; + int g2new = con->geom[1]; + + // not box-box: skip + if (model->geom_type[g1new] != mjGEOM_BOX || + model->geom_type[g2new] != mjGEOM_BOX) { + continue; + } + + // same geom pair: skip + if (g1 == g1new && g2 == g2new) { + continue; + } + + g1 = g1new; + g2 = g2new; + + // call low-level box-box collider + int num = mjc_BoxBox(model, data, con_raw, g1, g2, con->includemargin); + + // allocate and clear arrays marking already matched contacts + mju_zeroInt(match_raw, num); + mju_zeroInt(match, data->ncon); + + // loop over raw contacts, match with contact array using pos + int nmatched = 0; + for (int i = 0; i < num; i++) { + for (int j = 0; j < data->ncon; j++) { + if (!match[j] && + con_raw[i].pos[0] == data->contact[j].pos[0] && + con_raw[i].pos[1] == data->contact[j].pos[1] && + con_raw[i].pos[2] == data->contact[j].pos[2]) { + match_raw[i] = match[j] = 1; + nmatched++; + } + } + } + + // expect some contacts to have been removed + EXPECT_LT(nmatched, num); + + // loop over raw contacts, find removed + for (int i = 0; i < num; i++) { + if (!match_raw[i]) { + // === check if duplicate + bool duplicate = false; + for (int j = 0; j < num; j++) { + if (duplicate || i == j) { + continue; + } + if (con_raw[i].pos[0] == con_raw[j].pos[0] && + con_raw[i].pos[1] == con_raw[j].pos[1] && + con_raw[i].pos[2] == con_raw[j].pos[2]) { + duplicate = true; + } + } + + // expect that removed contact was duplicated + EXPECT_TRUE(duplicate); + } + } + } + + mj_freeStack(data); + mj_deleteData(data); + mj_deleteModel(model); +} + + +static const char* const kDeepFilePath = + "engine/testdata/collision_box/boxbox_deep.xml"; + +TEST_F(MjCollisionBoxTest, DeepPenetration) { + const std::string xml_path = GetTestDataFilePath(kDeepFilePath); + mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, 0, 0); + ASSERT_THAT(model, NotNull()); + mjData* data = mj_makeData(model); + mj_forward(model, data); + + // expect 4 contact + EXPECT_EQ(data->ncon ,4); + + mj_deleteData(data); + mj_deleteModel(model); +} + + +} // namespace +} // namespace mujoco diff --git a/test/engine/engine_vis_state_test.cc b/test/engine/engine_vis_state_test.cc index 48d48efd..6fefde47 100644 --- a/test/engine/engine_vis_state_test.cc +++ b/test/engine/engine_vis_state_test.cc @@ -35,17 +35,17 @@ static const char* const kTendonPath = "engine/testdata/island/tendon_wrap.xml"; static const char* const kFrustumPath = "engine/testdata/vis_visualize/frustum.xml"; -static const char* const kModelPath = - "testdata/model.xml"; +static const char* const kFlex = "testdata/flex.xml"; +static const char* const kModelPath = "testdata/model.xml"; #define EXPECT_ZERO(exp) EXPECT_EQ(0, exp); TEST_F(MjvSceneStateTest, CanUpdateFromState) { for (const char* path : - {kHammockPath, kTendonPath, kModelPath, kFrustumPath}) { + {kHammockPath, kTendonPath, kModelPath, kFrustumPath, kFlex}) { const std::string xml_path = GetTestDataFilePath(path); mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, 0, 0); - ASSERT_THAT(model, NotNull()); + ASSERT_THAT(model, NotNull()) << "Failed to load model from " << path; mjData* data = mj_makeData(model); while (data->time < 2) { diff --git a/test/engine/testdata/collision_box/boxbox_bad0.xml b/test/engine/testdata/collision_box/boxbox_bad0.xml new file mode 100644 index 00000000..a3e08973 --- /dev/null +++ b/test/engine/testdata/collision_box/boxbox_bad0.xml @@ -0,0 +1,36 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/test/engine/testdata/collision_box/boxbox_bad1.xml b/test/engine/testdata/collision_box/boxbox_bad1.xml new file mode 100644 index 00000000..04f16290 --- /dev/null +++ b/test/engine/testdata/collision_box/boxbox_bad1.xml @@ -0,0 +1,24 @@ + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/test/engine/testdata/collision_box/boxbox_deep.xml b/test/engine/testdata/collision_box/boxbox_deep.xml new file mode 100644 index 00000000..65f14e5f --- /dev/null +++ b/test/engine/testdata/collision_box/boxbox_deep.xml @@ -0,0 +1,11 @@ + + + + + + + + + + + diff --git a/test/engine/testdata/collision_box/boxbox_duplicate.xml b/test/engine/testdata/collision_box/boxbox_duplicate.xml new file mode 100644 index 00000000..6582f5cf --- /dev/null +++ b/test/engine/testdata/collision_box/boxbox_duplicate.xml @@ -0,0 +1,11 @@ + + + + + + + + + + + diff --git a/test/sample/testspeed_test.sh b/test/sample/testspeed_test.sh index 942c2a98..f48342bc 100755 --- a/test/sample/testspeed_test.sh +++ b/test/sample/testspeed_test.sh @@ -26,8 +26,17 @@ test_model() { echo "Testing $model" >&2 local iterations=10 - if [[ "$model" == */composite/particle.xml && ${TESTSPEED_ASAN:-0} != 0 ]]; then - iterations=2 + # for particularly slow models, only run 2 steps under ASAN, or skip. + if [[ ${TESTSPEED_ASAN:-0} != 0 ]]; then + if [[ "$model" == */composite/particle.xml ]]; then + # this test can take several minutes under ASAN + return 0 + fi + if [[ "$model" == */benchmark/testdata/humanoid200.xml + || "$model" == */engine/testdata/collision_convex/stacked_boxes.xml + ]]; then + iterations=2 + fi fi # run testspeed, writing its output to stderr. diff --git a/test/user/testdata/load_twice.xml b/test/user/testdata/load_twice.xml new file mode 100644 index 00000000..4e9304a7 --- /dev/null +++ b/test/user/testdata/load_twice.xml @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/test/user/user_model_test.cc b/test/user/user_model_test.cc index 3abae496..2079b1d1 100644 --- a/test/user/user_model_test.cc +++ b/test/user/user_model_test.cc @@ -14,6 +14,7 @@ // Tests for user/user_model.cc. +#include #include #include @@ -21,6 +22,7 @@ #include #include #include +#include "src/cc/array_safety.h" #include "test/fixture.h" namespace mujoco { @@ -28,16 +30,41 @@ namespace { using ::testing::DoubleNear; using ::testing::ElementsAre; +using ::testing::HasSubstr; +using ::testing::IsNull; using ::testing::NotNull; -using UserDataTest = MujocoTest; static std::vector GetRow(const mjtNum* array, int ncolumn, int row) { return std::vector(array + ncolumn * row, array + ncolumn * (row + 1)); } +// ----------------------------- test mjCModel -------------------------------- + +using UserCModelTest = MujocoTest; + +TEST_F(UserCModelTest, RepeatedNames) { + static constexpr char xml[] = R"( + + + + + + + + + )"; + + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(model, IsNull()); + EXPECT_THAT(error.data(), HasSubstr("repeated name 'geom1' in geom")); +} + // ------------- test automatic inference of nuser_xxx ------------------------- +using UserDataTest = MujocoTest; + TEST_F(UserDataTest, AutoNUserBody) { static constexpr char xml[] = R"( @@ -190,6 +217,26 @@ TEST_F(UserDataTest, AutoNUserSensor) { mj_deleteModel(m); } +// ------------- test duplicate names ------------------------------------------ +TEST_F(UserDataTest, DuplicateNames) { + static const char* const kFilePath = "user/testdata/load_twice.xml"; + const std::string xml_path = GetTestDataFilePath(kFilePath); + + std::array error; + mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size()); + + EXPECT_THAT(m, NotNull()) << error.data(); + EXPECT_THAT(m->nmesh, 2); + + for (int i = 0; i < m->nmesh; i++) { + char mesh_name[mjMAXUINAME] = ""; + util::strcat_arr(mesh_name, m->names + m->name_meshadr[i]); + EXPECT_THAT(std::string(mesh_name), "cube_" + std::to_string(i)); + } + + mj_deleteModel(m); +} + // ------------- test fusestatic ----------------------------------------------- using FuseStaticTest = MujocoTest; diff --git a/test/user/user_objects_test.cc b/test/user/user_objects_test.cc index 37154b13..75fc9422 100644 --- a/test/user/user_objects_test.cc +++ b/test/user/user_objects_test.cc @@ -1404,38 +1404,47 @@ TEST_F(MujocoTest, Frame) { - + - - - + + + + + + + - + - + - + + + + + )"; + constexpr mjtNum eps = 1e-14; std::array error; mjModel* m = LoadModelFromString(xml, error.data(), error.size()); EXPECT_THAT(m, testing::NotNull()) << error.data(); - EXPECT_EQ(m->nbody, 4); + EXPECT_EQ(m->nbody, 5); // geom quat transformed to euler = 0 0 50 EXPECT_NEAR(m->geom_quat[0], mju_cos(25. * mjPI / 180.), 1e-3); @@ -1444,15 +1453,15 @@ TEST_F(MujocoTest, Frame) { EXPECT_NEAR(m->geom_quat[3], mju_sin(25. * mjPI / 180.), 1e-3); // geom transformed to frame 0 1 0, 0 0 1, 1 0 0 - EXPECT_NEAR(m->geom_quat[4], .5, 1e-6); - EXPECT_NEAR(m->geom_quat[5], .5, 1e-6); - EXPECT_NEAR(m->geom_quat[6], .5, 1e-6); - EXPECT_NEAR(m->geom_quat[7], .5, 1e-6); + EXPECT_NEAR(m->geom_quat[4], .5, eps); + EXPECT_NEAR(m->geom_quat[5], .5, eps); + EXPECT_NEAR(m->geom_quat[6], .5, eps); + EXPECT_NEAR(m->geom_quat[7], .5, eps); // geom pos transformed from 0 1 0 to 0 2 0 - EXPECT_EQ(m->geom_pos[6], 0); - EXPECT_EQ(m->geom_pos[7], 2); - EXPECT_EQ(m->geom_pos[8], 0); + EXPECT_EQ(m->geom_pos[ 9], 0); + EXPECT_EQ(m->geom_pos[10], 2); + EXPECT_EQ(m->geom_pos[11], 0); // body pos transformed from 1 0 0 to 1 1 0 EXPECT_EQ(m->body_pos[6], 1); @@ -1460,16 +1469,29 @@ TEST_F(MujocoTest, Frame) { EXPECT_EQ(m->body_pos[8], 0); // nested geom pos not transformed - EXPECT_EQ(m->geom_pos[ 9], 0); - EXPECT_EQ(m->geom_pos[10], 0); - EXPECT_EQ(m->geom_pos[11], 1); + EXPECT_EQ(m->geom_pos[12], 0); + EXPECT_EQ(m->geom_pos[13], 0); + EXPECT_EQ(m->geom_pos[14], 1); // joint axis transformed to 0 -1 0 - EXPECT_NEAR(m->jnt_axis[0], 0, 1e-6); - EXPECT_NEAR(m->jnt_axis[1], -1, 1e-6); - EXPECT_NEAR(m->jnt_axis[2], 0, 1e-6); + EXPECT_NEAR(m->jnt_axis[0], 0, eps); + EXPECT_NEAR(m->jnt_axis[1], -1, eps); + EXPECT_NEAR(m->jnt_axis[2], 0, eps); + + mjData* d = mj_makeData(m); + mj_kinematics(m, d); + + // body and frame equivalence geom 2 vs 6 + EXPECT_NEAR(d->geom_xpos[6], d->geom_xpos[18], eps); + EXPECT_NEAR(d->geom_xpos[7], d->geom_xpos[19], eps); + EXPECT_NEAR(d->geom_xpos[8], d->geom_xpos[20], eps); + EXPECT_NEAR(d->geom_xmat[18], d->geom_xmat[54], eps); + EXPECT_NEAR(d->geom_xmat[19], d->geom_xmat[55], eps); + EXPECT_NEAR(d->geom_xmat[20], d->geom_xmat[56], eps); + EXPECT_NEAR(d->geom_xmat[21], d->geom_xmat[57], eps); mj_deleteModel(m); + mj_deleteData(d); } } // namespace diff --git a/test/xml/xml_native_writer_test.cc b/test/xml/xml_native_writer_test.cc index 52b21923..4b5b7f9e 100644 --- a/test/xml/xml_native_writer_test.cc +++ b/test/xml/xml_native_writer_test.cc @@ -1135,7 +1135,7 @@ using DecompilerTest = MujocoTest; TEST_F(DecompilerTest, SavesStatitics) { static constexpr char xml[] = R"( - + )"; mjModel* model = LoadModelFromString(xml); @@ -1145,10 +1145,60 @@ TEST_F(DecompilerTest, SavesStatitics) { model->stat.center[0] = 9; model->stat.center[1] = 10; model->stat.center[2] = 11; + model->stat.meanmass = 12; + model->stat.meaninertia = 13; std::string saved_xml = SaveAndReadXml(model); EXPECT_THAT(saved_xml, HasSubstr("meansize=\"7\"")); EXPECT_THAT(saved_xml, HasSubstr("extent=\"8\"")); EXPECT_THAT(saved_xml, HasSubstr("center=\"9 10 11\"")); + EXPECT_THAT(saved_xml, HasSubstr("meanmass=\"12\"")); + EXPECT_THAT(saved_xml, HasSubstr("meaninertia=\"13\"")); + mj_deleteModel(model); +} + +TEST_F(DecompilerTest, DoesntSaveInferredStatitics) { + static constexpr char xml[] = R"( + + + + + + + + )"; + mjModel* model = LoadModelFromString(xml); + ASSERT_THAT(model, NotNull()); + std::string saved_xml = SaveAndReadXml(model); + EXPECT_THAT(saved_xml, Not(HasSubstr("meansize"))); + EXPECT_THAT(saved_xml, Not(HasSubstr("meanmass"))); + EXPECT_THAT(saved_xml, Not(HasSubstr("meaninertia"))); + EXPECT_THAT(saved_xml, Not(HasSubstr("center"))); + EXPECT_THAT(saved_xml, Not(HasSubstr("extent"))); + EXPECT_THAT(saved_xml, Not(HasSubstr("statistic"))); + mj_deleteModel(model); +} + +TEST_F(DecompilerTest, VeryLargeNumbers) { + static constexpr char xml[] = R"( + + + + + + + + + + + )"; + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, NotNull()) << error.data(); + std::string saved_xml = SaveAndReadXml(model); + // note, focal is float and loses precision 16777217 -> 16777216 + EXPECT_THAT(saved_xml, HasSubstr("focal=\"16777216 1\"")); + EXPECT_THAT(saved_xml, HasSubstr("pos=\"1e+20 0 0\"")); + EXPECT_THAT(saved_xml, HasSubstr("range=\"-1e+10 1e+10\"")); mj_deleteModel(model); } diff --git a/unity/Editor/Bindings/MujocoBinaryRetriever.cs b/unity/Editor/Bindings/MujocoBinaryRetriever.cs index f670431b..2d6e9ea0 100644 --- a/unity/Editor/Bindings/MujocoBinaryRetriever.cs +++ b/unity/Editor/Bindings/MujocoBinaryRetriever.cs @@ -37,7 +37,7 @@ public class MujocoBinaryRetriever { if (AssetDatabase.LoadMainAssetAtPath(mujocoPath + "/mujoco.dylib") == null) { File.Copy( "/Applications/MuJoCo.app/Contents/Frameworks" + - "/mujoco.framework/Versions/Current/libmujoco.3.0.2.dylib", + "/mujoco.framework/Versions/Current/libmujoco.3.1.2.dylib", mujocoPath + "/mujoco.dylib"); AssetDatabase.Refresh(); } @@ -45,7 +45,7 @@ public class MujocoBinaryRetriever { if (AssetDatabase.LoadMainAssetAtPath(mujocoPath + "/libmujoco.so") == null) { File.Copy( Environment.GetFolderPath(Environment.SpecialFolder.UserProfile) + - "/.mujoco/mujoco-3.0.2/lib/libmujoco.so.3.0.2", + "/.mujoco/mujoco-3.1.2/lib/libmujoco.so.3.1.2", mujocoPath + "/libmujoco.so"); AssetDatabase.Refresh(); } diff --git a/unity/Editor/Components/MjMouseSpring.cs b/unity/Editor/Components/MjMouseSpring.cs index b2264727..7e1c2462 100644 --- a/unity/Editor/Components/MjMouseSpring.cs +++ b/unity/Editor/Components/MjMouseSpring.cs @@ -12,7 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#if UNITY_EDITOR using System; using UnityEditor; using UnityEngine; @@ -23,7 +22,7 @@ namespace Mujoco { // and left mouse drag will apply a force on the body. Holding shift down will change the applied // force direction between World XZ plane and Y[camera-up]. - [CustomEditor(typeof(MjBody))] + [CustomEditor(typeof(MjComponent), true)] public class MjMouseSpring : Editor { private bool _lastShiftKeyState = false; @@ -102,7 +101,11 @@ namespace Mujoco { return; } - MjBody body = target as MjBody; + var targetObject = target as MjComponent; + MjBody body = targetObject.GetComponentInParent(); + if(!body) + return; + Vector3 bodyPosition = body != null ? body.transform.position : Vector3.zero; var scene = MjScene.Instance; @@ -188,4 +191,3 @@ namespace Mujoco { } } } -#endif diff --git a/unity/Editor/Components/MjShapeComponentEditor.cs b/unity/Editor/Components/MjShapeComponentEditor.cs index 99e24740..20281e61 100644 --- a/unity/Editor/Components/MjShapeComponentEditor.cs +++ b/unity/Editor/Components/MjShapeComponentEditor.cs @@ -22,7 +22,7 @@ namespace Mujoco { [CustomEditor(typeof(MjShapeComponent), true)] [CanEditMultipleObjects] -public class MjShapeComponentEditor : Editor { +public class MjShapeComponentEditor : MjMouseSpring { public override void OnInspectorGUI() { diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index a17a2694..fd2c33bf 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -108,7 +108,7 @@ public const int mjMAXLINEPNT = 1000; public const int mjMAXPLANEGRID = 200; public const bool THIRD_PARTY_MUJOCO_MJXMACRO_H_ = true; public const bool THIRD_PARTY_MUJOCO_MUJOCO_H_ = true; -public const int mjVERSION_HEADER = 302; +public const int mjVERSION_HEADER = 312; // ------------------------------------Enums------------------------------------ @@ -191,10 +191,11 @@ public enum mjtGeom : int{ mjGEOM_ARROW1 = 101, mjGEOM_ARROW2 = 102, mjGEOM_LINE = 103, - mjGEOM_FLEX = 104, - mjGEOM_SKIN = 105, - mjGEOM_LABEL = 106, - mjGEOM_TRIANGLE = 107, + mjGEOM_LINEBOX = 104, + mjGEOM_FLEX = 105, + mjGEOM_SKIN = 106, + mjGEOM_LABEL = 107, + mjGEOM_TRIANGLE = 108, mjGEOM_NONE = 1001, } public enum mjtCamLight : int{ @@ -301,6 +302,7 @@ public enum mjtObj : int{ mjOBJ_TUPLE = 23, mjOBJ_KEY = 24, mjOBJ_PLUGIN = 25, + mjNOBJECT = 26, } public enum mjtConstraint : int{ mjCNSTR_EQUALITY = 0, @@ -6093,6 +6095,7 @@ public unsafe struct model { public int nnames; public int npaths; public int nsensordata; + public int narena; public mjOption_ opt; public mjVisual_ vis; public mjStatistic_ stat; @@ -6285,6 +6288,7 @@ public unsafe struct data { public int* wrap_obj; public double* ten_length; public double* wrap_xpos; + public double* bvh_aabb_dyn; public byte* bvh_active; public int* island_dofadr; public int* island_dofind; @@ -6294,6 +6298,7 @@ public unsafe struct data { public double* flexvert_xpos; public mjContact_* contact; public double* efc_force; + public void* arena; } [StructLayout(LayoutKind.Sequential)] diff --git a/unity/package.json b/unity/package.json index 9c093316..cff13baf 100644 --- a/unity/package.json +++ b/unity/package.json @@ -1,7 +1,7 @@ { "name": "org.mujoco", "displayName": "MuJoCo", - "version": "3.0.2", + "version": "3.1.2", "description": "MuJoCo importer and runtime plug-in", "dependencies": {}, "author": {