diff --git a/CMakeLists.txt b/CMakeLists.txt index 3cab120d..f01d420d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -28,7 +28,7 @@ set(MSVC_INCREMENTAL_DEFAULT ON) project( mujoco - VERSION 3.3.3 + VERSION 3.3.4 DESCRIPTION "MuJoCo Physics Simulator" HOMEPAGE_URL "https://mujoco.org" ) @@ -42,14 +42,26 @@ option(MUJOCO_BUILD_EXAMPLES "Build samples for MuJoCo" ON) option(MUJOCO_BUILD_SIMULATE "Build simulate library for MuJoCo" ON) option(MUJOCO_BUILD_TESTS "Build tests for MuJoCo" ON) option(MUJOCO_TEST_PYTHON_UTIL "Build and test utility libraries for Python bindings" ON) -option(MUJOCO_BUILD_USD_PLUGINS "Build OpenUSD plugins" OFF) +option(MUJOCO_WITH_USD "Build with OpenUSD" OFF) -# USD libs to compile against. -set(MUJOCO_USD_ALLOWED_TARGET_VALUES "USD" "Houdini") -set(MUJOCO_USD_TARGET "USD" CACHE STRING "Select the USD target for the project.") -set_property(CACHE MUJOCO_USD_TARGET - PROPERTY STRINGS ${MUJOCO_USD_ALLOWED_TARGET_VALUES} -) +# Option to provide a path to an existing USD build directory or to Houdini HFS directory. +set(USD_DIR "" CACHE PATH "Path to an existing USD build directory.") +set(HOUDINI_HFS_DIR "" CACHE PATH "Path to Houdini HFS directory to build USD plugins against.") +if(USD_DIR) + if(EXISTS "${USD_DIR}") + # If the path is provided, set MUJOCO_WITH_USD to ON and add the path to CMAKE_PREFIX_PATH. + set(MUJOCO_WITH_USD ON CACHE BOOL "Build with OpenUSD" FORCE) + list(PREPEND CMAKE_PREFIX_PATH "${USD_DIR}") + message(STATUS "Using custom USD build directory: ${USD_DIR}") + else() + message(WARNING "Invalid path provided for USD_DIR: ${USD_DIR}") + endif() +elseif(HOUDINI_HFS_DIR) + if(EXISTS "${HOUDINI_HFS_DIR}") + set(MUJOCO_WITH_USD ON CACHE BOOL "Build with OpenUSD" FORCE) + message(STATUS "Building Mujoco USD against Houdini HFS: ${HOUDINI_HFS_DIR}") + endif() +endif() if(APPLE AND (MUJOCO_BUILD_EXAMPLES OR MUJOCO_BUILD_SIMULATE)) enable_language(OBJC) @@ -189,8 +201,8 @@ if(MUJOCO_BUILD_EXAMPLES) add_subdirectory(sample) endif() -if(MUJOCO_BUILD_USD_PLUGINS) - add_subdirectory(src/experimental/usd/plugins) +if(MUJOCO_WITH_USD) + add_subdirectory(src/experimental/usd) endif() if(BUILD_TESTING AND MUJOCO_BUILD_TESTS) diff --git a/cmake/MujocoDependencies.cmake b/cmake/MujocoDependencies.cmake index 9d5dc01c..78522705 100644 --- a/cmake/MujocoDependencies.cmake +++ b/cmake/MujocoDependencies.cmake @@ -15,7 +15,7 @@ # Build configuration for third party libraries used in MuJoCo. set(MUJOCO_DEP_VERSION_lodepng - b4ed2cd7ecf61d29076169b49199371456d4f90b + 17d08dd26cac4d63f43af217ebd70318bfb8189c CACHE STRING "Version of `lodepng` to be fetched." ) set(MUJOCO_DEP_VERSION_tinyxml2 @@ -35,21 +35,21 @@ set(MUJOCO_DEP_VERSION_ccd CACHE STRING "Version of `ccd` to be fetched." ) set(MUJOCO_DEP_VERSION_qhull - 0c8fc90d2037588024d9964515c1e684f6007ecc + c7bee59d068a69f427b1273e71cdc5bc455a5bdd CACHE STRING "Version of `qhull` to be fetched." ) set(MUJOCO_DEP_VERSION_Eigen3 - 464c1d097891a1462ab28bf8bb763c1683883892 + d0b490ee091629068e0c11953419eb089f9e6bb2 CACHE STRING "Version of `Eigen3` to be fetched." ) set(MUJOCO_DEP_VERSION_abseil - d9e4955c65cd4367dd6bf46f4ccb8cd3d100540b # LTS 20250127.1 + bc257a88f7c1939f24e0379f14a3589e926c950c # LTS 20250512.0 CACHE STRING "Version of `abseil` to be fetched." ) set(MUJOCO_DEP_VERSION_gtest - 6910c9d9165801d8827d628cb72eb7ea9dd538c5 # v1.16.0 + 52eb8108c5bdec04579160ae17225d66034bd723 # v1.17.0 CACHE STRING "Version of `gtest` to be fetched." ) diff --git a/cmake/TargetAddRpath.cmake b/cmake/TargetAddRpath.cmake index 03542231..397014ab 100644 --- a/cmake/TargetAddRpath.cmake +++ b/cmake/TargetAddRpath.cmake @@ -179,7 +179,7 @@ function(target_add_rpath) set_target_properties( ${_target} - PROPERTIES INSTALL_RPATH ${_install_rpath} + PROPERTIES INSTALL_RPATH "${_install_rpath}" INSTALL_RPATH_USE_LINK_PATH ${_ARGS_USE_LINK_PATH} BUILD_WITH_INSTALL_RPATH TRUE MACOSX_RPATH ON # This is ON by default. diff --git a/dist/mujoco.rc b/dist/mujoco.rc index 6191980f..37227828 100644 --- a/dist/mujoco.rc +++ b/dist/mujoco.rc @@ -1,6 +1,6 @@ 1 VERSIONINFO -FILEVERSION 3,3,3,0 -PRODUCTVERSION 3,3,3,0 +FILEVERSION 3,3,4,0 +PRODUCTVERSION 3,3,4,0 FILEOS 0x4 FILETYPE 0x1 { @@ -9,9 +9,9 @@ FILETYPE 0x1 BLOCK "040904b0" { VALUE "ProductName", "MuJoCo" - VALUE "ProductVersion", "3.3.3" + VALUE "ProductVersion", "3.3.4" VALUE "FileDescription", "MuJoCo" - VALUE "FileVersion", "3.3.3" + VALUE "FileVersion", "3.3.4" VALUE "InternalName", "mujoco.dll" VALUE "OriginalFilename", "mujoco.dll" VALUE "CompanyName", "Google DeepMind" diff --git a/dist/simulate.rc b/dist/simulate.rc index fca19ae2..aaf59660 100644 --- a/dist/simulate.rc +++ b/dist/simulate.rc @@ -1,8 +1,8 @@ MUJOCO ICON "mujoco.ico" 1 VERSIONINFO -FILEVERSION 3,3,3,0 -PRODUCTVERSION 3,3,3,0 +FILEVERSION 3,3,4,0 +PRODUCTVERSION 3,3,4,0 FILEOS 0x4 FILETYPE 0x1 { @@ -11,9 +11,9 @@ FILETYPE 0x1 BLOCK "040904b0" { VALUE "ProductName", "MuJoCo" - VALUE "ProductVersion", "3.3.3" + VALUE "ProductVersion", "3.3.4" VALUE "FileDescription", "MuJoCo" - VALUE "FileVersion", "3.3.3" + VALUE "FileVersion", "3.3.4" 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 11077782..5f6d824f 100644 --- a/doc/APIreference/APIglobals.rst +++ b/doc/APIreference/APIglobals.rst @@ -517,7 +517,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`` - - 333 + - 334 - 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/functions.rst b/doc/APIreference/functions.rst index 2bba2a3c..f43b0f20 100644 --- a/doc/APIreference/functions.rst +++ b/doc/APIreference/functions.rst @@ -418,7 +418,7 @@ and :math:`\dot q` is the generalized velocity ``mjData.qvel``, then :math:`h = .. mujoco-include:: mj_name2id -Get id of object with the specified mjtObj type and name, returns -1 if id not found. +Get id of object with the specified :ref:`mjtObj` type and name, returns -1 if id not found. .. _mj_id2name: @@ -427,7 +427,7 @@ Get id of object with the specified mjtObj type and name, returns -1 if id not f .. mujoco-include:: mj_id2name -Get name of object with the specified mjtObj type and id, returns NULL if name not found. +Get name of object with the specified :ref:`mjtObj` type and id, returns ``NULL`` if name not found. .. _mj_fullM: @@ -3783,24 +3783,6 @@ Attachment Attach child to a parent, return the attached element if success or NULL otherwise. -.. _mjs_detachBody: - -`mjs_detachBody <#mjs_detachBody>`__ -~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ - -.. mujoco-include:: mjs_detachBody - -Delete body and descendants from mjSpec, remove all references, return 0 on success. - -.. _mjs_detachDefault: - -`mjs_detachDefault <#mjs_detachDefault>`__ -~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ - -.. mujoco-include:: mjs_detachDefault - -Delete default class and descendants from mjSpec, remove all references, return 0 on success. - .. _AddTreeElements: Tree elements diff --git a/doc/APIreference/functions_override.rst b/doc/APIreference/functions_override.rst index 5d00db0a..da9c913a 100644 --- a/doc/APIreference/functions_override.rst +++ b/doc/APIreference/functions_override.rst @@ -231,6 +231,14 @@ generalized velocities to subtree angular momentum. More precisely if :math:`h` body index ``body`` in ``mjData.subtree_angmom`` (reported by the :ref:`subtreeangmom` sensor) and :math:`\dot q` is the generalized velocity ``mjData.qvel``, then :math:`h = H \dot q`. +.. _mj_name2id: + +Get id of object with the specified :ref:`mjtObj` type and name, returns -1 if id not found. + +.. _mj_id2name: + +Get name of object with the specified :ref:`mjtObj` type and id, returns ``NULL`` if name not found. + .. _mj_geomDistance: Returns the smallest signed distance between two geoms and optionally the segment from ``geom1`` to ``geom2``. diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index 588235ab..c2e47857 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -724,13 +724,20 @@ has any effect. The settings here are global and apply to the entire model. compiler converts degrees into radians, and mjModel always uses radians. For URDF models the parser sets this attribute to "radian" internally, regardless of the XML setting. +.. image:: images/changelog/meshfit.png + :align: right + :width: 40% + .. _compiler-fitaabb: :at:`fitaabb`: :at-val:`[false, true], "false"` The compiler is able to replace a mesh with a geometric primitive fitted to that mesh; see :ref:`geom ` below. If this attribute is "true", the fitting procedure uses the axis-aligned bounding box (aabb) of the mesh. Otherwise it uses the equivalent-inertia box of the mesh. The type of geometric primitive used for fitting is - specified separately for each geom. + specified separately for each geom. The models used to generate the image on the right can be found + `here `__ (fit inertia + box) and `here `__ (fit + aabb). .. _compiler-eulerseq: @@ -792,12 +799,14 @@ has any effect. The settings here are global and apply to the entire model. :at:`fusestatic`: :at-val:`[false, true], "false" for MJCF, "true" for URDF` This attribute controls a compiler optimization feature where static bodies are fused with their parent, and any - elements defined in those bodies are reassigned to the parent. This feature can only be used in models which do not - have elements capable of named references inside the kinematic tree - namely skins, contact pairs, excludes, tendons, - actuators, sensors, tuples, cameras, lights. If a model has any these elements, fusestatic does nothing even if - enabled. This optimization is particularly useful when importing URDF models which often have many dummy bodies, but - can also be used to optimize MJCF models. After optimization, the new model has identical kinematics and dynamics as - the original but is faster to simulate. + elements defined in those bodies are reassigned to the parent. Static bodies are fused with their parent unless + + - They are referenced by another element in the model. + - They contain a site which is referenced by a :ref:`force` or :ref:`torque` sensor. + + This optimization is particularly useful when importing URDF models which often have many dummy bodies, but can also + be used to optimize MJCF models. After optimization, the new model has identical kinematics and dynamics as the + original but is faster to simulate. .. _compiler-inertiafromgeom: @@ -1228,7 +1237,7 @@ The full list of processing steps applied by the compiler to each mesh is as fol :at:`content_type`: :at-val:`string, optional` If the file attribute is specified, then this sets the - `Media Type `_ (formerly known as MIME type) of the + `Media Type `__ (formerly known as MIME type) of the file to be loaded. Any filename extensions will be overloaded. Currently ``model/vnd.mujoco.msh``, ``model/obj``, and ``model/stl`` are supported. @@ -1776,15 +1785,19 @@ properties are grouped together. .. _asset-material-metallic: -:at:`metallic`: :at-val:`real, "0"` +:at:`metallic`: :at-val:`real, "-1"` This attribute corresponds to uniform metallicity coefficient applied to the entire material. This attribute has no - effect in MuJoCo's native renderer, but it can be useful when rendering scenes with an external renderer. + effect in MuJoCo's native renderer, but it can be useful when rendering scenes with a physically-based renderer. In + this case, if a non-negative value is specified, this metallic value should be multiplied by the metallic texture + sampled value to obtain the final metallicity of the material. .. _asset-material-roughness: -:at:`roughness`: :at-val:`real, "1"` +:at:`roughness`: :at-val:`real, "-1"` This attribute corresponds to uniform roughness coefficient applied to the entire material. This attribute has no - effect in MuJoCo's native renderer, but it can be useful when rendering scenes with an external renderer. + effect in MuJoCo's native renderer, but it can be useful when rendering scenes with a physically-based renderer. In + this case, if a non-negative value is specified, this roughness value should be multiplied by the roughness texture + sampled value to obtain the final roughness of the material. .. _asset-material-rgba: @@ -7528,6 +7541,12 @@ coordinated visual settings corresponding to a "theme", and then include this fi While all settings in mjVisual are global, the settings here could not be fit into any of the other subsections. So this is effectively a miscellaneous subsection. +.. _visual-global-cameraid: + +:at:`cameraid`: :at-val:`int, "-1"` + The id of the camera used when initially loading the model in the visualizer. The default value of -1 means the free + camera. In order to specify a :ref:`modeled camera`, use the camera's id as given by :ref:`mj_name2id`. + .. _visual-global-orthographic: :at:`orthographic`: :at-val:`[false, true], "false"` diff --git a/doc/XMLschema.rst b/doc/XMLschema.rst index 21272c90..b0ba5dd0 100644 --- a/doc/XMLschema.rst +++ b/doc/XMLschema.rst @@ -1298,11 +1298,13 @@ | :ref:`global | ? | :class: mjcf-attributes | | ` | | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`orthographic` | :ref:`fovy` | :ref:`ipd` | :ref:`azimuth` | | +| | | | :ref:`cameraid` | :ref:`orthographic` | :ref:`fovy` | :ref:`ipd` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`elevation` | :ref:`linewidth` | :ref:`glow` | :ref:`offwidth` | | +| | | | :ref:`azimuth` | :ref:`elevation` | :ref:`linewidth` | :ref:`glow` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`offheight` | :ref:`realtime` | :ref:`ellipsoidinertia` | :ref:`bvactive` | | +| | | | :ref:`offwidth` | :ref:`offheight` | :ref:`realtime` | :ref:`ellipsoidinertia` | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +| | | | :ref:`bvactive` | | | | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ | |_| visual |br| |_| |L| | | .. table:: | diff --git a/doc/changelog.rst b/doc/changelog.rst index 324d41e7..2f8e4375 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -2,37 +2,64 @@ Changelog ========= -Upcoming version (not yet release) ----------------------------------- +Upcoming version (not yet released) +----------------------------------- + +.. admonition:: Breaking API changes + :class: attention + + - The functions ``mjs_detachBody`` and ``mjs_detachDefault`` have been replaced by :ref:`mjs_delete`. + - The Python functions ``element.delete`` have been replaced by ``spec.delete(element)``. General ^^^^^^^ -- Refactored island implementation so that island data is memory-contiguous. This speeds up island processing in the - solver and clears the way for the addition of the Newton and PGS solvers (currently only CG is supported). -- Removed the :at:`shell` plugin. This is now supported by :ref:`flexcomp` and is active depending on - the :ref:`elastic2d` attribute (off by default). -- Replaced the :ref:`directional` (boolean) field for lights with a - :ref:`type` field (of type :ref:`mjtLightType`) to allow for additional lighting - types. -- Added new sub-component :ref:`mj_makeM` which combines the :ref:`mj_crb` call with additional logic to support the - introduction in 3.3.1 of :ref:`tendon armature`. In addition to the traditional - ``mjData.qM``, :ref:`mj_makeM` also computes ``mjData.M``, a CSR representation of the same matrix. -- Added a new function :ref:`mj_copyBack` to copy real-valued arrays in an mjModel to a compatible mjSpec. +- Added support for setting the initial camera in the viewer using + :ref:`visual/global/cameraid`. +- Added support to only sync the state in the Python :ref:`passive viewer`'s ``Sync`` method, this is + useful to improve performance. The default behavior is unchanged and copies the entire model and data. + +Bug fixes +^^^^^^^^^ +- Inverse dynamics were not being computed correctly when :ref:`tendon armature` was present, + now fixed. + +Version 3.3.3 (June 10, 2025) +----------------------------- + +General +^^^^^^^ +1. Refactored island implementation so that island data is memory-contiguous. This speeds up island processing in the + solver and clears the way for the addition of the Newton and PGS solvers (currently only CG is supported). +2. Removed the :at:`shell` plugin. This is now supported by :ref:`flexcomp` and is active depending on + the :ref:`elastic2d` attribute (off by default). +3. Replaced the :ref:`directional` (boolean) field for lights with a + :ref:`type` field (of type :ref:`mjtLightType`) to allow for additional lighting + types. +4. Added new sub-component :ref:`mj_makeM` which combines the :ref:`mj_crb` call with additional logic to support the + introduction in 3.3.1 of :ref:`tendon armature`. In addition to the traditional + ``mjData.qM``, :ref:`mj_makeM` also computes ``mjData.M``, a CSR representation of the same matrix. +5. Added a new function :ref:`mj_copyBack` to copy real-valued arrays in an mjModel to a compatible mjSpec. +6. Removed the limitation of :ref:`fusestatic` to models which contain no references. The fusestatic + flag will now fuse all bodies which are not referenced and ignore bodies which are referenced. Simulate ^^^^^^^^ -- The struct ``mjv_sceneState`` has been removed. This struct was used for partial synchronization of ``mjModel`` and - ``mjData`` when the Python viewer is used in passive mode. This functionality is now provided by :ref:`mjv_copyModel` - and :ref:`mjv_copyData`, which don't copy arrays which are not required for visualization. +7. The struct ``mjv_sceneState`` has been removed. This struct was used for partial synchronization of ``mjModel`` and + ``mjData`` when the Python viewer is used in passive mode. This functionality is now provided by :ref:`mjv_copyModel` + and :ref:`mjv_copyData`, which don't copy arrays which are not required for visualization. .. image:: images/changelog/procedural_terrain_generation.png - :width: 25% + :width: 33% :align: right Python bindings ^^^^^^^^^^^^^^^ -- Added examples of procedural terrain generation to the Model Editing tutorial: |mjspec_colab| +8. Added examples of procedural terrain generation to the Model Editing tutorial: |mjspec_colab| + +MJX +^^^ +9. Added tendon armature. Version 3.3.2 (April 28, 2025) ------------------------------ diff --git a/doc/computation/index.rst b/doc/computation/index.rst index ff87af05..aca4c663 100644 --- a/doc/computation/index.rst +++ b/doc/computation/index.rst @@ -583,8 +583,7 @@ Fast implicit-in-velocity (``implicitfast``) performance. **Euler**: - Use ``Euler`` for compatibillity with older models and :ref:`MJX`. Specifically for MJX, - setting the :ref:`eulerdamp` disable flag can :ref:`improve performance`. + Use ``Euler`` for compatibillity with older models. **implicitfast**: The ``implicitfast`` integrator has similar computational cost to ``Euler``, yet provides increased stability, and is therefore a strict improvement. It is the recommended integrator for most models. diff --git a/doc/includes/references.h b/doc/includes/references.h index 3f725ad9..3f121f15 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -795,8 +795,8 @@ struct mjLROpt_ { // options for mj_setLengthRange() mjtNum tolrange; // convergence tolerance (relative to range) }; typedef struct mjLROpt_ mjLROpt; -struct mjVFS_ { // virtual file system for loading from memory - void* impl_; // internal pointer to VFS memory +struct mjVFS_ { // virtual file system for loading from memory + void* impl_; // internal pointer to VFS memory }; typedef struct mjVFS_ mjVFS; struct mjOption_ { // physics options @@ -844,7 +844,8 @@ struct mjOption_ { // physics options typedef struct mjOption_ mjOption; struct mjVisual_ { // visualization options struct { // global parameters - int orthographic; // is the free camera orthographic (0: no, 1: yes) + int cameraid; // initial camera id (-1: free) + int orthographic; // is the free camera orthographic (0: no, 1: yes) float fovy; // y field-of-view of free camera (orthographic ? length : degree) float ipd; // inter-pupilary distance for free camera float azimuth; // initial azimuth of free camera (degrees) @@ -958,6 +959,7 @@ struct mjModel_ { int nbvh; // number of total bounding volumes in all bodies int nbvhstatic; // number of static bounding volumes (aabb stored in mjModel) int nbvhdynamic; // number of dynamic bounding volumes (aabb stored in mjData) + int noct; // number of total octree cells in all meshes int njnt; // number of joints int ngeom; // number of geoms int nsite; // number of sites @@ -1091,6 +1093,11 @@ struct mjModel_ { int* bvh_nodeid; // geom or elem id of node; -1: non-leaf (nbvh x 1) mjtNum* bvh_aabb; // local bounding box (center, size) (nbvhstatic x 6) + // octree spatial partitioning + int* oct_depth; // depth in the octree (noct x 1) + int* oct_child; // children of octree node (noct x 8) + mjtNum* oct_aabb; // octree node bounding box (center, size) (noct x 6) + // joints int* jnt_type; // type of joint (mjtJoint) (njnt x 1) int* jnt_qposadr; // start addr in 'qpos' for joint's data (njnt x 1) @@ -1278,6 +1285,8 @@ struct mjModel_ { int* mesh_facenum; // number of faces (nmesh x 1) int* mesh_bvhadr; // address of bvh root (nmesh x 1) int* mesh_bvhnum; // number of bvh (nmesh x 1) + int* mesh_octadr; // address of octree root (nmesh x 1) + int* mesh_octnum; // number of octree nodes (nmesh x 1) int* mesh_normaladr; // first normal address (nmesh x 1) int* mesh_normalnum; // number of normals (nmesh x 1) int* mesh_texcoordadr; // texcoord data address; -1: no texcoord (nmesh x 1) @@ -2875,6 +2884,7 @@ struct mjvOption_ { // abstract visualization options mjtByte skingroup[mjNGROUP]; // skin visualization by group mjtByte flags[mjNVISFLAG]; // visualization flags (indexed by mjtVisFlag) int bvh_depth; // depth of the bounding volume hierarchy to be visualized + int oct_depth; // depth of the octree to be visualized int flex_layer; // element layer to be visualized for 3D flex }; typedef struct mjvOption_ mjvOption; @@ -3410,8 +3420,6 @@ void mju_defaultTask(mjTask* task); void mju_taskJoin(mjTask* task); mjsElement* mjs_attach(mjsElement* parent, const mjsElement* child, const char* prefix, const char* suffix); -int mjs_detachBody(mjSpec* s, mjsBody* b); -int mjs_detachDefault(mjSpec* s, mjsDefault* d); mjsBody* mjs_addBody(mjsBody* body, const mjsDefault* def); mjsSite* mjs_addSite(mjsBody* body, const mjsDefault* def); mjsJoint* mjs_addJoint(mjsBody* body, const mjsDefault* def); @@ -3420,7 +3428,7 @@ mjsGeom* mjs_addGeom(mjsBody* body, const mjsDefault* def); mjsCamera* mjs_addCamera(mjsBody* body, const mjsDefault* def); mjsLight* mjs_addLight(mjsBody* body, const mjsDefault* def); mjsFrame* mjs_addFrame(mjsBody* body, mjsFrame* parentframe); -int mjs_delete(mjsElement* element); +int mjs_delete(mjSpec* spec, mjsElement* element); mjsActuator* mjs_addActuator(mjSpec* s, const mjsDefault* def); mjsSensor* mjs_addSensor(mjSpec* s); mjsFlex* mjs_addFlex(mjSpec* s); @@ -3438,6 +3446,19 @@ mjsTuple* mjs_addTuple(mjSpec* s); mjsKey* mjs_addKey(mjSpec* s); mjsPlugin* mjs_addPlugin(mjSpec* s); mjsDefault* mjs_addDefault(mjSpec* s, const char* classname, const mjsDefault* parent); +const char* mjs_setToMotor(mjsActuator* actuator); +const char* mjs_setToPosition(mjsActuator* actuator, double kp, double kv[1], + double dampratio[1], double timeconst[1], double inheritrange); +const char* mjs_setToIntVelocity(mjsActuator* actuator, double kp, double kv[1], + double dampratio[1], double timeconst[1], double inheritrange); +const char* mjs_setToVelocity(mjsActuator* actuator, double kv); +const char* mjs_setToDamper(mjsActuator* actuator, double kv); +const char* mjs_setToCylinder(mjsActuator* actuator, double timeconst, + double bias, double area, double diameter); +const char* mjs_setToMuscle(mjsActuator* actuator, double timeconst[2], double tausmooth, + double range[2], double force, double scale, double lmin, + double lmax, double vmax, double fpmax, double fvmax); +const char* mjs_setToAdhesion(mjsActuator* actuator, double gain); mjsMesh* mjs_addMesh(mjSpec* s, const mjsDefault* def); mjsHField* mjs_addHField(mjSpec* s); mjsSkin* mjs_addSkin(mjSpec* s); diff --git a/doc/mjx.rst b/doc/mjx.rst index e0106b5f..79b725d4 100644 --- a/doc/mjx.rst +++ b/doc/mjx.rst @@ -206,7 +206,7 @@ The following features are **fully supported** in MJX: * - Category - Feature * - Dynamics - - :ref:`Forward ` + - :ref:`Forward `, :ref:`Inverse ` * - :ref:`Joint ` - ``FREE``, ``BALL``, ``SLIDE``, ``HINGE`` * - :ref:`Transmission ` @@ -235,8 +235,6 @@ The following features are **fully supported** in MJX: - 1, 3, 4, 6 (1 is not supported with ``ELLIPTIC``) * - :ref:`Solver ` - ``CG``, ``NEWTON`` - * - Dynamics - - :ref:`Inverse ` * - Fluid Model - :ref:`flInertia` * - :ref:`Tendons ` @@ -248,6 +246,8 @@ The following features are **fully supported** in MJX: ``FRAMEANGVEL``, ``SUBTREELINVEL``, ``SUBTREEANGMOM``, ``TOUCH``, ``ACCELEROMETER``, ``FORCE``, ``TORQUE``, ``ACTUATORFRC``, ``JOINTACTFRC``, ``TENDONACTFRC``, ``FRAMELINACC``, ``FRAMEANGACC`` (``ACCELEROMETER``, ``FORCE``, ``TORQUE`` not supported with connect or weld equality constraints) + * - Lights + - Positions and directions of lights The following features are **in development** and coming soon: @@ -268,8 +268,6 @@ The following features are **in development** and coming soon: - :ref:`flEllipsoid` * - :ref:`Sensors ` - All except ``PLUGIN``, ``USER`` - * - Lights - - Positions and directions of lights The following features are **unsupported**: diff --git a/doc/programming/modeledit.rst b/doc/programming/modeledit.rst index ac74efdf..95e9c221 100644 --- a/doc/programming/modeledit.rst +++ b/doc/programming/modeledit.rst @@ -96,25 +96,37 @@ Elements cannot be created directly; they are returned to the user by the corres my_geom->type = mjGEOM_BOX; // set geom type my_geom->size[0] = my_geom->size[1] = my_geom->size[2] = 0.5; // set box size mjModel* model = mj_compile(spec, NULL); // compile to mjModel + ... + mj_deleteModel(model); // free model + mj_deleteSpec(spec); // free spec The ``NULL`` second argument to :ref:`mjs_addGeom` is the optional default class pointer. When using defaults procedurally, default classes are passed in explicitly to element constructors. The global defaults of all elements (used when no default class is passed in) can be inspected in `user_init.c `__. +.. _meMemory: + +Memory management +^^^^^^^^^^^^^^^^^ + +As seen in the examples above, model elements are never allocated by the user directly, but rather returned by a +constructor. The library takes ownership of all elements and frees them when the parent :ref:`mjSpec` is deleted using +:ref:`mj_deleteSpec`. The user is only responsible for freeing :ref:`mjSpec` structs. + .. _meAttachment: Attachment ^^^^^^^^^^ -This framework introduces a powerful new feature: attaching and detaching model subtrees. This feature is already used -to power the :ref:`attach` an :ref:`replicate` meta-elements in MJCF. Attachment allows the user -to move or copy a subtree from one model into another, while also copying or moving related referenced assets and -referencing elements from outside the kinematic tree (e.g., actuators and sensors). Similarly, detaching a subtree will -remove all associated elements from the model. The default behavior is to move the child into the parent while -attaching, so subsequent changes to the child will also change the parent. Alternatively, the user can choose to make an -entirely new copy during attach using :ref:`mjs_setDeepCopy`. This flag is temporarily set to true while parsing XMLs. -It is possible to :ref:`attach a body to a frame`: +This framework introduces a powerful new feature: attaching and deleting model subtrees. This feature is already used to +power the :ref:`attach` an :ref:`replicate` meta-elements in MJCF. Attachment allows the user to +move or copy a subtree from one model into another, while also copying or moving related referenced assets and +referencing elements from outside the kinematic tree (e.g., actuators and sensors). Similarly, deleting a subtree will +remove all associated elements from the model. The default behavior ("shallow copy") is to move the child into the +parent while attaching, so subsequent changes to the child will also change the parent. Alternatively, the user can +choose to make an entirely new copy during attach using :ref:`mjs_setDeepCopy`. This flag is temporarily set to true +while parsing XMLs. It is possible to :ref:`attach a body to a frame`: .. code-block:: C diff --git a/doc/python.rst b/doc/python.rst index 61ebcdc8..cf4c3dd2 100644 --- a/doc/python.rst +++ b/doc/python.rst @@ -50,25 +50,17 @@ Interactive viewer An interactive GUI viewer is provided as part of the Python package in the ``mujoco.viewer`` module. It is based on the same codebase as the :ref:`simulate` application that ships with the MuJoCo binary releases. Three distinct -use cases are supported: - -.. _PyViewerApp: - -Standalone app --------------- - -- ``python -m mujoco.viewer`` launches an empty visualization session, where a model can be loaded by drag-and-drop. -- ``python -m mujoco.viewer --mjcf=/path/to/some/mjcf.xml`` launches a visualization session for the specified - model file. +use cases are supported: :ref:`managed viewer`, :ref:`standalone app`, and :ref:`passive +viewer`. .. _PyViewerManaged: Managed viewer -------------- -Called from a Python program/script, through the function ``viewer.launch``. This function *blocks user code* to -support precise timing of the physics loop. This mode should be used if user code is implemented as -:ref:`engine plugins` or :ref:`physics callbacks`, and is called by MuJoCo during :ref:`mj_step`. +The ``viewer.launch`` function launches the interactive viewer and *blocks user code* which is useful to support precise +timing of the physics loop. This mode should be used if user code is implemented as :ref:`engine +plugins` or :ref:`physics callbacks`, and is called by MuJoCo during :ref:`mj_step`. - ``viewer.launch()`` launches an empty visualization session, where a model can be loaded by drag-and-drop. - ``viewer.launch(model)`` launches a visualization session for the given ``mjModel`` where the visualizer @@ -76,21 +68,33 @@ support precise timing of the physics loop. This mode should be used if user cod - ``viewer.launch(model, data)`` is the same as above, except that the visualizer operates directly on the given ``mjData`` instance -- upon exit the ``data`` object will have been modified. +.. _PyViewerApp: + +Standalone app +-------------- + +The ``mujoco.viewer`` Python package uses the ``if __name__ == '__main__'`` mechanism to allow the :ref:`managed +viewer` to be called directly from the command line as a standalone app: + +- ``python -m mujoco.viewer`` launches an empty visualization session, where a model can be loaded by drag-and-drop. +- ``python -m mujoco.viewer --mjcf=/path/to/some/mjcf.xml`` launches a visualization session for the specified + model file. + .. _PyViewerPassive: Passive viewer -------------- -By calling ``viewer.launch_passive(model, data)``. This function *does not block*, allowing user code to continue -execution. In this mode, the user's script is responsible for timing and advancing the physics state, and mouse-drag -perturbations will not work unless the user explicitly synchronizes incoming events. +The ``viewer.launch_passive`` function launches the interactive viewer in a way which *does not block*, allowing user +code to continue execution. In this mode, the user's script is responsible for timing and advancing the physics state, +and mouse-drag perturbations will not work unless the user explicitly synchronizes incoming events. .. warning:: - On MacOS, ``launch_passive`` requires that the user script is executed via a special ``mjpython`` launcher. - The ``mjpython`` command is installed as part of the ``mujoco`` package, and can be used as a drop-in replacement - for the usual ``python`` command and supports an identical set of command line flags and arguments. For example, - a script can be executed via ``mjpython my_script.py``, and an IPython shell can be launched via - ``mjpython -m IPython``. + On MacOS, ``launch_passive`` requires that the user script is executed via a special ``mjpython`` launcher, this is + needed to circumvent a platform limitation which requires the main thread to be one that does the rendering. The + ``mjpython`` command is installed as part of the ``mujoco`` package, and can be used as a drop-in replacement for the + usual ``python`` command and supports an identical set of command line flags and arguments. For example, a script can + be executed via ``mjpython my_script.py``, and an IPython shell can be launched via ``mjpython -m IPython``. The ``launch_passive`` function returns a handle which can be used to interact with the viewer. It has the following attributes: @@ -103,10 +107,14 @@ attributes: state. These include the ``mjModel`` and ``mjData`` instance passed to ``launch_passive``, and also the ``cam``, ``opt``, and ``pert`` properties of the viewer handle. -- ``sync()``: synchronizes state between ``mjModel``, ``mjData``, and GUI user inputs since the previous call to - ``sync``. In order to allow user scripts to make arbitrary modifications to ``mjModel`` and ``mjData`` without - needing to hold the viewer lock, the passive viewer does not access or modify these structs outside of ``sync`` - calls. +- ``sync(state_only=False)``: synchronizes between the user's ``mjModel``, ``mjData`` and the GUI. In order to allow + user scripts to make arbitrary modifications to ``mjModel`` and ``mjData`` without needing to hold the viewer lock, + the passive viewer does not access or modify these structs outside of ``sync`` calls. If the ``state_only`` argument + is ``True``, instead of syncing everything, only the ``mjData`` fields corresponding to + :ref:`mjSTATE_INTEGRATION` are synced, followed by a call to :ref:`mj_forward`. The latter option is much + faster, but would not pick up arbitrary changes as in the default case. Changes made via the GUI are picked up in + either case but changing e.g., ``mjModel.geom_rgba`` via code will be picked up when ``state_only=False`` but not when + ``state_only=True``. User scripts must call ``sync`` in order for the viewer to reflect physics state changes. The ``sync`` function also transfers user inputs from the GUI back into ``mjOption`` (inside ``mjModel``) and ``mjData``, including @@ -603,11 +611,10 @@ Lists of all elements in a spec can be accessed using named properties, using th Element removal ^^^^^^^^^^^^^^^ -For elements that can have children (bodies and defaults), the methods ``spec.detach_body(body)`` and -``spec.detach_default(def)`` remove, respectively, ``body`` and ``def`` from the spec, together with all of their -children. When detaching body subtrees, all elements which reference elements in the subtree, will also be removed. For -all other elements, the method ``delete()`` removes the corresponding element from the spec, e.g. -``spec.geom('my_geom').delete()`` will remove the geom named "my_geom" and all of the elements that reference it. +The method ``delete()`` removes the corresponding element from the spec, e.g. ``spec.delete(spec.geom('my_geom'))`` will +remove the geom named "my_geom" and all of the elements that reference it. For elements that can have children (bodies +and defaults), ``delete`` also removes all of their children. When deleting body subtrees, all elements which reference +elements in the subtree, will also be removed. Tree traversal ^^^^^^^^^^^^^^ @@ -1061,5 +1068,5 @@ non-exhaustive list of specific mujoco-py features: This is the one context in which the MuJoCo library (and therefore also ``mujoco``) is stateful: it holds a copy in memory of the last XML that was compiled, which is used in :ref:`mujoco.mj_saveLastXML(fname) `. Note that mujoco-py’s implementation has a convenient extra feature, whereby the pose (as determined by ``sim.data``’s - state) is transformed to a keyframe that’s added to the model before saving. This extra feature is not currently + state) is transformed to a keyframe that’s added to the model before saving. This extra feature is not currently available in ``mujoco``. diff --git a/doc/unity.rst b/doc/unity.rst index 876a96a6..aec5a7b6 100644 --- a/doc/unity.rst +++ b/doc/unity.rst @@ -37,14 +37,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.3.3.dylib`` (it can be +``/Applications/MuJoCo.app/Contents/Frameworks/mujoco.framework/Versions/Current/libmujoco.3.3.4.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.3.3/lib/libmujoco.so.3.3.3`` and rename it as ``libmujoco.so``. +``~/.mujoco/mujoco-3.3.4/lib/libmujoco.so.3.3.4`` and rename it as ``libmujoco.so``. Windows _______ diff --git a/include/mujoco/experimental/usd/layer_sink.h b/include/mujoco/experimental/usd/layer_sink.h new file mode 100644 index 00000000..34af9134 --- /dev/null +++ b/include/mujoco/experimental/usd/layer_sink.h @@ -0,0 +1,41 @@ +// Copyright 2025 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. + +#ifndef MUJOCO_SRC_EXPERIMENTAL_USD_LAYER_SINK_H_ +#define MUJOCO_SRC_EXPERIMENTAL_USD_LAYER_SINK_H_ + +#include + +#include +#include +#include + +namespace mujoco { +namespace usd { + +// A sink for writing pose data to an SDF layer. +// Opinions will be authored on the edit target layer for the +// passed stage argument. +class LayerSink { + public: + explicit LayerSink(pxr::UsdStageRefPtr stage); + void Update(const mjData* const data, std::vector body_paths_); + + private: + pxr::UsdStageRefPtr stage_; +}; +} // namespace usd +} // namespace mujoco + +#endif // MUJOCO_SRC_EXPERIMENTAL_USD_LAYER_SINK_H_ diff --git a/src/experimental/usd/mjcPhysics/actuatorAPI.h b/include/mujoco/experimental/usd/mjcPhysics/actuatorAPI.h similarity index 97% rename from src/experimental/usd/mjcPhysics/actuatorAPI.h rename to include/mujoco/experimental/usd/mjcPhysics/actuatorAPI.h index 49e6721a..7e910e84 100644 --- a/src/experimental/usd/mjcPhysics/actuatorAPI.h +++ b/include/mujoco/experimental/usd/mjcPhysics/actuatorAPI.h @@ -17,18 +17,18 @@ /// \file mjcPhysics/actuatorAPI.h -#include "./api.h" -#include "./tokens.h" -#include "pxr/base/gf/matrix4d.h" -#include "pxr/base/gf/vec3d.h" -#include "pxr/base/gf/vec3f.h" -#include "pxr/base/tf/token.h" -#include "pxr/base/tf/type.h" -#include "pxr/base/vt/value.h" -#include "pxr/pxr.h" -#include "pxr/usd/usd/apiSchemaBase.h" -#include "pxr/usd/usd/prim.h" -#include "pxr/usd/usd/stage.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include PXR_NAMESPACE_OPEN_SCOPE @@ -775,23 +775,6 @@ class MjcPhysicsActuatorAPI : public UsdAPISchemaBase { MJCPHYSICS_API UsdRelationship CreateMjcRefSiteRel() const; - public: - // --------------------------------------------------------------------- // - // MJCCRANKSITE - // --------------------------------------------------------------------- // - /// If specified, the actuator acts on a slider-crank mechanism which is - /// implicitly determined by the actuator (i.e., it is not a separate model - /// element). The target site corresponds to the pin joining the crank and the - /// connecting rod. - /// - MJCPHYSICS_API - UsdRelationship GetMjcCrankSiteRel() const; - - /// See GetMjcCrankSiteRel(), and also - /// \ref Usd_Create_Or_Get_Property for when to use Get vs Create - MJCPHYSICS_API - UsdRelationship CreateMjcCrankSiteRel() const; - public: // --------------------------------------------------------------------- // // MJCSLIDERSITE diff --git a/src/experimental/usd/mjcPhysics/api.h b/include/mujoco/experimental/usd/mjcPhysics/api.h similarity index 97% rename from src/experimental/usd/mjcPhysics/api.h rename to include/mujoco/experimental/usd/mjcPhysics/api.h index dc5476c9..529c31f5 100644 --- a/src/experimental/usd/mjcPhysics/api.h +++ b/include/mujoco/experimental/usd/mjcPhysics/api.h @@ -15,7 +15,7 @@ #ifndef MJCPHYSICS_API_H #define MJCPHYSICS_API_H -#include "pxr/base/arch/export.h" +#include #if defined(PXR_STATIC) #define MJCPHYSICS_API diff --git a/src/experimental/usd/mjcPhysics/collisionAPI.h b/include/mujoco/experimental/usd/mjcPhysics/collisionAPI.h similarity index 93% rename from src/experimental/usd/mjcPhysics/collisionAPI.h rename to include/mujoco/experimental/usd/mjcPhysics/collisionAPI.h index e3681526..482d4456 100644 --- a/src/experimental/usd/mjcPhysics/collisionAPI.h +++ b/include/mujoco/experimental/usd/mjcPhysics/collisionAPI.h @@ -17,18 +17,18 @@ /// \file mjcPhysics/collisionAPI.h -#include "./api.h" -#include "./tokens.h" -#include "pxr/base/gf/matrix4d.h" -#include "pxr/base/gf/vec3d.h" -#include "pxr/base/gf/vec3f.h" -#include "pxr/base/tf/token.h" -#include "pxr/base/tf/type.h" -#include "pxr/base/vt/value.h" -#include "pxr/pxr.h" -#include "pxr/usd/usd/apiSchemaBase.h" -#include "pxr/usd/usd/prim.h" -#include "pxr/usd/usd/stage.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include PXR_NAMESPACE_OPEN_SCOPE diff --git a/include/mujoco/experimental/usd/mjcPhysics/jointAPI.h b/include/mujoco/experimental/usd/mjcPhysics/jointAPI.h new file mode 100644 index 00000000..75d04637 --- /dev/null +++ b/include/mujoco/experimental/usd/mjcPhysics/jointAPI.h @@ -0,0 +1,622 @@ +// Copyright 2025 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. + +#ifndef MJCPHYSICS_GENERATED_JOINTAPI_H +#define MJCPHYSICS_GENERATED_JOINTAPI_H + +/// \file mjcPhysics/jointAPI.h + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +PXR_NAMESPACE_OPEN_SCOPE + +class SdfAssetPath; + +// -------------------------------------------------------------------------- // +// PHYSICSJOINTSAPI // +// -------------------------------------------------------------------------- // + +/// \class MjcPhysicsJointAPI +/// +/// API describing a Mujoco joint. +/// +/// For any described attribute \em Fallback \em Value or \em Allowed \em Values +/// below that are text/tokens, the actual token is published and defined in +/// \ref MjcPhysicsTokens. So to set an attribute to the value "rightHanded", +/// use MjcPhysicsTokens->rightHanded as the value. +/// +class MjcPhysicsJointAPI : public UsdAPISchemaBase { + public: + /// Compile time constant representing what kind of schema this class is. + /// + /// \sa UsdSchemaKind + static const UsdSchemaKind schemaKind = UsdSchemaKind::SingleApplyAPI; + + /// Construct a MjcPhysicsJointAPI on UsdPrim \p prim . + /// Equivalent to MjcPhysicsJointAPI::Get(prim.GetStage(), prim.GetPath()) + /// for a \em valid \p prim, but will not immediately throw an error for + /// an invalid \p prim + explicit MjcPhysicsJointAPI(const UsdPrim &prim = UsdPrim()) + : UsdAPISchemaBase(prim) {} + + /// Construct a MjcPhysicsJointAPI on the prim held by \p schemaObj . + /// Should be preferred over MjcPhysicsJointAPI(schemaObj.GetPrim()), + /// as it preserves SchemaBase state. + explicit MjcPhysicsJointAPI(const UsdSchemaBase &schemaObj) + : UsdAPISchemaBase(schemaObj) {} + + /// Destructor. + MJCPHYSICS_API + virtual ~MjcPhysicsJointAPI(); + + /// Return a vector of names of all pre-declared attributes for this schema + /// class and all its ancestor classes. Does not include attributes that + /// may be authored by custom/extended methods of the schemas involved. + MJCPHYSICS_API + static const TfTokenVector &GetSchemaAttributeNames( + bool includeInherited = true); + + /// Return a MjcPhysicsJointAPI holding the prim adhering to this + /// schema at \p path on \p stage. If no prim exists at \p path on + /// \p stage, or if the prim at that path does not adhere to this schema, + /// return an invalid schema object. This is shorthand for the following: + /// + /// \code + /// MjcPhysicsJointAPI(stage->GetPrimAtPath(path)); + /// \endcode + /// + MJCPHYSICS_API + static MjcPhysicsJointAPI Get(const UsdStagePtr &stage, const SdfPath &path); + + /// Returns true if this single-apply API schema can be applied to + /// the given \p prim. If this schema can not be a applied to the prim, + /// this returns false and, if provided, populates \p whyNot with the + /// reason it can not be applied. + /// + /// Note that if CanApply returns false, that does not necessarily imply + /// that calling Apply will fail. Callers are expected to call CanApply + /// before calling Apply if they want to ensure that it is valid to + /// apply a schema. + /// + /// \sa UsdPrim::GetAppliedSchemas() + /// \sa UsdPrim::HasAPI() + /// \sa UsdPrim::CanApplyAPI() + /// \sa UsdPrim::ApplyAPI() + /// \sa UsdPrim::RemoveAPI() + /// + MJCPHYSICS_API + static bool CanApply(const UsdPrim &prim, std::string *whyNot = nullptr); + + /// Applies this single-apply API schema to the given \p prim. + /// This information is stored by adding "PhysicsJointsAPI" to the + /// token-valued, listOp metadata \em apiSchemas on the prim. + /// + /// \return A valid MjcPhysicsJointAPI object is returned upon success. + /// An invalid (or empty) MjcPhysicsJointAPI object is returned upon + /// failure. See \ref UsdPrim::ApplyAPI() for conditions + /// resulting in failure. + /// + /// \sa UsdPrim::GetAppliedSchemas() + /// \sa UsdPrim::HasAPI() + /// \sa UsdPrim::CanApplyAPI() + /// \sa UsdPrim::ApplyAPI() + /// \sa UsdPrim::RemoveAPI() + /// + MJCPHYSICS_API + static MjcPhysicsJointAPI Apply(const UsdPrim &prim); + + protected: + /// Returns the kind of schema this class belongs to. + /// + /// \sa UsdSchemaKind + MJCPHYSICS_API + UsdSchemaKind _GetSchemaKind() const override; + + private: + // needs to invoke _GetStaticTfType. + friend class UsdSchemaRegistry; + MJCPHYSICS_API + static const TfType &_GetStaticTfType(); + + static bool _IsTypedSchema(); + + // override SchemaBase virtuals. + MJCPHYSICS_API + const TfType &_GetTfType() const override; + + public: + // --------------------------------------------------------------------- // + // MJCSPRINGDAMPER + // --------------------------------------------------------------------- // + /// When both numbers are positive, the compiler will override any stiffness + /// and damping values specified with the attributes below, and will instead + /// set them automatically so that the resulting mass-spring-damper for this + /// joint has the desired time constant (first value) and damping ratio + /// (second value). This is done by taking into account the joint inertia in + /// the model reference configuration. Note that the format is the same as the + /// solref parameter of the constraint solver. + /// + /// | || + /// | -- | -- | + /// | Declaration | `uniform double[] mjc:springdamper = [0, 0]` | + /// | C++ Type | VtArray | + /// | \ref Usd_Datatypes "Usd Type" | SdfValueTypeNames->DoubleArray | + /// | \ref SdfVariability "Variability" | SdfVariabilityUniform | + MJCPHYSICS_API + UsdAttribute GetMjcSpringdamperAttr() const; + + /// See GetMjcSpringdamperAttr(), and also + /// \ref Usd_Create_Or_Get_Property for when to use Get vs Create. + /// If specified, author \p defaultValue as the attribute's default, + /// sparsely (when it makes sense to do so) if \p writeSparsely is \c true - + /// the default for \p writeSparsely is \c false. + MJCPHYSICS_API + UsdAttribute CreateMjcSpringdamperAttr( + VtValue const &defaultValue = VtValue(), + bool writeSparsely = false) const; + + public: + // --------------------------------------------------------------------- // + // MJCSOLREFLIMIT + // --------------------------------------------------------------------- // + /// Constraint solver parameters for simulating joint limits. + /// + /// | || + /// | -- | -- | + /// | Declaration | `uniform double[] mjc:solreflimit = [0.02, 1]` | + /// | C++ Type | VtArray | + /// | \ref Usd_Datatypes "Usd Type" | SdfValueTypeNames->DoubleArray | + /// | \ref SdfVariability "Variability" | SdfVariabilityUniform | + MJCPHYSICS_API + UsdAttribute GetMjcSolreflimitAttr() const; + + /// See GetMjcSolreflimitAttr(), and also + /// \ref Usd_Create_Or_Get_Property for when to use Get vs Create. + /// If specified, author \p defaultValue as the attribute's default, + /// sparsely (when it makes sense to do so) if \p writeSparsely is \c true - + /// the default for \p writeSparsely is \c false. + MJCPHYSICS_API + UsdAttribute CreateMjcSolreflimitAttr(VtValue const &defaultValue = VtValue(), + bool writeSparsely = false) const; + + public: + // --------------------------------------------------------------------- // + // MJCSOLIMPLIMIT + // --------------------------------------------------------------------- // + /// Constraint solver parameters for simulating joint limits. + /// + /// | || + /// | -- | -- | + /// | Declaration | `uniform double[] mjc:solimplimit = [0.9, 0.95, 0.001, + /// 0.5, 2]` | | C++ Type | VtArray | | \ref Usd_Datatypes "Usd Type" + /// | SdfValueTypeNames->DoubleArray | | \ref SdfVariability "Variability" | + /// SdfVariabilityUniform | + MJCPHYSICS_API + UsdAttribute GetMjcSolimplimitAttr() const; + + /// See GetMjcSolimplimitAttr(), and also + /// \ref Usd_Create_Or_Get_Property for when to use Get vs Create. + /// If specified, author \p defaultValue as the attribute's default, + /// sparsely (when it makes sense to do so) if \p writeSparsely is \c true - + /// the default for \p writeSparsely is \c false. + MJCPHYSICS_API + UsdAttribute CreateMjcSolimplimitAttr(VtValue const &defaultValue = VtValue(), + bool writeSparsely = false) const; + + public: + // --------------------------------------------------------------------- // + // MJCSOLREFFRICTION + // --------------------------------------------------------------------- // + /// Constraint solver parameters for simulating dry friction. + /// + /// | || + /// | -- | -- | + /// | Declaration | `uniform double[] mjc:solreffriction = [0.02, 1]` | + /// | C++ Type | VtArray | + /// | \ref Usd_Datatypes "Usd Type" | SdfValueTypeNames->DoubleArray | + /// | \ref SdfVariability "Variability" | SdfVariabilityUniform | + MJCPHYSICS_API + UsdAttribute GetMjcSolreffrictionAttr() const; + + /// See GetMjcSolreffrictionAttr(), and also + /// \ref Usd_Create_Or_Get_Property for when to use Get vs Create. + /// If specified, author \p defaultValue as the attribute's default, + /// sparsely (when it makes sense to do so) if \p writeSparsely is \c true - + /// the default for \p writeSparsely is \c false. + MJCPHYSICS_API + UsdAttribute CreateMjcSolreffrictionAttr( + VtValue const &defaultValue = VtValue(), + bool writeSparsely = false) const; + + public: + // --------------------------------------------------------------------- // + // MJCSOLIMPFRICTION + // --------------------------------------------------------------------- // + /// Constraint solver parameters for simulating dry friction. + /// + /// | || + /// | -- | -- | + /// | Declaration | `uniform double[] mjc:solimpfriction = [0.9, 0.95, 0.001, + /// 0.5, 2]` | | C++ Type | VtArray | | \ref Usd_Datatypes "Usd Type" + /// | SdfValueTypeNames->DoubleArray | | \ref SdfVariability "Variability" | + /// SdfVariabilityUniform | + MJCPHYSICS_API + UsdAttribute GetMjcSolimpfrictionAttr() const; + + /// See GetMjcSolimpfrictionAttr(), and also + /// \ref Usd_Create_Or_Get_Property for when to use Get vs Create. + /// If specified, author \p defaultValue as the attribute's default, + /// sparsely (when it makes sense to do so) if \p writeSparsely is \c true - + /// the default for \p writeSparsely is \c false. + MJCPHYSICS_API + UsdAttribute CreateMjcSolimpfrictionAttr( + VtValue const &defaultValue = VtValue(), + bool writeSparsely = false) const; + + public: + // --------------------------------------------------------------------- // + // MJCSTIFFNESS + // --------------------------------------------------------------------- // + /// Joint stiffness. If this value is positive, a spring will be created with + /// equilibrium position given by springref below. The spring force is + /// computed along with the other passive forces. + /// + /// | || + /// | -- | -- | + /// | Declaration | `uniform double mjc:stiffness = 0` | + /// | C++ Type | double | + /// | \ref Usd_Datatypes "Usd Type" | SdfValueTypeNames->Double | + /// | \ref SdfVariability "Variability" | SdfVariabilityUniform | + MJCPHYSICS_API + UsdAttribute GetMjcStiffnessAttr() const; + + /// See GetMjcStiffnessAttr(), and also + /// \ref Usd_Create_Or_Get_Property for when to use Get vs Create. + /// If specified, author \p defaultValue as the attribute's default, + /// sparsely (when it makes sense to do so) if \p writeSparsely is \c true - + /// the default for \p writeSparsely is \c false. + MJCPHYSICS_API + UsdAttribute CreateMjcStiffnessAttr(VtValue const &defaultValue = VtValue(), + bool writeSparsely = false) const; + + public: + // --------------------------------------------------------------------- // + // MJCACTUATORFRCRANGEMIN + // --------------------------------------------------------------------- // + /// Minimum range for clamping total actuator forces acting on this joint. See + /// Force limits for details. It is available only for scalar joints (hinge + /// and slider) and ignored for ball and free joints. The compiler expects the + /// first value to be smaller than the second value. Setting this attribute + /// without specifying actuatorfrclimited is an error if compiler-autolimits + /// is 'false'. + /// + /// | || + /// | -- | -- | + /// | Declaration | `uniform double mjc:actuatorfrcrange:min = 0` | + /// | C++ Type | double | + /// | \ref Usd_Datatypes "Usd Type" | SdfValueTypeNames->Double | + /// | \ref SdfVariability "Variability" | SdfVariabilityUniform | + MJCPHYSICS_API + UsdAttribute GetMjcActuatorfrcrangeMinAttr() const; + + /// See GetMjcActuatorfrcrangeMinAttr(), and also + /// \ref Usd_Create_Or_Get_Property for when to use Get vs Create. + /// If specified, author \p defaultValue as the attribute's default, + /// sparsely (when it makes sense to do so) if \p writeSparsely is \c true - + /// the default for \p writeSparsely is \c false. + MJCPHYSICS_API + UsdAttribute CreateMjcActuatorfrcrangeMinAttr( + VtValue const &defaultValue = VtValue(), + bool writeSparsely = false) const; + + public: + // --------------------------------------------------------------------- // + // MJCACTUATORFRCRANGEMAX + // --------------------------------------------------------------------- // + /// Maximum range for clamping total actuator forces acting on this joint. See + /// Force limits for details. It is available only for scalar joints (hinge + /// and slider) and ignored for ball and free joints. The compiler expects the + /// first value to be smaller than the second value. Setting this attribute + /// without specifying actuatorfrclimited is an error if compiler-autolimits + /// is 'false'. + /// + /// | || + /// | -- | -- | + /// | Declaration | `uniform double mjc:actuatorfrcrange:max = 0` | + /// | C++ Type | double | + /// | \ref Usd_Datatypes "Usd Type" | SdfValueTypeNames->Double | + /// | \ref SdfVariability "Variability" | SdfVariabilityUniform | + MJCPHYSICS_API + UsdAttribute GetMjcActuatorfrcrangeMaxAttr() const; + + /// See GetMjcActuatorfrcrangeMaxAttr(), and also + /// \ref Usd_Create_Or_Get_Property for when to use Get vs Create. + /// If specified, author \p defaultValue as the attribute's default, + /// sparsely (when it makes sense to do so) if \p writeSparsely is \c true - + /// the default for \p writeSparsely is \c false. + MJCPHYSICS_API + UsdAttribute CreateMjcActuatorfrcrangeMaxAttr( + VtValue const &defaultValue = VtValue(), + bool writeSparsely = false) const; + + public: + // --------------------------------------------------------------------- // + // MJCACTUATORFRCLIMITED + // --------------------------------------------------------------------- // + /// This attribute specifies whether actuator forces acting on the joint + /// should be clamped. See Force limits for details. It is available only for + /// scalar joints (hinge and slider) and ignored for ball and free joints. + /// This attribute interacts with the actuatorfrcrange attribute. If this + /// attribute is 'false', actuator force clamping is disabled. If it is + /// 'true', actuator force clamping is enabled. If this attribute is 'auto', + /// and autolimits is set in compiler, actuator force clamping will be enabled + /// if actuatorfrcrange is defined. + /// + /// | || + /// | -- | -- | + /// | Declaration | `uniform token mjc:actuatorfrclimited = "auto"` | + /// | C++ Type | TfToken | + /// | \ref Usd_Datatypes "Usd Type" | SdfValueTypeNames->Token | + /// | \ref SdfVariability "Variability" | SdfVariabilityUniform | + /// | \ref MjcPhysicsTokens "Allowed Values" | false, true, auto | + MJCPHYSICS_API + UsdAttribute GetMjcActuatorfrclimitedAttr() const; + + /// See GetMjcActuatorfrclimitedAttr(), and also + /// \ref Usd_Create_Or_Get_Property for when to use Get vs Create. + /// If specified, author \p defaultValue as the attribute's default, + /// sparsely (when it makes sense to do so) if \p writeSparsely is \c true - + /// the default for \p writeSparsely is \c false. + MJCPHYSICS_API + UsdAttribute CreateMjcActuatorfrclimitedAttr( + VtValue const &defaultValue = VtValue(), + bool writeSparsely = false) const; + + public: + // --------------------------------------------------------------------- // + // MJCACTUATORGRAVCOMP + // --------------------------------------------------------------------- // + /// If this flag is enabled, gravity compensation applied to this joint is + /// added to actuator forces (mjData.qfrc_actuator) rather than passive forces + /// (mjData.qfrc_passive). Notionally, this means that gravity compensation is + /// the result of a control system rather than natural buoyancy. In practice, + /// enabling this flag is useful when joint-level actuator force clamping is + /// used. In this case, the total actuation force applied on a joint, + /// including gravity compensation, is guaranteed to not exceed the specified + /// limits. See Force limits and actuatorfrcrange for more details on this + /// type of force limit. + /// + /// | || + /// | -- | -- | + /// | Declaration | `uniform bool mjc:actuatorgravcomp = 0` | + /// | C++ Type | bool | + /// | \ref Usd_Datatypes "Usd Type" | SdfValueTypeNames->Bool | + /// | \ref SdfVariability "Variability" | SdfVariabilityUniform | + MJCPHYSICS_API + UsdAttribute GetMjcActuatorgravcompAttr() const; + + /// See GetMjcActuatorgravcompAttr(), and also + /// \ref Usd_Create_Or_Get_Property for when to use Get vs Create. + /// If specified, author \p defaultValue as the attribute's default, + /// sparsely (when it makes sense to do so) if \p writeSparsely is \c true - + /// the default for \p writeSparsely is \c false. + MJCPHYSICS_API + UsdAttribute CreateMjcActuatorgravcompAttr( + VtValue const &defaultValue = VtValue(), + bool writeSparsely = false) const; + + public: + // --------------------------------------------------------------------- // + // MJCMARGIN + // --------------------------------------------------------------------- // + /// The distance threshold below which limits become active. Recall that the + /// Constraint solver normally generates forces as soon as a constraint + /// becomes active, even if the margin parameter makes that happen at a + /// distance. This attribute together with solreflimit and solimplimit can be + /// used to model a soft joint limit. + /// + /// | || + /// | -- | -- | + /// | Declaration | `uniform double mjc:margin = 0` | + /// | C++ Type | double | + /// | \ref Usd_Datatypes "Usd Type" | SdfValueTypeNames->Double | + /// | \ref SdfVariability "Variability" | SdfVariabilityUniform | + MJCPHYSICS_API + UsdAttribute GetMjcMarginAttr() const; + + /// See GetMjcMarginAttr(), and also + /// \ref Usd_Create_Or_Get_Property for when to use Get vs Create. + /// If specified, author \p defaultValue as the attribute's default, + /// sparsely (when it makes sense to do so) if \p writeSparsely is \c true - + /// the default for \p writeSparsely is \c false. + MJCPHYSICS_API + UsdAttribute CreateMjcMarginAttr(VtValue const &defaultValue = VtValue(), + bool writeSparsely = false) const; + + public: + // --------------------------------------------------------------------- // + // MJCREF + // --------------------------------------------------------------------- // + /// The reference position or angle of the joint. This attribute is only used + /// for slide and hinge joints. It defines the joint value corresponding to + /// the initial model configuration. The amount of spatial transformation that + /// the joint applies at runtime equals the current joint value stored in + /// mjData.qpos minus this reference value stored in mjModel.qpos0. The + /// meaning of these vectors was discussed in the Stand-alone section in the + /// Overview chapter. + /// + /// | || + /// | -- | -- | + /// | Declaration | `uniform double mjc:ref = 0` | + /// | C++ Type | double | + /// | \ref Usd_Datatypes "Usd Type" | SdfValueTypeNames->Double | + /// | \ref SdfVariability "Variability" | SdfVariabilityUniform | + MJCPHYSICS_API + UsdAttribute GetMjcRefAttr() const; + + /// See GetMjcRefAttr(), and also + /// \ref Usd_Create_Or_Get_Property for when to use Get vs Create. + /// If specified, author \p defaultValue as the attribute's default, + /// sparsely (when it makes sense to do so) if \p writeSparsely is \c true - + /// the default for \p writeSparsely is \c false. + MJCPHYSICS_API + UsdAttribute CreateMjcRefAttr(VtValue const &defaultValue = VtValue(), + bool writeSparsely = false) const; + + public: + // --------------------------------------------------------------------- // + // MJCSPRINGREF + // --------------------------------------------------------------------- // + /// The joint position or angle in which the joint spring (if any) achieves + /// equilibrium. Similar to the vector mjModel.qpos0 which stores all joint + /// reference values specified with the ref attribute above, all spring + /// reference values specified with this attribute are stored in the vector + /// mjModel.qpos_spring. The model configuration corresponding to + /// mjModel.qpos_spring is also used to compute the spring reference lengths + /// of all tendons, stored in mjModel.tendon_lengthspring. This is because + /// tendons can also have springs. + /// + /// | || + /// | -- | -- | + /// | Declaration | `uniform double mjc:springref = 0` | + /// | C++ Type | double | + /// | \ref Usd_Datatypes "Usd Type" | SdfValueTypeNames->Double | + /// | \ref SdfVariability "Variability" | SdfVariabilityUniform | + MJCPHYSICS_API + UsdAttribute GetMjcSpringrefAttr() const; + + /// See GetMjcSpringrefAttr(), and also + /// \ref Usd_Create_Or_Get_Property for when to use Get vs Create. + /// If specified, author \p defaultValue as the attribute's default, + /// sparsely (when it makes sense to do so) if \p writeSparsely is \c true - + /// the default for \p writeSparsely is \c false. + MJCPHYSICS_API + UsdAttribute CreateMjcSpringrefAttr(VtValue const &defaultValue = VtValue(), + bool writeSparsely = false) const; + + public: + // --------------------------------------------------------------------- // + // MJCARMATURE + // --------------------------------------------------------------------- // + /// Additional inertia associated with movement of the joint that is not due + /// to body mass. This added inertia is usually due to a rotor (a.k.a + /// armature) spinning faster than the joint itself due to a geared + /// transmission. The value applies to all degrees of freedom created by this + /// joint. Besides increasing the realism of joints with geared transmission, + /// positive armature significantly improves simulation stability, even for + /// small values, and is a recommended possible fix when encountering + /// stability issues. + /// + /// | || + /// | -- | -- | + /// | Declaration | `uniform double mjc:armature = 0` | + /// | C++ Type | double | + /// | \ref Usd_Datatypes "Usd Type" | SdfValueTypeNames->Double | + /// | \ref SdfVariability "Variability" | SdfVariabilityUniform | + MJCPHYSICS_API + UsdAttribute GetMjcArmatureAttr() const; + + /// See GetMjcArmatureAttr(), and also + /// \ref Usd_Create_Or_Get_Property for when to use Get vs Create. + /// If specified, author \p defaultValue as the attribute's default, + /// sparsely (when it makes sense to do so) if \p writeSparsely is \c true - + /// the default for \p writeSparsely is \c false. + MJCPHYSICS_API + UsdAttribute CreateMjcArmatureAttr(VtValue const &defaultValue = VtValue(), + bool writeSparsely = false) const; + + public: + // --------------------------------------------------------------------- // + // MJCDAMPING + // --------------------------------------------------------------------- // + /// Damping applied to all degrees of freedom created by this joint. Unlike + /// friction loss which is computed by the constraint solver, damping is + /// simply a force linear in velocity. It is included in the passive forces. + /// Despite this simplicity, larger damping values can make numerical + /// integrators unstable, which is why our Euler integrator handles damping + /// implicitly. See Integration in the Computation chapter. + /// + /// | || + /// | -- | -- | + /// | Declaration | `uniform double mjc:damping = 0` | + /// | C++ Type | double | + /// | \ref Usd_Datatypes "Usd Type" | SdfValueTypeNames->Double | + /// | \ref SdfVariability "Variability" | SdfVariabilityUniform | + MJCPHYSICS_API + UsdAttribute GetMjcDampingAttr() const; + + /// See GetMjcDampingAttr(), and also + /// \ref Usd_Create_Or_Get_Property for when to use Get vs Create. + /// If specified, author \p defaultValue as the attribute's default, + /// sparsely (when it makes sense to do so) if \p writeSparsely is \c true - + /// the default for \p writeSparsely is \c false. + MJCPHYSICS_API + UsdAttribute CreateMjcDampingAttr(VtValue const &defaultValue = VtValue(), + bool writeSparsely = false) const; + + public: + // --------------------------------------------------------------------- // + // MJCFRICTIONLOSS + // --------------------------------------------------------------------- // + /// Friction loss due to dry friction. This value is the same for all degrees + /// of freedom created by this joint. Semantically friction loss does not make + /// sense for free joints, but the compiler allows it. To enable friction + /// loss, set this attribute to a positive value. + /// + /// | || + /// | -- | -- | + /// | Declaration | `uniform double mjc:frictionloss = 0` | + /// | C++ Type | double | + /// | \ref Usd_Datatypes "Usd Type" | SdfValueTypeNames->Double | + /// | \ref SdfVariability "Variability" | SdfVariabilityUniform | + MJCPHYSICS_API + UsdAttribute GetMjcFrictionlossAttr() const; + + /// See GetMjcFrictionlossAttr(), and also + /// \ref Usd_Create_Or_Get_Property for when to use Get vs Create. + /// If specified, author \p defaultValue as the attribute's default, + /// sparsely (when it makes sense to do so) if \p writeSparsely is \c true - + /// the default for \p writeSparsely is \c false. + MJCPHYSICS_API + UsdAttribute CreateMjcFrictionlossAttr( + VtValue const &defaultValue = VtValue(), + bool writeSparsely = false) const; + + public: + // ===================================================================== // + // Feel free to add custom code below this line, it will be preserved by + // the code generator. + // + // Just remember to: + // - Close the class declaration with }; + // - Close the namespace with PXR_NAMESPACE_CLOSE_SCOPE + // - Close the include guard with #endif + // ===================================================================== // + // --(BEGIN CUSTOM CODE)-- +}; + +PXR_NAMESPACE_CLOSE_SCOPE + +#endif diff --git a/include/mujoco/experimental/usd/mjcPhysics/keyframe.h b/include/mujoco/experimental/usd/mjcPhysics/keyframe.h new file mode 100644 index 00000000..bf66efb2 --- /dev/null +++ b/include/mujoco/experimental/usd/mjcPhysics/keyframe.h @@ -0,0 +1,292 @@ +// Copyright 2025 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. + +#ifndef MJCPHYSICS_GENERATED_KEYFRAME_H +#define MJCPHYSICS_GENERATED_KEYFRAME_H + +/// \file mjcPhysics/keyframe.h + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +PXR_NAMESPACE_OPEN_SCOPE + +class SdfAssetPath; + +// -------------------------------------------------------------------------- // +// KEYFRAME // +// -------------------------------------------------------------------------- // + +/// \class MjcPhysicsKeyframe +/// +/// Represents time independent keyframe values. +/// +class MjcPhysicsKeyframe : public UsdTyped { + public: + /// Compile time constant representing what kind of schema this class is. + /// + /// \sa UsdSchemaKind + static const UsdSchemaKind schemaKind = UsdSchemaKind::ConcreteTyped; + + /// Construct a MjcPhysicsKeyframe on UsdPrim \p prim . + /// Equivalent to MjcPhysicsKeyframe::Get(prim.GetStage(), prim.GetPath()) + /// for a \em valid \p prim, but will not immediately throw an error for + /// an invalid \p prim + explicit MjcPhysicsKeyframe(const UsdPrim &prim = UsdPrim()) + : UsdTyped(prim) {} + + /// Construct a MjcPhysicsKeyframe on the prim held by \p schemaObj . + /// Should be preferred over MjcPhysicsKeyframe(schemaObj.GetPrim()), + /// as it preserves SchemaBase state. + explicit MjcPhysicsKeyframe(const UsdSchemaBase &schemaObj) + : UsdTyped(schemaObj) {} + + /// Destructor. + MJCPHYSICS_API + virtual ~MjcPhysicsKeyframe(); + + /// Return a vector of names of all pre-declared attributes for this schema + /// class and all its ancestor classes. Does not include attributes that + /// may be authored by custom/extended methods of the schemas involved. + MJCPHYSICS_API + static const TfTokenVector &GetSchemaAttributeNames( + bool includeInherited = true); + + /// Return a MjcPhysicsKeyframe holding the prim adhering to this + /// schema at \p path on \p stage. If no prim exists at \p path on + /// \p stage, or if the prim at that path does not adhere to this schema, + /// return an invalid schema object. This is shorthand for the following: + /// + /// \code + /// MjcPhysicsKeyframe(stage->GetPrimAtPath(path)); + /// \endcode + /// + MJCPHYSICS_API + static MjcPhysicsKeyframe Get(const UsdStagePtr &stage, const SdfPath &path); + + /// Attempt to ensure a \a UsdPrim adhering to this schema at \p path + /// is defined (according to UsdPrim::IsDefined()) on this stage. + /// + /// If a prim adhering to this schema at \p path is already defined on this + /// stage, return that prim. Otherwise author an \a SdfPrimSpec with + /// \a specifier == \a SdfSpecifierDef and this schema's prim type name for + /// the prim at \p path at the current EditTarget. Author \a SdfPrimSpec s + /// with \p specifier == \a SdfSpecifierDef and empty typeName at the + /// current EditTarget for any nonexistent, or existing but not \a Defined + /// ancestors. + /// + /// The given \a path must be an absolute prim path that does not contain + /// any variant selections. + /// + /// If it is impossible to author any of the necessary PrimSpecs, (for + /// example, in case \a path cannot map to the current UsdEditTarget's + /// namespace) issue an error and return an invalid \a UsdPrim. + /// + /// Note that this method may return a defined prim whose typeName does not + /// specify this schema class, in case a stronger typeName opinion overrides + /// the opinion at the current EditTarget. + /// + MJCPHYSICS_API + static MjcPhysicsKeyframe Define(const UsdStagePtr &stage, + const SdfPath &path); + + protected: + /// Returns the kind of schema this class belongs to. + /// + /// \sa UsdSchemaKind + MJCPHYSICS_API + UsdSchemaKind _GetSchemaKind() const override; + + private: + // needs to invoke _GetStaticTfType. + friend class UsdSchemaRegistry; + MJCPHYSICS_API + static const TfType &_GetStaticTfType(); + + static bool _IsTypedSchema(); + + // override SchemaBase virtuals. + MJCPHYSICS_API + const TfType &_GetTfType() const override; + + public: + // --------------------------------------------------------------------- // + // MJCQPOS + // --------------------------------------------------------------------- // + /// Vector of joint positions, copied into mjData.qpos when the simulation + /// state is set to this keyframe. + /// + /// | || + /// | -- | -- | + /// | Declaration | `double[] mjc:qpos` | + /// | C++ Type | VtArray | + /// | \ref Usd_Datatypes "Usd Type" | SdfValueTypeNames->DoubleArray | + MJCPHYSICS_API + UsdAttribute GetMjcQposAttr() const; + + /// See GetMjcQposAttr(), and also + /// \ref Usd_Create_Or_Get_Property for when to use Get vs Create. + /// If specified, author \p defaultValue as the attribute's default, + /// sparsely (when it makes sense to do so) if \p writeSparsely is \c true - + /// the default for \p writeSparsely is \c false. + MJCPHYSICS_API + UsdAttribute CreateMjcQposAttr(VtValue const &defaultValue = VtValue(), + bool writeSparsely = false) const; + + public: + // --------------------------------------------------------------------- // + // MJCQVEL + // --------------------------------------------------------------------- // + /// Vector of joint velocities, copied into mjData.qvel when the simulation + /// state is set to this keyframe. + /// + /// | || + /// | -- | -- | + /// | Declaration | `double[] mjc:qvel` | + /// | C++ Type | VtArray | + /// | \ref Usd_Datatypes "Usd Type" | SdfValueTypeNames->DoubleArray | + MJCPHYSICS_API + UsdAttribute GetMjcQvelAttr() const; + + /// See GetMjcQvelAttr(), and also + /// \ref Usd_Create_Or_Get_Property for when to use Get vs Create. + /// If specified, author \p defaultValue as the attribute's default, + /// sparsely (when it makes sense to do so) if \p writeSparsely is \c true - + /// the default for \p writeSparsely is \c false. + MJCPHYSICS_API + UsdAttribute CreateMjcQvelAttr(VtValue const &defaultValue = VtValue(), + bool writeSparsely = false) const; + + public: + // --------------------------------------------------------------------- // + // MJCACT + // --------------------------------------------------------------------- // + /// Vector of actuator activations, copied into mjData.act when the simulation + /// state is set to this keyframe. + /// + /// | || + /// | -- | -- | + /// | Declaration | `double[] mjc:act` | + /// | C++ Type | VtArray | + /// | \ref Usd_Datatypes "Usd Type" | SdfValueTypeNames->DoubleArray | + MJCPHYSICS_API + UsdAttribute GetMjcActAttr() const; + + /// See GetMjcActAttr(), and also + /// \ref Usd_Create_Or_Get_Property for when to use Get vs Create. + /// If specified, author \p defaultValue as the attribute's default, + /// sparsely (when it makes sense to do so) if \p writeSparsely is \c true - + /// the default for \p writeSparsely is \c false. + MJCPHYSICS_API + UsdAttribute CreateMjcActAttr(VtValue const &defaultValue = VtValue(), + bool writeSparsely = false) const; + + public: + // --------------------------------------------------------------------- // + // MJCCTRL + // --------------------------------------------------------------------- // + /// Vector of controls, copied into mjData.ctrl when the simulation state is + /// set to this keyframe. + /// + /// | || + /// | -- | -- | + /// | Declaration | `double[] mjc:ctrl` | + /// | C++ Type | VtArray | + /// | \ref Usd_Datatypes "Usd Type" | SdfValueTypeNames->DoubleArray | + MJCPHYSICS_API + UsdAttribute GetMjcCtrlAttr() const; + + /// See GetMjcCtrlAttr(), and also + /// \ref Usd_Create_Or_Get_Property for when to use Get vs Create. + /// If specified, author \p defaultValue as the attribute's default, + /// sparsely (when it makes sense to do so) if \p writeSparsely is \c true - + /// the default for \p writeSparsely is \c false. + MJCPHYSICS_API + UsdAttribute CreateMjcCtrlAttr(VtValue const &defaultValue = VtValue(), + bool writeSparsely = false) const; + + public: + // --------------------------------------------------------------------- // + // MJCMPOS + // --------------------------------------------------------------------- // + /// Vector of mocap body positions, copied into mjData.mocap_pos when the + /// simulation state is set to this keyframe. + /// + /// | || + /// | -- | -- | + /// | Declaration | `double[] mjc:mpos` | + /// | C++ Type | VtArray | + /// | \ref Usd_Datatypes "Usd Type" | SdfValueTypeNames->DoubleArray | + MJCPHYSICS_API + UsdAttribute GetMjcMposAttr() const; + + /// See GetMjcMposAttr(), and also + /// \ref Usd_Create_Or_Get_Property for when to use Get vs Create. + /// If specified, author \p defaultValue as the attribute's default, + /// sparsely (when it makes sense to do so) if \p writeSparsely is \c true - + /// the default for \p writeSparsely is \c false. + MJCPHYSICS_API + UsdAttribute CreateMjcMposAttr(VtValue const &defaultValue = VtValue(), + bool writeSparsely = false) const; + + public: + // --------------------------------------------------------------------- // + // MJCMQUAT + // --------------------------------------------------------------------- // + /// Vector of mocap body quaternions, copied into mjData.mocap_quat when the + /// simulation state is set to this keyframe. + /// + /// | || + /// | -- | -- | + /// | Declaration | `double[] mjc:mquat` | + /// | C++ Type | VtArray | + /// | \ref Usd_Datatypes "Usd Type" | SdfValueTypeNames->DoubleArray | + MJCPHYSICS_API + UsdAttribute GetMjcMquatAttr() const; + + /// See GetMjcMquatAttr(), and also + /// \ref Usd_Create_Or_Get_Property for when to use Get vs Create. + /// If specified, author \p defaultValue as the attribute's default, + /// sparsely (when it makes sense to do so) if \p writeSparsely is \c true - + /// the default for \p writeSparsely is \c false. + MJCPHYSICS_API + UsdAttribute CreateMjcMquatAttr(VtValue const &defaultValue = VtValue(), + bool writeSparsely = false) const; + + public: + // ===================================================================== // + // Feel free to add custom code below this line, it will be preserved by + // the code generator. + // + // Just remember to: + // - Close the class declaration with }; + // - Close the namespace with PXR_NAMESPACE_CLOSE_SCOPE + // - Close the include guard with #endif + // ===================================================================== // + // --(BEGIN CUSTOM CODE)-- +}; + +PXR_NAMESPACE_CLOSE_SCOPE + +#endif diff --git a/src/experimental/usd/mjcPhysics/meshCollisionAPI.h b/include/mujoco/experimental/usd/mjcPhysics/meshCollisionAPI.h similarity index 94% rename from src/experimental/usd/mjcPhysics/meshCollisionAPI.h rename to include/mujoco/experimental/usd/mjcPhysics/meshCollisionAPI.h index 55c8c27e..45bc881a 100644 --- a/src/experimental/usd/mjcPhysics/meshCollisionAPI.h +++ b/include/mujoco/experimental/usd/mjcPhysics/meshCollisionAPI.h @@ -17,18 +17,18 @@ /// \file mjcPhysics/meshCollisionAPI.h -#include "./api.h" -#include "./tokens.h" -#include "pxr/base/gf/matrix4d.h" -#include "pxr/base/gf/vec3d.h" -#include "pxr/base/gf/vec3f.h" -#include "pxr/base/tf/token.h" -#include "pxr/base/tf/type.h" -#include "pxr/base/vt/value.h" -#include "pxr/pxr.h" -#include "pxr/usd/usd/apiSchemaBase.h" -#include "pxr/usd/usd/prim.h" -#include "pxr/usd/usd/stage.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include PXR_NAMESPACE_OPEN_SCOPE diff --git a/src/experimental/usd/mjcPhysics/sceneAPI.h b/include/mujoco/experimental/usd/mjcPhysics/sceneAPI.h similarity index 99% rename from src/experimental/usd/mjcPhysics/sceneAPI.h rename to include/mujoco/experimental/usd/mjcPhysics/sceneAPI.h index 2ac745c7..ceae9164 100644 --- a/src/experimental/usd/mjcPhysics/sceneAPI.h +++ b/include/mujoco/experimental/usd/mjcPhysics/sceneAPI.h @@ -17,18 +17,18 @@ /// \file mjcPhysics/sceneAPI.h -#include "./api.h" -#include "./tokens.h" -#include "pxr/base/gf/matrix4d.h" -#include "pxr/base/gf/vec3d.h" -#include "pxr/base/gf/vec3f.h" -#include "pxr/base/tf/token.h" -#include "pxr/base/tf/type.h" -#include "pxr/base/vt/value.h" -#include "pxr/pxr.h" -#include "pxr/usd/usd/apiSchemaBase.h" -#include "pxr/usd/usd/prim.h" -#include "pxr/usd/usd/stage.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include PXR_NAMESPACE_OPEN_SCOPE diff --git a/src/experimental/usd/mjcPhysics/siteAPI.h b/include/mujoco/experimental/usd/mjcPhysics/siteAPI.h similarity index 93% rename from src/experimental/usd/mjcPhysics/siteAPI.h rename to include/mujoco/experimental/usd/mjcPhysics/siteAPI.h index 537345ce..fedfee7d 100644 --- a/src/experimental/usd/mjcPhysics/siteAPI.h +++ b/include/mujoco/experimental/usd/mjcPhysics/siteAPI.h @@ -17,17 +17,17 @@ /// \file mjcPhysics/siteAPI.h -#include "./api.h" -#include "pxr/base/gf/matrix4d.h" -#include "pxr/base/gf/vec3d.h" -#include "pxr/base/gf/vec3f.h" -#include "pxr/base/tf/token.h" -#include "pxr/base/tf/type.h" -#include "pxr/base/vt/value.h" -#include "pxr/pxr.h" -#include "pxr/usd/usd/apiSchemaBase.h" -#include "pxr/usd/usd/prim.h" -#include "pxr/usd/usd/stage.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include PXR_NAMESPACE_OPEN_SCOPE diff --git a/src/experimental/usd/mjcPhysics/tokens.h b/include/mujoco/experimental/usd/mjcPhysics/tokens.h similarity index 82% rename from src/experimental/usd/mjcPhysics/tokens.h rename to include/mujoco/experimental/usd/mjcPhysics/tokens.h index 862d2ef8..c5de4644 100644 --- a/src/experimental/usd/mjcPhysics/tokens.h +++ b/include/mujoco/experimental/usd/mjcPhysics/tokens.h @@ -26,10 +26,10 @@ #include -#include "./api.h" -#include "pxr/base/tf/staticData.h" -#include "pxr/base/tf/token.h" -#include "pxr/pxr.h" +#include +#include +#include +#include PXR_NAMESPACE_OPEN_SCOPE @@ -63,7 +63,8 @@ struct MjcPhysicsTokensType { /// Fallback value for MjcPhysicsSceneAPI::GetJacobianAttr(), Fallback value /// for MjcPhysicsActuatorAPI::GetMjcActLimitedAttr(), Fallback value for /// MjcPhysicsActuatorAPI::GetMjcCtrlLimitedAttr(), Fallback value for - /// MjcPhysicsActuatorAPI::GetMjcForceLimitedAttr(), This token represents + /// MjcPhysicsActuatorAPI::GetMjcForceLimitedAttr(), Fallback value for + /// MjcPhysicsJointAPI::GetMjcActuatorfrclimitedAttr(), This token represents /// the auto constraint Jacobian and matrices computed from it. const TfToken auto_; /// \brief "cg" @@ -98,7 +99,8 @@ struct MjcPhysicsTokensType { /// /// Possible value for MjcPhysicsActuatorAPI::GetMjcActLimitedAttr(), Possible /// value for MjcPhysicsActuatorAPI::GetMjcCtrlLimitedAttr(), Possible value - /// for MjcPhysicsActuatorAPI::GetMjcForceLimitedAttr() + /// for MjcPhysicsActuatorAPI::GetMjcForceLimitedAttr(), Possible value for + /// MjcPhysicsJointAPI::GetMjcActuatorfrclimitedAttr() const TfToken false_; /// \brief "filter" /// @@ -130,6 +132,10 @@ struct MjcPhysicsTokensType { /// /// Fallback value for MjcPhysicsMeshCollisionAPI::GetInertiaAttr() const TfToken legacy; + /// \brief "mjc:act" + /// + /// MjcPhysicsKeyframe + const TfToken mjcAct; /// \brief "mjc:actDim" /// /// MjcPhysicsActuatorAPI @@ -150,6 +156,26 @@ struct MjcPhysicsTokensType { /// /// MjcPhysicsActuatorAPI const TfToken mjcActRangeMin; + /// \brief "mjc:actuatorfrclimited" + /// + /// MjcPhysicsJointAPI + const TfToken mjcActuatorfrclimited; + /// \brief "mjc:actuatorfrcrange:max" + /// + /// MjcPhysicsJointAPI + const TfToken mjcActuatorfrcrangeMax; + /// \brief "mjc:actuatorfrcrange:min" + /// + /// MjcPhysicsJointAPI + const TfToken mjcActuatorfrcrangeMin; + /// \brief "mjc:actuatorgravcomp" + /// + /// MjcPhysicsJointAPI + const TfToken mjcActuatorgravcomp; + /// \brief "mjc:armature" + /// + /// MjcPhysicsJointAPI + const TfToken mjcArmature; /// \brief "mjc:biasPrm" /// /// MjcPhysicsActuatorAPI @@ -162,10 +188,10 @@ struct MjcPhysicsTokensType { /// /// MjcPhysicsActuatorAPI const TfToken mjcCrankLength; - /// \brief "mjc:crankSite" + /// \brief "mjc:ctrl" /// - /// MjcPhysicsActuatorAPI - const TfToken mjcCrankSite; + /// MjcPhysicsKeyframe + const TfToken mjcCtrl; /// \brief "mjc:ctrlLimited" /// /// MjcPhysicsActuatorAPI @@ -178,6 +204,10 @@ struct MjcPhysicsTokensType { /// /// MjcPhysicsActuatorAPI const TfToken mjcCtrlRangeMin; + /// \brief "mjc:damping" + /// + /// MjcPhysicsJointAPI + const TfToken mjcDamping; /// \brief "mjc:dynPrm" /// /// MjcPhysicsActuatorAPI @@ -290,6 +320,10 @@ struct MjcPhysicsTokensType { /// /// MjcPhysicsActuatorAPI const TfToken mjcForceRangeMin; + /// \brief "mjc:frictionloss" + /// + /// MjcPhysicsJointAPI + const TfToken mjcFrictionloss; /// \brief "mjc:gainPrm" /// /// MjcPhysicsActuatorAPI @@ -318,6 +352,18 @@ struct MjcPhysicsTokensType { /// /// MjcPhysicsActuatorAPI const TfToken mjcLengthRangeMin; + /// \brief "mjc:margin" + /// + /// MjcPhysicsJointAPI + const TfToken mjcMargin; + /// \brief "mjc:mpos" + /// + /// MjcPhysicsKeyframe + const TfToken mjcMpos; + /// \brief "mjc:mquat" + /// + /// MjcPhysicsKeyframe + const TfToken mjcMquat; /// \brief "mjc:option:actuatorgroupdisable" /// /// MjcPhysicsSceneAPI @@ -422,6 +468,18 @@ struct MjcPhysicsTokensType { /// /// MjcPhysicsSceneAPI const TfToken mjcOptionWind; + /// \brief "mjc:qpos" + /// + /// MjcPhysicsKeyframe + const TfToken mjcQpos; + /// \brief "mjc:qvel" + /// + /// MjcPhysicsKeyframe + const TfToken mjcQvel; + /// \brief "mjc:ref" + /// + /// MjcPhysicsJointAPI + const TfToken mjcRef; /// \brief "mjc:refSite" /// /// MjcPhysicsActuatorAPI @@ -434,6 +492,34 @@ struct MjcPhysicsTokensType { /// /// MjcPhysicsActuatorAPI const TfToken mjcSliderSite; + /// \brief "mjc:solimpfriction" + /// + /// MjcPhysicsJointAPI + const TfToken mjcSolimpfriction; + /// \brief "mjc:solimplimit" + /// + /// MjcPhysicsJointAPI + const TfToken mjcSolimplimit; + /// \brief "mjc:solreffriction" + /// + /// MjcPhysicsJointAPI + const TfToken mjcSolreffriction; + /// \brief "mjc:solreflimit" + /// + /// MjcPhysicsJointAPI + const TfToken mjcSolreflimit; + /// \brief "mjc:springdamper" + /// + /// MjcPhysicsJointAPI + const TfToken mjcSpringdamper; + /// \brief "mjc:springref" + /// + /// MjcPhysicsJointAPI + const TfToken mjcSpringref; + /// \brief "mjc:stiffness" + /// + /// MjcPhysicsJointAPI + const TfToken mjcStiffness; /// \brief "muscle" /// /// Possible value for MjcPhysicsActuatorAPI::GetMjcBiasTypeAttr(), Possible @@ -478,7 +564,8 @@ struct MjcPhysicsTokensType { /// /// Possible value for MjcPhysicsActuatorAPI::GetMjcActLimitedAttr(), Possible /// value for MjcPhysicsActuatorAPI::GetMjcCtrlLimitedAttr(), Possible value - /// for MjcPhysicsActuatorAPI::GetMjcForceLimitedAttr() + /// for MjcPhysicsActuatorAPI::GetMjcForceLimitedAttr(), Possible value for + /// MjcPhysicsJointAPI::GetMjcActuatorfrclimitedAttr() const TfToken true_; /// \brief "user" /// @@ -488,23 +575,31 @@ struct MjcPhysicsTokensType { const TfToken user; /// \brief "CollisionAPI" /// - /// Schema identifer and family for MjcPhysicsCollisionAPI + /// Schema identifier and family for MjcPhysicsCollisionAPI const TfToken CollisionAPI; + /// \brief "Keyframe" + /// + /// Schema identifier and family for MjcPhysicsKeyframe + const TfToken Keyframe; /// \brief "MeshCollisionAPI" /// - /// Schema identifer and family for MjcPhysicsMeshCollisionAPI + /// Schema identifier and family for MjcPhysicsMeshCollisionAPI const TfToken MeshCollisionAPI; /// \brief "PhysicsActuatorAPI" /// - /// Schema identifer and family for MjcPhysicsActuatorAPI + /// Schema identifier and family for MjcPhysicsActuatorAPI const TfToken PhysicsActuatorAPI; + /// \brief "PhysicsJointsAPI" + /// + /// Schema identifier and family for MjcPhysicsJointAPI + const TfToken PhysicsJointsAPI; /// \brief "SceneAPI" /// - /// Schema identifer and family for MjcPhysicsSceneAPI + /// Schema identifier and family for MjcPhysicsSceneAPI const TfToken SceneAPI; /// \brief "SiteAPI" /// - /// Schema identifer and family for MjcPhysicsSiteAPI + /// Schema identifier and family for MjcPhysicsSiteAPI const TfToken SiteAPI; /// A vector of all of the tokens listed above. const std::vector allTokens; diff --git a/include/mujoco/experimental/usd/usd.h b/include/mujoco/experimental/usd/usd.h new file mode 100644 index 00000000..f75b70ad --- /dev/null +++ b/include/mujoco/experimental/usd/usd.h @@ -0,0 +1,28 @@ +// Copyright 2025 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. + +#ifndef MUJOCO_EXPERIMENTAL_SRC_USD_USD_TO_MJSPEC_H_ +#define MUJOCO_EXPERIMENTAL_SRC_USD_USD_TO_MJSPEC_H_ + +#include +#include + +// Given a USD stage, this function will do a best effort conversion to +// mjSpec. +// +// Particular care is taken for physics data to be lossless but visual +// data such as materials may be lossy. +MJAPI mjSpec* mj_parseUSDStage(pxr::UsdStageRefPtr stage); + +#endif // MUJOCO_EXPERIMENTAL_SRC_USD_USD_TO_MJSPEC_H_ diff --git a/src/experimental/usd/utils.h b/include/mujoco/experimental/usd/utils.h similarity index 100% rename from src/experimental/usd/utils.h rename to include/mujoco/experimental/usd/utils.h diff --git a/include/mujoco/experimental/usd/writer.h b/include/mujoco/experimental/usd/writer.h new file mode 100644 index 00000000..f3d191d1 --- /dev/null +++ b/include/mujoco/experimental/usd/writer.h @@ -0,0 +1,52 @@ +// Copyright 2025 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. + +#ifndef MUJOCO_SRC_EXPERIMENTAL_USD_WRITER_H_ +#define MUJOCO_SRC_EXPERIMENTAL_USD_WRITER_H_ + +#include +#include + +#include +#include +#include + +namespace mujoco { +namespace usd { +using PoseSinkFn = + std::function)>; + +class Writer { + public: + Writer(pxr::UsdStageRefPtr stage, mjSpec* spec, mjModel_* model); + ~Writer(); + Writer(const Writer&) = delete; + Writer& operator=(const Writer&) = delete; + + void Update(const mjData* const data); + void AddSink(PoseSinkFn sink_fn); + + private: + void BuildMjUsdMapping(); + + pxr::UsdStageRefPtr stage_; + std::vector body_id_to_path_; + mjSpec* spec_ = nullptr; + mjModel_* model_ = nullptr; + std::vector pose_sinks_; +}; +} // namespace usd +} // namespace mujoco + +#endif // MUJOCO_SRC_EXPERIMENTAL_USD_WRITER_H_ diff --git a/include/mujoco/mjmodel.h b/include/mujoco/mjmodel.h index 75eb3cd0..0ceb9d53 100644 --- a/include/mujoco/mjmodel.h +++ b/include/mujoco/mjmodel.h @@ -452,8 +452,8 @@ typedef struct mjLROpt_ mjLROpt; //---------------------------------- mjVFS --------------------------------------------------------- -struct mjVFS_ { // virtual file system for loading from memory - void* impl_; // internal pointer to VFS memory +struct mjVFS_ { // virtual file system for loading from memory + void* impl_; // internal pointer to VFS memory }; typedef struct mjVFS_ mjVFS; @@ -508,7 +508,8 @@ typedef struct mjOption_ mjOption; struct mjVisual_ { // visualization options struct { // global parameters - int orthographic; // is the free camera orthographic (0: no, 1: yes) + int cameraid; // initial camera id (-1: free) + int orthographic; // is the free camera orthographic (0: no, 1: yes) float fovy; // y field-of-view of free camera (orthographic ? length : degree) float ipd; // inter-pupilary distance for free camera float azimuth; // initial azimuth of free camera (degrees) @@ -630,6 +631,7 @@ struct mjModel_ { int nbvh; // number of total bounding volumes in all bodies int nbvhstatic; // number of static bounding volumes (aabb stored in mjModel) int nbvhdynamic; // number of dynamic bounding volumes (aabb stored in mjData) + int noct; // number of total octree cells in all meshes int njnt; // number of joints int ngeom; // number of geoms int nsite; // number of sites @@ -763,6 +765,11 @@ struct mjModel_ { int* bvh_nodeid; // geom or elem id of node; -1: non-leaf (nbvh x 1) mjtNum* bvh_aabb; // local bounding box (center, size) (nbvhstatic x 6) + // octree spatial partitioning + int* oct_depth; // depth in the octree (noct x 1) + int* oct_child; // children of octree node (noct x 8) + mjtNum* oct_aabb; // octree node bounding box (center, size) (noct x 6) + // joints int* jnt_type; // type of joint (mjtJoint) (njnt x 1) int* jnt_qposadr; // start addr in 'qpos' for joint's data (njnt x 1) @@ -950,6 +957,8 @@ struct mjModel_ { int* mesh_facenum; // number of faces (nmesh x 1) int* mesh_bvhadr; // address of bvh root (nmesh x 1) int* mesh_bvhnum; // number of bvh (nmesh x 1) + int* mesh_octadr; // address of octree root (nmesh x 1) + int* mesh_octnum; // number of octree nodes (nmesh x 1) int* mesh_normaladr; // first normal address (nmesh x 1) int* mesh_normalnum; // number of normals (nmesh x 1) int* mesh_texcoordadr; // texcoord data address; -1: no texcoord (nmesh x 1) diff --git a/include/mujoco/mjvisualize.h b/include/mujoco/mjvisualize.h index efff4d03..2c7fd535 100644 --- a/include/mujoco/mjvisualize.h +++ b/include/mujoco/mjvisualize.h @@ -292,6 +292,7 @@ struct mjvOption_ { // abstract visualization options mjtByte skingroup[mjNGROUP]; // skin visualization by group mjtByte flags[mjNVISFLAG]; // visualization flags (indexed by mjtVisFlag) int bvh_depth; // depth of the bounding volume hierarchy to be visualized + int oct_depth; // depth of the octree to be visualized int flex_layer; // element layer to be visualized for 3D flex }; typedef struct mjvOption_ mjvOption; diff --git a/include/mujoco/mjxmacro.h b/include/mujoco/mjxmacro.h index 93542a88..3da0df54 100644 --- a/include/mujoco/mjxmacro.h +++ b/include/mujoco/mjxmacro.h @@ -75,6 +75,7 @@ X( nbvh ) \ X( nbvhstatic ) \ X( nbvhdynamic ) \ + X( noct ) \ X( njnt ) \ X( ngeom ) \ X( nsite ) \ @@ -216,6 +217,9 @@ X ( int, bvh_child, nbvh, 2 ) \ X ( int, bvh_nodeid, nbvh, 1 ) \ X ( mjtNum, bvh_aabb, nbvhstatic, 6 ) \ + X ( int, oct_depth, noct, 1 ) \ + X ( int, oct_child, noct, 8 ) \ + X ( mjtNum, oct_aabb, noct, 6 ) \ X ( int, jnt_type, njnt, 1 ) \ X ( int, jnt_qposadr, njnt, 1 ) \ X ( int, jnt_dofadr, njnt, 1 ) \ @@ -390,6 +394,8 @@ X ( int, mesh_facenum, nmesh, 1 ) \ X ( int, mesh_bvhadr, nmesh, 1 ) \ X ( int, mesh_bvhnum, nmesh, 1 ) \ + X ( int, mesh_octadr, nmesh, 1 ) \ + X ( int, mesh_octnum, nmesh, 1 ) \ X ( int, mesh_graphadr, nmesh, 1 ) \ X ( mjtNum, mesh_scale, nmesh, 3 ) \ X ( mjtNum, mesh_pos, nmesh, 3 ) \ diff --git a/include/mujoco/mujoco.h b/include/mujoco/mujoco.h index 59283756..1ad172a7 100644 --- a/include/mujoco/mujoco.h +++ b/include/mujoco/mujoco.h @@ -16,7 +16,7 @@ #define MUJOCO_MUJOCO_H_ // header version; should match the library version as returned by mj_version() -#define mjVERSION_HEADER 333 +#define mjVERSION_HEADER 334 // needed to define size_t, fabs and log10 #include @@ -1411,11 +1411,6 @@ MJAPI void mju_taskJoin(mjTask* task); MJAPI mjsElement* mjs_attach(mjsElement* parent, const mjsElement* child, const char* prefix, const char* suffix); -// Delete body and descendants from mjSpec, remove all references, return 0 on success. -MJAPI int mjs_detachBody(mjSpec* s, mjsBody* b); - -// Delete default class and descendants from mjSpec, remove all references, return 0 on success. -MJAPI int mjs_detachDefault(mjSpec* s, mjsDefault* d); //---------------------------------- Tree elements ------------------------------------------------- @@ -1443,8 +1438,8 @@ MJAPI mjsLight* mjs_addLight(mjsBody* body, const mjsDefault* def); // Add frame to body. MJAPI mjsFrame* mjs_addFrame(mjsBody* body, mjsFrame* parentframe); -// Delete object corresponding to the given element, return 0 on success. -MJAPI int mjs_delete(mjsElement* element); +// Remove object corresponding to the given element, return 0 on success. +MJAPI int mjs_delete(mjSpec* spec, mjsElement* element); //---------------------------------- Non-tree elements --------------------------------------------- @@ -1501,6 +1496,38 @@ MJAPI mjsPlugin* mjs_addPlugin(mjSpec* s); MJAPI mjsDefault* mjs_addDefault(mjSpec* s, const char* classname, const mjsDefault* parent); +//---------------------------------- Set actuator parameters --------------------------------------- + +// Set actuator to motor, return error if any. +MJAPI const char* mjs_setToMotor(mjsActuator* actuator); + +// Set actuator to position, return error if any. +MJAPI const char* mjs_setToPosition(mjsActuator* actuator, double kp, double kv[1], + double dampratio[1], double timeconst[1], double inheritrange); + +// Set actuator to integrated velocity, return error if any. +MJAPI const char* mjs_setToIntVelocity(mjsActuator* actuator, double kp, double kv[1], + double dampratio[1], double timeconst[1], double inheritrange); + +// Set actuator to velocity servo, return error if any. +MJAPI const char* mjs_setToVelocity(mjsActuator* actuator, double kv); + +// Set actuator to activate damper, return error if any. +MJAPI const char* mjs_setToDamper(mjsActuator* actuator, double kv); + +// Set actuator to hydraulic or pneumatic cylinder, return error if any. +MJAPI const char* mjs_setToCylinder(mjsActuator* actuator, double timeconst, + double bias, double area, double diameter); + +// Set actuator to muscle, return error if any.a +MJAPI const char* mjs_setToMuscle(mjsActuator* actuator, double timeconst[2], double tausmooth, + double range[2], double force, double scale, double lmin, + double lmax, double vmax, double fpmax, double fvmax); + +// Set actuator to active adhesion, return error if any. +MJAPI const char* mjs_setToAdhesion(mjsActuator* actuator, double gain); + + //---------------------------------- Assets -------------------------------------------------------- // Add mesh. diff --git a/mjx/mujoco/mjx/__init__.py b/mjx/mujoco/mjx/__init__.py index c60c785c..d71d6a24 100644 --- a/mjx/mujoco/mjx/__init__.py +++ b/mjx/mujoco/mjx/__init__.py @@ -48,6 +48,8 @@ from mujoco.mjx._src.smooth import rne from mujoco.mjx._src.smooth import rne_postconstraint from mujoco.mjx._src.smooth import subtree_vel from mujoco.mjx._src.smooth import tendon +from mujoco.mjx._src.smooth import tendon_armature +from mujoco.mjx._src.smooth import tendon_bias from mujoco.mjx._src.smooth import transmission from mujoco.mjx._src.solver import solve from mujoco.mjx._src.support import apply_ft diff --git a/mjx/mujoco/mjx/_src/forward.py b/mjx/mujoco/mjx/_src/forward.py index 62535122..d8faf293 100644 --- a/mjx/mujoco/mjx/_src/forward.py +++ b/mjx/mujoco/mjx/_src/forward.py @@ -73,6 +73,7 @@ def fwd_position(m: Model, d: Data) -> Data: d = smooth.camlight(m, d) d = smooth.tendon(m, d) d = smooth.crb(m, d) + d = smooth.tendon_armature(m, d) d = smooth.factor_m(m, d) d = collision_driver.collision(m, d) d = constraint.make_constraint(m, d) @@ -93,6 +94,7 @@ def fwd_velocity(m: Model, d: Data) -> Data: d = smooth.com_vel(m, d) d = passive.passive(m, d) d = smooth.rne(m, d) + d = smooth.tendon_bias(m, d) return d diff --git a/mjx/mujoco/mjx/_src/inverse.py b/mjx/mujoco/mjx/_src/inverse.py index 5df72de9..086dd101 100644 --- a/mjx/mujoco/mjx/_src/inverse.py +++ b/mjx/mujoco/mjx/_src/inverse.py @@ -93,11 +93,15 @@ def inverse(m: Model, d: Data) -> Data: d = discrete_acc(m, d) d = inv_constraint(m, d) - d = smooth.rne(m, d, flg_acc=True) + d = smooth.rne(m, d) + d = smooth.tendon_bias(m, d) d = sensor.sensor_acc(m, d) qfrc_inverse = ( - d.qfrc_bias + m.dof_armature * d.qacc - d.qfrc_passive - d.qfrc_constraint + d.qfrc_bias + + support.mul_m(m, d, d.qacc) + - d.qfrc_passive + - d.qfrc_constraint ) if m.opt.enableflags & EnableBit.INVDISCRETE: diff --git a/mjx/mujoco/mjx/_src/inverse_test.py b/mjx/mujoco/mjx/_src/inverse_test.py index 227406e3..63720a15 100644 --- a/mjx/mujoco/mjx/_src/inverse_test.py +++ b/mjx/mujoco/mjx/_src/inverse_test.py @@ -19,6 +19,7 @@ from jax import numpy as jp import mujoco from mujoco import mjx from mujoco.mjx._src import support +from mujoco.mjx._src import test_util import numpy as np # tolerance for difference between MuJoCo and MJX calculations - mostly @@ -109,6 +110,41 @@ class InverseTest(parameterized.TestCase): self.assertLess(fwdinv1, 1.0e-3) _assert_eq(dxinv.qacc, dx.qacc, 'qacc') + def test_inverse_tendon_armature(self): + m = test_util.load_test_file('tendon/armature.xml') + + d = mujoco.MjData(m) + d.qvel = np.random.uniform(low=-0.01, high=0.01, size=d.qvel.shape) + d.ctrl = np.random.uniform(low=-0.01, high=0.01, size=d.ctrl.shape) + d.qfrc_applied = np.random.uniform( + low=-0.01, high=0.01, size=d.qfrc_applied.shape + ) + d.xfrc_applied = np.random.uniform( + low=-0.01, high=0.01, size=d.xfrc_applied.shape + ) + mujoco.mj_step(m, d, 10) + + mx = mjx.put_model(m) + dx = mjx.put_data(m, d) + + dx = mjx.forward(mx, dx) + dxinv = mjx.inverse(mx, dx) + + fwdinv0 = jp.linalg.norm( + dxinv.qfrc_constraint - dx.qfrc_constraint, ord=np.inf + ) + fwdinv1 = jp.linalg.norm( + dxinv.qfrc_inverse + - ( + dx.qfrc_applied + dx.qfrc_actuator + support.xfrc_accumulate(mx, dx) + ), + ord=np.inf, + ) + + self.assertLess(fwdinv0, 1.0e-3) + self.assertLess(fwdinv1, 1.0e-3) + _assert_eq(dxinv.qacc, dx.qacc, 'qacc') + if __name__ == '__main__': absltest.main() diff --git a/mjx/mujoco/mjx/_src/io.py b/mjx/mujoco/mjx/_src/io.py index f8aaf565..ae747e85 100644 --- a/mjx/mujoco/mjx/_src/io.py +++ b/mjx/mujoco/mjx/_src/io.py @@ -41,51 +41,51 @@ def _is_cuda_gpu_device(device: jax.Device) -> bool: return device in cuda_devices -def _resolve_backend_impl( +def _resolve_impl( device: jax.Device, -) -> types.BackendImpl: - """Pick a default backend impl based on the device specified.""" +) -> types.Impl: + """Pick a default implementation based on the device specified.""" if _is_cuda_gpu_device(device): # TODO(btaba): Remove flag once Warp is ready to launch. mjx_warp_enabled = os.environ.get('MJX_WARP_ENABLED', 'f').lower() == 'true' if mjx_warp_enabled: - logging.debug('Picking default backend implementation: Warp.') - return types.BackendImpl.WARP + logging.debug('Picking default implementation: Warp.') + return types.Impl.WARP logging.info('MJX Warp is disabled via MJX_WARP_ENABLED=false.') if device.platform in ('gpu', 'tpu'): - logging.debug('Picking default backend implementation: JAX.') - return types.BackendImpl.JAX + logging.debug('Picking default implementation: JAX.') + return types.Impl.JAX if device.platform == 'cpu': mjx_c_default = ( os.environ.get('MJX_C_DEFAULT_ENABLED', 'f').lower() == 'true' ) if mjx_c_default: - logging.debug('Picking default backend implementation: C.') - return types.BackendImpl.C - return types.BackendImpl.JAX + logging.debug('Picking default implementation: C.') + return types.Impl.C + return types.Impl.JAX raise ValueError(f'Unsupported device: {device}') def _resolve_device( - backend_impl: types.BackendImpl, + impl: types.Impl, ) -> jax.Device: - """Resolves a device based on the backend implementation.""" - backend_impl = types.BackendImpl(backend_impl) - if backend_impl == types.BackendImpl.JAX: + """Resolves a device based on the implementation.""" + impl = types.Impl(impl) + if impl == types.Impl.JAX: device_0 = jax.devices()[0] logging.debug('Picking default device: %s.', device_0) return device_0 - if backend_impl == types.BackendImpl.C: + if impl == types.Impl.C: cpu_0 = jax.devices('cpu')[0] logging.debug('Picking default device: %s', cpu_0) return cpu_0 - if backend_impl == types.BackendImpl.WARP: - # WARP backend requires a CUDA GPU. + if impl == types.Impl.WARP: + # WARP implementation requires a CUDA GPU. cuda_gpus = [d for d in jax.devices('cuda')] if not cuda_gpus: raise AssertionError( @@ -96,64 +96,64 @@ def _resolve_device( logging.debug('Picking default device: %s', cuda_gpus[0]) return cuda_gpus[0] - raise ValueError(f'Unsupported backend implementation: {backend_impl}') + raise ValueError(f'Unsupported implementation: {impl}') -def _check_backend_impl_device_compatibility( - backend_impl: Union[str, types.BackendImpl], +def _check_impl_device_compatibility( + impl: Union[str, types.Impl], device: jax.Device, ) -> None: - """Checks that the backend implementation is compatible with the device.""" - if backend_impl is None: - raise ValueError('No backend implementation specified.') + """Checks that the implementation is compatible with the device.""" + if impl is None: + raise ValueError('No implementation specified.') - backend_impl = types.BackendImpl(backend_impl) + impl = types.Impl(impl) - if backend_impl == types.BackendImpl.WARP: + if impl == types.Impl.WARP: if not _is_cuda_gpu_device(device): raise AssertionError( - 'Warp backend implementation requires a CUDA GPU device, got ' + 'Warp implementation requires a CUDA GPU device, got ' f'{device}.' ) mjx_warp_enabled = os.environ.get('MJX_WARP_ENABLED', 'f').lower() == 'true' if not mjx_warp_enabled: raise AssertionError( - 'Warp backend implementation is disabled via MJX_WARP_ENABLED=false.' + 'Warp implementation is disabled via MJX_WARP_ENABLED=false.' ) is_cpu_device = device.platform == 'cpu' - if backend_impl == types.BackendImpl.C: + if impl == types.Impl.C: if not is_cpu_device: raise AssertionError( - f'C backend implementation requires a CPU device, got {device}.' + f'C implementation requires a CPU device, got {device}.' ) - # NB: JAX backend works with any device. + # NB: JAX implementation works with any device. -def _resolve_backend_impl_and_device( - backend_impl: Optional[Union[str, types.BackendImpl]], +def _resolve_impl_and_device( + impl: Optional[Union[str, types.Impl]], device: Optional[jax.Device] = None, -) -> Tuple[types.BackendImpl, jax.Device]: - """Resolves a backend implementation and device.""" - if backend_impl: - backend_impl = types.BackendImpl(backend_impl) +) -> Tuple[types.Impl, jax.Device]: + """Resolves a implementation and device.""" + if impl: + impl = types.Impl(impl) - has_backend_impl, has_device = backend_impl is not None, device is not None - if (has_backend_impl, has_device) == (True, True): + has_impl, has_device = impl is not None, device is not None + if (has_impl, has_device) == (True, True): pass - elif (has_backend_impl, has_device) == (True, False): - device = _resolve_device(backend_impl) - elif (has_backend_impl, has_device) == (False, True): - backend_impl = _resolve_backend_impl(device) + elif (has_impl, has_device) == (True, False): + device = _resolve_device(impl) + elif (has_impl, has_device) == (False, True): + impl = _resolve_impl(device) else: device = jax.devices(jax.default_backend())[0] logging.info('Using JAX default device: %s.', device) - backend_impl = _resolve_backend_impl(device) + impl = _resolve_impl(device) - _check_backend_impl_device_compatibility(backend_impl, device) - return backend_impl, device # pytype: disable=bad-return-type + _check_impl_device_compatibility(impl, device) + return impl, device # pytype: disable=bad-return-type def _strip_weak_type(tree): @@ -167,7 +167,7 @@ def _strip_weak_type(tree): def _put_option( o: mujoco.MjOption, - backend_impl: types.BackendImpl, + impl: types.Impl, impl_fields: Optional[dict[str, Any]] = None, ) -> types.Option: """Returns mjx.Option given mujoco.MjOption.""" @@ -195,7 +195,7 @@ def _put_option( fields['disableflags'] = types.DisableBit(o.disableflags) fields['enableflags'] = types.EnableBit(o.enableflags) - if backend_impl == types.BackendImpl.JAX: + if impl == types.Impl.JAX: has_fluid_params = o.density > 0 or o.viscosity > 0 or o.wind.any() implicitfast = o.integrator == mujoco.mjtIntegrator.mjINT_IMPLICITFAST if implicitfast and has_fluid_params: @@ -203,12 +203,12 @@ def _put_option( fields['has_fluid_params'] = has_fluid_params return types.OptionJAX(**fields, **(impl_fields or {})) - if backend_impl == types.BackendImpl.C: + if impl == types.Impl.C: c_field_keys = types.OptionC.__annotations__.keys() - fields.keys() c_fields = {k: getattr(o, k, None) for k in c_field_keys} return types.OptionC(**fields, **c_fields, **(impl_fields or {})) - raise NotImplementedError(f'Unsupported backend: {backend_impl}') + raise NotImplementedError(f'Unsupported implementation: {impl}') def _put_statistic(s: mujoco.MjStatistic) -> types.Statistic: @@ -248,23 +248,6 @@ def _put_model_jax( if t == mujoco.mjtGeom.mjGEOM_MESH: mesh_geomid.add(g) - # check for unsupported sensor and equality constraint combinations - sensor_rne_postconstraint = ( - np.any(m.sensor_type == types.SensorType.ACCELEROMETER) - | np.any(m.sensor_type == types.SensorType.FORCE) - | np.any(m.sensor_type == types.SensorType.TORQUE) - | np.any(m.sensor_type == types.SensorType.FRAMELINACC) - | np.any(m.sensor_type == types.SensorType.FRAMEANGACC) - ) - eq_connect_weld = np.any(m.eq_type == types.EqType.CONNECT) | np.any( - m.eq_type == types.EqType.WELD - ) - if sensor_rne_postconstraint and eq_connect_weld: - raise NotImplementedError( - 'rne_postconstraint not implemented with equality constraints:' - ' connect, weld.' - ) - for enum_field, enum_type, mj_type in ( (m.actuator_biastype, types.BiasType, mujoco.mjtBias), (m.actuator_dyntype, types.DynType, mujoco.mjtDyn), @@ -283,7 +266,7 @@ def _put_model_jax( mj_field_names = {f.name for f in types.Model.fields() if f.name != '_impl'} fields = {f: getattr(m, f) for f in mj_field_names} fields['cam_mat0'] = fields['cam_mat0'].reshape((-1, 3, 3)) - fields['opt'] = _put_option(m.opt, types.BackendImpl.JAX) + fields['opt'] = _put_option(m.opt, types.Impl.JAX) fields['stat'] = _put_statistic(m.stat) fields_jax = {} @@ -340,7 +323,7 @@ def _put_model_c( mj_field_names = {f.name for f in types.Model.fields() if f.name != '_impl'} fields = {f: getattr(m, f) for f in mj_field_names} fields['cam_mat0'] = fields['cam_mat0'].reshape((-1, 3, 3)) - fields['opt'] = _put_option(m.opt, backend_impl=types.BackendImpl.C) + fields['opt'] = _put_option(m.opt, impl=types.Impl.C) fields['stat'] = _put_statistic(m.stat) c_impl_keys = ( @@ -359,7 +342,7 @@ def _put_model_c( def put_model( m: mujoco.MjModel, device: Optional[jax.Device] = None, - backend_impl: Optional[Union[str, types.BackendImpl]] = None, + impl: Optional[Union[str, types.Impl]] = None, _full_compat: bool = False, # pylint: disable=invalid-name ) -> types.Model: """Puts mujoco.MjModel onto a device, resulting in mjx.Model. @@ -367,7 +350,7 @@ def put_model( Args: m: the model to put onto device device: which device to use - if unspecified picks the default device - backend_impl: backend implementation to use + impl: implementation to use _full_compat: put all MjModel fields onto device irrespective of MJX support This is an experimental feature. Avoid using it for now. @@ -375,28 +358,29 @@ def put_model( an mjx.Model placed on device Raises: - ValueError: if backend_impl is not supported + ValueError: if impl is not supported DeprecationWarning: if _full_compat is True """ if _full_compat: warnings.warn( - 'mjx.put_model(..., _full_compat=True) is deprecated. Use' - ' mjx.put_model(..., backend_impl=types.BackendImpl.C) instead.', + 'mjx.put_model(..., _full_compat=True) is deprecated and will be' + ' removed in MuJoCo >=3.4. Use mjx.put_model(..., impl=types.Impl.C)' + ' instead.', DeprecationWarning, stacklevel=2, ) - backend_impl = types.BackendImpl.C + impl = types.Impl.C - backend_impl, device = _resolve_backend_impl_and_device(backend_impl, device) - if backend_impl == types.BackendImpl.JAX: + impl, device = _resolve_impl_and_device(impl, device) + if impl == types.Impl.JAX: return _put_model_jax(m, device) - elif backend_impl == types.BackendImpl.C: + elif impl == types.Impl.C: return _put_model_c(m, device) - elif backend_impl == types.BackendImpl.WARP: - raise NotImplementedError('Warp backend not implemented yet.') + elif impl == types.Impl.WARP: + raise NotImplementedError('Warp implementation not implemented yet.') else: - raise ValueError(f'Unsupported backend implementation: {backend_impl}') + raise ValueError(f'Unsupported implementation: {impl}') def _make_data_public_fields(m: types.Model) -> Dict[str, Any]: @@ -476,7 +460,7 @@ def _make_data_contact_jax( def _make_data_jax( - m: types.Model, + m: Union[types.Model, mujoco.MjModel], device: Optional[jax.Device] = None, ) -> types.Data: """Allocate and initialize Data for the JAX implementation.""" @@ -564,7 +548,7 @@ def _make_data_jax( def _make_data_c( - m: types.Model, + m: Union[types.Model, mujoco.MjModel], device: Optional[jax.Device] = None, ) -> types.Data: """Allocate and initialize Data for the C implementation.""" @@ -582,11 +566,14 @@ def _make_data_c( # TODO(stunya): remove the JAX contact from C data. contact = _make_data_contact_jax(dim, efc_address) - nflexvert = m.nflexvert - nflexedge = m.nflexedge - nflexelem = m.nflexelem - nbvh = m.nbvh - nbvhdynamic = m.nbvhdynamic + def get(m, name: str): + return getattr(m._impl, name) if hasattr(m, '_impl') else getattr(m, name) # pylint: disable=protected-access + + nflexvert = get(m, 'nflexvert') + nflexedge = get(m, 'nflexedge') + nflexelem = get(m, 'nflexelem') + nbvh = get(m, 'nbvh') + nbvhdynamic = get(m, 'nbvhdynamic') zero_impl_fields = { 'solver_niter': (int_,), 'cdof': (m.nv, 6, float_), @@ -626,7 +613,7 @@ def _make_data_c( 'qLDiagInv': (m.nv, float_), 'ten_velocity': (m.ntendon, float_), 'actuator_velocity': (m.nu, float_), - 'plugin_data': (m.nplugin, np.uint64), + 'plugin_data': (get(m, 'nplugin'), np.uint64), 'B_rownnz': (m.nbody, np.int32), 'B_rowadr': (m.nbody, np.int32), 'B_colind': (m.nB, np.int32), @@ -696,7 +683,7 @@ def _make_data_c( def make_data( m: Union[types.Model, mujoco.MjModel], device: Optional[jax.Device] = None, - backend_impl: Optional[Union[str, types.BackendImpl]] = None, + impl: Optional[Union[str, types.Impl]] = None, _full_compat: bool = False, # pylint: disable=invalid-name ) -> types.Data: """Allocate and initialize Data. @@ -704,7 +691,7 @@ def make_data( Args: m: the model to use device: which device to use - if unspecified picks the default device - backend_impl: backend implementation to use + impl: implementation to use ('jax', 'warp') _full_compat: put all fields onto device irrespective of MJX support This is an experimental feature. Avoid using it for now. If using this flag, also use _full_compat for put_model. @@ -713,35 +700,34 @@ def make_data( an initialized mjx.Data placed on device Raises: - ValueError: if the model's backend_impl does not match the make_data - backend_impl - NotImplementedError: if the backend_impl is not implemented yet + ValueError: if the model's impl does not match the make_data impl + NotImplementedError: if the impl is not implemented yet DeprecationWarning: if _full_compat is used """ if _full_compat: warnings.warn( 'mjx.make_data(..., _full_compat=True) is deprecated. Use' - ' mjx.make_data(..., backend_impl=types.BackendImpl.C) instead.', + ' mjx.make_data(..., impl=types.Impl.C) instead.', DeprecationWarning, stacklevel=2, ) - backend_impl = types.BackendImpl.C + impl = types.Impl.C - backend_impl, device = _resolve_backend_impl_and_device(backend_impl, device) + impl, device = _resolve_impl_and_device(impl, device) - if isinstance(m, types.Model) and m.backend_impl != backend_impl: + if isinstance(m, types.Model) and m.impl != impl: raise ValueError( - f'Model backend_impl {m.backend_impl} does not match make_data ' - f'backend_impl {backend_impl}.' + f'Model impl {m.impl} does not match make_data ' + f'implementation {impl}.' ) - if backend_impl == types.BackendImpl.JAX: + if impl == types.Impl.JAX: return _make_data_jax(m, device) - elif backend_impl == types.BackendImpl.C: + elif impl == types.Impl.C: return _make_data_c(m, device) raise NotImplementedError( - f'make_data for backend_impl "{backend_impl}" not implemented yet.' + f'make_data for implementation "{impl}" not implemented yet.' ) @@ -951,7 +937,7 @@ def _put_data_c( if hasattr(d, f.name) } - # TODO(stunya): support islanding via C backend impl. + # TODO(stunya): support islanding via C impl. impl_fields['solver_niter'] = impl_fields['solver_niter'][0] # TODO(btaba): remove dense actuator moment. @@ -1039,7 +1025,7 @@ def put_data( m: mujoco.MjModel, d: mujoco.MjData, device: Optional[jax.Device] = None, - backend_impl: Optional[Union[str, types.BackendImpl]] = None, + impl: Optional[Union[str, types.Impl]] = None, _full_compat: bool = False, # pylint: disable=invalid-name ) -> types.Data: """Puts mujoco.MjData onto a device, resulting in mjx.Data. @@ -1048,7 +1034,7 @@ def put_data( m: the model to use d: the data to put on device device: which device to use - if unspecified picks the default device - backend_impl: backend implementation to use + impl: implementation to use ('jax', 'warp') _full_compat: put all MjModel fields onto device irrespective of MJX support This is an experimental feature. Avoid using it for now. If using this flag, also use _full_compat for put_model. @@ -1059,20 +1045,20 @@ def put_data( if _full_compat: warnings.warn( 'mjx.put_data(..., _full_compat=True) is deprecated. Use' - ' mjx.put_data(..., backend_impl=types.BackendImpl.C) instead.', + ' mjx.put_data(..., impl=types.Impl.C) instead.', DeprecationWarning, stacklevel=2, ) - backend_impl = types.BackendImpl.C + impl = types.Impl.C - backend_impl, device = _resolve_backend_impl_and_device(backend_impl, device) - if backend_impl == types.BackendImpl.JAX: + impl, device = _resolve_impl_and_device(impl, device) + if impl == types.Impl.JAX: return _put_data_jax(m, d, device) - elif backend_impl == types.BackendImpl.C: + elif impl == types.Impl.C: return _put_data_c(m, d, device) raise NotImplementedError( - f'put_data for backend_impl "{backend_impl}" not implemented yet.' + f'put_data for implementation "{impl}" not implemented yet.' ) @@ -1097,7 +1083,7 @@ def _get_data_into( batch_size = d.qpos.shape[0] if batched else 1 dof_i, dof_j = [], [] - if d.backend_impl == types.BackendImpl.JAX: + if d.impl == types.Impl.JAX: for i in range(m.nv): j = i while j > -1: @@ -1116,13 +1102,13 @@ def _get_data_into( if ncon != result_i.ncon or nefc != result_i.nefc or nj != result_i.nJ: mujoco._functions._realloc_con_efc(result_i, ncon=ncon, nefc=nefc, nJ=nj) # pylint: disable=protected-access - if d.backend_impl == types.BackendImpl.JAX: + if d.impl == types.Impl.JAX: all_fields = types.Data.fields() + types.DataJAX.fields() - elif d.backend_impl == types.BackendImpl.C: + elif d.impl == types.Impl.C: all_fields = types.Data.fields() + types.DataC.fields() else: raise NotImplementedError( - f'get_data_into for backend_impl "{d.backend_impl}" not implemented' + f'get_data_into for implementation "{d.impl}" not implemented' ' yet.' ) @@ -1188,7 +1174,7 @@ def _get_data_into( value = value.reshape(-1) elif field.name.startswith('efc_'): value = value[efc_active] - if d.backend_impl == types.BackendImpl.JAX: + if d.impl == types.Impl.JAX: if field.name == 'qM' and not support.is_sparse(m): value = value[dof_i, dof_j] elif field.name == 'qLD' and not support.is_sparse(m): @@ -1207,6 +1193,10 @@ def _get_data_into( else: setattr(result_i, field.name, value) + # TODO(taylorhowell): remove mapping once qM is deprecated + # map inertia (sparse) to reduced inertia (compressed sparse) representation + result_i.M[:] = result_i.qM[result_i.mapM2M] + # recalculate qLD and qLDiagInv as MJX and MuJoCo have different # representations of the Cholesky decomposition. mujoco.mj_factorM(m, result_i) @@ -1226,12 +1216,12 @@ def get_data_into( d = jax.device_get(d) - if d.backend_impl in (types.BackendImpl.JAX, types.BackendImpl.C): + if d.impl in (types.Impl.JAX, types.Impl.C): # TODO(stunya): Split out _get_data_into once codepaths diverge enough. return _get_data_into(result, m, d) raise NotImplementedError( - f'get_data_into for backend_impl "{d.backend_impl}" not implemented yet.' + f'get_data_into for implementation "{d.impl}" not implemented yet.' ) diff --git a/mjx/mujoco/mjx/_src/io_test.py b/mjx/mujoco/mjx/_src/io_test.py index 33565999..f8d56d09 100644 --- a/mjx/mujoco/mjx/_src/io_test.py +++ b/mjx/mujoco/mjx/_src/io_test.py @@ -26,8 +26,9 @@ from mujoco.mjx._src import io as mjx_io from mujoco.mjx._src import test_util # pylint: disable=g-importing-member -from mujoco.mjx._src.types import BackendImpl from mujoco.mjx._src.types import ConeType +from mujoco.mjx._src.types import Impl +from mujoco.mjx._src.types import JacobianType # pylint: enable=g-importing-member import numpy as np @@ -114,11 +115,11 @@ class ModelIOTest(parameterized.TestCase): @parameterized.product( xml=(_MULTIPLE_CONVEX_OBJECTS, _MULTIPLE_CONSTRAINTS), - backend_impl=('jax', 'c'), + impl=('jax', 'c'), ) - def test_put_model(self, xml, backend_impl): + def test_put_model(self, xml, impl): m = mujoco.MjModel.from_xml_string(xml) - mx = mjx.put_model(m, backend_impl=backend_impl) + mx = mjx.put_model(m, impl=impl) def assert_not_weak_type(x): if isinstance(x, jax.Array): @@ -140,10 +141,10 @@ class ModelIOTest(parameterized.TestCase): self.assertEqual(mx.nM, m.nM) self.assertAlmostEqual(mx.opt.timestep, m.opt.timestep) - if backend_impl == 'jax': + if impl == 'jax': # fields restricted to MuJoCo should not be populated self.assertFalse(hasattr(mx, 'bvh_aabb')) - elif backend_impl == 'c': + elif impl == 'c': # Options specific to C are populated. self.assertEqual(mx.opt.apirate, m.opt.apirate) # Fields private to C backend impl are populated. @@ -177,7 +178,7 @@ class ModelIOTest(parameterized.TestCase): mujoco.MjModel.from_xml_string( '' ), - backend_impl='jax', + impl='jax', ) self.assertTrue(m.opt.has_fluid_params) @@ -218,7 +219,7 @@ class ModelIOTest(parameterized.TestCase): """), - backend_impl='jax', + impl='jax', ) def test_implicitfast_fluid_not_implemented(self): @@ -229,18 +230,18 @@ class ModelIOTest(parameterized.TestCase): """), - backend_impl='jax', + impl='jax', ) def test_wrap_inside(self): m = test_util.load_test_file('tendon/wrap_sidesite.xml') - mx0 = mjx.put_model(m, backend_impl='jax') + mx0 = mjx.put_model(m, impl='jax') np.testing.assert_equal( mx0._impl.is_wrap_inside, np.array([1, 0, 1, 0, 1, 1, 0]), ) m.site_pos[2] = m.site_pos[1] - mx1 = mjx.put_model(m, backend_impl='jax') + mx1 = mjx.put_model(m, impl='jax') np.testing.assert_equal( mx1._impl.is_wrap_inside, np.array([0, 0, 1, 0, 1, 0, 0]), @@ -251,10 +252,10 @@ class DataIOTest(parameterized.TestCase): """IO tests for mjx.Data.""" @parameterized.parameters('jax', 'c') - def test_make_data(self, backend_impl: str): + def test_make_data(self, impl: str): """Test that make_data returns the correct shapes.""" m = mujoco.MjModel.from_xml_string(_MULTIPLE_CONVEX_OBJECTS) - d = mjx.make_data(m, backend_impl=backend_impl) + d = mjx.make_data(m, impl=impl) nq = 22 nbody = 5 @@ -312,34 +313,34 @@ class DataIOTest(parameterized.TestCase): self.assertEqual(d.qfrc_inverse.shape, (nv,)) self.assertEqual(d._impl.efc_force.shape, (nefc,)) - if backend_impl == 'jax': + if impl == 'jax': self.assertEqual(d._impl.qM.shape, (nv, nv)) self.assertEqual(d._impl.qLD.shape, (nv, nv)) self.assertEqual(d._impl.qLDiagInv.shape, (0,)) - elif backend_impl == 'c': + elif impl == 'c': self.assertEqual(d._impl.qM.shape, (nm,)) self.assertEqual(d._impl.qLD.shape, (nm,)) self.assertEqual(d._impl.qLDiagInv.shape, (nv,)) # test sparse m.opt.jacobian = mujoco.mjtJacobian.mjJAC_SPARSE - d = mjx.make_data(m, backend_impl=backend_impl) + d = mjx.make_data(m, impl=impl) self.assertEqual(d._impl.qM.shape, (nm,)) self.assertEqual(d._impl.qLD.shape, (nm,)) self.assertEqual(d._impl.qLDiagInv.shape, (nv,)) - if backend_impl == 'c': + if impl == 'c': # check C specific fields self.assertEqual(d._impl.light_xpos.shape, (m.nlight, 3)) self.assertEqual(d._impl.bvh_active.shape, (m.nbvh,)) @parameterized.parameters('jax', 'c') - def test_put_data(self, backend_impl: str): + def test_put_data(self, impl: str): """Test that put_data puts the correct data for dense and sparse.""" m = mujoco.MjModel.from_xml_string(_MULTIPLE_CONSTRAINTS) d = mujoco.MjData(m) mujoco.mj_step(m, d, 2) - dx = mjx.put_data(m, d, backend_impl=backend_impl) + dx = mjx.put_data(m, d, impl=impl) # check a few fields np.testing.assert_allclose(dx.qpos, d.qpos) @@ -356,12 +357,12 @@ class DataIOTest(parameterized.TestCase): ) ) - if backend_impl == 'jax': + if impl == 'jax': # check that qM is transformed properly qm = np.zeros((m.nv, m.nv), dtype=np.float64) mujoco.mj_fullM(m, qm, d.qM) np.testing.assert_allclose(qm, mjx.full_m(mjx.put_model(m), dx)) - elif backend_impl == 'c': + elif impl == 'c': np.testing.assert_allclose(dx._impl.qM, d.qM) np.testing.assert_allclose(dx._impl.qLD, d.qLD) np.testing.assert_allclose(dx._impl.qLDiagInv, d.qLDiagInv) @@ -417,7 +418,7 @@ class DataIOTest(parameterized.TestCase): m.opt.jacobian = mujoco.mjtJacobian.mjJAC_SPARSE d = mujoco.MjData(m) mujoco.mj_step(m, d, 2) - dx_sparse = mjx.put_data(m, d, backend_impl=backend_impl) + dx_sparse = mjx.put_data(m, d, impl=impl) np.testing.assert_allclose(dx_sparse._impl.efc_J, dx._impl.efc_J, atol=1e-8) # check sparse mass matrices are correct @@ -431,25 +432,25 @@ class DataIOTest(parameterized.TestCase): m.opt.jacobian = mujoco.mjtJacobian.mjJAC_DENSE d = mujoco.MjData(m) mujoco.mj_step(m, d, 2) - dx_from_dense = mjx.put_data(m, d, backend_impl=backend_impl) - if backend_impl == 'jax': + dx_from_dense = mjx.put_data(m, d, impl=impl) + if impl == 'jax': qm = np.zeros((m.nv, m.nv)) mujoco.mj_fullM(m, qm, d.qM) np.testing.assert_allclose(dx_from_dense._impl.qM, qm, atol=1e-8) - elif backend_impl == 'c': + elif impl == 'c': np.testing.assert_allclose(dx_from_dense._impl.qM, d.qM, atol=1e-8) @parameterized.parameters( ('jax', False), ('jax', True), ('c', False), ('c', True) ) - def test_get_data(self, backend_impl: str, sparse: bool): + def test_get_data(self, impl: str, sparse: bool): """Test that get_data makes correct MjData.""" m = mujoco.MjModel.from_xml_string(_MULTIPLE_CONSTRAINTS) if sparse: m.opt.jacobian = mujoco.mjtJacobian.mjJAC_SPARSE d = mujoco.MjData(m) mujoco.mj_step(m, d, 2) - dx = mjx.put_data(m, d, backend_impl=backend_impl) + dx = mjx.put_data(m, d, impl=impl) d_2: mujoco.MjData = mjx.get_data(m, dx) # check a few fields @@ -505,7 +506,7 @@ class DataIOTest(parameterized.TestCase): np.testing.assert_allclose(d_2.efc_aref, d.efc_aref) np.testing.assert_allclose(d_2.contact.efc_address, d.contact.efc_address) - if backend_impl == 'c': + if impl == 'c': # check fields specific to the C implementation np.testing.assert_allclose(d_2.bvh_active, d.bvh_active) @@ -540,13 +541,13 @@ class DataIOTest(parameterized.TestCase): mjx.get_data(m, dx) @parameterized.parameters('jax', 'c') - def test_get_data_batched(self, backend_impl): + def test_get_data_batched(self, impl): """Test that get_data makes correct List[MjData] for batched Data.""" m = mujoco.MjModel.from_xml_string(_MULTIPLE_CONSTRAINTS) d = mujoco.MjData(m) mujoco.mj_step(m, d, 2) - dx = mjx.put_data(m, d, backend_impl=backend_impl) + dx = mjx.put_data(m, d, impl=impl) # second data in batch has contact dist > 0, disables contact dx_b = jax.tree_util.tree_map(lambda x: jp.stack((x, x + 0.05)), dx) ds = mjx.get_data(m, dx_b) @@ -557,13 +558,13 @@ class DataIOTest(parameterized.TestCase): self.assertEqual(ds[1].ncon, 0) @parameterized.parameters('jax', 'c') - def test_get_data_into(self, backend_impl): + def test_get_data_into(self, impl): """Test that get_data_into correctly populates an MjData.""" m = mujoco.MjModel.from_xml_string(_MULTIPLE_CONSTRAINTS) d = mujoco.MjData(m) mujoco.mj_step(m, d, 2) - dx = mjx.put_data(m, d, backend_impl=backend_impl) + dx = mjx.put_data(m, d, impl=impl) d_2 = mujoco.MjData(m) mjx.get_data_into(d_2, m, dx) @@ -580,32 +581,32 @@ class DataIOTest(parameterized.TestCase): np.testing.assert_allclose(d_2.contact.frame, d.contact.frame) @parameterized.parameters('jax', 'c') - def test_get_data_into_wrong_shape(self, backend_impl): + def test_get_data_into_wrong_shape(self, impl): """Tests that get_data_into throwsif input and output shapes don't match.""" m = mujoco.MjModel.from_xml_string(_MULTIPLE_CONSTRAINTS) d = mujoco.MjData(m) mujoco.mj_step(m, d, 2) - dx = mjx.put_data(m, d, backend_impl=backend_impl) + dx = mjx.put_data(m, d, impl=impl) m_2 = mujoco.MjModel.from_xml_string(_MULTIPLE_CONVEX_OBJECTS) d_2 = mujoco.MjData(m_2) with self.assertRaisesRegex(ValueError, r'Input field.*has shape.*'): mjx.get_data_into(d_2, m, dx) @parameterized.parameters('jax', 'c') - def test_make_matches_put(self, backend_impl): + def test_make_matches_put(self, impl): """Test that make_data produces a pytree that matches put_data.""" m = mujoco.MjModel.from_xml_string(_MULTIPLE_CONSTRAINTS) d = mujoco.MjData(m) mujoco.mj_step(m, d, 2) - dx = mjx.put_data(m, d, backend_impl=backend_impl) + dx = mjx.put_data(m, d, impl=impl) step_fn = lambda d: d.replace(time=d.time + 1) step_fn_jit = jax.jit(step_fn).lower(dx).compile() # placing an MjData onto device should yield the same treedef mjx.Data as # calling make_data. they should be interchangeable for jax functions: - step_fn_jit(mjx.make_data(m, backend_impl=backend_impl)) + step_fn_jit(mjx.make_data(m, impl=impl)) def test_contact_elliptic_condim1(self): """Test that condim=1 with ConeType.ELLIPTIC is not implemented.""" @@ -624,37 +625,24 @@ class DataIOTest(parameterized.TestCase): with self.assertRaises(NotImplementedError): mjx.make_data(m) - @parameterized.product( - sensor=['accelerometer', 'force', 'torque'], equality=['connect', 'weld'] - ) - def test_sensor_constraint_compatibility(self, sensor, equality): - """Test unsupported sensor and equality constraint combinations.""" - equality_constraint = f'{equality} body1="body1" body2="body2"' - if equality == 'connect': - equality_constraint += ' anchor="0 0 0"' - m = mujoco.MjModel.from_xml_string(f""" - - - - - - - - - - - - - - <{equality_constraint}/> - - - <{sensor} site="site1"/> - - - """) - with self.assertRaises(NotImplementedError): - mjx.put_model(m, backend_impl='jax') + @parameterized.parameters(JacobianType.DENSE, JacobianType.SPARSE) + def test_qm_mapm2m(self, jacobian): + """Test that qM is mapped to M.""" + m = test_util.load_test_file('humanoid/humanoid.xml') + m.opt.jacobian = jacobian + d = mujoco.MjData(m) + mx = mjx.put_model(m, impl='jax') + dx = mjx.make_data(m, impl='jax') + dx = mjx.forward(mx, dx) + + mjx.get_data_into(d, m, dx) + + res_mj = np.zeros((1, m.nv)) + mujoco.mj_solveM(m, d, res_mj, np.ones((1, m.nv))) + + res = mjx._src.smooth.solve_m(mx, dx, jp.ones(m.nv)) + + np.testing.assert_allclose(res_mj[0], res, rtol=1e-3, atol=1e-3) class FullCompatTest(parameterized.TestCase): @@ -678,71 +666,71 @@ class FullCompatTest(parameterized.TestCase): m = mujoco.MjModel.from_xml_string(xml) with self.assertWarns(DeprecationWarning): out = mjx_io.put_model(m, _full_compat=True) - self.assertEqual(out.backend_impl, BackendImpl.C) + self.assertEqual(out.impl, Impl.C) with self.assertWarns(DeprecationWarning): out = mjx_io.make_data(m, _full_compat=True) - self.assertEqual(out.backend_impl, BackendImpl.C) + self.assertEqual(out.impl, Impl.C) -# Test cases for `_resolve_backend_impl_and_device` where the device is +# Test cases for `_resolve_impl_and_device` where the device is # specified by the user and the device is available. _DEVICE_TEST_CASES = [ # Arguments use the following format: - # (device_type_str, backend_impl_str, - # (expected_device, expected_backend_impl))) + # (device_type_str, impl_str, + # (expected_device, expected_impl))) # No backend specified. - ('cpu', None, ('cpu', BackendImpl.C)), - ('gpu-notnvidia', None, ('gpu', BackendImpl.JAX)), - ('gpu-nvidia', None, ('gpu', BackendImpl.WARP)), - ('tpu', None, ('tpu', BackendImpl.JAX)), + ('cpu', None, ('cpu', Impl.C)), + ('gpu-notnvidia', None, ('gpu', Impl.JAX)), + ('gpu-nvidia', None, ('gpu', Impl.WARP)), + ('tpu', None, ('tpu', Impl.JAX)), # JAX backend specified. - ('cpu', 'jax', ('cpu', BackendImpl.JAX)), - ('gpu-notnvidia', 'jax', ('gpu', BackendImpl.JAX)), - ('gpu-nvidia', 'jax', ('gpu', BackendImpl.JAX)), - ('tpu', 'jax', ('tpu', BackendImpl.JAX)), + ('cpu', 'jax', ('cpu', Impl.JAX)), + ('gpu-notnvidia', 'jax', ('gpu', Impl.JAX)), + ('gpu-nvidia', 'jax', ('gpu', Impl.JAX)), + ('tpu', 'jax', ('tpu', Impl.JAX)), # WARP backend specified. ('cpu', 'warp', ('cpu', 'error')), ('gpu-notnvidia', 'warp', ('cpu', 'error')), - ('gpu-nvidia', 'warp', ('gpu', BackendImpl.WARP)), + ('gpu-nvidia', 'warp', ('gpu', Impl.WARP)), ('tpu', 'warp', ('tpu', 'error')), # C backend specified. - ('cpu', 'c', ('cpu', BackendImpl.C)), + ('cpu', 'c', ('cpu', Impl.C)), ('gpu-notnvidia', 'c', ('cpu', 'error')), ('gpu-nvidia', 'c', ('cpu', 'error')), ('tpu', 'c', ('tpu', 'error')), ] -# Test cases for `_resolve_backend_impl_and_device` where the user does NOT +# Test cases for `_resolve_impl_and_device` where the user does NOT # specify a device. We mock the JAX default device. _DEFAULT_DEVICE_TEST_CASES = [ # Arguments use the following format: - # (jax.default_device, backend_impl_str, - # (expected_device, expected_backend_impl)) + # (jax.default_device, impl_str, + # (expected_device, expected_impl)) # No backend impl specified. - ('cpu', None, ('cpu', BackendImpl.C)), - ('gpu-notnvidia', None, ('gpu', BackendImpl.JAX)), - ('gpu-nvidia', None, ('gpu', BackendImpl.WARP)), - ('tpu', None, ('tpu', BackendImpl.JAX)), + ('cpu', None, ('cpu', Impl.C)), + ('gpu-notnvidia', None, ('gpu', Impl.JAX)), + ('gpu-nvidia', None, ('gpu', Impl.WARP)), + ('tpu', None, ('tpu', Impl.JAX)), # JAX backend impl specified. - ('cpu', 'jax', ('cpu', BackendImpl.JAX)), - ('gpu-notnvidia', 'jax', ('gpu', BackendImpl.JAX)), - ('gpu-nvidia', 'jax', ('gpu', BackendImpl.JAX)), - ('tpu', 'jax', ('tpu', BackendImpl.JAX)), + ('cpu', 'jax', ('cpu', Impl.JAX)), + ('gpu-notnvidia', 'jax', ('gpu', Impl.JAX)), + ('gpu-nvidia', 'jax', ('gpu', Impl.JAX)), + ('tpu', 'jax', ('tpu', Impl.JAX)), # WARP backend impl specified. ('cpu', 'warp', ('cpu', 'error')), ('gpu-notnvidia', 'warp', ('cpu', 'error')), - ('gpu-nvidia', 'warp', ('gpu', BackendImpl.WARP)), + ('gpu-nvidia', 'warp', ('gpu', Impl.WARP)), ('tpu', 'warp', ('tpu', 'error')), # C backend impl specified, CPU should always be available. - ('cpu', 'c', ('cpu', BackendImpl.C)), - ('gpu-notnvidia', 'c', ('cpu', BackendImpl.C)), - ('gpu-nvidia', 'c', ('cpu', BackendImpl.C)), - ('tpu', 'c', ('cpu', BackendImpl.C)), + ('cpu', 'c', ('cpu', Impl.C)), + ('gpu-notnvidia', 'c', ('cpu', Impl.C)), + ('gpu-nvidia', 'c', ('cpu', Impl.C)), + ('tpu', 'c', ('cpu', Impl.C)), ] -class ResolveBackendImplAndDeviceTest(parameterized.TestCase): - """Tests for the _resolve_backend_impl_and_device function.""" +class ResolveImplAndDeviceTest(parameterized.TestCase): + """Tests for the _resolve_impl_and_device function.""" def setUp(self): super().setUp() @@ -783,7 +771,7 @@ class ResolveBackendImplAndDeviceTest(parameterized.TestCase): def test_resolve_with_device( self, device_type_str, - backend_impl_str, + impl_str, expected, ): """Tests various combinations of device and backend impls.""" @@ -811,21 +799,21 @@ class ResolveBackendImplAndDeviceTest(parameterized.TestCase): self.mock_jax_devices.side_effect = devices_side_effect - expected_device, expected_backend_impl = expected - if expected_backend_impl == 'error': + expected_device, expected_impl = expected + if expected_impl == 'error': with self.assertRaises(AssertionError): - mjx_io._resolve_backend_impl_and_device( - backend_impl=backend_impl_str, device=input_device + mjx_io._resolve_impl_and_device( + impl=impl_str, device=input_device ) return - actual_backend_impl, actual_device = ( - mjx_io._resolve_backend_impl_and_device( - backend_impl=backend_impl_str, device=input_device + actual_impl, actual_device = ( + mjx_io._resolve_impl_and_device( + impl=impl_str, device=input_device ) ) - self.assertEqual(actual_backend_impl, expected_backend_impl) + self.assertEqual(actual_impl, expected_impl) self.assertIsNotNone(actual_device) self.assertEqual(actual_device.platform, expected_device) @@ -839,7 +827,7 @@ class ResolveBackendImplAndDeviceTest(parameterized.TestCase): def test_resolve_without_device( self, default_device_str, - backend_impl_str, + impl_str, expected, ): """Tests various combinations of jax.default_device and backend impls.""" @@ -878,32 +866,32 @@ class ResolveBackendImplAndDeviceTest(parameterized.TestCase): lambda: default_device_side_effect_str ) - expected_device, expected_backend_impl = expected + expected_device, expected_impl = expected if ( - expected_backend_impl == 'error' + expected_impl == 'error' and default_device_str != 'gpu-nvidia' - and backend_impl_str == 'warp' + and impl_str == 'warp' ): with self.assertRaisesRegex(RuntimeError, 'cuda backend not supported'): - mjx_io._resolve_backend_impl_and_device( - backend_impl=backend_impl_str, device=None + mjx_io._resolve_impl_and_device( + impl=impl_str, device=None ) return - if expected_backend_impl == 'error': + if expected_impl == 'error': with self.assertRaises(AssertionError): - mjx_io._resolve_backend_impl_and_device( - backend_impl=backend_impl_str, device=None + mjx_io._resolve_impl_and_device( + impl=impl_str, device=None ) return - actual_backend_impl, actual_device = ( - mjx_io._resolve_backend_impl_and_device( - backend_impl=backend_impl_str, device=None + actual_impl, actual_device = ( + mjx_io._resolve_impl_and_device( + impl=impl_str, device=None ) ) - self.assertEqual(actual_backend_impl, expected_backend_impl) + self.assertEqual(actual_impl, expected_impl) self.assertIsNotNone(actual_device) self.assertEqual(actual_device.platform, expected_device) @@ -918,26 +906,26 @@ class ResolveBackendImplAndDeviceTest(parameterized.TestCase): self.mock_default_backend.side_effect = lambda: 'gpu' # Default to JAX instead of WARP on NVIDIA GPU. - backend_impl, device = mjx_io._resolve_backend_impl_and_device( - backend_impl=None, device=None + impl, device = mjx_io._resolve_impl_and_device( + impl=None, device=None ) - self.assertEqual(backend_impl, BackendImpl.JAX) + self.assertEqual(impl, Impl.JAX) self.assertEqual(device.platform, 'gpu') # Specifying an NVIDIA GPU should still choose JAX. - backend_impl, device = mjx_io._resolve_backend_impl_and_device( - backend_impl=None, device=self.mock_nvidia_gpu + impl, device = mjx_io._resolve_impl_and_device( + impl=None, device=self.mock_nvidia_gpu ) - self.assertEqual(backend_impl, BackendImpl.JAX) + self.assertEqual(impl, Impl.JAX) self.assertEqual(device.platform, 'gpu') # Requesting warp explicitly should fail since it is disabled. with self.assertRaises(AssertionError): - mjx_io._resolve_backend_impl_and_device( - backend_impl='warp', device=self.mock_nvidia_gpu + mjx_io._resolve_impl_and_device( + impl='warp', device=self.mock_nvidia_gpu ) with self.assertRaises(AssertionError): - mjx_io._resolve_backend_impl_and_device(backend_impl='warp', device=None) + mjx_io._resolve_impl_and_device(impl='warp', device=None) @mock.patch.dict(os.environ, {'MJX_C_DEFAULT_ENABLED': 'false'}) def test_resolve_c_disabled(self): @@ -950,30 +938,30 @@ class ResolveBackendImplAndDeviceTest(parameterized.TestCase): self.mock_default_backend.side_effect = lambda: 'cpu' # Default to JAX instead of C on CPU. - backend_impl, device = mjx_io._resolve_backend_impl_and_device( - backend_impl=None, device=None + impl, device = mjx_io._resolve_impl_and_device( + impl=None, device=None ) - self.assertEqual(backend_impl, BackendImpl.JAX) + self.assertEqual(impl, Impl.JAX) self.assertEqual(device.platform, 'cpu') # Specifing CPU should still choose JAX. - backend_impl, device = mjx_io._resolve_backend_impl_and_device( - backend_impl=None, device=self.mock_cpu + impl, device = mjx_io._resolve_impl_and_device( + impl=None, device=self.mock_cpu ) - self.assertEqual(backend_impl, BackendImpl.JAX) + self.assertEqual(impl, Impl.JAX) self.assertEqual(device.platform, 'cpu') # Specifying C should choose C! - backend_impl, device = mjx_io._resolve_backend_impl_and_device( - backend_impl='c', device=None + impl, device = mjx_io._resolve_impl_and_device( + impl='c', device=None ) - self.assertEqual(backend_impl, BackendImpl.C) + self.assertEqual(impl, Impl.C) self.assertEqual(device.platform, 'cpu') - backend_impl, device = mjx_io._resolve_backend_impl_and_device( - backend_impl='c', device=self.mock_cpu + impl, device = mjx_io._resolve_impl_and_device( + impl='c', device=self.mock_cpu ) - self.assertEqual(backend_impl, BackendImpl.C) + self.assertEqual(impl, Impl.C) self.assertEqual(device.platform, 'cpu') diff --git a/mjx/mujoco/mjx/_src/sensor.py b/mjx/mujoco/mjx/_src/sensor.py index 41751527..bbdab811 100644 --- a/mjx/mujoco/mjx/_src/sensor.py +++ b/mjx/mujoco/mjx/_src/sensor.py @@ -22,7 +22,7 @@ from mujoco.mjx._src import math from mujoco.mjx._src import ray from mujoco.mjx._src import smooth from mujoco.mjx._src import support -from mujoco.mjx._src.types import BackendImpl +from mujoco.mjx._src.types import Impl from mujoco.mjx._src.types import Data from mujoco.mjx._src.types import DataJAX from mujoco.mjx._src.types import DisableBit diff --git a/mjx/mujoco/mjx/_src/smooth.py b/mjx/mujoco/mjx/_src/smooth.py index f87c2ed3..8f19f6f1 100644 --- a/mjx/mujoco/mjx/_src/smooth.py +++ b/mjx/mujoco/mjx/_src/smooth.py @@ -29,6 +29,7 @@ from mujoco.mjx._src.types import EqType from mujoco.mjx._src.types import JointType from mujoco.mjx._src.types import Model from mujoco.mjx._src.types import ModelJAX +from mujoco.mjx._src.types import ObjType from mujoco.mjx._src.types import TrnType from mujoco.mjx._src.types import WrapType # pylint: enable=g-importing-member @@ -659,11 +660,126 @@ def rne_postconstraint(m: Model, d: Data) -> Data: cfrc_contact.reshape((-1, 6)) ) - # TODO(taylorhowell): connect and weld constraints - if np.any(m.eq_type == EqType.CONNECT): - raise NotImplementedError('Connect constraints are not implemented.') - if np.any(m.eq_type == EqType.WELD): - raise NotImplementedError('Weld constraints are not implemented.') + # cfrc_ext += connect, weld + cfrc_ext_equality = [] + cfrc_ext_equality_adr = [] + + connect_id = m.eq_type == EqType.CONNECT + nconnect = connect_id.sum() + + if nconnect: + cfrc_connect_force = d._impl.efc_force[: 3 * nconnect].reshape( + (nconnect, 3) + ) + + is_site = m.eq_objtype == ObjType.SITE + body1id = np.copy(m.eq_obj1id) + body2id = np.copy(m.eq_obj2id) + pos1 = m.eq_data[:, :3] + pos2 = m.eq_data[:, 3:6] + + if m.nsite: + body1id[is_site] = m.site_bodyid[m.eq_obj1id[is_site]] + body2id[is_site] = m.site_bodyid[m.eq_obj2id[is_site]] + pos1 = jp.where(is_site[:, None], m.site_pos[m.eq_obj1id], pos1) + pos2 = jp.where(is_site[:, None], m.site_pos[m.eq_obj2id], pos2) + + # body 1 + k1_connect = body1id[connect_id] + k1_connect_mask = k1_connect != 0 + offset1_connect = pos1[connect_id] + + pos1_connect = jax.vmap(lambda pnt, mat, vec: mat @ pnt + vec)( + offset1_connect, d.xmat[k1_connect], d.xpos[k1_connect] + ) + subtree_com1_connect = d.subtree_com[jp.array(m.body_rootid)[k1_connect]] + cfrc_com1_connect = jax.vmap( + lambda dif, frc, mask: mask * jp.concatenate([-jp.cross(dif, frc), frc]) + )(subtree_com1_connect - pos1_connect, cfrc_connect_force, k1_connect_mask) + + # body 2 + k2_connect = body2id[connect_id] + k2_connect_mask = -1 * (k2_connect != 0) + offset2_connect = pos2[connect_id] + + pos2_connect = jax.vmap(lambda pnt, mat, vec: mat @ pnt + vec)( + offset2_connect, d.xmat[k2_connect], d.xpos[k2_connect] + ) + subtree_com2_connect = d.subtree_com[jp.array(m.body_rootid)[k2_connect]] + cfrc_com2_connect = jax.vmap( + lambda dif, frc, mask: mask * jp.concatenate([-jp.cross(dif, frc), frc]) + )(subtree_com2_connect - pos2_connect, cfrc_connect_force, k2_connect_mask) + + cfrc_ext_equality.append(jp.vstack([cfrc_com1_connect, cfrc_com2_connect])) + cfrc_ext_equality_adr.append(jp.concatenate([k1_connect, k2_connect])) + + weld_id = m.eq_type == EqType.WELD + nweld = weld_id.sum() + + if nweld: + cfrc_weld = d._impl.efc_force[ + 3 * nconnect : 3 * nconnect + 6 * nweld + ].reshape((nweld, 6)) + cfrc_weld_force = cfrc_weld[:, :3] + cfrc_weld_torque = cfrc_weld[:, 3:] + + is_site = m.eq_objtype == ObjType.SITE + body1id = np.copy(m.eq_obj1id) + body2id = np.copy(m.eq_obj2id) + pos1 = m.eq_data[:, 3:6] + pos2 = m.eq_data[:, :3] + + if m.nsite: + body1id[is_site] = m.site_bodyid[m.eq_obj1id[is_site]] + body2id[is_site] = m.site_bodyid[m.eq_obj2id[is_site]] + pos1 = jp.where(is_site[:, None], m.site_pos[m.eq_obj1id], pos1) + pos2 = jp.where(is_site[:, None], m.site_pos[m.eq_obj2id], pos2) + + # body 1 + k1_weld = body1id[weld_id] + k1_weld_mask = k1_weld != 0 + offset1_weld = pos1[weld_id] + + pos1_weld = jax.vmap(lambda pnt, mat, vec: mat @ pnt + vec)( + offset1_weld, d.xmat[k1_weld], d.xpos[k1_weld] + ) + subtree_com1_weld = d.subtree_com[jp.array(m.body_rootid)[k1_weld]] + cfrc_com1_weld = jax.vmap( + lambda dif, frc, trq, mask: mask + * jp.concatenate([trq - jp.cross(dif, frc), frc]) + )( + subtree_com1_weld - pos1_weld, + cfrc_weld_force, + cfrc_weld_torque, + k1_weld_mask, + ) + + # body 2 + k2_weld = body2id[weld_id] + k2_weld_mask = -1 * (k2_weld != 0) + offset2_weld = pos2[weld_id] + + pos2_weld = jax.vmap(lambda pnt, mat, vec: mat @ pnt + vec)( + offset2_weld, d.xmat[k2_weld], d.xpos[k2_weld] + ) + subtree_com2_weld = d.subtree_com[jp.array(m.body_rootid)[k2_weld]] + cfrc_com2_weld = jax.vmap( + lambda dif, frc, trq, mask: mask + * jp.concatenate([trq - jp.cross(dif, frc), frc]) + )( + subtree_com2_weld - pos2_weld, + cfrc_weld_force, + cfrc_weld_torque, + k2_weld_mask, + ) + + cfrc_ext_equality.append(jp.vstack([cfrc_com1_weld, cfrc_com2_weld])) + cfrc_ext_equality_adr.append(jp.concatenate([k1_weld, k2_weld])) + + if nconnect or nweld: + cfrc_ext = cfrc_ext.at[jp.concatenate(cfrc_ext_equality_adr)].add( + jp.vstack(cfrc_ext_equality) + ) # forward pass over bodies: compute cacc, cfrc_int def _forward(carry, cfrc_ext, cinert, cvel, body_dofadr, body_dofnum): @@ -1184,3 +1300,158 @@ def transmission(m: Model, d: Data) -> Data: {'_impl.actuator_length': length, '_impl.actuator_moment': moment} ) return d + + +def tendon_armature(m: Model, d: Data) -> Data: + """Add tendon armature to qM.""" + if not isinstance(m._impl, ModelJAX) or not isinstance(d._impl, DataJAX): + raise ValueError('tendon_armature requires JAX backend implementation.') + + if not m.ntendon: + return d + + # TODO(taylorhowell): if sparse, compute sparse JTAJ + JTAJ = d._impl.ten_J.T @ jax.vmap(jp.multiply)( + d._impl.ten_J, m.tendon_armature + ) + + if support.is_sparse(m): + ij = [] + for i in range(m.nv): + j = i + while j > -1: + ij.append((i, j)) + j = m.dof_parentid[j] + + i, j = (jp.array(x) for x in zip(*ij)) + JTAJ = JTAJ[(i, j)] + + return d.tree_replace({'_impl.qM': d._impl.qM + JTAJ}) + + +def tendon_dot(m: Model, d: Data) -> jax.Array: + """Compute time derivative of dense tendon Jacobian for one tendon.""" + if not isinstance(m._impl, ModelJAX) or not isinstance(d._impl, DataJAX): + raise ValueError('tendon_dot requires JAX backend implementation.') + + ten_Jdot = jp.zeros((m.ntendon, m.nv)) # pylint: disable=invalid-name + + if not m.ntendon: + return ten_Jdot + + # process pulleys + (wrap_id_pulley,) = np.nonzero(m.wrap_type == WrapType.PULLEY) + + divisor = np.ones(m.nwrap) + for adr, num in zip(m.tendon_adr, m.tendon_num): + for id_pulley in wrap_id_pulley: + if adr <= id_pulley < adr + num: + divisor[id_pulley : adr + num] = np.maximum( + mujoco.mjMINVAL, m.wrap_prm[id_pulley] + ) + + # process spatial tendon sites + (wrap_id_site,) = np.nonzero(m.wrap_type == WrapType.SITE) + + # find consecutive sites, skipping tendon transitions + (pair_id,) = np.nonzero(np.diff(wrap_id_site) == 1) + wrap_id_site_pair = np.setdiff1d(wrap_id_site[pair_id], m.tendon_adr[1:] - 1) + wrap_objid_site0 = m.wrap_objid[wrap_id_site_pair] + wrap_objid_site1 = m.wrap_objid[wrap_id_site_pair + 1] + site_bodyid0 = m.site_bodyid[wrap_objid_site0] + site_bodyid1 = m.site_bodyid[wrap_objid_site1] + site_xpos0 = d.site_xpos[wrap_objid_site0] + site_xpos1 = d.site_xpos[wrap_objid_site1] + subtree_com0 = d.subtree_com[m.body_rootid[site_bodyid0]] + subtree_com1 = d.subtree_com[m.body_rootid[site_bodyid1]] + site_vel0 = jax.vmap(lambda a, b: a[3:] - jp.cross(b, a[:3]))( + d.cvel[site_bodyid0], site_xpos0 - subtree_com0 + ) + site_vel1 = jax.vmap(lambda a, b: a[3:] - jp.cross(b, a[:3]))( + d.cvel[site_bodyid1], site_xpos1 - subtree_com1 + ) + + @jax.vmap + def _momentdot(wpnt0, wpnt1, wvel0, wvel1, body0, body1): + # dpnt = 3D position difference, normalize + dpnt = wpnt1 - wpnt0 + norm = math.norm(dpnt) + dpnt = jp.where( + norm < mujoco.mjMINVAL, jp.array([1.0, 0.0, 0.0]), dpnt / norm + ) + + # dvel = d / dt(dpnt) + dvel = wvel1 - wvel0 + dot = jp.dot(dpnt, dvel) + dvel += dpnt * -dot + dvel = jp.where(norm > mujoco.mjMINVAL, dvel / norm, 0.0) + + # get endpoint JacobianDots, subtract + jacp1, _ = support.jac_dot(m, d, wpnt0, body0) + jacp2, _ = support.jac_dot(m, d, wpnt1, body1) + jacdif = jacp2 - jacp1 + + # chain rule, first term: Jdot += d / dt(jac2 - jac1) * dpnt + tmp0 = jacdif @ dpnt + + # get endpoint Jacobians, subtract + jacp1, _ = support.jac(m, d, wpnt0, body0) + jacp2, _ = support.jac(m, d, wpnt1, body1) + jacdif = jacp2 - jacp1 + + # chain rule, second term: Jdot += (jac2 - jac1) * d/dt (dpnt) + tmp1 = jacdif @ dvel + + return jp.where(body0 != body1, tmp0 + tmp1, jp.zeros(m.nv)) + + momentdots = _momentdot( + site_xpos0, + site_xpos1, + site_vel0, + site_vel1, + site_bodyid0, + site_bodyid1, + ) + + if wrap_id_site_pair.size: + divisor_site_pair = divisor[wrap_id_site_pair] + momentdots /= divisor_site_pair[:, None] + + tendon_nsite = np.array([ + sum((wrap_id_site_pair >= adr) & (wrap_id_site_pair < adr + num)) + for adr, num in zip(m.tendon_adr, m.tendon_num) + ]) + tendon_has_site = tendon_nsite > 0 + (tendon_id_site,) = np.nonzero(tendon_has_site) + tendon_nsite = tendon_nsite[tendon_has_site] + tendon_with_site = tendon_nsite.size + ten_site_id = np.repeat(np.arange(tendon_with_site), tendon_nsite) + + momentdot = jax.ops.segment_sum(momentdots, ten_site_id, tendon_with_site) + ten_Jdot = ten_Jdot.at[tendon_id_site].set(momentdot) # pylint: disable=invalid-name + + # TODO(taylorhowell): time derivatives for geoms + + return ten_Jdot + + +def tendon_bias(m: Model, d: Data) -> Data: + """Add bias force due to tendon armature.""" + if not isinstance(m._impl, ModelJAX) or not isinstance(d._impl, DataJAX): + raise ValueError('tendon_bias requires JAX backend implementation.') + + if not m.ntendon: + return d + + # get dense d/dt(tendon Jacobian) for each tendon + ten_Jdot = tendon_dot(m, d) # pylint: disable=invalid-name + + # add bias term: qfrc += ten_J * armature * ten_Jdot @ qvel + coef = m.tendon_armature * jp.dot(ten_Jdot, d.qvel) + + return d.tree_replace({ + 'qfrc_bias': ( + d.qfrc_bias + + jp.sum(jax.vmap(jp.multiply)(d._impl.ten_J, coef), axis=0) + ) + }) diff --git a/mjx/mujoco/mjx/_src/smooth_test.py b/mjx/mujoco/mjx/_src/smooth_test.py index 96470a21..061087f9 100644 --- a/mjx/mujoco/mjx/_src/smooth_test.py +++ b/mjx/mujoco/mjx/_src/smooth_test.py @@ -17,10 +17,12 @@ from absl.testing import absltest from absl.testing import parameterized import jax +from jax import numpy as jp import mujoco from mujoco import mjx from mujoco.mjx._src import test_util from mujoco.mjx._src.types import ConeType # pylint: disable=g-importing-member +from mujoco.mjx._src.types import JacobianType # pylint: disable=g-importing-member import numpy as np # tolerance for difference between MuJoCo and MJX smooth calculations - mostly @@ -228,21 +230,98 @@ class SmoothTest(absltest.TestCase): class RnePostConstraintTest(parameterized.TestCase): + _CONNECT_SITE = """ + + + + """ + _CONNECT_BODY = """ + + + + """ + _WELD_SITE = """ + + + + """ + _WELD_BODY = """ + + + + """ + _CONNECT_SITE_WELD_SITE = """ + + + + + """ + _WELD_SITE_CONNECT_SITE = """ + + + + + """ + _WELD_SITE_CONNECT_SITE_WELD_BODY = """ + + + + + + """ + _CONNECT_SITE_WELD_SITE_WELD_BODY = """ + + + + + + """ + _CONNECT_SITE_CONNECT_BODY_CONNECT_WELD = """ + + + + + + """ - @parameterized.parameters(ConeType) - def test_rnepostconstraint(self, cone_type): + @parameterized.parameters( + ('', ConeType.PYRAMIDAL, None), + ('', ConeType.ELLIPTIC, None), + (_CONNECT_SITE, ConeType.PYRAMIDAL, None), + (_CONNECT_BODY, ConeType.PYRAMIDAL, None), + (_WELD_SITE, ConeType.PYRAMIDAL, None), + (_WELD_BODY, ConeType.PYRAMIDAL, None), + (_CONNECT_SITE_WELD_SITE, ConeType.PYRAMIDAL, None), + ( + _WELD_SITE_CONNECT_SITE, + ConeType.PYRAMIDAL, + np.array([6, 7, 8, 0, 1, 2, 3, 4, 5]), + ), + ( + _WELD_SITE_CONNECT_SITE_WELD_BODY, + ConeType.PYRAMIDAL, + np.array([6, 7, 8, 0, 1, 2, 3, 4, 5]), + ), + (_CONNECT_SITE_WELD_SITE_WELD_BODY, ConeType.PYRAMIDAL, None), + (_CONNECT_SITE_CONNECT_BODY_CONNECT_WELD, ConeType.PYRAMIDAL, None), + ) + def test_rnepostconstraint(self, equality, cone_type, efc_map): """Tests MJX rne_postconstraint function to match MuJoCo mj_rnePostConstraint.""" - m = mujoco.MjModel.from_xml_string(""" + m = mujoco.MjModel.from_xml_string(f""" - + + + + + @@ -254,6 +333,7 @@ class RnePostConstraintTest(parameterized.TestCase): + {equality} @@ -271,6 +351,11 @@ class RnePostConstraintTest(parameterized.TestCase): mx = mjx.put_model(m) dx = mjx.put_data(m, d) + if efc_map is not None: + efc_force = d.efc_force.copy() + efc_force[: len(efc_map)] = d.efc_force[efc_map] + dx = dx.tree_replace({'_impl.efc_force': jp.array(efc_force)}) + # rne postconstraint mujoco.mj_rnePostConstraint(m, d) dx = jax.jit(mjx.rne_postconstraint)(mx, dx) @@ -316,6 +401,36 @@ class TendonTest(parameterized.TestCase): _assert_eq(d.wrap_obj, dx._impl.wrap_obj, 'wrap_obj') _assert_eq(d.wrap_xpos, dx._impl.wrap_xpos, 'wrap_xpos') + @parameterized.parameters(JacobianType.DENSE, JacobianType.SPARSE) + def test_tendon_armature(self, jacobian): + """Tests MJX tendon armature matches MuJoCo.""" + m = test_util.load_test_file('tendon/armature.xml') + m.opt.jacobian = jacobian + d = mujoco.MjData(m) + mujoco.mj_resetDataKeyframe(m, d, 0) + mujoco.mj_forward(m, d) + + mx = mjx.put_model(m) + dx = mjx.put_data(m, d) + + dx = dx.tree_replace( + {'_impl.qM': jp.zeros((m.nv, m.nv)), 'qfrc_bias': jp.zeros(m.nv)} + ) + + dx = mjx.crb(mx, dx) + dx = mjx.tendon_armature(mx, dx) + + if jacobian == JacobianType.DENSE: + qM = np.zeros((m.nv, m.nv)) # pylint: disable=invalid-name + mujoco.mj_fullM(m, qM, d.qM) + else: + qM = d.qM # pylint: disable=invalid-name + _assert_eq(dx._impl.qM, qM, 'qM') + + dx = mjx.rne(mx, dx) + dx = mjx.tendon_bias(mx, dx) + _assert_eq(dx.qfrc_bias, d.qfrc_bias, 'qfrc_bias') + if __name__ == '__main__': absltest.main() diff --git a/mjx/mujoco/mjx/_src/solver.py b/mjx/mujoco/mjx/_src/solver.py index 98ec3d27..90c1add8 100644 --- a/mjx/mujoco/mjx/_src/solver.py +++ b/mjx/mujoco/mjx/_src/solver.py @@ -403,7 +403,9 @@ def _update_gradient(m: Model, d: Data, ctx: Context) -> Context: else: h = (d._impl.efc_J.T * d._impl.efc_D * ctx.active) @ d._impl.efc_J h = support.full_m(m, d) + h - h_ = jax.scipy.linalg.cho_factor(h) + # Symmetrize to reduce the chance of numerical issues in cholesky. + h_sym = (h + h.T) * 0.5 + h_ = jax.scipy.linalg.cho_factor(h_sym) mgrad = jax.scipy.linalg.cho_solve(h_, grad) else: raise NotImplementedError(f'unsupported solver type: {m.opt.solver}') diff --git a/mjx/mujoco/mjx/_src/support.py b/mjx/mujoco/mjx/_src/support.py index 15715021..ba62d9e3 100644 --- a/mjx/mujoco/mjx/_src/support.py +++ b/mjx/mujoco/mjx/_src/support.py @@ -142,6 +142,7 @@ def jac( m: Model, d: Data, point: jax.Array, body_id: jax.Array ) -> Tuple[jax.Array, jax.Array]: """Compute pair of (NV, 3) Jacobians of global point attached to body.""" + # TODO(taylorhowell): statically construct mask fn = lambda carry, b: b if carry is None else b + carry mask = (jp.arange(m.nbody) == body_id) * 1 mask = scan.body_tree(m, fn, 'b', 'b', mask, reverse=True) @@ -155,6 +156,42 @@ def jac( return jacp, jacr +def jac_dot( + m: Model, d: Data, point: jax.Array, body_id: jax.Array +) -> Tuple[jax.Array, jax.Array]: + """Compute pair of (NV, 3) Jacobian time derivatives of global point attached to body.""" + # TODO(taylorhowell): statically construct mask + fn = lambda carry, b: b if carry is None else b + carry + mask = (jp.arange(m.nbody) == body_id) * 1 + mask = scan.body_tree(m, fn, 'b', 'b', mask, reverse=True) + mask = mask[jp.array(m.dof_bodyid)] > 0 + + offset = point - d.subtree_com[jp.array(m.body_rootid)[body_id]] + pvel_lin = d.cvel[body_id][3:] - jp.cross(offset, d.cvel[body_id][:3]) + + cdof = d._impl.cdof + cdof_dot = d._impl.cdof_dot + + # check for quaternion + jnt_type = m.jnt_type[m.dof_jntid] + dof_adr = m.jnt_dofadr[m.dof_jntid] + is_quat = (jnt_type == JointType.BALL) | ( + jnt_type == JointType.FREE & (np.arange(m.nv) >= dof_adr + 3) + ) + + # compute cdof_dot for quaternion (use current body cvel) + cdof_dot_quat = jax.vmap(math.motion_cross)(d.cvel[m.dof_bodyid], cdof) + cdof_dot = jp.where(is_quat[:, None], cdof_dot_quat, cdof_dot) + + jacp = jax.vmap( + lambda a, b: a[3:] + jp.cross(a[:3], offset) + jp.cross(b[:3], pvel_lin) + )(cdof_dot, cdof) + jacp = jax.vmap(jp.multiply)(jacp, mask) + jacr = jax.vmap(jp.multiply)(cdof_dot[:, :3], mask) # pytype: disable=attribute-error + + return jacp, jacr + + def apply_ft( m: Model, d: Data, diff --git a/mjx/mujoco/mjx/_src/support_test.py b/mjx/mujoco/mjx/_src/support_test.py index 3303aa62..6268f14b 100644 --- a/mjx/mujoco/mjx/_src/support_test.py +++ b/mjx/mujoco/mjx/_src/support_test.py @@ -379,6 +379,19 @@ class SupportTest(parameterized.TestCase): ' 15297169659434471387 != 2785811613804955188', ) + # what happens when we bind to an actuator that was removed? + bygone_actuators = [] + for act in s.actuators: + bygone_actuators.append(act) + s.delete(act) + m = s.compile() + d = mujoco.MjData(m) + mx = mjx.put_model(m) + dx = mjx.put_data(m, d) + dx = mjx.step(mx, dx) + with self.assertRaisesRegex(KeyError, 'invalid id: -1'): + dx.bind(mx, bygone_actuators) + _CONTACTS = """ diff --git a/mjx/mujoco/mjx/_src/types.py b/mjx/mujoco/mjx/_src/types.py index a60a1dd6..57d68208 100644 --- a/mjx/mujoco/mjx/_src/types.py +++ b/mjx/mujoco/mjx/_src/types.py @@ -24,8 +24,8 @@ from mujoco.mjx._src.dataclasses import PyTreeNode # pylint: disable=g-importin import numpy as np -class BackendImpl(enum.Enum): - """Backend implementation to use.""" +class Impl(enum.Enum): + """Implementation to use.""" C = 'c' JAX = 'jax' @@ -34,7 +34,7 @@ class BackendImpl(enum.Enum): @classmethod def _missing_(cls, value): # This method is called only when lookup by value fails - # (e.g., BackendImpl('JAX') fails initially because 'JAX' != 'jax') + # (e.g., Impl('JAX') fails initially because 'JAX' != 'jax') if not isinstance(value, str): return None for member in cls: @@ -871,10 +871,10 @@ class Model(PyTreeNode): _impl: Union[ModelC, ModelJAX] @property - def backend_impl(self) -> BackendImpl: + def impl(self) -> Impl: return { - ModelC: BackendImpl.C, - ModelJAX: BackendImpl.JAX, + ModelC: Impl.C, + ModelJAX: Impl.JAX, }[type(self._impl)] def __getattr__(self, name: str): @@ -1119,10 +1119,10 @@ class Data(PyTreeNode): _impl: Union[DataC, DataJAX] @property - def backend_impl(self) -> BackendImpl: + def impl(self) -> Impl: return { - DataC: BackendImpl.C, - DataJAX: BackendImpl.JAX, + DataC: Impl.C, + DataJAX: Impl.JAX, }[type(self._impl)] def __getattr__(self, name: str): diff --git a/mjx/mujoco/mjx/test_data/tendon/armature.xml b/mjx/mujoco/mjx/test_data/tendon/armature.xml new file mode 100644 index 00000000..0b416feb --- /dev/null +++ b/mjx/mujoco/mjx/test_data/tendon/armature.xml @@ -0,0 +1,24 @@ + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/mjx/pyproject.toml b/mjx/pyproject.toml index ee03aa0a..5bcb53b0 100644 --- a/mjx/pyproject.toml +++ b/mjx/pyproject.toml @@ -4,7 +4,7 @@ build-backend = "setuptools.build_meta" [project] name="mujoco-mjx" -version = "3.3.3" +version = "3.3.4" authors = [ {name = "Google DeepMind", email = "mujoco@deepmind.com"}, ] @@ -30,7 +30,7 @@ dependencies = [ "etils[epath]", "jax", "jaxlib", - "mujoco>=3.3.3.dev0", + "mujoco>=3.3.4.dev0", "scipy", "trimesh", ] @@ -41,9 +41,9 @@ mjx-viewer = "mujoco.mjx.viewer:main" [project.urls] Homepage = "https://github.com/google-deepmind/mujoco/tree/main/mjx" -Documentation = "https://mujoco.readthedocs.io/en/3.3.3" +Documentation = "https://mujoco.readthedocs.io/en/3.3.4" Repository = "https://github.com/google-deepmind/mujoco/tree/main/mjx" -Changelog = "https://mujoco.readthedocs.io/en/3.3.3/changelog.html" +Changelog = "https://mujoco.readthedocs.io/en/3.3.4/changelog.html" [tool.isort] force_single_line = true diff --git a/model/flex/pulley.xml b/model/flex/pulley.xml index 5e07430d..38223546 100644 --- a/model/flex/pulley.xml +++ b/model/flex/pulley.xml @@ -22,6 +22,12 @@ + + + + + @@ -30,12 +36,12 @@ - + - + diff --git a/model/plugin/sdf/octree.xml b/model/plugin/sdf/octree.xml new file mode 100644 index 00000000..63bcf8df --- /dev/null +++ b/model/plugin/sdf/octree.xml @@ -0,0 +1,31 @@ + + + + + + + + + + diff --git a/plugin/sensor/touch_stress.cc b/plugin/sensor/touch_stress.cc index ba7b1d4c..7ec924f7 100644 --- a/plugin/sensor/touch_stress.cc +++ b/plugin/sensor/touch_stress.cc @@ -213,14 +213,21 @@ TouchStress::TouchStress(const mjModel* m, mjData* d, int instance, // Get parent weld id. int site_id = m->sensor_objid[id_]; - int parent_body = m->body_weldid[m->site_bodyid[site_id]]; + int parent_body = m->site_bodyid[site_id]; parent_weld_ = m->body_weldid[parent_body]; // Get geom id. - if (m->body_geomnum[parent_body] != 1) { - mju_error("Touch sensor must be attached to a body with exactly one geom"); + int collision_geoms = 0; + for (int i = 0; i < m->body_geomnum[parent_body]; ++i) { + int geom_id = m->body_geomadr[parent_body]+i; + if (m->geom_contype[geom_id] || m->geom_conaffinity[geom_id]) { + collision_geoms++; + geom_id_ = geom_id; + } + } + if (collision_geoms == 0) { + mju_error("Touch sensor requires a body with at least one collision geom"); } - geom_id_ = m->body_geomadr[parent_body]; // Create bin edges. x_edges_.assign(size[0] + 1, 0); @@ -236,7 +243,7 @@ TouchStress::TouchStress(const mjModel* m, mjData* d, int instance, mjtNum aer[3]; aer[0] = 0.5*(x_edges_[i+1]+x_edges_[i]); aer[1] = 0.5*(y_edges_[j+1]+y_edges_[j]); - aer[2] = m->geom_size[3*geom_id_]; + aer[2] = m->geom_rbound[geom_id_]; SphericalToCartesian(aer, pos_.data() + 3 * (i * size[1] + j)); dist_[i*size[1]+j] = mju_abs(aer[2]); TangentFrame(aer, mat_.data() + 9 * (i * size[1] + j)); @@ -302,6 +309,11 @@ void TouchStress::Compute(const mjModel* m, mjData* d, int instance) { geomtype[0] = (mjtGeom)m->geom_type[geom]; } + // Skip mesh geoms. + if (geomtype[0] == mjGEOM_MESH) { + continue; + } + // Set SDF parameters. mjSDF geom_sdf; geom_sdf.id = &sdf_instance[0]; diff --git a/python/mjspec.ipynb b/python/mjspec.ipynb index ff4b997e..2b98508c 100644 --- a/python/mjspec.ipynb +++ b/python/mjspec.ipynb @@ -1827,7 +1827,7 @@ "\n", "# Remove all bodies in the list from the spec\n", "for body in delete_list:\n", - " spec.detach_body(body)\n", + " spec.delete(body)\n", "\n", "# # Add another humanoid\n", "spec_humanoid = mj.MjSpec.from_file(humanoid_file)\n", @@ -1900,8 +1900,8 @@ "shoulder_right = torso.add_frame(pos=arm_right.pos)\n", "\n", "# Remove the arms\n", - "spec.detach_body(arm_left)\n", - "spec.detach_body(arm_right)\n", + "spec.delete(arm_left)\n", + "spec.delete(arm_right)\n", "\n", "# Add new legs\n", "shoulder_left.attach_body(leg_left, 'shoulder', 'left')\n", @@ -1942,7 +1942,7 @@ "arm_right = spec.body('upper_arm_right')\n", "torso = spec.body('torso')\n", "shoulder_right = torso.add_frame(pos=arm_right.pos, quat=[0, 0.8509035, 0, 0.525322])\n", - "spec.detach_body(arm_right)\n", + "spec.delete(arm_right)\n", "\n", "# Attach Franka arm to humanoid\n", "franka_arm = franka.body('fr3_link2')\n", @@ -2000,7 +2000,7 @@ "\n", "# Delete all key frames to avoid name conflicts\n", "while humanoid.keys:\n", - " humanoid.keys[-1].delete()\n", + " humanoid.delete(keys[-1])\n", "\n", "# Create a grid of humanoids by attaching humanoid to spec multiple times\n", "for i in range(4):\n", @@ -2335,7 +2335,7 @@ "fly = mj.MjSpec.from_file(fly_file)\n", "# Remove lights from fly so they are not duplicated in line-up\n", "for light in fly.lights:\n", - " light.delete()\n", + " fly.delete(light)\n", "\n", "small_fly = scale_spec(fly, 1.25)\n", "large_fly = scale_spec(fly, 2)\n", diff --git a/python/mujoco/CMakeLists.txt b/python/mujoco/CMakeLists.txt index b1aec9ed..87441234 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.3.3.dylib + ${MUJOCO_FRAMEWORK}/mujoco.framework/Versions/A/libmujoco.3.3.4.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.3.3 HINTS ${MUJOCO_LIBRARY_DIR} REQUIRED + MUJOCO_LIBRARY mujoco mujoco.3.3.4 HINTS ${MUJOCO_LIBRARY_DIR} REQUIRED ) find_path(MUJOCO_INCLUDE mujoco/mujoco.h HINTS ${MUJOCO_INCLUDE_DIR} REQUIRED) message("MuJoCo is at ${MUJOCO_LIBRARY}") @@ -140,7 +140,7 @@ findorfetch( GIT_REPO https://github.com/abseil/abseil-cpp GIT_TAG - d9e4955c65cd4367dd6bf46f4ccb8cd3d100540b # LTS 20250127.1 + bc257a88f7c1939f24e0379f14a3589e926c950c # LTS 20250512.0 TARGETS ${MUJOCO_PYTHON_ABSL_TARGETS} EXCLUDE_FROM_ALL @@ -173,7 +173,7 @@ findorfetch( GIT_REPO https://gitlab.com/libeigen/eigen GIT_TAG - 464c1d097891a1462ab28bf8bb763c1683883892 + d0b490ee091629068e0c11953419eb089f9e6bb2 TARGETS Eigen3::Eigen EXCLUDE_FROM_ALL diff --git a/python/mujoco/introspect/functions.py b/python/mujoco/introspect/functions.py index 1c4877b9..fe0742df 100644 --- a/python/mujoco/introspect/functions.py +++ b/python/mujoco/introspect/functions.py @@ -8987,46 +8987,6 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ ), doc='Attach child to a parent, return the attached element if success or NULL otherwise.', # pylint: disable=line-too-long )), - ('mjs_detachBody', - FunctionDecl( - name='mjs_detachBody', - return_type=ValueType(name='int'), - parameters=( - FunctionParameterDecl( - name='s', - type=PointerType( - inner_type=ValueType(name='mjSpec'), - ), - ), - FunctionParameterDecl( - name='b', - type=PointerType( - inner_type=ValueType(name='mjsBody'), - ), - ), - ), - doc='Delete body and descendants from mjSpec, remove all references, return 0 on success.', # pylint: disable=line-too-long - )), - ('mjs_detachDefault', - FunctionDecl( - name='mjs_detachDefault', - return_type=ValueType(name='int'), - parameters=( - FunctionParameterDecl( - name='s', - type=PointerType( - inner_type=ValueType(name='mjSpec'), - ), - ), - FunctionParameterDecl( - name='d', - type=PointerType( - inner_type=ValueType(name='mjsDefault'), - ), - ), - ), - doc='Delete default class and descendants from mjSpec, remove all references, return 0 on success.', # pylint: disable=line-too-long - )), ('mjs_addBody', FunctionDecl( name='mjs_addBody', @@ -9202,6 +9162,12 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ name='mjs_delete', return_type=ValueType(name='int'), parameters=( + FunctionParameterDecl( + name='spec', + type=PointerType( + inner_type=ValueType(name='mjSpec'), + ), + ), FunctionParameterDecl( name='element', type=PointerType( @@ -9209,7 +9175,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ ), ), ), - doc='Delete object corresponding to the given element, return 0 on success.', # pylint: disable=line-too-long + doc='Remove object corresponding to the given element, return 0 on success.', # pylint: disable=line-too-long )), ('mjs_addActuator', FunctionDecl( @@ -9551,6 +9517,266 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ ), doc='Add default.', )), + ('mjs_setToMotor', + FunctionDecl( + name='mjs_setToMotor', + return_type=PointerType( + inner_type=ValueType(name='char', is_const=True), + ), + parameters=( + FunctionParameterDecl( + name='actuator', + type=PointerType( + inner_type=ValueType(name='mjsActuator'), + ), + ), + ), + doc='Set actuator to motor, return error if any.', + )), + ('mjs_setToPosition', + FunctionDecl( + name='mjs_setToPosition', + return_type=PointerType( + inner_type=ValueType(name='char', is_const=True), + ), + parameters=( + FunctionParameterDecl( + name='actuator', + type=PointerType( + inner_type=ValueType(name='mjsActuator'), + ), + ), + FunctionParameterDecl( + name='kp', + type=ValueType(name='double'), + ), + FunctionParameterDecl( + name='kv', + type=ArrayType( + inner_type=ValueType(name='double'), + extents=(1,), + ), + ), + FunctionParameterDecl( + name='dampratio', + type=ArrayType( + inner_type=ValueType(name='double'), + extents=(1,), + ), + ), + FunctionParameterDecl( + name='timeconst', + type=ArrayType( + inner_type=ValueType(name='double'), + extents=(1,), + ), + ), + FunctionParameterDecl( + name='inheritrange', + type=ValueType(name='double'), + ), + ), + doc='Set actuator to position, return error if any.', + )), + ('mjs_setToIntVelocity', + FunctionDecl( + name='mjs_setToIntVelocity', + return_type=PointerType( + inner_type=ValueType(name='char', is_const=True), + ), + parameters=( + FunctionParameterDecl( + name='actuator', + type=PointerType( + inner_type=ValueType(name='mjsActuator'), + ), + ), + FunctionParameterDecl( + name='kp', + type=ValueType(name='double'), + ), + FunctionParameterDecl( + name='kv', + type=ArrayType( + inner_type=ValueType(name='double'), + extents=(1,), + ), + ), + FunctionParameterDecl( + name='dampratio', + type=ArrayType( + inner_type=ValueType(name='double'), + extents=(1,), + ), + ), + FunctionParameterDecl( + name='timeconst', + type=ArrayType( + inner_type=ValueType(name='double'), + extents=(1,), + ), + ), + FunctionParameterDecl( + name='inheritrange', + type=ValueType(name='double'), + ), + ), + doc='Set actuator to integrated velocity, return error if any.', + )), + ('mjs_setToVelocity', + FunctionDecl( + name='mjs_setToVelocity', + return_type=PointerType( + inner_type=ValueType(name='char', is_const=True), + ), + parameters=( + FunctionParameterDecl( + name='actuator', + type=PointerType( + inner_type=ValueType(name='mjsActuator'), + ), + ), + FunctionParameterDecl( + name='kv', + type=ValueType(name='double'), + ), + ), + doc='Set actuator to velocity servo, return error if any.', + )), + ('mjs_setToDamper', + FunctionDecl( + name='mjs_setToDamper', + return_type=PointerType( + inner_type=ValueType(name='char', is_const=True), + ), + parameters=( + FunctionParameterDecl( + name='actuator', + type=PointerType( + inner_type=ValueType(name='mjsActuator'), + ), + ), + FunctionParameterDecl( + name='kv', + type=ValueType(name='double'), + ), + ), + doc='Set actuator to activate damper, return error if any.', + )), + ('mjs_setToCylinder', + FunctionDecl( + name='mjs_setToCylinder', + return_type=PointerType( + inner_type=ValueType(name='char', is_const=True), + ), + parameters=( + FunctionParameterDecl( + name='actuator', + type=PointerType( + inner_type=ValueType(name='mjsActuator'), + ), + ), + FunctionParameterDecl( + name='timeconst', + type=ValueType(name='double'), + ), + FunctionParameterDecl( + name='bias', + type=ValueType(name='double'), + ), + FunctionParameterDecl( + name='area', + type=ValueType(name='double'), + ), + FunctionParameterDecl( + name='diameter', + type=ValueType(name='double'), + ), + ), + doc='Set actuator to hydraulic or pneumatic cylinder, return error if any.', # pylint: disable=line-too-long + )), + ('mjs_setToMuscle', + FunctionDecl( + name='mjs_setToMuscle', + return_type=PointerType( + inner_type=ValueType(name='char', is_const=True), + ), + parameters=( + FunctionParameterDecl( + name='actuator', + type=PointerType( + inner_type=ValueType(name='mjsActuator'), + ), + ), + FunctionParameterDecl( + name='timeconst', + type=ArrayType( + inner_type=ValueType(name='double'), + extents=(2,), + ), + ), + FunctionParameterDecl( + name='tausmooth', + type=ValueType(name='double'), + ), + FunctionParameterDecl( + name='range', + type=ArrayType( + inner_type=ValueType(name='double'), + extents=(2,), + ), + ), + FunctionParameterDecl( + name='force', + type=ValueType(name='double'), + ), + FunctionParameterDecl( + name='scale', + type=ValueType(name='double'), + ), + FunctionParameterDecl( + name='lmin', + type=ValueType(name='double'), + ), + FunctionParameterDecl( + name='lmax', + type=ValueType(name='double'), + ), + FunctionParameterDecl( + name='vmax', + type=ValueType(name='double'), + ), + FunctionParameterDecl( + name='fpmax', + type=ValueType(name='double'), + ), + FunctionParameterDecl( + name='fvmax', + type=ValueType(name='double'), + ), + ), + doc='Set actuator to muscle, return error if any.a', + )), + ('mjs_setToAdhesion', + FunctionDecl( + name='mjs_setToAdhesion', + return_type=PointerType( + inner_type=ValueType(name='char', is_const=True), + ), + parameters=( + FunctionParameterDecl( + name='actuator', + type=PointerType( + inner_type=ValueType(name='mjsActuator'), + ), + ), + FunctionParameterDecl( + name='gain', + type=ValueType(name='double'), + ), + ), + doc='Set actuator to active adhesion, return error if any.', + )), ('mjs_addMesh', FunctionDecl( name='mjs_addMesh', diff --git a/python/mujoco/introspect/structs.py b/python/mujoco/introspect/structs.py index 49b8752b..a6577b7e 100644 --- a/python/mujoco/introspect/structs.py +++ b/python/mujoco/introspect/structs.py @@ -278,6 +278,11 @@ STRUCTS: Mapping[str, StructDecl] = dict([ name='global', type=AnonymousStructDecl( fields=( + StructFieldDecl( + name='cameraid', + type=ValueType(name='int'), + doc='initial camera id (-1: free)', + ), StructFieldDecl( name='orthographic', type=ValueType(name='int'), @@ -868,6 +873,11 @@ STRUCTS: Mapping[str, StructDecl] = dict([ type=ValueType(name='int'), doc='number of dynamic bounding volumes (aabb stored in mjData)', # pylint: disable=line-too-long ), + StructFieldDecl( + name='noct', + type=ValueType(name='int'), + doc='number of total octree cells in all meshes', + ), StructFieldDecl( name='njnt', type=ValueType(name='int'), @@ -1555,6 +1565,30 @@ STRUCTS: Mapping[str, StructDecl] = dict([ doc='local bounding box (center, size)', array_extent=('nbvhstatic', 6), ), + StructFieldDecl( + name='oct_depth', + type=PointerType( + inner_type=ValueType(name='int'), + ), + doc='depth in the octree', + array_extent=('noct',), + ), + StructFieldDecl( + name='oct_child', + type=PointerType( + inner_type=ValueType(name='int'), + ), + doc='children of octree node', + array_extent=('noct', 8), + ), + StructFieldDecl( + name='oct_aabb', + type=PointerType( + inner_type=ValueType(name='mjtNum'), + ), + doc='octree node bounding box (center, size)', + array_extent=('noct', 6), + ), StructFieldDecl( name='jnt_type', type=PointerType( @@ -2915,6 +2949,22 @@ STRUCTS: Mapping[str, StructDecl] = dict([ doc='number of bvh', array_extent=('nmesh',), ), + StructFieldDecl( + name='mesh_octadr', + type=PointerType( + inner_type=ValueType(name='int'), + ), + doc='address of octree root', + array_extent=('nmesh',), + ), + StructFieldDecl( + name='mesh_octnum', + type=PointerType( + inner_type=ValueType(name='int'), + ), + doc='number of octree nodes', + array_extent=('nmesh',), + ), StructFieldDecl( name='mesh_normaladr', type=PointerType( @@ -6868,6 +6918,11 @@ STRUCTS: Mapping[str, StructDecl] = dict([ type=ValueType(name='int'), doc='depth of the bounding volume hierarchy to be visualized', ), + StructFieldDecl( + name='oct_depth', + type=ValueType(name='int'), + doc='depth of the octree to be visualized', + ), StructFieldDecl( name='flex_layer', type=ValueType(name='int'), diff --git a/python/mujoco/mjpython/Info.plist b/python/mujoco/mjpython/Info.plist index 475027c5..0838e2bd 100644 --- a/python/mujoco/mjpython/Info.plist +++ b/python/mujoco/mjpython/Info.plist @@ -7,13 +7,13 @@ CFBundleIdentifier org.mujoco.mjpython CFBundleVersion - 3.3.3 + 3.3.4 CFBundleGetInfoString - 3.3.3 + 3.3.4 CFBundleLongVersionString - 3.3.3 + 3.3.4 CFBundleShortVersionString - 3.3.3 + 3.3.4 CFBundleExecutable mjpython CFBundleIconFile diff --git a/python/mujoco/simulate.cc b/python/mujoco/simulate.cc index a62d1b6b..f44bd9bd 100644 --- a/python/mujoco/simulate.cc +++ b/python/mujoco/simulate.cc @@ -336,6 +336,7 @@ PYBIND11_MODULE(_simulate, pymodule) { CallIfNotNull(&mujoco::Simulate::LoadMessageClear), py::call_guard()) .def("sync", CallIfNotNull(&mujoco::Simulate::Sync), + py::arg("state_only") = false, py::call_guard()) .def("add_to_history", CallIfNotNull(&mujoco::Simulate::AddToHistory), py::call_guard()) diff --git a/python/mujoco/specs.cc b/python/mujoco/specs.cc index 51846d2a..2b477c04 100644 --- a/python/mujoco/specs.cc +++ b/python/mujoco/specs.cc @@ -396,8 +396,8 @@ PYBIND11_MODULE(_specs, m) { return mjs_addDefault(spec->ptr, classname.c_str(), parent); }, py::return_value_policy::reference_internal); - mjSpec.def("detach_default", [](MjSpec& self, raw::MjsDefault& def) { - if (mjs_detachDefault(self.ptr, &def) != 0) { + mjSpec.def("delete", [](MjSpec& self, raw::MjsDefault& def) { + if (mjs_delete(self.ptr, def.element) != 0) { throw pybind11::value_error(mjs_getError(self.ptr)); } }); @@ -407,8 +407,8 @@ PYBIND11_MODULE(_specs, m) { return mjs_getSpecDefault(self.ptr); }, py::return_value_policy::reference_internal); - mjSpec.def("detach_body", [](MjSpec& self, raw::MjsBody& body) { - mjs_detachBody(self.ptr, &body); + mjSpec.def("delete", [](MjSpec& self, raw::MjsBody& body) { + mjs_delete(self.ptr, body.element); }); mjSpec.def( "attach", @@ -759,7 +759,9 @@ PYBIND11_MODULE(_specs, m) { // ============================= MJSFRAME ==================================== - mjsFrame.def("delete", [](raw::MjsFrame& self) { mjs_delete(self.element); }); + mjSpec.def("delete", [](MjSpec& self, raw::MjsFrame& obj) { + mjs_delete(self.ptr, obj.element); + }); mjsFrame.def("set_frame", [](raw::MjsFrame& self, raw::MjsFrame& frame) { if (mjs_setFrame(self.element, &frame) != 0) { throw pybind11::value_error(mjs_getError(mjs_getSpec(self.element))); @@ -796,7 +798,9 @@ PYBIND11_MODULE(_specs, m) { py::return_value_policy::reference_internal); // ============================= MJSGEOM ===================================== - mjsGeom.def("delete", [](raw::MjsGeom& self) { mjs_delete(self.element); }); + mjSpec.def("delete", [](MjSpec& self, raw::MjsGeom& obj) { + mjs_delete(self.ptr, obj.element); + }); mjsGeom.def("set_frame", [](raw::MjsGeom& self, raw::MjsFrame& frame) { if (mjs_setFrame(self.element, &frame) != 0) { throw pybind11::value_error(mjs_getError(mjs_getSpec(self.element))); @@ -824,7 +828,9 @@ PYBIND11_MODULE(_specs, m) { py::return_value_policy::reference_internal); // ============================= MJSJOINT ==================================== - mjsJoint.def("delete", [](raw::MjsJoint& self) { mjs_delete(self.element); }); + mjSpec.def("delete", [](MjSpec& self, raw::MjsJoint& obj) { + mjs_delete(self.ptr, obj.element); + }); mjsJoint.def("set_frame", [](raw::MjsJoint& self, raw::MjsFrame& frame) { if (mjs_setFrame(self.element, &frame) != 0) { throw pybind11::value_error(mjs_getError(mjs_getSpec(self.element))); @@ -852,7 +858,9 @@ PYBIND11_MODULE(_specs, m) { py::return_value_policy::reference_internal); // ============================= MJSSITE ===================================== - mjsSite.def("delete", [](raw::MjsSite& self) { mjs_delete(self.element); }); + mjSpec.def("delete", [](MjSpec& self, raw::MjsSite& obj) { + mjs_delete(self.ptr, obj.element); + }); mjsSite.def("set_frame", [](raw::MjsSite& self, raw::MjsFrame& frame) { if (mjs_setFrame(self.element, &frame) != 0) { throw pybind11::value_error(mjs_getError(mjs_getSpec(self.element))); @@ -897,8 +905,9 @@ PYBIND11_MODULE(_specs, m) { py::return_value_policy::reference_internal); // ============================= MJSCAMERA =================================== - mjsCamera.def("delete", - [](raw::MjsCamera& self) { mjs_delete(self.element); }); + mjSpec.def("delete", [](MjSpec& self, raw::MjsCamera& obj) { + mjs_delete(self.ptr, obj.element); + }); mjsCamera.def("set_frame", [](raw::MjsCamera& self, raw::MjsFrame& frame) { if (mjs_setFrame(self.element, &frame) != 0) { throw pybind11::value_error(mjs_getError(mjs_getSpec(self.element))); @@ -926,7 +935,9 @@ PYBIND11_MODULE(_specs, m) { py::return_value_policy::reference_internal); // ============================= MJSLIGHT ==================================== - mjsLight.def("delete", [](raw::MjsLight& self) { mjs_delete(self.element); }); + mjSpec.def("delete", [](MjSpec& self, raw::MjsLight& obj) { + mjs_delete(self.ptr, obj.element); + }); mjsLight.def("set_frame", [](raw::MjsLight& self, raw::MjsFrame& frame) { if (mjs_setFrame(self.element, &frame) != 0) { throw pybind11::value_error(mjs_getError(mjs_getSpec(self.element))); @@ -954,8 +965,9 @@ PYBIND11_MODULE(_specs, m) { py::return_value_policy::reference_internal); // ============================= MJSMATERIAL ================================= - mjsMaterial.def("delete", - [](raw::MjsMaterial& self) { mjs_delete(self.element); }); + mjSpec.def("delete", [](MjSpec& self, raw::MjsMaterial& obj) { + mjs_delete(self.ptr, obj.element); + }); mjsMaterial.def_property( "classname", [](raw::MjsMaterial& self) -> raw::MjsDefault* { @@ -966,7 +978,9 @@ PYBIND11_MODULE(_specs, m) { }); // ============================= MJSMESH ===================================== - mjsMesh.def("delete", [](raw::MjsMesh& self) { mjs_delete(self.element); }); + mjSpec.def("delete", [](MjSpec& self, raw::MjsMesh& obj) { + mjs_delete(self.ptr, obj.element); + }); mjsMesh.def_property( "classname", [](raw::MjsMesh& self) -> raw::MjsDefault* { @@ -977,7 +991,9 @@ PYBIND11_MODULE(_specs, m) { }); // ============================= MJSPAIR ===================================== - mjsPair.def("delete", [](raw::MjsPair& self) { mjs_delete(self.element); }); + mjSpec.def("delete", [](MjSpec& self, raw::MjsPair& obj) { + mjs_delete(self.ptr, obj.element); + }); mjsPair.def_property( "classname", [](raw::MjsPair& self) -> raw::MjsDefault* { @@ -988,8 +1004,9 @@ PYBIND11_MODULE(_specs, m) { }); // ============================= MJSEQUAL ==================================== - mjsEquality.def("delete", - [](raw::MjsEquality& self) { mjs_delete(self.element); }); + mjSpec.def("delete", [](MjSpec& self, raw::MjsEquality& obj) { + mjs_delete(self.ptr, obj.element); + }); mjsEquality.def_property( "classname", [](raw::MjsEquality& self) -> raw::MjsDefault* { @@ -1000,8 +1017,9 @@ PYBIND11_MODULE(_specs, m) { }); // ============================= MJSACTUATOR ================================= - mjsActuator.def("delete", - [](raw::MjsActuator& self) { mjs_delete(self.element); }); + mjSpec.def("delete", [](MjSpec& self, raw::MjsActuator& obj) { + mjs_delete(self.ptr, obj.element); + }); mjsActuator.def_property( "classname", [](raw::MjsActuator& self) -> raw::MjsDefault* { @@ -1010,10 +1028,100 @@ PYBIND11_MODULE(_specs, m) { [](raw::MjsActuator& self, raw::MjsDefault& default_) -> void { mjs_setDefault(self.element, &default_); }); + mjsActuator.def("set_to_motor", [](raw::MjsActuator* self) { + std::string err = mjs_setToMotor(self); + if (!err.empty()) { + throw pybind11::value_error(err); + } + }); + mjsActuator.def( + "set_to_position", + [](raw::MjsActuator* self, double kp, double kv, double dampratio, + double timeconst, bool inheritrange) { + std::string err = mjs_setToPosition( + self, kp, kv == -1 ? nullptr : &kv, + dampratio == -1 ? nullptr : &dampratio, + timeconst == -1 ? nullptr : &timeconst, inheritrange); + if (!err.empty()) { + throw pybind11::value_error(err); + } + }, + py::arg("kp"), py::arg("kv") = -1, py::arg("dampratio") = -1, + py::arg("timeconst") = -1, py::arg("inheritrange") = false); + mjsActuator.def( + "set_to_intvelocity", + [](raw::MjsActuator* self, double kp, double kv, double dampratio, + double timeconst, bool inheritrange) { + std::string err = mjs_setToIntVelocity( + self, kp, kv == -1 ? nullptr : &kv, + dampratio == -1 ? nullptr : &dampratio, + timeconst == -1 ? nullptr : &timeconst, inheritrange); + if (!err.empty()) { + throw pybind11::value_error(err); + } + }, + py::arg("kp"), py::arg("kv") = -1, py::arg("dampratio") = -1, + py::arg("timeconst") = -1, py::arg("inheritrange") = false); + mjsActuator.def( + "set_to_velocity", + [](raw::MjsActuator* self, double kv) { + std::string err = mjs_setToVelocity(self, kv); + if (!err.empty()) { + throw pybind11::value_error(err); + } + }, + py::arg("kv")); + mjsActuator.def( + "set_to_damper", + [](raw::MjsActuator* self, double kv) { + std::string err = mjs_setToDamper(self, kv); + if (!err.empty()) { + throw pybind11::value_error(err); + } + }, + py::arg("kv")); + mjsActuator.def( + "set_to_cylinder", + [](raw::MjsActuator* self, double timeconst, double bias, double area, + double diameter) { + std::string err = + mjs_setToCylinder(self, timeconst, bias, area, diameter); + if (!err.empty()) { + throw pybind11::value_error(err); + } + }, + py::arg("timeconst"), py::arg("bias"), py::arg("area"), + py::arg("diameter") = -1); + mjsActuator.def( + "set_to_muscle", + [](raw::MjsActuator* self, double timeconst[2], double tausmooth, + double range[2], double force, double scale, double lmin, double lmax, + double vmax, double fpmax, double fvmax) { + std::string err = + mjs_setToMuscle(self, timeconst, tausmooth, range, force, scale, + lmin, lmax, vmax, fpmax, fvmax); + if (!err.empty()) { + throw pybind11::value_error(err); + } + }, + py::arg("timeconst") = -1, py::arg("tausmooth"), + py::arg("range") = std::array{-1, -1}, py::arg("force") = -1, + py::arg("scale") = -1, py::arg("lmin") = -1, py::arg("lmax") = -1, + py::arg("vmax") = -1, py::arg("fpmax") = -1, py::arg("fvmax") = -1); + mjsActuator.def( + "set_to_adhesion", + [](raw::MjsActuator* self, double gain) { + std::string err = mjs_setToAdhesion(self, gain); + if (!err.empty()) { + throw pybind11::value_error(err); + } + }, + py::arg("gain")); // ============================= MJSTENDON =================================== - mjsTendon.def("delete", - [](raw::MjsTendon& self) { mjs_delete(self.element); }); + mjSpec.def("delete", [](MjSpec& self, raw::MjsTendon& obj) { + mjs_delete(self.ptr, obj.element); + }); mjsTendon.def( "default", [](raw::MjsTendon& self) -> raw::MjsDefault* { @@ -1046,39 +1154,54 @@ PYBIND11_MODULE(_specs, m) { py::return_value_policy::reference_internal); // ============================= MJSSENSOR =================================== - mjsSensor.def("delete", - [](raw::MjsSensor& self) { mjs_delete(self.element); }); + mjSpec.def("delete", [](MjSpec& self, raw::MjsSensor& obj) { + mjs_delete(self.ptr, obj.element); + }); // ============================= MJSFLEX ===================================== - mjsFlex.def("delete", [](raw::MjsFlex& self) { mjs_delete(self.element); }); + mjSpec.def("delete", [](MjSpec& self, raw::MjsFlex& obj) { + mjs_delete(self.ptr, obj.element); + }); // ============================= MJSHFIELD =================================== - mjsHField.def("delete", - [](raw::MjsHField& self) { mjs_delete(self.element); }); + mjSpec.def("delete", [](MjSpec& self, raw::MjsHField& obj) { + mjs_delete(self.ptr, obj.element); + }); // ============================= MJSSKIN ===================================== - mjsSkin.def("delete", [](raw::MjsSkin& self) { mjs_delete(self.element); }); + mjSpec.def("delete", [](MjSpec& self, raw::MjsSkin& obj) { + mjs_delete(self.ptr, obj.element); + }); // ============================= MJSTEXTURE ================================== - mjsTexture.def("delete", - [](raw::MjsTexture& self) { mjs_delete(self.element); }); + mjSpec.def("delete", [](MjSpec& self, raw::MjsTexture& obj) { + mjs_delete(self.ptr, obj.element); + }); // ============================= MJSKEY ====================================== - mjsKey.def("delete", [](raw::MjsKey& self) { mjs_delete(self.element); }); + mjSpec.def("delete", [](MjSpec& self, raw::MjsKey& obj) { + mjs_delete(self.ptr, obj.element); + }); // ============================= MJSTEXT ===================================== - mjsText.def("delete", [](raw::MjsText& self) { mjs_delete(self.element); }); + mjSpec.def("delete", [](MjSpec& self, raw::MjsText& obj) { + mjs_delete(self.ptr, obj.element); + }); // ============================= MJSNUMERIC ================================== - mjsNumeric.def("delete", - [](raw::MjsNumeric& self) { mjs_delete(self.element); }); + mjSpec.def("delete", [](MjSpec& self, raw::MjsNumeric& obj) { + mjs_delete(self.ptr, obj.element); + }); // ============================= MJSEXCLUDE ================================== - mjsExclude.def("delete", - [](raw::MjsExclude& self) { mjs_delete(self.element); }); + mjSpec.def("delete", [](MjSpec& self, raw::MjsExclude& obj) { + mjs_delete(self.ptr, obj.element); + }); // ============================= MJSTUPLE ==================================== - mjsTuple.def("delete", [](raw::MjsTuple& self) { mjs_delete(self.element); }); + mjSpec.def("delete", [](MjSpec& self, raw::MjsTuple& obj) { + mjs_delete(self.ptr, obj.element); + }); // ============================= MJSPLUGIN =================================== mjsPlugin.def_property( @@ -1087,8 +1210,9 @@ PYBIND11_MODULE(_specs, m) { [](raw::MjsPlugin& self, raw::MjsPlugin* other) { self.element = other->element; }); - mjsPlugin.def("delete", - [](raw::MjsPlugin& self) { mjs_delete(self.element); }); + mjSpec.def("delete", [](MjSpec& self, raw::MjsPlugin& obj) { + mjs_delete(self.ptr, obj.element); + }); mjsPlugin.def_property( "config", [](raw::MjsPlugin& self) -> py::dict { diff --git a/python/mujoco/specs_test.py b/python/mujoco/specs_test.py index 41a2043c..7f444c73 100644 --- a/python/mujoco/specs_test.py +++ b/python/mujoco/specs_test.py @@ -604,7 +604,7 @@ class SpecsTest(absltest.TestCase): # test delete default def1 = spec.find_default('def1') - spec.detach_default(def1) + spec.delete(def1) def1 = spec.find_default('def1') self.assertIsNone(def1) @@ -838,9 +838,9 @@ class SpecsTest(absltest.TestCase): self.assertIsNotNone(site) self.assertEqual(site, spec.site('head')) - site.delete() - spec.sensors[-1].delete() - spec.sensors[-1].delete() + spec.delete(site) + spec.delete(spec.sensors[-1]) + spec.delete(spec.sensors[-1]) model = spec.compile() self.assertIsNotNone(model) @@ -861,7 +861,7 @@ class SpecsTest(absltest.TestCase): body = spec.worldbody.add_body() body.plugin = plugin - body.plugin.plugin_name = 'mujoco.elasticity.cable' + body.plugin.name = 'instance_name' body.plugin.active = True geom = body.add_geom() @@ -930,7 +930,7 @@ class SpecsTest(absltest.TestCase): """) plugin = spec.plugins[0] self.assertIsNotNone(plugin) - plugin.delete() + spec.delete(plugin) model = spec.compile() self.assertIsNotNone(model) @@ -1001,6 +1001,14 @@ class SpecsTest(absltest.TestCase): texture.data = np.zeros((2, 2, 3), dtype=np.uint8).tobytes() spec.compile() + def test_find_unnamed_texture(self): + spec = mujoco.MjSpec() + texture_file = spec.add_texture(file='file.png') + texture_name = spec.add_texture(name='name') + self.assertEqual(spec.texture('file'), texture_file) + self.assertEqual(spec.texture('name'), texture_name) + self.assertIsNone(spec.texture('none')) + def test_attach_units(self): child = mujoco.MjSpec() parent = mujoco.MjSpec() @@ -1232,5 +1240,61 @@ class SpecsTest(absltest.TestCase): self.assertGreater(spec3._address, 0) self.assertLen({spec1._address, spec2._address, spec3._address}, 3) + def test_actuator_shortname(self): + spec = mujoco.MjSpec() + actuator = spec.add_actuator( + gainprm=np.zeros((10, 1)), + dyntype=mujoco.mjtDyn.mjDYN_FILTER, + gaintype=mujoco.mjtGain.mjGAIN_AFFINE, + biastype=mujoco.mjtBias.mjBIAS_AFFINE, + ) + actuator.set_to_motor() + self.assertEqual(actuator.gainprm[0], 1) + self.assertEqual(actuator.dyntype, mujoco.mjtDyn.mjDYN_NONE) + self.assertEqual(actuator.gaintype, mujoco.mjtGain.mjGAIN_FIXED) + self.assertEqual(actuator.biastype, mujoco.mjtBias.mjBIAS_NONE) + + actuator.set_to_position(kp=2.0, kv=3.0, timeconst=4.0, inheritrange=True) + self.assertEqual(actuator.gainprm[0], 2) + self.assertEqual(actuator.biasprm[1], -2) + self.assertEqual(actuator.biasprm[2], -3) + self.assertEqual(actuator.dynprm[0], 4) + self.assertEqual(actuator.dyntype, mujoco.mjtDyn.mjDYN_FILTEREXACT) + self.assertEqual(actuator.gaintype, mujoco.mjtGain.mjGAIN_FIXED) + self.assertEqual(actuator.biastype, mujoco.mjtBias.mjBIAS_AFFINE) + self.assertEqual(actuator.inheritrange, True) + + actuator.set_to_intvelocity( + kp=2.0, kv=3.0, timeconst=4.0, inheritrange=True + ) + self.assertEqual(actuator.gainprm[0], 2) + self.assertEqual(actuator.biasprm[1], -2) + self.assertEqual(actuator.biasprm[2], -3) + self.assertEqual(actuator.dynprm[0], 4) + self.assertEqual(actuator.dyntype, mujoco.mjtDyn.mjDYN_INTEGRATOR) + self.assertEqual(actuator.gaintype, mujoco.mjtGain.mjGAIN_FIXED) + self.assertEqual(actuator.biastype, mujoco.mjtBias.mjBIAS_AFFINE) + self.assertEqual(actuator.inheritrange, True) + + actuator.set_to_velocity(kv=5.0) + self.assertEqual(actuator.gainprm[0], 5) + self.assertEqual(actuator.biasprm[2], -5) + self.assertEqual(actuator.dyntype, mujoco.mjtDyn.mjDYN_NONE) + self.assertEqual(actuator.gaintype, mujoco.mjtGain.mjGAIN_FIXED) + self.assertEqual(actuator.biastype, mujoco.mjtBias.mjBIAS_AFFINE) + + actuator.set_to_damper(kv=6.0) + self.assertEqual(actuator.gainprm[0], 0) + self.assertEqual(actuator.gainprm[2], -6) + self.assertEqual(actuator.dyntype, mujoco.mjtDyn.mjDYN_NONE) + self.assertEqual(actuator.gaintype, mujoco.mjtGain.mjGAIN_AFFINE) + self.assertEqual(actuator.biastype, mujoco.mjtBias.mjBIAS_NONE) + + actuator.set_to_adhesion(gain=7.0) + self.assertEqual(actuator.gainprm[0], 7) + self.assertEqual(actuator.dyntype, mujoco.mjtDyn.mjDYN_NONE) + self.assertEqual(actuator.gaintype, mujoco.mjtGain.mjGAIN_FIXED) + self.assertEqual(actuator.biastype, mujoco.mjtBias.mjBIAS_NONE) + if __name__ == '__main__': absltest.main() diff --git a/python/mujoco/structs.cc b/python/mujoco/structs.cc index 1b92c61a..f9421e50 100644 --- a/python/mujoco/structs.cc +++ b/python/mujoco/structs.cc @@ -175,6 +175,7 @@ PYBIND11_MODULE(_structs, m) { }); DefineStructFunctions(mjVisualGlobal); #define X(var) mjVisualGlobal.def_readwrite(#var, &raw::MjVisualGlobal::var) + X(cameraid); X(orthographic); X(fovy); X(ipd); @@ -1109,6 +1110,7 @@ This is useful for example when the MJB is not available as a file on disk.)")); X(label); X(frame); X(bvh_depth); + X(oct_depth); X(flex_layer); #undef X diff --git a/python/mujoco/viewer.py b/python/mujoco/viewer.py index c1de94ec..7140773d 100644 --- a/python/mujoco/viewer.py +++ b/python/mujoco/viewer.py @@ -230,10 +230,10 @@ class Handle: return sim.lock() return contextlib.nullcontext() - def sync(self): + def sync(self, state_only: bool = False): sim = self._get_sim() if sim is not None: - sim.sync() # locks internally + sim.sync(state_only) # locks internally def update_hfield(self, hfieldid: int): sim = self._get_sim() diff --git a/python/pyproject.toml b/python/pyproject.toml index c178ad79..1735c6c5 100644 --- a/python/pyproject.toml +++ b/python/pyproject.toml @@ -4,7 +4,7 @@ build-backend = "setuptools.build_meta" [project] name = "mujoco" -version = "3.3.3" +version = "3.3.4" authors = [ {name = "Google DeepMind", email = "mujoco@deepmind.com"}, ] @@ -35,9 +35,9 @@ dynamic = ["readme", "scripts"] [project.urls] Homepage = "https://github.com/google-deepmind/mujoco" -Documentation = "https://mujoco.readthedocs.io/en/3.3.3" +Documentation = "https://mujoco.readthedocs.io/en/3.3.4" Repository = "https://github.com/google-deepmind/mujoco" -Changelog = "https://mujoco.readthedocs.io/en/3.3.3/changelog.html" +Changelog = "https://mujoco.readthedocs.io/en/3.3.4/changelog.html" [tool.setuptools] include-package-data = false diff --git a/python/rollout.ipynb b/python/rollout.ipynb index 5df4ba50..2f7b067d 100644 --- a/python/rollout.ipynb +++ b/python/rollout.ipynb @@ -5,6 +5,7 @@ "id": "6adc68e0-a943-44ab-9af5-4bc62cc19f34", "metadata": { "editable": true, + "id": "6adc68e0-a943-44ab-9af5-4bc62cc19f34", "tags": [] }, "source": [ @@ -16,7 +17,7 @@ "\n", "This notebook describes the `rollout` module included in the MuJoCo Python library. It performs simulation \"rollouts\" with an underlying C++ function. The rollouts can be multithreaded.\n", "\n", - "Below, the usage of each argument is explained with examples. Then some examples for advanced use cases are provided. Finally, `rollout` is benchmarked against pure python and MJX. \n", + "Below, the usage of each argument is explained with examples. Then some examples for advanced use cases are provided. Finally, `rollout` is benchmarked against pure python and MJX.\n", "\n", "Note the benchmarks were designed to run on >16 thread CPU and an RTX 4090 or A100. They do not run in a reasonable amount of time on a typical free colab runtime.\n", "\n", @@ -41,6 +42,7 @@ "id": "5d8a6604-0948-4a42-a48d-249c7f0c462b", "metadata": { "editable": true, + "id": "5d8a6604-0948-4a42-a48d-249c7f0c462b", "tags": [] }, "source": [ @@ -53,6 +55,7 @@ "id": "0f9fbad1-59d0-40ac-b2b6-99f37313670f", "metadata": { "editable": true, + "id": "0f9fbad1-59d0-40ac-b2b6-99f37313670f", "tags": [ "hide-input" ] @@ -154,7 +157,9 @@ { "cell_type": "markdown", "id": "fc69d0f4", - "metadata": {}, + "metadata": { + "id": "fc69d0f4" + }, "source": [ "# Helper Functions" ] @@ -165,6 +170,7 @@ "id": "082482c7", "metadata": { "editable": true, + "id": "082482c7", "tags": [ "hide-input" ] @@ -251,7 +257,7 @@ " light.cutoff = 45\n", " light.diffuse = [0.8, 0.8, 0.8]\n", " light.dir = [0, 0, -1]\n", - " light.directional = 0\n", + " light.type = mujoco.mjtLightType.mjLIGHT_SPOT\n", " light.exponent = 10\n", " light.headlight = 0\n", " light.specular = [0.3, 0.3, 0.3]\n", @@ -267,7 +273,9 @@ { "cell_type": "markdown", "id": "c0570c2c", - "metadata": {}, + "metadata": { + "id": "c0570c2c" + }, "source": [ "# Using `rollout`\n", "\n", @@ -280,9 +288,11 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 0, "id": "849b93e5", - "metadata": {}, + "metadata": { + "id": "849b93e5" + }, "outputs": [], "source": [ "#@title Benchmarked models\n", @@ -368,7 +378,9 @@ { "cell_type": "markdown", "id": "55d171f7-541b-4441-aa18-da86d6716410", - "metadata": {}, + "metadata": { + "id": "55d171f7-541b-4441-aa18-da86d6716410" + }, "source": [ "## Usage\n", "\n", @@ -380,9 +392,11 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 0, "id": "9cd2f94a-11df-4247-986c-5a56af69a1f5", - "metadata": {}, + "metadata": { + "id": "9cd2f94a-11df-4247-986c-5a56af69a1f5" + }, "outputs": [], "source": [ "print(rollout.rollout.__doc__)" @@ -392,6 +406,7 @@ "cell_type": "markdown", "id": "b6f7a094-8352-4b07-99ee-5278e3036cd5", "metadata": { + "id": "b6f7a094-8352-4b07-99ee-5278e3036cd5", "tags": [] }, "source": [ @@ -403,9 +418,11 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 0, "id": "849af5f2-9de1-4cb9-bc3a-c9b7acf0e3fe", - "metadata": {}, + "metadata": { + "id": "849af5f2-9de1-4cb9-bc3a-c9b7acf0e3fe" + }, "outputs": [], "source": [ "nbatch = 100 # Simulate this many tops\n", @@ -437,16 +454,20 @@ { "cell_type": "markdown", "id": "aa2cf151-bf9a-4a23-b7fe-6a766979d93f", - "metadata": {}, + "metadata": { + "id": "aa2cf151-bf9a-4a23-b7fe-6a766979d93f" + }, "source": [ "Our model has an angular velocity sensor the middle of the top. Let's plot the response using the `sensordata` array that rollout returns." ] }, { "cell_type": "code", - "execution_count": null, + "execution_count": 0, "id": "957b8566-da31-410b-b385-e78241c5247a", - "metadata": {}, + "metadata": { + "id": "957b8566-da31-410b-b385-e78241c5247a" + }, "outputs": [], "source": [ "plt.figure(figsize=(12, 8))\n", @@ -462,7 +483,9 @@ { "cell_type": "markdown", "id": "58044bc1-f98c-4bbf-a703-40ba075552a0", - "metadata": {}, + "metadata": { + "id": "58044bc1-f98c-4bbf-a703-40ba075552a0" + }, "source": [ "### Example: different models\n", "100 gray tops is kind of boring. It would be better if they were colorful and different sizes!\n", @@ -474,9 +497,11 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 0, "id": "7c39e79e-8942-4fea-b306-ea0cb3c826e2", - "metadata": {}, + "metadata": { + "id": "7c39e79e-8942-4fea-b306-ea0cb3c826e2" + }, "outputs": [], "source": [ "# Make 100 tippe tops with different colors and sizes\n", @@ -544,16 +569,20 @@ { "cell_type": "markdown", "id": "cf485c08-72be-4169-89b6-9d93df8ebbe3", - "metadata": {}, + "metadata": { + "id": "cf485c08-72be-4169-89b6-9d93df8ebbe3" + }, "source": [ "Because the models are now different, the measurements of the gyro sensor are not consistent even though the initial state for each rollout was the same." ] }, { "cell_type": "code", - "execution_count": null, + "execution_count": 0, "id": "b8a5d3d4-24e7-41a1-b3bd-7b63c1812b03", - "metadata": {}, + "metadata": { + "id": "b8a5d3d4-24e7-41a1-b3bd-7b63c1812b03" + }, "outputs": [], "source": [ "plt.figure(figsize=(12, 8))\n", @@ -569,7 +598,9 @@ { "cell_type": "markdown", "id": "3841a669-6cd1-427e-a629-20a10a6e3a34", - "metadata": {}, + "metadata": { + "id": "3841a669-6cd1-427e-a629-20a10a6e3a34" + }, "source": [ "### Example: control inputs\n", "Open loop controls can be passed to `rollout` via the `control` argument. If passed, `nstep` no longer needs to be specified as it can be inferred from the size of `control`.\n", @@ -579,9 +610,11 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 0, "id": "2a184873-8d24-45da-b444-8d21f5dcd733", - "metadata": {}, + "metadata": { + "id": "2a184873-8d24-45da-b444-8d21f5dcd733" + }, "outputs": [], "source": [ "# Episode parameters.\n", @@ -632,7 +665,9 @@ { "cell_type": "markdown", "id": "4d89e4fb-7711-4d23-8ff3-eb5030fa8bf7", - "metadata": {}, + "metadata": { + "id": "4d89e4fb-7711-4d23-8ff3-eb5030fa8bf7" + }, "source": [ "`rollout`'s `control_spec` argument can be used to indicate `control` contains values for actuators, generalized forces, cartesian forces, mocap poses, and/or the activation/deactivation of equality constraints. Internally, this is managed through [mj_setState](https://mujoco.readthedocs.io/en/stable/APIreference/APIfunctions.html#mj-setstate) and `control_spec` corresponds to `mj_setState`'s `spec` argument.\n", "\n", @@ -641,9 +676,11 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 0, "id": "4b02bb61-912d-47de-a956-aadfcd4c5cd5", - "metadata": {}, + "metadata": { + "id": "4b02bb61-912d-47de-a956-aadfcd4c5cd5" + }, "outputs": [], "source": [ "xfrc_size = mujoco.mj_stateSize(humanoid_model, mujoco.mjtState.mjSTATE_XFRC_APPLIED)\n", @@ -676,7 +713,9 @@ { "cell_type": "markdown", "id": "0961c3ec-a691-4875-9a55-227a3d29c472", - "metadata": {}, + "metadata": { + "id": "0961c3ec-a691-4875-9a55-227a3d29c472" + }, "source": [ "# Advanced usage" ] @@ -684,7 +723,9 @@ { "cell_type": "markdown", "id": "VfYIyXWcLKfg", - "metadata": {}, + "metadata": { + "id": "VfYIyXWcLKfg" + }, "source": [ "## skip_checks\n", "\n", @@ -706,9 +747,11 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 0, "id": "d02cc8e8-63cd-4852-ab3c-364a18025a95", - "metadata": {}, + "metadata": { + "id": "d02cc8e8-63cd-4852-ab3c-364a18025a95" + }, "outputs": [], "source": [ "nbatch = 1000\n", @@ -752,7 +795,9 @@ { "cell_type": "markdown", "id": "92627030-4726-4689-be8b-f1ba75905104", - "metadata": {}, + "metadata": { + "id": "92627030-4726-4689-be8b-f1ba75905104" + }, "source": [ "As expected, as `nstep` increases, the benefits of using skip checks fades quickly. However, at low nstep and high batch sizes, it can make a significant difference.\n", "\n", @@ -762,7 +807,9 @@ { "cell_type": "markdown", "id": "d32a77b5-24bd-4d17-80ac-15cc4d03731c", - "metadata": {}, + "metadata": { + "id": "d32a77b5-24bd-4d17-80ac-15cc4d03731c" + }, "source": [ "## Reusing threadpools (`Rollout` class)\n", "\n", @@ -773,9 +820,11 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 0, "id": "dd05bbdf-f389-4e4e-b389-d47fe976cb49", - "metadata": {}, + "metadata": { + "id": "dd05bbdf-f389-4e4e-b389-d47fe976cb49" + }, "outputs": [], "source": [ "nbatch = 100\n", @@ -813,7 +862,9 @@ { "cell_type": "markdown", "id": "9b3e14a1-71f3-430d-a3c2-1aadcf6c2671", - "metadata": {}, + "metadata": { + "id": "9b3e14a1-71f3-430d-a3c2-1aadcf6c2671" + }, "source": [ "## Reusing threadpools (`rollout` method)\n", "\n", @@ -824,9 +875,11 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 0, "id": "b6aa6801", - "metadata": {}, + "metadata": { + "id": "b6aa6801" + }, "outputs": [], "source": [ "nbatch = 1000\n", @@ -846,16 +899,20 @@ { "cell_type": "markdown", "id": "144378d3", - "metadata": {}, + "metadata": { + "id": "144378d3" + }, "source": [ "Second, if `rollout` reuses the same threadpool between calls, it is no longer safe to call `rollout` from multiple threads. For example the following is not allowed (the offending lines are commented out to avoid crashing the interpreter):" ] }, { "cell_type": "code", - "execution_count": null, + "execution_count": 0, "id": "7f46a6d8", - "metadata": {}, + "metadata": { + "id": "7f46a6d8" + }, "outputs": [], "source": [ "thread1 = threading.Thread(target=lambda: rollout.rollout(top_model, top_datas, initial_states, nstep=nstep, persistent_pool=True))\n", @@ -871,7 +928,9 @@ { "cell_type": "markdown", "id": "78c1f864-5238-4e27-a7ec-d03c45484d9a", - "metadata": {}, + "metadata": { + "id": "78c1f864-5238-4e27-a7ec-d03c45484d9a" + }, "source": [ "## chunk_size\n", "\n", @@ -882,9 +941,11 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 0, "id": "a1be8f93", - "metadata": {}, + "metadata": { + "id": "a1be8f93" + }, "outputs": [], "source": [ "nbatch = 100\n", @@ -928,7 +989,9 @@ { "cell_type": "markdown", "id": "84c746e0-5d2e-47dc-ac76-f2b6f790b7c7", - "metadata": {}, + "metadata": { + "id": "84c746e0-5d2e-47dc-ac76-f2b6f790b7c7" + }, "source": [ "## Warmstarting\n", "\n", @@ -941,9 +1004,11 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 0, "id": "d4d9f660-f83c-432e-a579-124a7ecab4fb", - "metadata": {}, + "metadata": { + "id": "d4d9f660-f83c-432e-a579-124a7ecab4fb" + }, "outputs": [], "source": [ "top_model_cg = copy.copy(top_model)\n", @@ -1004,7 +1069,9 @@ { "cell_type": "markdown", "id": "7c2cf4fa", - "metadata": {}, + "metadata": { + "id": "7c2cf4fa" + }, "source": [ "As expected, the middle animation (with warmstarting) matches the continuous rollout on the left. However, the model that did not use warmstarting diverged." ] @@ -1012,7 +1079,9 @@ { "cell_type": "markdown", "id": "7944637f", - "metadata": {}, + "metadata": { + "id": "7944637f" + }, "source": [ "# Benchmarks\n", "\n", @@ -1029,9 +1098,11 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 0, "id": "cb6355dd", - "metadata": {}, + "metadata": { + "id": "cb6355dd" + }, "outputs": [], "source": [ "def python_rollout(model, data, nbatch, nstep):\n", @@ -1043,7 +1114,9 @@ { "cell_type": "markdown", "id": "6fe4a78b", - "metadata": {}, + "metadata": { + "id": "6fe4a78b" + }, "source": [ "To run nbatch rollouts with `rollout`, we need to make an array of nbatch initial states to start the rollouts from.\n", "\n", @@ -1055,9 +1128,11 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 0, "id": "74f143e2", - "metadata": {}, + "metadata": { + "id": "74f143e2" + }, "outputs": [], "source": [ "def nthread_rollout(model, data, nbatch, nstep, nthread, rollout_):\n", @@ -1071,7 +1146,9 @@ { "cell_type": "markdown", "id": "b75dc44c", - "metadata": {}, + "metadata": { + "id": "b75dc44c" + }, "source": [ "Next, we benchmark the Python loop and `rollout` in both single threaded and multithreaded modes. The three benchmarks take about 2.5 minutes in total to run in total on an AMD 5800X3D." ] @@ -1081,7 +1158,8 @@ "execution_count": 0, "id": "0301e3ee", "metadata": { - "cellView": "form" + "cellView": "form", + "id": "0301e3ee" }, "outputs": [], "source": [ @@ -1204,16 +1282,20 @@ { "cell_type": "markdown", "id": "08fb0c12", - "metadata": {}, + "metadata": { + "id": "08fb0c12" + }, "source": [ "### Tippe Top Benchmark" ] }, { "cell_type": "code", - "execution_count": null, + "execution_count": 0, "id": "f7e54830", - "metadata": {}, + "metadata": { + "id": "f7e54830" + }, "outputs": [], "source": [ "nominal_nbatch = 256 # Batch size to use when testing different nstep\n", @@ -1232,16 +1314,20 @@ { "cell_type": "markdown", "id": "edefb26e", - "metadata": {}, + "metadata": { + "id": "edefb26e" + }, "source": [ "### Humanoid Benchmark" ] }, { "cell_type": "code", - "execution_count": null, + "execution_count": 0, "id": "c9e58c6c", - "metadata": {}, + "metadata": { + "id": "c9e58c6c" + }, "outputs": [], "source": [ "nominal_nbatch = 256 # Batch size to use when testing different nstep\n", @@ -1260,16 +1346,20 @@ { "cell_type": "markdown", "id": "468903bb", - "metadata": {}, + "metadata": { + "id": "468903bb" + }, "source": [ "### Humanoid100 Benchmark" ] }, { "cell_type": "code", - "execution_count": null, + "execution_count": 0, "id": "83d775d4", - "metadata": {}, + "metadata": { + "id": "83d775d4" + }, "outputs": [], "source": [ "nominal_nbatch = 128 # Batch size to use when testing different nstep\n", @@ -1293,7 +1383,9 @@ { "cell_type": "markdown", "id": "d1133084", - "metadata": {}, + "metadata": { + "id": "d1133084" + }, "source": [ "# MJX versus `rollout`" ] @@ -1301,7 +1393,9 @@ { "cell_type": "markdown", "id": "c1638f2d", - "metadata": {}, + "metadata": { + "id": "c1638f2d" + }, "source": [ "Next we will benchmark `rollout` and MJX using the tippe top and humanoid models (humanoid100 is not supported by MJX).\n", "\n", @@ -1315,7 +1409,8 @@ "execution_count": 0, "id": "7c86d157", "metadata": { - "cellView": "form" + "cellView": "form", + "id": "7c86d157" }, "outputs": [], "source": [ @@ -1436,16 +1531,20 @@ { "cell_type": "markdown", "id": "a2dafd2e", - "metadata": {}, + "metadata": { + "id": "a2dafd2e" + }, "source": [ "### MJX Tippe Top Benchmark" ] }, { "cell_type": "code", - "execution_count": null, + "execution_count": 0, "id": "98c580b0", - "metadata": {}, + "metadata": { + "id": "98c580b0" + }, "outputs": [], "source": [ "nominal_nbatch = 16384 # Batch size to use when testing different nstep\n", @@ -1461,16 +1560,20 @@ { "cell_type": "markdown", "id": "205da5da", - "metadata": {}, + "metadata": { + "id": "205da5da" + }, "source": [ "### MJX Humanoid Benchmark" ] }, { "cell_type": "code", - "execution_count": null, + "execution_count": 0, "id": "53166ae1", - "metadata": {}, + "metadata": { + "id": "53166ae1" + }, "outputs": [], "source": [ "nominal_nbatch = 4096 # Batch size to use when testing different nstep\n", @@ -1486,7 +1589,9 @@ { "cell_type": "markdown", "id": "fb2caa72", - "metadata": {}, + "metadata": { + "id": "fb2caa72" + }, "source": [ "### MJX Multiple Humanoids in one model\n", "\n", @@ -1499,9 +1604,11 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 0, "id": "3d6be608", - "metadata": {}, + "metadata": { + "id": "3d6be608" + }, "outputs": [], "source": [ "max_humanoids = 10\n", @@ -1549,9 +1656,11 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 0, "id": "b6c5fc2e", - "metadata": {}, + "metadata": { + "id": "b6c5fc2e" + }, "outputs": [], "source": [ "#@title Plot MJX nhumanoid benchmark\n", @@ -1584,6 +1693,16 @@ "\n", "plot_mjx_nhumanoid_benchmark(t_rollout, t_mjx, nbatch, nstep, max_humanoids)" ] + }, + { + "cell_type": "code", + "execution_count": 0, + "id": "UW0aoKXK7ALd", + "metadata": { + "id": "UW0aoKXK7ALd" + }, + "outputs": [], + "source": [] } ], "metadata": { diff --git a/sample/CMakeLists.txt b/sample/CMakeLists.txt index f9457f2c..3be0e9ec 100644 --- a/sample/CMakeLists.txt +++ b/sample/CMakeLists.txt @@ -24,7 +24,7 @@ set(MSVC_INCREMENTAL_DEFAULT ON) project( mujoco_samples - VERSION 3.3.3 + VERSION 3.3.4 DESCRIPTION "MuJoCo samples binaries" HOMEPAGE_URL "https://mujoco.org" ) diff --git a/simulate/CMakeLists.txt b/simulate/CMakeLists.txt index f2a58a08..1378b9f4 100644 --- a/simulate/CMakeLists.txt +++ b/simulate/CMakeLists.txt @@ -29,7 +29,7 @@ set(MUJOCO_DEP_VERSION_lodepng project( mujoco_simulate - VERSION 3.3.3 + VERSION 3.3.4 DESCRIPTION "MuJoCo simulate binaries" HOMEPAGE_URL "https://mujoco.org" ) @@ -183,6 +183,14 @@ if(SIMULATE_BUILD_EXECUTABLE) lodepng ) + if (MUJOCO_WITH_USD) + target_link_libraries( + simulate + mujoco::usd + mujoco::usd::mjcf + ) + endif() + target_link_options(simulate PRIVATE ${MUJOCO_SIMULATE_LINK_OPTIONS}) if(APPLE AND MUJOCO_BUILD_MACOS_FRAMEWORKS) @@ -249,6 +257,22 @@ if(SIMULATE_BUILD_EXECUTABLE) MUJOCO_ENABLE_RPATH ) + if (MUJOCO_WITH_USD) + target_compile_definitions(simulate PUBLIC SIMULATE_WITH_USD) + + # Add support to RPATH for the samples. + target_add_rpath( + TARGETS + simulate + INSTALL_DIRECTORY + "${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_BINDIR}" + LIB_DIRS + ${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}/mujocoUsd ${USD_DIR}/lib + DEPENDS + MUJOCO_ENABLE_RPATH + ) + endif() + install( TARGETS simulate EXPORT ${PROJECT_NAME} diff --git a/simulate/main.cc b/simulate/main.cc index 8d70e6e8..e3d42a03 100644 --- a/simulate/main.cc +++ b/simulate/main.cc @@ -25,6 +25,12 @@ #include #include +#if defined(SIMULATE_WITH_USD) +#include +#include +#include +#include +#endif #include #include "glfw_adapter.h" #include "simulate.h" @@ -224,13 +230,49 @@ mjModel* LoadModel(const char* file, mj::Simulate& sim) { char loadError[kErrorLength] = ""; mjModel* mnew = 0; auto load_start = mj::Simulate::Clock::now(); - if (mju::strlen_arr(filename)>4 && - !std::strncmp(filename + mju::strlen_arr(filename) - 4, ".mjb", - mju::sizeof_arr(filename) - mju::strlen_arr(filename)+4)) { + + std::string filename_str(filename); + std::string extension; + size_t dot_pos = filename_str.rfind('.'); + + if (dot_pos != std::string::npos && dot_pos < filename_str.length() - 1) { + extension = filename_str.substr(dot_pos); + } + + if (extension == ".mjb") { mnew = mj_loadModel(filename, nullptr); if (!mnew) { mju::strcpy_arr(loadError, "could not load binary model"); } +#if defined(SIMULATE_WITH_USD) + } else if (extension == ".usda" || extension == ".usd" || + extension == ".usdc" || extension == ".usdz" ) { + auto stage = pxr::UsdStage::Open(filename); + if (!stage) { + mju::strcpy_arr(loadError, "could not open USD stage"); + } else { + mjSpec* spec = mj_parseUSDStage(stage); + if (!spec) { + mju::strcpy_arr(loadError, "could not parse USD stage to mjSpec"); + } else { + mjModel* model = mj_compile(spec, nullptr); + if (!model) { + mju::strcpy_arr(loadError, + "could not compile USD parsed mjSpec to mjModel:\n"); + mju::strcat_arr(loadError, mjs_getError(spec)); + } else { + // handle compile warning + if (mjs_isWarning(spec)) { + mju::strcpy_arr( + loadError, + "warning while compiling USD parsed mjSpec to mjModel:\n"); + mju::strcat_arr(loadError, mjs_getError(spec)); + } + } + mnew = model; + } + } +#endif } else { mnew = mj_loadXML(filename, nullptr, loadError, kErrorLength); @@ -353,7 +395,7 @@ void PhysicsLoop(mj::Simulate& sim) { // requested slow-down factor double slowdown = 100 / sim.percentRealTime[sim.real_time_index]; - // misalignment condition: distance from target sim time is bigger than syncmisalign + // misalignment condition: distance from target sim time is bigger than syncMisalign bool misaligned = std::abs(Seconds(elapsedCPU).count()/slowdown - elapsedSim) > syncMisalign; @@ -495,6 +537,11 @@ int main(int argc, char** argv) { // scan for libraries in the plugin directory to load additional plugins scanPluginLibraries(); +#if defined(SIMULATE_WITH_USD) + // If USD is used, print the version. + std::printf("OpenUSD version v%d.%02d\n", PXR_MINOR_VERSION, PXR_PATCH_VERSION); +#endif + mjvCamera cam; mjv_defaultCamera(&cam); diff --git a/simulate/simulate.cc b/simulate/simulate.cc index b6a1b9e4..17ac9e82 100644 --- a/simulate/simulate.cc +++ b/simulate/simulate.cc @@ -112,6 +112,7 @@ enum { // right ui SECT_JOINT = 0, SECT_CONTROL, + SECT_EQUALITY, NSECT1 }; @@ -1127,6 +1128,36 @@ void MakeControlSection(mj::Simulate* sim) { } } +// make equality section of UI +void MakeEqualitySection(mj::Simulate* sim) { + mjuiDef defEquality[] = { + {mjITEM_SECTION, "Equality", mjPRESERVE, nullptr, "AE"}, + {mjITEM_END} + }; + mjuiDef defCheckBox[] = { + {mjITEM_CHECKBYTE, "", 2, nullptr, ""}, + {mjITEM_END} + }; + + // add section + mjui_add(&sim->ui1, defEquality); + + // add equalities, exit if UI limit reached + for (int i= 0; i < sim->m_->neq && id_->eq_active[i]; + + // set name + if (!sim->equality_names_[i].empty()) { + mju::strcpy_arr(defCheckBox[0].name, sim->equality_names_[i].c_str()); + } else { + mju::sprintf_arr(defCheckBox[0].name, "equality %d", i); + } + + mjui_add(&sim->ui1, defCheckBox); + } +} + // make model-dependent UI sections void MakeUiSections(mj::Simulate* sim, const mjModel* m, const mjData* d) { // clear model-dependent sections of UI @@ -1140,14 +1171,23 @@ void MakeUiSections(mj::Simulate* sim, const mjModel* m, const mjData* d) { MakeGroupSection(sim); MakeJointSection(sim); MakeControlSection(sim); + MakeEqualitySection(sim); } //---------------------------------- utility functions --------------------------------------------- // align and scale view void AlignAndScaleView(mj::Simulate* sim, const mjModel* m) { - // use default free camera parameters - mjv_defaultFreeCamera(m, &sim->cam); + // if the id is valid, use the initial fixed camera + if (m->vis.global.cameraid >= 0 && m->vis.global.cameraid < m->ncam) { + sim->cam.fixedcamid = m->vis.global.cameraid; + sim->cam.type = mjCAMERA_FIXED; + } + + // otherwise use default free camera + else { + mjv_defaultFreeCamera(m, &sim->cam); + } } @@ -1872,7 +1912,7 @@ Simulate::Simulate(std::unique_ptr platform_ui, //------------------------- Synchronize render and physics threads --------------------------------- // operations which require holding the mutex, prevents racing with physics thread -void Simulate::Sync() { +void Simulate::Sync(bool state_only) { MutexLock lock(this->mtx); if (!m_) { @@ -1931,6 +1971,18 @@ void Simulate::Sync() { } } + for (int i = 0; i < m_->neq; ++i) { + if (eq_active_[i] != eq_active_prev_[i]) { + d_->eq_active[i] = eq_active_[i]; + } else { + eq_active_[i] = d_->eq_active[i]; + } + if (eq_active_prev_[i] != eq_active_[i]) { + pending_.ui_update_equality = true; + eq_active_prev_[i] = eq_active_[i]; + } + } + // in passive mode, synchronize user's mjModel with changes made via the UI if (is_passive_) { // synchronize mjModel.opt @@ -2107,8 +2159,17 @@ void Simulate::Sync() { if (!is_passive_) { mjv_updateScene(m_, d_, &this->opt, &this->pert, &this->cam, mjCAT_ALL, &this->scn); } else { - mjv_copyModel(m_passive_, m_); - mjv_copyData(d_passive_, m_passive_, d_); + if (state_only) { + int state_size = mj_stateSize(m_, mjSTATE_INTEGRATION); + mjtNum* state = new mjtNum[state_size]; + mj_getState(m_, d_, state, mjSTATE_INTEGRATION); + mj_setState(m_passive_, d_passive_, state, mjSTATE_INTEGRATION); + mj_forward(m_passive_, d_passive_); + delete[] state; + } else { + mjv_copyModel(m_passive_, m_); + mjv_copyData(d_passive_, m_passive_, d_); + } // append geoms from user_scn to scratch space if (user_scn) { @@ -2261,6 +2322,12 @@ void Simulate::LoadOnRenderThread() { actuator_names_.emplace_back(this->m_->names + this->m_->name_actuatoradr[i]); } + equality_names_.clear(); + equality_names_.reserve(this->m_->neq); + for (int i = 0; i < this->m_->neq; ++i) { + equality_names_.emplace_back(this->m_->names + this->m_->name_eqadr[i]); + } + qpos_.resize(this->m_->nq); std::memcpy(qpos_.data(), this->d_->qpos, sizeof(this->d_->qpos[0]) * this->m_->nq); qpos_prev_ = qpos_; @@ -2269,6 +2336,10 @@ void Simulate::LoadOnRenderThread() { std::memcpy(ctrl_.data(), this->d_->ctrl, sizeof(this->d_->ctrl[0]) * this->m_->nu); ctrl_prev_ = ctrl_; + eq_active_.resize(this->m_->neq); + std::memcpy(eq_active_.data(), this->d_->eq_active, sizeof(this->d_->eq_active[0]) * this->m_->neq); + eq_active_prev_ = eq_active_; + // allocate history buffer: smaller of {2000 states, 100 MB} if (!this->is_passive_) { constexpr int kMaxHistoryBytes = 1e8; @@ -2518,6 +2589,13 @@ void Simulate::Render() { pending_.ui_update_ctrl = false; } + if (pending_.ui_update_equality) { + if (this->ui1_enable && this->ui1.sect[SECT_EQUALITY].state) { + mjui_update(SECT_EQUALITY, -1, &this->ui1, &this->uistate, &this->platform_ui->mjr_context()); + } + pending_.ui_update_equality = false; + } + // render scene mjr_render(rect, &this->scn, &this->platform_ui->mjr_context()); diff --git a/simulate/simulate.h b/simulate/simulate.h index 38234e4c..a949615e 100644 --- a/simulate/simulate.h +++ b/simulate/simulate.h @@ -54,9 +54,10 @@ class Simulate { std::unique_ptr platform_ui_adapter, mjvCamera* cam, mjvOption* opt, mjvPerturb* pert, bool is_passive); - // Synchronize mjModel and mjData state with UI inputs, and update - // visualization. - void Sync(); + // Synchronize state with UI inputs, and update visualization. If state_only + // is false mjData and mjModel will be updated, otherwise only the subset of + // mjData corresponding to mjSTATE_INTEGRATION will be synced. + void Sync(bool state_only = false); void UpdateHField(int hfieldid); void UpdateMesh(int meshid); @@ -122,6 +123,8 @@ class Simulate { std::vector>> actuator_ctrlrange_; std::vector actuator_names_; + std::vector equality_names_; + std::vector history_; // history buffer (nhistory x state_size) // mjModel and mjData fields that can be modified by the user through the GUI @@ -129,6 +132,8 @@ class Simulate { std::vector qpos_prev_; std::vector ctrl_; std::vector ctrl_prev_; + std::vector eq_active_; + std::vector eq_active_prev_; // in passive mode the user owns m_ and d_, these "passive" instances are // owned by Simulate, updated from the user by the Sync() method @@ -167,6 +172,7 @@ class Simulate { bool ui_update_visualization; bool ui_update_joint; bool ui_update_ctrl; + bool ui_update_equality; bool ui_remake_ctrl; } pending_ = {}; diff --git a/src/engine/engine_collision_sdf.c b/src/engine/engine_collision_sdf.c index 591c96b8..b1942970 100644 --- a/src/engine/engine_collision_sdf.c +++ b/src/engine/engine_collision_sdf.c @@ -36,6 +36,19 @@ //---------------------------- primitives sdf --------------------------------------------- +static void radialField3d(mjtNum field[3], const mjtNum a[3], const mjtNum x[3], + const mjtNum size[3]) { + field[0] = -size[0] / a[0]; + field[1] = -size[1] / a[1]; + field[2] = -size[2] / a[2]; + mju_normalize3(field); + + // flip sign if necessary + if (x[0] < 0) field[0] = -field[0]; + if (x[1] < 0) field[1] = -field[1]; + if (x[2] < 0) field[2] = -field[2]; +} + static mjtNum geomDistance(const mjModel* m, const mjData* d, const mjpPlugin* p, int i, const mjtNum x[3], mjtGeom type) { mjtNum a[3], b[3]; @@ -48,13 +61,23 @@ static mjtNum geomDistance(const mjModel* m, const mjData* d, const mjpPlugin* p case mjGEOM_SPHERE: return mju_norm3(x) - size[0]; case mjGEOM_BOX: + // compute shortest distance to box surface if outside, otherwise + // intersect with a unit gradient that linearly rotates from radial to the face normals a[0] = mju_abs(x[0]) - size[0]; a[1] = mju_abs(x[1]) - size[1]; a[2] = mju_abs(x[2]) - size[2]; - b[0] = mju_max(a[0], 0); - b[1] = mju_max(a[1], 0); - b[2] = mju_max(a[2], 0); - return mju_norm3(b) + mju_min(mju_max(a[0], mju_max(a[1], a[2])), 0); + if (a[0] >= 0 || a[1] >= 0 || a[2] >= 0) { + b[0] = mju_max(a[0], 0); + b[1] = mju_max(a[1], 0); + b[2] = mju_max(a[2], 0); + return mju_norm3(b) + mju_min(mju_max(a[0], mju_max(a[1], a[2])), 0); + } + radialField3d(b, a, x, size); + mjtNum t[3]; + t[0] = -a[0] / mju_abs(b[0]); + t[1] = -a[1] / mju_abs(b[1]); + t[2] = -a[2] / mju_abs(b[2]); + return -mju_min(t[0], mju_min(t[1], t[2])) * mju_norm3(b); case mjGEOM_CAPSULE: a[0] = x[0]; a[1] = x[1]; @@ -111,7 +134,7 @@ static void geomGradient(mjtNum gradient[3], const mjModel* m, const mjData* d, int k = a[0] > a[1] ? 0 : 1; int l = a[2] > a[k] ? 2 : k; if (a[l] < 0) { - gradient[l] = x[l] / mju_abs(x[l]); + radialField3d(gradient, a, x, size); } else { b[0] = mju_max(a[0], 0); b[1] = mju_max(a[1], 0); diff --git a/src/engine/engine_core_constraint.c b/src/engine/engine_core_constraint.c index 3e625f49..b77d546e 100644 --- a/src/engine/engine_core_constraint.c +++ b/src/engine/engine_core_constraint.c @@ -430,244 +430,246 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) { // find active equality constraints for (int i=0; i < m->neq; i++) { - if (d->eq_active[i]) { - // get constraint data - data = m->eq_data + mjNEQDATA*i; - id[0] = m->eq_obj1id[i]; - id[1] = m->eq_obj2id[i]; - size = 0; - NV = 0; - NV2 = 0; - int body_id[2]; + if (!d->eq_active[i]) { + continue; + } - // process according to type - switch ((mjtEq) m->eq_type[i]) { - case mjEQ_CONNECT: // connect bodies with ball joint - // find global points, body semantic - if (m->eq_objtype[i] == mjOBJ_BODY) { - for (int j=0; j < 2; j++) { - mju_mulMatVec3(pos[j], d->xmat + 9*id[j], data + 3*j); - mju_addTo3(pos[j], d->xpos + 3*id[j]); - body_id[j] = id[j]; - } + // get constraint data + data = m->eq_data + mjNEQDATA*i; + id[0] = m->eq_obj1id[i]; + id[1] = m->eq_obj2id[i]; + size = 0; + NV = 0; + NV2 = 0; + int body_id[2]; + + // process according to type + switch ((mjtEq) m->eq_type[i]) { + case mjEQ_CONNECT: // connect bodies with ball joint + // find global points, body semantic + if (m->eq_objtype[i] == mjOBJ_BODY) { + for (int j=0; j < 2; j++) { + mju_mulMatVec3(pos[j], d->xmat + 9*id[j], data + 3*j); + mju_addTo3(pos[j], d->xpos + 3*id[j]); + body_id[j] = id[j]; } - - // find global points, site semantic - else { - for (int j=0; j < 2; j++) { - mju_copy3(pos[j], d->site_xpos + 3*id[j]); - body_id[j] = m->site_bodyid[id[j]]; - } - } - - // compute position error - mju_sub3(cpos, pos[0], pos[1]); - - // compute Jacobian difference (opposite of contact: 0 - 1) - NV = mj_jacDifPair(m, d, chain, body_id[1], body_id[0], pos[1], pos[0], - jac[1], jac[0], jacdif, NULL, NULL, NULL); - - // copy difference into jac[0] - mju_copy(jac[0], jacdif, 3*NV); - - size = 3; - break; - - case mjEQ_WELD: // fix relative position and orientation - // find global points, body semantic - if (m->eq_objtype[i] == mjOBJ_BODY) { - for (int j=0; j < 2; j++) { - mjtNum* anchor = data + 3*(1-j); - mju_mulMatVec3(pos[j], d->xmat + 9*id[j], anchor); - mju_addTo3(pos[j], d->xpos + 3*id[j]); - body_id[j] = id[j]; - } - } - - // find global points, site semantic - else { - for (int j=0; j < 2; j++) { - mju_copy3(pos[j], d->site_xpos + 3*id[j]); - body_id[j] = m->site_bodyid[id[j]]; - } - } - - // compute position error - mju_sub3(cpos, pos[0], pos[1]); - - // get torquescale coefficient - mjtNum torquescale = data[10]; - - // compute error Jacobian (opposite of contact: 0 - 1) - NV = mj_jacDifPair(m, d, chain, body_id[1], body_id[0], pos[1], pos[0], - jac[1], jac[0], jacdif, - jac[1]+3*nv, jac[0]+3*nv, jacdif+3*nv); - - // copy difference into jac[0], compress translation:rotation if sparse - mju_copy(jac[0], jacdif, 3*NV); - mju_copy(jac[0]+3*NV, jacdif+3*nv, 3*NV); - - // orientation, body semantic - if (m->eq_objtype[i] == mjOBJ_BODY) { - // compute orientation error: neg(q1) * q0 * relpose (axis components only) - mjtNum* relpose = data+6; - mju_mulQuat(quat, d->xquat+4*id[0], relpose); // quat = q0*relpose - mju_negQuat(quat1, d->xquat+4*id[1]); // quat1 = neg(q1) - } - - // orientation, site semantic - else { - mjtNum quat_site1[4]; - mju_mulQuat(quat, d->xquat+4*body_id[0], m->site_quat+4*id[0]); - mju_mulQuat(quat_site1, d->xquat+4*body_id[1], m->site_quat+4*id[1]); - mju_negQuat(quat1, quat_site1); - } - - mju_mulQuat(quat2, quat1, quat); - mju_scl3(cpos+3, quat2+1, torquescale); // scale axis components by torquescale - - // correct rotation Jacobian: 0.5 * neg(q1) * (jac0-jac1) * q0 * relpose - for (int j=0; j < NV; j++) { - // axis = [jac0-jac1]_col(j) - axis[0] = jac[0][3*NV+j]; - axis[1] = jac[0][4*NV+j]; - axis[2] = jac[0][5*NV+j]; - - // apply formula - mju_mulQuatAxis(quat2, quat1, axis); // quat2 = neg(q1)*(jac0-jac1) - mju_mulQuat(quat3, quat2, quat); // quat3 = neg(q1)*(jac0-jac1)*q0*relpose - - // correct Jacobian - jac[0][3*NV+j] = 0.5*quat3[1]; - jac[0][4*NV+j] = 0.5*quat3[2]; - jac[0][5*NV+j] = 0.5*quat3[3]; - } - - // scale rotational jacobian by torquescale - mju_scl(jac[0]+3*NV, jac[0]+3*NV, torquescale, 3*NV); - - size = 6; - break; - - case mjEQ_JOINT: // couple joint values with cubic - case mjEQ_TENDON: // couple tendon lengths with cubic - // get scalar positions and their Jacobians - for (int j=0; j < 1+(id[1] >= 0); j++) { - if (m->eq_type[i] == mjEQ_JOINT) { // joint object - pos[j][0] = d->qpos[m->jnt_qposadr[id[j]]]; - ref[j] = m->qpos0[m->jnt_qposadr[id[j]]]; - - // make Jacobian: sparse or dense - if (issparse) { - // add first or second joint - if (j == 0) { - NV = 1; - chain[0] = m->jnt_dofadr[id[j]]; - jac[j][0] = 1; - } else { - NV2 = 1; - chain2[0] = m->jnt_dofadr[id[j]]; - jac[j][0] = 1; - } - } else { - mju_zero(jac[j], nv); - jac[j][m->jnt_dofadr[id[j]]] = 1; - } - } else { // tendon object - pos[j][0] = d->ten_length[id[j]]; - ref[j] = m->tendon_length0[id[j]]; - - // set tendon_efcadr - if (d->tendon_efcadr[id[j]] == -1) { - d->tendon_efcadr[id[j]] = i; - } - - // copy Jacobian: sparse or dense - if (issparse) { - // add first or second chain - if (j == 0) { - NV = d->ten_J_rownnz[id[j]]; - mju_copyInt(chain, d->ten_J_colind+d->ten_J_rowadr[id[j]], NV); - mju_copy(jac[j], d->ten_J+d->ten_J_rowadr[id[j]], NV); - } else { - NV2 = d->ten_J_rownnz[id[j]]; - mju_copyInt(chain2, d->ten_J_colind+d->ten_J_rowadr[id[j]], NV2); - mju_copy(jac[j], d->ten_J+d->ten_J_rowadr[id[j]], NV2); - } - } else { - mju_copy(jac[j], d->ten_J+id[j]*nv, nv); - } - } - } - - // both objects defined - if (id[1] >= 0) { - // compute position error - dif = pos[1][0] - ref[1]; - cpos[0] = pos[0][0] - ref[0] - data[0] - - (data[1]*dif + data[2]*dif*dif + data[3]*dif*dif*dif + data[4]*dif*dif*dif*dif); - - // compute derivative - deriv = data[1] + 2*data[2]*dif + 3*data[3]*dif*dif + 4*data[4]*dif*dif*dif; - - // compute Jacobian: sparse or dense - if (issparse) { - NV = mju_combineSparse(jac[0], jac[1], 1, -deriv, NV, NV2, chain, - chain2, sparse_buf, buf_ind); - } else { - mju_addToScl(jac[0], jac[1], -deriv, nv); - } - } - - // only one object defined - else { - // compute position error - cpos[0] = pos[0][0] - ref[0] - data[0]; - - // jac[0] already has the correct Jacobian - } - - size = 1; - break; - - case mjEQ_FLEX: - flex_edgeadr = m->flex_edgeadr[id[0]]; - flex_edgenum = m->flex_edgenum[id[0]]; - // add one constraint per non-rigid edge - for (int e=flex_edgeadr; e < flex_edgeadr+flex_edgenum; e++) { - // skip rigid - if (m->flexedge_rigid[e]) { - continue; - } - - // position error - cpos[0] = d->flexedge_length[e] - m->flexedge_length0[e]; - - // add constraint: sparse or dense - if (issparse) { - mj_addConstraint(m, d, d->flexedge_J+d->flexedge_J_rowadr[e], cpos, 0, 0, - 1, mjCNSTR_EQUALITY, i, - d->flexedge_J_rownnz[e], - d->flexedge_J_colind+d->flexedge_J_rowadr[e]); - } else { - mj_addConstraint(m, d, d->flexedge_J+e*nv, cpos, 0, 0, - 1, mjCNSTR_EQUALITY, i, - 0, NULL); - } - } - break; - - default: // SHOULD NOT OCCUR - mjERROR("invalid equality constraint type %d", m->eq_type[i]); } - // add constraint - if (size) { - mj_addConstraint(m, d, jac[0], cpos, 0, 0, - size, mjCNSTR_EQUALITY, i, - issparse ? NV : 0, - issparse ? chain : NULL); + // find global points, site semantic + else { + for (int j=0; j < 2; j++) { + mju_copy3(pos[j], d->site_xpos + 3*id[j]); + body_id[j] = m->site_bodyid[id[j]]; + } } + + // compute position error + mju_sub3(cpos, pos[0], pos[1]); + + // compute Jacobian difference (opposite of contact: 0 - 1) + NV = mj_jacDifPair(m, d, chain, body_id[1], body_id[0], pos[1], pos[0], + jac[1], jac[0], jacdif, NULL, NULL, NULL); + + // copy difference into jac[0] + mju_copy(jac[0], jacdif, 3*NV); + + size = 3; + break; + + case mjEQ_WELD: // fix relative position and orientation + // find global points, body semantic + if (m->eq_objtype[i] == mjOBJ_BODY) { + for (int j=0; j < 2; j++) { + mjtNum* anchor = data + 3*(1-j); + mju_mulMatVec3(pos[j], d->xmat + 9*id[j], anchor); + mju_addTo3(pos[j], d->xpos + 3*id[j]); + body_id[j] = id[j]; + } + } + + // find global points, site semantic + else { + for (int j=0; j < 2; j++) { + mju_copy3(pos[j], d->site_xpos + 3*id[j]); + body_id[j] = m->site_bodyid[id[j]]; + } + } + + // compute position error + mju_sub3(cpos, pos[0], pos[1]); + + // get torquescale coefficient + mjtNum torquescale = data[10]; + + // compute error Jacobian (opposite of contact: 0 - 1) + NV = mj_jacDifPair(m, d, chain, body_id[1], body_id[0], pos[1], pos[0], + jac[1], jac[0], jacdif, + jac[1]+3*nv, jac[0]+3*nv, jacdif+3*nv); + + // copy difference into jac[0], compress translation:rotation if sparse + mju_copy(jac[0], jacdif, 3*NV); + mju_copy(jac[0]+3*NV, jacdif+3*nv, 3*NV); + + // orientation, body semantic + if (m->eq_objtype[i] == mjOBJ_BODY) { + // compute orientation error: neg(q1) * q0 * relpose (axis components only) + mjtNum* relpose = data+6; + mju_mulQuat(quat, d->xquat+4*id[0], relpose); // quat = q0*relpose + mju_negQuat(quat1, d->xquat+4*id[1]); // quat1 = neg(q1) + } + + // orientation, site semantic + else { + mjtNum quat_site1[4]; + mju_mulQuat(quat, d->xquat+4*body_id[0], m->site_quat+4*id[0]); + mju_mulQuat(quat_site1, d->xquat+4*body_id[1], m->site_quat+4*id[1]); + mju_negQuat(quat1, quat_site1); + } + + mju_mulQuat(quat2, quat1, quat); + mju_scl3(cpos+3, quat2+1, torquescale); // scale axis components by torquescale + + // correct rotation Jacobian: 0.5 * neg(q1) * (jac0-jac1) * q0 * relpose + for (int j=0; j < NV; j++) { + // axis = [jac0-jac1]_col(j) + axis[0] = jac[0][3*NV+j]; + axis[1] = jac[0][4*NV+j]; + axis[2] = jac[0][5*NV+j]; + + // apply formula + mju_mulQuatAxis(quat2, quat1, axis); // quat2 = neg(q1)*(jac0-jac1) + mju_mulQuat(quat3, quat2, quat); // quat3 = neg(q1)*(jac0-jac1)*q0*relpose + + // correct Jacobian + jac[0][3*NV+j] = 0.5*quat3[1]; + jac[0][4*NV+j] = 0.5*quat3[2]; + jac[0][5*NV+j] = 0.5*quat3[3]; + } + + // scale rotational jacobian by torquescale + mju_scl(jac[0]+3*NV, jac[0]+3*NV, torquescale, 3*NV); + + size = 6; + break; + + case mjEQ_JOINT: // couple joint values with cubic + case mjEQ_TENDON: // couple tendon lengths with cubic + // get scalar positions and their Jacobians + for (int j=0; j < 1+(id[1] >= 0); j++) { + if (m->eq_type[i] == mjEQ_JOINT) { // joint object + pos[j][0] = d->qpos[m->jnt_qposadr[id[j]]]; + ref[j] = m->qpos0[m->jnt_qposadr[id[j]]]; + + // make Jacobian: sparse or dense + if (issparse) { + // add first or second joint + if (j == 0) { + NV = 1; + chain[0] = m->jnt_dofadr[id[j]]; + jac[j][0] = 1; + } else { + NV2 = 1; + chain2[0] = m->jnt_dofadr[id[j]]; + jac[j][0] = 1; + } + } else { + mju_zero(jac[j], nv); + jac[j][m->jnt_dofadr[id[j]]] = 1; + } + } else { // tendon object + pos[j][0] = d->ten_length[id[j]]; + ref[j] = m->tendon_length0[id[j]]; + + // set tendon_efcadr + if (d->tendon_efcadr[id[j]] == -1) { + d->tendon_efcadr[id[j]] = i; + } + + // copy Jacobian: sparse or dense + if (issparse) { + // add first or second chain + if (j == 0) { + NV = d->ten_J_rownnz[id[j]]; + mju_copyInt(chain, d->ten_J_colind+d->ten_J_rowadr[id[j]], NV); + mju_copy(jac[j], d->ten_J+d->ten_J_rowadr[id[j]], NV); + } else { + NV2 = d->ten_J_rownnz[id[j]]; + mju_copyInt(chain2, d->ten_J_colind+d->ten_J_rowadr[id[j]], NV2); + mju_copy(jac[j], d->ten_J+d->ten_J_rowadr[id[j]], NV2); + } + } else { + mju_copy(jac[j], d->ten_J+id[j]*nv, nv); + } + } + } + + // both objects defined + if (id[1] >= 0) { + // compute position error + dif = pos[1][0] - ref[1]; + cpos[0] = pos[0][0] - ref[0] - data[0] - + (data[1]*dif + data[2]*dif*dif + data[3]*dif*dif*dif + data[4]*dif*dif*dif*dif); + + // compute derivative + deriv = data[1] + 2*data[2]*dif + 3*data[3]*dif*dif + 4*data[4]*dif*dif*dif; + + // compute Jacobian: sparse or dense + if (issparse) { + NV = mju_combineSparse(jac[0], jac[1], 1, -deriv, NV, NV2, chain, + chain2, sparse_buf, buf_ind); + } else { + mju_addToScl(jac[0], jac[1], -deriv, nv); + } + } + + // only one object defined + else { + // compute position error + cpos[0] = pos[0][0] - ref[0] - data[0]; + + // jac[0] already has the correct Jacobian + } + + size = 1; + break; + + case mjEQ_FLEX: + flex_edgeadr = m->flex_edgeadr[id[0]]; + flex_edgenum = m->flex_edgenum[id[0]]; + // add one constraint per non-rigid edge + for (int e=flex_edgeadr; e < flex_edgeadr+flex_edgenum; e++) { + // skip rigid + if (m->flexedge_rigid[e]) { + continue; + } + + // position error + cpos[0] = d->flexedge_length[e] - m->flexedge_length0[e]; + + // add constraint: sparse or dense + if (issparse) { + mj_addConstraint(m, d, d->flexedge_J+d->flexedge_J_rowadr[e], cpos, 0, 0, + 1, mjCNSTR_EQUALITY, i, + d->flexedge_J_rownnz[e], + d->flexedge_J_colind+d->flexedge_J_rowadr[e]); + } else { + mj_addConstraint(m, d, d->flexedge_J+e*nv, cpos, 0, 0, + 1, mjCNSTR_EQUALITY, i, + 0, NULL); + } + } + break; + + default: // SHOULD NOT OCCUR + mjERROR("invalid equality constraint type %d", m->eq_type[i]); + } + + // add constraint + if (size) { + mj_addConstraint(m, d, jac[0], cpos, 0, 0, + size, mjCNSTR_EQUALITY, i, + issparse ? NV : 0, + issparse ? chain : NULL); } } @@ -1388,7 +1390,7 @@ static void getimpedance(const mjtNum* solimp, mjtNum pos, mjtNum margin, yP = solimp[4] * a*power(x, solimp[4]-1); } - // y(x) = 1-b*(1-x)^p is x>midpoint + // y(x) = 1-b*(1-x)^p if x>midpoint else { mjtNum b = 1/power(1-solimp[3], solimp[4]-1); y = 1-b*power(1-x, solimp[4]); @@ -2193,12 +2195,9 @@ void mj_projectConstraint(const mjModel* m, mjData* d) { } // pre-count A nonzeros (compute AR_rownnz, AR_rowadr) - mju_sqrMatTDSparseCount(d->efc_AR_rownnz, d->efc_AR_rowadr, nefc, - BT_rownnz, BT_rowadr, BT_colind, - B_rownnz, B_rowadr, B_colind, B_rowsuper, d, /*flg_upper=*/1); - - // nA = total number of nonzeros in A - d->nA = d->efc_AR_rownnz[nefc - 1] + d->efc_AR_rowadr[nefc - 1]; + d->nA = mju_sqrMatTDSparseCount(d->efc_AR_rownnz, d->efc_AR_rowadr, nefc, + BT_rownnz, BT_rowadr, BT_colind, + B_rownnz, B_rowadr, B_colind, B_rowsuper, d, /*flg_upper=*/1); // allocate A values and column indices on arena d->efc_AR = mj_arenaAllocByte(d, sizeof(mjtNum) * d->nA, _Alignof(mjtNum)); diff --git a/src/engine/engine_core_smooth.c b/src/engine/engine_core_smooth.c index d4deb5b3..14b2dfe6 100644 --- a/src/engine/engine_core_smooth.c +++ b/src/engine/engine_core_smooth.c @@ -1469,9 +1469,12 @@ void mj_transmission(const mjModel* m, mjData* d) { //-------------------------- inertia --------------------------------------------------------------- -// add tendon armature to qM +// add tendon armature to M void mj_tendonArmature(const mjModel* m, mjData* d) { int nv = m->nv, ntendon = m->ntendon, issparse = mj_isSparse(m); + const int* M_rownnz = d->M_rownnz; + const int* M_rowadr = d->M_rowadr; + const int* M_colind = d->M_colind; for (int k=0; k < ntendon; k++) { mjtNum armature = m->tendon_armature[k]; @@ -1482,11 +1485,19 @@ void mj_tendonArmature(const mjModel* m, mjData* d) { // dense if (!issparse) { + // M += armature * ten_J' * ten_J mjtNum* ten_J = d->ten_J + nv*k; - for (int i=0; i < m->nv; i++) { - int Madr = m->dof_Madr[i]; - for (int j = i; j >= 0; j = m->dof_parentid[j]) { - d->qM[Madr++] += armature * ten_J[j] * ten_J[i]; + for (int i=0; i < nv; i++) { + mjtNum ten_J_i = ten_J[i]; + if (!ten_J_i) { + continue; + } + + // M[i,:] += armature * ten_J[i] * ten_J + int start = M_rowadr[i]; + int end = start + M_rownnz[i]; + for (int adr = start; adr < end; adr++) { + d->M[adr] += armature * ten_J_i * ten_J[M_colind[adr]]; } } } @@ -1494,29 +1505,24 @@ void mj_tendonArmature(const mjModel* m, mjData* d) { // sparse else { // get sparse info for tendon k - int rowadr = d->ten_J_rowadr[k]; - int rownnz = d->ten_J_rownnz[k]; - const int* colind = d->ten_J_colind + rowadr; - mjtNum* ten_J = d->ten_J + rowadr; + int J_rowadr = d->ten_J_rowadr[k]; + int J_rownnz = d->ten_J_rownnz[k]; + const int* J_colind = d->ten_J_colind + J_rowadr; + mjtNum* ten_J = d->ten_J + J_rowadr; - // iterate forward on nonzero rows i - for (int adr_i=0; adr_i < rownnz; adr_i++) { - int i = colind[adr_i]; - int Madr = m->dof_Madr[i]; - int adr_j = rownnz - 1; - - // iterate backward on ancestors of i, find matching column j - for (int j = i; j >= 0; j = m->dof_parentid[j]) { - // reduce adr_j until column index is no bigger than j - while (colind[adr_j] > j && adr_j >= 0) { - adr_j--; - } - - // found match, update qM - if (colind[adr_j] == j) { - d->qM[Madr++] += armature * ten_J[adr_j] * ten_J[adr_i]; - } + // M += armature * ten_J' * ten_J + for (int j=0; j < J_rownnz; j++) { + mjtNum ten_J_i = ten_J[j]; + if (!ten_J_i) { + continue; } + + // M[i,:] += armature * ten_J[i] * ten_J + int i = J_colind[j]; + int M_adr = M_rowadr[i]; + mju_addToSclSparseInc(d->M + M_adr, ten_J, + M_rownnz[i], M_colind + M_adr, + J_rownnz, J_colind, armature * ten_J_i); } } } @@ -1526,22 +1532,22 @@ void mj_tendonArmature(const mjModel* m, mjData* d) { // composite rigid body inertia algorithm void mj_crb(const mjModel* m, mjData* d) { + int nv = m->nv; mjtNum buf[6]; mjtNum* crb = d->crb; - int last_body = m->nbody - 1, nv = m->nv; // crb = cinert mju_copy(crb, d->cinert, 10*m->nbody); // backward pass over bodies, accumulate composite inertias - for (int i=last_body; i > 0; i--) { + for (int i=m->nbody - 1; i > 0; i--) { if (m->body_parentid[i] > 0) { mju_addTo(crb+10*m->body_parentid[i], crb+10*i, 10); } } - // clear qM - mju_zero(d->qM, m->nM); + // clear M + mju_zero(d->M, m->nC); // dense forward pass over dofs for (int i=0; i < nv; i++) { @@ -1549,7 +1555,7 @@ void mj_crb(const mjModel* m, mjData* d) { if (m->dof_simplenum[i]) { int n = i + m->dof_simplenum[i]; for (; i < n; i++) { - d->qM[m->dof_Madr[i]] = m->dof_M0[i]; + d->M[d->M_rowadr[i]] = m->dof_M0[i]; } // finish or else fall through with next row @@ -1559,8 +1565,8 @@ void mj_crb(const mjModel* m, mjData* d) { } // init M(i,i) with armature inertia - int Madr_ij = m->dof_Madr[i]; - d->qM[Madr_ij] = m->dof_armature[i]; + int Madr_ij = d->M_rowadr[i] + d->M_rownnz[i] - 1; + d->M[Madr_ij] = m->dof_armature[i]; // precompute buf = crb_body_i * cdof_i mju_mulInertVec(buf, crb+10*m->dof_bodyid[i], d->cdof+6*i); @@ -1568,7 +1574,7 @@ void mj_crb(const mjModel* m, mjData* d) { // sparse backward pass over ancestors for (int j=i; j >= 0; j = m->dof_parentid[j]) { // M(i,j) += cdof_j * (crb_body_i * cdof_i) - d->qM[Madr_ij++] += mju_dot(d->cdof+6*j, buf, 6); + d->M[Madr_ij--] += mju_dot(d->cdof+6*j, buf, 6); } } } @@ -1579,7 +1585,7 @@ void mj_makeM(const mjModel* m, mjData* d) { TM_START; mj_crb(m, d); mj_tendonArmature(m, d); - mju_gather(d->M, d->qM, d->mapM2M, m->nC); + mju_scatter(d->qM, d->M, d->mapM2M, m->nC); TM_END(mjTIMER_POS_INERTIA); } diff --git a/src/engine/engine_forward.c b/src/engine/engine_forward.c index 16d33cae..3acaa713 100644 --- a/src/engine/engine_forward.c +++ b/src/engine/engine_forward.c @@ -173,9 +173,7 @@ void mj_fwdPosition(const mjModel* m, mjData* d) { TM_RESTART; mj_makeConstraint(m, d); - if (mjENABLED(mjENBL_ISLAND)) { - mj_island(m, d); - } + mj_island(m, d); TM_END(mjTIMER_POS_MAKE); TM_RESTART; diff --git a/src/engine/engine_inverse.c b/src/engine/engine_inverse.c index de8b9a52..b16b0d02 100644 --- a/src/engine/engine_inverse.c +++ b/src/engine/engine_inverse.c @@ -46,8 +46,8 @@ void mj_invPosition(const mjModel* m, mjData* d) { mj_tendon(m, d); TM_END(mjTIMER_POS_KINEMATICS); - mj_makeM(m, d); // timed internally (POS_INERTIA) - mj_factorM(m, d); // timed internally (POS_INERTIA) + mj_makeM(m, d); // timed internally (POS_INERTIA) + mj_factorM(m, d); // timed internally (POS_INERTIA) mj_collision(m, d); // timed internally (POS_COLLISION) @@ -226,15 +226,22 @@ void mj_inverseSkip(const mjModel* m, mjData* d, // acceleration-dependent mj_invConstraint(m, d); - mj_rne(m, d, 1, d->qfrc_inverse); + + // sum of bias forces in qfrc_inverse = centripetal + Coriolis + tendon bias + mj_rne(m, d, 0, d->qfrc_inverse); + mj_tendonBias(m, d, d->qfrc_inverse); + if (!skipsensor) { mj_sensorAcc(m, d); } - // qfrc_inverse += armature*qacc - qfrc_passive - qfrc_constraint + // compute Ma = M*qacc + mjtNum* Ma = mjSTACKALLOC(d, nv, mjtNum); + mj_mulM(m, d, Ma, d->qacc); + + // qfrc_inverse += Ma - qfrc_passive - qfrc_constraint for (int i=0; i < nv; i++) { - d->qfrc_inverse[i] += m->dof_armature[i]*d->qacc[i] - - d->qfrc_passive[i] - d->qfrc_constraint[i]; + d->qfrc_inverse[i] += Ma[i] - d->qfrc_passive[i] - d->qfrc_constraint[i]; } if (mjENABLED(mjENBL_INVDISCRETE)) { diff --git a/src/engine/engine_io.c b/src/engine/engine_io.c index 9a9ad366..0fc78434 100644 --- a/src/engine/engine_io.c +++ b/src/engine/engine_io.c @@ -183,6 +183,7 @@ static void setf4(float* rgba, float r, float g, float b, float a) { // set visual options to default values void mj_defaultVisual(mjVisual* vis) { // global + vis->global.cameraid = -1; vis->global.orthographic = 0; vis->global.fovy = 45; vis->global.ipd = 0.068; @@ -459,7 +460,7 @@ static void freeModelBuffers(mjModel* m) { // allocate and initialize mjModel structure void mj_makeModel(mjModel** dest, int nq, int nv, int nu, int na, int nbody, int nbvh, - int nbvhstatic, int nbvhdynamic, int njnt, int ngeom, int nsite, int ncam, + int nbvhstatic, int nbvhdynamic, int noct, int njnt, int ngeom, int nsite, int ncam, int nlight, int nflex, int nflexnode, int nflexvert, int nflexedge, int nflexelem, int nflexelemdata, int nflexelemedge, int nflexshelldata, int nflexevpair, int nflextexcoord, int nmesh, int nmeshvert, int nmeshnormal, int nmeshtexcoord, int nmeshface, @@ -498,6 +499,7 @@ void mj_makeModel(mjModel** dest, m->nbvh = nbvh; m->nbvhstatic = nbvhstatic; m->nbvhdynamic = nbvhdynamic; + m->noct = noct; m->njnt = njnt; m->ngeom = ngeom; m->nsite = nsite; @@ -638,7 +640,7 @@ mjModel* mj_copyModel(mjModel* dest, const mjModel* src) { if (!dest) { mj_makeModel(&dest, src->nq, src->nv, src->nu, src->na, src->nbody, src->nbvh, - src->nbvhstatic, src->nbvhdynamic, src->njnt, src->ngeom, src->nsite, + src->nbvhstatic, src->nbvhdynamic, src->noct, src->njnt, src->ngeom, src->nsite, src->ncam, src->nlight, src->nflex, src->nflexnode, src->nflexvert, src->nflexedge, src->nflexelem, src->nflexelemdata, src->nflexelemedge, src->nflexshelldata, src->nflexevpair, src->nflextexcoord, src->nmesh, src->nmeshvert, @@ -834,7 +836,7 @@ mjModel* mj_loadModelBuffer(const void* buffer, int buffer_sz) { ints[42], ints[43], ints[44], ints[45], ints[46], ints[47], ints[48], ints[49], ints[50], ints[51], ints[52], ints[53], ints[54], ints[55], ints[56], ints[57], ints[58], ints[59], ints[60], ints[61], ints[62], - ints[63], ints[64], ints[65], ints[66], ints[67], ints[68]); + ints[63], ints[64], ints[65], ints[66], ints[67], ints[68], ints[69]); if (!m || m->nbuffer != sizes[getnsize()-1]) { mju_warning("Corrupted model, wrong size parameters"); mj_deleteModel(m); @@ -1989,6 +1991,9 @@ static void _resetData(const mjModel* m, mjData* d, unsigned char debug_value) { mju_zero(d->mocap_pos, 3*m->nmocap); mju_zero(d->mocap_quat, 4*m->nmocap); + // zero out qM, special case because scattering from M skips simple body off-diagonals + mju_zero(d->qM, m->nM); + // copy qpos0 from model if (m->qpos0) { memcpy(d->qpos, m->qpos0, m->nq*sizeof(mjtNum)); diff --git a/src/engine/engine_io.h b/src/engine/engine_io.h index bdf0e2ce..d0950384 100644 --- a/src/engine/engine_io.h +++ b/src/engine/engine_io.h @@ -52,7 +52,7 @@ void mj_defaultStatistic(mjStatistic* stat); // allocate mjModel void mj_makeModel(mjModel** dest, - int nq, int nv, int nu, int na, int nbody, int nbvh, int nbvhstatic, int nbvhdynamic, + int nq, int nv, int nu, int na, int nbody, int nbvh, int nbvhstatic, int nbvhdynamic, int noct, int njnt, int ngeom, int nsite, int ncam, int nlight, int nflex, int nflexnode, int nflexvert, int nflexedge, int nflexelem, int nflexelemdata, int nflexelemedge, int nflexshelldata, int nflexevpair, int nflextexcoord, int nmesh, int nmeshvert, int nmeshnormal, diff --git a/src/engine/engine_island.c b/src/engine/engine_island.c index 05364373..7f898f8c 100644 --- a/src/engine/engine_island.c +++ b/src/engine/engine_island.c @@ -141,29 +141,6 @@ int mj_floodFill(int* island, int nr, const int* rownnz, const int* rowadr, cons -// return upper bound on number of tree-tree edges -static int countMaxEdge(const mjModel* m, const mjData* d) { - int nedge_max = 0; - nedge_max += 2*d->ncon; // contact: 2 edges - nedge_max += 2*d->ne; // equality: 2 edges - nedge_max += d->nl; // limit: 1 edges (always within same tree) - nedge_max += d->nf; // joint friction: 1 edge (always within same tree) - - // tendon limits and friction add up to tendon_num edges - for (int i=0; i < m->ntendon; i++) { - if (m->tendon_frictionloss[i]) { - nedge_max += m->tendon_num[i]; - } - if (m->tendon_limited[i]) { - nedge_max += m->tendon_num[i]; - } - } - - return nedge_max; -} - - - // return id of next tree in Jacobian row i that is different from tree, -1 if not found // start search from *index // write the index of the found tree to *index @@ -242,21 +219,27 @@ static int treeFirst(const mjModel* m, const mjData* d, int tree[2], int i) { if (efc_type == mjCNSTR_CONTACT_FRICTIONLESS || efc_type == mjCNSTR_CONTACT_PYRAMIDAL || efc_type == mjCNSTR_CONTACT_ELLIPTIC) { - tree[0] = m->body_treeid[m->geom_bodyid[d->contact[efc_id].geom[0]]]; - tree[1] = m->body_treeid[m->geom_bodyid[d->contact[efc_id].geom[1]]]; + int g1 = d->contact[efc_id].geom[0]; + int g2 = d->contact[efc_id].geom[1]; - // handle static bodies - if (tree[0] < 0) { - if (tree[1] < 0) { - mjERROR("contact %d is between two static bodies", efc_id); // SHOULD NOT OCCUR - } else { - int tmp = tree[0]; - tree[0] = tree[1]; - tree[1] = tmp; + // no shortcut for flex contacts (handled in the generic case) + if (g1 >=0 && g2 >= 0) { + tree[0] = m->body_treeid[m->geom_bodyid[g1]]; + tree[1] = m->body_treeid[m->geom_bodyid[g2]]; + + // handle static bodies + if (tree[0] < 0) { + if (tree[1] < 0) { + mjERROR("contact %d is between two static bodies", efc_id); // SHOULD NOT OCCUR + } else { + int tmp = tree[0]; + tree[0] = tree[1]; + tree[1] = tmp; + } } - } - return -1; + return -1; + } } // connect or weld constraints @@ -373,9 +356,12 @@ static int findEdges(const mjModel* m, const mjData* d, int* treenedge, int* edg int nedge = 0; for (int i=0; i < nefc; i++) { - // row i is still in the same constraint: skip + // row i is still in the same constraint: skip it, if (efc_type == d->efc_type[i] && efc_id == d->efc_id[i]) { - continue; + // unless it is a flex equality, where the tree pattern changes per dof + if (!(efc_type == mjCNSTR_EQUALITY && m->eq_type[efc_id] == mjEQ_FLEX)) { + continue; + } } efc_type = d->efc_type[i]; efc_id = d->efc_id[i]; @@ -425,21 +411,19 @@ void mj_island(const mjModel* m, mjData* d) { int nv = m->nv, nefc = d->nefc, ntree=m->ntree; // no constraints: quick return - if (!nefc || m->nflex) { // TODO: add flex support to island discovery - d->nisland = 0; - d->nidof = 0; + if (!mjENABLED(mjENBL_ISLAND) || !nefc) { + d->nisland = d->nidof = 0; return; } mj_markStack(d); - // allocate edge array - int nedge_max = countMaxEdge(m, d); - int* edge = mjSTACKALLOC(d, 2*nedge_max, int); + // allocate edge array, nJ is an upper bound + int* edge = mjSTACKALLOC(d, 2*d->nJ, int); // get tree-tree edges and rownnz counts from efc arrays int* rownnz = mjSTACKALLOC(d, ntree, int); // number of edges per tree - int nedge = findEdges(m, d, rownnz, edge, nedge_max); + int nedge = findEdges(m, d, rownnz, edge, d->nJ); // compute starting address of tree's column indices while resetting rownnz int* rowadr = mjSTACKALLOC(d, ntree, int); diff --git a/src/engine/engine_solver.c b/src/engine/engine_solver.c index 635fa6af..16286d4a 100644 --- a/src/engine/engine_solver.c +++ b/src/engine/engine_solver.c @@ -1533,15 +1533,14 @@ static void MakeHessian(mjData* d, mjCGContext* ctx) { // sparse if (ctx->is_sparse) { - // initialize Hessian rowadr, rownnz - mju_sqrMatTDSparseCount(ctx->H_rownnz, ctx->H_rowadr, nv, - ctx->J_rownnz, ctx->J_rowadr, ctx->J_colind, - ctx->JT_rownnz, ctx->JT_rowadr, ctx->JT_colind, - ctx->JT_rowsuper, d, /*flg_upper=*/0); + // initialize Hessian rowadr, rownnz; get total nonzeros + ctx->nH = mju_sqrMatTDSparseCount(ctx->H_rownnz, ctx->H_rowadr, nv, + ctx->J_rownnz, ctx->J_rowadr, ctx->J_colind, + ctx->JT_rownnz, ctx->JT_rowadr, ctx->JT_colind, + ctx->JT_rowsuper, d, /*flg_upper=*/0); - // add nC to Hessian total nonzeros (unavoidable overcounting since H_colind is still unknown) - ctx->nH = ctx->M_rowadr[nv - 1] + ctx->M_rownnz[nv - 1] + - ctx->H_rowadr[nv - 1] + ctx->H_rownnz[nv - 1]; + // add M nonzeros to Hessian total (unavoidable overcounting since H_colind is still unknown) + ctx->nH += ctx->M_rowadr[nv - 1] + ctx->M_rownnz[nv - 1]; // shift H row addresses to make room for C int shift = 0; diff --git a/src/engine/engine_support.c b/src/engine/engine_support.c index 2173879a..001fb8e3 100644 --- a/src/engine/engine_support.c +++ b/src/engine/engine_support.c @@ -41,8 +41,8 @@ //-------------------------- Constants ------------------------------------------------------------- - #define mjVERSION 333 -#define mjVERSIONSTRING "3.3.3" + #define mjVERSION 334 +#define mjVERSIONSTRING "3.3.4" // names of disable flags const char* mjDISABLESTRING[mjNDISABLE] = { diff --git a/src/engine/engine_util_sparse.c b/src/engine/engine_util_sparse.c index c9cf3bab..ad38edb7 100644 --- a/src/engine/engine_util_sparse.c +++ b/src/engine/engine_util_sparse.c @@ -636,11 +636,11 @@ void mju_superSparse(int nr, int* rowsuper, } -// precount res_rownnz and precompute res_rowadr for mju_sqrMatTDSparse -void mju_sqrMatTDSparseCount(int* res_rownnz, int* res_rowadr, int nr, - const int* rownnz, const int* rowadr, const int* colind, - const int* rownnzT, const int* rowadrT, const int* colindT, - const int* rowsuperT, mjData* d, int flg_upper) { +// precount res_rownnz and precompute res_rowadr for mju_sqrMatTDSparse, return total non-zeros +int mju_sqrMatTDSparseCount(int* res_rownnz, int* res_rowadr, int nr, + const int* rownnz, const int* rowadr, const int* colind, + const int* rownnzT, const int* rowadrT, const int* colindT, + const int* rowsuperT, mjData* d, int flg_upper) { mj_markStack(d); int* chain = mjSTACKALLOC(d, 2*nr, int); int nchain = 0; @@ -726,6 +726,8 @@ void mju_sqrMatTDSparseCount(int* res_rownnz, int* res_rowadr, int nr, } mj_freeStack(d); + + return res_rowadr[nr-1] + res_rownnz[nr-1]; } diff --git a/src/engine/engine_util_sparse.h b/src/engine/engine_util_sparse.h index f6c2f7c2..4bf82472 100644 --- a/src/engine/engine_util_sparse.h +++ b/src/engine/engine_util_sparse.h @@ -120,11 +120,11 @@ MJAPI void mju_sqrMatTDSparse_row(mjtNum* res, const mjtNum* mat, const mjtNum* const int* colindT, const int* rowsuperT, mjData* d, int* diagind); -// precount res_rownnz and precompute res_rowadr for mju_sqrMatTDSparse -MJAPI void mju_sqrMatTDSparseCount(int* res_rownnz, int* res_rowadr, int nr, - const int* rownnz, const int* rowadr, const int* colind, - const int* rownnzT, const int* rowadrT, const int* colindT, - const int* rowsuperT, mjData* d, int flg_upper); +// precount res_rownnz and precompute res_rowadr for mju_sqrMatTDSparse, return total non-zeros +MJAPI int mju_sqrMatTDSparseCount(int* res_rownnz, int* res_rowadr, int nr, + const int* rownnz, const int* rowadr, const int* colind, + const int* rownnzT, const int* rowadrT, const int* colindT, + const int* rowsuperT, mjData* d, int flg_upper); // precompute res_rowadr for mju_sqrMatTDSparse using uncompressed memory MJAPI void mju_sqrMatTDUncompressedInit(int* res_rowadr, int nc); diff --git a/src/engine/engine_vis_visualize.c b/src/engine/engine_vis_visualize.c index adcc410c..3973ecb0 100644 --- a/src/engine/engine_vis_visualize.c +++ b/src/engine/engine_vis_visualize.c @@ -763,7 +763,7 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, if (vopt->flags[mjVIS_MESHBVH]) { for (int geomid = 0; geomid < m->ngeom; geomid++) { int meshid = m->geom_dataid[geomid]; - if (meshid == -1) { + if (m->geom_type[geomid] == mjGEOM_SDF || meshid == -1) { continue; } @@ -806,6 +806,43 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, } } + // mesh octree + category = mjCAT_DECOR; + objtype = mjOBJ_UNKNOWN; + if (vopt->flags[mjVIS_MESHBVH]) { + for (int geomid = 0; geomid < m->ngeom; geomid++) { + int meshid = m->geom_dataid[geomid]; + if (m->geom_type[geomid] != mjGEOM_SDF || meshid == -1) { + continue; + } + + for (int b = 0; b < m->mesh_octnum[meshid]; b++) { + int i = b + m->mesh_octadr[meshid]; + + START + if (m->oct_depth[i] != vopt->bvh_depth) { + continue; + } + + // box color + const float* rgba = m->vis.rgba.bv; + + // get xpos, xmat, size + const mjtNum* xpos = d->geom_xpos + 3 * geomid; + const mjtNum* xmat = d->geom_xmat + 9 * geomid; + const mjtNum *size = m->oct_aabb + 6*i + 3; + + // offset xpos with aabb center (not always at geom origin) + const mjtNum *center = m->oct_aabb + 6*i; + mjtNum pos[3]; + mju_mulMatVec3(pos, xmat, center); + mju_addTo3(pos, xpos); + mjv_initGeom(thisgeom, mjGEOM_LINEBOX, size, pos, xmat, rgba); + FINISH + } + } + } + // inertia objtype = mjOBJ_BODY; if (vopt->flags[mjVIS_INERTIA]) { diff --git a/src/experimental/usd/CMakeLists.txt b/src/experimental/usd/CMakeLists.txt new file mode 100644 index 00000000..96cd9eeb --- /dev/null +++ b/src/experimental/usd/CMakeLists.txt @@ -0,0 +1,270 @@ +# Copyright 2025 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 +# +# https://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. + +# --- Global Configuration --- +# Plugin target names (used for library and plugInfo.json) +set(MJCF_PLUGIN_TARGET_NAME usdMjcf) +set(MJC_PHYSICS_PLUGIN_TARGET_NAME mjcPhysics) +set(MJ_USD_TARGET_NAME mj_usd) + +# Installation directory for USD plugins +set(MJ_USD_INSTALL_DIR_LIB ${CMAKE_INSTALL_LIBDIR}/mujocoUsd) + +# Common OpenUSD libraries +set(OPENUSD_CORE_LIBS + usd ar kind tf gf vt usdShade usdLux usdGeom usdImaging usdPhysics +) + +# We need to set the visibility to default until core type symbol visibility +# is resolved in OpenUSD https://github.com/PixarAnimationStudios/OpenUSD/issues/1475 +set(DEFAULT_CXX_VISIBILITY_PROPS + CXX_VISIBILITY_PRESET default +) + +# --- Helper Functions --- +function(configure_and_install_usd_plugin_info plugin_name source_subpath install_base_dir) + # Determine shared library prefix + if(CMAKE_SHARED_LIBRARY_PREFIX) + set(LIB_PREFIX ${CMAKE_SHARED_LIBRARY_PREFIX}) + else() + set(LIB_PREFIX "") + endif() + + set(PLUG_INFO_LIBRARY_PATH "../../${LIB_PREFIX}${plugin_name}${CMAKE_SHARED_LIBRARY_SUFFIX}") + set(OUTPUT_DIR "${CMAKE_BINARY_DIR}/${install_base_dir}/resources/${plugin_name}") + set(OUTPUT_FILE "${OUTPUT_DIR}/plugInfo.json") + + # Create the output directory if it doesn't exist + file(MAKE_DIRECTORY "${OUTPUT_DIR}") + + # Configure the plugInfo.json file to replace @PLUG_INFO_LIBRARY_PATH@ instances. + configure_file( + "${CMAKE_CURRENT_SOURCE_DIR}/${source_subpath}/plugInfo.json" + "${OUTPUT_FILE}" + @ONLY + ) + + install(FILES "${OUTPUT_FILE}" + DESTINATION "${install_base_dir}/resources/${plugin_name}" + ) +endfunction() + +# Function to apply Houdini-specific link directories and include paths +function(configure_houdini_target target_name link_scope include_scope) + set(HOUDINI_LIBS_DIR "${HOUDINI_HFS_DIR}/../Libraries") + get_filename_component(HOUDINI_LIBS_DIR "${HOUDINI_LIBS_DIR}" ABSOLUTE) # Normalize the path + + # Assume everyone using Houdini on 3.11 for now. + set(USD_MJCF_PYTHON_LIB python3.11) + set(USD_MJCF_PYTHON_LIB_NUMBER python311) # For boost library names + set(PYTHON_LIB "${HOUDINI_HFS_DIR}/Frameworks/Python.framework/Versions/3.11/Python") + set(PXR_LIB_PREFIX "pxr_") + + # Houdini-specific PXR and other libraries + set(HOUDINI_SPECIFIC_LIBS + ${PXR_LIB_PREFIX}usd + ${PXR_LIB_PREFIX}ar + ${PXR_LIB_PREFIX}kind + ${PXR_LIB_PREFIX}tf + ${PXR_LIB_PREFIX}gf + ${PXR_LIB_PREFIX}vt + ${PXR_LIB_PREFIX}sdf + ${PXR_LIB_PREFIX}usdShade + ${PXR_LIB_PREFIX}usdLux + ${PXR_LIB_PREFIX}usdGeom + ${PXR_LIB_PREFIX}usdImaging + ${PXR_LIB_PREFIX}usdPhysics + tbb + hboost_${USD_MJCF_PYTHON_LIB_NUMBER} + ${PYTHON_LIB} + ) + + target_link_directories(${target_name} ${link_scope} ${HOUDINI_LIBS_DIR}) + target_include_directories(${target_name} ${include_scope} + "${HOUDINI_HFS_DIR}/toolkit/include" + "${HOUDINI_HFS_DIR}/toolkit/include/python3.11" + ) + target_link_libraries(${target_name} ${link_scope} + ${HOUDINI_SPECIFIC_LIBS} + mujoco + tinyxml2 + ) +endfunction() + +## usdMjcf +set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE) + +# The list of RPATHs: +# 1. $ORIGIN: For libmjcPhysics.so (since it's in the same install dir) +# 2. Path to OpenUSD libraries: Typically ${USD_DIR}/lib +# 3. Path to MuJoCo library: Assuming it's in ${MUJOCO_INSTALL_DIR}/lib +if(APPLE) + set(RPATH_LIST "@loader_path") + # Add OpenUSD lib to RPATH for macOS + if(DEFINED USD_DIR AND EXISTS "${USD_DIR}/lib") + list(APPEND RPATH_LIST "${USD_DIR}/lib") + endif() + # Add MuJoCo lib to RPATH for macOS + if(DEFINED MUJOCO_INSTALL_DIR AND EXISTS "${MUJOCO_INSTALL_DIR}/lib") + list(APPEND RPATH_LIST "${MUJOCO_INSTALL_DIR}/lib") + endif() + + string(JOIN ":" CMAKE_INSTALL_RPATH "${RPATH_LIST}") # macOS uses colon for RPATH separator + message(STATUS "Final CMAKE_INSTALL_RPATH: ${CMAKE_INSTALL_RPATH}") +elseif(UNIX) + + # For adjacent libs. + set(RPATH_LIST "\$ORIGIN") + + # Add OpenUSD library directory + if(DEFINED USD_DIR AND EXISTS "${USD_DIR}/lib") + list(APPEND RPATH_LIST "${USD_DIR}/lib") + message(STATUS "Adding OpenUSD lib to RPATH: ${USD_DIR}/lib") + else() + message(WARNING "USD_DIR is not defined or ${USD_DIR}/lib does not exist. OpenUSD libraries might not be found via RPATH.") + endif() + + # Add MuJoCo library directory + if(DEFINED MUJOCO_INSTALL_DIR AND EXISTS "${MUJOCO_INSTALL_DIR}/lib") + list(APPEND RPATH_LIST "${MUJOCO_INSTALL_DIR}/lib") + message(STATUS "Adding MuJoCo lib to RPATH: ${MUJOCO_INSTALL_DIR}/lib") + elseif(DEFINED MUJOCO_INSTALL_DIR AND EXISTS "${MUJOCO_INSTALL_DIR}/bin") + list(APPEND RPATH_LIST "${MUJOCO_INSTALL_DIR}/bin") + message(STATUS "Adding MuJoCo bin to RPATH: ${MUJOCO_INSTALL_DIR}/bin") + endif() + + # Set the final RPATH for installation + string(JOIN ";" CMAKE_INSTALL_RPATH "${RPATH_LIST}") + message(STATUS "Final CMAKE_INSTALL_RPATH: ${CMAKE_INSTALL_RPATH}") +endif() + +set(CMAKE_BUILD_WITH_INSTALL_RPATH TRUE) + +add_library(${MJCF_PLUGIN_TARGET_NAME} SHARED) +add_library(mujoco::usd::mjcf ALIAS ${MJCF_PLUGIN_TARGET_NAME}) +target_sources(${MJCF_PLUGIN_TARGET_NAME} PRIVATE + plugins/mjcf/mjcf_file_format.cc + plugins/mjcf/mjcf_file_format.h + plugins/mjcf/mujoco_to_usd.cc + plugins/mjcf/mujoco_to_usd.h + plugins/mjcf/utils.cc + plugins/mjcf/utils.h +) +set_target_properties(${MJCF_PLUGIN_TARGET_NAME} PROPERTIES + OUTPUT_NAME ${MJCF_PLUGIN_TARGET_NAME} + ${DEFAULT_CXX_VISIBILITY_PROPS} +) +target_include_directories(${MJCF_PLUGIN_TARGET_NAME} PRIVATE + "${CMAKE_CURRENT_SOURCE_DIR}/plugins" +) + +## mjcPhysics + +add_library(${MJC_PHYSICS_PLUGIN_TARGET_NAME} SHARED) +target_sources(${MJC_PHYSICS_PLUGIN_TARGET_NAME} PRIVATE + mjcPhysics/actuatorAPI.cpp + mjcPhysics/collisionAPI.cpp + mjcPhysics/meshCollisionAPI.cpp + mjcPhysics/sceneAPI.cpp + mjcPhysics/siteAPI.cpp + mjcPhysics/tokens.cpp +) +set_target_properties(${MJC_PHYSICS_PLUGIN_TARGET_NAME} PROPERTIES + OUTPUT_NAME ${MJC_PHYSICS_PLUGIN_TARGET_NAME} + ${DEFAULT_CXX_VISIBILITY_PROPS} +) +target_include_directories(${MJC_PHYSICS_PLUGIN_TARGET_NAME} PUBLIC + "${CMAKE_CURRENT_SOURCE_DIR}/mjcPhysics" +) + +## mj_usd + +add_library(${MJ_USD_TARGET_NAME} SHARED) +add_library(mujoco::usd ALIAS ${MJ_USD_TARGET_NAME}) +target_sources(${MJ_USD_TARGET_NAME} PRIVATE + writer.cc + layer_sink.cc + usd_to_mjspec.cc + utils.cc +) + + +if (USD_DIR) + find_package(pxr REQUIRED) + + target_link_libraries(${MJC_PHYSICS_PLUGIN_TARGET_NAME} PRIVATE + ${OPENUSD_CORE_LIBS} + mujoco + tinyxml2 + ) + + target_link_libraries(${MJCF_PLUGIN_TARGET_NAME} PRIVATE + ${OPENUSD_CORE_LIBS} + mujoco + tinyxml2 + ${MJC_PHYSICS_PLUGIN_TARGET_NAME} + ) + + target_link_libraries(${MJ_USD_TARGET_NAME} PUBLIC + ${OPENUSD_CORE_LIBS} + mujoco + tinyxml2 + ${MJC_PHYSICS_PLUGIN_TARGET_NAME} + ) +elseif (HOUDINI_HFS_DIR) + message(STATUS "Configuring for Houdini target...") + + # In Houdini, the Houdini package we would typically use via find_package + # does not have all the USD dependencies that we need (namely UsdPhysics) + # so we need to manually link all the required libraries. + configure_houdini_target(${MJC_PHYSICS_PLUGIN_TARGET_NAME} PRIVATE PRIVATE) + configure_houdini_target(${MJCF_PLUGIN_TARGET_NAME} PRIVATE PRIVATE) + configure_houdini_target(${MJ_USD_TARGET_NAME} PUBLIC PUBLIC) + + # Re-add inter-plugin dependencies after Houdini-specific linking + target_link_libraries(${MJCF_PLUGIN_TARGET_NAME} PRIVATE ${MJC_PHYSICS_PLUGIN_TARGET_NAME}) + target_link_libraries(${MJ_USD_TARGET_NAME} PUBLIC ${MJC_PHYSICS_PLUGIN_TARGET_NAME}) + +endif() + +## Installation + +message(STATUS "Mujoco USD plugins will be installed to: ${CMAKE_INSTALL_PREFIX}/${MJ_USD_INSTALL_DIR_LIB}") +message(STATUS "Make sure PXR_PLUGINPATH_NAME includes: ${CMAKE_INSTALL_PREFIX}/${MJ_USD_INSTALL_DIR_LIB}/*/resources") + +# Generate and install plugInfo.json for each plugin +configure_and_install_usd_plugin_info( + ${MJCF_PLUGIN_TARGET_NAME} + plugins/mjcf + ${MJ_USD_INSTALL_DIR_LIB} +) +configure_and_install_usd_plugin_info( + ${MJC_PHYSICS_PLUGIN_TARGET_NAME} + mjcPhysics + ${MJ_USD_INSTALL_DIR_LIB} +) + +# Install specific resources that are not plugInfo.json (like generatedSchema.usda) +install(FILES + mjcPhysics/generatedSchema.usda + DESTINATION ${MJ_USD_INSTALL_DIR_LIB}/resources/mjcPhysics +) + +# Install shared libraries +install(TARGETS + ${MJCF_PLUGIN_TARGET_NAME} + ${MJC_PHYSICS_PLUGIN_TARGET_NAME} + ${MJ_USD_TARGET_NAME} + LIBRARY DESTINATION ${MJ_USD_INSTALL_DIR_LIB} +) diff --git a/src/experimental/usd/layer_sink.cc b/src/experimental/usd/layer_sink.cc new file mode 100644 index 00000000..2ae8d55a --- /dev/null +++ b/src/experimental/usd/layer_sink.cc @@ -0,0 +1,132 @@ +// Copyright 2025 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. + +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +PXR_NAMESPACE_OPEN_SCOPE +TF_DEFINE_PRIVATE_TOKENS(_layer_sink_tokens, + ((xformOpTransform, + "xformOp:transform:mujoco"))(Xform)); +PXR_NAMESPACE_CLOSE_SCOPE + +namespace { +pxr::SdfAttributeSpecHandle GetOrCreateAttribute( + pxr::SdfLayerHandle layer, pxr::SdfPrimSpecHandle prim_spec, + const pxr::SdfPath& path, const pxr::TfToken& token, + const pxr::SdfValueTypeName& type, pxr::SdfVariability variability) { + // Find or create transform attribute. + pxr::SdfAttributeSpecHandle attr = + layer->GetAttributeAtPath(path.AppendProperty(token)); + + if (!attr) { + attr = pxr::SdfAttributeSpec::New(prim_spec, token, type, variability); + } + return attr; +} +} // namespace + +namespace mujoco { +namespace usd { +LayerSink::LayerSink(pxr::UsdStageRefPtr stage) : stage_(stage) {} + +void LayerSink::Update(const mjData* const data, + std::vector body_paths) { + pxr::SdfLayerHandle layer = stage_->GetEditTarget().GetLayer(); + + pxr::UsdTimeCode timecode = data->time * layer->GetTimeCodesPerSecond(); + pxr::UsdGeomXformCache xform_cache(timecode); + + pxr::SdfChangeBlock change_block; + // TODO(shaves) this can be parallelised. + for (int i = 0; i < body_paths.size(); i++) { + const pxr::SdfPath& path = body_paths[i]; + + // Author transform timesample + // Convert pose data. + mjtNum* xpos = &data->xpos[i * 3]; + mjtNum* xquat = &data->xquat[i * 4]; + pxr::GfVec3d translation(xpos[0], xpos[1], xpos[2]); + pxr::GfVec3d imag_part(xquat[1], xquat[2], xquat[3]); + // Some bodies such as the world body don't have mapped USD prims to + // transform. + if (path.IsEmpty()) { + continue; + } + + pxr::SdfPrimSpecHandle prim_spec = pxr::SdfCreatePrimInLayer(layer, path); + + // Find or create transform attribute. + pxr::SdfAttributeSpecHandle xform_attr = GetOrCreateAttribute( + layer, prim_spec, path, pxr::_layer_sink_tokens->xformOpTransform, + pxr::SdfValueTypeNames->Matrix4d, pxr::SdfVariabilityVarying); + + pxr::GfQuaternion quat(xquat[0], imag_part); + pxr::GfRotation rotation(quat); + + // Identities + pxr::GfVec3d scale(1.0); + pxr::GfVec3d pivot_position(0); + pxr::GfRotation pivot_rotation(pxr::GfQuaternion::GetIdentity()); + + // Set time sample + pxr::GfTransform xform(translation, rotation, scale, pivot_position, + pivot_rotation); + pxr::GfMatrix4d parent_xform = + xform_cache.GetParentToWorldTransform(stage_->GetPrimAtPath(path)); + pxr::GfMatrix4d relative_xform = + xform.GetMatrix() * parent_xform.GetInverse(); + layer->SetTimeSample(xform_attr->GetPath(), timecode.GetValue(), + relative_xform); + + // Create xformop_order attribute if missing. + pxr::SdfAttributeSpecHandle op_order_attr = GetOrCreateAttribute( + layer, prim_spec, path, pxr::UsdGeomTokens->xformOpOrder, + pxr::SdfValueTypeNames->TokenArray, pxr::SdfVariabilityUniform); + + // Override all local transforms so that this is the absolute position. + // We don't reset the xform stack here, so parent transforms will still + // affect the world pose. + const pxr::VtTokenArray xformop_order( + {pxr::_layer_sink_tokens->xformOpTransform}); + + op_order_attr->SetDefaultValue(pxr::VtValue(xformop_order)); + } +} +} // namespace usd +} // namespace mujoco diff --git a/src/experimental/usd/mjcPhysics/actuatorAPI.cpp b/src/experimental/usd/mjcPhysics/actuatorAPI.cpp index fb43eee7..830226a9 100644 --- a/src/experimental/usd/mjcPhysics/actuatorAPI.cpp +++ b/src/experimental/usd/mjcPhysics/actuatorAPI.cpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "./actuatorAPI.h" +#include -#include "pxr/usd/sdf/assetPath.h" -#include "pxr/usd/sdf/types.h" -#include "pxr/usd/usd/schemaRegistry.h" -#include "pxr/usd/usd/typed.h" +#include +#include +#include +#include PXR_NAMESPACE_OPEN_SCOPE @@ -325,15 +325,6 @@ UsdRelationship MjcPhysicsActuatorAPI::CreateMjcRefSiteRel() const { /* custom = */ false); } -UsdRelationship MjcPhysicsActuatorAPI::GetMjcCrankSiteRel() const { - return GetPrim().GetRelationship(MjcPhysicsTokens->mjcCrankSite); -} - -UsdRelationship MjcPhysicsActuatorAPI::CreateMjcCrankSiteRel() const { - return GetPrim().CreateRelationship(MjcPhysicsTokens->mjcCrankSite, - /* custom = */ false); -} - UsdRelationship MjcPhysicsActuatorAPI::GetMjcSliderSiteRel() const { return GetPrim().GetRelationship(MjcPhysicsTokens->mjcSliderSite); } diff --git a/src/experimental/usd/mjcPhysics/collisionAPI.cpp b/src/experimental/usd/mjcPhysics/collisionAPI.cpp index ff113849..0ff817fb 100644 --- a/src/experimental/usd/mjcPhysics/collisionAPI.cpp +++ b/src/experimental/usd/mjcPhysics/collisionAPI.cpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "./collisionAPI.h" +#include -#include "pxr/usd/sdf/assetPath.h" -#include "pxr/usd/sdf/types.h" -#include "pxr/usd/usd/schemaRegistry.h" -#include "pxr/usd/usd/typed.h" +#include +#include +#include +#include PXR_NAMESPACE_OPEN_SCOPE diff --git a/src/experimental/usd/mjcPhysics/generatedSchema.usda b/src/experimental/usd/mjcPhysics/generatedSchema.usda index b911cf59..43afe14e 100644 --- a/src/experimental/usd/mjcPhysics/generatedSchema.usda +++ b/src/experimental/usd/mjcPhysics/generatedSchema.usda @@ -277,9 +277,6 @@ class "PhysicsActuatorAPI" ( uniform double mjc:crankLength = 0 ( doc = "Used only for the slider-crank transmission type. Specifies the length of the connecting rod. The compiler expects this value to be positive when a slider-crank transmission is present." ) - rel mjc:crankSite ( - doc = "If specified, the actuator acts on a slider-crank mechanism which is implicitly determined by the actuator (i.e., it is not a separate model element). The target site corresponds to the pin joining the crank and the connecting rod." - ) uniform token mjc:ctrlLimited = "auto" ( allowedTokens = ["false", "true", "auto"] doc = "If true, the control input to this actuator is automatically clamped to ctrlrange at runtime. If false, control input clamping is disabled. If 'auto' and autolimits is set in compiler, control clamping will automatically be set to true if ctrlrange is defined without explicitly setting this attribute to 'true'. Note that control input clamping can also be globally disabled with the clampctrl attribute of option/flag." @@ -334,3 +331,82 @@ class "PhysicsActuatorAPI" ( ) } +class Keyframe "Keyframe" ( + doc = "Represents time independent keyframe values." +) +{ + double[] mjc:act ( + doc = "Vector of actuator activations, copied into mjData.act when the simulation state is set to this keyframe." + ) + double[] mjc:ctrl ( + doc = "Vector of controls, copied into mjData.ctrl when the simulation state is set to this keyframe." + ) + double[] mjc:mpos ( + doc = "Vector of mocap body positions, copied into mjData.mocap_pos when the simulation state is set to this keyframe." + ) + double[] mjc:mquat ( + doc = "Vector of mocap body quaternions, copied into mjData.mocap_quat when the simulation state is set to this keyframe." + ) + double[] mjc:qpos ( + doc = "Vector of joint positions, copied into mjData.qpos when the simulation state is set to this keyframe." + ) + double[] mjc:qvel ( + doc = "Vector of joint velocities, copied into mjData.qvel when the simulation state is set to this keyframe." + ) +} + +class "PhysicsJointsAPI" ( + doc = "API describing a Mujoco joint." +) +{ + uniform token mjc:actuatorfrclimited = "auto" ( + allowedTokens = ["false", "true", "auto"] + doc = "This attribute specifies whether actuator forces acting on the joint should be clamped. See Force limits for details. It is available only for scalar joints (hinge and slider) and ignored for ball and free joints. This attribute interacts with the actuatorfrcrange attribute. If this attribute is 'false', actuator force clamping is disabled. If it is 'true', actuator force clamping is enabled. If this attribute is 'auto', and autolimits is set in compiler, actuator force clamping will be enabled if actuatorfrcrange is defined." + ) + uniform double mjc:actuatorfrcrange:max = 0 ( + doc = "Maximum range for clamping total actuator forces acting on this joint. See Force limits for details. It is available only for scalar joints (hinge and slider) and ignored for ball and free joints. The compiler expects the first value to be smaller than the second value. Setting this attribute without specifying actuatorfrclimited is an error if compiler-autolimits is 'false'." + ) + uniform double mjc:actuatorfrcrange:min = 0 ( + doc = "Minimum range for clamping total actuator forces acting on this joint. See Force limits for details. It is available only for scalar joints (hinge and slider) and ignored for ball and free joints. The compiler expects the first value to be smaller than the second value. Setting this attribute without specifying actuatorfrclimited is an error if compiler-autolimits is 'false'." + ) + uniform bool mjc:actuatorgravcomp = 0 ( + doc = "If this flag is enabled, gravity compensation applied to this joint is added to actuator forces (mjData.qfrc_actuator) rather than passive forces (mjData.qfrc_passive). Notionally, this means that gravity compensation is the result of a control system rather than natural buoyancy. In practice, enabling this flag is useful when joint-level actuator force clamping is used. In this case, the total actuation force applied on a joint, including gravity compensation, is guaranteed to not exceed the specified limits. See Force limits and actuatorfrcrange for more details on this type of force limit." + ) + uniform double mjc:armature = 0 ( + doc = "Additional inertia associated with movement of the joint that is not due to body mass. This added inertia is usually due to a rotor (a.k.a armature) spinning faster than the joint itself due to a geared transmission. The value applies to all degrees of freedom created by this joint. Besides increasing the realism of joints with geared transmission, positive armature significantly improves simulation stability, even for small values, and is a recommended possible fix when encountering stability issues." + ) + uniform double mjc:damping = 0 ( + doc = "Damping applied to all degrees of freedom created by this joint. Unlike friction loss which is computed by the constraint solver, damping is simply a force linear in velocity. It is included in the passive forces. Despite this simplicity, larger damping values can make numerical integrators unstable, which is why our Euler integrator handles damping implicitly. See Integration in the Computation chapter." + ) + uniform double mjc:frictionloss = 0 ( + doc = "Friction loss due to dry friction. This value is the same for all degrees of freedom created by this joint. Semantically friction loss does not make sense for free joints, but the compiler allows it. To enable friction loss, set this attribute to a positive value." + ) + uniform double mjc:margin = 0 ( + doc = "The distance threshold below which limits become active. Recall that the Constraint solver normally generates forces as soon as a constraint becomes active, even if the margin parameter makes that happen at a distance. This attribute together with solreflimit and solimplimit can be used to model a soft joint limit." + ) + uniform double mjc:ref = 0 ( + doc = "The reference position or angle of the joint. This attribute is only used for slide and hinge joints. It defines the joint value corresponding to the initial model configuration. The amount of spatial transformation that the joint applies at runtime equals the current joint value stored in mjData.qpos minus this reference value stored in mjModel.qpos0. The meaning of these vectors was discussed in the Stand-alone section in the Overview chapter." + ) + uniform double[] mjc:solimpfriction = [0.9, 0.95, 0.001, 0.5, 2] ( + doc = "Constraint solver parameters for simulating dry friction." + ) + uniform double[] mjc:solimplimit = [0.9, 0.95, 0.001, 0.5, 2] ( + doc = "Constraint solver parameters for simulating joint limits." + ) + uniform double[] mjc:solreffriction = [0.02, 1] ( + doc = "Constraint solver parameters for simulating dry friction." + ) + uniform double[] mjc:solreflimit = [0.02, 1] ( + doc = "Constraint solver parameters for simulating joint limits." + ) + uniform double[] mjc:springdamper = [0, 0] ( + doc = "When both numbers are positive, the compiler will override any stiffness and damping values specified with the attributes below, and will instead set them automatically so that the resulting mass-spring-damper for this joint has the desired time constant (first value) and damping ratio (second value). This is done by taking into account the joint inertia in the model reference configuration. Note that the format is the same as the solref parameter of the constraint solver." + ) + uniform double mjc:springref = 0 ( + doc = "The joint position or angle in which the joint spring (if any) achieves equilibrium. Similar to the vector mjModel.qpos0 which stores all joint reference values specified with the ref attribute above, all spring reference values specified with this attribute are stored in the vector mjModel.qpos_spring. The model configuration corresponding to mjModel.qpos_spring is also used to compute the spring reference lengths of all tendons, stored in mjModel.tendon_lengthspring. This is because tendons can also have springs." + ) + uniform double mjc:stiffness = 0 ( + doc = "Joint stiffness. If this value is positive, a spring will be created with equilibrium position given by springref below. The spring force is computed along with the other passive forces." + ) +} + diff --git a/src/experimental/usd/mjcPhysics/jointAPI.cpp b/src/experimental/usd/mjcPhysics/jointAPI.cpp new file mode 100644 index 00000000..0adaf2ed --- /dev/null +++ b/src/experimental/usd/mjcPhysics/jointAPI.cpp @@ -0,0 +1,303 @@ +// Copyright 2025 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. + +#include + +#include +#include +#include +#include + +PXR_NAMESPACE_OPEN_SCOPE + +// Register the schema with the TfType system. +TF_REGISTRY_FUNCTION(TfType) { + TfType::Define >(); +} + +/* virtual */ +MjcPhysicsJointAPI::~MjcPhysicsJointAPI() {} + +/* static */ +MjcPhysicsJointAPI MjcPhysicsJointAPI::Get(const UsdStagePtr &stage, + const SdfPath &path) { + if (!stage) { + TF_CODING_ERROR("Invalid stage"); + return MjcPhysicsJointAPI(); + } + return MjcPhysicsJointAPI(stage->GetPrimAtPath(path)); +} + +/* virtual */ +UsdSchemaKind MjcPhysicsJointAPI::_GetSchemaKind() const { + return MjcPhysicsJointAPI::schemaKind; +} + +/* static */ +bool MjcPhysicsJointAPI::CanApply(const UsdPrim &prim, std::string *whyNot) { + return prim.CanApplyAPI(whyNot); +} + +/* static */ +MjcPhysicsJointAPI MjcPhysicsJointAPI::Apply(const UsdPrim &prim) { + if (prim.ApplyAPI()) { + return MjcPhysicsJointAPI(prim); + } + return MjcPhysicsJointAPI(); +} + +/* static */ +const TfType &MjcPhysicsJointAPI::_GetStaticTfType() { + static TfType tfType = TfType::Find(); + return tfType; +} + +/* static */ +bool MjcPhysicsJointAPI::_IsTypedSchema() { + static bool isTyped = _GetStaticTfType().IsA(); + return isTyped; +} + +/* virtual */ +const TfType &MjcPhysicsJointAPI::_GetTfType() const { + return _GetStaticTfType(); +} + +UsdAttribute MjcPhysicsJointAPI::GetMjcSpringdamperAttr() const { + return GetPrim().GetAttribute(MjcPhysicsTokens->mjcSpringdamper); +} + +UsdAttribute MjcPhysicsJointAPI::CreateMjcSpringdamperAttr( + VtValue const &defaultValue, bool writeSparsely) const { + return UsdSchemaBase::_CreateAttr( + MjcPhysicsTokens->mjcSpringdamper, SdfValueTypeNames->DoubleArray, + /* custom = */ false, SdfVariabilityUniform, defaultValue, writeSparsely); +} + +UsdAttribute MjcPhysicsJointAPI::GetMjcSolreflimitAttr() const { + return GetPrim().GetAttribute(MjcPhysicsTokens->mjcSolreflimit); +} + +UsdAttribute MjcPhysicsJointAPI::CreateMjcSolreflimitAttr( + VtValue const &defaultValue, bool writeSparsely) const { + return UsdSchemaBase::_CreateAttr( + MjcPhysicsTokens->mjcSolreflimit, SdfValueTypeNames->DoubleArray, + /* custom = */ false, SdfVariabilityUniform, defaultValue, writeSparsely); +} + +UsdAttribute MjcPhysicsJointAPI::GetMjcSolimplimitAttr() const { + return GetPrim().GetAttribute(MjcPhysicsTokens->mjcSolimplimit); +} + +UsdAttribute MjcPhysicsJointAPI::CreateMjcSolimplimitAttr( + VtValue const &defaultValue, bool writeSparsely) const { + return UsdSchemaBase::_CreateAttr( + MjcPhysicsTokens->mjcSolimplimit, SdfValueTypeNames->DoubleArray, + /* custom = */ false, SdfVariabilityUniform, defaultValue, writeSparsely); +} + +UsdAttribute MjcPhysicsJointAPI::GetMjcSolreffrictionAttr() const { + return GetPrim().GetAttribute(MjcPhysicsTokens->mjcSolreffriction); +} + +UsdAttribute MjcPhysicsJointAPI::CreateMjcSolreffrictionAttr( + VtValue const &defaultValue, bool writeSparsely) const { + return UsdSchemaBase::_CreateAttr( + MjcPhysicsTokens->mjcSolreffriction, SdfValueTypeNames->DoubleArray, + /* custom = */ false, SdfVariabilityUniform, defaultValue, writeSparsely); +} + +UsdAttribute MjcPhysicsJointAPI::GetMjcSolimpfrictionAttr() const { + return GetPrim().GetAttribute(MjcPhysicsTokens->mjcSolimpfriction); +} + +UsdAttribute MjcPhysicsJointAPI::CreateMjcSolimpfrictionAttr( + VtValue const &defaultValue, bool writeSparsely) const { + return UsdSchemaBase::_CreateAttr( + MjcPhysicsTokens->mjcSolimpfriction, SdfValueTypeNames->DoubleArray, + /* custom = */ false, SdfVariabilityUniform, defaultValue, writeSparsely); +} + +UsdAttribute MjcPhysicsJointAPI::GetMjcStiffnessAttr() const { + return GetPrim().GetAttribute(MjcPhysicsTokens->mjcStiffness); +} + +UsdAttribute MjcPhysicsJointAPI::CreateMjcStiffnessAttr( + VtValue const &defaultValue, bool writeSparsely) const { + return UsdSchemaBase::_CreateAttr( + MjcPhysicsTokens->mjcStiffness, SdfValueTypeNames->Double, + /* custom = */ false, SdfVariabilityUniform, defaultValue, writeSparsely); +} + +UsdAttribute MjcPhysicsJointAPI::GetMjcActuatorfrcrangeMinAttr() const { + return GetPrim().GetAttribute(MjcPhysicsTokens->mjcActuatorfrcrangeMin); +} + +UsdAttribute MjcPhysicsJointAPI::CreateMjcActuatorfrcrangeMinAttr( + VtValue const &defaultValue, bool writeSparsely) const { + return UsdSchemaBase::_CreateAttr( + MjcPhysicsTokens->mjcActuatorfrcrangeMin, SdfValueTypeNames->Double, + /* custom = */ false, SdfVariabilityUniform, defaultValue, writeSparsely); +} + +UsdAttribute MjcPhysicsJointAPI::GetMjcActuatorfrcrangeMaxAttr() const { + return GetPrim().GetAttribute(MjcPhysicsTokens->mjcActuatorfrcrangeMax); +} + +UsdAttribute MjcPhysicsJointAPI::CreateMjcActuatorfrcrangeMaxAttr( + VtValue const &defaultValue, bool writeSparsely) const { + return UsdSchemaBase::_CreateAttr( + MjcPhysicsTokens->mjcActuatorfrcrangeMax, SdfValueTypeNames->Double, + /* custom = */ false, SdfVariabilityUniform, defaultValue, writeSparsely); +} + +UsdAttribute MjcPhysicsJointAPI::GetMjcActuatorfrclimitedAttr() const { + return GetPrim().GetAttribute(MjcPhysicsTokens->mjcActuatorfrclimited); +} + +UsdAttribute MjcPhysicsJointAPI::CreateMjcActuatorfrclimitedAttr( + VtValue const &defaultValue, bool writeSparsely) const { + return UsdSchemaBase::_CreateAttr( + MjcPhysicsTokens->mjcActuatorfrclimited, SdfValueTypeNames->Token, + /* custom = */ false, SdfVariabilityUniform, defaultValue, writeSparsely); +} + +UsdAttribute MjcPhysicsJointAPI::GetMjcActuatorgravcompAttr() const { + return GetPrim().GetAttribute(MjcPhysicsTokens->mjcActuatorgravcomp); +} + +UsdAttribute MjcPhysicsJointAPI::CreateMjcActuatorgravcompAttr( + VtValue const &defaultValue, bool writeSparsely) const { + return UsdSchemaBase::_CreateAttr( + MjcPhysicsTokens->mjcActuatorgravcomp, SdfValueTypeNames->Bool, + /* custom = */ false, SdfVariabilityUniform, defaultValue, writeSparsely); +} + +UsdAttribute MjcPhysicsJointAPI::GetMjcMarginAttr() const { + return GetPrim().GetAttribute(MjcPhysicsTokens->mjcMargin); +} + +UsdAttribute MjcPhysicsJointAPI::CreateMjcMarginAttr( + VtValue const &defaultValue, bool writeSparsely) const { + return UsdSchemaBase::_CreateAttr( + MjcPhysicsTokens->mjcMargin, SdfValueTypeNames->Double, + /* custom = */ false, SdfVariabilityUniform, defaultValue, writeSparsely); +} + +UsdAttribute MjcPhysicsJointAPI::GetMjcRefAttr() const { + return GetPrim().GetAttribute(MjcPhysicsTokens->mjcRef); +} + +UsdAttribute MjcPhysicsJointAPI::CreateMjcRefAttr(VtValue const &defaultValue, + bool writeSparsely) const { + return UsdSchemaBase::_CreateAttr( + MjcPhysicsTokens->mjcRef, SdfValueTypeNames->Double, + /* custom = */ false, SdfVariabilityUniform, defaultValue, writeSparsely); +} + +UsdAttribute MjcPhysicsJointAPI::GetMjcSpringrefAttr() const { + return GetPrim().GetAttribute(MjcPhysicsTokens->mjcSpringref); +} + +UsdAttribute MjcPhysicsJointAPI::CreateMjcSpringrefAttr( + VtValue const &defaultValue, bool writeSparsely) const { + return UsdSchemaBase::_CreateAttr( + MjcPhysicsTokens->mjcSpringref, SdfValueTypeNames->Double, + /* custom = */ false, SdfVariabilityUniform, defaultValue, writeSparsely); +} + +UsdAttribute MjcPhysicsJointAPI::GetMjcArmatureAttr() const { + return GetPrim().GetAttribute(MjcPhysicsTokens->mjcArmature); +} + +UsdAttribute MjcPhysicsJointAPI::CreateMjcArmatureAttr( + VtValue const &defaultValue, bool writeSparsely) const { + return UsdSchemaBase::_CreateAttr( + MjcPhysicsTokens->mjcArmature, SdfValueTypeNames->Double, + /* custom = */ false, SdfVariabilityUniform, defaultValue, writeSparsely); +} + +UsdAttribute MjcPhysicsJointAPI::GetMjcDampingAttr() const { + return GetPrim().GetAttribute(MjcPhysicsTokens->mjcDamping); +} + +UsdAttribute MjcPhysicsJointAPI::CreateMjcDampingAttr( + VtValue const &defaultValue, bool writeSparsely) const { + return UsdSchemaBase::_CreateAttr( + MjcPhysicsTokens->mjcDamping, SdfValueTypeNames->Double, + /* custom = */ false, SdfVariabilityUniform, defaultValue, writeSparsely); +} + +UsdAttribute MjcPhysicsJointAPI::GetMjcFrictionlossAttr() const { + return GetPrim().GetAttribute(MjcPhysicsTokens->mjcFrictionloss); +} + +UsdAttribute MjcPhysicsJointAPI::CreateMjcFrictionlossAttr( + VtValue const &defaultValue, bool writeSparsely) const { + return UsdSchemaBase::_CreateAttr( + MjcPhysicsTokens->mjcFrictionloss, SdfValueTypeNames->Double, + /* custom = */ false, SdfVariabilityUniform, defaultValue, writeSparsely); +} + +namespace { +static inline TfTokenVector _ConcatenateAttributeNames( + const TfTokenVector &left, const TfTokenVector &right) { + TfTokenVector result; + result.reserve(left.size() + right.size()); + result.insert(result.end(), left.begin(), left.end()); + result.insert(result.end(), right.begin(), right.end()); + return result; +} +} // namespace + +/*static*/ +const TfTokenVector &MjcPhysicsJointAPI::GetSchemaAttributeNames( + bool includeInherited) { + static TfTokenVector localNames = { + MjcPhysicsTokens->mjcSpringdamper, + MjcPhysicsTokens->mjcSolreflimit, + MjcPhysicsTokens->mjcSolimplimit, + MjcPhysicsTokens->mjcSolreffriction, + MjcPhysicsTokens->mjcSolimpfriction, + MjcPhysicsTokens->mjcStiffness, + MjcPhysicsTokens->mjcActuatorfrcrangeMin, + MjcPhysicsTokens->mjcActuatorfrcrangeMax, + MjcPhysicsTokens->mjcActuatorfrclimited, + MjcPhysicsTokens->mjcActuatorgravcomp, + MjcPhysicsTokens->mjcMargin, + MjcPhysicsTokens->mjcRef, + MjcPhysicsTokens->mjcSpringref, + MjcPhysicsTokens->mjcArmature, + MjcPhysicsTokens->mjcDamping, + MjcPhysicsTokens->mjcFrictionloss, + }; + static TfTokenVector allNames = _ConcatenateAttributeNames( + UsdAPISchemaBase::GetSchemaAttributeNames(true), localNames); + + if (includeInherited) + return allNames; + else + return localNames; +} + +PXR_NAMESPACE_CLOSE_SCOPE + +// ===================================================================== // +// Feel free to add custom code below this line. It will be preserved by +// the code generator. +// +// Just remember to wrap code in the appropriate delimiters: +// 'PXR_NAMESPACE_OPEN_SCOPE', 'PXR_NAMESPACE_CLOSE_SCOPE'. +// ===================================================================== // +// --(BEGIN CUSTOM CODE)-- diff --git a/src/experimental/usd/mjcPhysics/keyframe.cpp b/src/experimental/usd/mjcPhysics/keyframe.cpp new file mode 100644 index 00000000..05a4eb46 --- /dev/null +++ b/src/experimental/usd/mjcPhysics/keyframe.cpp @@ -0,0 +1,185 @@ +// Copyright 2025 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. + +#include + +#include +#include +#include +#include + +PXR_NAMESPACE_OPEN_SCOPE + +// Register the schema with the TfType system. +TF_REGISTRY_FUNCTION(TfType) { + TfType::Define >(); + + // Register the usd prim typename as an alias under UsdSchemaBase. This + // enables one to call + // TfType::Find().FindDerivedByName("Keyframe") + // to find TfType, which is how IsA queries are + // answered. + TfType::AddAlias("Keyframe"); +} + +/* virtual */ +MjcPhysicsKeyframe::~MjcPhysicsKeyframe() {} + +/* static */ +MjcPhysicsKeyframe MjcPhysicsKeyframe::Get(const UsdStagePtr &stage, + const SdfPath &path) { + if (!stage) { + TF_CODING_ERROR("Invalid stage"); + return MjcPhysicsKeyframe(); + } + return MjcPhysicsKeyframe(stage->GetPrimAtPath(path)); +} + +/* static */ +MjcPhysicsKeyframe MjcPhysicsKeyframe::Define(const UsdStagePtr &stage, + const SdfPath &path) { + static TfToken usdPrimTypeName("Keyframe"); + if (!stage) { + TF_CODING_ERROR("Invalid stage"); + return MjcPhysicsKeyframe(); + } + return MjcPhysicsKeyframe(stage->DefinePrim(path, usdPrimTypeName)); +} + +/* virtual */ +UsdSchemaKind MjcPhysicsKeyframe::_GetSchemaKind() const { + return MjcPhysicsKeyframe::schemaKind; +} + +/* static */ +const TfType &MjcPhysicsKeyframe::_GetStaticTfType() { + static TfType tfType = TfType::Find(); + return tfType; +} + +/* static */ +bool MjcPhysicsKeyframe::_IsTypedSchema() { + static bool isTyped = _GetStaticTfType().IsA(); + return isTyped; +} + +/* virtual */ +const TfType &MjcPhysicsKeyframe::_GetTfType() const { + return _GetStaticTfType(); +} + +UsdAttribute MjcPhysicsKeyframe::GetMjcQposAttr() const { + return GetPrim().GetAttribute(MjcPhysicsTokens->mjcQpos); +} + +UsdAttribute MjcPhysicsKeyframe::CreateMjcQposAttr(VtValue const &defaultValue, + bool writeSparsely) const { + return UsdSchemaBase::_CreateAttr( + MjcPhysicsTokens->mjcQpos, SdfValueTypeNames->DoubleArray, + /* custom = */ false, SdfVariabilityVarying, defaultValue, writeSparsely); +} + +UsdAttribute MjcPhysicsKeyframe::GetMjcQvelAttr() const { + return GetPrim().GetAttribute(MjcPhysicsTokens->mjcQvel); +} + +UsdAttribute MjcPhysicsKeyframe::CreateMjcQvelAttr(VtValue const &defaultValue, + bool writeSparsely) const { + return UsdSchemaBase::_CreateAttr( + MjcPhysicsTokens->mjcQvel, SdfValueTypeNames->DoubleArray, + /* custom = */ false, SdfVariabilityVarying, defaultValue, writeSparsely); +} + +UsdAttribute MjcPhysicsKeyframe::GetMjcActAttr() const { + return GetPrim().GetAttribute(MjcPhysicsTokens->mjcAct); +} + +UsdAttribute MjcPhysicsKeyframe::CreateMjcActAttr(VtValue const &defaultValue, + bool writeSparsely) const { + return UsdSchemaBase::_CreateAttr( + MjcPhysicsTokens->mjcAct, SdfValueTypeNames->DoubleArray, + /* custom = */ false, SdfVariabilityVarying, defaultValue, writeSparsely); +} + +UsdAttribute MjcPhysicsKeyframe::GetMjcCtrlAttr() const { + return GetPrim().GetAttribute(MjcPhysicsTokens->mjcCtrl); +} + +UsdAttribute MjcPhysicsKeyframe::CreateMjcCtrlAttr(VtValue const &defaultValue, + bool writeSparsely) const { + return UsdSchemaBase::_CreateAttr( + MjcPhysicsTokens->mjcCtrl, SdfValueTypeNames->DoubleArray, + /* custom = */ false, SdfVariabilityVarying, defaultValue, writeSparsely); +} + +UsdAttribute MjcPhysicsKeyframe::GetMjcMposAttr() const { + return GetPrim().GetAttribute(MjcPhysicsTokens->mjcMpos); +} + +UsdAttribute MjcPhysicsKeyframe::CreateMjcMposAttr(VtValue const &defaultValue, + bool writeSparsely) const { + return UsdSchemaBase::_CreateAttr( + MjcPhysicsTokens->mjcMpos, SdfValueTypeNames->DoubleArray, + /* custom = */ false, SdfVariabilityVarying, defaultValue, writeSparsely); +} + +UsdAttribute MjcPhysicsKeyframe::GetMjcMquatAttr() const { + return GetPrim().GetAttribute(MjcPhysicsTokens->mjcMquat); +} + +UsdAttribute MjcPhysicsKeyframe::CreateMjcMquatAttr(VtValue const &defaultValue, + bool writeSparsely) const { + return UsdSchemaBase::_CreateAttr( + MjcPhysicsTokens->mjcMquat, SdfValueTypeNames->DoubleArray, + /* custom = */ false, SdfVariabilityVarying, defaultValue, writeSparsely); +} + +namespace { +static inline TfTokenVector _ConcatenateAttributeNames( + const TfTokenVector &left, const TfTokenVector &right) { + TfTokenVector result; + result.reserve(left.size() + right.size()); + result.insert(result.end(), left.begin(), left.end()); + result.insert(result.end(), right.begin(), right.end()); + return result; +} +} // namespace + +/*static*/ +const TfTokenVector &MjcPhysicsKeyframe::GetSchemaAttributeNames( + bool includeInherited) { + static TfTokenVector localNames = { + MjcPhysicsTokens->mjcQpos, MjcPhysicsTokens->mjcQvel, + MjcPhysicsTokens->mjcAct, MjcPhysicsTokens->mjcCtrl, + MjcPhysicsTokens->mjcMpos, MjcPhysicsTokens->mjcMquat, + }; + static TfTokenVector allNames = _ConcatenateAttributeNames( + UsdTyped::GetSchemaAttributeNames(true), localNames); + + if (includeInherited) + return allNames; + else + return localNames; +} + +PXR_NAMESPACE_CLOSE_SCOPE + +// ===================================================================== // +// Feel free to add custom code below this line. It will be preserved by +// the code generator. +// +// Just remember to wrap code in the appropriate delimiters: +// 'PXR_NAMESPACE_OPEN_SCOPE', 'PXR_NAMESPACE_CLOSE_SCOPE'. +// ===================================================================== // +// --(BEGIN CUSTOM CODE)-- diff --git a/src/experimental/usd/mjcPhysics/meshCollisionAPI.cpp b/src/experimental/usd/mjcPhysics/meshCollisionAPI.cpp index 50c579dd..9d9b00dc 100644 --- a/src/experimental/usd/mjcPhysics/meshCollisionAPI.cpp +++ b/src/experimental/usd/mjcPhysics/meshCollisionAPI.cpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "./meshCollisionAPI.h" +#include -#include "pxr/usd/sdf/assetPath.h" -#include "pxr/usd/sdf/types.h" -#include "pxr/usd/usd/schemaRegistry.h" -#include "pxr/usd/usd/typed.h" +#include +#include +#include +#include PXR_NAMESPACE_OPEN_SCOPE diff --git a/src/experimental/usd/mjcPhysics/plugInfo.json b/src/experimental/usd/mjcPhysics/plugInfo.json index 908651ba..a55a4f2d 100644 --- a/src/experimental/usd/mjcPhysics/plugInfo.json +++ b/src/experimental/usd/mjcPhysics/plugInfo.json @@ -1,6 +1,3 @@ -#Portions of this file auto - generated by usdGenSchema. -#Edits will survive regeneration except for comments and -#changes to types with autoGenerated = true. { "Plugins": [ { @@ -26,6 +23,26 @@ ], "schemaKind": "singleApplyAPI" }, + "MjcPhysicsJointAPI": { + "alias": { + "UsdSchemaBase": "PhysicsJointsAPI" + }, + "autoGenerated": true, + "bases": [ + "UsdAPISchemaBase" + ], + "schemaKind": "singleApplyAPI" + }, + "MjcPhysicsKeyframe": { + "alias": { + "UsdSchemaBase": "Keyframe" + }, + "autoGenerated": true, + "bases": [ + "UsdTyped" + ], + "schemaKind": "concreteTyped" + }, "MjcPhysicsMeshCollisionAPI": { "alias": { "UsdSchemaBase": "MeshCollisionAPI" @@ -58,7 +75,7 @@ } } }, - "LibraryPath": "", + "LibraryPath": "@PLUG_INFO_LIBRARY_PATH@", "Name": "mjcPhysics", "ResourcePath": "", "Root": ".", diff --git a/src/experimental/usd/mjcPhysics/sceneAPI.cpp b/src/experimental/usd/mjcPhysics/sceneAPI.cpp index 7bce5c2d..455190a7 100644 --- a/src/experimental/usd/mjcPhysics/sceneAPI.cpp +++ b/src/experimental/usd/mjcPhysics/sceneAPI.cpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "./sceneAPI.h" +#include -#include "pxr/usd/sdf/assetPath.h" -#include "pxr/usd/sdf/types.h" -#include "pxr/usd/usd/schemaRegistry.h" -#include "pxr/usd/usd/typed.h" +#include +#include +#include +#include PXR_NAMESPACE_OPEN_SCOPE diff --git a/src/experimental/usd/mjcPhysics/schema.usda b/src/experimental/usd/mjcPhysics/schema.usda index a59c6bb9..4892f809 100644 --- a/src/experimental/usd/mjcPhysics/schema.usda +++ b/src/experimental/usd/mjcPhysics/schema.usda @@ -663,3 +663,110 @@ class "PhysicsActuatorAPI" } +class Keyframe "Keyframe" +( + doc = """Represents time independent keyframe values.""" + + inherits = +) +{ + double[] mjc:qpos ( + doc = "Vector of joint positions, copied into mjData.qpos when the simulation state is set to this keyframe." + ) + + double[] mjc:qvel ( + doc = "Vector of joint velocities, copied into mjData.qvel when the simulation state is set to this keyframe." + ) + + double[] mjc:act ( + doc = "Vector of actuator activations, copied into mjData.act when the simulation state is set to this keyframe." + ) + + double[] mjc:ctrl ( + doc = "Vector of controls, copied into mjData.ctrl when the simulation state is set to this keyframe." + ) + + double[] mjc:mpos ( + doc = "Vector of mocap body positions, copied into mjData.mocap_pos when the simulation state is set to this keyframe." + ) + + double[] mjc:mquat ( + doc = "Vector of mocap body quaternions, copied into mjData.mocap_quat when the simulation state is set to this keyframe." + ) +} + +class "PhysicsJointsAPI" +( + customData = { + string className = "JointAPI" + } + doc = """API describing a Mujoco joint.""" + + inherits = +) +{ + uniform double[] mjc:springdamper = [0, 0] ( + doc = "When both numbers are positive, the compiler will override any stiffness and damping values specified with the attributes below, and will instead set them automatically so that the resulting mass-spring-damper for this joint has the desired time constant (first value) and damping ratio (second value). This is done by taking into account the joint inertia in the model reference configuration. Note that the format is the same as the solref parameter of the constraint solver." + ) + + uniform double[] mjc:solreflimit = [0.02, 1.0] ( + doc = "Constraint solver parameters for simulating joint limits." + ) + + uniform double[] mjc:solimplimit = [0.9, 0.95, 0.001, 0.5, 2.0] ( + doc = "Constraint solver parameters for simulating joint limits." + ) + + uniform double[] mjc:solreffriction = [0.02, 1.0] ( + doc = "Constraint solver parameters for simulating dry friction." + ) + + uniform double[] mjc:solimpfriction = [0.9, 0.95, 0.001, 0.5, 2.0] ( + doc = "Constraint solver parameters for simulating dry friction." + ) + + uniform double mjc:stiffness = 0 ( + doc = "Joint stiffness. If this value is positive, a spring will be created with equilibrium position given by springref below. The spring force is computed along with the other passive forces." + ) + + uniform double mjc:actuatorfrcrange:min = 0 ( + doc = "Minimum range for clamping total actuator forces acting on this joint. See Force limits for details. It is available only for scalar joints (hinge and slider) and ignored for ball and free joints. The compiler expects the first value to be smaller than the second value. Setting this attribute without specifying actuatorfrclimited is an error if compiler-autolimits is 'false'." + ) + + uniform double mjc:actuatorfrcrange:max = 0 ( + doc = "Maximum range for clamping total actuator forces acting on this joint. See Force limits for details. It is available only for scalar joints (hinge and slider) and ignored for ball and free joints. The compiler expects the first value to be smaller than the second value. Setting this attribute without specifying actuatorfrclimited is an error if compiler-autolimits is 'false'." + ) + + uniform token mjc:actuatorfrclimited = "auto" ( + doc = "This attribute specifies whether actuator forces acting on the joint should be clamped. See Force limits for details. It is available only for scalar joints (hinge and slider) and ignored for ball and free joints. This attribute interacts with the actuatorfrcrange attribute. If this attribute is 'false', actuator force clamping is disabled. If it is 'true', actuator force clamping is enabled. If this attribute is 'auto', and autolimits is set in compiler, actuator force clamping will be enabled if actuatorfrcrange is defined." + allowedTokens = ["false", "true", "auto"] + ) + + uniform bool mjc:actuatorgravcomp = false ( + doc = "If this flag is enabled, gravity compensation applied to this joint is added to actuator forces (mjData.qfrc_actuator) rather than passive forces (mjData.qfrc_passive). Notionally, this means that gravity compensation is the result of a control system rather than natural buoyancy. In practice, enabling this flag is useful when joint-level actuator force clamping is used. In this case, the total actuation force applied on a joint, including gravity compensation, is guaranteed to not exceed the specified limits. See Force limits and actuatorfrcrange for more details on this type of force limit." + ) + + uniform double mjc:margin = 0 ( + doc = "The distance threshold below which limits become active. Recall that the Constraint solver normally generates forces as soon as a constraint becomes active, even if the margin parameter makes that happen at a distance. This attribute together with solreflimit and solimplimit can be used to model a soft joint limit." + ) + + uniform double mjc:ref = 0 ( + doc = "The reference position or angle of the joint. This attribute is only used for slide and hinge joints. It defines the joint value corresponding to the initial model configuration. The amount of spatial transformation that the joint applies at runtime equals the current joint value stored in mjData.qpos minus this reference value stored in mjModel.qpos0. The meaning of these vectors was discussed in the Stand-alone section in the Overview chapter." + ) + + uniform double mjc:springref = 0 ( + doc = "The joint position or angle in which the joint spring (if any) achieves equilibrium. Similar to the vector mjModel.qpos0 which stores all joint reference values specified with the ref attribute above, all spring reference values specified with this attribute are stored in the vector mjModel.qpos_spring. The model configuration corresponding to mjModel.qpos_spring is also used to compute the spring reference lengths of all tendons, stored in mjModel.tendon_lengthspring. This is because tendons can also have springs." + ) + + uniform double mjc:armature = 0 ( + doc = "Additional inertia associated with movement of the joint that is not due to body mass. This added inertia is usually due to a rotor (a.k.a armature) spinning faster than the joint itself due to a geared transmission. The value applies to all degrees of freedom created by this joint. Besides increasing the realism of joints with geared transmission, positive armature significantly improves simulation stability, even for small values, and is a recommended possible fix when encountering stability issues." + ) + + uniform double mjc:damping = 0 ( + doc = "Damping applied to all degrees of freedom created by this joint. Unlike friction loss which is computed by the constraint solver, damping is simply a force linear in velocity. It is included in the passive forces. Despite this simplicity, larger damping values can make numerical integrators unstable, which is why our Euler integrator handles damping implicitly. See Integration in the Computation chapter." + ) + + uniform double mjc:frictionloss = 0 ( + doc = "Friction loss due to dry friction. This value is the same for all degrees of freedom created by this joint. Semantically friction loss does not make sense for free joints, but the compiler allows it. To enable friction loss, set this attribute to a positive value." + ) +} diff --git a/src/experimental/usd/mjcPhysics/siteAPI.cpp b/src/experimental/usd/mjcPhysics/siteAPI.cpp index 1693dd16..d9269b41 100644 --- a/src/experimental/usd/mjcPhysics/siteAPI.cpp +++ b/src/experimental/usd/mjcPhysics/siteAPI.cpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "./siteAPI.h" +#include -#include "pxr/usd/sdf/assetPath.h" -#include "pxr/usd/sdf/types.h" -#include "pxr/usd/usd/schemaRegistry.h" -#include "pxr/usd/usd/typed.h" +#include +#include +#include +#include PXR_NAMESPACE_OPEN_SCOPE diff --git a/src/experimental/usd/mjcPhysics/tokens.cpp b/src/experimental/usd/mjcPhysics/tokens.cpp index 2f33d763..de990c4c 100644 --- a/src/experimental/usd/mjcPhysics/tokens.cpp +++ b/src/experimental/usd/mjcPhysics/tokens.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "./tokens.h" +#include PXR_NAMESPACE_OPEN_SCOPE @@ -33,18 +33,25 @@ MjcPhysicsTokensType::MjcPhysicsTokensType() implicitfast("implicitfast", TfToken::Immortal), integrator("integrator", TfToken::Immortal), legacy("legacy", TfToken::Immortal), + mjcAct("mjc:act", TfToken::Immortal), mjcActDim("mjc:actDim", TfToken::Immortal), mjcActEarly("mjc:actEarly", TfToken::Immortal), mjcActLimited("mjc:actLimited", TfToken::Immortal), mjcActRangeMax("mjc:actRange:max", TfToken::Immortal), mjcActRangeMin("mjc:actRange:min", TfToken::Immortal), + mjcActuatorfrclimited("mjc:actuatorfrclimited", TfToken::Immortal), + mjcActuatorfrcrangeMax("mjc:actuatorfrcrange:max", TfToken::Immortal), + mjcActuatorfrcrangeMin("mjc:actuatorfrcrange:min", TfToken::Immortal), + mjcActuatorgravcomp("mjc:actuatorgravcomp", TfToken::Immortal), + mjcArmature("mjc:armature", TfToken::Immortal), mjcBiasPrm("mjc:biasPrm", TfToken::Immortal), mjcBiasType("mjc:biasType", TfToken::Immortal), mjcCrankLength("mjc:crankLength", TfToken::Immortal), - mjcCrankSite("mjc:crankSite", TfToken::Immortal), + mjcCtrl("mjc:ctrl", TfToken::Immortal), mjcCtrlLimited("mjc:ctrlLimited", TfToken::Immortal), mjcCtrlRangeMax("mjc:ctrlRange:max", TfToken::Immortal), mjcCtrlRangeMin("mjc:ctrlRange:min", TfToken::Immortal), + mjcDamping("mjc:damping", TfToken::Immortal), mjcDynPrm("mjc:dynPrm", TfToken::Immortal), mjcDynType("mjc:dynType", TfToken::Immortal), mjcFlagActuation("mjc:flag:actuation", TfToken::Immortal), @@ -73,6 +80,7 @@ MjcPhysicsTokensType::MjcPhysicsTokensType() mjcForceLimited("mjc:forceLimited", TfToken::Immortal), mjcForceRangeMax("mjc:forceRange:max", TfToken::Immortal), mjcForceRangeMin("mjc:forceRange:min", TfToken::Immortal), + mjcFrictionloss("mjc:frictionloss", TfToken::Immortal), mjcGainPrm("mjc:gainPrm", TfToken::Immortal), mjcGainType("mjc:gainType", TfToken::Immortal), mjcGear("mjc:gear", TfToken::Immortal), @@ -80,6 +88,9 @@ MjcPhysicsTokensType::MjcPhysicsTokensType() mjcJointInParent("mjc:jointInParent", TfToken::Immortal), mjcLengthRangeMax("mjc:lengthRange:max", TfToken::Immortal), mjcLengthRangeMin("mjc:lengthRange:min", TfToken::Immortal), + mjcMargin("mjc:margin", TfToken::Immortal), + mjcMpos("mjc:mpos", TfToken::Immortal), + mjcMquat("mjc:mquat", TfToken::Immortal), mjcOptionActuatorgroupdisable("mjc:option:actuatorgroupdisable", TfToken::Immortal), mjcOptionApirate("mjc:option:apirate", TfToken::Immortal), @@ -109,9 +120,19 @@ MjcPhysicsTokensType::MjcPhysicsTokensType() mjcOptionTolerance("mjc:option:tolerance", TfToken::Immortal), mjcOptionViscosity("mjc:option:viscosity", TfToken::Immortal), mjcOptionWind("mjc:option:wind", TfToken::Immortal), + mjcQpos("mjc:qpos", TfToken::Immortal), + mjcQvel("mjc:qvel", TfToken::Immortal), + mjcRef("mjc:ref", TfToken::Immortal), mjcRefSite("mjc:refSite", TfToken::Immortal), mjcShellinertia("mjc:shellinertia", TfToken::Immortal), mjcSliderSite("mjc:sliderSite", TfToken::Immortal), + mjcSolimpfriction("mjc:solimpfriction", TfToken::Immortal), + mjcSolimplimit("mjc:solimplimit", TfToken::Immortal), + mjcSolreffriction("mjc:solreffriction", TfToken::Immortal), + mjcSolreflimit("mjc:solreflimit", TfToken::Immortal), + mjcSpringdamper("mjc:springdamper", TfToken::Immortal), + mjcSpringref("mjc:springref", TfToken::Immortal), + mjcStiffness("mjc:stiffness", TfToken::Immortal), muscle("muscle", TfToken::Immortal), newton("newton", TfToken::Immortal), none("none", TfToken::Immortal), @@ -123,8 +144,10 @@ MjcPhysicsTokensType::MjcPhysicsTokensType() true_("true", TfToken::Immortal), user("user", TfToken::Immortal), CollisionAPI("CollisionAPI", TfToken::Immortal), + Keyframe("Keyframe", TfToken::Immortal), MeshCollisionAPI("MeshCollisionAPI", TfToken::Immortal), PhysicsActuatorAPI("PhysicsActuatorAPI", TfToken::Immortal), + PhysicsJointsAPI("PhysicsJointsAPI", TfToken::Immortal), SceneAPI("SceneAPI", TfToken::Immortal), SiteAPI("SiteAPI", TfToken::Immortal), allTokens({affine, @@ -143,18 +166,25 @@ MjcPhysicsTokensType::MjcPhysicsTokensType() implicitfast, integrator, legacy, + mjcAct, mjcActDim, mjcActEarly, mjcActLimited, mjcActRangeMax, mjcActRangeMin, + mjcActuatorfrclimited, + mjcActuatorfrcrangeMax, + mjcActuatorfrcrangeMin, + mjcActuatorgravcomp, + mjcArmature, mjcBiasPrm, mjcBiasType, mjcCrankLength, - mjcCrankSite, + mjcCtrl, mjcCtrlLimited, mjcCtrlRangeMax, mjcCtrlRangeMin, + mjcDamping, mjcDynPrm, mjcDynType, mjcFlagActuation, @@ -183,6 +213,7 @@ MjcPhysicsTokensType::MjcPhysicsTokensType() mjcForceLimited, mjcForceRangeMax, mjcForceRangeMin, + mjcFrictionloss, mjcGainPrm, mjcGainType, mjcGear, @@ -190,6 +221,9 @@ MjcPhysicsTokensType::MjcPhysicsTokensType() mjcJointInParent, mjcLengthRangeMax, mjcLengthRangeMin, + mjcMargin, + mjcMpos, + mjcMquat, mjcOptionActuatorgroupdisable, mjcOptionApirate, mjcOptionCcd_iterations, @@ -216,9 +250,19 @@ MjcPhysicsTokensType::MjcPhysicsTokensType() mjcOptionTolerance, mjcOptionViscosity, mjcOptionWind, + mjcQpos, + mjcQvel, + mjcRef, mjcRefSite, mjcShellinertia, mjcSliderSite, + mjcSolimpfriction, + mjcSolimplimit, + mjcSolreffriction, + mjcSolreflimit, + mjcSpringdamper, + mjcSpringref, + mjcStiffness, muscle, newton, none, @@ -230,8 +274,10 @@ MjcPhysicsTokensType::MjcPhysicsTokensType() true_, user, CollisionAPI, + Keyframe, MeshCollisionAPI, PhysicsActuatorAPI, + PhysicsJointsAPI, SceneAPI, SiteAPI}) {} diff --git a/src/experimental/usd/plugins/CMakeLists.txt b/src/experimental/usd/plugins/CMakeLists.txt deleted file mode 100644 index 6e506493..00000000 --- a/src/experimental/usd/plugins/CMakeLists.txt +++ /dev/null @@ -1,137 +0,0 @@ -# Copyright 2025 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 -# -# https://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. - - -# Plugin target name (used for library and plugInfo.json) -set(MJCF_PLUGIN_TARGET_NAME usdMjcf) - -add_library(${MJCF_PLUGIN_TARGET_NAME} SHARED - mjcf/mjcf_file_format.cc - mjcf/mjcf_file_format.h - mjcf/mujoco_to_usd.cc - mjcf/mujoco_to_usd.h - mjcf/utils.cc - mjcf/utils.h -) - -# We need to set the visibility to default until core type symbol visibility -# is resolved in OpenUSD https://github.com/PixarAnimationStudios/OpenUSD/issues/1475 -# Otherwise we will run into issues during composition on MacOS due to std::type_info -# comparisons failing for pxr::TfTokenVector and the like that we place in SdfAbstractData. -set_target_properties(${MJCF_PLUGIN_TARGET_NAME} PROPERTIES - OUTPUT_NAME ${MJCF_PLUGIN_TARGET_NAME} - CXX_VISIBILITY_PRESET default -) - -target_include_directories(${MJCF_PLUGIN_TARGET_NAME} PRIVATE - "${CMAKE_CURRENT_SOURCE_DIR}" -) - -if (MUJOCO_USD_TARGET STREQUAL "USD") - find_package(pxr REQUIRED) - - # --- Link Dependencies --- - # Link against the necessary OpenUSD components - target_link_libraries(${MJCF_PLUGIN_TARGET_NAME} PRIVATE - usd - ar - kind - tf - gf - vt - usdShade - usdLux - usdGeom - usdImaging - usdPhysics - mujoco - tinyxml2 - ) -elseif (MUJOCO_USD_TARGET STREQUAL "Houdini") - - if (NOT DEFINED ENV{HFS}) - message(FATAL_ERROR "Environment variable 'HFS' is not defined: $ENV{HFS}. Please run houdini_setup.") - endif() - - # In Houdini, the Houdini package we would typically use via find_package - # does not have all the USD dependencies that we need (namely UsdPhysics) - # so we need to manually link all the required libraries. - - set(HFS_ENV "$ENV{HFS}") - set(HOUDINI_LIBS "${HFS_ENV}/../Libraries") - get_filename_component(HOUDINI_LIBS "${HOUDINI_LIBS}" ABSOLUTE) # Normalize the path - target_link_directories(${MJCF_PLUGIN_TARGET_NAME} PRIVATE ${HOUDINI_LIBS}) - - target_include_directories(${MJCF_PLUGIN_TARGET_NAME} PRIVATE - "${HFS_ENV}/toolkit/include" - "${HFS_ENV}/toolkit/include/python3.11" - ) - - # Assume everyone using Houdini on 3.11 for now. - set(USD_MJCF_PYTHON_LIB python3.11) - set(USD_MJCF_PYTHON_LIB_NUMBER python311) - set(PYTHON_LIB "${HFS_ENV}/Frameworks/Python.framework/Versions/3.11/Python") - set(PXR_LIB_PREFIX "pxr_") - - # --- Link Dependencies --- - # Link against the necessary OpenUSD components - target_link_libraries(${MJCF_PLUGIN_TARGET_NAME} PRIVATE - ${PXR_LIB_PREFIX}usd - ${PXR_LIB_PREFIX}ar - ${PXR_LIB_PREFIX}kind - ${PXR_LIB_PREFIX}tf - ${PXR_LIB_PREFIX}gf - ${PXR_LIB_PREFIX}vt - ${PXR_LIB_PREFIX}sdf - ${PXR_LIB_PREFIX}usdShade - ${PXR_LIB_PREFIX}usdLux - ${PXR_LIB_PREFIX}usdGeom - ${PXR_LIB_PREFIX}usdImaging - ${PXR_LIB_PREFIX}usdPhysics - tbb - hboost_${USD_MJCF_PYTHON_LIB_NUMBER} - ${PYTHON_LIB} - mujoco - tinyxml2 - ) -endif() - - -# --- Generate plugInfo.json --- -if(CMAKE_SHARED_LIBRARY_PREFIX) - set(LIB_PREFIX ${CMAKE_SHARED_LIBRARY_PREFIX}) # Usually "lib" on Unix -else() - set(LIB_PREFIX "") -endif() -set(PLUG_INFO_LIBRARY_PATH "${LIB_PREFIX}${MJCF_PLUGIN_TARGET_NAME}${CMAKE_SHARED_LIBRARY_SUFFIX}") - -# --- Installation --- - -set(USD_PLUGIN_INSTALL_DIR_LIB ${CMAKE_INSTALL_LIBDIR}/usdMjcf) - -message(STATUS "Copying plugInfo.json to ${CMAKE_BINARY_DIR}/${USD_PLUGIN_INSTALL_DIR_LIB}/plugInfo.json") -configure_file( - mjcf/plugInfo.json - ${CMAKE_BINARY_DIR}/${USD_PLUGIN_INSTALL_DIR_LIB}/plugInfo.json -) - -install(FILES ${CMAKE_BINARY_DIR}/${USD_PLUGIN_INSTALL_DIR_LIB}/plugInfo.json DESTINATION ${USD_PLUGIN_INSTALL_DIR_LIB}) - -# Install shared lib and plugInfo to same location for simplicity. -install(TARGETS ${MJCF_PLUGIN_TARGET_NAME} - LIBRARY DESTINATION ${USD_PLUGIN_INSTALL_DIR_LIB} -) - -message(STATUS "USD MJCF Plugin will be installed to: ${CMAKE_INSTALL_PREFIX}/${USD_PLUGIN_INSTALL_DIR_LIB}") -message(STATUS "Make sure PXR_PLUGINPATH_NAME includes: ${CMAKE_INSTALL_PREFIX}/${USD_PLUGIN_INSTALL_DIR_LIB}") diff --git a/src/experimental/usd/plugins/mjcf/mjcf_file_format.cc b/src/experimental/usd/plugins/mjcf/mjcf_file_format.cc index 904c84fd..180986b7 100644 --- a/src/experimental/usd/plugins/mjcf/mjcf_file_format.cc +++ b/src/experimental/usd/plugins/mjcf/mjcf_file_format.cc @@ -21,6 +21,7 @@ #include #include "mjcf/mujoco_to_usd.h" +#include "tinyxml2.h" #include #include #include @@ -37,7 +38,6 @@ #include #include #include -#include "tinyxml2.h" PXR_NAMESPACE_OPEN_SCOPE diff --git a/src/experimental/usd/plugins/mjcf/mujoco_to_usd.cc b/src/experimental/usd/plugins/mjcf/mujoco_to_usd.cc index aa5f7232..0a5ee0e2 100644 --- a/src/experimental/usd/plugins/mjcf/mujoco_to_usd.cc +++ b/src/experimental/usd/plugins/mjcf/mujoco_to_usd.cc @@ -21,8 +21,9 @@ #include #include +#include +#include #include -#include "third_party/mujoco/src/experimental/usd/mjcPhysics/tokens.h" #include "mjcf/utils.h" #include #include @@ -54,6 +55,11 @@ #include #include #include +#include +#include +#include +#include +#include #include #include #include @@ -77,7 +83,7 @@ TF_DEFINE_PRIVATE_TOKENS(kTokens, ((light, "Light")) ((meshScope, "MeshSources")) ((materialsScope, "Materials")) - ((surface, "PreviewSurface")) + ((previewSurface, "PreviewSurface")) ((world, "World")) ((xformOpTransform, "xformOp:transform")) ((xformOpScale, "xformOp:scale")) @@ -90,10 +96,18 @@ TF_DEFINE_PRIVATE_TOKENS(kTokens, ((inputsWrapS, "inputs:wrapS")) ((inputsWrapT, "inputs:wrapT")) ((inputsDiffuseColor, "inputs:diffuseColor")) + ((inputsEmissiveColor, "inputs:emissiveColor")) ((outputsRgb, "outputs:rgb")) + ((outputsR, "outputs:r")) + ((outputsG, "outputs:g")) + ((outputsB, "outputs:b")) ((inputsMetallic, "inputs:metallic")) + ((inputsOcclusion, "inputs:occlusion")) + ((inputsRoughness, "inputs:roughness")) (repeat) ((sourceMesh, pxr::UsdGeomTokens->Mesh)) + ((inputsNormal, "inputs:normal")) + ((joint, "Joint")) ); // Using to satisfy TF_REGISTRY_FUNCTION macro below and avoid operating in PXR_NS. @@ -157,10 +171,14 @@ class ModelWriter { : spec_(spec), model_(model), data_(data), class_path_("/Bad_Path") { body_paths_ = std::vector(model->nbody); site_paths_ = std::vector(model->nsite); + joint_paths_ = std::vector(model->njnt); } ~ModelWriter() { mj_deleteModel(model_); } void Write(bool write_physics) { + // Set working parameters. + write_physics_ = write_physics; + // Create top level class holder. class_path_ = CreateClassSpec(data_, pxr::SdfPath::AbsoluteRootPath(), pxr::TfToken("__class__")); @@ -168,9 +186,6 @@ class ModelWriter { // Create the world body. body_paths_[kWorldIndex] = WriteWorldBody(kWorldIndex); - // Set working parameters. - write_physics_ = write_physics; - SetLayerMetadata(data_, pxr::SdfFieldKeys->Documentation, "Generated by mujoco model writer."); // Mujoco is Z up by default. @@ -207,6 +222,8 @@ class ModelWriter { std::vector body_paths_; // Mapping from Mujoco site id to SdfPath. std::vector site_paths_; + // Mapping from Mujoco joint id to SdfPath. + std::vector joint_paths_; // Mapping from mesh names to Mesh prim path. std::unordered_map mesh_paths_; // Whether to write physics data. @@ -540,13 +557,13 @@ class ModelWriter { MjcPhysicsTokens->mjcOptionMagnetic, magnetic); pxr::VtArray o_solref(spec_->option.o_solref, - spec_->option.o_solref + 2); + spec_->option.o_solref + mjNREF); WriteUniformAttribute(physics_scene_path, pxr::SdfValueTypeNames->DoubleArray, MjcPhysicsTokens->mjcOptionO_solref, o_solref); pxr::VtArray o_solimp(spec_->option.o_solimp, - spec_->option.o_solimp + 5); + spec_->option.o_solimp + mjNIMP); WriteUniformAttribute(physics_scene_path, pxr::SdfValueTypeNames->DoubleArray, MjcPhysicsTokens->mjcOptionO_solimp, o_solimp); @@ -630,12 +647,10 @@ class ModelWriter { } } - pxr::SdfPath AddTextureShader(const pxr::SdfPath &material_path, - const char *texture_file) { - // Shader "uvmap" + pxr::SdfPath AddUVTextureShader(const pxr::SdfPath &material_path, + const pxr::TfToken &name) { pxr::SdfPath uvmap_shader_path = - CreatePrimSpec(data_, material_path, pxr::TfToken("uvmap"), - pxr::UsdShadeTokens->Shader); + CreatePrimSpec(data_, material_path, name, pxr::UsdShadeTokens->Shader); pxr::SdfPath uvmap_info_id_attr = CreateAttributeSpec( data_, uvmap_shader_path, pxr::UsdShadeTokens->infoId, @@ -652,10 +667,15 @@ class ModelWriter { CreateAttributeSpec(data_, uvmap_shader_path, kTokens->outputsSt, pxr::SdfValueTypeNames->Float2); - // Shader "texture" + return uvmap_st_output_attr; + } + + std::vector AddTextureShader( + const pxr::SdfPath &material_path, const char *texture_file, + const pxr::TfToken &name, const pxr::SdfPath &uvmap_st_output_attr, + const std::vector &output_channels) { pxr::SdfPath texture_shader_path = - CreatePrimSpec(data_, material_path, pxr::TfToken("texture"), - pxr::UsdShadeTokens->Shader); + CreatePrimSpec(data_, material_path, name, pxr::UsdShadeTokens->Shader); pxr::SdfPath texture_info_id_attr = CreateAttributeSpec( data_, texture_shader_path, pxr::UsdShadeTokens->infoId, pxr::SdfValueTypeNames->Token, pxr::SdfVariabilityUniform); @@ -683,78 +703,216 @@ class ModelWriter { pxr::SdfValueTypeNames->Token); SetAttributeDefault(data_, texture_wrap_t_attr, kTokens->repeat); - pxr::SdfPath texture_rgb_output_attr = - CreateAttributeSpec(data_, texture_shader_path, kTokens->outputsRgb, - pxr::SdfValueTypeNames->Float3); - - return texture_rgb_output_attr; + std::vector texture_output_attrs; + for (const auto &output_channel : output_channels) { + pxr::SdfValueTypeName value_type; + if (output_channel == kTokens->outputsRgb) { + value_type = pxr::SdfValueTypeNames->Float3; + } else { + // Assume the other specified channels are outputR, outputG, outputB. + value_type = pxr::SdfValueTypeNames->Float; + } + texture_output_attrs.push_back(CreateAttributeSpec( + data_, texture_shader_path, output_channel, value_type)); + } + return texture_output_attrs; } void WriteMaterial(mjsMaterial *material, const pxr::SdfPath &parent_path) { + // Create a Material prim. auto name = GetAvailablePrimName( *material->name, pxr::UsdShadeTokens->Material, parent_path); pxr::SdfPath material_path = CreatePrimSpec(data_, parent_path, name, pxr::UsdShadeTokens->Material); - // Shader "PreviewSurface" - pxr::SdfPath preview_surface_shader_path = CreatePrimSpec( - data_, material_path, kTokens->surface, pxr::UsdShadeTokens->Shader); + // Create a Shader prim "PreviewSurface" under the Material prim. + pxr::SdfPath preview_surface_shader_path = + CreatePrimSpec(data_, material_path, kTokens->previewSurface, + pxr::UsdShadeTokens->Shader); + // Set the Shader'sinfoId attribute to UsdPreviewSurface, a standard surface + // shader. pxr::SdfPath info_id_attr = CreateAttributeSpec( data_, preview_surface_shader_path, pxr::UsdShadeTokens->infoId, pxr::SdfValueTypeNames->Token, pxr::SdfVariabilityUniform); SetAttributeDefault(data_, info_id_attr, pxr::UsdImagingTokens->UsdPreviewSurface); + // Connect material's surface output to the preview surface's surface + // output. pxr::SdfPath surface_output_attr = CreateAttributeSpec( data_, preview_surface_shader_path, pxr::UsdShadeTokens->outputsSurface, pxr::SdfValueTypeNames->Token); + pxr::SdfPath material_surface_output_attr = CreateAttributeSpec( + data_, material_path, pxr::UsdShadeTokens->outputsSurface, + pxr::SdfValueTypeNames->Token); + AddAttributeConnection(data_, material_surface_output_attr, + surface_output_attr); + // Connect material's displacement output to the preview surface's + // displacement output. pxr::SdfPath displacement_output_attr = CreateAttributeSpec(data_, preview_surface_shader_path, pxr::UsdShadeTokens->outputsDisplacement, pxr::SdfValueTypeNames->Token); + pxr::SdfPath material_displacement_output_attr = CreateAttributeSpec( + data_, material_path, pxr::UsdShadeTokens->outputsDisplacement, + pxr::SdfValueTypeNames->Token); + AddAttributeConnection(data_, material_displacement_output_attr, + displacement_output_attr); + // Add an st (uv) Shader, a prim var reader for the UV coordinates. + const pxr::SdfPath &uvmap_st_output_attr = + AddUVTextureShader(material_path, pxr::TfToken("uvmap")); + const mjStringVec &textures = *(material->textures); + + // Set the values of metallic, roughness and occlusion. These can come from + // an ORM packed texture, as individual textures, or as a values defined in + // mjsMaterial_ (with the exception of occlusion). + pxr::SdfPath metallic_attr = CreateAttributeSpec( + data_, preview_surface_shader_path, kTokens->inputsMetallic, + pxr::SdfValueTypeNames->Float); + pxr::SdfPath roughness_attr = CreateAttributeSpec( + data_, preview_surface_shader_path, kTokens->inputsRoughness, + pxr::SdfValueTypeNames->Float); + // Find the occlusion, roughness, and metallic textures. + if (mjTEXROLE_ORM < textures.size()) { + std::string orm_texture_name = textures[mjTEXROLE_ORM]; + mjsTexture *orm_texture = mjs_asTexture( + mjs_findElement(spec_, mjOBJ_TEXTURE, orm_texture_name.c_str())); + std::string occlusion_texture_name = textures[mjTEXROLE_OCCLUSION]; + mjsTexture *occlusion_texture = mjs_asTexture(mjs_findElement( + spec_, mjOBJ_TEXTURE, occlusion_texture_name.c_str())); + std::string roughness_texture_name = textures[mjTEXROLE_ROUGHNESS]; + mjsTexture *roughness_texture = mjs_asTexture(mjs_findElement( + spec_, mjOBJ_TEXTURE, roughness_texture_name.c_str())); + std::string metallic_texture_name = textures[mjTEXROLE_METALLIC]; + mjsTexture *metallic_texture = mjs_asTexture( + mjs_findElement(spec_, mjOBJ_TEXTURE, metallic_texture_name.c_str())); + if (orm_texture) { + // Create the ORM shader and connect its output to the preview + // surface ORM attrs. + const std::vector orm_output_attrs = AddTextureShader( + material_path, orm_texture->file->c_str(), + pxr::TfToken("orm_packed"), uvmap_st_output_attr, + {kTokens->outputsR, kTokens->outputsG, kTokens->outputsB}); + if (orm_output_attrs.size() == 3) { + pxr::SdfPath occlusion_attr = CreateAttributeSpec( + data_, preview_surface_shader_path, kTokens->inputsOcclusion, + pxr::SdfValueTypeNames->Float); + AddAttributeConnection(data_, occlusion_attr, orm_output_attrs[0]); + AddAttributeConnection(data_, roughness_attr, orm_output_attrs[1]); + AddAttributeConnection(data_, metallic_attr, orm_output_attrs[2]); + } + } else { + if (metallic_texture) { + const std::vector metallic_output_attrs = + AddTextureShader(material_path, metallic_texture->file->c_str(), + pxr::TfToken("metallic"), uvmap_st_output_attr, + {kTokens->outputsRgb}); + if (metallic_output_attrs.size() == 1) { + AddAttributeConnection(data_, metallic_attr, + metallic_output_attrs[0]); + } + } else { + SetAttributeDefault(data_, metallic_attr, material->metallic); + } + if (roughness_texture) { + const std::vector roughness_output_attrs = + AddTextureShader(material_path, roughness_texture->file->c_str(), + pxr::TfToken("roughness"), uvmap_st_output_attr, + {kTokens->outputsRgb}); + if (roughness_output_attrs.size() == 1) { + AddAttributeConnection(data_, roughness_attr, + roughness_output_attrs[0]); + } + } else { + SetAttributeDefault(data_, roughness_attr, material->roughness); + } + if (occlusion_texture) { + pxr::SdfPath occlusion_attr = CreateAttributeSpec( + data_, preview_surface_shader_path, kTokens->inputsOcclusion, + pxr::SdfValueTypeNames->Float); + const std::vector occlusion_output_attrs = + AddTextureShader(material_path, occlusion_texture->file->c_str(), + pxr::TfToken("occlusion"), uvmap_st_output_attr, + {kTokens->outputsRgb}); + if (occlusion_output_attrs.size() == 1) { + AddAttributeConnection(data_, occlusion_attr, + occlusion_output_attrs[0]); + } + } + } + } + // Find the normal texture if specified. + if (mjTEXROLE_NORMAL < textures.size()) { + std::string normal_texture_name = textures[mjTEXROLE_NORMAL]; + mjsTexture *normal_texture = mjs_asTexture( + mjs_findElement(spec_, mjOBJ_TEXTURE, normal_texture_name.c_str())); + if (normal_texture) { + pxr::SdfPath normal_attr = CreateAttributeSpec( + data_, preview_surface_shader_path, kTokens->inputsNormal, + pxr::SdfValueTypeNames->Normal3f); + // Create the normal map shader and connect its output to the preview + // surface normal attr. + const std::vector normal_map_output_attrs = + AddTextureShader(material_path, normal_texture->file->c_str(), + pxr::TfToken("normal"), uvmap_st_output_attr, + {kTokens->outputsRgb}); + if (normal_map_output_attrs.size() == 1) { + AddAttributeConnection(data_, normal_attr, + normal_map_output_attrs[0]); + } + } + } + + // Connect an emissive texture if specified. + if (mjTEXROLE_EMISSIVE < textures.size()) { + std::string emissive_texture_name = textures[mjTEXROLE_EMISSIVE]; + mjsTexture *emissive_texture = mjs_asTexture( + mjs_findElement(spec_, mjOBJ_TEXTURE, emissive_texture_name.c_str())); + if (emissive_texture) { + pxr::SdfPath emissive_attr = CreateAttributeSpec( + data_, preview_surface_shader_path, kTokens->inputsEmissiveColor, + pxr::SdfValueTypeNames->Color3f); + const std::vector emissive_map_output_attrs = + AddTextureShader(material_path, emissive_texture->file->c_str(), + pxr::TfToken("emissive"), uvmap_st_output_attr, + {kTokens->outputsRgb}); + if (emissive_map_output_attrs.size() == 1) { + AddAttributeConnection(data_, emissive_attr, + emissive_map_output_attrs[0]); + } + } + } + + // Set the value of diffuse color. This can come from a diffuse texture + // or as a value defined in mjsMaterial_. pxr::SdfPath diffuse_color_attr = CreateAttributeSpec( data_, preview_surface_shader_path, kTokens->inputsDiffuseColor, pxr::SdfValueTypeNames->Color3f); // Find the main texture if specified. - std::string main_texture_name = (*material->textures)[mjTEXROLE_RGB]; + std::string main_texture_name = textures[mjTEXROLE_RGB]; mjsTexture *main_texture = mjs_asTexture( mjs_findElement(spec_, mjOBJ_TEXTURE, main_texture_name.c_str())); if (main_texture) { // Create the texture shader and connect it to the diffuse color // attribute. - pxr::SdfPath texture_rgb_output_attr = - AddTextureShader(material_path, main_texture->file->c_str()); - AddAttributeConnection(data_, diffuse_color_attr, - texture_rgb_output_attr); + const std::vector texture_diffuse_output_attrs = + AddTextureShader(material_path, main_texture->file->c_str(), + pxr::TfToken("diffuse"), uvmap_st_output_attr, + {kTokens->outputsRgb}); + if (texture_diffuse_output_attrs.size() == 1) { + AddAttributeConnection(data_, diffuse_color_attr, + texture_diffuse_output_attrs[0]); + } } else { // If no texture is specified, use the rgba diffuse color. SetAttributeDefault(data_, diffuse_color_attr, pxr::GfVec3f(material->rgba[0], material->rgba[1], material->rgba[2])); } - - pxr::SdfPath metallic_attr = CreateAttributeSpec( - data_, preview_surface_shader_path, kTokens->inputsMetallic, - pxr::SdfValueTypeNames->Float); - SetAttributeDefault(data_, metallic_attr, material->metallic); - - pxr::SdfPath material_surface_output_attr = CreateAttributeSpec( - data_, material_path, pxr::UsdShadeTokens->outputsSurface, - pxr::SdfValueTypeNames->Token); - - AddAttributeConnection(data_, material_surface_output_attr, - surface_output_attr); - - pxr::SdfPath material_displacement_output_attr = CreateAttributeSpec( - data_, material_path, pxr::UsdShadeTokens->outputsDisplacement, - pxr::SdfValueTypeNames->Token); - - AddAttributeConnection(data_, material_displacement_output_attr, - displacement_output_attr); } void WriteMaterials() { @@ -780,6 +938,9 @@ class ModelWriter { actuator->trntype == mjtTrn::mjTRN_SLIDERCRANK) { int site_id = mj_name2id(model_, mjOBJ_SITE, actuator->target->c_str()); transmission_path = site_paths_[site_id]; + } else if (actuator->trntype == mjtTrn::mjTRN_JOINT) { + int joint_id = mj_name2id(model_, mjOBJ_JOINT, actuator->target->c_str()); + transmission_path = joint_paths_[joint_id]; } else { TF_WARN(UnsupportedActuatorTypeError, "Unsupported actuator type for actuator %d", @@ -791,22 +952,23 @@ class ModelWriter { MjcPhysicsTokens->PhysicsActuatorAPI); if (!actuator->refsite->empty()) { - int refsite_id = mj_name2id(model_, mjOBJ_SITE, actuator->refsite->c_str()); + int refsite_id = + mj_name2id(model_, mjOBJ_SITE, actuator->refsite->c_str()); pxr::SdfPath refsite_path = site_paths_[refsite_id]; CreateRelationshipSpec(data_, transmission_path, - MjcPhysicsTokens->mjcRefSite, - refsite_path, pxr::SdfVariabilityUniform); + MjcPhysicsTokens->mjcRefSite, refsite_path, + pxr::SdfVariabilityUniform); } if (!actuator->slidersite->empty()) { - int slidersite_id = mj_name2id(model_, mjOBJ_SITE, actuator->slidersite->c_str()); + int slidersite_id = + mj_name2id(model_, mjOBJ_SITE, actuator->slidersite->c_str()); pxr::SdfPath slidersite_path = site_paths_[slidersite_id]; CreateRelationshipSpec(data_, transmission_path, - MjcPhysicsTokens->mjcSliderSite, - slidersite_path, pxr::SdfVariabilityUniform); + MjcPhysicsTokens->mjcSliderSite, slidersite_path, + pxr::SdfVariabilityUniform); } - const std::vector> limited_attributes = { {MjcPhysicsTokens->mjcCtrlLimited, actuator->ctrllimited}, {MjcPhysicsTokens->mjcForceLimited, actuator->forcelimited}, @@ -872,7 +1034,6 @@ class ModelWriter { MjcPhysicsTokens->mjcDynPrm, pxr::VtDoubleArray(actuator->dynprm, actuator->dynprm + 10)); - pxr::TfToken gain_type; if (actuator->gaintype == mjtGain::mjGAIN_FIXED) { gain_type = MjcPhysicsTokens->fixed; @@ -1345,6 +1506,296 @@ class ModelWriter { } } + void WriteJoints(mjsBody *body) { + if (!write_physics_) return; + + int body_id = mjs_getId(body->element); + if (body_id == kWorldIndex) return; + + mjsJoint *joint = mjs_asJoint(mjs_firstChild(body, mjOBJ_JOINT, false)); + + if (!joint) { + // If no joint is found, then we pass nullptr to create a FixedJoint. + // WriteJoint properly handles the case where the parent is the worldbody. + WriteJoint(nullptr, body); + } else { + WriteJoint(joint, body); + if (mjs_asJoint(mjs_nextChild(body, joint->element, false))) { + TF_WARN( + "Multiple joints found for body %d. Only writing the first one.", + body_id); + } + } + } + + // Write the joint. If null, then a FixedJoint is created. + void WriteJoint(mjsJoint *joint, const mjsBody *parent_mj_body) { + // Default to fixed joint if joint is null. + pxr::TfToken joint_prim_type = pxr::UsdPhysicsTokens->PhysicsFixedJoint; + + int joint_id = -1; + if (joint) { + joint_id = mjs_getId(joint->element); + mjtJoint type = (mjtJoint)model_->jnt_type[joint_id]; + switch (type) { + case mjJNT_FREE: + // Free joints are guaranteed to only ever be on the top-level body so + // we just write no joint. As a top-level body with no joint it will + // be considered a floating-base body. + return; + case mjJNT_HINGE: + joint_prim_type = pxr::UsdPhysicsTokens->PhysicsRevoluteJoint; + break; + case mjJNT_SLIDE: + joint_prim_type = pxr::UsdPhysicsTokens->PhysicsPrismaticJoint; + break; + default: + TF_WARN("Unsupported joint type '%d' for joint '%s'. Skipping.", + (int)type, joint->name->c_str()); + return; + } + } + + int body_id = mjs_getId(parent_mj_body->element); + + // the joint connects the current body as body1, to its parent body as + // body0. + int body1_id_usd = body_id; + int body0_id_usd = model_->body_parentid[body_id]; + + const pxr::SdfPath &body1_path_usd = body_paths_[body1_id_usd]; + auto joint_name = joint ? *joint->name : "FixedJoint"; + pxr::TfToken joint_name_token = + GetAvailablePrimName(joint_name, kTokens->joint, body1_path_usd); + pxr::SdfPath joint_path = CreatePrimSpec(data_, body1_path_usd, + joint_name_token, joint_prim_type); + + // Set body0 and body1 relationships + // For the initial joints that connect to the world, we signal this by + // keeping the body0 relationship empty. + if (body0_id_usd != kWorldIndex) { + const pxr::SdfPath &body0_path_usd = body_paths_[body0_id_usd]; + CreateRelationshipSpec(data_, joint_path, + pxr::UsdPhysicsTokens->physicsBody0, + body0_path_usd, pxr::SdfVariabilityUniform); + } + CreateRelationshipSpec(data_, joint_path, + pxr::UsdPhysicsTokens->physicsBody1, body1_path_usd, + pxr::SdfVariabilityUniform); + + // Joint frame in MuJoCo is defined by jnt_pos and jnt_axis in body1's frame + // For FixedJoint, these are both unity. + pxr::GfVec3d mj_jnt_pos = pxr::GfVec3d(0.0); + pxr::GfVec3d mj_jnt_axis = pxr::GfVec3d(0.0, 0.0, 1.0); + if (joint) { + mj_jnt_pos = pxr::GfVec3d(&model_->jnt_pos[joint_id * 3]); + mj_jnt_axis = pxr::GfVec3d(&model_->jnt_axis[joint_id * 3]); + } + + // Local joint frame for body1 + pxr::GfVec3f local_pos1(mj_jnt_pos); + pxr::GfRotation().SetRotateInto(pxr::GfVec3f::ZAxis(), mj_jnt_axis); + pxr::GfQuatf local_rot1( + pxr::GfRotation() + .SetRotateInto(pxr::GfVec3f::ZAxis(), mj_jnt_axis) + .GetQuat()); + + SetAttributeDefault( + data_, + CreateAttributeSpec(data_, joint_path, + pxr::UsdPhysicsTokens->physicsLocalPos1, + pxr::SdfValueTypeNames->Float3), + local_pos1); + if (joint_prim_type == pxr::UsdPhysicsTokens->PhysicsRevoluteJoint || + joint_prim_type == pxr::UsdPhysicsTokens->PhysicsPrismaticJoint) { + SetAttributeDefault( + data_, + CreateAttributeSpec(data_, joint_path, + pxr::UsdPhysicsTokens->physicsLocalRot1, + pxr::SdfValueTypeNames->Quatf), + local_rot1); + } else { + SetAttributeDefault( + data_, + CreateAttributeSpec(data_, joint_path, + pxr::UsdPhysicsTokens->physicsLocalRot1, + pxr::SdfValueTypeNames->Quatf), + pxr::GfQuatf::GetIdentity()); + } + + // Calculate local joint frame for body0 + pxr::GfMatrix4d body1_transform_local = + MujocoPosQuatToTransform(&model_->body_pos[body1_id_usd * 3], + &model_->body_quat[body1_id_usd * 4]); + pxr::GfVec3d jnt_pos_parent_local = + body1_transform_local.Transform(mj_jnt_pos); + pxr::GfVec3d jnt_axis_parent_local = + body1_transform_local.TransformDir(mj_jnt_axis); + + pxr::GfVec3f local_pos0(jnt_pos_parent_local); + + SetAttributeDefault( + data_, + CreateAttributeSpec(data_, joint_path, + pxr::UsdPhysicsTokens->physicsLocalPos0, + pxr::SdfValueTypeNames->Float3), + local_pos0); + + if (joint_prim_type == pxr::UsdPhysicsTokens->PhysicsRevoluteJoint || + joint_prim_type == pxr::UsdPhysicsTokens->PhysicsPrismaticJoint) { + pxr::GfQuatf other_rot0( + pxr::GfRotation() + .SetRotateInto(pxr::GfVec3f::ZAxis(), jnt_axis_parent_local) + .GetQuat()); + + SetAttributeDefault( + data_, + CreateAttributeSpec(data_, joint_path, + pxr::UsdPhysicsTokens->physicsLocalRot0, + pxr::SdfValueTypeNames->Quatf), + other_rot0); + } else { + // Fixed joints have no frame and no axis per se. We simply need the + // rotation quaternion of the body its on. + SetAttributeDefault( + data_, + CreateAttributeSpec(data_, joint_path, + pxr::UsdPhysicsTokens->physicsLocalRot0, + pxr::SdfValueTypeNames->Quatf), + body1_transform_local.ExtractRotationQuat()); + } + + if (joint) { + mjtJoint type = (mjtJoint)model_->jnt_type[joint_id]; + + // Joint-specific attributes + if (type == mjJNT_HINGE || type == mjJNT_SLIDE) { + // The joint motion occurs around/along the Z-axis of the joint frame + // established by localRot0/1. + SetAttributeDefault( + data_, + CreateAttributeSpec(data_, joint_path, + pxr::UsdPhysicsTokens->physicsAxis, + pxr::SdfValueTypeNames->Token), + pxr::UsdPhysicsTokens->z); // "Z" axis + } + + if (model_->jnt_limited[joint_id]) { + float lower_limit = model_->jnt_range[joint_id * 2]; + float upper_limit = model_->jnt_range[joint_id * 2 + 1]; + + if (type == mjJNT_HINGE) { + // Convert radians to degrees for USD + // As per the XML Reference, "mjModel always uses radians" + lower_limit *= (180.0 / mjPI); + upper_limit *= (180.0 / mjPI); + SetAttributeDefault( + data_, + CreateAttributeSpec(data_, joint_path, + pxr::UsdPhysicsTokens->physicsLowerLimit, + pxr::SdfValueTypeNames->Float), + lower_limit); + SetAttributeDefault( + data_, + CreateAttributeSpec(data_, joint_path, + pxr::UsdPhysicsTokens->physicsUpperLimit, + pxr::SdfValueTypeNames->Float), + upper_limit); + } else if (type == mjJNT_SLIDE) { + SetAttributeDefault( + data_, + CreateAttributeSpec(data_, joint_path, + pxr::UsdPhysicsTokens->physicsLowerLimit, + pxr::SdfValueTypeNames->Float), + lower_limit); + SetAttributeDefault( + data_, + CreateAttributeSpec(data_, joint_path, + pxr::UsdPhysicsTokens->physicsUpperLimit, + pxr::SdfValueTypeNames->Float), + upper_limit); + } + } + + // Finally write the mjcPhysicsJointAPI attributes. + ApplyApiSchema(data_, joint_path, MjcPhysicsTokens->PhysicsJointsAPI); + + WriteUniformAttribute( + joint_path, pxr::SdfValueTypeNames->DoubleArray, + MjcPhysicsTokens->mjcSpringdamper, + pxr::VtArray(joint->springdamper, joint->springdamper + 2)); + + WriteUniformAttribute(joint_path, pxr::SdfValueTypeNames->DoubleArray, + MjcPhysicsTokens->mjcSolreflimit, + pxr::VtArray(joint->solref_limit, + joint->solref_limit + mjNREF)); + + WriteUniformAttribute(joint_path, pxr::SdfValueTypeNames->DoubleArray, + MjcPhysicsTokens->mjcSolimplimit, + pxr::VtArray(joint->solimp_limit, + joint->solimp_limit + mjNIMP)); + + WriteUniformAttribute( + joint_path, pxr::SdfValueTypeNames->DoubleArray, + MjcPhysicsTokens->mjcSolreffriction, + pxr::VtArray(joint->solref_friction, + joint->solref_friction + mjNREF)); + + WriteUniformAttribute( + joint_path, pxr::SdfValueTypeNames->DoubleArray, + MjcPhysicsTokens->mjcSolimpfriction, + pxr::VtArray(joint->solimp_friction, + joint->solimp_friction + mjNIMP)); + + WriteUniformAttribute(joint_path, pxr::SdfValueTypeNames->Double, + MjcPhysicsTokens->mjcStiffness, joint->stiffness); + + WriteUniformAttribute(joint_path, pxr::SdfValueTypeNames->Double, + MjcPhysicsTokens->mjcActuatorfrcrangeMin, + joint->actfrcrange[0]); + + WriteUniformAttribute(joint_path, pxr::SdfValueTypeNames->Double, + MjcPhysicsTokens->mjcActuatorfrcrangeMax, + joint->actfrcrange[1]); + + pxr::TfToken actuatorfrclimited_token = MjcPhysicsTokens->auto_; + if (joint->actfrclimited == mjLIMITED_TRUE) { + actuatorfrclimited_token = MjcPhysicsTokens->true_; + } else if (joint->actfrclimited == mjLIMITED_FALSE) { + actuatorfrclimited_token = MjcPhysicsTokens->false_; + } + WriteUniformAttribute(joint_path, pxr::SdfValueTypeNames->Token, + MjcPhysicsTokens->mjcActuatorfrclimited, + actuatorfrclimited_token); + + WriteUniformAttribute(joint_path, pxr::SdfValueTypeNames->Bool, + MjcPhysicsTokens->mjcActuatorgravcomp, + static_cast(joint->actgravcomp)); + + WriteUniformAttribute(joint_path, pxr::SdfValueTypeNames->Double, + MjcPhysicsTokens->mjcMargin, joint->margin); + + WriteUniformAttribute(joint_path, pxr::SdfValueTypeNames->Double, + MjcPhysicsTokens->mjcRef, joint->ref); + + WriteUniformAttribute(joint_path, pxr::SdfValueTypeNames->Double, + MjcPhysicsTokens->mjcSpringref, joint->springref); + + WriteUniformAttribute(joint_path, pxr::SdfValueTypeNames->Double, + MjcPhysicsTokens->mjcArmature, joint->armature); + + WriteUniformAttribute(joint_path, pxr::SdfValueTypeNames->Double, + MjcPhysicsTokens->mjcDamping, joint->damping); + + WriteUniformAttribute(joint_path, pxr::SdfValueTypeNames->Double, + MjcPhysicsTokens->mjcFrictionloss, + joint->frictionloss); + } + if (joint_id >= 0) { + joint_paths_[joint_id] = joint_path; + } + } + void WriteCamera(mjsCamera *spec_cam, const mjsBody *body) { const auto &body_path = body_paths_[mjs_getId(body->element)]; auto name = GetAvailablePrimName(*spec_cam->name, @@ -1444,6 +1895,16 @@ class ModelWriter { : pxr::KindTokens->subcomponent; SetPrimKind(data_, body_path, kind); + // If the parent is not the world body, but is child of the world body + // then we need to apply the articulation root API. + if (parent_id != kWorldIndex) { + int parent_parent_id = mjs_getId(mjs_getParent(parent->element)->element); + if (parent_parent_id == kWorldIndex) { + ApplyApiSchema(data_, parent_path, + pxr::UsdPhysicsTokens->PhysicsArticulationRootAPI); + } + } + // Apply the PhysicsRigidBodyAPI schema if we are writing physics. if (write_physics_) { // If the body had a mass specified then it must have either inertia or @@ -1486,17 +1947,6 @@ class ModelWriter { ApplyApiSchema(data_, body_path, pxr::UsdPhysicsTokens->PhysicsRigidBodyAPI); - - // If the parent is not the world body, but is child of the world body - // then we need to apply the articulation root API. - if (parent_id != kWorldIndex) { - int parent_parent_id = - mjs_getId(mjs_getParent(parent->element)->element); - if (parent_parent_id == kWorldIndex) { - ApplyApiSchema(data_, parent_path, - pxr::UsdPhysicsTokens->PhysicsArticulationRootAPI); - } - } } // Create classes if necessary @@ -1543,6 +1993,7 @@ class ModelWriter { } WriteSites(body); WriteGeoms(body); + WriteJoints(body); WriteCameras(body); WriteLights(body); body = mjs_asBody(mjs_nextElement(spec_, body->element)); @@ -1558,6 +2009,7 @@ class ModelWriter { CreatePrimSpec(data_, pxr::SdfPath::AbsoluteRootPath(), name, pxr::UsdGeomTokens->Xform); SetPrimKind(data_, world_group_path, pxr::KindTokens->group); + return world_group_path; } }; diff --git a/src/experimental/usd/usd_to_mjspec.cc b/src/experimental/usd/usd_to_mjspec.cc new file mode 100644 index 00000000..7a25e156 --- /dev/null +++ b/src/experimental/usd/usd_to_mjspec.cc @@ -0,0 +1,1649 @@ +// Copyright 2025 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. + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +namespace { + +using pxr::MjcPhysicsTokens; +using pxr::TfToken; + +void SetDoubleArrFromGfVec3d(double* to, const pxr::GfVec3d& from) { + to[0] = from[0]; + to[1] = from[1]; + to[2] = from[2]; +} + +void SetMjtNumArrFromGfVec3d(mjtNum* to, const pxr::GfVec3d& from) { + to[0] = from[0]; + to[1] = from[1]; + to[2] = from[2]; +} + +void SetDoubleArrFromGfQuatd(double* to, const pxr::GfQuatd& from) { + // pxr::GfQuatd uses wxyz stored as a real and an imaginary component. + to[0] = from.GetReal(); + to[1] = from.GetImaginary()[0]; + to[2] = from.GetImaginary()[1]; + to[3] = from.GetImaginary()[2]; +} + +template +void SetLocalPoseFromPrim(const pxr::UsdPrim& prim, + const pxr::UsdPrim& parent_prim, T* element, + pxr::UsdGeomXformCache& xform_cache) { + pxr::GfMatrix4d xform = xform_cache.GetLocalToWorldTransform(prim); + pxr::GfMatrix4d parent_xform = + xform_cache.GetLocalToWorldTransform(parent_prim); + pxr::GfMatrix4d relative_xform = xform * parent_xform.GetInverse(); + + pxr::GfVec3d translation = relative_xform.ExtractTranslation(); + pxr::GfQuatd rotation = + relative_xform.RemoveScaleShear().ExtractRotationQuat(); + + SetDoubleArrFromGfVec3d(element->pos, translation); + SetDoubleArrFromGfQuatd(element->quat, rotation); +} + +pxr::GfVec3d GetScale(const pxr::GfMatrix4d& matrix) { + pxr::GfMatrix4d rotation; + pxr::GfVec3d scale; + pxr::GfMatrix4d u; + pxr::GfVec3d translation; + pxr::GfMatrix4d p; + if (!matrix.Factor(&rotation, &scale, &u, &translation, &p)) { + // In the event that we could not factorize, return the identity. + mju_error("Could not factorise matrix."); + return pxr::GfVec3d(1, 1, 1); + } + return scale; +} + +bool IsUniformScale(const pxr::GfVec3d& scale) { + static const double epsilon = 1e-6; + return fabs(scale[0] - scale[1]) < epsilon && + fabs(scale[1] - scale[2]) < epsilon; +} + +template +void SetScale(T* element, const pxr::GfMatrix4d& world_transform, + const pxr::GfVec3d& scale) { + pxr::GfVec3d transform_scale = + pxr::GfCompMult(GetScale(world_transform), scale); + element->size[0] = transform_scale[0]; + element->size[1] = transform_scale[1]; + element->size[2] = transform_scale[2]; +} + +template +bool MaybeParseGeomPrimitive(const pxr::UsdPrim& prim, T* element, + pxr::UsdGeomXformCache& xform_cache) { + auto world_xform = xform_cache.GetLocalToWorldTransform(prim); + auto scale = GetScale(world_xform); + if (prim.IsA()) { + double radius; + if (!pxr::UsdGeomSphere(prim).GetRadiusAttr().Get(&radius)) { + mju_error("Could not get sphere radius attr."); + return false; + } + // If scale is uniform (or *very close*) then create a sphere, otherwise + // this is an ellipsoid. + if (IsUniformScale(scale)) { + element->type = mjGEOM_SPHERE; + element->size[0] = scale[0] * radius; + element->size[1] = scale[0] * radius; + element->size[2] = scale[0] * radius; + } else { + element->type = mjGEOM_ELLIPSOID; + element->size[0] = scale[0] * radius; + element->size[1] = scale[1] * radius; + element->size[2] = scale[2] * radius; + } + } else if (prim.IsA()) { + auto cylinder = pxr::UsdGeomCylinder(prim); + element->type = mjGEOM_CYLINDER; + double radius; + if (!cylinder.GetRadiusAttr().Get(&radius)) { + mju_error("Could not get cylinder radius attr."); + return false; + } + + double height; + if (!cylinder.GetHeightAttr().Get(&height)) { + mju_error("Could not get cylinder height attr."); + return false; + } + element->size[0] = scale[0] * radius; + element->size[1] = scale[1] * height / 2.0f; + element->size[2] = 0; + } else if (prim.IsA()) { + auto capsule = pxr::UsdGeomCapsule(prim); + element->type = mjGEOM_CAPSULE; + double radius; + if (!capsule.GetRadiusAttr().Get(&radius)) { + mju_error("Could not get capsule radius attr."); + return false; + } + + double height; + if (!capsule.GetHeightAttr().Get(&height)) { + mju_error("Could not get capsule height attr."); + return false; + } + element->size[0] = scale[0] * radius; + element->size[1] = scale[1] * height / 2.0f; + element->size[2] = 0; + } else if (prim.IsA()) { + element->type = mjGEOM_BOX; + auto cube = pxr::UsdGeomCube(prim); + double size; + if (!cube.GetSizeAttr().Get(&size)) { + mju_error("Could not get cube size attr."); + return false; + } + // MuJoCo uses half-length for box size. + SetScale(element, xform_cache.GetLocalToWorldTransform(prim), + pxr::GfVec3d(size / 2)); + + } else { + return false; + } + + return true; +} + +void ParseUsdPhysicsScene(mjSpec* spec, + const pxr::UsdPhysicsScene& physics_scene) { + // Parse gravity and gravity direction. + pxr::GfVec3f gravity_direction; + physics_scene.GetGravityDirectionAttr().Get(&gravity_direction); + + float gravity_magnitude; + physics_scene.GetGravityMagnitudeAttr().Get(&gravity_magnitude); + + gravity_direction *= gravity_magnitude; + + spec->option.gravity[0] = gravity_direction[0]; + spec->option.gravity[1] = gravity_direction[1]; + spec->option.gravity[2] = gravity_direction[2]; + + // Early exit if theres no MjcPhysicsSceneAPI applied. + if (!physics_scene.GetPrim().HasAPI()) { + return; + } + auto mjc_physics_scene = pxr::MjcPhysicsSceneAPI(physics_scene.GetPrim()); + + double timestep; + mjc_physics_scene.GetTimestepAttr().Get(×tep); + spec->option.timestep = timestep; + + double apirate; + mjc_physics_scene.GetApiRateAttr().Get(&apirate); + spec->option.apirate = apirate; + + double impratio; + mjc_physics_scene.GetImpRatioAttr().Get(&impratio); + spec->option.impratio = impratio; + + double tolerance; + mjc_physics_scene.GetToleranceAttr().Get(&tolerance); + spec->option.tolerance = tolerance; + + double ls_tolerance; + mjc_physics_scene.GetLSToleranceAttr().Get(&ls_tolerance); + spec->option.ls_tolerance = ls_tolerance; + + double noslip_tolerance; + mjc_physics_scene.GetNoslipToleranceAttr().Get(&noslip_tolerance); + spec->option.noslip_tolerance = noslip_tolerance; + + double ccd_tolerance; + mjc_physics_scene.GetCCDToleranceAttr().Get(&ccd_tolerance); + spec->option.ccd_tolerance = ccd_tolerance; + + pxr::GfVec3d wind; + mjc_physics_scene.GetWindAttr().Get(&wind); + SetMjtNumArrFromGfVec3d(spec->option.wind, wind); + + pxr::GfVec3d magnetic; + mjc_physics_scene.GetMagneticAttr().Get(&magnetic); + SetMjtNumArrFromGfVec3d(spec->option.magnetic, magnetic); + + double density; + mjc_physics_scene.GetDensityAttr().Get(&density); + spec->option.density = density; + + double viscosity; + mjc_physics_scene.GetViscosityAttr().Get(&viscosity); + spec->option.viscosity = viscosity; + + double o_margin; + mjc_physics_scene.GetOMarginAttr().Get(&o_margin); + spec->option.o_margin = o_margin; + + pxr::VtDoubleArray o_solref; + mjc_physics_scene.GetOSolRefAttr().Get(&o_solref); + if (o_solref.size() != mjNREF) { + mju_error("Invalid size for o_solref attribute: %zu expected %d", + o_solref.size(), mjNREF); + return; + } + for (int i = 0; i < mjNREF; ++i) { + spec->option.o_solref[i] = o_solref[i]; + } + + pxr::VtDoubleArray o_solimp; + mjc_physics_scene.GetOSolImpAttr().Get(&o_solimp); + if (o_solimp.size() != mjNIMP) { + mju_error("Invalid size for o_solimp attribute: %zu expected %d", + o_solimp.size(), mjNIMP); + return; + } + for (int i = 0; i < mjNIMP; ++i) { + spec->option.o_solimp[i] = o_solimp[i]; + } + + pxr::VtDoubleArray o_friction; + mjc_physics_scene.GetOFrictionAttr().Get(&o_friction); + if (o_friction.size() != 5) { + mju_error("Invalid size for o_friction attribute: %zu expected %d", + o_friction.size(), 5); + return; + } + for (int i = 0; i < 5; ++i) { + spec->option.o_friction[i] = o_friction[i]; + } + + TfToken integrator; + mjc_physics_scene.GetIntegratorAttr().Get(&integrator); + if (integrator == MjcPhysicsTokens->euler) { + spec->option.integrator = mjINT_EULER; + } else if (integrator == MjcPhysicsTokens->rk4) { + spec->option.integrator = mjINT_RK4; + } else if (integrator == MjcPhysicsTokens->implicit) { + spec->option.integrator = mjINT_IMPLICIT; + } else if (integrator == MjcPhysicsTokens->implicitfast) { + spec->option.integrator = mjINT_IMPLICITFAST; + } + + TfToken cone; + mjc_physics_scene.GetConeAttr().Get(&cone); + if (cone == MjcPhysicsTokens->elliptic) { + spec->option.cone = mjCONE_ELLIPTIC; + } else if (cone == MjcPhysicsTokens->pyramidal) { + spec->option.cone = mjCONE_PYRAMIDAL; + } + + TfToken jacobian; + mjc_physics_scene.GetJacobianAttr().Get(&jacobian); + if (jacobian == MjcPhysicsTokens->auto_) { + spec->option.jacobian = mjJAC_AUTO; + } else if (jacobian == MjcPhysicsTokens->dense) { + spec->option.jacobian = mjJAC_DENSE; + } else if (jacobian == MjcPhysicsTokens->sparse) { + spec->option.jacobian = mjJAC_SPARSE; + } + + TfToken solver; + mjc_physics_scene.GetSolverAttr().Get(&solver); + if (solver == MjcPhysicsTokens->newton) { + spec->option.solver = mjSOL_NEWTON; + } else if (solver == MjcPhysicsTokens->cg) { + spec->option.solver = mjSOL_CG; + } else if (solver == MjcPhysicsTokens->pgs) { + spec->option.solver = mjSOL_PGS; + } + + int iterations; + mjc_physics_scene.GetIterationsAttr().Get(&iterations); + spec->option.iterations = iterations; + + int ls_iterations; + mjc_physics_scene.GetLSIterationsAttr().Get(&ls_iterations); + spec->option.ls_iterations = ls_iterations; + + int noslip_iterations; + mjc_physics_scene.GetNoslipIterationsAttr().Get(&noslip_iterations); + spec->option.noslip_iterations = noslip_iterations; + + int ccd_iterations; + mjc_physics_scene.GetCCDIterationsAttr().Get(&ccd_iterations); + spec->option.ccd_iterations = ccd_iterations; + + int sdf_initpoints; + mjc_physics_scene.GetSDFInitPointsAttr().Get(&sdf_initpoints); + spec->option.sdf_initpoints = sdf_initpoints; + + int sdf_iterations; + mjc_physics_scene.GetSDFIterationsAttr().Get(&sdf_iterations); + spec->option.sdf_iterations = sdf_iterations; + + bool constraint_flag; + mjc_physics_scene.GetConstraintFlagAttr().Get(&constraint_flag); + spec->option.disableflags |= (!constraint_flag ? mjDSBL_CONSTRAINT : 0); + + bool equality_flag; + mjc_physics_scene.GetEqualityFlagAttr().Get(&equality_flag); + spec->option.disableflags |= (!equality_flag ? mjDSBL_EQUALITY : 0); + + bool frictionloss_flag; + mjc_physics_scene.GetFrictionLossFlagAttr().Get(&frictionloss_flag); + spec->option.disableflags |= (!frictionloss_flag ? mjDSBL_FRICTIONLOSS : 0); + + bool limit_flag; + mjc_physics_scene.GetLimitFlagAttr().Get(&limit_flag); + spec->option.disableflags |= (!limit_flag ? mjDSBL_LIMIT : 0); + + bool contact_flag; + mjc_physics_scene.GetContactFlagAttr().Get(&contact_flag); + spec->option.disableflags |= (!contact_flag ? mjDSBL_CONTACT : 0); + + bool passive_flag; + mjc_physics_scene.GetPassiveFlagAttr().Get(&passive_flag); + spec->option.disableflags |= (!passive_flag ? mjDSBL_PASSIVE : 0); + + bool gravity_flag; + mjc_physics_scene.GetGravityFlagAttr().Get(&gravity_flag); + spec->option.disableflags |= (!gravity_flag ? mjDSBL_GRAVITY : 0); + + bool clampctrl_flag; + mjc_physics_scene.GetClampCtrlFlagAttr().Get(&clampctrl_flag); + spec->option.disableflags |= (!clampctrl_flag ? mjDSBL_CLAMPCTRL : 0); + + bool warmstart_flag; + mjc_physics_scene.GetWarmStartFlagAttr().Get(&warmstart_flag); + spec->option.disableflags |= (!warmstart_flag ? mjDSBL_WARMSTART : 0); + + bool filterparent_flag; + mjc_physics_scene.GetFilterParentFlagAttr().Get(&filterparent_flag); + spec->option.disableflags |= (!filterparent_flag ? mjDSBL_FILTERPARENT : 0); + + bool actuation_flag; + mjc_physics_scene.GetActuationFlagAttr().Get(&actuation_flag); + spec->option.disableflags |= (!actuation_flag ? mjDSBL_ACTUATION : 0); + + bool refsafe_flag; + mjc_physics_scene.GetRefSafeFlagAttr().Get(&refsafe_flag); + spec->option.disableflags |= (!refsafe_flag ? mjDSBL_REFSAFE : 0); + + bool sensor_flag; + mjc_physics_scene.GetSensorFlagAttr().Get(&sensor_flag); + spec->option.disableflags |= (!sensor_flag ? mjDSBL_SENSOR : 0); + + bool midphase_flag; + mjc_physics_scene.GetMidPhaseFlagAttr().Get(&midphase_flag); + spec->option.disableflags |= (!midphase_flag ? mjDSBL_MIDPHASE : 0); + + bool nativeccd_flag; + mjc_physics_scene.GetNativeCCDFlagAttr().Get(&nativeccd_flag); + spec->option.disableflags |= (!nativeccd_flag ? mjDSBL_NATIVECCD : 0); + + bool eulerdamp_flag; + mjc_physics_scene.GetEulerDampFlagAttr().Get(&eulerdamp_flag); + spec->option.disableflags |= (!eulerdamp_flag ? mjDSBL_EULERDAMP : 0); + + bool autoreset_flag; + mjc_physics_scene.GetAutoResetFlagAttr().Get(&autoreset_flag); + spec->option.disableflags |= (!autoreset_flag ? mjDSBL_AUTORESET : 0); + + bool override_flag; + mjc_physics_scene.GetOverrideFlagAttr().Get(&override_flag); + spec->option.enableflags |= (override_flag ? mjENBL_OVERRIDE : 0); + + bool energy_flag; + mjc_physics_scene.GetEnergyFlagAttr().Get(&energy_flag); + spec->option.enableflags |= (energy_flag ? mjENBL_ENERGY : 0); + + bool fwdinv_flag; + mjc_physics_scene.GetFwdinvFlagAttr().Get(&fwdinv_flag); + spec->option.enableflags |= (fwdinv_flag ? mjENBL_FWDINV : 0); + + bool invdiscrete_flag; + mjc_physics_scene.GetInvDiscreteFlagAttr().Get(&invdiscrete_flag); + spec->option.enableflags |= (invdiscrete_flag ? mjENBL_INVDISCRETE : 0); + + bool multiccd_flag; + mjc_physics_scene.GetMultiCCDFlagAttr().Get(&multiccd_flag); + spec->option.enableflags |= (multiccd_flag ? mjENBL_MULTICCD : 0); + + bool island_flag; + mjc_physics_scene.GetIslandFlagAttr().Get(&island_flag); + spec->option.enableflags |= (island_flag ? mjENBL_ISLAND : 0); +} + +void ParseUsdPhysicsMassAPIForBody(mjsBody* body, + const pxr::UsdPhysicsMassAPI& mass_api) { + auto mass_attr = mass_api.GetMassAttr(); + if (mass_attr.HasAuthoredValue()) { + float mass; + mass_attr.Get(&mass); + body->mass = mass; + } + + auto com_attr = mass_api.GetCenterOfMassAttr(); + if (com_attr.HasAuthoredValue()) { + pxr::GfVec3f com; + com_attr.Get(&com); + SetDoubleArrFromGfVec3d(body->ipos, com); + } + + auto principle_axes_attr = mass_api.GetPrincipalAxesAttr(); + if (principle_axes_attr.HasAuthoredValue()) { + pxr::GfQuatf principle_axes; + principle_axes_attr.Get(&principle_axes); + SetDoubleArrFromGfQuatd(body->iquat, principle_axes); + } + + auto diag_inertia_attr = mass_api.GetDiagonalInertiaAttr(); + if (diag_inertia_attr.HasAuthoredValue()) { + pxr::GfVec3f diag_inertia; + diag_inertia_attr.Get(&diag_inertia); + SetDoubleArrFromGfVec3d(body->inertia, diag_inertia); + } +} + +void ParseUsdPhysicsMassAPIForGeom(mjsGeom* geom, + const pxr::UsdPhysicsMassAPI& mass_api) { + auto mass_attr = mass_api.GetMassAttr(); + if (mass_attr.HasAuthoredValue()) { + float mass; + mass_attr.Get(&mass); + geom->mass = mass; + } + + auto density_attr = mass_api.GetDensityAttr(); + if (density_attr.HasAuthoredValue()) { + float density; + density_attr.Get(&density); + geom->density = density; + } +} + +void ParseMjcPhysicsCollisionAPI( + mjsGeom* geom, const pxr::MjcPhysicsCollisionAPI& collision_api) { + auto shell_inertia_attr = collision_api.GetShellInertiaAttr(); + if (shell_inertia_attr.HasAuthoredValue()) { + bool shell_inertia; + shell_inertia_attr.Get(&shell_inertia); + geom->typeinertia = shell_inertia ? mjtGeomInertia::mjINERTIA_SHELL + : mjtGeomInertia::mjINERTIA_VOLUME; + } +} + +void ParseMjcPhysicsMeshCollisionAPI( + mjsMesh* mesh, const pxr::MjcPhysicsMeshCollisionAPI& mesh_collision_api) { + auto inertia_attr = mesh_collision_api.GetInertiaAttr(); + if (inertia_attr.HasAuthoredValue()) { + pxr::TfToken inertia; + inertia_attr.Get(&inertia); + if (inertia == MjcPhysicsTokens->shell) { + mesh->inertia = mjtMeshInertia::mjMESH_INERTIA_SHELL; + } else if (inertia == MjcPhysicsTokens->exact) { + mesh->inertia = mjtMeshInertia::mjMESH_INERTIA_EXACT; + } else if (inertia == MjcPhysicsTokens->convex) { + mesh->inertia = mjtMeshInertia::mjMESH_INERTIA_CONVEX; + } else { + mesh->inertia = mjtMeshInertia::mjMESH_INERTIA_LEGACY; + } + } +} + +void ParseMjcPhysicsGeneralActuatorAPI(mjSpec* spec, + const pxr::MjcPhysicsActuatorAPI& act, + mjtTrn tran_type, + const std::string* name) { + pxr::UsdPrim prim = act.GetPrim(); + mjsActuator* mj_act = mjs_addActuator(spec, nullptr); + mjs_setString(mj_act->name, prim.GetPath().GetAsString().c_str()); + mjs_setString(mj_act->target, name->c_str()); + mj_act->trntype = tran_type; + + if (tran_type == mjtTrn::mjTRN_SLIDERCRANK) { + pxr::SdfPathVector slider_sites; + act.GetMjcSliderSiteRel().GetTargets(&slider_sites); + if (slider_sites.size() > 1) { + mju_warning( + "Slider crank slider site relationship has more than one target, " + "using the first."); + } + mjs_setString(mj_act->slidersite, slider_sites[0].GetAsString().c_str()); + } + + auto setLimitedField = [](mjsActuator* mj_act, + const pxr::UsdAttribute& usd_attribute, + int* mj_limited_field) { + if (usd_attribute.HasAuthoredValue()) { + pxr::TfToken limited; + usd_attribute.Get(&limited); + if (limited == MjcPhysicsTokens->true_) { + *mj_limited_field = mjLIMITED_TRUE; + } else if (limited == MjcPhysicsTokens->false_) { + *mj_limited_field = mjLIMITED_FALSE; + } else if (limited == MjcPhysicsTokens->auto_) { + *mj_limited_field = mjLIMITED_AUTO; + } + } + }; + + setLimitedField(mj_act, act.GetMjcCtrlLimitedAttr(), &mj_act->ctrllimited); + setLimitedField(mj_act, act.GetMjcForceLimitedAttr(), &mj_act->forcelimited); + setLimitedField(mj_act, act.GetMjcActLimitedAttr(), &mj_act->actlimited); + + auto setRangeField = + [](mjsActuator* mj_act, const pxr::UsdAttribute& usd_min_attribute, + const pxr::UsdAttribute& usd_max_attribute, double* range) { + if (usd_min_attribute.HasAuthoredValue()) { + double range_min; + usd_min_attribute.Get(&range_min); + range[0] = range_min; + } + if (usd_max_attribute.HasAuthoredValue()) { + double range_max; + usd_max_attribute.Get(&range_max); + range[1] = range_max; + } + }; + + setRangeField(mj_act, act.GetMjcCtrlRangeMinAttr(), + act.GetMjcCtrlRangeMaxAttr(), mj_act->ctrlrange); + setRangeField(mj_act, act.GetMjcForceRangeMinAttr(), + act.GetMjcForceRangeMaxAttr(), mj_act->forcerange); + setRangeField(mj_act, act.GetMjcActRangeMinAttr(), + act.GetMjcActRangeMaxAttr(), mj_act->actrange); + setRangeField(mj_act, act.GetMjcLengthRangeMinAttr(), + act.GetMjcLengthRangeMaxAttr(), mj_act->lengthrange); + + auto gear_attr = act.GetMjcGearAttr(); + if (gear_attr.HasAuthoredValue()) { + pxr::VtDoubleArray gear; + gear_attr.Get(&gear); + for (int i = 0; i < 6; ++i) { + mj_act->gear[i] = gear[i]; + } + } + + auto crank_length_attr = act.GetMjcCrankLengthAttr(); + if (crank_length_attr.HasAuthoredValue()) { + double crank_length; + crank_length_attr.Get(&crank_length); + mj_act->cranklength = crank_length; + } + + auto dyn_type_attr = act.GetMjcDynTypeAttr(); + if (dyn_type_attr.HasAuthoredValue()) { + pxr::TfToken dyn_type; + dyn_type_attr.Get(&dyn_type); + if (dyn_type == MjcPhysicsTokens->none) { + mj_act->dyntype = mjtDyn::mjDYN_NONE; + } else if (dyn_type == MjcPhysicsTokens->integrator) { + mj_act->dyntype = mjtDyn::mjDYN_INTEGRATOR; + } else if (dyn_type == MjcPhysicsTokens->filter) { + mj_act->dyntype = mjtDyn::mjDYN_FILTER; + } else if (dyn_type == MjcPhysicsTokens->filterexact) { + mj_act->dyntype = mjtDyn::mjDYN_FILTEREXACT; + } else if (dyn_type == MjcPhysicsTokens->muscle) { + mj_act->dyntype = mjtDyn::mjDYN_MUSCLE; + } else if (dyn_type == MjcPhysicsTokens->user) { + mj_act->dyntype = mjtDyn::mjDYN_USER; + } + } + + auto gain_type_attr = act.GetMjcGainTypeAttr(); + if (gain_type_attr.HasAuthoredValue()) { + pxr::TfToken gain_type; + gain_type_attr.Get(&gain_type); + if (gain_type == MjcPhysicsTokens->fixed) { + mj_act->gaintype = mjtGain::mjGAIN_FIXED; + } else if (gain_type == MjcPhysicsTokens->affine) { + mj_act->gaintype = mjtGain::mjGAIN_AFFINE; + } else if (gain_type == MjcPhysicsTokens->muscle) { + mj_act->gaintype = mjtGain::mjGAIN_MUSCLE; + } else if (gain_type == MjcPhysicsTokens->user) { + mj_act->gaintype = mjtGain::mjGAIN_USER; + } + } + + auto biastype_attr = act.GetMjcBiasTypeAttr(); + if (biastype_attr.HasAuthoredValue()) { + pxr::TfToken biastype; + biastype_attr.Get(&biastype); + if (biastype == MjcPhysicsTokens->none) { + mj_act->biastype = mjtBias::mjBIAS_NONE; + } else if (biastype == MjcPhysicsTokens->affine) { + mj_act->biastype = mjtBias::mjBIAS_AFFINE; + } else if (biastype == MjcPhysicsTokens->muscle) { + mj_act->biastype = mjtBias::mjBIAS_MUSCLE; + } else if (biastype == MjcPhysicsTokens->user) { + mj_act->biastype = mjtBias::mjBIAS_USER; + } + } + + auto setPrmField = [](mjsActuator* mj_act, + const pxr::UsdAttribute& usd_attribute, double* prm) { + if (usd_attribute.HasAuthoredValue()) { + pxr::VtDoubleArray usd_prm; + usd_attribute.Get(&usd_prm); + int n_elems = usd_prm.size() < 10 ? usd_prm.size() : 10; + for (int i = 0; i < n_elems; ++i) { + prm[i] = usd_prm[i]; + } + } + }; + + setPrmField(mj_act, act.GetMjcDynPrmAttr(), mj_act->dynprm); + setPrmField(mj_act, act.GetMjcBiasPrmAttr(), mj_act->biasprm); + setPrmField(mj_act, act.GetMjcGainPrmAttr(), mj_act->gainprm); + + auto act_dim_attr = act.GetMjcActDimAttr(); + if (act_dim_attr.HasAuthoredValue()) { + int act_dim; + act_dim_attr.Get(&act_dim); + mj_act->actdim = act_dim; + } + + auto act_early_attr = act.GetMjcActEarlyAttr(); + if (act_early_attr.HasAuthoredValue()) { + bool act_early; + act_early_attr.Get(&act_early); + mj_act->actearly = (int)act_early; + } + + auto ref_site_rel = act.GetMjcRefSiteRel(); + if (ref_site_rel.HasAuthoredTargets()) { + pxr::SdfPathVector targets; + ref_site_rel.GetTargets(&targets); + pxr::SdfPath first_path = targets[0]; + mjs_setString(mj_act->refsite, first_path.GetString().c_str()); + } +} + +void ParseMjcPhysicsJointAPI(mjsJoint* mj_joint, + const pxr::MjcPhysicsJointAPI& joint_api) { + auto springdamper_attr = joint_api.GetMjcSpringdamperAttr(); + if (springdamper_attr.HasAuthoredValue()) { + pxr::VtDoubleArray springdamper; + springdamper_attr.Get(&springdamper); + if (springdamper.size() == 2) { + mj_joint->springdamper[0] = springdamper[0]; + mj_joint->springdamper[1] = springdamper[1]; + } else { + mju_warning( + "springdamper attribute for joint %s has incorrect size %zu, " + "expected 2.", + mj_joint->name->c_str(), springdamper.size()); + } + } + + auto solreflimit_attr = joint_api.GetMjcSolreflimitAttr(); + if (solreflimit_attr.HasAuthoredValue()) { + pxr::VtDoubleArray solreflimit; + solreflimit_attr.Get(&solreflimit); + if (solreflimit.size() == mjNREF) { + for (int i = 0; i < mjNREF; ++i) { + mj_joint->solref_limit[i] = solreflimit[i]; + } + } else { + mju_warning( + "solreflimit attribute for joint %s has incorrect size %zu, " + "expected %d.", + mj_joint->name->c_str(), solreflimit.size(), mjNREF); + } + } + + auto solimplimit_attr = joint_api.GetMjcSolimplimitAttr(); + if (solimplimit_attr.HasAuthoredValue()) { + pxr::VtDoubleArray solimplimit; + solimplimit_attr.Get(&solimplimit); + if (solimplimit.size() == mjNIMP) { + for (int i = 0; i < mjNIMP; ++i) { + mj_joint->solimp_limit[i] = solimplimit[i]; + } + } else { + mju_warning( + "solimplimit attribute for joint %s has incorrect size %zu, " + "expected %d.", + mj_joint->name->c_str(), solimplimit.size(), mjNIMP); + } + } + + auto solreffriction_attr = joint_api.GetMjcSolreffrictionAttr(); + if (solreffriction_attr.HasAuthoredValue()) { + pxr::VtDoubleArray solreffriction; + solreffriction_attr.Get(&solreffriction); + if (solreffriction.size() == mjNREF) { + for (int i = 0; i < mjNREF; ++i) { + mj_joint->solref_friction[i] = solreffriction[i]; + } + } else { + mju_warning( + "solreffriction attribute for joint %s has incorrect size %zu, " + "expected %d.", + mj_joint->name->c_str(), solreffriction.size(), mjNREF); + } + } + + auto solimpfriction_attr = joint_api.GetMjcSolimpfrictionAttr(); + if (solimpfriction_attr.HasAuthoredValue()) { + pxr::VtDoubleArray solimpfriction; + solimpfriction_attr.Get(&solimpfriction); + if (solimpfriction.size() == mjNIMP) { + for (int i = 0; i < mjNIMP; ++i) { + mj_joint->solimp_friction[i] = solimpfriction[i]; + } + } else { + mju_warning( + "solimpfriction attribute for joint %s has incorrect size %zu, " + "expected %d.", + mj_joint->name->c_str(), solimpfriction.size(), mjNIMP); + } + } + + auto stiffness_attr = joint_api.GetMjcStiffnessAttr(); + if (stiffness_attr.HasAuthoredValue()) { + double stiffness; + stiffness_attr.Get(&stiffness); + mj_joint->stiffness = stiffness; + } + + auto actuatorfrcrange_min_attr = joint_api.GetMjcActuatorfrcrangeMinAttr(); + if (actuatorfrcrange_min_attr.HasAuthoredValue()) { + double min_val; + actuatorfrcrange_min_attr.Get(&min_val); + mj_joint->actfrcrange[0] = min_val; + } + auto actuatorfrcrange_max_attr = joint_api.GetMjcActuatorfrcrangeMaxAttr(); + if (actuatorfrcrange_max_attr.HasAuthoredValue()) { + double max_val; + actuatorfrcrange_max_attr.Get(&max_val); + mj_joint->actfrcrange[1] = max_val; + } + + auto actuatorfrclimited_attr = joint_api.GetMjcActuatorfrclimitedAttr(); + if (actuatorfrclimited_attr.HasAuthoredValue()) { + pxr::TfToken limited; + actuatorfrclimited_attr.Get(&limited); + if (limited == MjcPhysicsTokens->true_) { + mj_joint->actfrclimited = mjLIMITED_TRUE; + } else if (limited == MjcPhysicsTokens->false_) { + mj_joint->actfrclimited = mjLIMITED_FALSE; + } else if (limited == MjcPhysicsTokens->auto_) { + mj_joint->actfrclimited = mjLIMITED_AUTO; + } + } + + auto actuatorgravcomp_attr = joint_api.GetMjcActuatorgravcompAttr(); + if (actuatorgravcomp_attr.HasAuthoredValue()) { + bool gravcomp; + actuatorgravcomp_attr.Get(&gravcomp); + mj_joint->actgravcomp = gravcomp; + } + + auto margin_attr = joint_api.GetMjcMarginAttr(); + if (margin_attr.HasAuthoredValue()) { + double margin; + margin_attr.Get(&margin); + mj_joint->margin = margin; + } + + auto ref_attr = joint_api.GetMjcRefAttr(); + if (ref_attr.HasAuthoredValue()) { + double ref; + ref_attr.Get(&ref); + mj_joint->ref = ref; + } + + auto springref_attr = joint_api.GetMjcSpringrefAttr(); + if (springref_attr.HasAuthoredValue()) { + double springref; + springref_attr.Get(&springref); + mj_joint->springref = springref; + } + + auto armature_attr = joint_api.GetMjcArmatureAttr(); + if (armature_attr.HasAuthoredValue()) { + double armature; + armature_attr.Get(&armature); + mj_joint->armature = armature; + } + + auto damping_attr = joint_api.GetMjcDampingAttr(); + if (damping_attr.HasAuthoredValue()) { + double damping; + damping_attr.Get(&damping); + mj_joint->damping = damping; + } + + auto frictionloss_attr = joint_api.GetMjcFrictionlossAttr(); + if (frictionloss_attr.HasAuthoredValue()) { + double frictionloss; + frictionloss_attr.Get(&frictionloss); + mj_joint->frictionloss = frictionloss; + } +} + +void ParseUsdPhysicsCollider(mjSpec* spec, + const pxr::UsdPhysicsCollisionAPI& collision_api, + const pxr::UsdPrim& parent_prim, mjsBody* parent, + pxr::UsdGeomXformCache& xform_cache) { + pxr::UsdPrim prim = collision_api.GetPrim(); + // UsdPhysicsCollisionAPI can only be applied to gprim primitives. + if (!prim.IsA()) { + mju_warning( + "UsdPhysicsCollisionAPI applied to a non-UsdGeomGprim prim: %s. " + "Skipping.", + prim.GetPath().GetAsString().c_str()); + return; + } + + mjsGeom* geom = mjs_addGeom(parent, nullptr); + mjs_setString(geom->name, prim.GetPath().GetAsString().c_str()); + geom->contype = 1; + geom->conaffinity = 1; + + if (prim.HasAPI()) { + ParseUsdPhysicsMassAPIForGeom(geom, pxr::UsdPhysicsMassAPI(prim)); + } + + if (prim.HasAPI()) { + ParseMjcPhysicsCollisionAPI(geom, pxr::MjcPhysicsCollisionAPI(prim)); + } + + // Convert displayColor and displayOpacity to rgba. + // We want to support primvar inheritance, hence FindPrimvarWithInheritance. + pxr::UsdGeomPrimvarsAPI primvarsAPI(prim); + pxr::UsdGeomPrimvar displayColorPrimvar = + primvarsAPI.FindPrimvarWithInheritance( + pxr::UsdGeomTokens->primvarsDisplayColor); + pxr::UsdGeomPrimvar displayOpacityPrimvar = + primvarsAPI.FindPrimvarWithInheritance( + pxr::UsdGeomTokens->primvarsDisplayOpacity); + if (displayColorPrimvar.HasAuthoredValue()) { + pxr::VtArray display_color; + displayColorPrimvar.Get(&display_color); + if (!display_color.empty()) { + geom->rgba[0] = display_color[0][0]; + geom->rgba[1] = display_color[0][1]; + geom->rgba[2] = display_color[0][2]; + } + } + if (displayOpacityPrimvar.HasAuthoredValue()) { + pxr::VtArray display_opacity; + displayOpacityPrimvar.Get(&display_opacity); + if (!display_opacity.empty()) { + geom->rgba[3] = display_opacity[0]; + } + } + + SetLocalPoseFromPrim(prim, parent_prim, geom, xform_cache); + + if (!MaybeParseGeomPrimitive(prim, geom, xform_cache)) { + if (prim.IsA()) { + geom->type = mjGEOM_MESH; + pxr::UsdGeomMesh usd_mesh(prim); + std::vector uservert; + std::vector userface; + + pxr::VtVec3fArray points; + usd_mesh.GetPointsAttr().Get(&points); + + uservert.reserve(points.size() * 3); + for (const auto& pt : points) { + uservert.push_back(pt[0]); + uservert.push_back(pt[1]); + uservert.push_back(pt[2]); + } + + pxr::VtIntArray indices; + usd_mesh.GetFaceVertexIndicesAttr().Get(&indices); + pxr::VtIntArray counts; + usd_mesh.GetFaceVertexCountsAttr().Get(&counts); + + userface.reserve(indices.size()); + int vtx_idx = 0; + for (int count : counts) { + int k = 1; + // If the prim is a triangle create a triangle fan rooted + // at the first index. + while (k < count - 1) { + userface.push_back(indices[vtx_idx]); + userface.push_back(indices[vtx_idx + k]); + userface.push_back(indices[vtx_idx + k + 1]); + k++; + } + vtx_idx += count; + } + + mjsMesh* mesh = mjs_addMesh(spec, nullptr); + + if (prim.HasAPI()) { + ParseMjcPhysicsMeshCollisionAPI(mesh, + pxr::MjcPhysicsMeshCollisionAPI(prim)); + } + + std::string mesh_name = usd_mesh.GetPath().GetAsString(); + mjs_setString(mesh->name, mesh_name.c_str()); + mjs_setFloat(mesh->uservert, uservert.data(), uservert.size()); + mjs_setInt(mesh->userface, userface.data(), userface.size()); + + mjs_setString(geom->meshname, mesh_name.c_str()); + } else if (prim.IsA()) { + geom->type = mjGEOM_PLANE; + + pxr::UsdGeomPlane plane(prim); + TfToken axis; + if (!plane.GetAxisAttr().Get(&axis)) { + mju_error("Could not get plane axis attr."); + return; + } + if (axis != pxr::UsdGeomTokens->z) { + mju_error("Only z-axis planes are supported."); + return; + } + + // This block of code distributes the plane length and width along the + // scale as per the specification here: + // https://openusd.org/dev/api/class_usd_geom_plane.html#a89fa6076111984682db77fc8a4e57496. + double length; + if (!plane.GetLengthAttr().Get(&length)) { + mju_error("Could not get plane length attr."); + return; + } + double width; + if (!plane.GetWidthAttr().Get(&width)) { + mju_error("Could not get plane width attr."); + return; + } + // Plane geoms in mjc are always infinite. We set the scale here just + // for visualization. + SetScale(geom, xform_cache.GetLocalToWorldTransform(prim), + pxr::GfVec3d(width, length, 1)); + } + } +} + +void ParseUsdPhysicsJoint(mjSpec* spec, const pxr::UsdPrim& prim, mjsBody* body, + pxr::UsdGeomXformCache& xform_cache) { + pxr::UsdPhysicsJoint joint(prim); + + // A fixed joint means the bodies are welded. + if (prim.IsA()) { + // No joint needed for welded bodies. + return; + } + + mjtJoint type; + if (prim.IsA()) { + type = mjJNT_HINGE; + } else if (prim.IsA()) { + type = mjJNT_SLIDE; + } else { + // Unsupported joint type. + mju_warning("Unsupported joint type for %s", + prim.GetPath().GetAsString().c_str()); + return; + } + + mjsJoint* mj_joint = mjs_addJoint(body, nullptr); + mj_joint->type = type; + mjs_setString(mj_joint->name, prim.GetPath().GetAsString().c_str()); + + if (prim.IsA()) { + pxr::UsdPhysicsRevoluteJoint revolute(prim); + TfToken axis; + revolute.GetAxisAttr().Get(&axis); + if (axis == pxr::UsdGeomTokens->x) { + mj_joint->axis[0] = 1; + mj_joint->axis[1] = 0; + mj_joint->axis[2] = 0; + } else if (axis == pxr::UsdGeomTokens->y) { + mj_joint->axis[0] = 0; + mj_joint->axis[1] = 1; + mj_joint->axis[2] = 0; + } else { // Z is default + mj_joint->axis[0] = 0; + mj_joint->axis[1] = 0; + mj_joint->axis[2] = 1; + } + + float lower, upper; + if (revolute.GetLowerLimitAttr().Get(&lower) && + revolute.GetUpperLimitAttr().Get(&upper)) { + mj_joint->limited = mjLIMITED_TRUE; + // As per the XML Reference, the default unit for mjSpec is degrees, so we + // don't need to convert from USD (which is degrees). + mj_joint->range[0] = lower; + mj_joint->range[1] = upper; + } + } else if (prim.IsA()) { + pxr::UsdPhysicsPrismaticJoint prismatic(prim); + TfToken axis; + prismatic.GetAxisAttr().Get(&axis); + if (axis == pxr::UsdGeomTokens->x) { + mj_joint->axis[0] = 1; + mj_joint->axis[1] = 0; + mj_joint->axis[2] = 0; + } else if (axis == pxr::UsdGeomTokens->y) { + mj_joint->axis[0] = 0; + mj_joint->axis[1] = 1; + mj_joint->axis[2] = 0; + } else { // Z is default + mj_joint->axis[0] = 0; + mj_joint->axis[1] = 0; + mj_joint->axis[2] = 1; + } + float lower, upper; + if (prismatic.GetLowerLimitAttr().Get(&lower) && + prismatic.GetUpperLimitAttr().Get(&upper)) { + mj_joint->limited = mjLIMITED_TRUE; + mj_joint->range[0] = lower; + mj_joint->range[1] = upper; + } + } + + // localPose1 is joint frame in child body (body1) coordinates. + pxr::GfVec3f localPos1; + joint.GetLocalPos1Attr().Get(&localPos1); + pxr::GfVec3d pos(localPos1); + + pxr::GfQuatf localRot1; + joint.GetLocalRot1Attr().Get(&localRot1); + pxr::GfQuatd rot(localRot1); + + SetDoubleArrFromGfVec3d(mj_joint->pos, pos); + + // Transform joint axis by localPose1 rotation. + if (mj_joint->type == mjJNT_HINGE || mj_joint->type == mjJNT_SLIDE) { + pxr::GfVec3d axis_vec(mj_joint->axis[0], mj_joint->axis[1], + mj_joint->axis[2]); + pxr::GfVec3d rotated_axis = rot.Transform(axis_vec); + SetDoubleArrFromGfVec3d(mj_joint->axis, rotated_axis); + } + + if (prim.HasAPI()) { + ParseMjcPhysicsGeneralActuatorAPI(spec, pxr::MjcPhysicsActuatorAPI(prim), + mjtTrn::mjTRN_JOINT, mj_joint->name); + } + + if (prim.HasAPI()) { + ParseMjcPhysicsJointAPI(mj_joint, pxr::MjcPhysicsJointAPI(prim)); + } +} + +void ParseMjcPhysicsSite(mjSpec* spec, const pxr::MjcPhysicsSiteAPI& site_api, + const pxr::UsdPrim& parent_prim, mjsBody* parent, + pxr::UsdGeomXformCache& xform_cache) { + auto prim = site_api.GetPrim(); + mjsSite* site = mjs_addSite(parent, 0); + mjs_setString(site->name, site_api.GetPrim().GetPath().GetAsString().c_str()); + SetLocalPoseFromPrim(site_api.GetPrim(), parent_prim, site, xform_cache); + + // Convert USD type to MuJoCo type. + if (!MaybeParseGeomPrimitive(prim, site, xform_cache)) { + mju_error("Prim with SiteAPI has unsupported typej %s", + prim.GetTypeName().GetString().c_str()); + return; + } + + if (prim.HasAPI()) { + auto act_api = pxr::MjcPhysicsActuatorAPI(prim); + bool slider_crank = act_api.GetMjcSliderSiteRel().HasAuthoredTargets(); + ParseMjcPhysicsGeneralActuatorAPI( + spec, pxr::MjcPhysicsActuatorAPI(prim), + slider_crank ? mjtTrn::mjTRN_SLIDERCRANK : mjtTrn::mjTRN_SITE, + site->name); + } +} + +mjsBody* ParseUsdPhysicsRigidbody( + mjSpec* spec, const pxr::UsdPhysicsRigidBodyAPI& rigidbody_api, + const pxr::UsdPrim& parent_prim, mjsBody* parent, + pxr::UsdGeomXformCache& xform_cache) { + pxr::UsdPrim prim = rigidbody_api.GetPrim(); + mjsBody* body = mjs_addBody(parent, nullptr); + mjs_setString(body->name, prim.GetPath().GetAsString().c_str()); + SetLocalPoseFromPrim(prim, parent_prim, body, xform_cache); + + if (prim.HasAPI()) { + ParseUsdPhysicsMassAPIForBody(body, pxr::UsdPhysicsMassAPI(prim)); + } + + if (prim.HasAPI()) { + ParseMjcPhysicsGeneralActuatorAPI(spec, pxr::MjcPhysicsActuatorAPI(prim), + mjtTrn::mjTRN_BODY, body->name); + } + + mujoco::usd::SetUsdPrimPathUserValue(body->element, prim.GetPath()); + + return body; +} + +// There is no common base class for UsdPhysicsRigidBodyAPI and +// UsdPhysicsCollisionAPI so we need a templated function. +template +bool IsObjectInPhysicsScene(const T& object, + const pxr::UsdPhysicsScene& physics_scene) { + pxr::SdfPathVector sim_owners; + object.GetSimulationOwnerRel().GetTargets(&sim_owners); + for (const auto& sim_owner : sim_owners) { + if (physics_scene.GetPath() == sim_owner) { + return true; + } + } + return false; +}; + +// A struct to hold the forest representation. +// The forest is a map from a root path to its tree. +// A tree is an adjacency list, mapping a parent path to its children paths. +using MjUsdForest = + std::map>>; + +// A directed edge. +using Edge = std::pair; + +// A map from a directed edge to the path of the joint representing that edge. +using EdgesMap = std::map; + +// Constructs and validates a forest (a collection of disjoint trees) +// from a list of directed edges. It also considers all rigid bodies in the +// scene, treating those not involved in any edge as isolated, free-floating +// bodies. +// +// An empty 'from' path represents the world body. Returns `std::nullopt` for +// invalid forest structures (e.g. cycles, multiple parents). +std::optional BuildForestFromEdges( + const EdgesMap& edges, const std::vector& all_body_paths) { + if (edges.empty() && all_body_paths.empty()) { + return MjUsdForest{}; + } + + std::map> children_map; + std::map parent_map; + std::set all_nodes(all_body_paths.begin(), + all_body_paths.end()); + + // 1. Initial Pass: Build maps and perform local validation + for (const auto& edge_pair : edges) { + const auto& [from, to] = edge_pair.first; + if (from == to) { + mju_error("Self-loop detected at node %s", to.GetString().c_str()); + return std::nullopt; + } + if (parent_map.count(to)) { + mju_error("Node %s has multiple parents ('%s' and '%s').", + to.GetString().c_str(), parent_map.at(to).GetString().c_str(), + from.GetString().c_str()); + return std::nullopt; + } + + children_map[from].push_back(to); + parent_map[to] = from; + all_nodes.insert(from); + all_nodes.insert(to); + } + + // 2. Find all root nodes + // A root is a node that is not a child of any other node. This includes + // roots of kinematic trees and isolated rigid bodies. + std::set roots = all_nodes; + for (const auto& pair : parent_map) { + roots.erase(pair.first); // `pair.first` is a child node. + } + + if (roots.empty() && !all_nodes.empty()) { + mju_error("No root nodes found, but edges exist. A cycle is present."); + return std::nullopt; + } + + // 3. Build the forest, claiming nodes for each tree + MjUsdForest forest; + std::set claimed_nodes; + + for (const auto& root : roots) { + // Perform a traversal (BFS) to find all nodes in this tree + std::deque q; + q.push_back(root); + std::set nodes_in_this_tree; + + while (!q.empty()) { + pxr::SdfPath current_node = q.front(); + q.pop_front(); + + if (claimed_nodes.count(current_node)) { + mju_error("Node %s is shared between multiple trees.", + current_node.GetString().c_str()); + return std::nullopt; + } + + nodes_in_this_tree.insert(current_node); + claimed_nodes.insert(current_node); + + if (children_map.count(current_node)) { + for (const auto& child : children_map.at(current_node)) { + q.push_back(child); + } + } + } + + // Construct the adjacency list for this specific tree + std::map> tree_adj_list; + for (const auto& node : nodes_in_this_tree) { + if (children_map.count(node)) { + tree_adj_list[node] = children_map.at(node); + } + } + forest[root] = tree_adj_list; + } + + // 4. Final check for cycles (unclaimed nodes) + std::set unclaimed_nodes; // all_nodes - claimed_nodes + std::set_difference(all_nodes.begin(), all_nodes.end(), claimed_nodes.begin(), + claimed_nodes.end(), + std::inserter(unclaimed_nodes, unclaimed_nodes.begin())); + + if (!unclaimed_nodes.empty()) { + std::string unclaimed_str; + for (const auto& node : unclaimed_nodes) { + unclaimed_str += "'" + node.GetString() + "' "; + } + mju_error( + "Cycle detected. The following nodes are part of a cycle " + "and not reachable from any root: %s", + unclaimed_str.c_str()); + return std::nullopt; + } + + return forest; +} + +void TraverseAndBuildTree( + pxr::UsdStageRefPtr stage, mjSpec* spec, mjsBody* parent_mj_body, + const pxr::SdfPath& parent_body_path, const pxr::SdfPath& current_body_path, + const std::map>& tree, + const EdgesMap& edges, pxr::UsdGeomXformCache& xform_cache); + +// Traverses the prim and all its descendants in the USD hierarchy and parses +// supported entities like colliders and sites, attaching them to the given +// mjBody. The traversal for a given branch stops when: +// - a descendant with a RigidBodyAPI is found, as that will be handled by +// TraverseAndBuildTree. +// - a descendant with a ResetXformStack is found, as that will be handled by +// the top level traversal of independent prims. +void ParseCurrentAndDescendants(mjSpec* spec, const pxr::UsdPrim& prim, + const pxr::UsdPrim& parent_prim, mjsBody* body, + pxr::UsdGeomXformCache& xform_cache) { + if (prim.HasAPI()) { + ParseUsdPhysicsCollider(spec, pxr::UsdPhysicsCollisionAPI(prim), + parent_prim, body, xform_cache); + } + if (prim.HasAPI()) { + ParseMjcPhysicsSite(spec, pxr::MjcPhysicsSiteAPI(prim), parent_prim, body, + xform_cache); + } + + // Make sure we traverse into instance proxies to ensure we support + // instanceable references. + // See https://openusd.org/dev/api/_usd__page__scenegraph_instancing.html + for (const auto& child : + prim.GetFilteredChildren(pxr::UsdTraverseInstanceProxies())) { + if (child.HasAPI()) { + continue; + } + if (xform_cache.GetResetXformStack(child)) { + continue; + } + ParseCurrentAndDescendants(spec, child, prim, body, xform_cache); + } +} + +// Recursively traverses a kinematic tree, creating bodies and joints in the +// mjSpec. +void TraverseAndBuildTree( + pxr::UsdStageRefPtr stage, mjSpec* spec, mjsBody* parent_mj_body, + const pxr::SdfPath& parent_body_path, const pxr::SdfPath& current_body_path, + const std::map>& tree, + const EdgesMap& edges, pxr::UsdGeomXformCache& xform_cache) { + pxr::UsdPrim current_body_prim = stage->GetPrimAtPath(current_body_path); + pxr::UsdPrim parent_prim_for_xform = + parent_body_path.IsEmpty() ? stage->GetPseudoRoot() + : stage->GetPrimAtPath(parent_body_path); + + mjsBody* current_mj_body = ParseUsdPhysicsRigidbody( + spec, pxr::UsdPhysicsRigidBodyAPI(current_body_prim), + parent_prim_for_xform, parent_mj_body, xform_cache); + + auto edge_key = std::make_pair(parent_body_path, current_body_path); + auto it_edge = edges.find(edge_key); + + if (it_edge != edges.end()) { + // An edge exists, indicating a connection to the parent. This body is + // either world-attached or part of a larger articulation. + const pxr::SdfPath& joint_path = it_edge->second; + if (!joint_path.IsEmpty()) { + // An explicit joint prim exists, so we parse it. + pxr::UsdPrim joint_prim = stage->GetPrimAtPath(joint_path); + ParseUsdPhysicsJoint(spec, joint_prim, current_mj_body, xform_cache); + } + // If joint_path is empty, it's an implicit fixed joint. No joint is created + // in the mjSpec, effectively welding the body to its parent. + } else { + // No edge found. This condition is met for the root of a floating-base + // tree, which has no defined joint connecting it to the world. + if (parent_mj_body == mjs_findBody(spec, "world")) { + // We explicitly create a free joint to make it a floating-base body. + mjsJoint* free_joint = mjs_addJoint(current_mj_body, nullptr); + free_joint->type = mjJNT_FREE; + } + } + + // Parse all geoms/sites that are found on this body in the USD hierarchy. + ParseCurrentAndDescendants(spec, current_body_prim, parent_prim_for_xform, + current_mj_body, xform_cache); + // Recurse through the kinematic tree. + auto it_tree = tree.find(current_body_path); + if (it_tree != tree.end()) { + const auto& children_paths = it_tree->second; + for (const auto& child_path : children_paths) { + TraverseAndBuildTree(stage, spec, current_mj_body, current_body_path, + child_path, tree, edges, xform_cache); + } + } +} + +// Adds a new edge to the edges map, dealing with duplicates: +// - explicit joints always replace implicit joints +// - more than one explicit joint is unsupported so we print a warning and +// keep the first one found +void AddEdge(EdgesMap& edges, const pxr::SdfPath& from, const pxr::SdfPath& to, + const pxr::SdfPath& joint) { + auto edge_key = std::make_pair(from, to); + auto it = edges.find(edge_key); + + if (it == edges.end()) { + // No existing edge, add the new one. + edges[edge_key] = joint; + } else { + // Edge already exists. + pxr::SdfPath& existing_joint = it->second; + bool new_is_explicit = !joint.IsEmpty(); + bool existing_is_explicit = !existing_joint.IsEmpty(); + + if (new_is_explicit) { + if (existing_is_explicit) { + // Both are explicit: this is an error condition. + mju_warning( + "Multiple explicit joints defined between body %s and body %s. " + "Joint1: %s, Joint2: %s. Keeping the first one found: %s", + (from.IsEmpty() ? "" : from.GetString()).c_str(), + to.GetString().c_str(), existing_joint.GetString().c_str(), + joint.GetString().c_str(), existing_joint.GetString().c_str()); + + } else { + // New is explicit, existing is implicit: replace. + existing_joint = joint; + } + } + // If new is implicit, and an edge already exists (either explicit or + // implicit), we keep the existing one. No action needed. + } +} + +// Returns the nesting body prim (or an invalid prim if there's +// any resets_xform_stack, or we've reached the end). +pxr::UsdPrim GetNestingBodyPrim(const pxr::UsdPrim& prim, + pxr::UsdGeomXformCache& xform_cache) { + if (xform_cache.GetResetXformStack(prim)) { + return pxr::UsdPrim(); + } + pxr::UsdPrim previous_prim = prim.GetParent(); + while (previous_prim.IsValid()) { + // If we find a rigid body, this is our answer. The prim is nested. + if (previous_prim.HasAPI()) { + return previous_prim; + } + // If we encounter a prim that resets the transform stack *before* finding + // a rigid body, the chain is broken. The prim is not nested. + if (xform_cache.GetResetXformStack(previous_prim)) { + return pxr::UsdPrim(); + } + previous_prim = previous_prim.GetParent(); + } + return pxr::UsdPrim(); +} +} // namespace + +mjSpec* mj_parseUSDStage(const pxr::UsdStageRefPtr stage) { + mjSpec* spec = mj_makeSpec(); + + mjsBody* world = mjs_findBody(spec, "world"); + + std::vector physics_scenes; + + // Xform cache to use for all queries when parsing. + pxr::UsdGeomXformCache xform_cache; + + // Search for UsdPhysicsScene type prim, use the first one that has + // the MjcPhysicsSceneAPI applied or the first UsdPhysicsScene otherwise. + std::optional physics_scene; + for (auto prim : stage->Traverse()) { + if (prim.IsA()) { + bool has_mjc_physics_api = prim.HasAPI(); + if (!physics_scene.has_value() || has_mjc_physics_api) { + physics_scene = pxr::UsdPhysicsScene(prim); + // If we've found the first scene with MjcPhysicsSceneAPI, we can stop + // searching. + if (has_mjc_physics_api) { + break; + } + } + } + } + + if (physics_scene.has_value()) { + ParseUsdPhysicsScene(spec, *physics_scene); + } + + pxr::SdfPath default_prim_path; + if (stage->GetDefaultPrim().IsValid()) { + default_prim_path = stage->GetDefaultPrim().GetPath(); + } + + std::vector body_paths; + EdgesMap edges; + // TODO(robinalazard): Re-introduce properly adding objects to their + // respective physics scene, or default. + + // Traverse all prims under the pseudo-root. + // We ensure to traverse into instance proxies to ensure we support + // instanceable references. + // See https://openusd.org/dev/api/_usd__page__scenegraph_instancing.html + for (auto prim : stage->Traverse(pxr::UsdTraverseInstanceProxies())) { + // When traversing the whole scene, if we encounter a rigidbody or a joint, + // then we populate the edges map and the list of body paths, which will be + // processed later to build the articulation trees. + // + // If we encounter _anything else_: + // - if we find that it's an independent prim (e.g. a static collider or + // site) not belonging to any rigidbody, then we add it directly to the + // world. + // - otherwise, they will be handled when building the articulation trees. + + if (prim.IsA()) { + pxr::SdfPath joint_path = prim.GetPath(); + pxr::UsdPhysicsJoint joint(prim); + + pxr::SdfPath body1_path; + pxr::SdfPathVector body1_paths; + joint.GetBody1Rel().GetTargets(&body1_paths); + if (body1_paths.empty()) { + mju_warning("Joint %s does not have body1 rel. Skipping.", + prim.GetPath().GetAsString().c_str()); + continue; + } else if (body1_paths.size() > 1) { + mju_warning("Joint %s has multiple body1 rels. Skipping.", + prim.GetPath().GetAsString().c_str()); + continue; + } + body1_path = body1_paths[0]; + + pxr::SdfPath body0_path; + pxr::SdfPathVector body0_paths; + joint.GetBody0Rel().GetTargets(&body0_paths); + if (body0_paths.size() > 1) { + mju_warning("Joint %s has multiple body0 rels. Skipping.", + prim.GetPath().GetAsString().c_str()); + continue; + } + // Empty body0, or body0 pointing to the default prim means we'll attach + // to the worldbody. + if (body0_paths.empty() || body0_paths[0] == default_prim_path) { + body0_path = pxr::SdfPath(); + } else { + body0_path = body0_paths[0]; + } + + AddEdge(edges, body0_path, body1_path, joint_path); + } else if (prim.HasAPI()) { + pxr::SdfPath body_path = prim.GetPath(); + body_paths.push_back(body_path); + + // Find whether we are nested under a parent body. + // Note: if any xform in between (including the current prim) resets the + // xform stack then we are not nested. + pxr::UsdPrim nesting_body_prim = GetNestingBodyPrim(prim, xform_cache); + if (nesting_body_prim.IsValid()) { + AddEdge(edges, nesting_body_prim.GetPath(), body_path, pxr::SdfPath()); + } + } else { + // TODO(robinalazard): the way we handle independent prims right now means + // their relative transforms will be ignored be always pass + // stage->GetPseudoRoot() as the parent. It works for most scene + // realistically. But we should fix it. + + if (prim.HasAPI()) { + // Find whether the collider belongs to a body. If yes, it will be we + // handled later when building the articulation trees. Otherwise, it's a + // static collider and we add it directly to the world. + pxr::UsdPrim nesting_body_prim = GetNestingBodyPrim(prim, xform_cache); + if (!nesting_body_prim.IsValid()) { + ParseUsdPhysicsCollider(spec, pxr::UsdPhysicsCollisionAPI(prim), + stage->GetPseudoRoot(), world, xform_cache); + } + } + if (prim.HasAPI()) { + // Find whether the site belongs to a body. If yes, it will be we + // handled later when building the articulation trees. Otherwise, it's a + // static site and we add it directly to the world. + pxr::UsdPrim nesting_body_prim = GetNestingBodyPrim(prim, xform_cache); + if (!nesting_body_prim.IsValid()) { + ParseMjcPhysicsSite(spec, pxr::MjcPhysicsSiteAPI(prim), + stage->GetPseudoRoot(), world, xform_cache); + } + } + } + } + + std::optional forest = BuildForestFromEdges(edges, body_paths); + if (forest.has_value()) { + // Now that we have the forest, we can walk through it and add the bodies + // and joints to the spec. From each body we can also visit and parse all + // the children to also add the corresponding colliders to the body in the + // spec. + for (const auto& [root_path, tree] : *forest) { + if (root_path.IsEmpty()) { + // This root is the world. This case handles all kinematic trees that + // are attached to the world (fixed-base articulations). + // We iterate through its direct children which are the root bodies of + // each world-attached tree. We iterate through these children and begin + // the recursive build from there. + const auto& children_of_world = tree.at(pxr::SdfPath()); + for (const auto& child_path : children_of_world) { + TraverseAndBuildTree(stage, spec, world, root_path, child_path, tree, + edges, xform_cache); + } + } else { + // Conversely, this case handles all the remaining top-level root bodies + // which are their own roots and are not attached to the world + // (floating-base articulations). This includes isolated bodies. + // We directly begin the recursive build from the toplevel root. + // + // Note: the absence of an edge in the `edges` map connecting the world + // to this root is what signals to `TraverseAndBuildTree` that this is a + // floating base, prompting the creation of a free joint. + TraverseAndBuildTree(stage, spec, world, pxr::SdfPath(), root_path, + tree, edges, xform_cache); + } + } + } + + return spec; +} + +mjSpec* mj_parseUSDStage(const char* usd_path) { + pxr::UsdStageRefPtr stage = pxr::UsdStage::Open(usd_path); + if (!stage) { + mju_error("Could not open USD stage: %s", usd_path); + return nullptr; + } + return mj_parseUSDStage(stage); +} diff --git a/src/experimental/usd/utils.cc b/src/experimental/usd/utils.cc index 220c4178..0dc66353 100644 --- a/src/experimental/usd/utils.cc +++ b/src/experimental/usd/utils.cc @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "third_party/mujoco/src/experimental/usd/utils.h" +#include #include #include diff --git a/src/experimental/usd/writer.cc b/src/experimental/usd/writer.cc new file mode 100644 index 00000000..55f196fe --- /dev/null +++ b/src/experimental/usd/writer.cc @@ -0,0 +1,68 @@ +// Copyright 2025 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. + +#include + +#include +#include + +#include +#include +#include +#include + +namespace mujoco { +namespace usd { + +// Writer is the main entry point for mj_usd. It is responsible +// for converting the USD scene into a Mujoco model and exposing stepping +// API. +Writer::Writer(pxr::UsdStageRefPtr stage, mjSpec* spec, mjModel_* model) + : stage_(stage), spec_(spec), model_(model) { + BuildMjUsdMapping(); +} + +Writer::~Writer() = default; + +void Writer::BuildMjUsdMapping() { + body_id_to_path_.assign(model_->nbody, pxr::SdfPath()); + + // World body (ID 0) maps to an empty SdfPath. + if (model_->nbody > 0) { + body_id_to_path_[0] = pxr::SdfPath(); + } + + // Iterate over MuJoCo body IDs from the mjModel. + for (int body_idx = 1; body_idx < model_->nbody; ++body_idx) { + const char* body_name = mj_id2name(model_, mjOBJ_BODY, body_idx); + if (body_name) { + mjsBody* spec_body = mjs_findBody(spec_, body_name); + body_id_to_path_[body_idx] = + mujoco::usd::GetUsdPrimPathUserValue(spec_body->element); + } + } +} + +void Writer::Update(const mjData* const data) { + for (const auto& sink_fn : pose_sinks_) { + sink_fn(data, body_id_to_path_); + } +} + +void Writer::AddSink(PoseSinkFn sink_fn) { + pose_sinks_.push_back(std::move(sink_fn)); +} + +} // namespace usd +} // namespace mujoco diff --git a/src/user/user_api.cc b/src/user/user_api.cc index c25049b1..0857f804 100644 --- a/src/user/user_api.cc +++ b/src/user/user_api.cc @@ -41,24 +41,6 @@ using mujoco::user::StringToVector; static constexpr std::size_t kGlobalCacheSize = 500 * (1 << 20); -// prepend prefix -template -static T& operator+(std::string_view prefix, T& base) { - base.prefix = std::string(prefix); - return base; -} - - - -// append suffix -template -static T& operator+(T& base, std::string_view suffix) { - base.suffix = std::string(suffix); - return base; -} - - - // create model mjSpec* mj_makeSpec() { mjCModel* modelC = new mjCModel; @@ -135,8 +117,11 @@ static void SetFrame(mjsBody* body, mjtObj objtype, mjsFrame* frame) { // attach body to a frame of the parent static mjsElement* attachBody(mjCFrame* parent, const mjCBody* child, const char* prefix, const char* suffix) { + mjCBody* mutable_child = const_cast(child); + mutable_child->prefix = prefix; + mutable_child->suffix = suffix; try { - *parent += std::string(prefix) + *(mjCBody*)child + std::string(suffix); + *parent += *mutable_child; } catch (mjCError& e) { parent->model->SetError(e); return nullptr; @@ -151,8 +136,11 @@ static mjsElement* attachBody(mjCFrame* parent, const mjCBody* child, // attach frame to a parent body static mjsElement* attachFrame(mjCBody* parent, const mjCFrame* child, const char* prefix, const char* suffix) { + mjCFrame* mutable_child = const_cast(child); + mutable_child->prefix = prefix; + mutable_child->suffix = suffix; try { - *parent += std::string(prefix) + *(mjCFrame*)child + std::string(suffix); + *parent += *mutable_child; } catch (mjCError& e) { parent->model->SetError(e); return nullptr; @@ -295,37 +283,6 @@ const char* mjs_getError(mjSpec* s) { -// detach body from mjSpec, return 0 on success -int mjs_detachBody(mjSpec* s, mjsBody* b) { - mjCModel* model = static_cast(s->element); - mjCBody* body = static_cast(b->element); - try { - *model -= *body; - } catch (mjCError& e) { - model->SetError(e); - return -1; - } - model->Detach(body); - return 0; -} - -// detach default from mjSpec, return 0 on success -int mjs_detachDefault(mjSpec* s, mjsDefault* def) { - mjCModel* modelC = static_cast(s->element); - if (!def) { - modelC->SetError(mjCError(0, "Cannot detach, default is null")); - return -1; - } - mjCDef* defC = static_cast(def->element); - try { - *modelC -= *defC; - } catch (mjCError& e) { - modelC->SetError(e); - return -1; - } - return 0; -} - // check if model has warnings int mjs_isWarning(mjSpec* s) { mjCModel* modelC = static_cast(s->element); @@ -383,16 +340,20 @@ int mj_copyBack(mjSpec* s, const mjModel* m) { -// delete object, return 0 on success -int mjs_delete(mjsElement* element) { - mjCModel* model; - if (element->elemtype == mjOBJ_DEFAULT) - model = static_cast(element)->model; - else - model = static_cast(element)->model; +// remove body from mjSpec, return 0 on success +int mjs_delete(mjSpec* s, mjsElement* element) { + mjCModel* model = static_cast(s->element); + if (!element) { + model->SetError(mjCError(0, "Element is null.")); + return -1; + } try { - // it will call the appropriate destructor since ~mjCBase is virtual - model->DeleteElement(element); + if (element->elemtype == mjOBJ_DEFAULT) { + mjCDef* def = static_cast(element); + *model -= *def; + } else { + *model -= element; + } return 0; } catch (mjCError& e) { model->SetError(e); @@ -695,6 +656,188 @@ mjsDefault* mjs_addDefault(mjSpec* s, const char* classname, const mjsDefault* p +// set actuator to motor +const char* mjs_setToMotor(mjsActuator* actuator) { + // unit gain + actuator->gainprm[0] = 1; + + // implied parameters + actuator->dyntype = mjDYN_NONE; + actuator->gaintype = mjGAIN_FIXED; + actuator->biastype = mjBIAS_NONE; + return ""; +} + + + +// set to position actuator +const char* mjs_setToPosition(mjsActuator* actuator, double kp, double kv[1], + double dampratio[1], double timeconst[1], double inheritrange) { + actuator->gainprm[0] = kp; + actuator->biasprm[1] = -kp; + + // set biasprm[2]; negative: regular damping, positive: dampratio + if (dampratio && kv) { + return "kv and dampratio cannot both be defined"; + } + + if (kv) { + if (*kv < 0) return "kv cannot be negative"; + actuator->biasprm[2] = -(*kv); + } + if (dampratio) { + if (*dampratio < 0) return "dampratio cannot be negative"; + actuator->biasprm[2] = *dampratio; + } + if (timeconst) { + if (*timeconst < 0) return "timeconst cannot be negative"; + actuator->dynprm[0] = *timeconst; + actuator->dyntype = *timeconst == 0 ? mjDYN_NONE : mjDYN_FILTEREXACT; + } + actuator->inheritrange = inheritrange; + + if (inheritrange > 0) { + if (actuator->ctrlrange[0] || actuator->ctrlrange[1]) { + return "ctrlrange and inheritrange cannot both be defined"; + } + } + + actuator->gaintype = mjGAIN_FIXED; + actuator->biastype = mjBIAS_AFFINE; + return ""; +} + + + +// Set to integrated velocity actuator. +const char* mjs_setToIntVelocity(mjsActuator* actuator, double kp, double kv[1], + double dampratio[1], double timeconst[1], double inheritrange) { + mjs_setToPosition(actuator, kp, kv, dampratio, timeconst, inheritrange); + actuator->dyntype = mjDYN_INTEGRATOR; + actuator->actlimited = 1; + + if (inheritrange > 0) { + if (actuator->actrange[0] || actuator->actrange[1]) { + return "actrange and inheritrange cannot both be defined"; + } + } + return ""; +} + + + +// Set to velocity actuator. +const char* mjs_setToVelocity(mjsActuator* actuator, double kv) { + mjuu_zerovec(actuator->biasprm, mjNBIAS); + actuator->gainprm[0] = kv; + actuator->biasprm[2] = -kv; + actuator->dyntype = mjDYN_NONE; + actuator->gaintype = mjGAIN_FIXED; + actuator->biastype = mjBIAS_AFFINE; + return ""; +} + + + +// Set to damper actuator. +const char* mjs_setToDamper(mjsActuator* actuator, double kv) { + mjuu_zerovec(actuator->gainprm, mjNGAIN); + actuator->gainprm[2] = -kv; + actuator->ctrllimited = 1; + actuator->dyntype = mjDYN_NONE; + actuator->gaintype = mjGAIN_AFFINE; + actuator->biastype = mjBIAS_NONE; + + if (kv < 0) { + return "damping coefficient cannot be negative"; + } + if (actuator->ctrlrange[0] < 0 || actuator->ctrlrange[1] < 0) { + return "damper control range cannot be negative"; + } + return ""; +} + + + +// Set to cylinder actuator. +const char* mjs_setToCylinder(mjsActuator* actuator, double timeconst, double bias, + double area, double diameter) { + actuator->dynprm[0] = timeconst; + actuator->biasprm[0] = bias; + actuator->gainprm[0] = area; + if (diameter >= 0) { + actuator->gainprm[0] = mjPI / 4 * diameter*diameter; + } + actuator->dyntype = mjDYN_FILTER; + actuator->gaintype = mjGAIN_FIXED; + actuator->biastype = mjBIAS_AFFINE; + return ""; +} + + + +// Set to muscle actuator. +const char* mjs_setToMuscle(mjsActuator* actuator, double timeconst[2], double tausmooth, + double range[2], double force, double scale, double lmin, + double lmax, double vmax, double fpmax, double fvmax) { + // set muscle defaults if same as global defaults + if (actuator->dynprm[0] == 1) actuator->dynprm[0] = 0.01; // tau act + if (actuator->dynprm[1] == 0) actuator->dynprm[1] = 0.04; // tau deact + if (actuator->gainprm[0] == 1) actuator->gainprm[0] = 0.75; // range[0] + if (actuator->gainprm[1] == 0) actuator->gainprm[1] = 1.05; // range[1] + if (actuator->gainprm[2] == 0) actuator->gainprm[2] = -1; // force + if (actuator->gainprm[3] == 0) actuator->gainprm[3] = 200; // scale + if (actuator->gainprm[4] == 0) actuator->gainprm[4] = 0.5; // lmin + if (actuator->gainprm[5] == 0) actuator->gainprm[5] = 1.6; // lmax + if (actuator->gainprm[6] == 0) actuator->gainprm[6] = 1.5; // vmax + if (actuator->gainprm[7] == 0) actuator->gainprm[7] = 1.3; // fpmax + if (actuator->gainprm[8] == 0) actuator->gainprm[8] = 1.2; // fvmax + + if (tausmooth < 0) + return "muscle tausmooth cannot be negative"; + + actuator->dynprm[2] = tausmooth; + if (timeconst[0] >= 0) actuator->dynprm[0] = timeconst[0]; + if (timeconst[1] >= 0) actuator->dynprm[1] = timeconst[1]; + if (range[0] >= 0) actuator->gainprm[0] = range[0]; + if (range[1] >= 0) actuator->gainprm[1] = range[1]; + if (force >= 0) actuator->gainprm[2] = force; + if (scale >= 0) actuator->gainprm[3] = scale; + if (lmin >= 0) actuator->gainprm[4] = lmin; + if (lmax >= 0) actuator->gainprm[5] = lmax; + if (vmax >= 0) actuator->gainprm[6] = vmax; + if (fpmax >= 0) actuator->gainprm[7] = fpmax; + if (fvmax >= 0) actuator->gainprm[8] = fvmax; + + // biasprm = gainprm + for (int n=0; n < 9; n++) { + actuator->biasprm[n] = actuator->gainprm[n]; + } + + actuator->dyntype = mjDYN_MUSCLE; + actuator->gaintype = mjGAIN_MUSCLE; + actuator->biastype = mjBIAS_MUSCLE; + return ""; +} + + + +// Set to adhesion actuator. +const char* mjs_setToAdhesion(mjsActuator* actuator, double gain) { + actuator->gainprm[0] = gain; + actuator->ctrllimited = 1; + actuator->gaintype = mjGAIN_FIXED; + actuator->biastype = mjBIAS_NONE; + + if (gain < 0) + return "adhesion gain cannot be negative"; + if (actuator->ctrlrange[0] < 0 || actuator->ctrlrange[1] < 0) + return "adhesion control range cannot be negative"; + return ""; +} + + + // get spec from body mjSpec* mjs_getSpec(mjsElement* element) { return &(static_cast(element)->model->spec); @@ -766,6 +909,8 @@ mjsElement* mjs_findElement(mjSpec* s, mjtObj type, const char* name) { case mjOBJ_LIGHT: case mjOBJ_FRAME: return model->FindTree(model->GetWorld(), type, std::string(name)); // recursive search + case mjOBJ_TEXTURE: + return model->FindTexture(std::string(name)); // check filename too default: return model->FindObject(type, std::string(name)); // always available } @@ -863,7 +1008,7 @@ const char* mjs_resolveOrientation(double quat[4], mjtByte degree, const char* s mjsFrame* mjs_bodyToFrame(mjsBody** body) { mjCBody* bodyC = static_cast((*body)->element); mjCFrame* frameC = bodyC->ToFrame(); - bodyC->model->Detach(bodyC); + *bodyC->model -= (*body)->element; *body = nullptr; return &frameC->spec; } diff --git a/src/user/user_api.h b/src/user/user_api.h index 04cdf13e..cfc33c60 100644 --- a/src/user/user_api.h +++ b/src/user/user_api.h @@ -76,11 +76,6 @@ MJAPI int mj_copyBack(mjSpec* s, const mjModel* m); MJAPI mjsElement* mjs_attach(mjsElement* parent, const mjsElement* child, const char* prefix, const char* suffix); -// Detach body from mjSpec, remove all references and delete the body, return 0 on success. -MJAPI int mjs_detachBody(mjSpec* s, mjsBody* b); - -// Detach default from mjSpec, remove all references and delete the default, return 0 on success. -MJAPI int mjs_detachDefault(mjSpec* s, mjsDefault* d); //---------------------------------- Add tree elements --------------------------------------------- @@ -108,8 +103,8 @@ MJAPI mjsLight* mjs_addLight(mjsBody* body, const mjsDefault* def); // Add frame to body. MJAPI mjsFrame* mjs_addFrame(mjsBody* body, mjsFrame* parentframe); -// Delete object corresponding to the given element, return 0 on success. -MJAPI int mjs_delete(mjsElement* element); +// Remove object corresponding to the given element, return 0 on success. +MJAPI int mjs_delete(mjSpec* s, mjsElement* element); //---------------------------------- Add non-tree elements ----------------------------------------- @@ -166,6 +161,37 @@ MJAPI mjsPlugin* mjs_addPlugin(mjSpec* s); MJAPI mjsDefault* mjs_addDefault(mjSpec* s, const char* classname, const mjsDefault* parent); +//---------------------------------- Set actuator parameters --------------------------------------- + +// Set actuator to motor, return error on failure. +MJAPI const char* mjs_setToMotor(mjsActuator* actuator); + +// Set actuator to position, return error on failure. +MJAPI const char* mjs_setToPosition(mjsActuator* actuator, double kp, double kv[1], + double dampratio[1], double timeconst[1], double inheritrange); + +// Set actuator to integrated velocity, return error on failure. +MJAPI const char* mjs_setToIntVelocity(mjsActuator* actuator, double kp, double kv[1], + double dampratio[1], double timeconst[1], double inheritrange); + +// Set actuator to velocity, return error on failure. +MJAPI const char* mjs_setToVelocity(mjsActuator* actuator, double kv); + +// Set actuator to damper, return error on failure. +MJAPI const char* mjs_setToDamper(mjsActuator* actuator, double kv); + +// Set actuator to cylinder actuator, return error on failure. +MJAPI const char* mjs_setToCylinder(mjsActuator* actuator, double timeconst, + double bias, double area, double diameter); + +// Set actuator to muscle, return error on failure. +MJAPI const char* mjs_setToMuscle(mjsActuator* actuator, double timeconst[2], double tausmooth, + double range[2], double force, double scale, double lmin, + double lmax, double vmax, double fpmax, double fvmax); + +// Set actuator to adhesion, return error on failure. +MJAPI const char* mjs_setToAdhesion(mjsActuator* actuator, double gain); + //---------------------------------- Add assets ---------------------------------------------------- // Add mesh. diff --git a/src/user/user_init.c b/src/user/user_init.c index 34199812..575f15d6 100644 --- a/src/user/user_init.c +++ b/src/user/user_init.c @@ -291,8 +291,8 @@ void mjs_defaultMaterial(mjsMaterial* material) { material->texrepeat[0] = material->texrepeat[1] = 1; material->specular = 0.5; material->shininess = 0.5; - material->metallic = 0.0; - material->roughness = 1.0; + material->metallic = -1.0; + material->roughness = -1.0; material->rgba[0] = material->rgba[1] = material->rgba[2] = material->rgba[3] = 1; } diff --git a/src/user/user_mesh.cc b/src/user/user_mesh.cc index 03ad956c..c8ef9cc6 100644 --- a/src/user/user_mesh.cc +++ b/src/user/user_mesh.cc @@ -139,6 +139,7 @@ mjCMesh::mjCMesh(mjCModel* _model, mjCDef* _def) { maxhullvert_ = -1; processed_ = false; visual_ = true; + needoct_ = false; // reset to default if given if (_def) { @@ -406,6 +407,7 @@ void mjCMesh::CacheMesh(mjCCache* cache, const mjResource* resource) { } mesh->tree_ = tree_; mesh->face_aabb_ = face_aabb_; + mesh->octree_ = octree_; // calculate estimated size of mesh std::size_t size = sizeof(mjCMesh) @@ -423,6 +425,7 @@ void mjCMesh::CacheMesh(mjCCache* cache, const mjResource* resource) { + (sizeof(double) * 18) + (sizeof(int) * ncenter) + tree_.Size() + + octree_.Size() + (sizeof(double) * face_aabb_.size()); std::shared_ptr cached_data(mesh, +[] (const void* data) { @@ -682,6 +685,14 @@ void mjCMesh::TryCompile(const mjVFS* vfs) { } } + // make octree + if (!needoct_) { + octree_.Clear(); + } else if (octree_.Nodes().empty()) { + octree_.SetFace(vert_, face_); + octree_.CreateOctree(aamm_); + } + // close resource if (resource_ != nullptr) { mju_closeResource(resource_); @@ -1073,6 +1084,7 @@ bool mjCMesh::LoadCachedMesh(mjCCache *cache, const mjResource* resource) { } tree_ = mesh->tree_; face_aabb_ = mesh->face_aabb_; + octree_ = mesh->octree_; return true; }; diff --git a/src/user/user_model.cc b/src/user/user_model.cc index 8d4e2b5a..25931205 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -24,6 +24,7 @@ #include #include #include +#include // NOLINT(build/c++17) #include #include #include @@ -653,7 +654,7 @@ mjCModel& mjCModel::operator-=(const mjCBody& subtree) { // add default tree to this model -mjCModel_& mjCModel::operator+=(mjCDef& subtree) { +mjCModel& mjCModel::operator+=(mjCDef& subtree) { defaults_.push_back(&subtree); def_map[subtree.name] = &subtree; subtree.model = this; @@ -732,7 +733,6 @@ void deletefromlist(std::vector* list, mjsElement* element) { for (int j = 0; j < list->size(); ++j) { list->at(j)->id = -1; if (list->at(j) == element) { - list->at(j)->Release(); list->erase(list->begin() + j); j--; } @@ -741,15 +741,47 @@ void deletefromlist(std::vector* list, mjsElement* element) { -// discard all invalid elements from all lists -void mjCModel::DeleteElement(mjsElement* el) { +// recursively delete all plugins in the subtree +void mjCModel::DeleteSubtreePlugin(mjCBody* subtree) { + mjsPlugin* plugin = &(subtree->spec.plugin); + if (plugin->active && plugin->name->empty()) { + *this -= plugin->element; + } + for (auto* body : subtree->Bodies()) { + DeleteSubtreePlugin(body); + } +} + + + +// remove the element from the model +void mjCModel::operator-=(mjsElement* el) { + if (el->elemtype == mjOBJ_BODY) { + mjCBody* body = static_cast(el); + *this -= *body; + } + + detached_.push_back(static_cast(el)); ResetTreeLists(); + if (el->elemtype != mjOBJ_DEFAULT) { + if (static_cast(el)->model != this) { + throw mjCError(nullptr, "element is not in this model"); + } + } else { + if (static_cast(el)->model != this) { + throw mjCError(nullptr, "default is not in this model"); + } + } + switch (el->elemtype) { case mjOBJ_BODY: + { MakeTreeLists(); // rebuild lists that were reset at the beginning of the function - throw mjCError(nullptr, "bodies cannot be deleted, use detach instead"); + mjCBody* subtree = static_cast(el); + DeleteSubtreePlugin(subtree); break; + } case mjOBJ_DEFAULT: MakeTreeLists(); // rebuild lists that were reset at the beginning of the function @@ -759,8 +791,8 @@ void mjCModel::DeleteElement(mjsElement* el) { case mjOBJ_GEOM: { mjCGeom* geom = static_cast(el); - if (geom->plugin.active && geom->plugin.name->empty() && geom->GetRef() == 1) { - DeleteElement(geom->plugin.element); + if (geom->plugin.active && geom->plugin.name->empty()) { + *this -= geom->plugin.element; } deletefromlist(&(geom->body->geoms), el); break; @@ -785,8 +817,8 @@ void mjCModel::DeleteElement(mjsElement* el) { case mjOBJ_MESH: { mjCMesh* mesh = static_cast(el); - if (mesh->plugin.active && mesh->plugin.name->empty() && mesh->GetRef() == 1) { - DeleteElement(mesh->plugin.element); + if (mesh->plugin.active && mesh->plugin.name->empty()) { + *this -= mesh->plugin.element; } deletefromlist(object_lists_[mjOBJ_MESH], el); break; @@ -795,8 +827,8 @@ void mjCModel::DeleteElement(mjsElement* el) { case mjOBJ_ACTUATOR: { mjCActuator* actuator = static_cast(el); - if (actuator->plugin.active && actuator->plugin.name->empty() && actuator->GetRef() == 1) { - DeleteElement(actuator->plugin.element); + if (actuator->plugin.active && actuator->plugin.name->empty()) { + *this -= actuator->plugin.element; } deletefromlist(object_lists_[mjOBJ_ACTUATOR], el); break; @@ -805,8 +837,8 @@ void mjCModel::DeleteElement(mjsElement* el) { case mjOBJ_SENSOR: { mjCSensor* sensor = static_cast(el); - if (sensor->plugin.active && sensor->plugin.name->empty() && sensor->GetRef() == 1) { - DeleteElement(sensor->plugin.element); + if (sensor->plugin.active && sensor->plugin.name->empty()) { + *this -= sensor->plugin.element; } deletefromlist(object_lists_[mjOBJ_SENSOR], el); break; @@ -827,29 +859,6 @@ void mjCModel::DeleteElement(mjsElement* el) { -// recursively delete all plugins in the subtree -void deletesubtreeplugin(mjCBody* subtree, mjCModel* model) { - mjsPlugin* plugin = &(subtree->spec.plugin); - if (plugin->active && plugin->name->empty()) { - model->DeleteElement(plugin->element); - } - for (auto* body : subtree->Bodies()) { - deletesubtreeplugin(body, model); - } -} - - - -// deletes all plugins in the subtree and then the subtree itself -void mjCModel::Detach(mjCBody* subtree) { - if (subtree->GetRef() == 1) { - deletesubtreeplugin(subtree, this); - } - subtree->Release(); -} - - - // TODO: we should not use C-type casting with multiple C++ inheritance void mjCModel::CreateObjectLists() { for (int i = 0; i < mjNOBJECT; ++i) { @@ -939,6 +948,7 @@ mjCModel::~mjCModel() { for (int i=0; i < defaults_.size(); i++) delete defaults_[i]; for (int i=0; i < specs_.size(); i++) mj_deleteSpec(specs_[i]); for (int i=0; i < plugins_.size(); i++) plugins_[i]->Release(); + for (int i=0; i < detached_.size(); i++) detached_[i]->Release(); // clear sizes and pointer lists created in Compile Clear(); @@ -953,6 +963,7 @@ void mjCModel::Clear() { nbvh = 0; nbvhstatic = 0; nbvhdynamic = 0; + noct = 0; njnt = 0; ngeom = 0; nsite = 0; @@ -1369,6 +1380,23 @@ static T* findobject(std::string_view name, const vector& list, const mjKeyM return list[id->second]; } + + +mjCBase* mjCModel::FindTexture(std::string_view name) const { + for (unsigned int i=0; i < textures_.size(); i++) { + if (textures_[i]->name == name) { + return textures_[i]; + } + if (textures_[i]->name.empty() && + std::filesystem::path(textures_[i]->spec_file_).filename().stem() == name) { + return textures_[i]; + } + } + return nullptr; +} + + + // find object in global lists given string type and name mjCBase* mjCModel::FindObject(mjtObj type, string name) const { if (!object_lists_[type]) { @@ -1699,6 +1727,7 @@ void mjCModel::IndexAssets(bool discard) { ((mjCMesh*)mesh)->SetNotVisual(); // reset to true by mesh->Compile() } geom->mesh = (discard && geom->visual_) ? nullptr : (mjCMesh*)mesh; + static_cast(mesh)->needoct_ |= geom->spec.type == mjGEOM_SDF; } else { throw mjCError(geom, "mesh '%s' not found in geom %d", geom->get_meshname().c_str(), i); } @@ -1909,6 +1938,7 @@ void mjCModel::SetSizes() { } for (int i=0; i < nmesh; i++) { nbvhstatic += meshes_[i]->tree().Nbvh(); + noct += meshes_[i]->octree().NumNodes(); } for (int i=0; i < nflex; i++) { nbvhdynamic += flexes_[i]->tree.Nbvh(); @@ -2916,7 +2946,7 @@ int mjCModel::CountNJmom(const mjModel* m) { // copy objects outside kinematic tree void mjCModel::CopyObjects(mjModel* m) { - int adr, bone_adr, vert_adr, node_adr, normal_adr, face_adr, texcoord_adr; + int adr, bone_adr, vert_adr, node_adr, normal_adr, face_adr, texcoord_adr, oct_adr; int edge_adr, elem_adr, elemdata_adr, elemedge_adr, shelldata_adr, evpair_adr; int bonevert_adr, graph_adr, data_adr, bvh_adr; int poly_adr, polymap_adr, polyvert_adr; @@ -2936,6 +2966,7 @@ void mjCModel::CopyObjects(mjModel* m) { } // meshes + oct_adr = 0; vert_adr = 0; normal_adr = 0; texcoord_adr = 0; @@ -2962,6 +2993,8 @@ void mjCModel::CopyObjects(mjModel* m) { m->mesh_graphadr[i] = (pme->szgraph() ? graph_adr : -1); m->mesh_bvhnum[i] = pme->tree().Nbvh(); m->mesh_bvhadr[i] = pme->tree().Nbvh() ? bvh_adr : -1; + m->mesh_octnum[i] = pme->octree().NumNodes(); + m->mesh_octadr[i] = pme->octree().NumNodes() ? oct_adr : -1; mjuu_copyvec(&m->mesh_scale[3 * i], pme->Scale(), 3); mjuu_copyvec(&m->mesh_pos[3 * i], pme->GetPosPtr(), 3); mjuu_copyvec(&m->mesh_quat[4 * i], pme->GetQuatPtr(), 4); @@ -2996,6 +3029,14 @@ void mjCModel::CopyObjects(mjModel* m) { } } + // copy octree data + if (pme->octree().NumNodes()) { + int n_oct = pme->octree().NumNodes(); + memcpy(m->oct_aabb + 6*oct_adr, pme->octree().Nodes().data(), 6*n_oct*sizeof(mjtNum)); + memcpy(m->oct_child + 8*oct_adr, pme->octree().Child().data(), 8*n_oct*sizeof(int)); + memcpy(m->oct_depth + oct_adr, pme->octree().Level().data(), n_oct*sizeof(int)); + } + // advance counters poly_adr += pme->npolygon(); polyvert_adr += pme->npolygonvert(); @@ -3006,6 +3047,7 @@ void mjCModel::CopyObjects(mjModel* m) { face_adr += pme->nface(); graph_adr += pme->szgraph(); bvh_adr += pme->tree().Nbvh(); + oct_adr += pme->octree().NumNodes(); } // flexes @@ -3824,24 +3866,77 @@ void mjCModel::FuseReindex(mjCBody* body) { +template +void mjCModel::ReassignChild(std::vector& dest, std::vector& list, + mjCBody* parent, mjCBody* body) { + for (int j=0; j < list.size(); j++) { + // assign + list[j]->body = parent; + dest.push_back(list[j]); + + // change frame + changeframe(list[j]->pos, list[j]->quat, body->pos, body->quat); + } + list.clear(); +} + + + +template +void mjCModel::ResolveReferences(std::vector& list, mjCBody* body) { + for (auto& item : list) { + item->CopyFromSpec(); + item->ResolveReferences(this); + } +} + + + +template <> +void mjCModel::ResolveReferences(std::vector& list, mjCBody* body) { + for (auto& item : list) { + item->CopyFromSpec(); + item->ResolveReferences(this); + } + for (mjCSensor* sensor : list) { + if (sensor->objtype == mjOBJ_SITE && + (sensor->type == mjSENS_FORCE || sensor->type == mjSENS_TORQUE) && + static_cast(sensor->obj)->body == body) { + throw mjCError(sensor, "cannot fuse a body used by a force/torque sensor"); + } + } +} + + + // fuse static bodies with their parent void mjCModel::FuseStatic(void) { - // skip if model has potential to reference elements with changed ids - if (!skins_.empty() || - !pairs_.empty() || - !excludes_.empty() || - !equalities_.empty() || - !tendons_.empty() || - !actuators_.empty() || - !sensors_.empty() || - !tuples_.empty() || - !cameras_.empty() || - !lights_.empty()) { - return; - } - - // process fusable bodies for (int i=1; i < bodies_.size(); i++) { + // check if the body can be fused + if (!bodies_[i]->name.empty()) { + ids[mjOBJ_BODY].erase(bodies_[i]->name); + + // try to resolve references without the name of this body, if it fails, skip + try { + ResolveReferences(cameras_); + ResolveReferences(lights_); + ResolveReferences(skins_); + ResolveReferences(pairs_); + ResolveReferences(excludes_); + ResolveReferences(equalities_); + ResolveReferences(tendons_); + ResolveReferences(actuators_); + ResolveReferences(sensors_, bodies_[i]); + ResolveReferences(tuples_); + } catch (mjCError err) { + ids[mjOBJ_BODY].insert({bodies_[i]->name, i}); + continue; + } + + // put body back the body name in the map + ids[mjOBJ_BODY].insert({bodies_[i]->name, i}); + } + // get body and parent mjCBody* body = bodies_[i]; mjCBody* par = body->parent; @@ -3891,25 +3986,8 @@ void mjCModel::FuseStatic(void) { //------------- assign geoms and sites to parent, change frames - // geoms - for (int j=0; j < body->geoms.size(); j++) { - // assign - body->geoms[j]->body = par; - par->geoms.push_back(body->geoms[j]); - - // change frame - changeframe(body->geoms[j]->pos, body->geoms[j]->quat, body->pos, body->quat); - } - - // sites - for (int j=0; j < body->sites.size(); j++) { - // assign - body->sites[j]->body = par; - par->sites.push_back(body->sites[j]); - - // change frame - changeframe(body->sites[j]->pos, body->sites[j]->quat, body->pos, body->quat); - } + ReassignChild(par->geoms, body->geoms, par, body); + ReassignChild(par->sites, body->sites, par, body); //------------- remove from global body list, reduce global counts @@ -3960,15 +4038,21 @@ void mjCModel::FuseStatic(void) { //------------- delete body (without deleting children) + // remove body name from map + if (!body->name.empty()) { + ids[mjOBJ_BODY].erase(body->name); + } + // delete allocation body->bodies.clear(); - body->geoms.clear(); - body->sites.clear(); delete body; // check index i again (we have a new body at this index) i--; } + + // remove empty names + processlist(ids, bodies_, mjOBJ_BODY, true); } @@ -4392,6 +4476,17 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) { bodies_[i]->Compile(); // also compiles joints, geoms, sites, cameras, lights, frames } + // fuse static if enabled + if (compiler.fusestatic) { + FuseStatic(); + for (int i=0; i < lights_.size(); i++) { + lights_[i]->Compile(); + } + for (int i=0; i < cameras_.size(); i++) { + cameras_[i]->Compile(); + } + } + // compile all other objects except for keyframes for (auto flex : flexes_) flex->Compile(vfs); for (auto skin : skins_) skin->Compile(vfs); @@ -4423,10 +4518,6 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) { // resolve asset references, compute sizes IndexAssets(compiler.discardvisual); SetSizes(); - // fuse static if enabled - if (compiler.fusestatic) { - FuseStatic(); - } // set nmocap and body.mocapid for (mjCBody* body : bodies_) { @@ -4439,16 +4530,15 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) { } // check mass and inertia of moving bodies - if (bodies_.size() > 1) { - // we ignore the first body as it is the world body - if (!CheckBodiesMassInertia(std::vector(bodies_.begin()+1, bodies_.end()))) { - throw mjCError(0, "mass and inertia of moving bodies must be larger than mjMINVAL"); + for (int i=1; i < bodies_.size(); i++) { + if (!bodies_[i]->joints.empty() && !CheckBodyMassInertia(bodies_[i])) { + throw mjCError(bodies_[i], "mass and inertia of moving bodies must be larger than mjMINVAL"); } } // create low-level model mj_makeModel(&m, - nq, nv, nu, na, nbody, nbvh, nbvhstatic, nbvhdynamic, njnt, ngeom, nsite, + nq, nv, nu, na, nbody, nbvh, nbvhstatic, nbvhdynamic, noct, njnt, ngeom, nsite, ncam, nlight, nflex, nflexnode, nflexvert, nflexedge, nflexelem, nflexelemdata, nflexelemedge, nflexshelldata, nflexevpair, nflextexcoord, nmesh, nmeshvert, nmeshnormal, nmeshtexcoord, nmeshface, nmeshgraph, nmeshpoly, @@ -4687,20 +4777,6 @@ uint64_t mjCModel::Signature() { -bool mjCModel::CheckBodiesMassInertia(std::vector bodies) { - // check mass and inertia of moving bodies - for (int i=0; i < bodies.size(); i++) { - if (!bodies[i]->joints.empty()) { - if (!CheckBodyMassInertia(bodies[i])) { - return false; - } - } - } - return true; -} - - - bool mjCModel::CheckBodyMassInertia(mjCBody* body) { // check if body has valid mass and inertia if (body->mass >= mjMINVAL && diff --git a/src/user/user_model.h b/src/user/user_model.h index cfd8082e..b5e937bd 100644 --- a/src/user/user_model.h +++ b/src/user/user_model.h @@ -87,6 +87,7 @@ class mjCModel_ : public mjsElement { int nbvh; // number of total boundary volume hierarchies int nbvhstatic; // number of static boundary volume hierarchies int nbvhdynamic; // number of dynamic boundary volume hierarchies + int noct; // number of total octree cells int nflexnode; // number of nodes in all flexes int nflexvert; // number of vertices in all flexes int nflexedge; // number of edges in all flexes @@ -187,7 +188,7 @@ class mjCModel : public mjCModel_, private mjSpec { mjCModel& operator=(const mjCModel& other); // copy other into this, if they are not the same mjCModel& operator+=(const mjCModel& other); // add other into this, even if they are the same mjCModel& operator-=(const mjCBody& subtree); // remove subtree and all references from model - mjCModel_& operator+=(mjCDef& subtree); // add default tree to this model + mjCModel& operator+=(mjCDef& subtree); // add default tree to this model mjCModel& operator-=(const mjCDef& subtree); // remove default tree from this model mjSpec spec; @@ -227,14 +228,11 @@ class mjCModel : public mjCModel_, private mjSpec { template void DeleteAll(std::vector& elements); // delete object from the corresponding list - void DeleteElement(mjsElement* el); + void operator-=(mjsElement* el); // delete default and all descendants void RemoveDefault(mjCDef* def); - // detach subtree from model - void Detach(mjCBody* subtree); - // API for access to model elements (outside tree) int NumObjects(mjtObj type); // number of objects in specified list mjCBase* GetObject(mjtObj type, int id); // pointer to specified object @@ -248,6 +246,7 @@ class mjCModel : public mjCModel_, private mjSpec { mjCDef* FindDefault(std::string name); // find defaults class name mjCDef* AddDefault(std::string name, mjCDef* parent = nullptr); // add defaults class to array mjCBase* FindObject(mjtObj type, std::string name) const; // find object given type and name + mjCBase* FindTexture(std::string_view name) const; // find texture given name mjCBase* FindTree(mjCBody* body, mjtObj type, std::string name); // find tree object given name mjSpec* FindSpec(std::string name) const; // find spec given name mjSpec* FindSpec(const mjsCompiler* compiler_); // find spec given mjsCompiler @@ -427,9 +426,6 @@ class mjCModel : public mjCModel_, private mjSpec { // compute qpos0 void ComputeReference(); - // return true if all bodies have valid mass and inertia - bool CheckBodiesMassInertia(std::vector bodies); - // return true if body has valid mass and inertia bool CheckBodyMassInertia(mjCBody* body); @@ -444,11 +440,23 @@ class mjCModel : public mjCModel_, private mjSpec { // generate a signature for the model uint64_t Signature(); + // reassign children of a body to a new parent + template + void ReassignChild(std::vector& dest, std::vector& list, mjCBody* parent, mjCBody* body); + + // resolve references in a list of objects + template + void ResolveReferences(std::vector& list, mjCBody* body = nullptr); + + // delete all plugins created by the subtree + void DeleteSubtreePlugin(mjCBody* subtree); + mjListKeyMap ids; // map from object names to ids mjCError errInfo; // last error info std::vector key_pending_; // attached keyframes bool deepcopy_; // copy objects when attaching bool attached_ = false; // true if model is attached to a parent model std::unordered_map compiler2spec_; // map from compiler to spec + std::vector detached_; // list of detached objects }; #endif // MUJOCO_SRC_USER_USER_MODEL_H_ diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index 6a1b2451..a0a4df06 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -404,9 +404,15 @@ mjCBoundingVolumeHierarchy::AddBoundingVolume(const int* id, int contype, int co // create bounding volume hierarchy void mjCBoundingVolumeHierarchy::CreateBVH() { + std::vector elements; + Make(elements); + MakeBVH(elements.begin(), elements.end()); +} + + +void mjCBoundingVolumeHierarchy::Make(std::vector& elements) { // precompute the positions of each element in the hierarchy's axes, and drop // visual-only elements. - std::vector elements; elements.reserve(bvleaf_.size()); double qinv[4] = {iquat_[0], -iquat_[1], -iquat_[2], -iquat_[3]}; for (int i = 0; i < bvleaf_.size(); i++) { @@ -420,9 +426,9 @@ void mjCBoundingVolumeHierarchy::CreateBVH() { elements.push_back(std::move(element)); } } - MakeBVH(elements.begin(), elements.end()); } + // compute bounding volume hierarchy int mjCBoundingVolumeHierarchy::MakeBVH( std::vector::iterator elements_begin, @@ -546,10 +552,109 @@ int mjCBoundingVolumeHierarchy::MakeBVH( return index; } + + +//------------------------- class mjCOctree implementation -------------------------------------------- + +void mjCOctree::SetFace(const std::vector& vert, const std::vector& face) { + for (int i = 0; i < face.size(); i += 3) { + std::array v0 = {vert[3*face[i+0]], vert[3*face[i+0]+1], vert[3*face[i+0]+2]}; + std::array v1 = {vert[3*face[i+1]], vert[3*face[i+1]+1], vert[3*face[i+1]+2]}; + std::array v2 = {vert[3*face[i+2]], vert[3*face[i+2]+1], vert[3*face[i+2]+2]}; + face_.push_back({v0, v1, v2}); + } +} + + +// TODO: use the same code as mjCBoundingVolumeHierarchy::Make() +void mjCOctree::Make(std::vector& elements) { + // rotate triangles to the body inertial frame + elements.assign(face_.size(), {{{0}}}); + double qinv[4] = {iquat_[0], -iquat_[1], -iquat_[2], -iquat_[3]}; + for (int i = 0; i < face_.size(); i++) { + for (int j = 0; j < 3; j++) { + double vert[3] = {face_[i][j][0] - ipos_[0], + face_[i][j][1] - ipos_[1], + face_[i][j][2] - ipos_[2]}; + mjuu_rotVecQuat(elements[i][j].data(), vert, qinv); + } + } +} + + +void mjCOctree::CreateOctree(const double aamm[6]) { + std::vector elements; + Make(elements); + std::vector elements_ptrs(elements.size()); + std::transform(elements.begin(), elements.end(), elements_ptrs.begin(), + [](Triangle& triangle) { return ▵ }); + MakeOctree(elements_ptrs, aamm); +} + + +static bool boxTriangle(const Triangle& element, const double aamm[6]) { + for (int i = 0; i < 3; i++) { + if (element[0][i] < aamm[i] && element[1][i] < aamm[i] && element[2][i] < aamm[i]) { + return false; + } + int j = i + 3; + if (element[0][i] > aamm[j] && element[1][i] > aamm[j] && element[2][i] > aamm[j]) { + return false; + } + } + // TODO: add additionally separating axis tests + return true; +} + + +int mjCOctree::MakeOctree(const std::vector& elements, const double aamm[6], int lev) { + level_.push_back(lev); + + // create a new node + int index = nnode_++; + double aabb[6] = {(aamm[0] + aamm[3]) / 2, (aamm[1] + aamm[4]) / 2, (aamm[2] + aamm[5]) / 2, + (aamm[3] - aamm[0]) / 2, (aamm[4] - aamm[1]) / 2, (aamm[5] - aamm[2]) / 2}; + for (int i = 0; i < 6; i++) { + node_.push_back(aabb[i]); + } + for (int i = 0; i < 8; i++) { + child_.push_back(-1); + } + + // find all triangles that intersect the current box + std::vector colliding; + for (auto* element : elements) { + if (boxTriangle(*element, aamm)) { + colliding.push_back(element); + } + } + + // return if the box is empty + if (colliding.empty() || lev >= 6) { + return index; + } + + // split the box into 8 sub-boxes + double new_aamm[8][6]; + for (int i = 0; i < 8; i++) { + new_aamm[i][0] = aabb[0] + aabb[3] * (i & 1 ? -1 : 0); + new_aamm[i][1] = aabb[1] + aabb[4] * (i & 2 ? -1 : 0); + new_aamm[i][2] = aabb[2] + aabb[5] * (i & 4 ? -1 : 0); + new_aamm[i][3] = aabb[0] + aabb[3] * (i & 1 ? 0 : 1); + new_aamm[i][4] = aabb[1] + aabb[4] * (i & 2 ? 0 : 1); + new_aamm[i][5] = aabb[2] + aabb[5] * (i & 4 ? 0 : 1); + } + + // recursive calls to create sub-boxes + for (int i = 0; i < 8; i++) { + child_[8*index + i] = MakeOctree(colliding, new_aamm[i], lev + 1); + } + + return index; +} + //------------------------- class mjCDef implementation -------------------------------------------- - - // constructor mjCDef::mjCDef() { name.clear(); @@ -2392,6 +2497,7 @@ int mjCJoint::Compile(void) { if (type == mjJNT_FREE) { limited = mjLIMITED_FALSE; } + // otherwise if limited is auto, check consistency wrt auto-limits else if (limited == mjLIMITED_AUTO) { bool hasrange = !(range[0] == 0 && range[1] == 0); @@ -2423,6 +2529,7 @@ int mjCJoint::Compile(void) { if (type == mjJNT_FREE || type == mjJNT_BALL) { actfrclimited = mjLIMITED_FALSE; } + // otherwise if actfrclimited is auto, check consistency wrt auto-limits else if (actfrclimited == mjLIMITED_AUTO) { bool hasrange = !(actfrcrange[0] == 0 && actfrcrange[1] == 0); @@ -3562,6 +3669,19 @@ void mjCCamera::CopyFromSpec() { +void mjCCamera::ResolveReferences(const mjCModel* m) { + if (!targetbody_.empty()) { + mjCBody* tb = (mjCBody*)m->FindObject(mjOBJ_BODY, targetbody_); + if (tb) { + targetbodyid = tb->id; + } else { + throw mjCError(this, "unknown target body in camera"); + } + } +} + + + // compiler void mjCCamera::Compile(void) { CopyFromSpec(); @@ -3587,14 +3707,7 @@ void mjCCamera::Compile(void) { mjuu_normvec(quat, 4); // get targetbodyid - if (!targetbody_.empty()) { - mjCBody* tb = (mjCBody*)model->FindObject(mjOBJ_BODY, targetbody_); - if (tb) { - targetbodyid = tb->id; - } else { - throw mjCError(this, "unknown target body in camera"); - } - } + ResolveReferences(model); // make sure the image size is finite if (fovy >= 180) { @@ -3716,6 +3829,27 @@ void mjCLight::CopyFromSpec() { +void mjCLight::ResolveReferences(const mjCModel* m) { + if (!targetbody_.empty()) { + mjCBody* tb = (mjCBody*)m->FindObject(mjOBJ_BODY, targetbody_); + if (tb) { + targetbodyid = tb->id; + } else { + throw mjCError(this, "unknown target body in light"); + } + } + if (!texture_.empty()) { + mjCTexture* tex = (mjCTexture*)m->FindObject(mjOBJ_TEXTURE, texture_); + if (tex) { + texid = tex->id; + } else { + throw mjCError(this, "unknown texture in light"); + } + } +} + + + // compiler void mjCLight::Compile(void) { CopyFromSpec(); @@ -3735,25 +3869,8 @@ void mjCLight::Compile(void) { throw mjCError(this, "zero direction in light"); } - // get targetbodyid - if (!targetbody_.empty()) { - mjCBody* tb = (mjCBody*)model->FindObject(mjOBJ_BODY, targetbody_); - if (tb) { - targetbodyid = tb->id; - } else { - throw mjCError(this, "unknown target body in light"); - } - } - - // get texture - if (!texture_.empty()) { - mjCTexture* tex = (mjCTexture*)model->FindObject(mjOBJ_TEXTURE, texture_); - if (tex) { - texid = tex->id; - } else { - throw mjCError(this, "unknown target body in light"); - } - } + // get targetbodyid and texid + ResolveReferences(model); } @@ -6493,6 +6610,8 @@ void mjCSensor::CopyPlugin() { void mjCSensor::ResolveReferences(const mjCModel* m) { + obj = nullptr; + ref = nullptr; objname_ = prefix + objname_ + suffix; refname_ = prefix + refname_ + suffix; diff --git a/src/user/user_objects.h b/src/user/user_objects.h index 18158c5c..dd013963 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -206,13 +206,54 @@ class mjCBoundingVolumeHierarchy : public mjCBoundingVolumeHierarchy_ { // position of the element in the BVH axes double lpos[3]; }; - + void Make(std::vector& elements); int MakeBVH(std::vector::iterator elements_begin, std::vector::iterator elements_end, int lev = 0); }; +//------------------------- class mjCOctree -------------------------------------------------------- + +typedef std::array, 3> Triangle; + +struct mjCOctree_ { + int nnode_ = 0; + std::vector child_; // children of each node (nnode x 8) + std::vector node_; // bounding boxes (nnode x 6) + std::vector level_; // levels of each node (nnode x 1) + std::vector face_; // mesh faces (nface x 3) + double ipos_[3] = {0, 0, 0}; + double iquat_[4] = {1, 0, 0, 0}; +}; + +class mjCOctree : public mjCOctree_ { + public: + void CreateOctree(const double aamm[6]); + + int NumNodes() const { return nnode_; } + const std::vector& Child() const { return child_; } + const std::vector& Nodes() const { return node_; } + const std::vector& Level() const { return level_; } + void SetFace(const std::vector& vert, const std::vector& face); + int Size() const { + return sizeof(int) * child_.size() + sizeof(double) * node_.size() + + sizeof(int) * level_.size() + sizeof(Triangle) * face_.size(); + } + void Clear() { + child_.clear(); + node_.clear(); + level_.clear(); + face_.clear(); + } + + private: + void Make(std::vector& elements); + int MakeOctree(const std::vector& elements, const double aamm[6], int lev = 0); +}; + + + //------------------------- class mjCBase ---------------------------------------------------------- // Generic functionality for all derived classes @@ -388,8 +429,8 @@ class mjCBody : public mjCBody_, private mjsBody { friend class mjXURDF; public: - mjCBody(mjCModel*); // constructor - ~mjCBody(); // destructor + explicit mjCBody(mjCModel*); + ~mjCBody(); // API for adding objects to body mjCBody* AddBody(mjCDef* = 0); @@ -571,7 +612,7 @@ class mjCJoint : public mjCJoint_, private mjsJoint { friend class mjXURDF; public: - mjCJoint(mjCModel* = nullptr, mjCDef* = nullptr); + explicit mjCJoint(mjCModel* = nullptr, mjCDef* = nullptr); mjCJoint(const mjCJoint& other); mjCJoint& operator=(const mjCJoint& other); @@ -651,7 +692,7 @@ class mjCGeom : public mjCGeom_, private mjsGeom { friend class mjXURDF; public: - mjCGeom(mjCModel* = nullptr, mjCDef* = nullptr); + explicit mjCGeom(mjCModel* = nullptr, mjCDef* = nullptr); mjCGeom(const mjCGeom& other); mjCGeom& operator=(const mjCGeom& other); @@ -716,7 +757,7 @@ class mjCSite : public mjCSite_, private mjsSite { friend class mjXURDF; public: - mjCSite(mjCModel* = nullptr, mjCDef* = nullptr); + explicit mjCSite(mjCModel* = nullptr, mjCDef* = nullptr); mjCSite(const mjCSite& other); mjCSite& operator=(const mjCSite& other); @@ -766,7 +807,7 @@ class mjCCamera : public mjCCamera_, private mjsCamera { friend class mjXWriter; public: - mjCCamera(mjCModel* = nullptr, mjCDef* = nullptr); + explicit mjCCamera(mjCModel* = nullptr, mjCDef* = nullptr); mjCCamera(const mjCCamera& other); mjCCamera& operator=(const mjCCamera& other); @@ -786,6 +827,7 @@ class mjCCamera : public mjCCamera_, private mjsCamera { void CopyFromSpec(void); void PointToLocal(void); void NameSpace(const mjCModel* m); + void ResolveReferences(const mjCModel* m); }; @@ -811,7 +853,7 @@ class mjCLight : public mjCLight_, private mjsLight { friend class mjXWriter; public: - mjCLight(mjCModel* = nullptr, mjCDef* = nullptr); + explicit mjCLight(mjCModel* = nullptr, mjCDef* = nullptr); mjCLight(const mjCLight& other); mjCLight& operator=(const mjCLight& other); @@ -831,6 +873,7 @@ class mjCLight : public mjCLight_, private mjsLight { void CopyFromSpec(void); void PointToLocal(void); void NameSpace(const mjCModel* m); + void ResolveReferences(const mjCModel* m); }; @@ -895,7 +938,7 @@ class mjCFlex: public mjCFlex_, private mjsFlex { friend class mjXWriter; public: - mjCFlex(mjCModel* = nullptr); + explicit mjCFlex(mjCModel* = nullptr); mjCFlex(const mjCFlex& other); mjCFlex& operator=(const mjCFlex& other); @@ -963,6 +1006,7 @@ class mjCMesh_ : public mjCBase { std::vector spec_facetexcoord_; // used by the compiler + bool needoct_; // needs octree bool visual_; // true: the mesh is only visual std::vector< std::pair > halfedge_; // half-edge data @@ -983,9 +1027,13 @@ class mjCMesh_ : public mjCBase { bool needhull_; // needs convex hull for collisions int maxhullvert_; // max vertex count of convex hull + // bounding volume hierarchy tree mjCBoundingVolumeHierarchy tree_; // bounding volume hierarchy std::vector face_aabb_; // bounding boxes of all faces + // octree + mjCOctree octree_; // octree of the mesh + // paths stored during model attachment mujoco::user::FilePath modelfiledir_; mujoco::user::FilePath meshdir_; @@ -995,7 +1043,7 @@ class mjCMesh: public mjCMesh_, private mjsMesh { friend class mjCModel; public: - mjCMesh(mjCModel* = nullptr, mjCDef* = nullptr); + explicit mjCMesh(mjCModel* = nullptr, mjCDef* = nullptr); mjCMesh(const mjCMesh& other); mjCMesh& operator=(const mjCMesh& other); ~mjCMesh(); @@ -1059,6 +1107,9 @@ class mjCMesh: public mjCMesh_, private mjsMesh { // bounding volume hierarchy tree const mjCBoundingVolumeHierarchy& tree() { return tree_; } + // octree + const mjCOctree& octree() { return octree_; } + void Compile(const mjVFS* vfs); // compiler double* GetPosPtr(); // get position double* GetQuatPtr(); // get orientation @@ -1198,7 +1249,7 @@ class mjCSkin: public mjCSkin_, private mjsSkin { friend class mjXWriter; public: - mjCSkin(mjCModel* = nullptr); + explicit mjCSkin(mjCModel* = nullptr); mjCSkin(const mjCSkin& other); mjCSkin& operator=(const mjCSkin& other); ~mjCSkin(); @@ -1256,7 +1307,7 @@ class mjCHField : public mjCHField_, private mjsHField { friend class mjXWriter; public: - mjCHField(mjCModel* model); + explicit mjCHField(mjCModel* model); mjCHField(const mjCHField& other); mjCHField& operator=(const mjCHField& other); ~mjCHField(); @@ -1308,7 +1359,7 @@ class mjCTexture : public mjCTexture_, private mjsTexture { friend class mjXWriter; public: - mjCTexture(mjCModel*); + explicit mjCTexture(mjCModel*); mjCTexture(const mjCTexture& other); mjCTexture& operator=(const mjCTexture& other); ~mjCTexture(); @@ -1369,7 +1420,7 @@ class mjCMaterial : public mjCMaterial_, private mjsMaterial { friend class mjXWriter; public: - mjCMaterial(mjCModel* = nullptr, mjCDef* = nullptr); + explicit mjCMaterial(mjCModel* = nullptr, mjCDef* = nullptr); mjCMaterial(const mjCMaterial& other); mjCMaterial& operator=(const mjCMaterial& other); @@ -1409,7 +1460,7 @@ class mjCPair : public mjCPair_, private mjsPair { friend class mjXWriter; public: - mjCPair(mjCModel* = nullptr, mjCDef* = nullptr); + explicit mjCPair(mjCModel* = nullptr, mjCDef* = nullptr); mjCPair(const mjCPair& other); mjCPair& operator=(const mjCPair& other); @@ -1458,7 +1509,7 @@ class mjCBodyPair : public mjCBodyPair_, private mjsExclude { friend class mjCModel; public: - mjCBodyPair(mjCModel*); + explicit mjCBodyPair(mjCModel*); mjCBodyPair(const mjCBodyPair& other); mjCBodyPair& operator=(const mjCBodyPair& other); @@ -1504,7 +1555,7 @@ class mjCEquality : public mjCEquality_, private mjsEquality { friend class mjXWriter; public: - mjCEquality(mjCModel* = 0, mjCDef* = 0); + explicit mjCEquality(mjCModel* = 0, mjCDef* = 0); mjCEquality(const mjCEquality& other); mjCEquality& operator=(const mjCEquality& other); @@ -1543,7 +1594,7 @@ class mjCTendon : public mjCTendon_, private mjsTendon { friend class mjXWriter; public: - mjCTendon(mjCModel* = nullptr, mjCDef* = nullptr); + explicit mjCTendon(mjCModel* = nullptr, mjCDef* = nullptr); mjCTendon(const mjCTendon& other); mjCTendon& operator=(const mjCTendon& other); ~mjCTendon(); @@ -1639,7 +1690,7 @@ class mjCPlugin : public mjCPlugin_ { friend class mjXWriter; public: - mjCPlugin(mjCModel*); + explicit mjCPlugin(mjCModel*); mjCPlugin(const mjCPlugin& other); mjCPlugin& operator=(const mjCPlugin& other); @@ -1687,7 +1738,7 @@ class mjCActuator : public mjCActuator_, private mjsActuator { friend class mjXWriter; public: - mjCActuator(mjCModel* = nullptr, mjCDef* = nullptr); + explicit mjCActuator(mjCModel* = nullptr, mjCDef* = nullptr); mjCActuator(const mjCActuator& other); mjCActuator& operator=(const mjCActuator& other); @@ -1748,7 +1799,7 @@ class mjCSensor : public mjCSensor_, private mjsSensor { friend class mjXWriter; public: - mjCSensor(mjCModel*); + explicit mjCSensor(mjCModel*); mjCSensor(const mjCSensor& other); mjCSensor& operator=(const mjCSensor& other); @@ -1789,7 +1840,7 @@ class mjCNumeric : public mjCNumeric_, private mjsNumeric { friend class mjXWriter; public: - mjCNumeric(mjCModel*); + explicit mjCNumeric(mjCModel*); mjCNumeric(const mjCNumeric& other); mjCNumeric& operator=(const mjCNumeric& other); ~mjCNumeric(); @@ -1821,7 +1872,7 @@ class mjCText : public mjCText_, private mjsText { friend class mjXWriter; public: - mjCText(mjCModel*); + explicit mjCText(mjCModel*); mjCText(const mjCText& other); mjCText& operator=(const mjCText& other); ~mjCText(); @@ -1858,7 +1909,7 @@ class mjCTuple : public mjCTuple_, private mjsTuple { friend class mjXWriter; public: - mjCTuple(mjCModel*); + explicit mjCTuple(mjCModel*); mjCTuple(const mjCTuple& other); mjCTuple& operator=(const mjCTuple& other); ~mjCTuple(); @@ -1902,7 +1953,7 @@ class mjCKey : public mjCKey_, private mjsKey { friend class mjXWriter; public: - mjCKey(mjCModel*); + explicit mjCKey(mjCModel*); mjCKey(const mjCKey& other); mjCKey& operator=(const mjCKey& other); ~mjCKey(); @@ -1928,7 +1979,7 @@ class mjCDef : public mjsElement { public: mjCDef(); - mjCDef(mjCModel*); + explicit mjCDef(mjCModel*); mjCDef(const mjCDef& other); mjCDef& operator=(const mjCDef& other); mjCDef& operator+=(const mjCDef& other); diff --git a/src/xml/xml.cc b/src/xml/xml.cc index 57b1e123..e7d9f581 100644 --- a/src/xml/xml.cc +++ b/src/xml/xml.cc @@ -27,8 +27,6 @@ #include #include -#include "tinyxml2.h" - #include #include #include @@ -42,6 +40,7 @@ #include "xml/xml_native_writer.h" #include "xml/xml_urdf.h" #include "xml/xml_util.h" +#include "tinyxml2.h" namespace { diff --git a/src/xml/xml_base.h b/src/xml/xml_base.h index 9dc261b1..f6e41683 100644 --- a/src/xml/xml_base.h +++ b/src/xml/xml_base.h @@ -18,10 +18,10 @@ #include #include -#include "tinyxml2.h" #include #include #include "xml/xml_util.h" +#include "tinyxml2.h" // keyword maps (defined in implementation files) diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index c5ad9c8f..89f145db 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -28,8 +28,6 @@ #include #include -#include "tinyxml2.h" - #include #include #include @@ -45,6 +43,7 @@ #include "user/user_util.h" #include "xml/xml_base.h" #include "xml/xml_util.h" +#include "tinyxml2.h" namespace { using std::string; @@ -127,8 +126,9 @@ const char* MJCF[nMJCF][mjXATTRNUM] = { {"visual", "*", "0"}, {"<"}, - {"global", "?", "12", "orthographic", "fovy", "ipd", "azimuth", "elevation", "linewidth", - "glow", "offwidth", "offheight", "realtime", "ellipsoidinertia", "bvactive"}, + {"global", "?", "13", "cameraid", "orthographic", "fovy", "ipd", "azimuth", "elevation", + "linewidth", "glow", "offwidth", "offheight", "realtime", "ellipsoidinertia", + "bvactive"}, {"quality", "?", "5", "shadowsize", "offsamples", "numslices", "numstacks", "numquads"}, {"headlight", "?", "4", "ambient", "diffuse", "specular", "active"}, @@ -2210,6 +2210,7 @@ void mjXReader::OneActuator(XMLElement* elem, mjsActuator* actuator) { type = elem->Value(); // explicit attributes + string err; if (type == "general") { // explicit attributes int n; @@ -2233,183 +2234,98 @@ void mjXReader::OneActuator(XMLElement* elem, mjsActuator* actuator) { // direct drive motor else if (type == "motor") { - // unit gain - actuator->gainprm[0] = 1; - - // implied parameters - actuator->dyntype = mjDYN_NONE; - actuator->gaintype = mjGAIN_FIXED; - actuator->biastype = mjBIAS_NONE; + err = mjs_setToMotor(actuator); } // position or integrated velocity servo else if (type == "position" || type == "intvelocity") { - // explicit attributes - ReadAttr(elem, "kp", 1, actuator->gainprm, text); - actuator->biasprm[1] = -actuator->gainprm[0]; + double kp = actuator->gainprm[0]; + ReadAttr(elem, "kp", 1, &kp, text); // read kv - double kv = -1; // -1: undefined - if (ReadAttr(elem, "kv", 1, &kv, text)) { - if (kv < 0) throw mjXError(elem, "kv cannot be negative"); + double kv_data; + double *kv = &kv_data; + if (!ReadAttr(elem, "kv", 1, kv, text)) { + kv = nullptr; } // read dampratio - double dampratio = -1; // -1: undefined - if (ReadAttr(elem, "dampratio", 1, &dampratio, text)) { - if (dampratio < 0) throw mjXError(elem, "dampratio cannot be negative"); + double dampratio_data; + double *dampratio = &dampratio_data; + if (!ReadAttr(elem, "dampratio", 1, dampratio, text)) { + dampratio = nullptr; } - // set biasprm[2]; negative: regular damping, positive: dampratio - if (dampratio > 0 && kv > 0) { - throw mjXError(elem, "kv and dampratio cannot both be defined"); - } - if (kv > 0) actuator->biasprm[2] = -kv; - if (dampratio > 0) actuator->biasprm[2] = dampratio; - // read timeconst, set dyntype - if (ReadAttr(elem, "timeconst", 1, actuator->dynprm, text)) { - if (actuator->dynprm[0] < 0) - throw mjXError(elem, "timeconst cannot be negative"); - actuator->dyntype = actuator->dynprm[0] ? mjDYN_FILTEREXACT : mjDYN_NONE; + double timeconst_data; + double *timeconst = &timeconst_data; + if (!ReadAttr(elem, "timeconst", 1, timeconst, text)) { + timeconst = nullptr; } // handle inheritrange - ReadAttr(elem, "inheritrange", 1, &actuator->inheritrange, text); - if (actuator->inheritrange > 0) { - if (type == "position") { - if (actuator->ctrlrange[0] || actuator->ctrlrange[1]) { - throw mjXError(elem, "ctrlrange and inheritrange cannot both be defined"); - } - } else { - if (actuator->actrange[0] || actuator->actrange[1]) { - throw mjXError(elem, "actrange and inheritrange cannot both be defined"); - } - } - } + double inheritrange = actuator->inheritrange; + ReadAttr(elem, "inheritrange", 1, &inheritrange, text); - // implied parameters - actuator->gaintype = mjGAIN_FIXED; - actuator->biastype = mjBIAS_AFFINE; - - if (type == "intvelocity") { - actuator->dyntype = mjDYN_INTEGRATOR; - actuator->actlimited = 1; + if (type == "position") { + err = mjs_setToPosition(actuator, kp, kv, dampratio, timeconst, inheritrange); + } else { + err = mjs_setToIntVelocity(actuator, kp, kv, dampratio, timeconst, inheritrange); } } // velocity servo else if (type == "velocity") { - // clear bias - mjuu_zerovec(actuator->biasprm, mjNBIAS); - - // explicit attributes - ReadAttr(elem, "kv", 1, actuator->gainprm, text); - actuator->biasprm[2] = -actuator->gainprm[0]; - - // implied parameters - actuator->dyntype = mjDYN_NONE; - actuator->gaintype = mjGAIN_FIXED; - actuator->biastype = mjBIAS_AFFINE; + double kv = actuator->gainprm[0]; + ReadAttr(elem, "kv", 1, &kv, text); + err = mjs_setToVelocity(actuator, kv); } // damper else if (type == "damper") { - // clear gain - mjuu_zerovec(actuator->gainprm, mjNGAIN); - - // explicit attributes - ReadAttr(elem, "kv", 1, actuator->gainprm+2, text); - if (actuator->gainprm[2] < 0) - throw mjXError(elem, "damping coefficient cannot be negative"); - actuator->gainprm[2] = -actuator->gainprm[2]; - - // require nonnegative range - if (actuator->ctrlrange[0] < 0 || actuator->ctrlrange[1] < 0) { - throw mjXError(elem, "damper control range cannot be negative"); - } - - // implied parameters - actuator->ctrllimited = 1; - actuator->dyntype = mjDYN_NONE; - actuator->gaintype = mjGAIN_AFFINE; - actuator->biastype = mjBIAS_NONE; + double kv = 0; + ReadAttr(elem, "kv", 1, &kv, text); + err = mjs_setToDamper(actuator, kv); } // cylinder else if (type == "cylinder") { - // explicit attributes - ReadAttr(elem, "timeconst", 1, actuator->dynprm, text); - ReadAttr(elem, "bias", 3, actuator->biasprm, text); - ReadAttr(elem, "area", 1, actuator->gainprm, text); - double diameter; - if (ReadAttr(elem, "diameter", 1, &diameter, text)) { - actuator->gainprm[0] = mjPI / 4 * diameter*diameter; - } - - // implied parameters - actuator->dyntype = mjDYN_FILTER; - actuator->gaintype = mjGAIN_FIXED; - actuator->biastype = mjBIAS_AFFINE; + double timeconst = actuator->dynprm[0]; + double bias = actuator->biasprm[0]; + double area = actuator->gainprm[0]; + double diameter = -1; + ReadAttr(elem, "timeconst", 1, &timeconst, text); + ReadAttr(elem, "bias", 3, &bias, text); + ReadAttr(elem, "area", 1, &area, text); + ReadAttr(elem, "diameter", 1, &diameter, text); + err = mjs_setToCylinder(actuator, timeconst, bias, area, diameter); } // muscle else if (type == "muscle") { - // set muscle defaults if same as global defaults - if (actuator->dynprm[0] == 1)actuator->dynprm[0] = 0.01; // tau act - if (actuator->dynprm[1] == 0)actuator->dynprm[1] = 0.04; // tau deact - if (actuator->gainprm[0] == 1)actuator->gainprm[0] = 0.75; // range[0] - if (actuator->gainprm[1] == 0)actuator->gainprm[1] = 1.05; // range[1] - if (actuator->gainprm[2] == 0)actuator->gainprm[2] = -1; // force - if (actuator->gainprm[3] == 0)actuator->gainprm[3] = 200; // scale - if (actuator->gainprm[4] == 0)actuator->gainprm[4] = 0.5; // lmin - if (actuator->gainprm[5] == 0)actuator->gainprm[5] = 1.6; // lmax - if (actuator->gainprm[6] == 0)actuator->gainprm[6] = 1.5; // vmax - if (actuator->gainprm[7] == 0)actuator->gainprm[7] = 1.3; // fpmax - if (actuator->gainprm[8] == 0)actuator->gainprm[8] = 1.2; // fvmax - - // explicit attributes - ReadAttr(elem, "timeconst", 2, actuator->dynprm, text); - ReadAttr(elem, "tausmooth", 1, actuator->dynprm+2, text); - if (actuator->dynprm[2] < 0) - throw mjXError(elem, "muscle tausmooth cannot be negative"); - ReadAttr(elem, "range", 2, actuator->gainprm, text); - ReadAttr(elem, "force", 1, actuator->gainprm+2, text); - ReadAttr(elem, "scale", 1, actuator->gainprm+3, text); - ReadAttr(elem, "lmin", 1, actuator->gainprm+4, text); - ReadAttr(elem, "lmax", 1, actuator->gainprm+5, text); - ReadAttr(elem, "vmax", 1, actuator->gainprm+6, text); - ReadAttr(elem, "fpmax", 1, actuator->gainprm+7, text); - ReadAttr(elem, "fvmax", 1, actuator->gainprm+8, text); - - // biasprm = gainprm - for (int n=0; n < 9; n++) { - actuator->biasprm[n] = actuator->gainprm[n]; - } - - // implied parameters - actuator->dyntype = mjDYN_MUSCLE; - actuator->gaintype = mjGAIN_MUSCLE; - actuator->biastype = mjBIAS_MUSCLE; + double tausmooth = actuator->dynprm[2]; + double force = -1, scale = -1, lmin = -1, lmax = -1, vmax = -1, fpmax = -1, fvmax = -1; + double range[2] = {-1, -1}, timeconst[2] = {-1, -1}; + ReadAttr(elem, "timeconst", 2, timeconst, text); + ReadAttr(elem, "tausmooth", 1, &tausmooth, text); + ReadAttr(elem, "range", 2, range, text); + ReadAttr(elem, "force", 1, &force, text); + ReadAttr(elem, "scale", 1, &scale, text); + ReadAttr(elem, "lmin", 1, &lmin, text); + ReadAttr(elem, "lmax", 1, &lmax, text); + ReadAttr(elem, "vmax", 1, &vmax, text); + ReadAttr(elem, "fpmax", 1, &fpmax, text); + ReadAttr(elem, "fvmax", 1, &fvmax, text); + err = mjs_setToMuscle(actuator, timeconst, tausmooth, range, force, scale, + lmin, lmax, vmax, fpmax, fvmax); } // adhesion else if (type == "adhesion") { - // explicit attributes - ReadAttr(elem, "gain", 1, actuator->gainprm, text); - if (actuator->gainprm[0] < 0) - throw mjXError(elem, "adhesion gain cannot be negative"); - - // require nonnegative range + double gain = actuator->gainprm[0]; + ReadAttr(elem, "gain", 1, &gain, text); ReadAttr(elem, "ctrlrange", 2, actuator->ctrlrange, text); - if (actuator->ctrlrange[0] < 0 || actuator->ctrlrange[1] < 0) { - throw mjXError(elem, "adhesion control range cannot be negative"); - } - - // implied parameters - actuator->ctrllimited = 1; - actuator->gaintype = mjGAIN_FIXED; - actuator->biastype = mjBIAS_NONE; + err = mjs_setToAdhesion(actuator, gain); } else if (type == "plugin") { @@ -2429,6 +2345,11 @@ void mjXReader::OneActuator(XMLElement* elem, mjsActuator* actuator) { throw mjXError(elem, "unrecognized actuator type: %s", type.c_str()); } + // throw error if any of the above failed + if (!err.empty()) { + throw mjXError(elem, err.c_str()); + } + // read userdata std::vector userdata; if (ReadVector(elem, "user", userdata, text)) { @@ -3077,6 +2998,7 @@ void mjXReader::Visual(XMLElement* section) { // global sub-element if (name == "global") { + ReadAttrInt(elem, "cameraid", &vis->global.cameraid); if (MapValue(elem, "orthographic", &n, bool_map, 2)) { vis->global.orthographic = (n == 1); } @@ -3633,7 +3555,7 @@ void mjXReader::Body(XMLElement* section, mjsBody* body, mjsFrame* frame, } // delete subtree - if (mjs_detachBody(spec, subtree)) { + if (mjs_delete(spec, subtree->element)) { throw mjXError(elem, mjs_getError(spec)); } } diff --git a/src/xml/xml_native_reader.h b/src/xml/xml_native_reader.h index 010ea2d5..1ad53464 100644 --- a/src/xml/xml_native_reader.h +++ b/src/xml/xml_native_reader.h @@ -18,13 +18,12 @@ #include #include -#include "tinyxml2.h" - #include #include #include "user/user_util.h" #include "xml/xml_base.h" #include "xml/xml_util.h" +#include "tinyxml2.h" class mjXReader : public mjXBase { public: diff --git a/src/xml/xml_native_writer.cc b/src/xml/xml_native_writer.cc index de2aef96..8b70e9a2 100644 --- a/src/xml/xml_native_writer.cc +++ b/src/xml/xml_native_writer.cc @@ -395,10 +395,14 @@ void mjXWriter::OneJoint(XMLElement* elem, const mjCJoint* joint, mjCDef* def, WriteAttr(elem, "solimpfriction", mjNIMP, joint->solimp_friction, def->Joint().solimp_friction, true); WriteAttr(elem, "stiffness", 1, &joint->stiffness, &def->Joint().stiffness); - WriteAttrKey(elem, "limited", TFAuto_map, 3, joint->limited, def->Joint().limited); + if (joint->type != mjJNT_FREE) { + WriteAttrKey(elem, "limited", TFAuto_map, 3, joint->limited, def->Joint().limited); + } WriteAttr(elem, "range", 2, joint->range, def->Joint().range); - WriteAttrKey(elem, "actuatorfrclimited", TFAuto_map, 3, joint->actfrclimited, - def->Joint().actfrclimited); + if (joint->type != mjJNT_FREE && joint->type != mjJNT_BALL) { + WriteAttrKey(elem, "actuatorfrclimited", TFAuto_map, 3, joint->actfrclimited, + def->Joint().actfrclimited); + } WriteAttrKey(elem, "actuatorgravcomp", bool_map, 2, joint->actgravcomp, def->Joint().actgravcomp); WriteAttr(elem, "actuatorfrcrange", 2, joint->actfrcrange, def->Joint().actfrcrange); WriteAttr(elem, "margin", 1, &joint->margin, &def->Joint().margin); @@ -1130,6 +1134,7 @@ void mjXWriter::Visual(XMLElement* root) { // global elem = InsertEnd(section, "global"); + WriteAttrInt(elem, "cameraid", vis->global.cameraid, visdef.global.cameraid); WriteAttrKey(elem, "orthographic", bool_map, 2, vis->global.orthographic, visdef.global.orthographic); WriteAttr(elem, "fovy", 1, &vis->global.fovy, &visdef.global.fovy); @@ -1595,7 +1600,8 @@ XMLElement* mjXWriter::OneFrame(XMLElement* elem, mjCFrame* frame) { return elem; } - if (frame->name.empty() && frame->classname.empty()) { + // TODO: empty classname should not occur (but does) + if (frame->name.empty() && (frame->classname.empty() || frame->classname == "main")) { return elem; } diff --git a/src/xml/xml_urdf.cc b/src/xml/xml_urdf.cc index 3667cf35..2f49d9c0 100644 --- a/src/xml/xml_urdf.cc +++ b/src/xml/xml_urdf.cc @@ -26,7 +26,6 @@ #include "xml/xml_native_reader.h" #include "xml/xml_urdf.h" #include "xml/xml_util.h" - #include "tinyxml2.h" using tinyxml2::XMLElement; @@ -330,7 +329,7 @@ void mjXURDF::Body(XMLElement* body_elem) { if (urGeomNames.find(geom_name) == urGeomNames.end()) { mjs_setString(pgeom->name, geom_name.c_str()); urGeomNames.insert(geom_name); - } else { + } else if (!geom_name.empty()) { std::cerr << "WARNING: Geom with duplicate name '" << geom_name << "' encountered in URDF, creating an unnamed geom." << std::endl; @@ -353,7 +352,7 @@ void mjXURDF::Body(XMLElement* body_elem) { if (urGeomNames.find(geom_name) == urGeomNames.end()) { mjs_setString(pgeom->name, geom_name.c_str()); urGeomNames.insert(geom_name); - } else { + } else if (!geom_name.empty()) { std::cerr << "WARNING: Geom with duplicate name '" << geom_name << "' encountered in URDF, creating an unnamed geom." << std::endl; diff --git a/src/xml/xml_util.cc b/src/xml/xml_util.cc index 5e7f52bc..ad1a3546 100644 --- a/src/xml/xml_util.cc +++ b/src/xml/xml_util.cc @@ -31,8 +31,6 @@ #include #include -#include "tinyxml2.h" - #include #include "cc/array_safety.h" #include "engine/engine_util_errmem.h" @@ -40,6 +38,7 @@ #include "user/user_util.h" #include "xml/xml_util.h" #include "xml/xml_numeric_format.h" +#include "tinyxml2.h" namespace { diff --git a/src/xml/xml_util.h b/src/xml/xml_util.h index 86d61948..37cd843b 100644 --- a/src/xml/xml_util.h +++ b/src/xml/xml_util.h @@ -24,10 +24,9 @@ #include #include -#include "tinyxml2.h" - #include #include "user/user_util.h" +#include "tinyxml2.h" // error string copy void mjCopyError(char* dst, const char* src, int maxlen); diff --git a/test/engine/engine_collision_sdf_test.cc b/test/engine/engine_collision_sdf_test.cc index e20d2fbe..5d38c627 100644 --- a/test/engine/engine_collision_sdf_test.cc +++ b/test/engine/engine_collision_sdf_test.cc @@ -60,7 +60,7 @@ TEST_F(SdfTest, SdfPrimitive) { {-1, 0, 0, mju_sqrt(2)-1, mju_sqrt(2)-1, mju_sqrt(3)-1}, // sphere {-.1, .9, .9, mju_sqrt(2)-.1, .9, mju_sqrt(2)-.1}, // capsule {-1, 0, 0, mju_sqrt(2)-1, 0, mju_sqrt(2)-1}, // cylinder - {-1, 0, 0, 0, 0, 0}, // box + {-mju_sqrt(3), 0, 0, 0, 0, 0}, // box }; mjtNum points[kpoints][3] = {{0, 0, 0}, {1, 0, 0}, {0, 1, 0}, {1, 1, 0}, {0, 1, 1}, {1, 1, 1}}; @@ -71,7 +71,7 @@ TEST_F(SdfTest, SdfPrimitive) { sdf.type = mjSDFTYPE_SINGLE; sdf.geomtype = (mjtGeom*)(model->geom_type+i); for (int j = 0; j < kpoints; j++) { - ASSERT_THAT(mjc_distance(model, data, &sdf, points[j]), dist[i][j]); + EXPECT_NEAR(mjc_distance(model, data, &sdf, points[j]), dist[i][j], 1e-9); mjc_gradient(model, data, &sdf, gradient, points[j]); } } diff --git a/test/engine/engine_core_smooth_test.cc b/test/engine/engine_core_smooth_test.cc index 880f3e7e..87897f7c 100644 --- a/test/engine/engine_core_smooth_test.cc +++ b/test/engine/engine_core_smooth_test.cc @@ -18,7 +18,6 @@ #include "src/engine/engine_util_misc.h" #include "src/engine/engine_util_sparse.h" -#include #include #include #include @@ -237,6 +236,7 @@ TEST_F(CoreSmoothTest, TendonArmature) { // put only CRB inertia in M2 mj_crb(m, d); + mju_scatter(d->qM, d->M, d->mapM2M, m->nC); vector M2(nv*nv); mj_fullM(m, M2.data(), d->qM); diff --git a/test/engine/engine_island_test.cc b/test/engine/engine_island_test.cc index e59ac3b4..80437e7c 100644 --- a/test/engine/engine_island_test.cc +++ b/test/engine/engine_island_test.cc @@ -28,12 +28,11 @@ namespace mujoco { namespace { +using ::testing::DoubleNear; using ::testing::ElementsAre; +using ::testing::Pointwise; using IslandTest = MujocoTest; -std::vector AsVector(const int* array, int n) { - return std::vector(array, array + n); -} TEST_F(IslandTest, FloodFillSingleton) { // adjacency matrix for the graph 0 1 2 @@ -368,6 +367,30 @@ TEST_F(IslandTest, IslandEfc) { mj_deleteModel(model); } +TEST_F(IslandTest, IslandFlex) { + const std::string xml_path = GetTestDataFilePath("testdata/flex.xml"); + mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0); + mjData* data1 = mj_makeData(model); + mjData* data2 = mj_makeData(model); + + model->opt.enableflags |= mjENBL_ISLAND; + while (data1->time < 0.2) { + mj_step(model, data1); + } + + model->opt.enableflags &= ~mjENBL_ISLAND; + while (data2->time < 0.2) { + mj_step(model, data2); + } + + EXPECT_THAT(AsVector(data1->qpos, model->nq), + Pointwise(DoubleNear(1e-6), AsVector(data2->qpos, model->nq))); + + mj_deleteData(data2); + mj_deleteData(data1); + mj_deleteModel(model); +} + static const char* const k2H100Path = "engine/testdata/island/2humanoid100.xml"; TEST_F(IslandTest, IslandJacobian) { diff --git a/test/engine/engine_solver_test.cc b/test/engine/engine_solver_test.cc index 75b42cf7..66e9c3fd 100644 --- a/test/engine/engine_solver_test.cc +++ b/test/engine/engine_solver_test.cc @@ -123,7 +123,7 @@ TEST_F(SolverTest, IslandsEquivalentForward) { model->opt.tolerance = 0; // set tolerance to 0 model->opt.ls_tolerance = 0; // set ls_tolerance to 0 - mjtNum rtol = 2e-6; + mjtNum rtol = 3e-6; mjData* data_island = mj_makeData(model); mjData* data_noisland = mj_makeData(model); diff --git a/test/experimental/CMakeLists.txt b/test/experimental/CMakeLists.txt index 9f5100db..92b73f58 100644 --- a/test/experimental/CMakeLists.txt +++ b/test/experimental/CMakeLists.txt @@ -12,7 +12,22 @@ # See the License for the specific language governing permissions and # limitations under the License. -if(MUJOCO_BUILD_USD_PLUGINS AND MUJOCO_USD_TARGET STREQUAL "USD") +# Only support testing when building against a standalone version of USD. +if(MUJOCO_WITH_USD AND USD_DIR) + find_package(pxr REQUIRED) + + add_library(usd_test_utils STATIC usd/test_utils.h usd/test_utils.cc) + target_include_directories(usd_test_utils PUBLIC ${MUJOCO_TEST_INCLUDE}) + target_compile_definitions(usd_test_utils PUBLIC MJSTATIC) + target_link_libraries( + usd_test_utils + PUBLIC usd + tf + gtest + gmock + mujoco + ) + add_subdirectory(usd/plugins/mjcf) endif() diff --git a/test/experimental/usd/mjcPhysics/mjc_physics_scene_test.cc b/test/experimental/usd/mjcPhysics/mjc_physics_scene_test.cc index 534dcffe..5748f5bb 100644 --- a/test/experimental/usd/mjcPhysics/mjc_physics_scene_test.cc +++ b/test/experimental/usd/mjcPhysics/mjc_physics_scene_test.cc @@ -14,8 +14,8 @@ #include #include +#include #include -#include "src/experimental/usd/mjcPhysics/sceneAPI.h" #include "test/fixture.h" #include #include diff --git a/test/experimental/usd/mjcPhysics/mjc_site_api_test.cc b/test/experimental/usd/mjcPhysics/mjc_site_api_test.cc index a193dbb1..360989f9 100644 --- a/test/experimental/usd/mjcPhysics/mjc_site_api_test.cc +++ b/test/experimental/usd/mjcPhysics/mjc_site_api_test.cc @@ -14,9 +14,9 @@ #include #include -#include +#include +#include #include -#include "src/experimental/usd/mjcPhysics/siteAPI.h" #include "test/fixture.h" #include #include diff --git a/test/experimental/usd/plugins/mjcf/CMakeLists.txt b/test/experimental/usd/plugins/mjcf/CMakeLists.txt index 1b747aee..d4c35f02 100644 --- a/test/experimental/usd/plugins/mjcf/CMakeLists.txt +++ b/test/experimental/usd/plugins/mjcf/CMakeLists.txt @@ -12,19 +12,13 @@ # See the License for the specific language governing permissions and # limitations under the License. -find_package(pxr REQUIRED) - -add_library(usd_fixture STATIC fixture.h fixture.cc) -target_include_directories(usd_fixture PUBLIC ${MUJOCO_TEST_INCLUDE}) -target_compile_definitions(usd_fixture PUBLIC MJSTATIC) - -target_link_libraries( - usd_fixture - PUBLIC usd - tf - gtest - gmock - mujoco -) - -mujoco_test(mjcf_file_format_test ADDITIONAL_LINK_LIBRARIES usd tf usdGeom usdImaging usdPhysics usdShade usd_fixture) +mujoco_test(mjcf_file_format_test ADDITIONAL_LINK_LIBRARIES + usd + tf + usdGeom + usdImaging + usdPhysics + usdShade + usd_test_utils + mjcPhysics + ) diff --git a/test/experimental/usd/plugins/mjcf/mjcf_file_format_test.cc b/test/experimental/usd/plugins/mjcf/mjcf_file_format_test.cc index b8f57bca..98fafdf9 100644 --- a/test/experimental/usd/plugins/mjcf/mjcf_file_format_test.cc +++ b/test/experimental/usd/plugins/mjcf/mjcf_file_format_test.cc @@ -17,14 +17,17 @@ #include #include -#include "src/experimental/usd/mjcPhysics/actuatorAPI.h" -#include "src/experimental/usd/mjcPhysics/collisionAPI.h" -#include "src/experimental/usd/mjcPhysics/meshCollisionAPI.h" -#include "src/experimental/usd/mjcPhysics/sceneAPI.h" -#include "src/experimental/usd/mjcPhysics/siteAPI.h" -#include "src/experimental/usd/mjcPhysics/tokens.h" +#include +#include +#include +#include +#include +#include +#include #include "test/experimental/usd/test_utils.h" #include "test/fixture.h" +#include +#include #include #include #include @@ -56,8 +59,12 @@ #include #include #include +#include +#include #include #include +#include +#include #include #include #include @@ -143,20 +150,122 @@ TEST_F(MjcfSdfFileFormatPluginTest, TestMaterials) { stage, "/mesh_test/Materials/material_red/PreviewSurface.inputs:diffuseColor", pxr::GfVec3f(0.8, 0, 0)); + ExpectAttributeHasConnection( + stage, "/mesh_test/Materials/material_red.outputs:surface", + "/mesh_test/Materials/material_red/PreviewSurface.outputs:surface"); + ExpectAttributeHasConnection( + stage, "/mesh_test/Materials/material_red.outputs:displacement", + "/mesh_test/Materials/material_red/PreviewSurface.outputs:displacement"); EXPECT_PRIM_VALID(stage, "/mesh_test/Materials/material_texture"); EXPECT_PRIM_VALID(stage, "/mesh_test/Materials/material_texture/PreviewSurface"); EXPECT_PRIM_VALID(stage, "/mesh_test/Materials/material_texture/uvmap"); - EXPECT_PRIM_VALID(stage, "/mesh_test/Materials/material_texture/texture"); + EXPECT_PRIM_VALID(stage, "/mesh_test/Materials/material_texture/diffuse"); ExpectAttributeHasConnection( stage, "/mesh_test/Materials/material_texture/" "PreviewSurface.inputs:diffuseColor", - "/mesh_test/Materials/material_texture/texture.outputs:rgb"); + "/mesh_test/Materials/material_texture/diffuse.outputs:rgb"); ExpectAttributeEqual( - stage, "/mesh_test/Materials/material_texture/texture.inputs:file", + stage, "/mesh_test/Materials/material_texture/diffuse.inputs:file", pxr::SdfAssetPath("textures/cube.png")); + + EXPECT_PRIM_VALID(stage, "/mesh_test/Materials/material_metallic"); + EXPECT_PRIM_VALID(stage, + "/mesh_test/Materials/material_metallic/PreviewSurface"); + ExpectAttributeEqual( + stage, + "/mesh_test/Materials/material_metallic/PreviewSurface.inputs:metallic", + 0.6f); +} + +TEST_F(MjcfSdfFileFormatPluginTest, TestMaterialLayers) { + const std::string xml_path = GetTestDataFilePath(kMaterialsPath); + auto stage = pxr::UsdStage::Open(xml_path); + EXPECT_THAT(stage, testing::NotNull()); + + EXPECT_PRIM_VALID(stage, "/mesh_test/Materials/material_layered"); + EXPECT_PRIM_VALID(stage, "/mesh_test/Materials/material_layered/uvmap"); + EXPECT_PRIM_VALID(stage, "/mesh_test/Materials/material_layered/diffuse"); + + EXPECT_PRIM_VALID(stage, "/mesh_test/Materials/material_layered/normal"); + ExpectAttributeHasConnection( + stage, + "/mesh_test/Materials/material_layered/" + "PreviewSurface.inputs:normal", + "/mesh_test/Materials/material_layered/normal.outputs:rgb"); + ExpectAttributeEqual( + stage, "/mesh_test/Materials/material_layered/normal.inputs:file", + pxr::SdfAssetPath("textures/normal.png")); + + EXPECT_PRIM_VALID(stage, "/mesh_test/Materials/material_layered/orm_packed"); + ExpectAttributeHasConnection( + stage, + "/mesh_test/Materials/material_layered/" + "PreviewSurface.inputs:occlusion", + "/mesh_test/Materials/material_layered/orm_packed.outputs:r"); + ExpectAttributeHasConnection( + stage, + "/mesh_test/Materials/material_layered/" + "PreviewSurface.inputs:roughness", + "/mesh_test/Materials/material_layered/orm_packed.outputs:g"); + ExpectAttributeHasConnection( + stage, + "/mesh_test/Materials/material_layered/" + "PreviewSurface.inputs:metallic", + "/mesh_test/Materials/material_layered/orm_packed.outputs:b"); + ExpectAttributeEqual( + stage, "/mesh_test/Materials/material_layered/orm_packed.inputs:file", + pxr::SdfAssetPath("textures/orm.png")); + + EXPECT_PRIM_VALID(stage, "/mesh_test/Materials/material_layered/emissive"); + ExpectAttributeHasConnection( + stage, + "/mesh_test/Materials/material_layered/" + "PreviewSurface.inputs:emissiveColor", + "/mesh_test/Materials/material_layered/emissive.outputs:rgb"); + ExpectAttributeEqual( + stage, "/mesh_test/Materials/material_layered/emissive.inputs:file", + pxr::SdfAssetPath("textures/emissive.png")); +} + +TEST_F(MjcfSdfFileFormatPluginTest, TestMaterialPBRSeparate) { + const std::string xml_path = GetTestDataFilePath(kMaterialsPath); + auto stage = pxr::UsdStage::Open(xml_path); + + EXPECT_PRIM_VALID(stage, "/mesh_test/Materials/material_pbr_separate"); + EXPECT_PRIM_VALID(stage, "/mesh_test/Materials/material_pbr_separate/uvmap"); + EXPECT_PRIM_VALID(stage, + "/mesh_test/Materials/material_pbr_separate/occlusion"); + ExpectAttributeHasConnection( + stage, + "/mesh_test/Materials/material_pbr_separate/" + "PreviewSurface.inputs:occlusion", + "/mesh_test/Materials/material_pbr_separate/occlusion.outputs:rgb"); + ExpectAttributeEqual( + stage, "/mesh_test/Materials/material_pbr_separate/occlusion.inputs:file", + pxr::SdfAssetPath("textures/occlusion.png")); + EXPECT_PRIM_VALID(stage, + "/mesh_test/Materials/material_pbr_separate/roughness"); + ExpectAttributeHasConnection( + stage, + "/mesh_test/Materials/material_pbr_separate/" + "PreviewSurface.inputs:roughness", + "/mesh_test/Materials/material_pbr_separate/roughness.outputs:rgb"); + ExpectAttributeEqual( + stage, "/mesh_test/Materials/material_pbr_separate/roughness.inputs:file", + pxr::SdfAssetPath("textures/roughness.png")); + EXPECT_PRIM_VALID(stage, + "/mesh_test/Materials/material_pbr_separate/metallic"); + ExpectAttributeHasConnection( + stage, + "/mesh_test/Materials/material_pbr_separate/" + "PreviewSurface.inputs:metallic", + "/mesh_test/Materials/material_pbr_separate/metallic.outputs:rgb"); + ExpectAttributeEqual( + stage, "/mesh_test/Materials/material_pbr_separate/metallic.inputs:file", + pxr::SdfAssetPath("textures/metallic.png")); } TEST_F(MjcfSdfFileFormatPluginTest, TestGeomRgba) { @@ -1031,6 +1140,9 @@ TEST_F(MjcfSdfFileFormatPluginTest, TestPhysicsRigidBody) { + + + )"; @@ -1047,12 +1159,22 @@ TEST_F(MjcfSdfFileFormatPluginTest, TestPhysicsRigidBody) { EXPECT_PRIM_API_APPLIED(stage, "/physics_test/test_body", pxr::UsdPhysicsRigidBodyAPI); - EXPECT_PRIM_API_APPLIED(stage, "/physics_test/test_body", - pxr::UsdPhysicsArticulationRootAPI); EXPECT_PRIM_API_APPLIED(stage, "/physics_test/test_body/test_body_2", pxr::UsdPhysicsRigidBodyAPI); + EXPECT_PRIM_API_APPLIED(stage, "/physics_test/test_body_3", + pxr::UsdPhysicsRigidBodyAPI); - // Only the root body should have the articulation API applied. + // Articulation root is applied to the children of the world body. + EXPECT_PRIM_API_APPLIED(stage, "/physics_test/test_body", + pxr::UsdPhysicsArticulationRootAPI); + // test_body_3 is a child of the world but has no children so should not be + // an articulation root. + EXPECT_PRIM_API_NOT_APPLIED(stage, "/physics_test/test_body_3", + pxr::UsdPhysicsArticulationRootAPI); + + // Articulation root is not applied to other bodies or world body. + EXPECT_PRIM_API_NOT_APPLIED(stage, "/physics_test", + pxr::UsdPhysicsArticulationRootAPI); EXPECT_PRIM_API_NOT_APPLIED(stage, "/physics_test/test_body/test_body_2", pxr::UsdPhysicsArticulationRootAPI); @@ -1393,6 +1515,31 @@ TEST_F(MjcfSdfFileFormatPluginTest, TestMjcPhysicsActuatorGeneral) { pxr::VtDoubleArray{{0, 1, 2, 3, 4, 5, 6, 7, 8, 9}}); } +TEST_F(MjcfSdfFileFormatPluginTest, TestMjcPhysicsJointActuator) { + static constexpr char xml[] = R"( + + + + + + + + + + + + + + )"; + auto stage = OpenStageWithPhysics(xml); + + EXPECT_PRIM_API_APPLIED(stage, "/test/axle/rod/rod_hinge", + pxr::MjcPhysicsActuatorAPI); +} + TEST_F(MjcfSdfFileFormatPluginTest, TestMjcPhysicsBodyActuator) { static constexpr char xml[] = R"( @@ -1443,6 +1590,409 @@ TEST_F(MjcfSdfFileFormatPluginTest, TestMjcPhysicsSliderCrankActuator) { ExpectAttributeEqual(stage, "/test/body/crank.mjc:crankLength", 1.23); } +TEST_F(MjcfSdfFileFormatPluginTest, TestMjcPhysicsJointAPI) { + static constexpr char xml[] = R"( + + + + + + + + + + + )"; + auto stage = OpenStageWithPhysics(xml); + + const SdfPath joint_path("/test/parent/child/my_joint"); + EXPECT_PRIM_API_APPLIED(stage, joint_path, pxr::MjcPhysicsJointAPI); + + ExpectAttributeEqual(stage, "/test/parent/child/my_joint.mjc:springdamper", + pxr::VtArray({1, 2})); + ExpectAttributeEqual(stage, "/test/parent/child/my_joint.mjc:solreflimit", + pxr::VtArray({0.1, 0.2})); + ExpectAttributeEqual(stage, "/test/parent/child/my_joint.mjc:solimplimit", + pxr::VtArray({0.3, 0.4, 0.5, 0.6, 0.7})); + ExpectAttributeEqual(stage, "/test/parent/child/my_joint.mjc:solreffriction", + pxr::VtArray({0.8, 0.9})); + ExpectAttributeEqual(stage, "/test/parent/child/my_joint.mjc:solimpfriction", + pxr::VtArray({1.0, 1.1, 1.2, 1.3, 1.4})); + ExpectAttributeEqual(stage, "/test/parent/child/my_joint.mjc:stiffness", 1.5); + ExpectAttributeEqual( + stage, "/test/parent/child/my_joint.mjc:actuatorfrcrange:min", -1.6); + ExpectAttributeEqual( + stage, "/test/parent/child/my_joint.mjc:actuatorfrcrange:max", 1.7); + ExpectAttributeEqual(stage, + "/test/parent/child/my_joint.mjc:actuatorfrclimited", + MjcPhysicsTokens->true_); + ExpectAttributeEqual( + stage, "/test/parent/child/my_joint.mjc:actuatorgravcomp", true); + ExpectAttributeEqual(stage, "/test/parent/child/my_joint.mjc:margin", 1.8); + ExpectAttributeEqual(stage, "/test/parent/child/my_joint.mjc:ref", 1.9); + ExpectAttributeEqual(stage, "/test/parent/child/my_joint.mjc:springref", 2.0); + ExpectAttributeEqual(stage, "/test/parent/child/my_joint.mjc:armature", 2.1); + ExpectAttributeEqual(stage, "/test/parent/child/my_joint.mjc:damping", 2.2); + ExpectAttributeEqual(stage, "/test/parent/child/my_joint.mjc:frictionloss", + 2.3); +} + +TEST_F(MjcfSdfFileFormatPluginTest, TestPhysicsFloatingAndFixedBaseBody) { + static constexpr char kXml[] = R"( + + + + + + + + + + + + )"; + + auto stage = OpenStageWithPhysics(kXml); + EXPECT_THAT(stage, testing::NotNull()); + + // Test that the fixed_base body has a UsdPhysicsJoint child connected to the + // worldbody. + EXPECT_PRIM_VALID(stage, "/test/fixed_base/FixedJoint"); + auto joint = pxr::UsdPhysicsFixedJoint::Get( + stage, SdfPath("/test/fixed_base/FixedJoint")); + ASSERT_TRUE(joint); + + // Initial joint to the worldbody does't set a body0 rel. + EXPECT_REL_TARGET_COUNT(stage, "/test/fixed_base/FixedJoint.physics:body0", + 0); + EXPECT_REL_HAS_TARGET(stage, "/test/fixed_base/FixedJoint.physics:body1", + "/test/fixed_base"); + + // Test that the floating_base body has no UsdPhysicsJoint children. + auto floating_base = stage->GetPrimAtPath(SdfPath("/test/floating_base")); + ASSERT_TRUE(floating_base); + for (const auto& child : floating_base.GetChildren()) { + EXPECT_FALSE(child.IsA()); + } +} + +TEST_F(MjcfSdfFileFormatPluginTest, TestPhysicsFixedJoint) { + static constexpr char kXml[] = R"( + + + + + + + + + + + + + + )"; + + auto stage = OpenStageWithPhysics(kXml); + EXPECT_THAT(stage, testing::NotNull()); + + EXPECT_PRIM_IS_A(stage, "/test/parent/FixedJoint", pxr::UsdPhysicsFixedJoint); + // Initial joint to the worldbody does't set a body0 rel. + EXPECT_REL_TARGET_COUNT(stage, "/test/parent/FixedJoint.physics:body0", 0); + EXPECT_REL_HAS_TARGET(stage, "/test/parent/FixedJoint.physics:body1", + "/test/parent"); + + EXPECT_PRIM_IS_A(stage, "/test/parent/child/FixedJoint", + pxr::UsdPhysicsFixedJoint); + EXPECT_REL_HAS_TARGET(stage, "/test/parent/child/FixedJoint.physics:body0", + "/test/parent"); + EXPECT_REL_HAS_TARGET(stage, "/test/parent/child/FixedJoint.physics:body1", + "/test/parent/child"); + + EXPECT_PRIM_IS_A(stage, "/test/parent/child/grandchild/FixedJoint", + pxr::UsdPhysicsFixedJoint); + EXPECT_REL_HAS_TARGET( + stage, "/test/parent/child/grandchild/FixedJoint.physics:body0", + "/test/parent/child"); + EXPECT_REL_HAS_TARGET( + stage, "/test/parent/child/grandchild/FixedJoint.physics:body1", + "/test/parent/child/grandchild"); +} + +TEST_F(MjcfSdfFileFormatPluginTest, TestPhysicsRevoluteJoint) { + static constexpr char kXml[] = R"( + + + + + + + + + + + + + + + + + )"; + + auto stage = OpenStageWithPhysics(kXml); + EXPECT_THAT(stage, testing::NotNull()); + + // hinge_root doesn't set a type so it's the default: a revolute joint. + EXPECT_PRIM_IS_A(stage, "/test/parent/hinge_root", + pxr::UsdPhysicsRevoluteJoint); + // Initial joint to the worldbody does't set a body0 rel. + EXPECT_REL_TARGET_COUNT(stage, "/test/parent/hinge_root.physics:body0", 0); + EXPECT_REL_HAS_TARGET(stage, "/test/parent/hinge_root.physics:body1", + "/test/parent"); + + EXPECT_PRIM_IS_A(stage, "/test/parent/child0/hinge_normal", + pxr::UsdPhysicsRevoluteJoint); + EXPECT_REL_HAS_TARGET(stage, "/test/parent/child0/hinge_normal.physics:body0", + "/test/parent"); + EXPECT_REL_HAS_TARGET(stage, "/test/parent/child0/hinge_normal.physics:body1", + "/test/parent/child0"); + ExpectAttributeEqual(stage, "/test/parent/child0/hinge_normal.physics:axis", + pxr::UsdPhysicsTokens->z); + EXPECT_ATTRIBUTE_HAS_NO_AUTHORED_VALUE( + stage, "/test/parent/child0/hinge_normal.physics:lowerLimit"); + EXPECT_ATTRIBUTE_HAS_NO_AUTHORED_VALUE( + stage, "/test/parent/child0/hinge_normal.physics:upperLimit"); + + EXPECT_PRIM_IS_A(stage, "/test/parent/child1/hinge_limited", + pxr::UsdPhysicsRevoluteJoint); + EXPECT_REL_HAS_TARGET( + stage, "/test/parent/child1/hinge_limited.physics:body0", "/test/parent"); + EXPECT_REL_HAS_TARGET(stage, + "/test/parent/child1/hinge_limited.physics:body1", + "/test/parent/child1"); + ExpectAttributeEqual(stage, "/test/parent/child1/hinge_limited.physics:axis", + pxr::UsdPhysicsTokens->z); + ExpectAttributeEqual( + stage, "/test/parent/child1/hinge_limited.physics:lowerLimit", -30.0f); + ExpectAttributeEqual( + stage, "/test/parent/child1/hinge_limited.physics:upperLimit", 45.0f); +} + +TEST_F(MjcfSdfFileFormatPluginTest, TestPhysicsPrismaticJoint) { + static constexpr char kXml[] = R"( + + + + + + + + + + + + + + + + + )"; + + auto stage = OpenStageWithPhysics(kXml); + EXPECT_THAT(stage, testing::NotNull()); + + EXPECT_PRIM_IS_A(stage, "/test/parent/slide_root", + pxr::UsdPhysicsPrismaticJoint); + // Initial joint to the worldbody does't set a body0 rel. + EXPECT_REL_TARGET_COUNT(stage, "/test/parent/slide_root.physics:body0", 0); + EXPECT_REL_HAS_TARGET(stage, "/test/parent/slide_root.physics:body1", + "/test/parent"); + + EXPECT_PRIM_IS_A(stage, "/test/parent/child0/slide_normal", + pxr::UsdPhysicsPrismaticJoint); + EXPECT_REL_HAS_TARGET(stage, "/test/parent/child0/slide_normal.physics:body0", + "/test/parent"); + EXPECT_REL_HAS_TARGET(stage, "/test/parent/child0/slide_normal.physics:body1", + "/test/parent/child0"); + ExpectAttributeEqual(stage, "/test/parent/child0/slide_normal.physics:axis", + pxr::UsdPhysicsTokens->z); + EXPECT_ATTRIBUTE_HAS_NO_AUTHORED_VALUE( + stage, "/test/parent/child0/slide_normal.physics:lowerLimit"); + EXPECT_ATTRIBUTE_HAS_NO_AUTHORED_VALUE( + stage, "/test/parent/child0/slide_normal.physics:upperLimit"); + + EXPECT_PRIM_IS_A(stage, "/test/parent/child1/slide_limited", + pxr::UsdPhysicsPrismaticJoint); + EXPECT_REL_HAS_TARGET( + stage, "/test/parent/child1/slide_limited.physics:body0", "/test/parent"); + EXPECT_REL_HAS_TARGET(stage, + "/test/parent/child1/slide_limited.physics:body1", + "/test/parent/child1"); + ExpectAttributeEqual(stage, "/test/parent/child1/slide_limited.physics:axis", + pxr::UsdPhysicsTokens->z); + ExpectAttributeEqual( + stage, "/test/parent/child1/slide_limited.physics:lowerLimit", -2.5f); + ExpectAttributeEqual( + stage, "/test/parent/child1/slide_limited.physics:upperLimit", 2.5f); +} + +TEST_F(MjcfSdfFileFormatPluginTest, TestRadianAnglesAreConvertedToDegrees) { + static constexpr char kXml[] = R"( + + + + + + + + + + + )"; + + auto stage = OpenStageWithPhysics(kXml); + EXPECT_THAT(stage, testing::NotNull()); + + EXPECT_PRIM_VALID(stage, "/test/parent/hinge"); + ExpectAttributeEqual(stage, "/test/parent/hinge.physics:lowerLimit", -180.0f); + ExpectAttributeEqual(stage, "/test/parent/hinge.physics:upperLimit", 45.0f); +} + +TEST_F(MjcfSdfFileFormatPluginTest, TestPhysicsJointFrames) { + static constexpr char kXml[] = R"( + + + + + + + + + + + + + + + + + + + )"; + + auto stage = OpenStageWithPhysics(kXml); + EXPECT_THAT(stage, testing::NotNull()); + + // Test the hinge joint. + EXPECT_PRIM_VALID(stage, "/test/parent/child0/hinge"); + auto hinge_joint = pxr::UsdPhysicsRevoluteJoint::Get( + stage, SdfPath("/test/parent/child0/hinge")); + ASSERT_TRUE(hinge_joint); + + ExpectAttributeEqual(stage, "/test/parent/child0/hinge.physics:localPos0", + pxr::GfVec3f(1.1, 0.2, 0.3)); + + pxr::GfRotation hinge_rot; + hinge_rot.SetRotateInto({0, 0, 1}, {0, 1, 0}); + pxr::GfQuatf expected_hinge_rot(hinge_rot.GetQuat()); + + pxr::GfQuatf hinge_local_rot0; + hinge_joint.GetLocalRot0Attr().Get(&hinge_local_rot0); + EXPECT_TRUE(AreQuatsSameRotation(expected_hinge_rot, hinge_local_rot0)); + + ExpectAttributeEqual(stage, "/test/parent/child0/hinge.physics:localPos1", + pxr::GfVec3f(0.1, 0.2, 0.3)); + + pxr::GfQuatf hinge_local_rot1; + hinge_joint.GetLocalRot1Attr().Get(&hinge_local_rot1); + EXPECT_TRUE(AreQuatsSameRotation(expected_hinge_rot, hinge_local_rot1)); + + // Test the slide joint. + EXPECT_PRIM_VALID(stage, "/test/parent/child1/slide"); + auto slide_joint = pxr::UsdPhysicsPrismaticJoint::Get( + stage, SdfPath("/test/parent/child1/slide")); + ASSERT_TRUE(slide_joint); + + ExpectAttributeEqual(stage, "/test/parent/child1/slide.physics:localPos0", + pxr::GfVec3f(2.4, 3.5, 4.6)); + + pxr::GfRotation slide_rot; + slide_rot.SetRotateInto({0, 0, 1}, {-1, 0, 0}); + pxr::GfQuatf expected_slide_rot(slide_rot.GetQuat()); + + pxr::GfQuatf slide_local_rot0; + slide_joint.GetLocalRot0Attr().Get(&slide_local_rot0); + EXPECT_TRUE(AreQuatsSameRotation(expected_slide_rot, slide_local_rot0)); + + ExpectAttributeEqual(stage, "/test/parent/child1/slide.physics:localPos1", + pxr::GfVec3f(0.4, 0.5, 0.6)); + + pxr::GfQuatf slide_local_rot1; + slide_joint.GetLocalRot1Attr().Get(&slide_local_rot1); + EXPECT_TRUE(AreQuatsSameRotation(expected_slide_rot, slide_local_rot1)); + + // Test the slide_nonaxis joint. + EXPECT_PRIM_VALID(stage, "/test/parent/child2/slide_nonaxis"); + auto slide_nonaxis_joint = pxr::UsdPhysicsPrismaticJoint::Get( + stage, SdfPath("/test/parent/child2/slide_nonaxis")); + ASSERT_TRUE(slide_nonaxis_joint); + + ExpectAttributeEqual(stage, + "/test/parent/child2/slide_nonaxis.physics:localPos0", + pxr::GfVec3f(5.7, 6.8, 7.9)); + + pxr::GfRotation slide_nonaxis_rot; + slide_nonaxis_rot.SetRotateInto({0, 0, 1}, {1, 1, 1}); + pxr::GfQuatf expected_slide_nonaxis_rot(slide_nonaxis_rot.GetQuat()); + + pxr::GfQuatf slide_nonaxis_local_rot0; + slide_nonaxis_joint.GetLocalRot0Attr().Get(&slide_nonaxis_local_rot0); + EXPECT_TRUE(AreQuatsSameRotation(expected_slide_nonaxis_rot, + slide_nonaxis_local_rot0)); + + ExpectAttributeEqual(stage, + "/test/parent/child2/slide_nonaxis.physics:localPos1", + pxr::GfVec3f(0.7, 0.8, 0.9)); + + pxr::GfQuatf slide_nonaxis_local_rot1; + slide_nonaxis_joint.GetLocalRot1Attr().Get(&slide_nonaxis_local_rot1); + EXPECT_TRUE(AreQuatsSameRotation(expected_slide_nonaxis_rot, + slide_nonaxis_local_rot1)); +} + +TEST_F(MjcfSdfFileFormatPluginTest, TestPhysicsUnsupportedJoint) { + static constexpr char kXml[] = R"( + + + + + + + + + )"; + + auto stage = OpenStageWithPhysics(kXml); + EXPECT_THAT(stage, testing::NotNull()); + + EXPECT_PRIM_INVALID(stage, "/test/parent/ball_joint"); +} } // namespace } // namespace usd } // namespace mujoco diff --git a/test/experimental/usd/plugins/mjcf/testdata/materials.xml b/test/experimental/usd/plugins/mjcf/testdata/materials.xml index 735a789d..3ca1ec8a 100644 --- a/test/experimental/usd/plugins/mjcf/testdata/materials.xml +++ b/test/experimental/usd/plugins/mjcf/testdata/materials.xml @@ -1,8 +1,27 @@ - + + + + + + + - + + + + + + + + + + + + + + 2d texture="diffuse" normal="normal" /> diff --git a/test/experimental/usd/plugins/mjcf/testdata/textures/emissive.png b/test/experimental/usd/plugins/mjcf/testdata/textures/emissive.png new file mode 100644 index 00000000..8c244eca Binary files /dev/null and b/test/experimental/usd/plugins/mjcf/testdata/textures/emissive.png differ diff --git a/test/experimental/usd/plugins/mjcf/testdata/textures/metallic.png b/test/experimental/usd/plugins/mjcf/testdata/textures/metallic.png new file mode 100644 index 00000000..a14f5f63 Binary files /dev/null and b/test/experimental/usd/plugins/mjcf/testdata/textures/metallic.png differ diff --git a/test/experimental/usd/plugins/mjcf/testdata/textures/normal.png b/test/experimental/usd/plugins/mjcf/testdata/textures/normal.png new file mode 100644 index 00000000..f5bdc79a Binary files /dev/null and b/test/experimental/usd/plugins/mjcf/testdata/textures/normal.png differ diff --git a/test/experimental/usd/plugins/mjcf/testdata/textures/occlusion.png b/test/experimental/usd/plugins/mjcf/testdata/textures/occlusion.png new file mode 100644 index 00000000..a14f5f63 Binary files /dev/null and b/test/experimental/usd/plugins/mjcf/testdata/textures/occlusion.png differ diff --git a/test/experimental/usd/plugins/mjcf/testdata/textures/orm.png b/test/experimental/usd/plugins/mjcf/testdata/textures/orm.png new file mode 100644 index 00000000..a14f5f63 Binary files /dev/null and b/test/experimental/usd/plugins/mjcf/testdata/textures/orm.png differ diff --git a/test/experimental/usd/plugins/mjcf/testdata/textures/roughness.png b/test/experimental/usd/plugins/mjcf/testdata/textures/roughness.png new file mode 100644 index 00000000..a14f5f63 Binary files /dev/null and b/test/experimental/usd/plugins/mjcf/testdata/textures/roughness.png differ diff --git a/test/experimental/usd/test_utils.cc b/test/experimental/usd/test_utils.cc index aedb13b7..eb008a68 100644 --- a/test/experimental/usd/test_utils.cc +++ b/test/experimental/usd/test_utils.cc @@ -18,6 +18,8 @@ #include #include +#include +#include #include #include #include @@ -97,7 +99,8 @@ void ExpectAllAuthoredAttributesMatchSchemaTypes(const pxr::UsdPrim& prim) { for (const pxr::SdfPropertySpecHandle& spec : propStack) { // We only care about attribute specs. - if (auto attrSpec = TfDynamic_cast(spec)) { + if (auto attrSpec = + pxr::TfDynamic_cast(spec)) { // 3. Check if this spec has an authored `typeName`. if (attrSpec->HasField(pxr::SdfFieldKeys->TypeName)) { const pxr::TfToken authoredTypeName = @@ -119,5 +122,20 @@ void ExpectAllAuthoredAttributesMatchSchemaTypes(const pxr::UsdPrim& prim) { } } } + +bool AreQuatsSameRotation(const pxr::GfQuatf& q1, const pxr::GfQuatf& q2, + float tolerance) { + // The dot product of two unit quaternions (q1 and q2) is cos(theta), where + // theta is the angle between them on the 4D hypersphere. + // + // If q1 is close to q2, dot(q1, q2) is close to 1. + // If q1 is close to -q2, dot(q1, q2) is close to -1. + // + // By taking the absolute value of the dot product, we can check for + // closeness to 1 to see if the quaternions are collinear, which is what + // we want. This works for both cases. + const float dot = pxr::GfDot(q1, q2); + return pxr::GfIsClose(pxr::GfAbs(dot), 1.0f, tolerance); +} } // namespace usd } // namespace mujoco diff --git a/test/experimental/usd/test_utils.h b/test/experimental/usd/test_utils.h index 2aa62a6c..46abaec2 100644 --- a/test/experimental/usd/test_utils.h +++ b/test/experimental/usd/test_utils.h @@ -18,6 +18,7 @@ #include #include +#include #include #include #include @@ -29,6 +30,9 @@ #define EXPECT_PRIM_VALID(stage, path) \ EXPECT_TRUE((stage)->GetPrimAtPath(SdfPath(path)).IsValid()); +#define EXPECT_PRIM_INVALID(stage, path) \ + EXPECT_FALSE((stage)->GetPrimAtPath(SdfPath(path)).IsValid()); + #define EXPECT_PRIM_IS_A(stage, path, type) \ { \ EXPECT_TRUE((stage)->GetPrimAtPath(SdfPath(path)).IsA()); \ @@ -66,6 +70,19 @@ #define EXPECT_ATTRIBUTE_HAS_NO_VALUE(stage, path) \ EXPECT_FALSE((stage)->GetAttributeAtPath(SdfPath(path)).HasValue()); +#define EXPECT_ATTRIBUTE_HAS_AUTHORED_VALUE(stage, path) \ + EXPECT_TRUE((stage)->GetAttributeAtPath(SdfPath(path)).HasAuthoredValue()); + +#define EXPECT_ATTRIBUTE_HAS_NO_AUTHORED_VALUE(stage, path) \ + EXPECT_FALSE((stage)->GetAttributeAtPath(SdfPath(path)).HasAuthoredValue()); + +#define EXPECT_REL_TARGET_COUNT(stage, path, count) \ + { \ + pxr::SdfPathVector targets; \ + (stage)->GetRelationshipAtPath(SdfPath(path)).GetTargets(&targets); \ + EXPECT_TRUE(targets.size() == count); \ + } + #define EXPECT_REL_HAS_TARGET(stage, path, target_path) \ { \ pxr::SdfPathVector targets; \ @@ -115,6 +132,10 @@ void ExpectAttributeHasConnection(pxr::UsdStageRefPtr stage, const char* path, // Checks that all authored attributes on the given prim have types that match // the schema types. void ExpectAllAuthoredAttributesMatchSchemaTypes(const pxr::UsdPrim& prim); + +// Checks if two quaternions represent the same rotation. +bool AreQuatsSameRotation(const pxr::GfQuatf& q1, const pxr::GfQuatf& q2, + float tolerance = 1e-6); } // namespace usd } // namespace mujoco #endif // MUJOCO_TEST_EXPERIMENTAL_USD_PLUGINS_MJCF_FIXTURE_H_ diff --git a/test/testdata/flex.xml b/test/testdata/flex.xml index 4e243dbf..0bc6403a 100644 --- a/test/testdata/flex.xml +++ b/test/testdata/flex.xml @@ -1,5 +1,7 @@ - diff --git a/test/testdata/model.xml b/test/testdata/model.xml index 5a1f9c81..ce7ea048 100644 --- a/test/testdata/model.xml +++ b/test/testdata/model.xml @@ -129,7 +129,7 @@ - + diff --git a/test/user/user_api_test.cc b/test/user/user_api_test.cc index a415d740..d0fc8e6e 100644 --- a/test/user/user_api_test.cc +++ b/test/user/user_api_test.cc @@ -18,6 +18,8 @@ #include #include #include +#include +#include #include #include #include @@ -217,7 +219,7 @@ TEST_F(PluginTest, DeletePlugin) { EXPECT_THAT(model->actuator_plugin[0], 0); // delete actuator - mjs_delete(actuator->element); + mjs_delete(spec, actuator->element); // recompile and check that the plugin is not present mjModel* newmodel = mj_compile(spec, NULL); @@ -324,7 +326,7 @@ TEST_F(PluginTest, DetachPlugin) { // detach the body and compile mjsBody* body_to_detach = mjs_findBody(parent, "child-body"); EXPECT_THAT(body_to_detach, NotNull()); - EXPECT_THAT(mjs_detachBody(parent, body_to_detach), 0); + EXPECT_THAT(mjs_delete(parent, body_to_detach->element), 0); mjModel* model = mj_compile(parent, nullptr); EXPECT_THAT(model, NotNull()); EXPECT_THAT(model->nbody, 2); @@ -343,32 +345,35 @@ TEST_F(PluginTest, AttachExplicitPlugin) { )"; - static constexpr char xml_child[] = R"( - - - - - - - - - - - - - - - - - - - )"; - std::array err; mjSpec* parent = mj_parseXMLString(xml_parent, 0, err.data(), err.size()); ASSERT_THAT(parent, NotNull()) << err.data(); - mjSpec* child = mj_parseXMLString(xml_child, 0, err.data(), err.size()); - ASSERT_THAT(child, NotNull()) << err.data(); + + mjSpec* child = mj_makeSpec(); + mjsBody* body = mjs_addBody(mjs_findBody(child, "world"), 0); + mjsGeom* geom = mjs_addGeom(body, 0); + mjsSite* site = mjs_addSite(body, 0); + mjsSensor* sensor = mjs_addSensor(child); + mjsPlugin* plugin = mjs_addPlugin(child); + mjs_activatePlugin(child, "mujoco.sensor.touch_grid"); + mjs_setString(plugin->plugin_name, "mujoco.sensor.touch_grid"); + mjs_setString(sensor->plugin.plugin_name, "mujoco.sensor.touch_grid"); + mjs_setString(body->name, "body"); + mjs_setString(sensor->name, "touch2"); + mjs_setString(sensor->objname, "touch2"); + mjs_setString(site->name, "touch2"); + geom->size[0] = 0.1; + site->size[0] = 0.001; + sensor->type = mjSENS_PLUGIN; + sensor->objtype = mjOBJ_SITE; + sensor->plugin.element = plugin->element; + sensor->plugin.active = true; + std::map > config_attribs; + config_attribs["size"] = "8 12"; + config_attribs["fov"] = "10 13"; + config_attribs["gamma"] = "0"; + config_attribs["nchannel"] = "1"; + mjs_setPluginAttributes(plugin, &config_attribs); mjsBody* body_parent = mjs_findBody(parent, "body"); EXPECT_THAT(body_parent, NotNull()); @@ -1363,12 +1368,8 @@ void TestDetachBody(bool compile) { mjsBody* body = mjs_findBody(child, "body"); EXPECT_THAT(body, NotNull()); - // get an error if trying to delete the body - EXPECT_EQ(mjs_delete(body->element), -1); - EXPECT_THAT(mjs_getError(child), HasSubstr("use detach instead")); - - // detach subtree - EXPECT_THAT(mjs_detachBody(child, body), 0); + // delete subtree + EXPECT_THAT(mjs_delete(child, body->element), 0); // try saving to XML before compiling again std::array e; @@ -1893,12 +1894,12 @@ TEST_F(MujocoTest, PreserveState) { // detach subtree mjsBody* body = mjs_findBody(spec, "detachable"); EXPECT_THAT(body, NotNull()); - EXPECT_THAT(mjs_detachBody(spec, body), 0); + EXPECT_THAT(mjs_delete(spec, body->element), 0); // detach mocap mjsBody* mocap_body = mjs_findBody(spec, "mocap_detach"); EXPECT_THAT(mocap_body, NotNull()); - EXPECT_THAT(mjs_detachBody(spec, mocap_body), 0); + EXPECT_THAT(mjs_delete(spec, mocap_body->element), 0); // add body mjsBody* newbody = mjs_addBody(mjs_findBody(spec, "world"), 0); @@ -2782,25 +2783,20 @@ TEST_F(MujocoTest, DetachDefault) { mjsDefault* child = mjs_findDefault(spec, "child1"); EXPECT_THAT(child, NotNull()); - // try using mjs_delete to remove default, should fail - EXPECT_EQ(mjs_delete(child->element), -1); - - // detach default - EXPECT_EQ(mjs_detachDefault(spec, child), 0); + // delete default + EXPECT_EQ(mjs_delete(spec, child->element), 0); child = mjs_findDefault(spec, "child1"); EXPECT_THAT(child, IsNull()); // try and detach previously detached default, should fail - EXPECT_EQ(mjs_detachDefault(spec, child), -1); + EXPECT_EQ(mjs_delete(spec, nullptr), -1); child = mjs_findDefault(spec, "child1"); EXPECT_THAT(child, IsNull()); - EXPECT_THAT(mjs_getError(spec), - HasSubstr("Cannot detach, default is null")); // detach parent mjsDefault* parent = mjs_findDefault(spec, "parent"); EXPECT_THAT(parent, NotNull()); - mjs_detachDefault(spec, parent); + mjs_delete(spec, parent->element); // both parent and remaining child should be removed parent = mjs_findDefault(spec, "parent"); @@ -2811,7 +2807,7 @@ TEST_F(MujocoTest, DetachDefault) { // error when trying to detach the 'main' default mjsDefault* main = mjs_findDefault(spec, "main"); EXPECT_THAT(main, NotNull()); - EXPECT_EQ(mjs_detachDefault(spec, main), -1); + EXPECT_EQ(mjs_delete(spec, main->element), -1); EXPECT_THAT(mjs_getError(spec), HasSubstr("cannot remove the global default ('main')")); diff --git a/test/user/user_mesh_test.cc b/test/user/user_mesh_test.cc index 759561b2..dfa736bf 100644 --- a/test/user/user_mesh_test.cc +++ b/test/user/user_mesh_test.cc @@ -529,7 +529,7 @@ TEST_F(MjCMeshTest, TinyInertiaFails) { - + @@ -542,6 +542,7 @@ TEST_F(MjCMeshTest, TinyInertiaFails) { error.data(), HasSubstr( "mass and inertia of moving bodies must be larger than mjMINVAL")); + EXPECT_THAT(error.data(), HasSubstr("Element name 'tiny_body'")); } TEST_F(MjCMeshTest, FlippedFaceAllowedLegacyInertia) { @@ -1263,6 +1264,30 @@ TEST_F(MjCMeshTest, LoadSkin) { mj_deleteSpec(spec); } +// ------------- test octree --------------------------------------------------- + +TEST_F(MjCMeshTest, Octree) { + const std::string xml_path = GetTestDataFilePath(kTorusPath); + std::array error; + mjSpec* spec = mj_parseXML(xml_path.c_str(), 0, error.data(), error.size()); + mjsGeom* geom = mjs_asGeom(mjs_firstElement(spec, mjOBJ_GEOM)); + geom->type = mjGEOM_SDF; + mjModel* model = mj_compile(spec, 0); + ASSERT_THAT(model, NotNull()) << error.data(); + EXPECT_GT(model->mesh_octnum[0], 0); + mj_deleteSpec(spec); + mj_deleteModel(model); +} + +TEST_F(MjCMeshTest, OctreeNotComputedForNonSDF) { + const std::string xml_path = GetTestDataFilePath(kTorusPath); + std::array error; + mjModel* model = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size()); + ASSERT_THAT(model, NotNull()) << error.data(); + EXPECT_EQ(model->noct, 0); + mj_deleteModel(model); +} + } // namespace } // namespace mujoco diff --git a/test/user/user_model_test.cc b/test/user/user_model_test.cc index 8b00753d..80697969 100644 --- a/test/user/user_model_test.cc +++ b/test/user/user_model_test.cc @@ -196,7 +196,7 @@ TEST_F(UserCModelTest, NestedZeroMassBodiesFail) { - + @@ -214,6 +214,7 @@ TEST_F(UserCModelTest, NestedZeroMassBodiesFail) { error, HasSubstr( "mass and inertia of moving bodies must be larger than mjMINVAL")); + EXPECT_THAT(error, HasSubstr("Element name 'bad'")); mj_deleteModel(model); } @@ -440,6 +441,100 @@ TEST_F(FuseStaticTest, FuseStaticEquivalent) { mj_deleteModel(m_no_fuse); } +TEST_F(FuseStaticTest, FuseStaticActuatorReferencedBody) { + static constexpr char xml_template[] = R"( + + + + + + + + + + + + + + + + + + + + + + + )"; + std::array error; + mjModel* m = LoadModelFromString(xml_template, error.data(), error.size()); + ASSERT_THAT(m, NotNull()) << error.data(); + EXPECT_EQ(m->nbody, 3) << "Expecting a world body and two others"; + mj_deleteModel(m); +} + +TEST_F(FuseStaticTest, FuseStaticLightReferencedBody) { + static constexpr char xml_template[] = R"( + + + + + + + + + + + + + + + + + + + + )"; + std::array error; + mjModel* m = LoadModelFromString(xml_template, error.data(), error.size()); + ASSERT_THAT(m, NotNull()) << error.data(); + EXPECT_EQ(m->nbody, 3) << "Expecting a world body and two others"; + mj_deleteModel(m); +} + +TEST_F(FuseStaticTest, FuseStaticForceSensorReferencedBody) { + static constexpr char xml_template[] = R"( + + + + + + + + + + + + + + + + + + + + + + + + )"; + std::array error; + mjModel* m = LoadModelFromString(xml_template, error.data(), error.size()); + ASSERT_THAT(m, NotNull()) << error.data(); + EXPECT_EQ(m->nbody, 3) << "Expecting a world body and two others"; + mj_deleteModel(m); +} + // ------------- test discardvisual -------------------------------------------- using DiscardVisualTest = MujocoTest; diff --git a/test/xml/xml_native_writer_test.cc b/test/xml/xml_native_writer_test.cc index 2ad13bc7..6876245a 100644 --- a/test/xml/xml_native_writer_test.cc +++ b/test/xml/xml_native_writer_test.cc @@ -762,7 +762,7 @@ TEST_F(XMLWriterTest, WritesFrameDefaults) { - + @@ -791,16 +791,14 @@ TEST_F(XMLWriterTest, WritesFrameDefaults) { - + - - - + diff --git a/unity/Editor/Bindings/MujocoBinaryRetriever.cs b/unity/Editor/Bindings/MujocoBinaryRetriever.cs index 3ff96ab5..85f05f1b 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.3.3.dylib", + "/mujoco.framework/Versions/Current/libmujoco.3.3.4.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.3.3/lib/libmujoco.so.3.3.3", + "/.mujoco/mujoco-3.3.4/lib/libmujoco.so.3.3.4", mujocoPath + "/libmujoco.so"); AssetDatabase.Refresh(); } diff --git a/unity/Editor/Importer/MjImporterWithAssets.cs b/unity/Editor/Importer/MjImporterWithAssets.cs index a9a93184..c724e3a0 100644 --- a/unity/Editor/Importer/MjImporterWithAssets.cs +++ b/unity/Editor/Importer/MjImporterWithAssets.cs @@ -153,28 +153,36 @@ public class MjImporterWithAssets : MjcfImporter { var assetReferenceName = MjEngineTool.Sanitize(unsanitizedAssetReferenceName); var sourceFilePath = Path.Combine(_sourceMeshesDir, fileName); - if (Path.GetExtension(sourceFilePath) == ".obj") { - throw new NotImplementedException("OBJ mesh file loading is not yet implemented. " + - "Please convert to binary STL. " + + if (Path.GetExtension(sourceFilePath) != ".obj" && Path.GetExtension(sourceFilePath) != ".stl") { + throw new NotImplementedException("Type of mesh file not yet supported. " + + "Please convert to binary STL or OBJ. " + $"Attempted to load: {sourceFilePath}"); } - var targetFilePath = Path.Combine(_targetMeshesDir, assetReferenceName + ".stl"); + var targetFilePath = + Path.Combine(_targetMeshesDir, assetReferenceName + Path.GetExtension(sourceFilePath)); if (File.Exists(targetFilePath)) { File.Delete(targetFilePath); } var scale = MjEngineTool.UnityVector3( parentNode.GetVector3Attribute("scale", defaultValue: Vector3.one)); CopyMeshAndRescale(sourceFilePath, targetFilePath, scale); - var assetPath = Path.Combine(_targetAssetDir, assetReferenceName + ".stl"); + var assetPath = Path.Combine(_targetAssetDir, assetReferenceName + Path.GetExtension(sourceFilePath)); // This asset path should be available because the MuJoCo compiler guarantees element names // are unique, but check for completeness (and in case sanitizing the name broke uniqueness): if (AssetDatabase.LoadMainAssetAtPath(assetPath) != null) { throw new Exception( $"Trying to import mesh {unsanitizedAssetReferenceName} but {assetPath} already exists."); } + AssetDatabase.ImportAsset(assetPath); - var copiedMesh = AssetDatabase.LoadMainAssetAtPath(assetPath) as Mesh; + ModelImporter importer = AssetImporter.GetAtPath(assetPath) as ModelImporter; + if (importer != null && !importer.isReadable) { + importer.isReadable = true; + importer.SaveAndReimport(); + } + + var copiedMesh = AssetDatabase.LoadAssetAtPath(assetPath); if (copiedMesh == null) { throw new Exception($"Mesh {assetPath} was not imported."); } @@ -186,9 +194,16 @@ public class MjImporterWithAssets : MjcfImporter { private void CopyMeshAndRescale( string sourceFilePath, string targetFilePath, Vector3 scale) { var originalMeshBytes = File.ReadAllBytes(sourceFilePath); - var mesh = StlMeshParser.ParseBinary(originalMeshBytes, scale); - var rescaledMeshBytes = StlMeshParser.SerializeBinary(mesh); - File.WriteAllBytes(targetFilePath, rescaledMeshBytes); + if (Path.GetExtension(sourceFilePath) == ".stl") { + var mesh = StlMeshParser.ParseBinary(originalMeshBytes, scale); + var rescaledMeshBytes = StlMeshParser.SerializeBinary(mesh); + File.WriteAllBytes(targetFilePath, rescaledMeshBytes); + } else if (Path.GetExtension(sourceFilePath) == ".obj") { + ObjMeshImportUtility.CopyAndScaleOBJFile(sourceFilePath, targetFilePath, scale); + } else { + throw new NotImplementedException($"Extension {Path.GetExtension(sourceFilePath)} " + + $"not yet supported for MuJoCo mesh asset."); + } } private void ParseMaterial(XmlElement parentNode) { @@ -276,8 +291,8 @@ public class MjImporterWithAssets : MjcfImporter { // If geom is nameless, use a random number. var name = MjEngineTool.Sanitize(parentNode.GetStringAttribute( - "name", defaultValue: $"{UnityEngine.Random.Range(0, 1000000)}")); - var assetPath = Path.Combine(_targetAssetDir, name + ".mat"); + "name", defaultValue: $"{UnityEngine.Random.Range(0, 1000000)}")); + var assetPath = Path.Combine(_targetAssetDir, name+".mat"); if (AssetDatabase.LoadMainAssetAtPath(assetPath) != null) { throw new Exception( $"Creating a material asset for the geom {name}, but {assetPath} already exists."); @@ -289,6 +304,7 @@ public class MjImporterWithAssets : MjcfImporter { material = DefaultMujocoMaterial; } } + if (parentNode.GetFloatAttribute("group") > 2) renderer.enabled = false; renderer.sharedMaterial = material; } } diff --git a/unity/Editor/Importer/ObjMeshImportUtility.cs b/unity/Editor/Importer/ObjMeshImportUtility.cs new file mode 100644 index 00000000..262fd128 --- /dev/null +++ b/unity/Editor/Importer/ObjMeshImportUtility.cs @@ -0,0 +1,93 @@ +// Copyright 2019 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. + +using System.Globalization; +using System.IO; +using System.Linq; +using System.Text; +using UnityEngine; + +namespace Mujoco { + +/// +/// Scale vertex data manually line by line. We skip normals. Warning: Parameter vertex points +/// (`vp`) were unclear to me how to handle with scaling, if you use them and notice an issue +/// please report it. +/// +public static class ObjMeshImportUtility { + private static Vector3 ToXZY(float x, float y, float z) => new Vector3(x, z, y); + + public static void CopyAndScaleOBJFile(string sourceFilePath, string targetFilePath, + Vector3 scale) { + // OBJ files are human-readable + string[] lines = File.ReadAllLines(sourceFilePath); + StringBuilder outputBuilder = new StringBuilder(); + // Culture info for consistent decimal point handling + CultureInfo invariantCulture = CultureInfo.InvariantCulture; + scale = ToXZY(scale.x, scale.y, scale.z); + foreach (string line in lines) { + if (line.StartsWith("v ")) // Vertex line + { + // Split the line into components + string[] parts = line.Split(' '); + if (parts.Length >= 4) { + // Scale the vertex. It is unclear to me why flipping along x axis was necessary, + // but without it meshes were incorrectly oriented. + float x = -float.Parse(parts[1], invariantCulture) * scale.x; + float y = float.Parse(parts[2], invariantCulture) * scale.y; + float z = float.Parse(parts[3], invariantCulture) * scale.z; + + var swizzled = ToXZY(x, y, z); + outputBuilder.AppendLine( + $"v {swizzled.x.ToString(invariantCulture)} "+ + $"{swizzled.y.ToString(invariantCulture)} "+ + $"{swizzled.z.ToString(invariantCulture)}"); + } + } else if (line.StartsWith("vn ")) { + // We swizzle the normals too + string[] parts = line.Split(' '); + if (parts.Length >= 4) { + float x = -float.Parse(parts[1], invariantCulture); + float y = float.Parse(parts[2], invariantCulture); + float z = float.Parse(parts[3], invariantCulture); + + var swizzled = ToXZY(x, y, z); + outputBuilder.AppendLine( + $"vn {swizzled.x.ToString(invariantCulture)} "+ + $"{swizzled.y.ToString(invariantCulture)} "+ + $"{swizzled.z.ToString(invariantCulture)}"); + } + } else if (line.StartsWith("f ") && scale.x*scale.y*scale.z < 0) { + // Faces definition, flip face by reordering vertices + string[] parts = line.Split(' '); + if (parts.Length >= 4) { + outputBuilder.Append(parts[0]+" "); + var face = parts.Skip(1).ToArray(); + if (face.Length >= 3) { + outputBuilder.Append(face[0]+" "); + outputBuilder.Append(face[2]+" "); + outputBuilder.Append(face[1]); + } + outputBuilder.AppendLine(); + } + } else { + // Copy non-vertex lines as-is + outputBuilder.AppendLine(line); + } + } + // Write the scaled OBJ to the target file + File.WriteAllText(targetFilePath, outputBuilder.ToString()); + } +} +} diff --git a/unity/Editor/Importer/ObjMeshImportUtility.cs.meta b/unity/Editor/Importer/ObjMeshImportUtility.cs.meta new file mode 100644 index 00000000..195f099c --- /dev/null +++ b/unity/Editor/Importer/ObjMeshImportUtility.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: adec9978c00bb934eb8bf974c26193e1 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index 11698ed6..ea06d907 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -110,7 +110,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 = 333; +public const int mjVERSION_HEADER = 334; // ------------------------------------Enums------------------------------------ @@ -5114,6 +5114,7 @@ public unsafe struct mjOption_ { [StructLayout(LayoutKind.Sequential)] public unsafe struct global { + public int cameraid; public int orthographic; public float fovy; public float ipd; @@ -5241,6 +5242,7 @@ public unsafe struct mjModel_ { public int nbvh; public int nbvhstatic; public int nbvhdynamic; + public int noct; public int njnt; public int ngeom; public int nsite; @@ -5357,6 +5359,9 @@ public unsafe struct mjModel_ { public int* bvh_child; public int* bvh_nodeid; public double* bvh_aabb; + public int* oct_depth; + public int* oct_child; + public double* oct_aabb; public int* jnt_type; public int* jnt_qposadr; public int* jnt_dofadr; @@ -5527,6 +5532,8 @@ public unsafe struct mjModel_ { public int* mesh_facenum; public int* mesh_bvhadr; public int* mesh_bvhnum; + public int* mesh_octadr; + public int* mesh_octnum; public int* mesh_normaladr; public int* mesh_normalnum; public int* mesh_texcoordadr; @@ -6110,6 +6117,7 @@ public unsafe struct mjvOption_ { public fixed byte skingroup[6]; public fixed byte flags[32]; public int bvh_depth; + public int oct_depth; public int flex_layer; } diff --git a/unity/Runtime/Components/Shapes/MjMeshFilter.cs b/unity/Runtime/Components/Shapes/MjMeshFilter.cs index 83f4963d..3f6fec32 100644 --- a/unity/Runtime/Components/Shapes/MjMeshFilter.cs +++ b/unity/Runtime/Components/Shapes/MjMeshFilter.cs @@ -41,19 +41,19 @@ public class MjMeshFilter : MonoBehaviour { return; } - _shapeChangeStamp = currentChangeStamp; - Tuple meshData = _geom.BuildMesh(); - - if (meshData == null) { - throw new ArgumentException("Unsupported geom shape detected"); - } - - if(_geom.ShapeType == MjShapeComponent.ShapeTypes.Mesh) { + if(_geom.ShapeType == MjShapeComponent.ShapeTypes.Mesh) { MjMeshShape meshShape = _geom.Shape as MjMeshShape; _meshFilter.sharedMesh = meshShape.Mesh; return; } + _shapeChangeStamp = currentChangeStamp; + Tuple meshData = _geom.BuildMesh(); + if (meshData == null) + { + throw new ArgumentException("Unsupported geom shape detected"); + } + DisposeCurrentMesh(); var mesh = new Mesh(); @@ -67,7 +67,6 @@ public class MjMeshFilter : MonoBehaviour { uvs[i] = new Vector2(mesh.vertices[i].x, mesh.vertices[i].z); } mesh.uv = uvs; - mesh.RecalculateNormals(); mesh.RecalculateTangents(); } diff --git a/unity/Runtime/Components/Shapes/MjMeshShape.cs b/unity/Runtime/Components/Shapes/MjMeshShape.cs index e4fa338d..22721209 100644 --- a/unity/Runtime/Components/Shapes/MjMeshShape.cs +++ b/unity/Runtime/Components/Shapes/MjMeshShape.cs @@ -36,7 +36,7 @@ public class MjMeshShape : IMjShape { var assetName = MjEngineTool.Sanitize( mjcf.GetStringAttribute("mesh", defaultValue: string.Empty)); if (!string.IsNullOrEmpty(assetName)) { - Mesh = (Mesh)Resources.Load(assetName); + Mesh = Resources.Load(assetName); } } diff --git a/unity/package.json b/unity/package.json index c91c5d6c..d548ea5a 100644 --- a/unity/package.json +++ b/unity/package.json @@ -1,7 +1,7 @@ { "name": "org.mujoco", "displayName": "MuJoCo", - "version": "3.3.3", + "version": "3.3.4", "description": "MuJoCo importer and runtime plug-in", "dependencies": {}, "author": {