diff --git a/doc/APIreference/functions.rst b/doc/APIreference/functions.rst index ef49bf39..fc513e20 100644 --- a/doc/APIreference/functions.rst +++ b/doc/APIreference/functions.rst @@ -1179,6 +1179,18 @@ It is also triggered for :ref:`user sensors` of :ref:`stage`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +.. mujoco-include:: mj_maxContact + +Return the maximum number of contacts that can be generated between two geoms. + +If has_margin is -1, then the margin is pulled from the model, otherwise if has_margin > 0 +indicates that the geoms have a positive margin. + .. _mj_collision: `mj_collision <#mj_collision>`__ @@ -1507,6 +1519,24 @@ Add file to VFS from buffer; return 0: success, 2: repeated name, -1: failed to Delete file from VFS; return 0: success, -1: not found in VFS. +.. _mj_containsBufferVFS: + +`mj_containsBufferVFS <#mj_containsBufferVFS>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +.. mujoco-include:: mj_containsBufferVFS + +Check if buffer exists in VFS; return 1: exists, 0: not found. + +.. _mj_containsFileVFS: + +`mj_containsFileVFS <#mj_containsFileVFS>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +.. mujoco-include:: mj_containsFileVFS + +Check if file exists in VFS; return 1: exists, 0: not found. + .. _mj_deleteVFS: `mj_deleteVFS <#mj_deleteVFS>`__ diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index 9a8eba6c..1bc3c846 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -677,7 +677,7 @@ from its default. .. _option-flag-multiccd: -:at:`multiccd`: :at-val:`[disable, enable], "disable"` +:at:`multiccd`: :at-val:`[disable, enable], "enable"` This flag enables multiple-contact collision detection for geom pairs that use a general-purpose convex-convex collider e.g., mesh-mesh collisions. This can be useful when the contacting geoms have a flat surface and the single contact point generated by the convex-convex collider cannot accurately capture the surface contact, leading @@ -3593,7 +3593,7 @@ saving the XML: .. _body-flexcomp-dof: -:at:`dof`: :at-val:`[full, radial, trilinear, quadratic], "full"` +:at:`dof`: :at-val:`[full, radial, trilinear, quadratic, 2d], "full"` The parametrization of the flex's degrees of freedom (dofs). See the video on the right illustrating the different parametrizations with deformable spheres. The three models in the video are respectively `sphere_full `__, @@ -3608,6 +3608,10 @@ saving the XML: requires a free joint at the flex's parent in order for free body motion to be possible. This type of parametrization is appropriate for shapes that are relatively spherical. + **2d** + Two orthogonal translational dofs (X and Y) per vertex. This restricts the motion of the vertices to planes + parallel to the parent body's X-Y plane. + **trilinear** Three translational dofs at each corner of the bounding box of the flex, for a total of 24 dofs for the entire flex, independent of the number of vertices. The positions of the vertices are updated using trilinear @@ -4324,7 +4328,7 @@ stress-strain relationship. See also :ref:`deformable ` objects and :at:`elastic2d`: :at-val:`[none, bend, stretch, both], "none"` Elastic contribution to passive forces of 2D flexes. "none": none, "bend": bending only, "stretch": stretching only, - "both": bending and stretching. + "both": bending and stretching. Not yet supported by :ref:`dof` **trilinear** and **quadratic**. .. _flex-contact: @@ -4888,8 +4892,11 @@ constraint type is only supported for dimension 3 flexes with trilinear or quadr :at:`flex`: :at-val:`string, required` Name of the flex whose strain is being constrained. +.. _equality-flexstrain-cell: - +:at:`cell`: :at-val:`int(3), optional` + 3D grid index (i, j, k) identifying the cell in the flex object. The grid size is specified in the :ref:`cellcount + ` attribute. .. _tendon: diff --git a/doc/XMLschema.rst b/doc/XMLschema.rst index 60d828be..f394f2ac 100755 --- a/doc/XMLschema.rst +++ b/doc/XMLschema.rst @@ -2062,6 +2062,9 @@ .. grid-item:: :ref:`flex` + .. grid-item:: + :ref:`cell` + .. grid-item:: :ref:`active` diff --git a/doc/changelog.rst b/doc/changelog.rst index b073a3f2..e3f311b2 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -7,9 +7,29 @@ Upcoming version (not yet released) General ^^^^^^^ - +- Added new :ref:`mj_maxContact` function to get the maximum number of possible contacts returned by + two geoms. +- Added ``mj_containsBufferVFS`` and ``mj_containsFileVFS`` to check for existence of buffers and files in VFS. - Added :ref:`multi-cell support` for trilinear and quadratic flexes. Note that the implicit integrator uses a dense solver for the flex degrees of freedom, which can be slow for multi-cell flexes. +- Refactored ``flexstrain`` equality constraints to be instantiated per cell instead of per flex object, reducing the + number of degrees of freedom per constraint row. The equality can be associated with a specific cell with the new + attribute ":ref:`cell ` + + .. admonition:: Breaking API changes + :class: attention + + - The feature :ref:`multiccd` is now enabled by default. This feature has little performance overhead + and gives better contact behavior for stability. + + **Migration:** The flag :ref:`multiccd` must be explicitly disabled. + +Bug fixes +^^^^^^^^^ + +- Asset paths in attached child specs are now resolved relative to the model file directory of the child spec, rather + than the parent spec. This prevents the origin of the parent spec to affect the resolution of asset paths in the child + spec. Version 3.7.0 (April 14, 2026) ------------------------------ diff --git a/doc/computation/index.rst b/doc/computation/index.rst index 01b06886..07f9adb5 100644 --- a/doc/computation/index.rst +++ b/doc/computation/index.rst @@ -1656,23 +1656,25 @@ Both pipelines are controlled by a tolerance (in units of distance) and maximum Multiple contacts ^^^^^^^^^^^^^^^^^ -Some colliders can return more than one contact per colliding pair to model line or surface contacts, as when two flat +Some colliders can return more than one contact per colliding pair to model edge or surface contacts, as when two flat objects touch. For example the capsule-plane and box-plane colliders can return up to two or four contacts, -respectively. Standard general-purpose convex collision algorithms like MPR and GJK always return a single contact +respectively. Standard general-purpose convex collision algorithms like MPR and GJK/EPA always return a single contact point, which is problematic for surface contact scenarios (e.g., box-stacking). Both of MuJoCo's CCD pipelines can return multiple points per contacting pair ("multiccd"). This behavior is controlled by the :ref:`multiccd` flag, but is implemented in different ways with different trade-offs: -libccd pipeline (legacy) +multi-run pipeline (legacy) Multiple contact points are found by rotating the two geoms by ±1e-3 radians around the tangential axes and re-running the collision routine. If a new contact is detected it is added, allowing for up to 4 additional contact - points. This method is effective, but increases the cost of each collision call by a factor of 5. + points. This method is effective, but increases the cost of each collision call by a factor of 5. This method is + used when the :ref:`nativeccd` flag is disabled, and for geoms collisions involving cylinders + and capsules or with :ref:`positive contact margins`. -native pipeline - Native multiccd discovers multiple contacts using a novel analysis of the contacting surfaces at the solution, - avoiding full re-runs of the collision routine, and is thus effectively "free". Note that native multiccd currently - does not support positive contact margins. If one of the two geoms has a positive margin, native multiccd will fall - back to legacy algorithm. +single-shot pipeline + The single-shot pipeline is used in conjunction with the native CCD pipeline, i.e., when the + :ref:`nativeccd` flag is enabled. As this pipeline is one-shot and most of the geom analysis + is done at compilation time, there is very little performance overhead. Supported geoms are boxes and meshes without + :ref:`positive contact margins`. .. _coDistance: @@ -1716,9 +1718,36 @@ work, but it pays off at runtime and yields both faster and more stable simulati Pair-wise colliders ^^^^^^^^^^^^^^^^^^^ -The table below provides information about the colliders used for different geom pairs. The second row in each cell -lists the maximum number of contacts generated, possibly with ``multiccd`` enabled. For example, ``Mesh`` / ``Mesh`` -will generate up to 1 contact or with ``multiccd`` up to 4 contacts. +The table below provides information about the colliders used for different geom pairs. These values can be computed +dynamically by the :ref:`mj_maxContact` function. Use the toggles to see the max number of contacts returned with the +parameters :ref:`nativeccd`, :ref:`multiccd`, and +:ref:`margin`. + +.. raw:: html + +
+
+ + nativeccd +
+
+ + multiccd +
+
+ + with margin +
+
.. list-table:: :header-rows: 1 @@ -1742,7 +1771,7 @@ will generate up to 1 contact or with ``multiccd`` up to 4 contacts. - | primitive | **1** - | primitive - | **2** + | **4** - | primitive | **4** - | primitive @@ -1785,12 +1814,28 @@ will generate up to 1 contact or with ``multiccd`` up to 4 contacts. | **2** - | CCD | **1** - - | CCD - | **1**, **4** + - + .. raw:: html + +
CCD
+
+
1
+
5
+
5
+
+ - | primitive | **2** - - | CCD - | **1**, **4** + - + .. raw:: html + +
CCD
+
+
1
+
5
+
5
+
+ - | SDF | :ref:`sdf_initpoints ` * - Ellipsoid @@ -1810,12 +1855,36 @@ will generate up to 1 contact or with ``multiccd`` up to 4 contacts. - - - - - | CCD - | **1**, **4** - - | CCD - | **1**, **4** - - | CCD - | **1**, **4** + - + .. raw:: html + +
CCD
+
+
1
+
5
+
5
+
+ + - + .. raw:: html + +
CCD
+
+
1
+
5
+
5
+
+ + - + .. raw:: html + +
CCD
+
+
1
+
5
+
5
+
+ - | SDF | :ref:`sdf_initpoints ` * - Box @@ -1825,8 +1894,16 @@ will generate up to 1 contact or with ``multiccd`` up to 4 contacts. - - | primitive | **8** - - | CCD - | **1**, **4** + - + .. raw:: html + +
CCD
+
+
1
+
4
+
5
+
+ - | SDF | :ref:`sdf_initpoints ` * - Mesh @@ -1835,8 +1912,16 @@ will generate up to 1 contact or with ``multiccd`` up to 4 contacts. - - - - - | CCD - | **1**, **4** + - + .. raw:: html + +
CCD
+
+
1
+
4
+
5
+
+ - | MeshSDF | :ref:`sdf_initpoints ` * - SDF @@ -1849,6 +1934,32 @@ will generate up to 1 contact or with ``multiccd`` up to 4 contacts. - | SDF | :ref:`sdf_initpoints ` +.. raw:: html + + + + + .. _Sleeping: Sleeping islands diff --git a/doc/css/theme_overrides.css b/doc/css/theme_overrides.css index 45f94131..a7318056 100644 --- a/doc/css/theme_overrides.css +++ b/doc/css/theme_overrides.css @@ -54,6 +54,98 @@ body[data-theme="dark"] table.docutils:not(.mjcf-attributes) { font-size: 85%; } +.pairwise-toggles { + display: flex; + align-items: center; + gap: 1.5em; + margin-bottom: 0.75em; +} + +.pairwise-toggle-item { + display: flex; + align-items: center; + gap: 0.5em; +} + +.pairwise-switch { + position: relative; + display: inline-block; + width: 36px; + height: 20px; +} + +.pairwise-switch input { + opacity: 0; + width: 0; + height: 0; +} + +.pairwise-slider { + position: absolute; + cursor: pointer; + inset: 0; + background-color: #ccc; + transition: 0.3s; + border-radius: 20px; +} + +.pairwise-slider:before { + content: ""; + position: absolute; + height: 14px; + width: 14px; + left: 3px; + bottom: 3px; + background-color: white; + transition: 0.3s; + border-radius: 50%; +} + +.pairwise-switch input:checked + .pairwise-slider { + background-color: var(--secondary-header-color, #123693); +} + +.pairwise-switch input:checked + .pairwise-slider:before { + transform: translateX(16px); +} + +.multiccd-off, +.multiccd-native, +.multiccd-legacy { + display: none; + margin: 0; +} + +.multiccd-off { + display: inline; +} + +.multiccd-enabled .multiccd-off { + display: none; +} + +.multiccd-enabled.nativeccd-enabled:not(.margin-enabled) .multiccd-native { + display: inline; +} + +.multiccd-enabled:not(.nativeccd-enabled) .multiccd-legacy, +.multiccd-enabled.nativeccd-enabled.margin-enabled .multiccd-legacy { + display: inline; +} + +.margin-show { + display: none; +} + +.margin-enabled .margin-hide { + display: none; +} + +.margin-enabled .margin-show { + display: inline; +} + + .small-centered td, .small-centered th, .table-pairwise td, .table-pairwise th { text-align: center !important; diff --git a/doc/includes/references.h b/doc/includes/references.h index 0ee5a22b..4d767933 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -494,8 +494,9 @@ typedef enum mjtDisableBit_ { // disable default feature bitflags mjDSBL_AUTORESET = 1<<16, // automatic reset when numerical issues are detected mjDSBL_NATIVECCD = 1<<17, // native convex collision detection mjDSBL_ISLAND = 1<<18, // constraint island discovery + mjDSBL_MULTICCD = 1<<19, // multiple CCD contact points - mjNDISABLE = 19 // number of disable flags + mjNDISABLE = 20 // number of disable flags } mjtDisableBit; typedef enum mjtEnableBit_ { // enable optional feature bitflags mjENBL_OVERRIDE = 1<<0, // override contact parameters @@ -503,10 +504,9 @@ typedef enum mjtEnableBit_ { // enable optional feature bitflags mjENBL_FWDINV = 1<<2, // record solver statistics mjENBL_INVDISCRETE = 1<<3, // discrete-time inverse dynamics // experimental features: - mjENBL_MULTICCD = 1<<4, // multi-point convex collision detection - mjENBL_SLEEP = 1<<5, // sleeping + mjENBL_SLEEP = 1<<4, // sleeping - mjNENABLE = 6 // number of enable flags + mjNENABLE = 5 // number of enable flags } mjtEnableBit; typedef enum mjtJoint_ { // type of degree of freedom mjJNT_FREE = 0, // global position and orientation (quat) (7) @@ -3161,6 +3161,8 @@ int mj_unmountVFS(mjVFS* vfs, const char* filename); int mj_addFileVFS(mjVFS* vfs, const char* directory, const char* filename); int mj_addBufferVFS(mjVFS* vfs, const char* name, const void* buffer, int nbuffer); int mj_deleteFileVFS(mjVFS* vfs, const char* filename); +int mj_containsBufferVFS(mjVFS* vfs, const char* name); +int mj_containsFileVFS(mjVFS* vfs, const char* directory, const char* filename); void mj_deleteVFS(mjVFS* vfs); size_t mj_getCacheSize(const mjCache* cache); size_t mj_getCacheCapacity(const mjCache* cache); @@ -3272,6 +3274,7 @@ void mj_passive(const mjModel* m, mjData* d); void mj_subtreeVel(const mjModel* m, mjData* d); void mj_rne(const mjModel* m, mjData* d, int flg_acc, mjtNum* result); void mj_rnePostConstraint(const mjModel* m, mjData* d); +int mj_maxContact(const mjModel* m, int g1, int g2, int has_margin); void mj_collision(const mjModel* m, mjData* d); void mj_makeConstraint(const mjModel* m, mjData* d); void mj_island(const mjModel* m, mjData* d); diff --git a/doc/mjwarp/index.rst b/doc/mjwarp/index.rst index bd65306b..f5d10431 100644 --- a/doc/mjwarp/index.rst +++ b/doc/mjwarp/index.rst @@ -1265,8 +1265,6 @@ is available by setting the ``NATIVECCD`` disable flag: The specialized collider generates up to 8 contact points, compared to up to 4 for the convex pipeline, and may improve contact stability for tasks involving box stacking or manipulation. -.. TODO(taylorhowell): update this section once multiccd is on by default. - CCD margin ---------- @@ -1283,8 +1281,8 @@ CCD colliders and will raise a ``NotImplementedError`` when calling :func:`mjw.p - Scenario - Workaround * - box-box, box-mesh, mesh-mesh - - :ref:`MULTICCD ` enabled - - Set margin to ``0`` or do not enable ``MULTICCD`` + - :ref:`MULTICCD ` enabled (on by default) + - Set margin to ``0`` or disable ``MULTICCD`` * - box-box - :ref:`NATIVECCD ` enabled (on by default) - Set margin to ``0`` or disable ``NATIVECCD`` diff --git a/doc/mjwarp/update_types.py b/doc/mjwarp/update_types.py index a19a0b61..316b2d2e 100644 --- a/doc/mjwarp/update_types.py +++ b/doc/mjwarp/update_types.py @@ -29,7 +29,7 @@ This script updates such instances with valid types @dataclasses.dataclass class Option: ... - timestep: wp.array(dtype=float) + timestep: wp.array[float] ... """ @@ -48,13 +48,13 @@ def replace_array_calls(match): dtype = args[-1] if n_args == 2: - return f'wp.array(dtype={dtype})' + return f'wp.array[{dtype}]' elif n_args == 3: - return f'wp.array2d(dtype={dtype})' + return f'wp.array2d[{dtype}]' elif n_args == 4: - return f'wp.array3d(dtype={dtype})' + return f'wp.array3d[{dtype}]' elif n_args == 5: - return f'wp.array4d(dtype={dtype})' + return f'wp.array4d[{dtype}]' else: return match.group(0) diff --git a/doc/modeling.rst b/doc/modeling.rst index 017e68fc..c5ed084a 100644 --- a/doc/modeling.rst +++ b/doc/modeling.rst @@ -1767,10 +1767,11 @@ better visualize and understand the contact configuration and resulting forces. a. Improve the geometry of the contacting geoms in order to add more contact points, possibly with non-flat geometry (e.g., bumps), so slippage is prevented by the normal force and not only frictional components. - b. If contacts are between flat surfaces, try enabling the :ref:`multiccd` flag, which allows - the detector to find more contacts than the single contact returned by the convex-convex collider. - c. Try enabling the native collision detection pipeline by setting the :ref:`nativeccd` flag, - which uses a more accurate and efficient convex collision detection algorithm. + b. If contacts are between flat surfaces, make sure that the flag :ref:`multiccd` is not + disabled (enabled by default), as it allows the detector to find more contacts than the single contact + returned by the convex-convex collider. + c. Make sure that the flag :ref:`nativeccd` is not disabled (enabled by default), + as NativeCCD is a more accurate and efficient convex collision detection algorithm. **High-frequency vibration** High-frequency, low-amplitude vibrations are also a real-world problem in many industrial settings, but unlike in diff --git a/doc/programming/index.rst b/doc/programming/index.rst index 48ef13f2..7b34cdcc 100644 --- a/doc/programming/index.rst +++ b/doc/programming/index.rst @@ -116,9 +116,10 @@ target directory. **Notes:** -- When building on Windows, use Visual Studio 2019 or later and make sure Windows SDK version 10.0.22000 or later is - installed (see :issue:`862` for more details). -- To optimize runtime performance build with ``-DCMAKE_BUILD_TYPE=Release`` +- To optimize runtime performance build with ``-DCMAKE_BUILD_TYPE=Release``. +- When building on Windows with MSVC, use Visual Studio 2019 or later and make sure Windows SDK version 10.0.22000 or + later is installed (see :issue:`862` for more details). +- We've found that performance on Windows is best when building with Clang, rather than MSVC. .. tip:: As a reference, a working build configuration can be found in MuJoCo's diff --git a/include/mujoco/mjmodel.h b/include/mujoco/mjmodel.h index fba680f9..1c9214ae 100644 --- a/include/mujoco/mjmodel.h +++ b/include/mujoco/mjmodel.h @@ -70,8 +70,9 @@ typedef enum mjtDisableBit_ { // disable default feature bitflags mjDSBL_AUTORESET = 1<<16, // automatic reset when numerical issues are detected mjDSBL_NATIVECCD = 1<<17, // native convex collision detection mjDSBL_ISLAND = 1<<18, // constraint island discovery + mjDSBL_MULTICCD = 1<<19, // multiple CCD contact points - mjNDISABLE = 19 // number of disable flags + mjNDISABLE = 20 // number of disable flags } mjtDisableBit; @@ -81,10 +82,9 @@ typedef enum mjtEnableBit_ { // enable optional feature bitflags mjENBL_FWDINV = 1<<2, // record solver statistics mjENBL_INVDISCRETE = 1<<3, // discrete-time inverse dynamics // experimental features: - mjENBL_MULTICCD = 1<<4, // multi-point convex collision detection - mjENBL_SLEEP = 1<<5, // sleeping + mjENBL_SLEEP = 1<<4, // sleeping - mjNENABLE = 6 // number of enable flags + mjNENABLE = 5 // number of enable flags } mjtEnableBit; diff --git a/include/mujoco/mujoco.h b/include/mujoco/mujoco.h index fa67e0bf..2bd0cae8 100644 --- a/include/mujoco/mujoco.h +++ b/include/mujoco/mujoco.h @@ -94,6 +94,12 @@ MJAPI int mj_addBufferVFS(mjVFS* vfs, const char* name, const void* buffer, int // Delete file from VFS; return 0: success, -1: not found in VFS. MJAPI int mj_deleteFileVFS(mjVFS* vfs, const char* filename); +// Check if buffer exists in VFS; return 1: exists, 0: not found. +MJAPI int mj_containsBufferVFS(mjVFS* vfs, const char* name); + +// Check if file exists in VFS; return 1: exists, 0: not found. +MJAPI int mj_containsFileVFS(mjVFS* vfs, const char* directory, const char* filename); + // Delete all files from VFS and deallocates VFS internal memory. MJAPI void mj_deleteVFS(mjVFS* vfs); @@ -460,6 +466,11 @@ MJAPI void mj_rne(const mjModel* m, mjData* d, int flg_acc, mjtNum* result); // RNE with complete data: compute cacc, cfrc_ext, cfrc_int. MJAPI void mj_rnePostConstraint(const mjModel* m, mjData* d); +// Return the maximum number of contacts that can be generated between two geoms. +// If has_margin is -1, then the margin is pulled from the model, otherwise if has_margin > 0 +// indicates that the geoms have a positive margin. +MJAPI int mj_maxContact(const mjModel* m, int g1, int g2, int has_margin); + // Run collision detection. MJAPI void mj_collision(const mjModel* m, mjData* d); diff --git a/mjx/mujoco/mjx/_src/types.py b/mjx/mujoco/mjx/_src/types.py index 8bc60bd2..ff08579c 100644 --- a/mjx/mujoco/mjx/_src/types.py +++ b/mjx/mujoco/mjx/_src/types.py @@ -92,8 +92,6 @@ class EnableBit(enum.IntFlag): INVDISCRETE = mujoco.mjtEnableBit.mjENBL_INVDISCRETE # unsupported: OVERRIDE, ENERGY, FWDINV, ISLAND - # required by the C implementation only, ignored otherwise: MULTICCD - MULTICCD = mujoco.mjtEnableBit.mjENBL_MULTICCD SLEEP = mujoco.mjtEnableBit.mjENBL_SLEEP diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_convex.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_convex.py index 4f6b21c5..88917ea7 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_convex.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_convex.py @@ -36,7 +36,7 @@ from mujoco.mjx.third_party.mujoco_warp._src.types import MJ_MAX_EPAHORIZON from mujoco.mjx.third_party.mujoco_warp._src.types import MJ_MAXCONPAIR from mujoco.mjx.third_party.mujoco_warp._src.types import MJ_MAXVAL from mujoco.mjx.third_party.mujoco_warp._src.types import Data -from mujoco.mjx.third_party.mujoco_warp._src.types import EnableBit +from mujoco.mjx.third_party.mujoco_warp._src.types import DisableBit from mujoco.mjx.third_party.mujoco_warp._src.types import GeomType from mujoco.mjx.third_party.mujoco_warp._src.types import Model from mujoco.mjx.third_party.mujoco_warp._src.types import mat43 @@ -1127,7 +1127,7 @@ def convex_narrowphase(m: Model, d: Data, ctx: CollisionContext, collision_table epa_iterations = 16 if nboxbox == ncollision else m.opt.ccd_iterations # set to true to enable multiccd - use_multiccd = m.opt.enableflags & EnableBit.MULTICCD + use_multiccd = m.opt.disableflags & DisableBit.MULTICCD == 0 # need at least 4 (square sides) if there's a box collision needing multiccd nmaxpolygon = 4 if nboxbox > 0 else 0 diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/io.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/io.py index 4a743681..56bbb18c 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/io.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/io.py @@ -370,7 +370,7 @@ def put_model(mjm: mujoco.MjModel) -> types.Model: ) # check for unsupported margin + multicontact / box-box CCD combinations - use_multiccd = mjm.opt.enableflags & types.EnableBit.MULTICCD + use_multiccd = (mjm.opt.disableflags & types.DisableBit.MULTICCD) == 0 nativeccd_disabled = mjm.opt.disableflags & types.DisableBit.NATIVECCD BOX = int(mujoco.mjtGeom.mjGEOM_BOX) MESH = int(mujoco.mjtGeom.mjGEOM_MESH) diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/types.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/types.py index 7a9dc2a5..bf4f1b6b 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/types.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/types.py @@ -184,6 +184,7 @@ class DisableBit(enum.IntFlag): EULERDAMP: implicit damping for Euler integration NATIVECCD: native convex collision detection (ignored in MJWarp) ISLAND: constraint islands + MULTICCD: multiple CCD contact points """ CONSTRAINT = mujoco.mjtDisableBit.mjDSBL_CONSTRAINT @@ -203,6 +204,7 @@ class DisableBit(enum.IntFlag): EULERDAMP = mujoco.mjtDisableBit.mjDSBL_EULERDAMP NATIVECCD = mujoco.mjtDisableBit.mjDSBL_NATIVECCD ISLAND = mujoco.mjtDisableBit.mjDSBL_ISLAND + MULTICCD = mujoco.mjtDisableBit.mjDSBL_MULTICCD # unsupported: MIDPHASE, AUTORESET @@ -212,12 +214,10 @@ class EnableBit(enum.IntFlag): Attributes: ENERGY: energy computation INVDISCRETE: discrete-time inverse dynamics - MULTICCD: multiple contacts with CCD """ ENERGY = mujoco.mjtEnableBit.mjENBL_ENERGY INVDISCRETE = mujoco.mjtEnableBit.mjENBL_INVDISCRETE - MULTICCD = mujoco.mjtEnableBit.mjENBL_MULTICCD # unsupported: OVERRIDE, FWDINV, ISLAND diff --git a/model/flex/bunny.xml b/model/flex/bunny.xml index ec31684e..0d10cad9 100644 --- a/model/flex/bunny.xml +++ b/model/flex/bunny.xml @@ -31,7 +31,7 @@ - + diff --git a/model/flex/bunny_multicell.xml b/model/flex/bunny_multicell.xml index a77f5376..730cbc5d 100644 --- a/model/flex/bunny_multicell.xml +++ b/model/flex/bunny_multicell.xml @@ -31,7 +31,7 @@ - + diff --git a/model/flex/bunny_quadratic.xml b/model/flex/bunny_quadratic.xml index 5b98759f..59e255aa 100644 --- a/model/flex/bunny_quadratic.xml +++ b/model/flex/bunny_quadratic.xml @@ -31,7 +31,7 @@ - + diff --git a/model/flex/bunny_with_uv.xml b/model/flex/bunny_with_uv.xml index c3cde418..9de1d8c2 100644 --- a/model/flex/bunny_with_uv.xml +++ b/model/flex/bunny_with_uv.xml @@ -37,7 +37,7 @@ - + diff --git a/model/flex/gripper_2d.xml b/model/flex/gripper_2d.xml new file mode 100644 index 00000000..b6c6c611 --- /dev/null +++ b/model/flex/gripper_2d.xml @@ -0,0 +1,429 @@ + + + + + + diff --git a/plugin/usd_decoder/usd_decoder.cc b/plugin/usd_decoder/usd_decoder.cc index f1ab1c05..04fe6976 100644 --- a/plugin/usd_decoder/usd_decoder.cc +++ b/plugin/usd_decoder/usd_decoder.cc @@ -704,7 +704,7 @@ void ParseUsdPhysicsScene(mjSpec* spec, bool multiccd_flag; mjc_physics_scene.GetMultiCCDFlagAttr().Get(&multiccd_flag); - spec->option.enableflags |= (multiccd_flag ? mjENBL_MULTICCD : 0); + spec->option.disableflags |= (!multiccd_flag ? mjDSBL_MULTICCD : 0); // Compiler attributes auto auto_limits_attr = mjc_physics_scene.GetAutoLimitsAttr(); diff --git a/python/mujoco/bindings_test.py b/python/mujoco/bindings_test.py index 8dd91c60..8df15728 100644 --- a/python/mujoco/bindings_test.py +++ b/python/mujoco/bindings_test.py @@ -966,7 +966,7 @@ Euler integrator, semi-implicit in velocity. self.assertEqual(mujoco.mjtEnableBit.mjENBL_OVERRIDE, 1 << 0) self.assertEqual(mujoco.mjtEnableBit.mjENBL_ENERGY, 1 << 1) self.assertEqual(mujoco.mjtEnableBit.mjENBL_FWDINV, 1 << 2) - self.assertEqual(mujoco.mjtEnableBit.mjNENABLE, 6) + self.assertEqual(mujoco.mjtEnableBit.mjNENABLE, 5) self.assertEqual(mujoco.mjtGeom.mjGEOM_PLANE, 0) self.assertEqual(mujoco.mjtGeom.mjGEOM_HFIELD, 1) self.assertEqual(mujoco.mjtGeom.mjGEOM_SPHERE, 2) @@ -1216,13 +1216,15 @@ Euler integrator, semi-implicit in velocity. mujoco.set_mjcb_control(lambda m, d: None) mujoco.mj_step(model_instances[-1], data_instances[-1]) mujoco.set_mjcb_control(None) + # Reference counting changed in Python 3.14. + expected_refcount = 2 if sys.version_info < (3, 14) else 1 while data_instances: d = data_instances.pop() - self.assertEqual(sys.getrefcount(d), 2) + self.assertEqual(sys.getrefcount(d), expected_refcount) del d while model_instances: m = model_instances.pop() - self.assertEqual(sys.getrefcount(m), 2) + self.assertEqual(sys.getrefcount(m), expected_refcount) # This test is disabled on PyPy as it uses sys.getrefcount # However PyPy is not officially supported by MuJoCo @@ -1236,7 +1238,9 @@ Euler integrator, semi-implicit in velocity. # passed to getrefcount. self.assertEqual(sys.getrefcount(data.model), 3) del data - self.assertEqual(sys.getrefcount(model), 2) + # Reference counting changed in Python 3.14. + expected_refcount = 2 if sys.version_info < (3, 14) else 1 + self.assertEqual(sys.getrefcount(model), expected_refcount) def test_can_initialize_mjv_structs(self): self.assertIsInstance(mujoco.MjvScene(), mujoco.MjvScene) diff --git a/python/mujoco/functions.cc b/python/mujoco/functions.cc index d6e0c724..03a1be1b 100644 --- a/python/mujoco/functions.cc +++ b/python/mujoco/functions.cc @@ -304,6 +304,7 @@ PYBIND11_MODULE(_functions, pymodule) { m, d, flg_acc, result.data()); }); Def(pymodule); + Def(pymodule); Def(pymodule); Def(pymodule); Def(pymodule); diff --git a/python/mujoco/introspect/enums.py b/python/mujoco/introspect/enums.py index d686e861..1ec77cb1 100644 --- a/python/mujoco/introspect/enums.py +++ b/python/mujoco/introspect/enums.py @@ -46,7 +46,8 @@ ENUMS: Mapping[str, EnumDecl] = dict([ ('mjDSBL_AUTORESET', 65536), ('mjDSBL_NATIVECCD', 131072), ('mjDSBL_ISLAND', 262144), - ('mjNDISABLE', 19), + ('mjDSBL_MULTICCD', 524288), + ('mjNDISABLE', 20), ]), )), ('mjtEnableBit', @@ -58,9 +59,8 @@ ENUMS: Mapping[str, EnumDecl] = dict([ ('mjENBL_ENERGY', 2), ('mjENBL_FWDINV', 4), ('mjENBL_INVDISCRETE', 8), - ('mjENBL_MULTICCD', 16), - ('mjENBL_SLEEP', 32), - ('mjNENABLE', 6), + ('mjENBL_SLEEP', 16), + ('mjNENABLE', 5), ]), )), ('mjtJoint', diff --git a/python/mujoco/introspect/enums_test.py b/python/mujoco/introspect/enums_test.py index d023fbcb..e373034b 100644 --- a/python/mujoco/introspect/enums_test.py +++ b/python/mujoco/introspect/enums_test.py @@ -42,9 +42,8 @@ class EnumsTest(absltest.TestCase): ('mjENBL_ENERGY', 1<<1), ('mjENBL_FWDINV', 1<<2), ('mjENBL_INVDISCRETE', 1<<3), - ('mjENBL_MULTICCD', 1<<4), - ('mjENBL_SLEEP', 1<<5), - ('mjNENABLE', 6))) + ('mjENBL_SLEEP', 1<<4), + ('mjNENABLE', 5))) # values mostly increment by one with occasional overrides def test_mjtGeom(self): # pylint: disable=invalid-name diff --git a/python/mujoco/introspect/functions.py b/python/mujoco/introspect/functions.py index f05bd8a5..e16f434f 100644 --- a/python/mujoco/introspect/functions.py +++ b/python/mujoco/introspect/functions.py @@ -162,6 +162,52 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ ), doc='Delete file from VFS; return 0: success, -1: not found in VFS.', )), + ('mj_containsBufferVFS', + FunctionDecl( + name='mj_containsBufferVFS', + return_type=ValueType(name='int'), + parameters=( + FunctionParameterDecl( + name='vfs', + type=PointerType( + inner_type=ValueType(name='mjVFS'), + ), + ), + FunctionParameterDecl( + name='name', + type=PointerType( + inner_type=ValueType(name='char', is_const=True), + ), + ), + ), + doc='Check if buffer exists in VFS; return 1: exists, 0: not found.', + )), + ('mj_containsFileVFS', + FunctionDecl( + name='mj_containsFileVFS', + return_type=ValueType(name='int'), + parameters=( + FunctionParameterDecl( + name='vfs', + type=PointerType( + inner_type=ValueType(name='mjVFS'), + ), + ), + FunctionParameterDecl( + name='directory', + type=PointerType( + inner_type=ValueType(name='char', is_const=True), + ), + ), + FunctionParameterDecl( + name='filename', + type=PointerType( + inner_type=ValueType(name='char', is_const=True), + ), + ), + ), + doc='Check if file exists in VFS; return 1: exists, 0: not found.', + )), ('mj_deleteVFS', FunctionDecl( name='mj_deleteVFS', @@ -2407,6 +2453,32 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ ), doc='RNE with complete data: compute cacc, cfrc_ext, cfrc_int.', )), + ('mj_maxContact', + FunctionDecl( + name='mj_maxContact', + return_type=ValueType(name='int'), + parameters=( + FunctionParameterDecl( + name='m', + type=PointerType( + inner_type=ValueType(name='mjModel', is_const=True), + ), + ), + FunctionParameterDecl( + name='g1', + type=ValueType(name='int'), + ), + FunctionParameterDecl( + name='g2', + type=ValueType(name='int'), + ), + FunctionParameterDecl( + name='has_margin', + type=ValueType(name='int'), + ), + ), + doc='Return the maximum number of contacts that can be generated between two geoms. If has_margin is -1, then the margin is pulled from the model, otherwise if has_margin > 0 indicates that the geoms have a positive margin.', # pylint: disable=line-too-long + )), ('mj_collision', FunctionDecl( name='mj_collision', diff --git a/simulate/simulate.cc b/simulate/simulate.cc index 522404b9..b4e29e35 100644 --- a/simulate/simulate.cc +++ b/simulate/simulate.cc @@ -1192,7 +1192,7 @@ void MakeJointSection(mj::Simulate* sim) { // set range if (sim->jnt_range_[i].has_value()) - mju::sprintf_arr(defSlider[0].other, "%.4g %.4g", + mju::sprintf_arr(defSlider[0].other, "%.17g %.17g", sim->jnt_range_[i]->first, sim->jnt_range_[i]->second); else if (sim->jnt_type_[i]==mjJNT_SLIDE) { mju::strcpy_arr(defSlider[0].other, "-1 1"); @@ -1251,7 +1251,7 @@ void MakeControlSection(mj::Simulate* sim) { // set range if (sim->actuator_ctrlrange_[i].has_value()) - mju::sprintf_arr(defSlider[0].other, "%.4g %.4g", + mju::sprintf_arr(defSlider[0].other, "%.17g %.17g", sim->actuator_ctrlrange_[i]->first, sim->actuator_ctrlrange_[i]->second); else { mju::strcpy_arr(defSlider[0].other, "-1 1"); diff --git a/src/engine/engine_collision_convex.c b/src/engine/engine_collision_convex.c index a847a099..46f4cf09 100644 --- a/src/engine/engine_collision_convex.c +++ b/src/engine/engine_collision_convex.c @@ -828,7 +828,7 @@ static int maxContacts(const mjModel* m, const mjCCDObj* obj1, const mjCCDObj* o // reduce mesh collisions to 4 contacts max if (type1 == mjGEOM_BOX || type1 == mjGEOM_MESH) { if (type2 == mjGEOM_BOX || type2 == mjGEOM_MESH) { - return mjENABLED(mjENBL_MULTICCD) ? 4 : 1; + return mjDISABLED(mjDSBL_MULTICCD) ? 1 : 4; } } @@ -857,7 +857,7 @@ int mjc_Convex(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtN } // look for additional contacts - if (ncon == 1 && mjENABLED(mjENBL_MULTICCD) // TODO(tassa) leave as bitflag or make geom attribute (?) + if (ncon == 1 && !mjDISABLED(mjDSBL_MULTICCD) // TODO(tassa) leave as bitflag or make geom attribute (?) && m->geom_type[g1] != mjGEOM_ELLIPSOID && m->geom_type[g1] != mjGEOM_SPHERE && m->geom_type[g2] != mjGEOM_ELLIPSOID && m->geom_type[g2] != mjGEOM_SPHERE) { // multiCCD parameters diff --git a/src/engine/engine_collision_driver.c b/src/engine/engine_collision_driver.c index b77164f6..ca271e5a 100644 --- a/src/engine/engine_collision_driver.c +++ b/src/engine/engine_collision_driver.c @@ -38,7 +38,7 @@ #include "engine/engine_util_spatial.h" -// table of pair-wise collision functions +// table of pairwise collision functions mjfCollision mjCOLLISIONFUNC[mjNGEOMTYPES][mjNGEOMTYPES] = { /* PLANE HFIELD SPHERE CAPSULE ELLIPSOID CYLINDER BOX MESH SDF */ /*PLANE */ {0, 0, mjc_PlaneSphere, mjc_PlaneCapsule, mjc_PlaneConvex, mjc_PlaneCylinder, mjc_PlaneBox, mjc_PlaneConvex, mjc_PlaneConvex}, @@ -56,6 +56,104 @@ mjfCollision mjCOLLISIONFUNC[mjNGEOMTYPES][mjNGEOMTYPES] = { //------------------------------------ utility functions ------------------------------------------ + +// return the maximum number of contacts that can be generated between two geoms +// if has_margin is -1, then the margin is pulled from the model, otherwise if has_margin > 0 +// indicates that the geoms have a positive margin +int mj_maxContact(const mjModel* m, int g1, int g2, int has_margin) { + int type1 = m->geom_type[g1]; + int type2 = m->geom_type[g2]; + + if (type1 == mjGEOM_SDF || type2 == mjGEOM_SDF) { + return m->opt.sdf_initpoints; + } + + if (type1 == mjGEOM_HFIELD || type2 == mjGEOM_HFIELD) { + int type = (type1 == mjGEOM_HFIELD) ? type2 : type1; + return (type != mjGEOM_PLANE && type != mjGEOM_HFIELD) ? mjMAXCONPAIR : 0; + } + + // spheres and ellipsoids always generate a single contact + if (type1 == mjGEOM_SPHERE || type1 == mjGEOM_ELLIPSOID || + type2 == mjGEOM_SPHERE || type2 == mjGEOM_ELLIPSOID) { + return 1; + } + + // box-box primitive collider + if (type1 == mjGEOM_BOX && type2 == mjGEOM_BOX) { + return 8; + } + + // capsule-capsule primitive collider + if (type1 == mjGEOM_CAPSULE && type2 == mjGEOM_CAPSULE) { + return 2; + } + + // capsule-box primitive collider + if ((type1 == mjGEOM_CAPSULE && type2 == mjGEOM_BOX) || + (type1 == mjGEOM_BOX && type2 == mjGEOM_CAPSULE)) { + return 4; + } + + // the remaining plane cases + if (type1 == mjGEOM_PLANE || type2 == mjGEOM_PLANE) { + int type = (type1 == mjGEOM_PLANE) ? type2 : type1; + switch (type) { + case mjGEOM_CAPSULE: + return 2; + case mjGEOM_CYLINDER: + case mjGEOM_BOX: + return 4; + case mjGEOM_MESH: + return 3; + default: + return 0; + } + } + + int is_multiccd = !mjDISABLED(mjDSBL_MULTICCD); + if (!is_multiccd) { + return 1; + } + + if (type1 == mjGEOM_CAPSULE || type2 == mjGEOM_CAPSULE || + type1 == mjGEOM_CYLINDER || type2 == mjGEOM_CYLINDER) { + return 5; + } + + if (mjDISABLED(mjDSBL_NATIVECCD)) { + return is_multiccd ? 5 : 1; // mesh-mesh or mesh-box with libccd + } + + // check margin from model + if (has_margin < 0) { + has_margin = 0; + if (mjENABLED(mjENBL_OVERRIDE)) { + has_margin = m->opt.o_margin > 0.0; + } else { + int npair = m->npair; + int ipair = -1; + for (int k=0; k < npair; k++) { + if ((m->pair_geom1[k] == g1 && m->pair_geom2[k] == g2) || + (m->pair_geom1[k] == g2 && m->pair_geom2[k] == g1)) { + ipair = k; + break; + } + } + + if (ipair > -1) { + has_margin = m->pair_margin[ipair] > 0.0; + } else { + has_margin = m->geom_margin[g1] > 0.0 || m->geom_margin[g2] > 0.0; + } + } + } + + // 4 contacts for mesh-mesh or mesh-box without margins, 5 with margins + return has_margin ? 5 : 4; +} + + // move arena pointer back to the end of the contact array static inline void resetArena(mjData* d) { d->parena = d->ncon * sizeof(mjContact); diff --git a/src/engine/engine_collision_driver.h b/src/engine/engine_collision_driver.h index 9583cf5f..e231eaaf 100644 --- a/src/engine/engine_collision_driver.h +++ b/src/engine/engine_collision_driver.h @@ -18,6 +18,7 @@ #include #include #include +#include #ifdef __cplusplus extern "C" { @@ -26,10 +27,15 @@ extern "C" { // collision function pointers and max contact pairs MJAPI extern mjfCollision mjCOLLISIONFUNC[mjNGEOMTYPES][mjNGEOMTYPES]; +// return the maximum number of contacts that can be generated between two geoms +// if has_margin is -1, then the margin is pulled from the model, otherwise if has_margin > 0 +// indicates that the geoms have a positive margin +MJAPI int mj_maxContact(const mjModel* m, int g1, int g2, int has_margin); + // collision detection entry point MJAPI void mj_collision(const mjModel* m, mjData* d); -// applies Separating Axis Theorem for rotated AABBs +// apply the Separating Axis Theorem for rotated AABBs MJAPI int mj_collideOBB(const mjtNum aabb1[6], const mjtNum aabb2[6], const mjtNum xpos1[3], const mjtNum xmat1[9], const mjtNum xpos2[3], const mjtNum xmat2[9], mjtNum margin, diff --git a/src/engine/engine_core_constraint.c b/src/engine/engine_core_constraint.c index 41ca6a39..39996c33 100644 --- a/src/engine/engine_core_constraint.c +++ b/src/engine/engine_core_constraint.c @@ -47,249 +47,85 @@ //-------------------------- utility functions ----------------------------------------------------- -// compute 3x3 matrix inverse, storing result in out -// assumes matrix is invertible (det != 0) -static void mat3_inverse(const mjtNum* mat, mjtNum* out) { - mjtNum det = mat[0]*(mat[4]*mat[8] - mat[5]*mat[7]) - - mat[1]*(mat[3]*mat[8] - mat[5]*mat[6]) + - mat[2]*(mat[3]*mat[7] - mat[4]*mat[6]); +// compute cell node Jacobians and combined chain for flex strain constraints +// npc: number of nodes per cell +// gindices: global indices of cell nodes in flex +// cell_node_jac: output array of size 3*npc*cell_nnz (allocated on stack) +// mj_{mark/free}Stack in calling function +static mjtNum* cell_pos_and_jac(const mjModel* m, mjData* d, int flex_id, int npc, const int* gindices, + int nv, const mjtNum* xpos_c, int* cell_chain, int* cell_nnz) { + int* nstart = m->flex_nodeadr + flex_id; + int* bodyid = m->flex_nodebodyid + *nstart; - out[0] = (mat[4]*mat[8] - mat[5]*mat[7]) / det; - out[1] = -(mat[1]*mat[8] - mat[2]*mat[7]) / det; - out[2] = (mat[1]*mat[5] - mat[2]*mat[4]) / det; - out[3] = -(mat[3]*mat[8] - mat[5]*mat[6]) / det; - out[4] = (mat[0]*mat[8] - mat[2]*mat[6]) / det; - out[5] = -(mat[0]*mat[5] - mat[2]*mat[3]) / det; - out[6] = (mat[3]*mat[7] - mat[4]*mat[6]) / det; - out[7] = -(mat[0]*mat[7] - mat[1]*mat[6]) / det; - out[8] = (mat[0]*mat[4] - mat[1]*mat[3]) / det; -} - - -// compute 3x3 matrix cofactor, storing result in out -static void mat3_cofactor(const mjtNum* mat, mjtNum* out) { - out[0] = mat[4]*mat[8] - mat[5]*mat[7]; - out[1] = -(mat[3]*mat[8] - mat[5]*mat[6]); - out[2] = mat[3]*mat[7] - mat[4]*mat[6]; - out[3] = -(mat[1]*mat[8] - mat[2]*mat[7]); - out[4] = mat[0]*mat[8] - mat[2]*mat[6]; - out[5] = -(mat[0]*mat[7] - mat[1]*mat[6]); - out[6] = mat[1]*mat[5] - mat[2]*mat[4]; - out[7] = -(mat[0]*mat[5] - mat[2]*mat[3]); - out[8] = mat[0]*mat[4] - mat[1]*mat[3]; -} - - -// compute 3x3 matrix determinant -static mjtNum mat3_det(const mjtNum* mat) { - return mat[0]*(mat[4]*mat[8] - mat[5]*mat[7]) - - mat[1]*(mat[3]*mat[8] - mat[5]*mat[6]) + - mat[2]*(mat[3]*mat[7] - mat[4]*mat[6]); -} - - -// compute node positions and Jacobians for flex strain constraints -// xpos: output array of size 3*nodenum (global node positions) -// node_jac: output array of size 3*nodenum*nv (dense Jacobians) -// combined_chain: output array of DOF indices used by any node (sparse mode) -// combined_nnz: output number of entries in combined_chain -static void node_pos_and_jac(const mjModel* m, mjData* d, int f, int nv, int issparse, mjtNum* xpos, - mjtNum* node_jac, int* combined_chain, int* combined_nnz) { - int nodenum = m->flex_nodenum[f]; - int* bodyid = m->flex_nodebodyid + m->flex_nodeadr[f]; - int nstart = m->flex_nodeadr[f]; - - for (int n = 0; n < nodenum; n++) { - if (m->flex_centered[f]) { - mju_copy3(xpos + 3*n, d->xpos + 3*bodyid[n]); - } else { - mju_mulMatVec3(xpos + 3*n, d->xmat + 9*bodyid[n], m->flex_node + 3*(n + nstart)); - mju_addTo3(xpos + 3*n, d->xpos + 3*bodyid[n]); + // build per-cell sparse chain: union of bodyChain for npc nodes + *cell_nnz = 0; + int* dof_used = mjSTACKALLOC(d, nv, int); + int* temp_chain = mjSTACKALLOC(d, nv, int); + mju_zeroInt(dof_used, nv); + for (int n = 0; n < npc; n++) { + int temp_nnz = mj_bodyChain(m, bodyid[gindices[n]], temp_chain); + for (int k = 0; k < temp_nnz; k++) { + dof_used[temp_chain[k]] = 1; + } + } + for (int q = 0; q < nv; q++) { + if (dof_used[q]) { + cell_chain[(*cell_nnz)++] = q; } } + // build per-cell node Jacobians: 3*npc x cell_nnz + mjtNum* cell_node_jac = mjSTACKALLOC(d, 3*npc*(*cell_nnz), mjtNum); + mju_zero(cell_node_jac, 3*npc*(*cell_nnz)); int* chain_col = mjSTACKALLOC(d, nv, int); mjtNum* blk_jac = mjSTACKALLOC(d, 3*nv, mjtNum); - mju_zero(node_jac, 3*nodenum*nv); - - for (int n = 0; n < nodenum; n++) { - int chain_nnz = mj_bodyChain(m, bodyid[n], chain_col); - mju_zero(blk_jac, 3*nv); - mj_jacSparse(m, d, blk_jac, NULL, xpos + 3*n, bodyid[n], chain_nnz, chain_col, 0); - + for (int n = 0; n < npc; n++) { + int body = bodyid[gindices[n]]; + int chain_n = mj_bodyChain(m, body, chain_col); + mju_zero(blk_jac, 3*chain_n); + mj_jacSparse(m, d, blk_jac, NULL, xpos_c + 3*n, + body, chain_n, chain_col, 0); + // map node's sparse chain into cell_chain indexing for (int r = 0; r < 3; r++) { - for (int k = 0; k < chain_nnz; k++) { - node_jac[(3*n + r)*nv + chain_col[k]] = blk_jac[r*chain_nnz + k]; + for (int k = 0; k < chain_n; k++) { + // find chain_col[k] in cell_chain via linear scan (chain is short) + for (int cc = 0; cc < *cell_nnz; cc++) { + if (cell_chain[cc] == chain_col[k]) { + cell_node_jac[(3*n + r)*(*cell_nnz) + cc] = blk_jac[r*chain_n + k]; + break; + } + } } } } - *combined_nnz = 0; - if (issparse) { - int* dof_used = mjSTACKALLOC(d, nv, int); - mju_zeroInt(dof_used, nv); - for (int n = 0; n < nodenum; n++) { - int temp_chain[200]; - int temp_nnz = mj_bodyChain(m, bodyid[n], temp_chain); - for (int k = 0; k < temp_nnz; k++) { - dof_used[temp_chain[k]] = 1; - } - } - - for (int q = 0; q < nv; q++) { - if (dof_used[q]) { - combined_chain[(*combined_nnz)++] = q; - } - } - } + return cell_node_jac; } -// compute strain Jacobian from strain derivative w.r.t. node positions -// dSdx: input array of size 3*nodenum (dStrain/dNodePosition) -// node_jac: input array of size 3*nodenum*nv (dense Jacobians) -// strain_jac: output array of size nv (dStrain/dq) -static void strain_jacobian(int nodenum, int nv, const mjtNum* dSdx, const mjtNum* node_jac, - mjtNum* strain_jac) { - mju_zero(strain_jac, nv); - for (int n = 0; n < nodenum; n++) { + +// compute strain Jacobian from strain derivative w.r.t. cell-local node positions +// dSdx_local: input array of size 3*npc (dStrain/dNodePosition for cell nodes) +// cell_node_jac: input array of size 3*npc*cell_nnz (sparse Jacobians) +// strain_jac: output array of size cell_nnz (dStrain/dq) +static void cell_strain_jacobian(int npc, int cell_nnz, + const mjtNum* dSdx_local, + const mjtNum* cell_node_jac, + mjtNum* strain_jac) { + mju_zero(strain_jac, cell_nnz); + for (int n = 0; n < npc; n++) { for (int c = 0; c < 3; c++) { + mjtNum w = dSdx_local[3*n + c]; + if (w == 0) continue; int row = 3*n + c; - for (int q = 0; q < nv; q++) { - strain_jac[q] += dSdx[row] * node_jac[row*nv + q]; + for (int k = 0; k < cell_nnz; k++) { + strain_jac[k] += w * cell_node_jac[row*cell_nnz + k]; } } } } -// basis functions for flex strain constraints -static void basis(int order, int i, mjtNum p, mjtNum* phi, mjtNum* dphi) { - if (order == 1) { - *phi = (i == 0 ? 1 - p : p); - *dphi = (i == 0 ? -1 : 1); - } else { - if (i == 0) { - *phi = 2 * p * p - 3 * p + 1; - *dphi = 4 * p - 3; - } else if (i == 1) { - *phi = 4 * (p - p * p); - *dphi = 4 * (1 - 2 * p); - } else { - *phi = 2 * p * p - p; - *dphi = 4 * p - 1; - } - } -} - - -// compute shape function gradients at a parametric point -// grad: output array of size nodenum x 3 (gradient w.r.t. parametric coords) -static void shape_gradients( - int order, const mjtNum* p, mjtNum grad[][3]) { - int npoint = (order + 1) * (order + 1) * (order + 1); - int stride = order + 1; - - for (int n = 0; n < npoint; n++) { - int ix = n / (stride * stride); - int iy = (n / stride) % stride; - int iz = n % stride; - - mjtNum phi_x, phi_y, phi_z, dphi_x, dphi_y, dphi_z; - basis(order, ix, p[0], &phi_x, &dphi_x); - basis(order, iy, p[1], &phi_y, &dphi_y); - basis(order, iz, p[2], &phi_z, &dphi_z); - - grad[n][0] = dphi_x * phi_y * phi_z; - grad[n][1] = phi_x * dphi_y * phi_z; - grad[n][2] = phi_x * phi_y * dphi_z; - } -} - - -// compute dStrain/dNodePosition for volumetric invariants (I1 or J-1) -// dSdx: output array of size 3*nodenum -static void volumetric_dSdx(int invariant_type, int nodenum, mjtNum grad[][3], - const mjtNum* F, const mjtNum* Fref_inv, mjtNum* dSdx) { - mju_zero(dSdx, 3*nodenum); - - if (invariant_type == 0) { - mjtNum dSdE[9] = {1.0, 0, 0, 0, 1.0, 0, 0, 0, 1.0}; - - for (int n = 0; n < nodenum; n++) { - for (int c = 0; c < 3; c++) { - mjtNum dS = 0; - for (int ij = 0; ij < 9; ij++) { - int ii = ij / 3; - int jj = ij % 3; - - mjtNum dF_ci = 0; - for (int k = 0; k < 3; k++) { - dF_ci += grad[n][k] * Fref_inv[k*3 + ii]; - } - mjtNum dF_cj = 0; - for (int k = 0; k < 3; k++) { - dF_cj += grad[n][k] * Fref_inv[k*3 + jj]; - } - - mjtNum dC_ij = dF_ci * F[c*3 + jj] + F[c*3 + ii] * dF_cj; - dS += dSdE[ij] * 0.5 * dC_ij; - } - dSdx[3*n + c] = dS; - } - } - } else { - mjtNum cofF[9]; - mat3_cofactor(F, cofF); - - for (int n = 0; n < nodenum; n++) { - for (int c = 0; c < 3; c++) { - mjtNum dJ = 0; - for (int b = 0; b < 3; b++) { - mjtNum dF_cb = 0; - for (int k = 0; k < 3; k++) { - dF_cb += grad[n][k] * Fref_inv[k*3 + b]; - } - dJ += cofF[c*3 + b] * dF_cb; - } - dSdx[3*n + c] = dJ; - } - } - } -} - - -// compute dStrain/dNodePosition for general strain invariants -// dSdx: output array of size 3*nodenum -static void invariant_dSdx(int nodenum, mjtNum grad[][3], const mjtNum* F, - const mjtNum* Fref_inv, const mjtNum* dSdE, mjtNum* dSdx) { - mju_zero(dSdx, 3*nodenum); - - for (int n = 0; n < nodenum; n++) { - for (int c = 0; c < 3; c++) { - mjtNum dS = 0; - for (int ij = 0; ij < 9; ij++) { - int ii = ij / 3; - int jj = ij % 3; - - mjtNum dF_ci = 0; - for (int k = 0; k < 3; k++) { - dF_ci += grad[n][k] * Fref_inv[k*3 + ii]; - } - mjtNum dF_cj = 0; - for (int k = 0; k < 3; k++) { - dF_cj += grad[n][k] * Fref_inv[k*3 + jj]; - } - - mjtNum dC_ij = dF_ci * F[c*3 + jj] + F[c*3 + ii] * dF_cj; - dS += dSdE[ij] * 0.5 * dC_ij; - } - dSdx[3*n + c] = dS; - } - } -} - - // allocate efc arrays on arena, return 1 on success, 0 on failure static int arenaAllocEfc(const mjModel* m, mjData* d) { #undef MJ_M @@ -439,6 +275,7 @@ static int mj_vertBodyWeight(const mjModel* m, const mjData* d, int f, int* v, } int order = m->flex_interp[f]; + order = order < 0 ? -order : order; int npc = (order+1)*(order+1)*(order+1); // number of nodes per cell // cell lookup: get local coords and node indices @@ -660,12 +497,12 @@ static void mj_equalityAnchors(const mjModel* m, const mjData* d, int eq_id, // equality constraints void mj_instantiateEquality(const mjModel* m, mjData* d) { int issparse = mj_isSparse(m), nv = m->nv; - int id[2], size, NV, NV2, *chain = NULL, *chain2 = NULL, *buf_ind = NULL; + int id[2], size, NV, NV2, *chain = NULL, *chain2 = NULL; int flex_edgeadr, flex_edgenum; int flex_vertadr, flex_vertnum; mjtNum cpos[6], pos[2][3], ref[2], dif, deriv; mjtNum quat[4], quat1[4], quat2[4], quat3[4], axis[3]; - mjtNum *jac[2], *jacdif, *data, *sparse_buf = NULL; + mjtNum *jac[2], *jacdif, *data; // disabled or no equality constraints: return if (mjDISABLED(mjDSBL_EQUALITY) || m->nemax == 0) { @@ -684,8 +521,6 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) { if (issparse) { chain = mjSTACKALLOC(d, nv, int); chain2 = mjSTACKALLOC(d, nv, int); - buf_ind = mjSTACKALLOC(d, nv, int); - sparse_buf = mjSTACKALLOC(d, nv, mjtNum); } // find active equality constraints @@ -853,8 +688,7 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) { // compute Jacobian: sparse or dense if (issparse) { - NV = mju_combineSparse(jac[0], jac[1], 1, -deriv, NV, NV2, chain, - chain2, sparse_buf, buf_ind); + NV = mju_combineSparse(jac[0], jac[1], 1, -deriv, NV, NV2, chain, chain2); } else { mju_addToScl(jac[0], jac[1], -deriv, nv); } @@ -872,217 +706,137 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) { break; case mjEQ_FLEXSTRAIN: { + // each constraint represents a single cell; cell index in eq_data int f = id[0]; int nodenum = m->flex_nodenum[f]; int order = m->flex_interp[f]; + order = order < 0 ? -order : order; // skip if not interpolated (order == 0 or no nodes) if (!order || !nodenum) { break; } + // only order 1 (trilinear) and 2 (quadratic) are supported + if (order > 2) { + mjERROR("flex strain constraints only support order 1 and 2, got %d", order); + } + int npc = (order+1)*(order+1)*(order+1); - int cx = m->flex_cellnum[3*f+0]; int cy = m->flex_cellnum[3*f+1]; int cz = m->flex_cellnum[3*f+2]; - - // allocate stack for node positions and Jacobians - mj_markStack(d); - mjtNum* xpos = mjSTACKALLOC(d, 3*nodenum, mjtNum); - mjtNum* node_jac = mjSTACKALLOC(d, 3*nodenum*nv, mjtNum); - int* combined_chain = mjSTACKALLOC(d, nv, int); - mjtNum* strain_jac = mjSTACKALLOC(d, nv, mjtNum); - int combined_nnz = 0; - - node_pos_and_jac(m, d, f, nv, issparse, xpos, node_jac, combined_chain, &combined_nnz); - - // Gauss-Legendre quadrature points in [0,1]^3 - // order=1: 2x2x2=8 points, order=2: 3x3x3=27 points - int nquad = order + 1; - int ngauss = nquad * nquad * nquad; - - // 1D Gauss points - mjtNum gp1d[3]; - if (nquad == 2) { - gp1d[0] = 0.5 - 0.5/mju_sqrt(3.0); - gp1d[1] = 0.5 + 0.5/mju_sqrt(3.0); - } else { - gp1d[0] = 0.5 - 0.5*mju_sqrt(0.6); - gp1d[1] = 0.5; - gp1d[2] = 0.5 + 0.5*mju_sqrt(0.6); - } - - // build 3D Gauss points array (max 27 points) - mjtNum gauss[27][3]; - for (int gi = 0; gi < nquad; gi++) { - for (int gj = 0; gj < nquad; gj++) { - for (int gk = 0; gk < nquad; gk++) { - int idx = gi*nquad*nquad + gj*nquad + gk; - gauss[idx][0] = gp1d[gi]; - gauss[idx][1] = gp1d[gj]; - gauss[idx][2] = gp1d[gk]; - } - } - } - - // reference positions for all nodes int nstart = m->flex_nodeadr[f]; - mjtNum* refpos = mjSTACKALLOC(d, 3*nodenum, mjtNum); - for (int n = 0; n < nodenum; n++) { - mju_copy3(refpos + 3*n, m->flex_node0 + 3*(n + nstart)); - } + int* bodyid = m->flex_nodebodyid + nstart; - // per-cell arrays + // read cell index from eq_data + int ci = (int)data[0]; + int cj = (int)data[1]; + int ck = (int)data[2]; + + mj_markStack(d); + + // get cell node indices + int gindices[125]; // max npc = 125 for quadratic + mju_flexGatherCellState(order, cy, cz, ci, cj, ck, + NULL, NULL, NULL, NULL, NULL, NULL, gindices, NULL); + + // compute positions only for cell nodes (npc << nodenum) mjtNum* xpos_c = mjSTACKALLOC(d, 3*npc, mjtNum); mjtNum* refpos_c = mjSTACKALLOC(d, 3*npc, mjtNum); + for (int n = 0; n < npc; n++) { + int gn = gindices[n]; + if (m->flex_centered[f] || + (m->flex_node[3*(gn + nstart)+0] == 0 && + m->flex_node[3*(gn + nstart)+1] == 0 && + m->flex_node[3*(gn + nstart)+2] == 0)) { + mju_copy3(xpos_c + 3*n, d->xpos + 3*bodyid[gn]); + } else { + mju_mulMatVec3(xpos_c + 3*n, d->xmat + 9*bodyid[gn], m->flex_node + 3*(gn + nstart)); + mju_addTo3(xpos_c + 3*n, d->xpos + 3*bodyid[gn]); + } + mju_copy3(refpos_c + 3*n, m->flex_node0 + 3*(gn + nstart)); + } + + // compute corotational quaternion from cell-local positions + mjtNum cell_quat[4] = {1, 0, 0, 0}; + { + mjtNum center[3] = {0.5, 0.5, 0.5}; + mjtNum mat[9]; + mju_defGradient(mat, center, xpos_c, order); + mju_mat2Rot(cell_quat, mat); + mju_negQuat(cell_quat, cell_quat); + } + + // build per-cell sparse chain and node Jacobians + int* cell_chain = mjSTACKALLOC(d, nv, int); + int cell_nnz = 0; + mjtNum* cell_node_jac = cell_pos_and_jac(m, d, f, npc, gindices, nv, xpos_c, cell_chain, + &cell_nnz); + + + mjtNum* strain_jac = mjSTACKALLOC(d, cell_nnz, mjtNum); mjtNum* dSdx_local = mjSTACKALLOC(d, 3*npc, mjtNum); - mjtNum* dSdx = mjSTACKALLOC(d, 3*nodenum, mjtNum); - int gindices[125]; // max npc = 125 for quadratic - // loop over cells - for (int ci = 0; ci < cx; ci++) { - for (int cj = 0; cj < cy; cj++) { - for (int ck = 0; ck < cz; ck++) { - // gather cell-local node positions - mju_flexGatherCellState(order, cy, cz, ci, cj, ck, xpos, NULL, refpos, xpos_c, NULL, - refpos_c, gindices, NULL); + // for dense mode: allocate and zero a dense Jacobian buffer once + mjtNum* dense_jac = NULL; + if (!issparse) { + dense_jac = mjSTACKALLOC(d, nv, mjtNum); + mju_zero(dense_jac, nv); + } - // B-bar: center-point volumetric constraints (trilinear) - if (order == 1) { - mjtNum center[3] = {0.5, 0.5, 0.5}; - mjtNum Fcur_c[9], Fref_c[9], Fref_inv_c[9], F_c[9]; + // read eigenmode data from flex_stiffness + int ndof_cell = 3 * npc; + int cell_idx = ci * m->flex_cellnum[3*f+1] * m->flex_cellnum[3*f+2] + + cj * m->flex_cellnum[3*f+2] + ck; + const mjtNum* k_cell = m->flex_stiffness + m->flex_stiffnessadr[f] + + cell_idx * ndof_cell * ndof_cell; + int neig = (int)k_cell[0]; - mju_defGradient(Fcur_c, center, xpos_c, order); - mju_defGradient(Fref_c, center, refpos_c, order); - mat3_inverse(Fref_c, Fref_inv_c); - mju_mulMatMat3(F_c, Fcur_c, Fref_inv_c); + // compute displacement in corotational frame + mjtNum* displ_c = mjSTACKALLOC(d, ndof_cell, mjtNum); + for (int n = 0; n < npc; n++) { + // rotate xpos_c to corotational frame + mjtNum xrot[3]; + mju_rotVecQuat(xrot, xpos_c + 3*n, cell_quat); + displ_c[3*n + 0] = xrot[0] - refpos_c[3*n + 0]; + displ_c[3*n + 1] = xrot[1] - refpos_c[3*n + 1]; + displ_c[3*n + 2] = xrot[2] - refpos_c[3*n + 2]; + } - mjtNum C_c[9], E_c[9]; - mju_mulMatTMat3(C_c, F_c, F_c); - mju_scl(E_c, C_c, 0.5, 9); - E_c[0] -= 0.5; E_c[4] -= 0.5; E_c[8] -= 0.5; + // compute inverse quaternion for rotating eigenvectors to world frame + mjtNum cell_quat_inv[4]; + mju_negQuat(cell_quat_inv, cell_quat); - mjtNum I1_c = E_c[0] + E_c[4] + E_c[8]; - mjtNum J_c = mat3_det(F_c); + // loop over eigenmodes + for (int eig = 0; eig < neig; eig++) { + const mjtNum* eigvec = k_cell + 1 + eig * ndof_cell; - mjtNum grad_c[8][3]; - shape_gradients(order, center, grad_c); + // constraint residual: dot product of scaled eigenvector with displacement + mjtNum residual = 0; + for (int j = 0; j < ndof_cell; j++) { + residual += eigvec[j] * displ_c[j]; + } + cpos[0] = residual; - for (int inv = 0; inv < 2; inv++) { - cpos[0] = (inv == 0) ? I1_c : J_c - 1.0; + // rotate eigenvector to world frame for Jacobian + // dSdx_local[3*n+c] = Σ_d R_inv[c][d] * eigvec[3*n+d] + for (int n = 0; n < npc; n++) { + mju_rotVecQuat(dSdx_local + 3*n, eigvec + 3*n, cell_quat_inv); + } - // compute local dSdx - volumetric_dSdx(inv, npc, grad_c, F_c, Fref_inv_c, dSdx_local); + // contract with cell_node_jac to get sparse Jacobian + cell_strain_jacobian(npc, cell_nnz, dSdx_local, cell_node_jac, strain_jac); - // scatter to global dSdx - mju_zero(dSdx, 3*nodenum); - for (int n = 0; n < npc; n++) { - mju_addTo3(dSdx + 3*gindices[n], dSdx_local + 3*n); - } - - strain_jacobian(nodenum, nv, dSdx, node_jac, strain_jac); - - if (issparse) { - mj_markStack(d); - mjtNum* sj = mjSTACKALLOC(d, combined_nnz, mjtNum); - for (int k = 0; k < combined_nnz; k++) { - sj[k] = strain_jac[combined_chain[k]]; - } - mj_addConstraint(m, d, sj, cpos, 0, 0, 1, mjCNSTR_EQUALITY, i, - combined_nnz, combined_chain); - mj_freeStack(d); - } else { - mj_addConstraint(m, d, strain_jac, cpos, 0, 0, 1, mjCNSTR_EQUALITY, i, 0, NULL); - } - } - } - - // Gauss integration per cell - for (int g = 0; g < ngauss; g++) { - mjtNum* p = gauss[g]; - - // F = Fcur * Fref_inv - mjtNum Fcur[9], Fref[9], Fref_inv[9], F[9]; - mju_defGradient(Fcur, p, xpos_c, order); - mju_defGradient(Fref, p, refpos_c, order); - mat3_inverse(Fref, Fref_inv); - mju_mulMatMat3(F, Fcur, Fref_inv); - - // Green-Lagrange strain E = 0.5*(C - I) - mjtNum C[9], E[9]; - mju_mulMatTMat3(C, F, F); - for (int j = 0; j < 9; j++) { - E[j] = 0.5 * C[j]; - } - E[0] -= 0.5; E[4] -= 0.5; E[8] -= 0.5; - - // 3 invariants of E - mjtNum I1 = E[0] + E[4] + E[8]; - mjtNum trE2 = E[0]*E[0] + E[1]*E[3] + E[2]*E[6] - + E[3]*E[1] + E[4]*E[4] + E[5]*E[7] - + E[6]*E[2] + E[7]*E[5] + E[8]*E[8]; - mjtNum I2 = 0.5 * (I1*I1 - trE2); - mjtNum I3 = mat3_det(E); - - // shape function gradients at Gauss point - mjtNum grad[27][3]; - shape_gradients(order, p, grad); - - for (int s = 0; s < 6; s++) { - // skip I1,I2,I3 for trilinear (B-bar handles vol) - if (order == 1 && (s == 0 || s == 1 || s == 2)) { - continue; - } - - mjtNum dSdE[9]; - mju_zero(dSdE, 9); - - if (s == 0) { - cpos[0] = I1; - dSdE[0] = dSdE[4] = dSdE[8] = 1.0; - } else if (s == 1) { - cpos[0] = I2; - dSdE[0] = I1-E[0]; dSdE[4] = I1-E[4]; - dSdE[8] = I1-E[8]; - dSdE[1] = -E[1]; dSdE[3] = -E[3]; - dSdE[2] = -E[2]; dSdE[6] = -E[6]; - dSdE[5] = -E[5]; dSdE[7] = -E[7]; - } else if (s == 2) { - cpos[0] = I3; - mat3_cofactor(E, dSdE); - } else { - int offdiag_idx[3] = {1, 2, 5}; - int ij = offdiag_idx[s - 3]; - cpos[0] = E[ij]; - dSdE[ij] = 1.0; - } - - // compute local dS/dx for cell nodes - invariant_dSdx(npc, grad, F, Fref_inv, dSdE, - dSdx_local); - - // scatter to global dSdx - mju_zero(dSdx, 3*nodenum); - for (int n = 0; n < npc; n++) { - mju_addTo3(dSdx + 3*gindices[n], dSdx_local + 3*n); - } - - strain_jacobian(nodenum, nv, dSdx, node_jac, strain_jac); - - if (issparse) { - mj_markStack(d); - mjtNum* sj = mjSTACKALLOC(d, combined_nnz, mjtNum); - for (int k = 0; k < combined_nnz; k++) { - sj[k] = strain_jac[combined_chain[k]]; - } - mj_addConstraint(m, d, sj, cpos, 0, 0, 1, mjCNSTR_EQUALITY, i, - combined_nnz, combined_chain); - mj_freeStack(d); - } else { - mj_addConstraint(m, d, strain_jac, cpos, 0, 0, 1, mjCNSTR_EQUALITY, i, 0, NULL); - } - } - } + if (issparse) { + mj_addConstraint(m, d, strain_jac, cpos, 0, 0, 1, mjCNSTR_EQUALITY, i, + cell_nnz, cell_chain); + } else { + for (int k = 0; k < cell_nnz; k++) { + dense_jac[cell_chain[k]] = strain_jac[k]; + } + mj_addConstraint(m, d, dense_jac, cpos, 0, 0, 1, mjCNSTR_EQUALITY, i, 0, NULL); + for (int k = 0; k < cell_nnz; k++) { + dense_jac[cell_chain[k]] = 0; } } } @@ -1920,26 +1674,36 @@ void mj_diagApprox(const mjModel* m, mjData* d) { break; case mjEQ_FLEXSTRAIN: { - // strain constraints: use average node inv weight + // strain constraints: per-cell, use avg inv weight of cell's npc nodes int flex_id = m->eq_obj1id[id]; - int nodenum = m->flex_nodenum[flex_id]; int nstart = m->flex_nodeadr[flex_id]; int order = m->flex_interp[flex_id]; + order = order < 0 ? -order : order; + int npc = (order+1)*(order+1)*(order+1); - // compute constraint count per cell, then multiply by ncells + // per-cell constraint count int nquad = order + 1; int ngauss = nquad * nquad * nquad; - int ncells = m->flex_cellnum[3*flex_id+0] - * m->flex_cellnum[3*flex_id+1] - * m->flex_cellnum[3*flex_id+2]; - int nconstraint = ncells * ((order == 1) ? (2 + 3 * ngauss) : (6 * ngauss)); + int nconstraint = (order == 1) ? (2 + 3 * ngauss) : (6 * ngauss); + + // get cell index from eq_data + int eq_id = d->efc_id[i]; + int ci_cell = (int)m->eq_data[mjNEQDATA*eq_id + 0]; + int cj_cell = (int)m->eq_data[mjNEQDATA*eq_id + 1]; + int ck_cell = (int)m->eq_data[mjNEQDATA*eq_id + 2]; + int cy = m->flex_cellnum[3*flex_id+1]; + int cz = m->flex_cellnum[3*flex_id+2]; + + int gindices[125]; + mju_flexGatherCellState(order, cy, cz, ci_cell, cj_cell, ck_cell, + NULL, NULL, NULL, NULL, NULL, NULL, gindices, NULL); mjtNum avg_invweight = 0; - for (int n = 0; n < nodenum; n++) { - int bodyid = m->flex_nodebodyid[nstart + n]; + for (int n = 0; n < npc; n++) { + int bodyid = m->flex_nodebodyid[nstart + gindices[n]]; avg_invweight += m->body_invweight0[2*bodyid]; } - avg_invweight /= nodenum; + avg_invweight /= npc; for (int c = 0; c < nconstraint; c++) { dA[i++] = avg_invweight; } @@ -2404,6 +2168,9 @@ static int mj_ne(const mjModel* m, mjData* d, int* nnz) { chain2 = mjSTACKALLOC(d, nv, int); } + // pre-allocate buffer for cell body IDs (max npc = 125 for order=2) + int* cell_bodies = nnz ? mjSTACKALLOC(d, 125, int) : NULL; + // find active equality constraints for (int i=0; i < neq; i++) { // skip inactive @@ -2529,34 +2296,37 @@ static int mj_ne(const mjModel* m, mjData* d, int* nnz) { break; case mjEQ_FLEXSTRAIN: { - // strain constraints: - // Q1: B-bar, 2 center (I1, J-1) + 3*8 shear = 26 - // Q2: full 3x3x3 Gauss, 6*27 = 162 - // skip if not interpolated (order == 0 or no nodes) - int order = m->flex_interp[id[0]]; - int nodenum = m->flex_nodenum[id[0]]; - if (!order || !nodenum) { + // per-cell strain constraints: each equality is one cell + int f = id[0]; + int order = m->flex_interp[f]; + order = order < 0 ? -order : order; + if (!order || !m->flex_nodenum[f]) { break; } - int nquad = order + 1; // 2 for order=1, 3 for order=2 - int ngauss = nquad * nquad * nquad; // 8 or 27 - int ncells = m->flex_cellnum[3*id[0]+0] - * m->flex_cellnum[3*id[0]+1] - * m->flex_cellnum[3*id[0]+2]; - size = ncells * ((order == 1) ? (2 + 3 * ngauss) : (6 * ngauss)); + int npc = (order+1)*(order+1)*(order+1); + + // read eigenmode count from flex_stiffness + int ndof_cell = 3 * npc; + int ci_cell = (int)m->eq_data[mjNEQDATA*i + 0]; + int cj_cell = (int)m->eq_data[mjNEQDATA*i + 1]; + int ck_cell = (int)m->eq_data[mjNEQDATA*i + 2]; + int cy = m->flex_cellnum[3*f+1]; + int cz = m->flex_cellnum[3*f+2]; + int cell_idx = ci_cell * cy * cz + cj_cell * cz + ck_cell; + const mjtNum* k_cell = m->flex_stiffness + m->flex_stiffnessadr[f] + + cell_idx * ndof_cell * ndof_cell; + size = (int)k_cell[0]; // neig stored as first element if (nnz) { - // Count unique DOFs across all node bodies (matching instantiation) - int nstart = m->flex_nodeadr[id[0]]; - int* nodebodies = mjSTACKALLOC(d, nodenum, int); - for (int n = 0; n < nodenum; n++) { - nodebodies[n] = m->flex_nodebodyid[nstart + n]; + // get the npc node body IDs for this cell + int gindices[125]; + mju_flexGatherCellState(order, cy, cz, ci_cell, cj_cell, ck_cell, + NULL, NULL, NULL, NULL, NULL, NULL, gindices, NULL); + int nstart = m->flex_nodeadr[f]; + for (int n = 0; n < npc; n++) { + cell_bodies[n] = m->flex_nodebodyid[nstart + gindices[n]]; } - - // mj_jacSumCount deduplicates shared DOFs - NV = mj_jacSumCount(m, d, chain, nodenum, nodebodies); - - // each constraint row shares this combined NV + NV = mj_jacSumCount(m, d, chain, npc, cell_bodies); // npc nodes only NV = size * NV; } break; diff --git a/src/engine/engine_core_smooth.c b/src/engine/engine_core_smooth.c index 8f3412f1..164a6527 100644 --- a/src/engine/engine_core_smooth.c +++ b/src/engine/engine_core_smooth.c @@ -564,16 +564,13 @@ void mj_flex(const mjModel* m, mjData* d) { // 0: vertices are the mesh vertices, 1: vertices are interpolated from nodal dofs if (m->flex_interp[f] == 0) { - // centered: copy body position - if (m->flex_centered[f]) { - for (int i=vstart; i < vend; i++) { + for (int i=vstart; i < vend; i++) { + if (m->flex_centered[f] || + (m->flex_vert[3*i+0] == 0 && + m->flex_vert[3*i+1] == 0 && + m->flex_vert[3*i+2] == 0)) { mji_copy3(d->flexvert_xpos+3*i, d->xpos+3*m->flex_vertbodyid[i]); - } - } - - // non-centered: map from local to global - else { - for (int i=vstart; i < vend; i++) { + } else { mji_mulMatVec3(d->flexvert_xpos+3*i, d->xmat+9*m->flex_vertbodyid[i], m->flex_vert+3*i); mji_addTo3(d->flexvert_xpos+3*i, d->xpos+3*m->flex_vertbodyid[i]); } @@ -585,19 +582,21 @@ void mj_flex(const mjModel* m, mjData* d) { int nodenum = nend - nstart; mj_markStack(d); mjtNum* nodexpos = mjSTACKALLOC(d, 3*nodenum, mjtNum); - if (m->flex_centered[f]) { - for (int i=nstart; i < nend; i++) { - mji_copy3(nodexpos + 3*(i-nstart), d->xpos + 3*m->flex_nodebodyid[i]); - } - } else { - for (int i=nstart; i < nend; i++) { - int j = i - nstart; + for (int i=nstart; i < nend; i++) { + int j = i - nstart; + if (m->flex_centered[f] || + (m->flex_node[3*i+0] == 0 && + m->flex_node[3*i+1] == 0 && + m->flex_node[3*i+2] == 0)) { + mji_copy3(nodexpos + 3*j, d->xpos + 3*m->flex_nodebodyid[i]); + } else { mji_mulMatVec3(nodexpos + 3*j, d->xmat + 9*m->flex_nodebodyid[i], m->flex_node + 3*i); mji_addTo3(nodexpos + 3*j, d->xpos + 3*m->flex_nodebodyid[i]); } } - int order = m->flex_interp[f]; + int interp = m->flex_interp[f]; + int order = interp < 0 ? -interp : interp; int cx = m->flex_cellnum[3*f+0]; int cy = m->flex_cellnum[3*f+1]; int cz = m->flex_cellnum[3*f+2]; @@ -2626,7 +2625,8 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) { case mjEQ_FLEXSTRAIN: { // increment: trilinear uses 2 center (I1,J-1) + 3*ngauss shear, quadratic uses 6*ngauss k = m->eq_obj1id[id]; - int order = m->flex_interp[k]; + int interp_k = m->flex_interp[k]; + int order = interp_k < 0 ? -interp_k : interp_k; int nodenum = m->flex_nodenum[k]; if (order && nodenum) { int nquad = order + 1; diff --git a/src/engine/engine_core_util.c b/src/engine/engine_core_util.c index bf2b6994..52c5d46f 100644 --- a/src/engine/engine_core_util.c +++ b/src/engine/engine_core_util.c @@ -988,28 +988,36 @@ void mj_local2Global(mjData* d, mjtNum xpos[3], mjtNum xmat[9], //-------------------------- miscellaneous utilities ----------------------------------------------- // gather global node positions and velocities -void mju_flexGatherState(const mjModel* m, mjData* d, int f, mjtNum* xpos, mjtNum* vel) { +void mju_flexGatherState(const mjModel* m, const mjData* d, int f, mjtNum* xpos, mjtNum* vel) { int nodenum = m->flex_nodenum[f]; int nstart = m->flex_nodeadr[f]; int* bodyid = m->flex_nodebodyid + m->flex_nodeadr[f]; - // compute positions - if (m->flex_centered[f]) { - for (int i=0; i < nodenum; i++) { - mju_copy3(xpos + 3*i, d->xpos + 3*bodyid[i]); - if (vel) { - mju_copy3(vel + 3*i, d->qvel + m->body_dofadr[bodyid[i]]); - } + // compute positions and velocities + for (int i=0; i < nodenum; i++) { + int bid = bodyid[i]; + if (m->flex_centered[f] || + (m->flex_node[3*(i+nstart)+0] == 0 && + m->flex_node[3*(i+nstart)+1] == 0 && + m->flex_node[3*(i+nstart)+2] == 0)) { + mju_copy3(xpos + 3*i, d->xpos + 3*bid); + } else { + mju_mulMatVec3(xpos + 3*i, d->xmat + 9*bid, m->flex_node + 3*(i+nstart)); + mju_addTo3(xpos + 3*i, d->xpos + 3*bid); } - } else { - mjtNum screw[6]; - for (int i=0; i < nodenum; i++) { - mju_mulMatVec3(xpos + 3*i, d->xmat + 9*bodyid[i], m->flex_node + 3*(i+nstart)); - mju_addTo3(xpos + 3*i, d->xpos + 3*bodyid[i]); - if (vel) { - mj_objectVelocity(m, d, mjOBJ_BODY, bodyid[i], screw, 0); - mju_copy3(vel + 3*i, screw + 3); - } + + if (vel) { + mjtNum body_vel[6]; + mj_objectVelocity(m, d, mjOBJ_BODY, bid, body_vel, 0); // returns [omega, v_CoM] in world frame + + // linear velocity at CoM + mju_copy3(vel + 3*i, body_vel + 3); + + // add omega x (xpos - xipos) + mjtNum r[3], cross[3]; + mju_sub3(r, xpos + 3*i, d->xipos + 3*bid); + mju_cross(cross, body_vel, r); + mju_addTo3(vel + 3*i, cross); } } } diff --git a/src/engine/engine_core_util.h b/src/engine/engine_core_util.h index 949aa257..36ec5fd0 100644 --- a/src/engine/engine_core_util.h +++ b/src/engine/engine_core_util.h @@ -130,7 +130,7 @@ MJAPI void mj_local2Global(mjData* d, mjtNum xpos[3], mjtNum xmat[9], //-------------------------- miscellaneous --------------------------------------------------------- // gather global node positions and velocities -MJAPI void mju_flexGatherState(const mjModel* m, mjData* d, int f, mjtNum* xpos, mjtNum* vel); +MJAPI void mju_flexGatherState(const mjModel* m, const mjData* d, int f, mjtNum* xpos, mjtNum* vel); // extract 6D force:torque for one contact, in contact frame MJAPI void mj_contactForce(const mjModel* m, const mjData* d, int id, mjtNum result[6]); diff --git a/src/engine/engine_derivative.c b/src/engine/engine_derivative.c index 8a7eabc7..60c80073 100644 --- a/src/engine/engine_derivative.c +++ b/src/engine/engine_derivative.c @@ -880,17 +880,63 @@ static void mjd_flexInterp_kernel(const mjModel* m, mjData* d, mjtFlexOp op, const int* dof_indices, int ndof, int nband) { int nv = m->nv; - // build global2local map for ADDH - int* global2local = NULL; + // compute upper bounds across all interpolated flexes + int max_nodenum = 0; + int max_npc = 0; + for (int f = 0; f < m->nflex; f++) { + if (!m->flex_interp[f]) continue; + if (m->flex_rigid[f]) continue; + int order = m->flex_interp[f]; + order = order < 0 ? -order : order; + int npc = (order+1)*(order+1)*(order+1); + if (npc > max_npc) max_npc = npc; + if (m->flex_nodenum[f] > max_nodenum) max_nodenum = m->flex_nodenum[f]; + } + + // nothing to do + if (max_npc == 0) { + return; + } + + int max_dim_c = 3 * max_npc; + + // single unconditional markStack + mj_markStack(d); + + // global2local map for ADDH + int* global2local = mjSTACKALLOC(d, nv, int); if (op == mjFLEXOP_ADDH) { - mj_markStack(d); - global2local = mjSTACKALLOC(d, nv, int); mju_fillInt(global2local, -1, nv); for (int i=0; inflex; f++) { // only process flex_interp @@ -899,10 +945,15 @@ static void mjd_flexInterp_kernel(const mjModel* m, mjData* d, mjtFlexOp op, } // get stiffness and damping - mjtNum* k = m->flex_stiffness + m->flex_stiffnessadr[f]; + mjtNum* K = m->flex_stiffness + m->flex_stiffnessadr[f]; // skip if rigid or no stiffness - if (m->flex_rigid[f] || k[0] == 0) { + if (m->flex_rigid[f] || K[0] == 0) { + continue; + } + + // skip if strain constraints present (stiffness handled by constraint solver) + if (m->flex_edgeequality[f] == 3) { continue; } @@ -916,32 +967,15 @@ static void mjd_flexInterp_kernel(const mjModel* m, mjData* d, mjtFlexOp op, } int order = m->flex_interp[f]; + order = order < 0 ? -order : order; int npc = (order+1)*(order+1)*(order+1); int cx = m->flex_cellnum[3*f+0]; int cy = m->flex_cellnum[3*f+1]; int cz = m->flex_cellnum[3*f+2]; - int nodenum = m->flex_nodenum[f]; int* bodyid = m->flex_nodebodyid + m->flex_nodeadr[f]; - // standard stack allocation - mj_markStack(d); - mjtNum* xpos = mjSTACKALLOC(d, 3*nodenum, mjtNum); - - // per-cell arrays int dim_c = 3 * npc; - mjtNum* xpos_c = mjSTACKALLOC(d, 3*npc, mjtNum); - mjtNum* K_rot_cell = mjSTACKALLOC(d, dim_c*dim_c, mjtNum); - - // sparse Jacobian for one cell - int* J_rownnz = mjSTACKALLOC(d, dim_c, int); - int* J_rowadr = mjSTACKALLOC(d, dim_c, int); - mjtNum* J_val = mjSTACKALLOC(d, dim_c*nv, mjtNum); - int* J_colind = mjSTACKALLOC(d, dim_c*nv, int); - - // temp allocations for chain - int* chain_colind = mjSTACKALLOC(d, nv, int); - mjtNum* blk_jac = mjSTACKALLOC(d, 3*nv, mjtNum); // gather raw node positions (unrotated) mju_flexGatherState(m, d, f, xpos, NULL); @@ -951,6 +985,16 @@ static void mjd_flexInterp_kernel(const mjModel* m, mjData* d, mjtFlexOp op, for (int ci = 0; ci < cx; ci++) { for (int cj = 0; cj < cy; cj++) { for (int ck = 0; ck < cz; ck++) { + // get cell stiffness + mjtNum* k_cell = K + cell_idx * 3*npc * 3*npc; + + // skip empty cells: stiffness buffer is zero-initialized at compile time + // (user_model.cc), and non-empty cells have strictly positive diagonal + if (k_cell[0] == 0) { + cell_idx++; + continue; + } + // gather cell-local node positions int gindices[125]; // max npc = 125 for quadratic mjtNum quat[4]; @@ -962,9 +1006,6 @@ static void mjd_flexInterp_kernel(const mjModel* m, mjData* d, mjtFlexOp op, mju_quat2Mat(R, quat); mju_transpose(RT, R, 3, 3); - // get cell stiffness - mjtNum* k_cell = k + cell_idx * 3*npc * 3*npc; - // compute K_rot_cell = RT * K_cell * R (block-wise) mju_zero(K_rot_cell, dim_c*dim_c); for (int a = 0; a < npc; a++) { @@ -1020,9 +1061,7 @@ static void mjd_flexInterp_kernel(const mjModel* m, mjData* d, mjtFlexOp op, addJTBJ_mulSparse(m, d, res, vec, J_rownnz, J_rowadr, J_colind, J_val, K_rot_cell, dim_c); } else if (op == mjFLEXOP_ADDH) { - mj_markStack(d); // H -= J_cell^T * K_rot_cell * J_cell (banded format) - mjtNum* J_reduced = mjSTACKALLOC(d, dim_c*ndof, mjtNum); mju_zero(J_reduced, dim_c*ndof); for (int i = 0; i < dim_c; i++) { @@ -1038,7 +1077,6 @@ static void mjd_flexInterp_kernel(const mjModel* m, mjData* d, mjtFlexOp op, } // KJ = K_rot_cell * J_reduced (dim_c x ndof) - mjtNum* KJ = mjSTACKALLOC(d, dim_c*ndof, mjtNum); mju_mulMatMat(KJ, K_rot_cell, J_reduced, dim_c, dim_c, ndof); // H[i,j] -= J_reduced[k,i] * KJ[k,j], store lower triangle in banded format @@ -1051,20 +1089,15 @@ static void mjd_flexInterp_kernel(const mjModel* m, mjData* d, mjtFlexOp op, res[i*nband + nband-1-(i-j)] -= val; } } - mj_freeStack(d); } cell_idx++; } } } - - mj_freeStack(d); } - if (op == mjFLEXOP_ADDH) { - mj_freeStack(d); // free global2local - } + mj_freeStack(d); } diff --git a/src/engine/engine_passive.c b/src/engine/engine_passive.c index e6d3b6fa..295ec4a7 100644 --- a/src/engine/engine_passive.c +++ b/src/engine/engine_passive.c @@ -229,8 +229,14 @@ static void mj_springdamper(const mjModel* m, mjData* d) { continue; } + // skip interpolated flex with strain constraints (stiffness in constraint solver) + if (m->flex_edgeequality[f] == 3) { + continue; + } + if (m->flex_interp[f]) { int order = m->flex_interp[f]; + order = order < 0 ? -order : order; int npc = (order+1)*(order+1)*(order+1); // nodes per cell int cx = m->flex_cellnum[3*f+0]; int cy = m->flex_cellnum[3*f+1]; @@ -268,16 +274,23 @@ static void mj_springdamper(const mjModel* m, mjData* d) { for (int ci = 0; ci < cx; ci++) { for (int cj = 0; cj < cy; cj++) { for (int ck = 0; ck < cz; ck++) { + // get cell stiffness matrix + mjtNum* k_cell = k + cell_idx * 3*npc * 3*npc; + + // skip empty cells (zero stiffness) + if (k_cell[0] == 0) { + cell_idx++; + continue; + } + // gather cell-local node data mjtNum quat[4]; - mjtNum p[3] = {.5, .5, .5}; mju_flexGatherCellState(order, cy, cz, ci, cj, ck, xpos_g, vel_g, xpos0, xpos_c, vel_c, xpos0_c, NULL, quat); // rotate to corotational frame for (int n = 0; n < npc; n++) { mju_rotVecQuat(xpos_c+3*n, xpos_c+3*n, quat); - mji_addTo3(xpos_c+3*n, p); mju_rotVecQuat(vel_c+3*n, vel_c+3*n, quat); } @@ -286,9 +299,6 @@ static void mj_springdamper(const mjModel* m, mjData* d) { mji_addScl3(displ_c+3*n, xpos_c+3*n, xpos0_c+3*n, -1); } - // get cell stiffness matrix - mjtNum* k_cell = k + cell_idx * 3*npc * 3*npc; - // compute force in corotational frame if (enbl_spring) { mju_mulMatVec(frc_c, k_cell, displ_c, 3*npc, 3*npc); @@ -329,12 +339,20 @@ static void mj_springdamper(const mjModel* m, mjData* d) { // apply accumulated forces to bodies for (int i = 0; i < nodenum; i++) { mju_scl3(dmp_g+3*i, dmp_g+3*i, m->flex_damping[f]); - if (m->flex_centered[f]) { - if (enbl_spring) mji_addTo3(d->qfrc_spring + m->body_dofadr[bodyid[i]], frc_g+3*i); - if (enbl_damper) mji_addTo3(d->qfrc_damper + m->body_dofadr[bodyid[i]], dmp_g+3*i); + int bid = bodyid[i]; + int nidx = i + m->flex_nodeadr[f]; + + // fast path: node at body origin (not pinned), direct DOF write + if (m->body_dofnum[bid] > 0 && + (m->flex_centered[f] || + (m->flex_node[3*nidx+0] == 0 && + m->flex_node[3*nidx+1] == 0 && + m->flex_node[3*nidx+2] == 0))) { + if (enbl_spring) mji_addTo3(d->qfrc_spring + m->body_dofadr[bid], frc_g+3*i); + if (enbl_damper) mji_addTo3(d->qfrc_damper + m->body_dofadr[bid], dmp_g+3*i); } else { - if (enbl_spring) mj_applyFT(m, d, frc_g+3*i, 0, xpos_g+3*i, bodyid[i], d->qfrc_spring); - if (enbl_damper) mj_applyFT(m, d, dmp_g+3*i, 0, xpos_g+3*i, bodyid[i], d->qfrc_damper); + if (enbl_spring) mj_applyFT(m, d, frc_g+3*i, 0, xpos_g+3*i, bid, d->qfrc_spring); + if (enbl_damper) mj_applyFT(m, d, dmp_g+3*i, 0, xpos_g+3*i, bid, d->qfrc_damper); } } diff --git a/src/engine/engine_setconst.c b/src/engine/engine_setconst.c index 24ef9955..754aa29b 100644 --- a/src/engine/engine_setconst.c +++ b/src/engine/engine_setconst.c @@ -714,6 +714,7 @@ static void makeFlexBandwidth(mjModel* m, mjData* d) { for (int f = 0; f < m->nflex; f++) { if (!m->flex_interp[f]) continue; int order = m->flex_interp[f]; + order = order < 0 ? -order : order; int nodeadr = m->flex_nodeadr[f]; int nodenum = m->flex_nodenum[f]; int cx = m->flex_cellnum[3*f+0]; diff --git a/src/engine/engine_solver.c b/src/engine/engine_solver.c index b02302fc..1644f841 100644 --- a/src/engine/engine_solver.c +++ b/src/engine/engine_solver.c @@ -810,6 +810,12 @@ typedef struct { mjtNum* Mgrad; // M\grad or H\grad (nv x 1) mjtNum* search; // linesearch vector (nv x 1) mjtNum* quad; // quadratic polynomials for constraint costs (nefc x 3) + int* oldstate; // previous constraint state (nefc x 1) + + // CG arrays (PrimalAllocate, CG only) + mjtNum* gradold; // previous gradient (nv x 1) + mjtNum* Mgradold; // previous preconditioned gradient (nv x 1) + mjtNum* Mgraddif; // gradient difference (nv x 1) // Newton arrays, known-size (PrimalAllocate) mjtNum* D; // constraint inertia (nefc x 1) @@ -823,8 +829,6 @@ typedef struct { int* L_rowadr; // Hessian factor row addresses (nv x 1) int* LT_rownnz; // Hessian factor transpose row nonzeros (nv x 1) int* LT_rowadr; // Hessian factor transpose row addresses (nv x 1) - int* buf_ind; // index buffer for sparse addition (nv x 1) - mjtNum* buf_val; // value buffer for sparse addition (nv x 1) // Newton arrays, computed-size (MakeHessian) int nH; // number of nonzeros in Hessian H @@ -960,55 +964,94 @@ static void PrimalPointers(const mjModel* m, const mjData* d, mjPrimalContext* c // allocate fixed-size arrays in mjPrimalContext // mj_{mark/free}Stack in calling function! static void PrimalAllocate(mjData* d, mjPrimalContext* ctx, int flg_Newton) { - // local sizes + // local sizes and flags int nv = ctx->nv; int nefc = ctx->nefc; + int nJ = ctx->is_sparse ? d->nJ : 0; + int is_sparse = ctx->is_sparse; + int is_elliptic = ctx->is_elliptic; - // common arrays - ctx->Jaref = mjSTACKALLOC(d, nefc, mjtNum); - ctx->Jv = mjSTACKALLOC(d, nefc, mjtNum); - ctx->Ma = mjSTACKALLOC(d, nv, mjtNum); - ctx->Mv = mjSTACKALLOC(d, nv, mjtNum); - ctx->grad = mjSTACKALLOC(d, nv, mjtNum); - ctx->Mgrad = mjSTACKALLOC(d, nv, mjtNum); - ctx->search = mjSTACKALLOC(d, nv, mjtNum); - ctx->quad = mjSTACKALLOC(d, nefc*3, mjtNum); + // compute mjtNum block size + size_t nNum = 5*nefc + 5*nv; // common arrays + if (is_sparse) nNum += nJ; // JT + if (flg_Newton) { + nNum += nefc + nv; // D, cholupd + if (is_elliptic) nNum += 6*nv; // LTJ + if (!is_sparse) { + nNum += nv*nv; // L (dense) + if (is_elliptic) nNum += nv*nv; // Lcone (dense) + } + } else { + nNum += 3*nv; // CG arrays + } - // sparse only, compute Jacobian transpose - if (ctx->is_sparse) { - ctx->JT_rownnz = mjSTACKALLOC(d, nv, int); - ctx->JT_rowadr = mjSTACKALLOC(d, nv, int); - ctx->JT_rowsuper = mjSTACKALLOC(d, nv, int); - ctx->JT_colind = mjSTACKALLOC(d, d->nJ, int); - ctx->JT = mjSTACKALLOC(d, d->nJ, mjtNum); - int offset = ctx->J_rowadr[0]; + // compute int block size + size_t nInt = nefc; // oldstate + if (is_sparse) { + nInt += 3*nv + nJ; // JT sparse + if (flg_Newton) nInt += 8*nv; // Newton sparse + } + + // allocate mjtNum and int blocks + mjtNum* numblock = mjSTACKALLOC(d, nNum, mjtNum); + int* intblock = mjSTACKALLOC(d, nInt, int); + + // carve mjtNum block + ctx->Jaref = numblock; numblock += nefc; + ctx->Jv = numblock; numblock += nefc; + ctx->Ma = numblock; numblock += nv; + ctx->Mv = numblock; numblock += nv; + ctx->grad = numblock; numblock += nv; + ctx->Mgrad = numblock; numblock += nv; + ctx->search = numblock; numblock += nv; + ctx->quad = numblock; numblock += 3*nefc; + if (is_sparse) { + ctx->JT = numblock; numblock += nJ; + } + if (flg_Newton) { + ctx->D = numblock; numblock += nefc; + ctx->cholupd = numblock; numblock += nv; + if (is_elliptic) { + ctx->LTJ = numblock; numblock += 6*nv; + } + if (!is_sparse) { + ctx->nL = nv*nv; + ctx->L = numblock; numblock += ctx->nL; + ctx->Lcone = is_elliptic ? numblock : NULL; + if (is_elliptic) numblock += ctx->nL; + } + } else { + ctx->gradold = numblock; numblock += nv; + ctx->Mgradold = numblock; numblock += nv; + ctx->Mgraddif = numblock; numblock += nv; + } + + // carve int block + ctx->oldstate = intblock; intblock += nefc; + if (is_sparse) { + ctx->JT_rownnz = intblock; intblock += nv; + ctx->JT_rowadr = intblock; intblock += nv; + ctx->JT_rowsuper = intblock; intblock += nv; + ctx->JT_colind = intblock; intblock += nJ; + } + if (flg_Newton && is_sparse) { + ctx->H_rowadr = intblock; intblock += nv; + ctx->H_rownnz = intblock; intblock += nv; + ctx->HT_rownnz = intblock; intblock += nv; + ctx->HT_rowadr = intblock; intblock += nv; + ctx->L_rownnz = intblock; intblock += nv; + ctx->L_rowadr = intblock; intblock += nv; + ctx->LT_rownnz = intblock; intblock += nv; + ctx->LT_rowadr = intblock; intblock += nv; + } + + // sparse: compute Jacobian transpose + if (is_sparse) { + int offset = ctx->J_rowadr[0]; mju_transposeSparse(ctx->JT, ctx->J + offset, nefc, nv, ctx->JT_rownnz, ctx->JT_rowadr, ctx->JT_colind, ctx->JT_rowsuper, ctx->J_rownnz, ctx->J_rowadr, ctx->J_colind + offset); } - - // Newton only, known-size arrays - if (flg_Newton) { - ctx->D = mjSTACKALLOC(d, nefc, mjtNum); - ctx->cholupd = mjSTACKALLOC(d, nv, mjtNum); - if (ctx->is_elliptic) { - ctx->LTJ = mjSTACKALLOC(d, 6*nv, mjtNum); - } - - // sparse Newton only - if (ctx->is_sparse) { - ctx->H_rowadr = mjSTACKALLOC(d, nv, int); - ctx->H_rownnz = mjSTACKALLOC(d, nv, int); - ctx->HT_rownnz = mjSTACKALLOC(d, nv, int); - ctx->HT_rowadr = mjSTACKALLOC(d, nv, int); - ctx->L_rownnz = mjSTACKALLOC(d, nv, int); - ctx->L_rowadr = mjSTACKALLOC(d, nv, int); - ctx->LT_rownnz = mjSTACKALLOC(d, nv, int); - ctx->LT_rowadr = mjSTACKALLOC(d, nv, int); - ctx->buf_val = mjSTACKALLOC(d, nv, mjtNum); - ctx->buf_ind = mjSTACKALLOC(d, nv, int); - } - } } @@ -1529,7 +1572,7 @@ static void MakeHessian(mjData* d, mjPrimalContext* ctx) { // sparse if (ctx->is_sparse) { - // initialize Hessian rowadr, rownnz; get total nonzeros + // count Hessian nonzeros, initialize rowadr, rownnz ctx->nH = mju_sqrMatTDSparseSymbolic( ctx->H_rownnz, ctx->H_rowadr, NULL, NULL, nefc, nv, ctx->J_rownnz, ctx->J_rowadr, ctx->J_colind, @@ -1538,17 +1581,19 @@ static void MakeHessian(mjData* d, mjPrimalContext* ctx) { // 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 + // nH is known: allocate H, H_colind, HT_colind + ctx->H = mjSTACKALLOC(d, ctx->nH, mjtNum); + int* H_intblock = mjSTACKALLOC(d, 2*ctx->nH, int); + ctx->H_colind = H_intblock; + ctx->HT_colind = H_intblock + ctx->nH; + + // shift H row addresses to make room for M int shift = 0; for (int r = 0; r < nv - 1; r++) { shift += ctx->M_rownnz[r]; ctx->H_rowadr[r + 1] += shift; } - // allocate H_colind and H - ctx->H_colind = mjSTACKALLOC(d, ctx->nH, int); - ctx->H = mjSTACKALLOC(d, ctx->nH, mjtNum); - // compute H = J'*D*J: symbolic phase mju_sqrMatTDSparseSymbolic( ctx->H_rownnz, ctx->H_rowadr, ctx->H_colind, NULL, @@ -1562,13 +1607,11 @@ static void MakeHessian(mjData* d, mjPrimalContext* ctx) { ctx->JT, ctx->JT_rownnz, ctx->JT_rowadr, ctx->JT_colind, ctx->JT_rowsuper, ctx->D, d); - // add mass matrix: H = J'*D*J + C + // add mass matrix: H = J'*D*J + M mju_addToMatSparse(ctx->H, ctx->H_rownnz, ctx->H_rowadr, ctx->H_colind, nv, - ctx->M, ctx->M_rownnz, ctx->M_rowadr, ctx->M_colind, - ctx->buf_val, ctx->buf_ind); + ctx->M, ctx->M_rownnz, ctx->M_rowadr, ctx->M_colind); - // compute H' (upper triangle, required for symbolic Cholesky) - ctx->HT_colind = mjSTACKALLOC(d, ctx->nH, int); + // compute H' sparse structure (upper triangle, required for symbolic Cholesky) mju_transposeSparse(NULL, NULL, nv, nv, ctx->HT_rownnz, ctx->HT_rowadr, ctx->HT_colind, NULL, ctx->H_rownnz, ctx->H_rowadr, ctx->H_colind); @@ -1578,16 +1621,16 @@ static void MakeHessian(mjData* d, mjPrimalContext* ctx) { ctx->HT_rownnz, ctx->HT_rowadr, ctx->HT_colind, nv, d); - // allocate L_colind, L, Lcone - ctx->L_colind = mjSTACKALLOC(d, ctx->nL, int); - ctx->L = mjSTACKALLOC(d, ctx->nL, mjtNum); - if (ctx->is_elliptic) { - ctx->Lcone = mjSTACKALLOC(d, ctx->nL, mjtNum); - } - - // allocate LT (CSC representation of L) - ctx->LT_colind = mjSTACKALLOC(d, ctx->nL, int); - ctx->LT_map = mjSTACKALLOC(d, ctx->nL, int); + // nL is known: allocate blocks and carve L_colind, LT_colind, LT_map, L, Lcone + size_t nL_int = 2*ctx->nL + ctx->nL; // L_colind + LT_colind + LT_map + size_t nL_num = ctx->is_elliptic ? 2*ctx->nL : ctx->nL; // L + Lcone + int* L_intblock = mjSTACKALLOC(d, nL_int, int); + mjtNum* L_numblock = mjSTACKALLOC(d, nL_num, mjtNum); + ctx->L_colind = L_intblock; + ctx->LT_colind = L_intblock + ctx->nL; + ctx->LT_map = L_intblock + 2*ctx->nL; + ctx->L = L_numblock; + ctx->Lcone = ctx->is_elliptic ? L_numblock + ctx->nL : NULL; // symbolic Cholesky: populate L_colind and LT structures mju_cholFactorSymbolic(ctx->L_colind, ctx->L_rownnz, ctx->L_rowadr, @@ -1598,13 +1641,6 @@ static void MakeHessian(mjData* d, mjPrimalContext* ctx) { // dense else { - // allocate L, Lcone - ctx->nL = nv*nv; - ctx->L = mjSTACKALLOC(d, ctx->nL, mjtNum); - if (ctx->is_elliptic) { - ctx->Lcone = mjSTACKALLOC(d, ctx->nL, mjtNum); - } - // compute H = M + J'*D*J mju_sqrMatTD_impl(ctx->L, ctx->J, ctx->D, nefc, nv, /*flg_upper=*/ 0); mju_addToSymSparse(ctx->L, ctx->M, ctx->nv, @@ -1647,8 +1683,7 @@ static void FactorizeHessian(mjData* d, mjPrimalContext* ctx, int flg_recompute) // add mass matrix: H = J'*D*J + C mju_addToMatSparse(ctx->H, ctx->H_rownnz, ctx->H_rowadr, ctx->H_colind, nv, - ctx->M, ctx->M_rownnz, ctx->M_rowadr, ctx->M_colind, - ctx->buf_val, ctx->buf_ind); + ctx->M, ctx->M_rownnz, ctx->M_rowadr, ctx->M_colind); } // numeric sparse factorization: L = chol(H) using pre-computed sparsity pattern @@ -1691,13 +1726,12 @@ static void FactorizeHessian(mjData* d, mjPrimalContext* ctx, int flg_recompute) // elliptic case: Hcone = H + cone_contributions static void HessianCone(mjData* d, mjPrimalContext* ctx) { int nv = ctx->nv, nefc = ctx->nefc; + mjtNum* LTJ = ctx->LTJ; mjtNum local[36]; // start with Hcone = H mju_copy(ctx->Lcone, ctx->L, ctx->nL); - mjtNum* LTJ = ctx->LTJ; - // add contributions for (int i=0; i < nefc; i++) { if (ctx->efc_state[i] == mjCNSTRSTATE_CONE) { @@ -1818,7 +1852,6 @@ static void HessianIncremental(mjData* d, mjPrimalContext* ctx, const int* oldst static void mj_solPrimal(const mjModel* m, mjData* d, int island, int maxiter, int flg_Newton) { int iter = 0; mjtNum alpha, beta; - mjtNum *gradold = NULL, *Mgradold = NULL, *Mgraddif = NULL; mjPrimalContext ctx; mj_markStack(d); @@ -1829,14 +1862,7 @@ static void mj_solPrimal(const mjModel* m, mjData* d, int island, int maxiter, i // local copies int nv = ctx.nv; int nefc = ctx.nefc; - - // allocate local storage - if (!flg_Newton) { - gradold = mjSTACKALLOC(d, nv, mjtNum); - Mgradold = mjSTACKALLOC(d, nv, mjtNum); - Mgraddif = mjSTACKALLOC(d, nv, mjtNum); - } - int* oldstate = mjSTACKALLOC(d, nefc, int); + int* oldstate = ctx.oldstate; // compute Ma = M * qacc mju_mulSymVecSparse(ctx.Ma, ctx.M, ctx.qacc, nv, @@ -1895,8 +1921,8 @@ static void mj_solPrimal(const mjModel* m, mjData* d, int island, int maxiter, i // save old if (!flg_Newton) { - mju_copy(gradold, ctx.grad, nv); - mju_copy(Mgradold, ctx.Mgrad, nv); + mju_copy(ctx.gradold, ctx.grad, nv); + mju_copy(ctx.Mgradold, ctx.Mgrad, nv); } mju_copyInt(oldstate, ctx.efc_state, nefc); mjtNum oldcost = ctx.cost; @@ -1933,9 +1959,9 @@ static void mj_solPrimal(const mjModel* m, mjData* d, int island, int maxiter, i mju_scl(ctx.search, ctx.Mgrad, -1, nv); } else { // Polak-Ribiere - mju_sub(Mgraddif, ctx.Mgrad, Mgradold, nv); - beta = mju_dot(ctx.grad, Mgraddif, nv) / - mju_max(mjMINVAL, mju_dot(gradold, Mgradold, nv)); + mju_sub(ctx.Mgraddif, ctx.Mgrad, ctx.Mgradold, nv); + beta = mju_dot(ctx.grad, ctx.Mgraddif, nv) / + mju_max(mjMINVAL, mju_dot(ctx.gradold, ctx.Mgradold, nv)); // reset if negative if (beta < 0) { diff --git a/src/engine/engine_support.c b/src/engine/engine_support.c index 27537d39..2cff8056 100644 --- a/src/engine/engine_support.c +++ b/src/engine/engine_support.c @@ -66,7 +66,8 @@ const char* mjDISABLESTRING[mjNDISABLE] = { "Eulerdamp", "AutoReset", "NativeCCD", - "Island" + "Island", + "MultiCCD" }; @@ -76,7 +77,6 @@ const char* mjENABLESTRING[mjNENABLE] = { "Energy", "Fwdinv", "InvDiscrete", - "MultiCCD", "Sleep" }; @@ -420,17 +420,11 @@ void mj_mulM2(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* vec) void mj_addM(const mjModel* m, mjData* d, mjtNum* dst, int* rownnz, int* rowadr, int* colind) { int nv = m->nv; + // sparse if (rownnz && rowadr && colind) { - mj_markStack(d); - mjtNum* buf_val = mjSTACKALLOC(d, nv, mjtNum); - int* buf_ind = mjSTACKALLOC(d, nv, int); - - mju_addToMatSparse(dst, rownnz, rowadr, colind, nv, - d->M, m->M_rownnz, m->M_rowadr, m->M_colind, - buf_val, buf_ind); - - mj_freeStack(d); + mju_addToMatSparse(dst, rownnz, rowadr, colind, nv, d->M, + m->M_rownnz, m->M_rowadr, m->M_colind); } // dense diff --git a/src/engine/engine_util_solve.c b/src/engine/engine_util_solve.c index 259f7c59..1f8b839b 100644 --- a/src/engine/engine_util_solve.c +++ b/src/engine/engine_util_solve.c @@ -144,10 +144,7 @@ int mju_cholFactorSparse(mjtNum* mat, int n, mjtNum mindiag, int* rownnz, const int* rowadr, int* colind, mjData* d) { int rank = n; - - mj_markStack(d); - mjtNum* buf = mjSTACKALLOC(d, n, mjtNum); - int* buf_ind = mjSTACKALLOC(d, n, int); + (void) d; // backpass over rows for (int r=n-1; r >= 0; r--) { @@ -175,15 +172,13 @@ int mju_cholFactorSparse(mjtNum* mat, int n, mjtNum mindiag, // mat(c,0:c) = mat(c,0:c) - mat(r,c) * mat(r,0:c) int nnz_c = mju_combineSparse(mat + rowadr[c], mat+rowadr[r], 1, -mat[adr+i], - rownnz[c], i+1, colind+rowadr[c], colind+rowadr[r], - buf, buf_ind); + rownnz[c], i+1, colind+rowadr[c], colind+rowadr[r]); // assign new nnz to row c rownnz[c] = nnz_c; } } - mj_freeStack(d); return rank; } diff --git a/src/engine/engine_util_sparse.c b/src/engine/engine_util_sparse.c index 05960567..cfed023b 100644 --- a/src/engine/engine_util_sparse.c +++ b/src/engine/engine_util_sparse.c @@ -198,15 +198,14 @@ void mju_mulMatTVecSparse(mjtNum* res, const mjtNum* mat, const mjtNum* vec, int } -// add sparse matrix M to sparse destination matrix, requires pre-allocated buffers +// add sparse matrix M to sparse destination matrix void mju_addToMatSparse(mjtNum* dst, int* rownnz, int* rowadr, int* colind, int nr, const mjtNum* M, const int* M_rownnz, const int* M_rowadr, - const int* M_colind, - mjtNum* buf_val, int* buf_ind) { + const int* M_colind) { for (int i=0; i < nr; i++) { rownnz[i] = mju_combineSparse(dst + rowadr[i], M + M_rowadr[i], 1, 1, rownnz[i], M_rownnz[i], colind + rowadr[i], - M_colind + M_rowadr[i], buf_val, buf_ind); + M_colind + M_rowadr[i]); } } @@ -256,8 +255,8 @@ void mju_mulSymVecSparse(mjtNum* restrict res, const mjtNum* restrict mat, for (int k=diag-1; k >= 0; k--) { int j = ind[k]; mjtNum val = row[k]; - res[i] += val * vec[j]; // strict lower - res[j] += val * vec[i]; // strict upper + res[i] += val * vec[j]; // strict lower + res[j] += val * vec[i]; // strict upper } } } diff --git a/src/engine/engine_util_sparse.h b/src/engine/engine_util_sparse.h index 1350ccff..68bda070 100644 --- a/src/engine/engine_util_sparse.h +++ b/src/engine/engine_util_sparse.h @@ -62,11 +62,10 @@ MJAPI void mju_mulMatVecSparse(mjtNum* res, const mjtNum* mat, const mjtNum* vec MJAPI void mju_mulMatTVecSparse(mjtNum* res, const mjtNum* mat, const mjtNum* vec, int nr, int nc, const int* rownnz, const int* rowadr, const int* colind); -// add sparse matrix M to sparse destination matrix, requires pre-allocated buffers +// add sparse matrix M to sparse destination matrix MJAPI void mju_addToMatSparse(mjtNum* dst, int* rownnz, int* rowadr, int* colind, int nr, const mjtNum* M, const int* M_rownnz, const int* M_rowadr, - const int* M_colind, - mjtNum* buf_val, int* buf_ind); + const int* M_colind); // add symmetric matrix (only lower triangle represented) to dense matrix MJAPI void mju_addToSymSparse(mjtNum* res, const mjtNum* mat, int n, @@ -294,8 +293,7 @@ void mju_addToSclScl(mjtNum* res, const mjtNum* vec, mjtNum scl1, mjtNum scl2, i // combine two sparse vectors: dst = a*dst + b*src, return nnz of result static inline int mju_combineSparse(mjtNum* dst, const mjtNum* src, mjtNum a, mjtNum b, - int dst_nnz, int src_nnz, int* dst_ind, const int* src_ind, - mjtNum* buf, int* buf_ind) { + int dst_nnz, int src_nnz, int* dst_ind, const int* src_ind) { // check for identical pattern if (dst_nnz == src_nnz) { if (mju_compare(dst_ind, src_ind, dst_nnz)) { @@ -305,49 +303,54 @@ int mju_combineSparse(mjtNum* dst, const mjtNum* src, mjtNum a, mjtNum b, } } - // copy dst into buf - if (dst_nnz) { - memcpy(buf, dst, dst_nnz * sizeof(mjtNum)); - memcpy(buf_ind, dst_ind, dst_nnz * sizeof(int)); - } + // compute total nnz of result + int nnz = mju_combineSparseCount(dst_nnz, src_nnz, dst_ind, src_ind); - // prepare to merge buf and src into dst - int bi = 0, si = 0, nnz = 0; - int buf_nnz = dst_nnz; + // set up read/write pointers at end of arrays + int bi = dst_nnz - 1, si = src_nnz - 1, w = nnz - 1; - // merge vectors - while (bi < buf_nnz && si < src_nnz) { - int badr = buf_ind[bi]; + // merge backwards + while (bi >= 0 && si >= 0) { + int badr = dst_ind[bi]; int sadr = src_ind[si]; if (badr == sadr) { - dst[nnz] = a*buf[bi++] + b*src[si++]; - dst_ind[nnz++] = badr; + dst[w] = a*dst[bi] + b*src[si]; + dst_ind[w] = badr; + bi--; + si--; } - // buf only - else if (badr < sadr) { - dst[nnz] = a*buf[bi++]; - dst_ind[nnz++] = badr; + // dst only + else if (badr > sadr) { + dst[w] = a*dst[bi]; + dst_ind[w] = badr; + bi--; } // src only else { - dst[nnz] = b*src[si++]; - dst_ind[nnz++] = sadr; + dst[w] = b*src[si]; + dst_ind[w] = sadr; + si--; } + w--; } - // the rest of src only - while (si < src_nnz) { - dst[nnz] = b*src[si]; - dst_ind[nnz++] = src_ind[si++]; + // remaining src elements + while (si >= 0) { + dst[w] = b*src[si]; + dst_ind[w] = src_ind[si]; + si--; + w--; } - // the rest of buf only - while (bi < buf_nnz) { - dst[nnz] = a*buf[bi]; - dst_ind[nnz++] = buf_ind[bi++]; + // remaining dst elements: already in place, scale by a + if (a != 1) { + while (bi >= 0) { + dst[bi] *= a; + bi--; + } } return nnz; diff --git a/src/engine/engine_vis_interact.c b/src/engine/engine_vis_interact.c index d1ce7ffe..60533e27 100644 --- a/src/engine/engine_vis_interact.c +++ b/src/engine/engine_vis_interact.c @@ -864,6 +864,7 @@ int mjv_select(const mjModel* m, const mjData* d, const mjvOption* vopt, if (m->flex_interp[i]) { mjtNum* coord = m->flex_vert0 + 3*(m->flex_vertadr[i] + vertid); int order = m->flex_interp[i]; + order = order < 0 ? -order : order; int npc = (order+1)*(order+1)*(order+1); // cell lookup: get local coords and node indices diff --git a/src/engine/engine_vis_visualize.c b/src/engine/engine_vis_visualize.c index c0072dbb..0e058bd1 100644 --- a/src/engine/engine_vis_visualize.c +++ b/src/engine/engine_vis_visualize.c @@ -1452,6 +1452,7 @@ static void addFlexBvhGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, int cy = m->flex_cellnum[3*f+1]; int cz = m->flex_cellnum[3*f+2]; int order = m->flex_interp[f]; + order = order < 0 ? -order : order; int NX = cx * order + 1; int NY = cy * order + 1; int NZ = cz * order + 1; @@ -1459,11 +1460,18 @@ static void addFlexBvhGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, for (int i=0; i < NX; i++) { for (int j=0; j < NY; j++) { for (int k=0; k < NZ; k++) { - int offset = 3*(i*NY*NZ + j*NZ + k); + int n0 = i*NY*NZ + j*NZ + k; + + // skip if this node is pinned (no joints on its body) + if (m->body_jntnum[bodyid[n0]] == 0) { + continue; + } + + int offset = 3*n0; int offset1 = 3*((i+1)*NY*NZ + j*NZ + k); int offset2 = 3*(i*NY*NZ + (j+1)*NZ + k); int offset3 = 3*(i*NY*NZ + j*NZ + (k+1)); - if (i < NX-1) { + if (i < NX-1 && m->body_jntnum[bodyid[(i+1)*NY*NZ + j*NZ + k]] > 0) { mjvGeom* thisgeom = acquireGeom(scn, i, mjCAT_DECOR, mjOBJ_UNKNOWN); if (!thisgeom) { return; @@ -1472,7 +1480,7 @@ static void addFlexBvhGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, mjv_connector(thisgeom, mjGEOM_LINE, 3, xpos+offset, xpos+offset1); releaseGeom(&thisgeom, scn); } - if (j < NY-1) { + if (j < NY-1 && m->body_jntnum[bodyid[i*NY*NZ + (j+1)*NZ + k]] > 0) { mjvGeom* thisgeom = acquireGeom(scn, i, mjCAT_DECOR, mjOBJ_UNKNOWN); if (!thisgeom) { return; @@ -1481,7 +1489,7 @@ static void addFlexBvhGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, mjv_connector(thisgeom, mjGEOM_LINE, 3, xpos+offset, xpos+offset2); releaseGeom(&thisgeom, scn); } - if (k < NZ-1) { + if (k < NZ-1 && m->body_jntnum[bodyid[i*NY*NZ + j*NZ + (k+1)]] > 0) { mjvGeom* thisgeom = acquireGeom(scn, i, mjCAT_DECOR, mjOBJ_UNKNOWN); if (!thisgeom) { return; diff --git a/src/experimental/filament/filament/builtins.cc b/src/experimental/filament/filament/builtins.cc index 5ee489b3..86a6a5c5 100644 --- a/src/experimental/filament/filament/builtins.cc +++ b/src/experimental/filament/filament/builtins.cc @@ -65,7 +65,8 @@ class BuiltinBuilder : MeshData { virtual ~BuiltinBuilder() = default; template - static MeshPtr Create(filament::Engine* engine, Args&&... args) { + static std::unique_ptr Create(filament::Engine* engine, + Args&&... args) { auto builder = new T(std::forward(args)...); MeshData* mesh_data = builder->PrepareMeshData(); mesh_data->release_callback = +[](void* user_data) { @@ -621,43 +622,43 @@ class DomeBuilder : public BuiltinBuilder { } }; -MeshPtr CreateLine(filament::Engine* engine) { +std::unique_ptr CreateLine(filament::Engine* engine) { return BuiltinBuilder::Create(engine); } -MeshPtr CreatePlane(filament::Engine* engine, int nquad) { +std::unique_ptr CreatePlane(filament::Engine* engine, int nquad) { return BuiltinBuilder::Create(engine, nquad); } -MeshPtr CreateTriangle(filament::Engine* engine) { +std::unique_ptr CreateTriangle(filament::Engine* engine) { return BuiltinBuilder::Create(engine); } -MeshPtr CreateBox(filament::Engine* engine, int nquad) { +std::unique_ptr CreateBox(filament::Engine* engine, int nquad) { return BuiltinBuilder::Create(engine, nquad); } -MeshPtr CreateLineBox(filament::Engine* engine) { +std::unique_ptr CreateLineBox(filament::Engine* engine) { return BuiltinBuilder::Create(engine); } -MeshPtr CreateSphere(filament::Engine* engine, int nstack, int nslice) { +std::unique_ptr CreateSphere(filament::Engine* engine, int nstack, int nslice) { return BuiltinBuilder::Create(engine, nstack, nslice); } -MeshPtr CreateTube(filament::Engine* engine, int nstack, int nslice) { +std::unique_ptr CreateTube(filament::Engine* engine, int nstack, int nslice) { return BuiltinBuilder::Create(engine, nstack, nslice); } -MeshPtr CreateDisk(filament::Engine* engine, int nslice) { +std::unique_ptr CreateDisk(filament::Engine* engine, int nslice) { return BuiltinBuilder::Create(engine, nslice); } -MeshPtr CreateDome(filament::Engine* engine, int nstack, int nslice) { +std::unique_ptr CreateDome(filament::Engine* engine, int nstack, int nslice) { return BuiltinBuilder::Create(engine, nstack, nslice); } -MeshPtr CreateCone(filament::Engine* engine, int nstack, int nslice) { +std::unique_ptr CreateCone(filament::Engine* engine, int nstack, int nslice) { return BuiltinBuilder::Create(engine, nstack, nslice); } diff --git a/src/experimental/filament/filament/builtins.h b/src/experimental/filament/filament/builtins.h index c698bca8..5fd5c8a5 100644 --- a/src/experimental/filament/filament/builtins.h +++ b/src/experimental/filament/filament/builtins.h @@ -15,22 +15,24 @@ #ifndef MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_BUILTINS_H_ #define MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_BUILTINS_H_ +#include + #include #include "experimental/filament/filament/mesh.h" // Generates buffers for built-in shapes. namespace mujoco { -MeshPtr CreateLine(filament::Engine* engine); -MeshPtr CreatePlane(filament::Engine* engine, int nquad); -MeshPtr CreateTriangle(filament::Engine* engine); -MeshPtr CreateBox(filament::Engine* engine, int nquad); -MeshPtr CreateLineBox(filament::Engine* engine); -MeshPtr CreateSphere(filament::Engine* engine, int nstack, int nslice); -MeshPtr CreateTube(filament::Engine* engine, int nstack, int nslice); -MeshPtr CreateDisk(filament::Engine* engine, int nslice); -MeshPtr CreateDome(filament::Engine* engine, int nstack, int nslice); -MeshPtr CreateCone(filament::Engine* engine, int nstack, int nslice); +std::unique_ptr CreateLine(filament::Engine* engine); +std::unique_ptr CreatePlane(filament::Engine* engine, int nquad); +std::unique_ptr CreateTriangle(filament::Engine* engine); +std::unique_ptr CreateBox(filament::Engine* engine, int nquad); +std::unique_ptr CreateLineBox(filament::Engine* engine); +std::unique_ptr CreateSphere(filament::Engine* engine, int nstack, int nslice); +std::unique_ptr CreateTube(filament::Engine* engine, int nstack, int nslice); +std::unique_ptr CreateDisk(filament::Engine* engine, int nslice); +std::unique_ptr CreateDome(filament::Engine* engine, int nstack, int nslice); +std::unique_ptr CreateCone(filament::Engine* engine, int nstack, int nslice); } // namespace mujoco diff --git a/src/experimental/filament/filament/filament_context.cc b/src/experimental/filament/filament/filament_context.cc index 4da0d63b..427306b8 100644 --- a/src/experimental/filament/filament/filament_context.cc +++ b/src/experimental/filament/filament/filament_context.cc @@ -156,7 +156,6 @@ void FilamentContext::Render(const mjrRect& viewport, const mjvScene* scene) { request.viewport = viewport; request.camera = last_camera_; request.enable_ux = (gui_swap_chain_target_ == kWindowSwapChain); - request.gui_scale = imgui_bridge_ ? imgui_bridge_->GetScale() : 1.0f; scene_view_->Render(renderer_, request); renderer_->endFrame(); } @@ -191,14 +190,17 @@ void FilamentContext::SetFrameBuffer(int framebuffer) { } void FilamentContext::PrepareRenderTargets(int width, int height) { - color_target_ = std::make_unique( - engine_, RenderTargetTextureType::kColor, - RenderTargetTextureType::kDepth); + RenderTargetConfig config; + DefaultRenderTargetConfig(&config); + + config.color_format = mjPIXEL_FORMAT_RGB8; + config.depth_format = mjPIXEL_FORMAT_DEPTH32F; + color_target_ = std::make_unique(engine_, config); color_target_->Prepare(width, height); - depth_target_ = std::make_unique( - engine_, RenderTargetTextureType::kDepthColor, - RenderTargetTextureType::kDepth); + config.color_format = mjPIXEL_FORMAT_R32F; + config.depth_format = mjPIXEL_FORMAT_DEPTH32F; + depth_target_ = std::make_unique(engine_, config); depth_target_->Prepare(width, height); } @@ -230,7 +232,6 @@ void FilamentContext::ReadPixels(mjrRect viewport, unsigned char* rgb, request.target = color_target_.get(); request.camera = last_camera_; request.enable_ux = (gui_swap_chain_target_ == kOffscreenSwapChain); - request.gui_scale = imgui_bridge_ ? imgui_bridge_->GetScale() : 1.0f; scene_view_->Render(renderer_, request); const size_t num_bytes = viewport.width * viewport.height * 3; diff --git a/src/experimental/filament/filament/imgui_bridge.cc b/src/experimental/filament/filament/imgui_bridge.cc index f5553ea8..97dd852a 100644 --- a/src/experimental/filament/filament/imgui_bridge.cc +++ b/src/experimental/filament/filament/imgui_bridge.cc @@ -21,6 +21,8 @@ #include #include +#include +#include #include #include #include "experimental/filament/filament/material.h" @@ -32,6 +34,9 @@ namespace mujoco { +using filament::math::float3; +using filament::math::mat3f; + ImguiBridge::ImguiBridge(ObjectManager* object_mgr, SceneView* scene_view) : object_mgr_(object_mgr), scene_view_(scene_view) {} @@ -235,11 +240,7 @@ void ImguiBridge::Update() { const int height = size.y * scale.y; auto& renderable = renderables_[renderable_index]; - if (renderable->GetNumMeshes() == 0) { - renderable->AppendMesh(mesh, index_offset, command.ElemCount); - } else { - renderable->UpdateMesh(0, mesh, index_offset, command.ElemCount); - } + renderable->SetMesh(mesh, index_offset, command.ElemCount); MaterialTextures textures; textures.color = textures_[command.GetTexID()].get(); @@ -259,6 +260,8 @@ void ImguiBridge::Update() { properties.scissor[3] = height; } renderable->UpdateMaterial(properties, textures); + renderable->SetTransform( + {float3{0, 0, 0}, mat3f(), float3(scale.x, scale.y, 1.0f)}); index_offset += command.ElemCount; ++renderable_index; @@ -268,8 +271,11 @@ void ImguiBridge::Update() { void ImguiBridge::PrepareRenderables(int count) { while (renderables_.size() < count) { + RenderableParams config; + DefaultRenderableParams(&config); + config.shading_model = ShadingModel::Ux; auto& r = renderables_.emplace_back( - std::make_unique(Renderable::Usage::Ux, object_mgr_)); + std::make_unique(object_mgr_, config)); r->SetCastShadows(false); r->SetReceiveShadows(false); r->SetBlendOrder(static_cast(renderables_.size())); @@ -281,10 +287,6 @@ void ImguiBridge::PrepareRenderables(int count) { } } -float ImguiBridge::GetScale() const { - return ImGui::GetIO().DisplayFramebufferScale.x; -} - static ImVec2 ClipSpaceToWindowCoordinates(float x, float y) { const ImVec2& display_size = ImGui::GetIO().DisplaySize; const float pos_x = display_size.x * ((x + 1) * 0.5f); diff --git a/src/experimental/filament/filament/imgui_bridge.h b/src/experimental/filament/filament/imgui_bridge.h index 03ee4f47..54b5f494 100644 --- a/src/experimental/filament/filament/imgui_bridge.h +++ b/src/experimental/filament/filament/imgui_bridge.h @@ -40,9 +40,6 @@ class ImguiBridge { // synced. void Update(); - // Returns the current ImGui scale factor. - float GetScale() const; - // Uploads texture to be used with ImGui's Image and ImageButton functions. uintptr_t UploadImage(uintptr_t tex_id, const uint8_t* pixels, int width, int height, int bpp); @@ -62,7 +59,7 @@ class ImguiBridge { ObjectManager* object_mgr_ = nullptr; SceneView* scene_view_ = nullptr; std::vector> renderables_; - std::vector meshes_; + std::vector> meshes_; std::unordered_map> textures_; }; diff --git a/src/experimental/filament/filament/math_util.h b/src/experimental/filament/filament/math_util.h index ee3b2741..5438b5e4 100644 --- a/src/experimental/filament/filament/math_util.h +++ b/src/experimental/filament/filament/math_util.h @@ -46,15 +46,31 @@ inline filament::math::float4 ReadFloat4(const T* arr, int index = 0) { // Reads a mat3 from an array buffer in the model/scene. template -inline filament::math::mat3 ReadMat3(const T* arr, int index = 0) { +inline filament::math::mat3f ReadMat3(const T* arr, int index = 0) { // clang-format off const T* ptr = arr + (9 * index); - return filament::math::mat3(ptr[0], ptr[3], ptr[6], - ptr[1], ptr[4], ptr[7], - ptr[2], ptr[5], ptr[8]); + return filament::math::mat3f(ptr[0], ptr[3], ptr[6], + ptr[1], ptr[4], ptr[7], + ptr[2], ptr[5], ptr[8]); // clang-format on } +// A tuple of translation, rotation, and size. +struct Trs { + filament::math::float3 translation{0.0f, 0.0f, 0.0f}; + filament::math::mat3f rotation; + // Note: this is _slightly_ different than scale. For example, for capsules, + // the size determines the length of the tube and the radius of the domes, + // but the shape remains a capsule. + filament::math::float3 size{1.0f, 1.0f, 1.0f}; + + // Converts the TRS to a transform matrix. + filament::math::mat4f ToTransform() const { + return filament::math::mat4f(rotation, translation) * + filament::math::mat4f::scaling(size); + } +}; + // Calculates a reflection matrix for a plane defined by its transform. filament::math::mat4 ToReflectionMatrix(const filament::math::mat4& xform); diff --git a/src/experimental/filament/filament/mesh.h b/src/experimental/filament/filament/mesh.h index cc30b6d3..b75ad6de 100644 --- a/src/experimental/filament/filament/mesh.h +++ b/src/experimental/filament/filament/mesh.h @@ -185,8 +185,6 @@ class Mesh { int num_attributes_ = 0; }; -using MeshPtr = std::unique_ptr; - } // namespace mujoco #endif // MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_MESH_H_ diff --git a/src/experimental/filament/filament/model_objects.cc b/src/experimental/filament/filament/model_objects.cc index 1f88dce0..15121743 100644 --- a/src/experimental/filament/filament/model_objects.cc +++ b/src/experimental/filament/filament/model_objects.cc @@ -630,20 +630,11 @@ void ModelObjects::UploadHeightField(const mjModel* model, int id) { height_fields_[id] = std::make_unique(engine_, data); } -MeshPtr ModelObjects::CreateFlexMesh(const mjvScene* scene, - const mjvGeom& geom) { +void ModelObjects::CreateSkinFlexMesh(const mjvScene* scene, const mjvGeom& geom) { MeshData data; DefaultMeshData(&data); UpdateSkinFlexMeshData(&data, model_, scene, geom); - return std::make_unique(engine_, data); -} - -MeshPtr ModelObjects::CreateSkinMesh(const mjvScene* scene, - const mjvGeom& geom) { - MeshData data; - DefaultMeshData(&data); - UpdateSkinFlexMeshData(&data, model_, scene, geom); - return std::make_unique(engine_, data); + dynamic_meshes_[geom.objid] = std::make_unique(engine_, data); } const Mesh* ModelObjects::GetMeshBuffer(int data_id) const { @@ -672,6 +663,11 @@ const Mesh* ModelObjects::GetShapeBuffer(ShapeType shape) const { return shapes_[shape].get(); } +const Mesh* ModelObjects::GetFlexSkinGeomMesh(int geom_id) const { + auto it = dynamic_meshes_.find(geom_id); + return it != dynamic_meshes_.end() ? it->second.get() : nullptr; +} + const Texture* ModelObjects::GetTexture(int tex_id) const { auto it = textures_.find(tex_id); return it != textures_.end() ? it->second.get() : nullptr; diff --git a/src/experimental/filament/filament/model_objects.h b/src/experimental/filament/filament/model_objects.h index bc6a005b..d85693e3 100644 --- a/src/experimental/filament/filament/model_objects.h +++ b/src/experimental/filament/filament/model_objects.h @@ -15,6 +15,7 @@ #ifndef MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_MODEL_OBJECTS_H_ #define MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_MODEL_OBJECTS_H_ +#include #include #include #include @@ -55,6 +56,8 @@ class ModelObjects { void UploadHeightField(const mjModel* model, int id); + void CreateSkinFlexMesh(const mjvScene* scene, const mjvGeom& geom); + // Returns the filament engine used by the ModelObjects to create filament // objects. filament::Engine* GetEngine() const { return engine_; } @@ -63,12 +66,10 @@ class ModelObjects { const Mesh* GetShapeBuffer(ShapeType shape) const; const Mesh* GetMeshBuffer(int data_id) const; const Mesh* GetHeightFieldBuffer(int hfield_id) const; + const Mesh* GetFlexSkinGeomMesh(int geom_id) const; const Texture* GetTexture(int tex_id) const; const Texture* GetTexture(int mat_id, int role) const; - MeshPtr CreateFlexMesh(const mjvScene* scene, const mjvGeom& geom); - MeshPtr CreateSkinMesh(const mjvScene* scene, const mjvGeom& geom); - filament::Skybox* CreateSkybox(); filament::IndirectLight* CreateIndirectLight(int tex_id, float intensity); @@ -86,10 +87,11 @@ class ModelObjects { filament::Engine* engine_ = nullptr; std::vector skyboxes_; std::vector indirect_lights_; - std::array shapes_; - std::unordered_map meshes_; - std::unordered_map convex_hulls_; - std::unordered_map height_fields_; + std::array, kNumShapes> shapes_; + std::unordered_map> meshes_; + std::unordered_map> convex_hulls_; + std::unordered_map> height_fields_; + std::unordered_map> dynamic_meshes_; std::unordered_map> textures_; float specular_multiplier_ = 0.2f; float shininess_multiplier_ = 0.1f; diff --git a/src/experimental/filament/filament/render_target.cc b/src/experimental/filament/filament/render_target.cc index eb2d3490..b01fb94c 100644 --- a/src/experimental/filament/filament/render_target.cc +++ b/src/experimental/filament/filament/render_target.cc @@ -30,10 +30,14 @@ namespace mujoco { +void DefaultRenderTargetConfig(RenderTargetConfig* config) { + config->color_format = mjPIXEL_FORMAT_RGBA8; + config->depth_format = mjPIXEL_FORMAT_DEPTH32F; +} + RenderTarget::RenderTarget(filament::Engine* engine, - RenderTargetTextureType color, - RenderTargetTextureType depth) - : engine_(engine), color_type_(color), depth_type_(depth) {} + const RenderTargetConfig& config) + : engine_(engine), config_(config) {} RenderTarget::~RenderTarget() noexcept { Destroy(); @@ -47,10 +51,29 @@ void RenderTarget::Prepare(int width, int height) { width_ = width; height_ = height; - color_texture_ = - std::make_unique(engine_, color_type_, width, height); - depth_texture_ = - std::make_unique(engine_, depth_type_, width, height); + TextureConfig color_config; + DefaultTextureConfig(&color_config); + Texture::InternalFlags color_flags; + color_config.width = width; + color_config.height = height; + color_config.target = mjTEXTURE_2D; + color_config.format = config_.color_format; + color_config.color_space = mjCOLORSPACE_LINEAR; + color_config.format = mjPIXEL_FORMAT_RGB8; + color_flags.color_attachment = true; + color_texture_ = std::make_unique(engine_, color_config, color_flags); + + TextureConfig depth_config; + DefaultTextureConfig(&depth_config); + Texture::InternalFlags depth_flags; + depth_config.width = width; + depth_config.height = height; + depth_config.target = mjTEXTURE_2D; + depth_config.format = config_.depth_format; + depth_config.color_space = mjCOLORSPACE_LINEAR; + depth_config.format = mjPIXEL_FORMAT_DEPTH32F; + depth_flags.depth_attachment = true; + depth_texture_ = std::make_unique(engine_, depth_config, depth_flags); filament::RenderTarget::Builder builder; builder.texture(filament::RenderTarget::AttachmentPoint::COLOR, @@ -65,19 +88,19 @@ void RenderTarget::ReadColorPixels(filament::Renderer* renderer, uint8_t* bytes, filament::backend::PixelDataFormat format; filament::backend::PixelDataType type; size_t expected_num_bytes = 0; - switch (color_type_) { - case RenderTargetTextureType::kColor: + switch (config_.color_format) { + case mjPIXEL_FORMAT_RGB8: format = filament::backend::PixelDataFormat::RGB; type = filament::backend::PixelDataType::UBYTE; expected_num_bytes = width_ * height_ * 3; break; - case RenderTargetTextureType::kDepthColor: + case mjPIXEL_FORMAT_R32F: format = filament::backend::PixelDataFormat::R; type = filament::backend::PixelDataType::FLOAT; expected_num_bytes = width_ * height_ * sizeof(float); break; default: - mju_error("Unsupported pixel format: %d", color_type_); + mju_error("Unsupported pixel format: %d", config_.color_format); return; } if (num_bytes != expected_num_bytes) { diff --git a/src/experimental/filament/filament/render_target.h b/src/experimental/filament/filament/render_target.h index ee033de9..22a02143 100644 --- a/src/experimental/filament/filament/render_target.h +++ b/src/experimental/filament/filament/render_target.h @@ -25,13 +25,21 @@ namespace mujoco { +// Defines the basic properties of a render target. +struct RenderTargetConfig { + mjtPixelFormat color_format; + mjtPixelFormat depth_format; +}; + +// Initializes the RenderTargetConfig to default values. +void DefaultRenderTargetConfig(RenderTargetConfig* config); + // Manages a filament RenderTarget and the textures which are bound to it. class RenderTarget { public: // Defines the types of textures to create for the color and depth // attachments. - RenderTarget(filament::Engine* engine, RenderTargetTextureType color, - RenderTargetTextureType depth); + RenderTarget(filament::Engine* engine, const RenderTargetConfig& config); ~RenderTarget() noexcept; RenderTarget(const RenderTarget&) = delete; @@ -58,11 +66,10 @@ class RenderTarget { void Destroy(); filament::Engine* engine_ = nullptr; + RenderTargetConfig config_; filament::RenderTarget* render_target_ = nullptr; std::unique_ptr color_texture_ = nullptr; std::unique_ptr depth_texture_ = nullptr; - RenderTargetTextureType color_type_; - RenderTargetTextureType depth_type_; int width_ = 0; int height_ = 0; }; diff --git a/src/experimental/filament/filament/renderable.cc b/src/experimental/filament/filament/renderable.cc index 6a3da110..f1a4e2bc 100644 --- a/src/experimental/filament/filament/renderable.cc +++ b/src/experimental/filament/filament/renderable.cc @@ -16,100 +16,105 @@ #include #include -#include +#include #include #include #include #include +#include +#include #include #include #include "experimental/filament/filament/draw_mode.h" #include "experimental/filament/filament/material.h" +#include "experimental/filament/filament/math_util.h" #include "experimental/filament/filament/mesh.h" #include "experimental/filament/filament/object_manager.h" namespace mujoco { -Renderable::Renderable(Usage usage, ObjectManager* object_mgr) - : usage_(usage), object_mgr_(object_mgr) {} +using filament::math::mat4f; + +void DefaultRenderableParams(RenderableParams* params) { + params->shading_model = ShadingModel::SceneObject; +} + +Renderable::Renderable(ObjectManager* object_mgr, const RenderableParams& params) + : object_mgr_(object_mgr), params_(params) {} Renderable::~Renderable() noexcept { - while (!entities_.empty()) { - RemoveLastEntity(); + filament::Engine* engine = GetEngine(); + utils::EntityManager& em = utils::EntityManager::get(); + + for (Part& part : parts_) { + if (assigned_scene_) { + assigned_scene_->remove(part.entity); + } + engine->destroy(part.entity); + em.destroy(part.entity); } for (int i = 0; i < kNumDrawModes; ++i) { if (instances_[i] != nullptr) { - GetEngine()->destroy(instances_[i]); + engine->destroy(instances_[i]); instances_[i] = nullptr; } } } -void Renderable::RemoveLastEntity() { - if (entities_.empty()) { - return; +void Renderable::SetMesh(const Mesh* mesh, int elem_offset, int elem_count) { + if (mesh == nullptr) { + mju_error("Cannot set mesh to nullptr."); } - - utils::EntityManager& em = utils::EntityManager::get(); - utils::Entity entity = entities_.back(); - - if (assigned_scene_) { - assigned_scene_->remove(entity); - } - - GetEngine()->destroy(entity); - em.destroy(entity); - entities_.pop_back(); - meshes_.pop_back(); -} - -void Renderable::UpdateMesh(int index, const Mesh* mesh, int elem_offset, - int elem_count) { - MeshInfo& mesh_info = SetMesh(index, mesh, nullptr, elem_offset, elem_count); - UpdateEntity(index, mesh_info); -} - -void Renderable::UpdateMesh(int index, MeshPtr mesh, int elem_offset, - int elem_count) { - MeshInfo& mesh_info = - SetMesh(index, mesh.get(), std::move(mesh), elem_offset, elem_count); - UpdateEntity(index, mesh_info); -} - -void Renderable::AppendMesh(const Mesh* mesh, int elem_offset, int elem_count) { - MeshInfo& mesh_info = SetMesh(-1, mesh, nullptr, elem_offset, elem_count); - AppendEntity(mesh_info); -} - -void Renderable::AppendMesh(MeshPtr mesh, int elem_offset, int elem_count) { - MeshInfo& mesh_info = - SetMesh(-1, mesh.get(), std::move(mesh), elem_offset, elem_count); - AppendEntity(mesh_info); -} - -void Renderable::AppendEntity(const MeshInfo& mesh_info) { - const Mesh* mesh = mesh_info.mesh; filament::VertexBuffer* vertex_buffer = mesh->GetFilamentVertexBuffer(); if (vertex_buffer == nullptr) { mju_error("Invalid (null) vertex buffer."); } - filament::IndexBuffer* index_buffer = mesh->GetFilamentIndexBuffer(); if (index_buffer == nullptr) { mju_error("Invalid (null) index buffer."); } - utils::Entity entity = utils::EntityManager::get().create(); - if (entity.isNull()) { + if (elem_count == 0) { + elem_count = index_buffer->getIndexCount() - elem_offset; + } + + if (parts_.empty()) { + Part& part = parts_.emplace_back(); + part.mesh = mesh; + part.elem_offset = elem_offset; + part.elem_count = elem_count; + InitPartEntity(part); + } else if (parts_.size() == 1) { + Part& part = parts_[0]; + part.mesh = mesh; + part.elem_offset = elem_offset; + part.elem_count = elem_count; + + filament::RenderableManager& rm = GetEngine()->getRenderableManager(); + rm.setGeometryAt(rm.getInstance(part.entity), 0, + part.mesh->GetPrimitiveType(), vertex_buffer, index_buffer, + part.elem_offset, part.elem_count); + + } else { + mju_error("Cannot set mesh for renderable with multiple parts."); + } +} + +void Renderable::InitPartEntity(Part& part) { + part.entity = utils::EntityManager::get().create(); + if (part.entity.isNull()) { mju_error("Failed to create entity."); } + filament::VertexBuffer* vertex_buffer = part.mesh->GetFilamentVertexBuffer(); + filament::IndexBuffer* index_buffer = part.mesh->GetFilamentIndexBuffer(); + filament::RenderableManager::Builder builder(1); - builder.geometry(0, mesh->GetPrimitiveType(), vertex_buffer, index_buffer, - mesh_info.elem_offset, mesh_info.elem_count); - if (mesh->HasBounds()) { - builder.boundingBox(mesh->GetBounds()); + builder.geometry(0, part.mesh->GetPrimitiveType(), vertex_buffer, index_buffer, + part.elem_offset, part.elem_count); + if (part.mesh->HasBounds()) { + builder.boundingBox(part.mesh->GetBounds()); } else { builder.culling(false); } @@ -123,58 +128,50 @@ void Renderable::AppendEntity(const MeshInfo& mesh_info) { builder.blendOrder(0, blend_order_); builder.screenSpaceContactShadows(true); - builder.build(*GetEngine(), entity); + builder.build(*GetEngine(), part.entity); if (assigned_scene_) { - assigned_scene_->addEntity(entity); + assigned_scene_->addEntity(part.entity); } - entities_.push_back(entity); } -void Renderable::UpdateEntity(int index, const MeshInfo& mesh_info) { - if (index < 0 || index >= entities_.size()) { - mju_error("Invalid index %d for renderable.", index); - } - utils::Entity entity = entities_[index]; - - const Mesh* mesh = mesh_info.mesh; - filament::VertexBuffer* vertex_buffer = mesh->GetFilamentVertexBuffer(); - if (vertex_buffer == nullptr) { - mju_error("Invalid (null) vertex buffer."); +void Renderable::SetTransform(const Trs& trs) { + if (parts_.empty()) { + transform_ = trs.ToTransform(); + return; } - filament::IndexBuffer* index_buffer = mesh->GetFilamentIndexBuffer(); - if (index_buffer == nullptr) { - mju_error("Invalid (null) index buffer."); + filament::TransformManager& tm = GetEngine()->getTransformManager(); + if (get_transform_fn_) { + for (int i = 0; i < parts_.size(); ++i) { + const mat4f& transform = get_transform_fn_(i, trs); + tm.setTransform(tm.getInstance(parts_[i].entity), transform); + } + } else { + for (Part& part : parts_) { + tm.setTransform(tm.getInstance(part.entity), trs.ToTransform()); + } } - - filament::RenderableManager& rm = GetEngine()->getRenderableManager(); - rm.setGeometryAt(rm.getInstance(entity), 0, mesh->GetPrimitiveType(), - vertex_buffer, index_buffer, mesh_info.elem_offset, - mesh_info.elem_count); + transform_ = tm.getTransform(tm.getInstance(parts_[0].entity)); } -Renderable::MeshInfo& Renderable::SetMesh(int index, const Mesh* mesh, - MeshPtr owned_mesh, int elem_offset, - int elem_count) { - if (index == -1) { - index = meshes_.size(); - meshes_.emplace_back(); - } - if (index < 0 || index >= static_cast(meshes_.size())) { - mju_error("Invalid index %d for renderable.", index); +const mat4f& Renderable::GetTransform() const { + return transform_; +} + +void Renderable::SetMeshes(std::span meshes, + GetTransformFn get_transform_fn) { + if (!parts_.empty()) { + mju_error("Cannot set meshes for renderable with multiple parts."); } - MeshInfo* mesh_info = &meshes_[index]; - mesh_info->owned_mesh = std::move(owned_mesh); - mesh_info->mesh = mesh; - mesh_info->elem_offset = elem_offset; - mesh_info->elem_count = elem_count; - if (mesh_info->elem_count == 0) { - const int total = - mesh_info->mesh->GetFilamentIndexBuffer()->getIndexCount(); - mesh_info->elem_count = total - mesh_info->elem_offset; + get_transform_fn_ = get_transform_fn; + for (int i = 0; i < meshes.size(); ++i) { + Part& part = parts_.emplace_back(); + part.mesh = meshes[i]; + part.elem_offset = 0; + part.elem_count = part.mesh->GetFilamentIndexBuffer()->getIndexCount(); + InitPartEntity(part); } - return *mesh_info; } void Renderable::AddToScene(filament::Scene* scene) { @@ -185,8 +182,8 @@ void Renderable::AddToScene(filament::Scene* scene) { // Entities are already added to the scene. return; } - for (utils::Entity& entity : entities_) { - scene->addEntity(entity); + for (Part& part : parts_) { + scene->addEntity(part.entity); } assigned_scene_ = scene; } @@ -195,26 +192,27 @@ void Renderable::RemoveFromScene(filament::Scene* scene) { if (assigned_scene_ != scene) { mju_error("Attempting to remove renderable from wrong scene."); } - for (utils::Entity& entity : entities_) { - scene->remove(entity); + for (Part& part : parts_) { + scene->remove(part.entity); } assigned_scene_ = nullptr; } void Renderable::UpdateMaterial(const MaterialParams& params, const MaterialTextures& textures) { - params_ = params; - textures_ = textures; + material_params_ = params; + material_textures_ = textures; AssignMaterial(DrawMode::Color, GetColorMaterialType()); - if (usage_ == Usage::SceneObject) { + if (params_.shading_model == ShadingModel::SceneObject) { AssignMaterial(DrawMode::Depth, ObjectManager::kUnlitDepth); AssignMaterial(DrawMode::Segmentation, ObjectManager::kUnlitSegmentation); } for (int i = 0; i < kNumDrawModes; ++i) { if (instances_[i]) { - UpdateMaterialInstance(instances_[i], params_, textures_, object_mgr_); + UpdateMaterialInstance(instances_[i], material_params_, + material_textures_, object_mgr_); } } SetDrawMode(draw_mode_); @@ -240,24 +238,24 @@ void Renderable::AssignMaterial(DrawMode mode, } const MaterialParams& Renderable::GetMaterialParams() const { - return params_; + return material_params_; } const MaterialTextures& Renderable::GetMaterialTextures() const { - return textures_; + return material_textures_; } void Renderable::SetDrawMode(DrawMode mode) { // Only SceneObjects support non-color draw modes. - if (usage_ != Usage::SceneObject) { + if (params_.shading_model != ShadingModel::SceneObject) { mode = DrawMode::Color; } filament::MaterialInstance* instance = instances_[static_cast(mode)]; if (instance) { filament::RenderableManager& rm = GetEngine()->getRenderableManager(); - for (utils::Entity& entity : entities_) { - filament::RenderableManager::Instance ri = rm.getInstance(entity); + for (Part& part : parts_) { + filament::RenderableManager::Instance ri = rm.getInstance(part.entity); rm.setMaterialInstanceAt(ri, 0, instance); } } @@ -270,8 +268,8 @@ std::uint8_t Renderable::SetLayerMask(std::uint8_t mask) { layer_mask_ = mask; filament::RenderableManager& rm = GetEngine()->getRenderableManager(); - for (utils::Entity& entity : entities_) { - rm.setLayerMask(rm.getInstance(entity), 0xff, layer_mask_); + for (Part& part : parts_) { + rm.setLayerMask(rm.getInstance(part.entity), 0xff, layer_mask_); } } return prev; @@ -283,8 +281,8 @@ std::uint8_t Renderable::SetPriority(std::uint8_t priority) { priority_ = priority; filament::RenderableManager& rm = GetEngine()->getRenderableManager(); - for (utils::Entity& entity : entities_) { - rm.setPriority(rm.getInstance(entity), priority_); + for (Part& part : parts_) { + rm.setPriority(rm.getInstance(part.entity), priority_); } } return prev; @@ -296,8 +294,8 @@ std::uint16_t Renderable::SetBlendOrder(std::uint16_t blend_order) { blend_order_ = blend_order; filament::RenderableManager& rm = GetEngine()->getRenderableManager(); - for (utils::Entity& entity : entities_) { - rm.setBlendOrderAt(rm.getInstance(entity), 0, blend_order_); + for (Part& part : parts_) { + rm.setBlendOrderAt(rm.getInstance(part.entity), 0, blend_order_); } } return prev; @@ -308,8 +306,8 @@ void Renderable::SetCastShadows(bool cast_shadows) { cast_shadows_ = cast_shadows; filament::RenderableManager& rm = GetEngine()->getRenderableManager(); - for (utils::Entity& entity : entities_) { - rm.setCastShadows(rm.getInstance(entity), cast_shadows_); + for (Part& part : parts_) { + rm.setCastShadows(rm.getInstance(part.entity), cast_shadows_); } } } @@ -319,8 +317,8 @@ void Renderable::SetReceiveShadows(bool receive_shadows) { receive_shadows_ = receive_shadows; filament::RenderableManager& rm = GetEngine()->getRenderableManager(); - for (utils::Entity& entity : entities_) { - rm.setReceiveShadows(rm.getInstance(entity), receive_shadows_); + for (Part& part : parts_) { + rm.setReceiveShadows(rm.getInstance(part.entity), receive_shadows_); } } } @@ -333,36 +331,33 @@ void Renderable::SetWireframe(bool wireframe) { wireframe_ = wireframe; filament::RenderableManager& rm = GetEngine()->getRenderableManager(); - for (int i = 0; i < entities_.size(); ++i) { - utils::Entity& entity = entities_[i]; - const Mesh* mesh = meshes_[i].mesh; - filament::VertexBuffer* vertex_buffer = mesh->GetFilamentVertexBuffer(); - filament::IndexBuffer* index_buffer = mesh->GetFilamentIndexBuffer(); - rm.setGeometryAt(rm.getInstance(entity), 0, - wireframe_ ? kWireframeType : mesh->GetPrimitiveType(), - vertex_buffer, index_buffer, meshes_[i].elem_offset, - meshes_[i].elem_count); + for (Part& part : parts_) { + filament::VertexBuffer* vertex_buffer = part.mesh->GetFilamentVertexBuffer(); + filament::IndexBuffer* index_buffer = part.mesh->GetFilamentIndexBuffer(); + rm.setGeometryAt(rm.getInstance(part.entity), 0, + wireframe_ ? kWireframeType : part.mesh->GetPrimitiveType(), + vertex_buffer, index_buffer, part.elem_offset, + part.elem_count); } } } - ObjectManager::MaterialType Renderable::GetColorMaterialType() const { - if (usage_ == Usage::DecorLines) { + if (params_.shading_model == ShadingModel::DecorLines) { return ObjectManager::kUnlitLine; - } else if (usage_ == Usage::Decor) { - return ObjectManager::kUnlitSegmentation; - } else if (usage_ == Usage::Ux) { + } else if (params_.shading_model == ShadingModel::Decor) { + return ObjectManager::kUnlitDecor; + } else if (params_.shading_model == ShadingModel::Ux) { return ObjectManager::kUnlitUi; - } else if (textures_.orm) { + } else if (material_textures_.orm) { return ObjectManager::kPbrPacked; - } else if (textures_.metallic) { + } else if (material_textures_.metallic) { return ObjectManager::kPbr; - } else if (textures_.roughness) { + } else if (material_textures_.roughness) { return ObjectManager::kPbr; - } else if (params_.metallic >= 0) { + } else if (material_params_.metallic >= 0) { return ObjectManager::kPbr; - } else if (params_.roughness >= 0) { + } else if (material_params_.roughness >= 0) { return ObjectManager::kPbr; } @@ -371,42 +366,42 @@ ObjectManager::MaterialType Renderable::GetColorMaterialType() const { // geometry) and `mesh_texcoordadr` stores the address of the mesh uvs if // it has them. bool has_texcoords = false; - if (!meshes_.empty()) { - const auto attribs = meshes_[0].mesh->GetVertexAttributes(); + if (!parts_.empty()) { + const auto attribs = parts_[0].mesh->GetVertexAttributes(); auto it = std::find(attribs.begin(), attribs.end(), filament::VertexAttribute::UV0); has_texcoords = (it != attribs.end()); } - if (textures_.color == nullptr) { - if (params_.color.a < 1.0f) { + if (material_textures_.color == nullptr) { + if (material_params_.color.a < 1.0f) { return ObjectManager::kPhongColorFade; - } else if (params_.reflective) { + } else if (material_params_.reflective) { return ObjectManager::kPhongColorReflect; } else { return ObjectManager::kPhongColor; } - } else if (textures_.color->GetFilamentTexture()->getTarget() == + } else if (material_textures_.color->GetFilamentTexture()->getTarget() == filament::Texture::Sampler::SAMPLER_CUBEMAP) { - if (params_.color.a < 1.0f) { + if (material_params_.color.a < 1.0f) { return ObjectManager::kPhongCubeFade; - } else if (params_.reflective) { + } else if (material_params_.reflective) { return ObjectManager::kPhongCubeReflect; } else { return ObjectManager::kPhongCube; } } else if (has_texcoords) { - if (params_.color.a < 1.0f) { + if (material_params_.color.a < 1.0f) { return ObjectManager::kPhong2dUvFade; - } else if (params_.reflective) { + } else if (material_params_.reflective) { return ObjectManager::kPhong2dUvReflect; } else { return ObjectManager::kPhong2dUv; } } else { - if (params_.color.a < 1.0f) { + if (material_params_.color.a < 1.0f) { return ObjectManager::kPhong2dFade; - } else if (params_.reflective) { + } else if (material_params_.reflective) { return ObjectManager::kPhong2dReflect; } else { return ObjectManager::kPhong2d; diff --git a/src/experimental/filament/filament/renderable.h b/src/experimental/filament/filament/renderable.h index dbe03498..9227e634 100644 --- a/src/experimental/filament/filament/renderable.h +++ b/src/experimental/filament/filament/renderable.h @@ -16,62 +16,81 @@ #define MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_RENDERABLE_H_ #include +#include +#include #include #include #include +#include #include #include "experimental/filament/filament/draw_mode.h" #include "experimental/filament/filament/material.h" +#include "experimental/filament/filament/math_util.h" #include "experimental/filament/filament/mesh.h" #include "experimental/filament/filament/object_manager.h" namespace mujoco { -// A collection of meshes and a material that, together, define an object that -// can be rendered in a scene. +// The shading model (material) for a Renderable. +enum class ShadingModel { + SceneObject, + Decor, + DecorLines, + Ux, +}; + +// Configuration parameters for a Renderable. +struct RenderableParams { + ShadingModel shading_model; +}; + +void DefaultRenderableParams(RenderableParams* params); + +// A Renderable is effectively two things: a mesh and a material. // -// Meshes can be added to the Renderable either by unique_ptr or raw pointer. -// This determines whether or not the Renderable takes ownership of the mesh. +// The mesh describes the surface geometry of the object and the material +// describes how that surface interacts with light (i.e. the color of each point +// on the surface). // -// Internally, the Renderable creates a filament::Entity for each mesh and -// assigns the same material instance to all of them. +// Defining the mesh is easy; just call SetMesh. +// +// Defining a Material happens in two stages. First, the user specifies the +// ShadingModel to use for Rendering. This describes the overall intent of +// how the Renderable will appear (e.g. lit, unlit, wireframe, etc.). Next, +// the user specifies the MaterialParams and MaterialTextures to use with the +// ShadingModel. Its these properties that ultimately define the actual material +// of the Renderable. class Renderable { public: - // How the material is to be used for rendering. - enum class Usage { - SceneObject, - Decor, - DecorLines, - Ux, - }; - // Default filament values for priority and layer mask. static constexpr std::uint8_t kDefaultPriority = 4; static constexpr std::uint8_t kDefaultLayerMask = 0x01; - Renderable(Usage usage, ObjectManager* object_mgr); + Renderable(ObjectManager* object_mgr, const RenderableParams& params); ~Renderable() noexcept; Renderable(const Renderable&) = delete; Renderable& operator=(const Renderable&) = delete; - // Appends a mesh to the renderable. The elem_offset and elem_count parameters - // can be used to specify a submesh to append. If elem_count is 0, assumes - // the entire mesh should be appended. - void AppendMesh(const Mesh* mesh, int elem_offset = 0, int elem_count = 0); - void AppendMesh(MeshPtr mesh, int elem_offset = 0, int elem_count = 0); + // Sets the mesh of the renderable. The elem_offset and elem_count parameters + // can be used to specify a submesh within the mesh. If elem_count is 0, + // assumes the entire mesh should be appended. + void SetMesh(const Mesh* mesh, int elem_offset = 0, int elem_count = 0); - // Replaces the mesh at the index with a new mesh. The elem_offset and - // elem_count parameters can be used to specify a submesh to append. If - // elem_count is 0, assumes the entire mesh should be appended. - void UpdateMesh(int index, const Mesh* mesh, int elem_offset = 0, - int elem_count = 0); - void UpdateMesh(int index, MeshPtr mesh, int elem_offset = 0, - int elem_count = 0); + // Sets the transform of the renderable. + void SetTransform(const Trs& trs); - // Returns the number of meshes that define the renderable. - int GetNumMeshes() const { return meshes_.size(); } + // Returns the current transform of the renderable. + const filament::math::mat4f& GetTransform() const; + + // Sets multiple meshes for a renderable. Users can optionally provide a + // function that will be used to compute the transform for each (sub)mesh + // relative to the transform of the renderable itself. This allows users to + // construct compound (but rigid) objects from multiple meshes. + using GetTransformFn = std::function; + void SetMeshes(std::span meshes, + GetTransformFn get_transform = nullptr); // Sets the layer mask for the managed filament Entities. Layer masks can be // used to show/hide the renderable in different views. Returns the previous @@ -103,7 +122,8 @@ class Renderable { // Removes the renderable from the given filament Scene. void RemoveFromScene(filament::Scene* scene); - // Sets the material instance for all managed entities. + // Further defines the material of the renderable. Only applies to renderables + // with a SceneObject shading model. void SetDrawMode(DrawMode mode); // Updates the parameters for the material. @@ -119,45 +139,30 @@ class Renderable { // Returns the filament Engine managing the renderables. filament::Engine* GetEngine(); - // Returns the underlying filament::entity for the given mesh. - utils::Entity operator[](int index) { return entities_[index]; } - private: - struct MeshInfo { - MeshPtr owned_mesh; + struct Part { + utils::Entity entity; const Mesh* mesh = nullptr; int elem_offset = 0; int elem_count = 0; }; - // Sets the mesh information for the mesh at the given index. If index is -1, - // a new mesh will be appended to the renderable. - MeshInfo& SetMesh(int index, const Mesh* mesh, MeshPtr owned_mesh, - int elem_offset, int elem_count); - - // Appends a new filament::Entity to the renderable, configured to use the - // given mesh. - void AppendEntity(const MeshInfo& mesh_info); - - // Updates the filament::Entity at the given index to use the given mesh. - void UpdateEntity(int index, const MeshInfo& mesh_info); - - // Removes the last filament::Entity from the renderable. - void RemoveLastEntity(); + void InitPartEntity(Part& part); void AssignMaterial(DrawMode mode, ObjectManager::MaterialType material_type); ObjectManager::MaterialType GetColorMaterialType() const; - Usage usage_; ObjectManager* object_mgr_; + RenderableParams params_; filament::MaterialInstance* instances_[kNumDrawModes] = {nullptr}; - MaterialParams params_; - MaterialTextures textures_; + MaterialParams material_params_; + MaterialTextures material_textures_; DrawMode draw_mode_ = DrawMode::Color; filament::Scene* assigned_scene_ = nullptr; - std::vector entities_; - std::vector meshes_; + std::vector parts_; + filament::math::mat4f transform_; + GetTransformFn get_transform_fn_; std::uint8_t priority_ = kDefaultPriority; std::uint8_t layer_mask_ = kDefaultLayerMask; std::uint16_t blend_order_ = 0; diff --git a/src/experimental/filament/filament/scene_bridge.cc b/src/experimental/filament/filament/scene_bridge.cc index e8198b00..3438b8c9 100644 --- a/src/experimental/filament/filament/scene_bridge.cc +++ b/src/experimental/filament/filament/scene_bridge.cc @@ -381,6 +381,10 @@ void SceneBridge::Update(const mjrRect& viewport, const mjvScene* scene) { } } + if (geom->type == mjGEOM_FLEX || geom->type == mjGEOM_SKIN) { + model_objects_->CreateSkinFlexMesh(scene, *geom); + } + std::unique_ptr renderable = CreateGeomRenderable( *geom, scene, object_mgr_, model_objects_.get(), headpos); diff --git a/src/experimental/filament/filament/scene_geom_util.cc b/src/experimental/filament/filament/scene_geom_util.cc index e60bacf4..80c653a7 100644 --- a/src/experimental/filament/filament/scene_geom_util.cc +++ b/src/experimental/filament/filament/scene_geom_util.cc @@ -18,6 +18,7 @@ #include #include #include +#include #include #include @@ -44,30 +45,12 @@ namespace mujoco { using filament::math::float2; using filament::math::float3; using filament::math::float4; -using filament::math::mat4; +using filament::math::mat4f; // An arbitrary scale factor for arrows. static constexpr float kArrowScale = 1.f / 6.f; static constexpr float kArrowHeadSize = 1.75f; -// Some built-in geometries are actually composed of multiple simple shapes. A -// capsule, for example, is a open-ended tube with two dome ends. We use these -// constants to help identify which entity (by index) represents which part of -// the overall shape. -static constexpr int kCapsuleTopDome = 1; -static constexpr int kCapsuleBottomDome = 2; -static constexpr int kCylinderTopDisk = 1; -static constexpr int kCylinderBottomDisk = 2; -static constexpr int kArrow0Cone = 1; -static constexpr int kArrow0ConeDisk = 2; -static constexpr int kArrow0BottomDisk = 3; -static constexpr int kArrow1Cone = 1; -static constexpr int kArrow1BottomDisk = 2; -static constexpr int kArrow2TopCone = 1; -static constexpr int kArrow2BottomCone = 2; -static constexpr int kArrow2TopConeDisk = 3; -static constexpr int kArrow2BottomConeDisk = 4; - // Returns the tile size for infinite plane texture alignment. // This is duplicated from engine_vis_visualize.c (re-center infinite plane) // to ensure UV scaling matches the re-centering increments. @@ -87,106 +70,265 @@ static bool IsBehind(const float* headpos, const float* pos, const float* mat) { 0.0f); } -static void AddMesh(Renderable& renderable, ModelObjects* model_objs, - int data_id) { +static const Mesh* GetMesh(ModelObjects* model_objs, int data_id) { const Mesh* mesh = model_objs->GetMeshBuffer(data_id); if (mesh == nullptr) { mju_error("Unknown mesh %d", data_id); } - renderable.AppendMesh(mesh); + return mesh; } -static void AddGeom(Renderable& renderable, ModelObjects* model_objs, - const mjvScene* scene, const mjvGeom& geom) { - if (geom.type == mjGEOM_FLEX) { - renderable.AppendMesh(model_objs->CreateFlexMesh(scene, geom)); - } else if (geom.type == mjGEOM_SKIN) { - renderable.AppendMesh(model_objs->CreateSkinMesh(scene, geom)); - } +static const Mesh* GetSkinFlexMesh(ModelObjects* model_objs, int objid) { + return model_objs->GetFlexSkinGeomMesh(objid); } -static void AddHeightField(Renderable& renderable, ModelObjects* model_objs, - int hfield_id) { +static const Mesh* GetHeightField(ModelObjects* model_objs, int hfield_id) { const Mesh* mesh = model_objs->GetHeightFieldBuffer(hfield_id); if (mesh == nullptr) { mju_error("Unknown height field %d", hfield_id); } - renderable.AppendMesh(mesh); + return mesh; } -static void AddShape(Renderable& renderable, ModelObjects* model_objs, - ModelObjects::ShapeType shape_type) { +static const Mesh* GetShape(ModelObjects* model_objs, + ModelObjects::ShapeType shape_type) { const Mesh* mesh = model_objs->GetShapeBuffer(shape_type); if (mesh == nullptr) { mju_error("Unknown shape %d", shape_type); } - renderable.AppendMesh(mesh); + return mesh; } static void PrepareGeomMeshes(Renderable& renderable, const mjvGeom& geom, const mjvScene* scene, ModelObjects* model_objects) { + std::vector meshes; + Renderable::GetTransformFn get_transforms; + + Trs trs = { + .translation = ReadFloat3(geom.pos), + .rotation = ReadMat3(geom.mat), + .size = ReadFloat3(geom.size), + }; + switch ((mjtGeom)geom.type) { case mjGEOM_MESH: - AddMesh(renderable, model_objects, geom.dataid); + meshes.push_back(GetMesh(model_objects, geom.dataid)); + // Ignore size for meshes. + trs.size = float3{1.0f, 1.0f, 1.0f}; break; case mjGEOM_HFIELD: - AddHeightField(renderable, model_objects, geom.dataid); + meshes.push_back(GetHeightField(model_objects, geom.dataid)); + // Ignore size for height fields. + trs.size = float3{1.0f, 1.0f, 1.0f}; break; - case mjGEOM_PLANE: - AddShape(renderable, model_objects, ModelObjects::kPlane); + case mjGEOM_PLANE: { + meshes.push_back(GetShape(model_objects, ModelObjects::kPlane)); + const bool is_infinite = !(trs.size.x > 0 && trs.size.y > 0); + if (is_infinite) { + // Infinite planes are scaled to match the tile size used by + // re-centering in engine_vis_visualize.c. + const float plane_scale = static_cast(mjMAXPLANEGRID) / 2.0f; + trs.size.x = plane_scale; + trs.size.y = plane_scale; + } + // Planes only define an xy size, so set the z-dimension to 1.0f. + trs.size.z = 1.0f; break; + } case mjGEOM_SPHERE: - AddShape(renderable, model_objects, ModelObjects::kSphere); + meshes.push_back(GetShape(model_objects, ModelObjects::kSphere)); break; case mjGEOM_ELLIPSOID: - AddShape(renderable, model_objects, ModelObjects::kSphere); + meshes.push_back(GetShape(model_objects, ModelObjects::kSphere)); break; case mjGEOM_BOX: - AddShape(renderable, model_objects, ModelObjects::kBox); + meshes.push_back(GetShape(model_objects, ModelObjects::kBox)); break; - case mjGEOM_CAPSULE: - AddShape(renderable, model_objects, ModelObjects::kTube); - AddShape(renderable, model_objects, ModelObjects::kDome); - AddShape(renderable, model_objects, ModelObjects::kDome); + case mjGEOM_CAPSULE: { + // Capsules are a tube with two domes at the ends. + meshes.push_back(GetShape(model_objects, ModelObjects::kTube)); + meshes.push_back(GetShape(model_objects, ModelObjects::kDome)); + meshes.push_back(GetShape(model_objects, ModelObjects::kDome)); + + get_transforms = [](int index, const Trs& trs) { + // We apply an inverse scale to the domes to counteract the capsule's + // overall scale so that the domes remain spherical in shape. + const float xz_size = 0.5f * (trs.size.x + trs.size.y); + if (index == 0) { + return trs.ToTransform(); + } else if (index == 1) { + // Move the first dome to the top of the capsule. + mat4f top = mat4f(trs.rotation, trs.translation); + top *= mat4f::translation(float3{0, 0, trs.size.z}); + top *= mat4f::scaling(float3{trs.size.x, trs.size.y, xz_size}); + return top; + } else if (index == 2) { + // Move the second dome to the bottom of the capsule and rotate it 180 + // degrees so that it's facing the right way. + mat4f bottom = mat4f(trs.rotation, trs.translation); + bottom *= mat4f::translation(float3{0, 0, -trs.size.z}); + bottom *= mat4f::rotation(std::numbers::pi, float3{1, 0, 0}); + bottom *= mat4f::scaling(float3{trs.size.x, trs.size.y, xz_size}); + return bottom; + } else { + mju_error("Invalid index for capsule geom: %d (expected [0,2])", index); + return trs.ToTransform(); + } + }; break; - case mjGEOM_CYLINDER: - AddShape(renderable, model_objects, ModelObjects::kTube); - AddShape(renderable, model_objects, ModelObjects::kDisk); - AddShape(renderable, model_objects, ModelObjects::kDisk); + } + case mjGEOM_CYLINDER: { + // Cylinders are a tube with two disks at the ends. + meshes.push_back(GetShape(model_objects, ModelObjects::kTube)); + meshes.push_back(GetShape(model_objects, ModelObjects::kDisk)); + meshes.push_back(GetShape(model_objects, ModelObjects::kDisk)); + + get_transforms = [](int index, const Trs& trs) { + if (index == 0) { + return trs.ToTransform(); + } else if (index == 1) { + // Move the first disk to the top of the cylinder. + mat4f top = mat4f(trs.rotation, trs.translation); + top *= mat4f::translation(float3{0, 0, trs.size.z}); + top *= mat4f::scaling(trs.size); + return top; + } else if (index == 2) { + // Move the second disk to the bottom of the cylinder. Rotate the disk + // 180 degrees so that the normals point outwards. + mat4f bottom = mat4f(trs.rotation, trs.translation); + bottom *= mat4f::translation(float3{0, 0, -trs.size.z}); + bottom *= mat4f::rotation(std::numbers::pi, float3{1, 0, 0}); + bottom *= mat4f::scaling(trs.size); + return bottom; + } else { + mju_error("Invalid index for cylinder geom: %d (expected [0,2])", index); + return trs.ToTransform(); + } + }; break; - case mjGEOM_ARROW: - AddShape(renderable, model_objects, ModelObjects::kTube); - AddShape(renderable, model_objects, ModelObjects::kCone); - AddShape(renderable, model_objects, ModelObjects::kDisk); + } + case mjGEOM_ARROW: { + meshes.push_back(GetShape(model_objects, ModelObjects::kTube)); + meshes.push_back(GetShape(model_objects, ModelObjects::kCone)); + meshes.push_back(GetShape(model_objects, ModelObjects::kDisk)); + meshes.push_back(GetShape(model_objects, ModelObjects::kDisk)); + + get_transforms = [](int index, const Trs& trs) { + mat4f base = mat4f(trs.rotation, trs.translation); + base *= mat4f::scaling(float3{1, 1, kArrowScale}); + base *= mat4f::translation(float3{0, 0, trs.size.z}); + if (index == 0) { + return base * mat4f::scaling(trs.size); + } else if (index == 1) { + mat4f top = base; + top *= mat4f::translation(float3{0, 0, trs.size.z}); + top *= mat4f::scaling(float3{kArrowHeadSize, kArrowHeadSize, 1.0f}); + return top * mat4f::scaling(trs.size); + } else if (index == 2) { + mat4f top_disk = base; + top_disk *= mat4f::translation(float3{0, 0, trs.size.z}); + top_disk *= mat4f::rotation(std::numbers::pi, float3{1, 0, 0}); + top_disk *= mat4f::scaling(float3{kArrowHeadSize, kArrowHeadSize, 1.0f}); + return top_disk * mat4f::scaling(trs.size); + } else if (index == 3) { + mat4f bottom = base; + bottom *= mat4f::translation(float3{0, 0, -trs.size.z}); + bottom *= mat4f::rotation(std::numbers::pi, float3{1, 0, 0}); + return bottom * mat4f::scaling(trs.size); + } else { + mju_error("Invalid index for arrow geom: %d (expected [0,3])", index); + return trs.ToTransform(); + } + }; break; - case mjGEOM_ARROW1: - AddShape(renderable, model_objects, ModelObjects::kTube); - AddShape(renderable, model_objects, ModelObjects::kCone); - AddShape(renderable, model_objects, ModelObjects::kDisk); - AddShape(renderable, model_objects, ModelObjects::kDisk); + } + case mjGEOM_ARROW1: { + meshes.push_back(GetShape(model_objects, ModelObjects::kTube)); + meshes.push_back(GetShape(model_objects, ModelObjects::kCone)); + meshes.push_back(GetShape(model_objects, ModelObjects::kDisk)); + + get_transforms = [](int index, const Trs& trs) { + mat4f base = mat4f(trs.rotation, trs.translation); + base *= mat4f::scaling(float3{1, 1, kArrowScale}); + base *= mat4f::translation(float3{0, 0, trs.size.z}); + if (index == 0) { + return base * mat4f::scaling(trs.size); + } else if (index == 1) { + mat4f top = base; + top *= mat4f::translation(float3{0, 0, trs.size.z}); + return top * mat4f::scaling(trs.size); + } else if (index == 2) { + mat4f bottom = base; + bottom *= mat4f::translation(float3{0, 0, -trs.size.z}); + bottom *= mat4f::rotation(std::numbers::pi, float3{1, 0, 0}); + return bottom * mat4f::scaling(trs.size); + } else { + mju_error("Invalid index for arrow1 geom: %d (expected [0,2])", index); + return trs.ToTransform(); + } + }; break; - case mjGEOM_ARROW2: - AddShape(renderable, model_objects, ModelObjects::kTube); - AddShape(renderable, model_objects, ModelObjects::kCone); - AddShape(renderable, model_objects, ModelObjects::kCone); - AddShape(renderable, model_objects, ModelObjects::kDisk); - AddShape(renderable, model_objects, ModelObjects::kDisk); + } + case mjGEOM_ARROW2: { + meshes.push_back(GetShape(model_objects, ModelObjects::kTube)); + meshes.push_back(GetShape(model_objects, ModelObjects::kCone)); + meshes.push_back(GetShape(model_objects, ModelObjects::kCone)); + meshes.push_back(GetShape(model_objects, ModelObjects::kDisk)); + meshes.push_back(GetShape(model_objects, ModelObjects::kDisk)); + + get_transforms = [](int index, const Trs& trs) { + mat4f base = mat4f(trs.rotation, trs.translation); + base *= mat4f::scaling(float3{1, 1, kArrowScale}); + base *= mat4f::translation(float3{0, 0, trs.size.z}); + if (index == 0) { + return base * mat4f::scaling(trs.size); + } else if (index == 1) { + mat4f top = base; + top *= mat4f::translation(float3{0, 0, trs.size.z}); + top *= mat4f::scaling(float3{kArrowHeadSize, kArrowHeadSize, 1.0f}); + return top * mat4f::scaling(trs.size); + } else if (index == 2) { + mat4f bottom = base; + bottom *= mat4f::translation(float3{0, 0, -trs.size.z}); + bottom *= mat4f::rotation(std::numbers::pi, float3{1, 0, 0}); + bottom *= mat4f::scaling(float3{kArrowHeadSize, kArrowHeadSize, 1.0f}); + return bottom * mat4f::scaling(trs.size); + } else if (index == 3) { + mat4f top_disk = base; + top_disk *= mat4f::translation(float3{0, 0, trs.size.z}); + top_disk *= mat4f::rotation(std::numbers::pi, float3{1, 0, 0}); + top_disk *= mat4f::scaling(float3{kArrowHeadSize, kArrowHeadSize, 1.0f}); + return top_disk * mat4f::scaling(trs.size); + } else if (index == 4) { + mat4f bottom_disk = base; + bottom_disk *= mat4f::translation(float3{0, 0, -trs.size.z}); + return bottom_disk * mat4f::scaling(trs.size); + } else { + mju_error("Invalid index for arrow2 geom: %d (expected [0,4])", index); + return trs.ToTransform(); + } + }; break; + } case mjGEOM_LINE: - AddShape(renderable, model_objects, ModelObjects::kLine); + meshes.push_back(GetShape(model_objects, ModelObjects::kLine)); break; case mjGEOM_LINEBOX: - AddShape(renderable, model_objects, ModelObjects::kLineBox); + meshes.push_back(GetShape(model_objects, ModelObjects::kLineBox)); break; case mjGEOM_TRIANGLE: - AddShape(renderable, model_objects, ModelObjects::kTriangle); + meshes.push_back(GetShape(model_objects, ModelObjects::kTriangle)); break; case mjGEOM_FLEX: - AddGeom(renderable, model_objects, scene, geom); + meshes.push_back(GetSkinFlexMesh(model_objects, geom.objid)); + // Flexes are defined in global space. + trs = Trs(); break; case mjGEOM_SKIN: - AddGeom(renderable, model_objects, scene, geom); + meshes.push_back(GetSkinFlexMesh(model_objects, geom.objid)); + // Skins are defined in global space. + trs = Trs(); break; case mjGEOM_NONE: case mjGEOM_LABEL: @@ -197,124 +339,9 @@ static void PrepareGeomMeshes(Renderable& renderable, const mjvGeom& geom, mju_warning("Unsupported geom type: %d", geom.type); break; } -} -static void SetGeomTransform(Renderable& renderable, const mjvGeom& geom) { - // Flex and skin geometries are in global space. - if (geom.type == mjGEOM_FLEX || geom.type == mjGEOM_SKIN) { - return; - } - - mat4 transform = mat4(ReadMat3(geom.mat), ReadFloat3(geom.pos)); - renderable.SetLayerMask(geom.category); - - float3 size = ReadFloat3(geom.size); - filament::TransformManager& tm = - renderable.GetEngine()->getTransformManager(); - for (int j = 0; j < renderable.GetNumMeshes(); ++j) { - const utils::Entity& entity = renderable[j]; - - // Update object transform. - mat4 entity_transform = transform; - - // Some built-in drawables are composed of multiple entities. For example, - // capsules are a combination of a open tube and two dome end caps. - - if (geom.type == mjGEOM_CYLINDER) { - // Cylinders are a tube with two disks at the ends. The "bottom" disk is - // rotated so that the normals point outwards. - if (j == kCylinderTopDisk) { - entity_transform *= mat4::translation(float3{0, 0, size.z}); - } else if (j == kCylinderBottomDisk) { - entity_transform *= mat4::translation(float3{0, 0, -size.z}); - entity_transform *= mat4::rotation(std::numbers::pi, float3{1, 0, 0}); - } - } else if (geom.type == mjGEOM_CAPSULE) { - // Capsules are a tube with two domes at the ends. We apply an inverse - // scale to the domes to "counteract" the capsule's overall scale so that - // the domes remain spherical in shape. - const float xz_size = 0.5f * (size.x + size.y); - if (j == kCapsuleTopDome) { - entity_transform *= mat4::translation(float3{0, 0, size.z}); - entity_transform *= mat4::scaling(float3{1, 1, xz_size / size.z}); - } else if (j == kCapsuleBottomDome) { - entity_transform *= mat4::translation(float3{0, 0, -size.z}); - entity_transform *= mat4::rotation(std::numbers::pi, float3{1, 0, 0}); - entity_transform *= mat4::scaling(float3{1, 1, xz_size / size.z}); - } - } else if (geom.type == mjGEOM_ARROW) { - // An arrow is a tube with a cone at the end and a disk cap at the other - // end. Because the cone head's base is larger than the tube, an extra - // disk is added to the base of the cone. This disk is rotated such that - // its normal points outwards. - entity_transform *= mat4::scaling(float3{1, 1, kArrowScale}); - entity_transform *= mat4::translation(float3{0, 0, size.z}); - if (j == kArrow0Cone) { - entity_transform *= mat4::translation(float3{0, 0, size.z}); - entity_transform *= - mat4::scaling(float3{kArrowHeadSize, kArrowHeadSize, 1.0f}); - } else if (j == kArrow0ConeDisk) { - entity_transform *= mat4::translation(float3{0, 0, size.z}); - entity_transform *= mat4::rotation(std::numbers::pi, float3{1, 0, 0}); - entity_transform *= - mat4::scaling(float3{kArrowHeadSize, kArrowHeadSize, 1.0f}); - } else if (j == kArrow0BottomDisk) { - entity_transform *= mat4::translation(float3{0, 0, -size.z}); - entity_transform *= mat4::rotation(std::numbers::pi, float3{1, 0, 0}); - } - } else if (geom.type == mjGEOM_ARROW1) { - // An arrow1 is a tube with a cone at the end and a disk cap at the other - // end. - entity_transform *= mat4::scaling(float3{1, 1, kArrowScale}); - entity_transform *= mat4::translation(float3{0, 0, size.z}); - if (j == kArrow1Cone) { - entity_transform *= mat4::translation(float3{0, 0, size.z}); - } else if (j == kArrow1BottomDisk) { - entity_transform *= mat4::translation(float3{0, 0, -size.z}); - entity_transform *= mat4::rotation(std::numbers::pi, float3{1, 0, 0}); - } - } else if (geom.type == mjGEOM_ARROW2) { - // An arrow2 is a tube with a cone at both ends. Like the standard arrow, - // an extra disk is added to the base of each cone. - entity_transform *= mat4::scaling(float3{1, 1, kArrowScale}); - entity_transform *= mat4::translation(float3{0, 0, size.z}); - if (j == kArrow2TopCone) { - entity_transform *= mat4::translation(float3{0, 0, size.z}); - entity_transform *= - mat4::scaling(float3{kArrowHeadSize, kArrowHeadSize, 1.0f}); - } else if (j == kArrow2BottomCone) { - entity_transform *= mat4::translation(float3{0, 0, -size.z}); - entity_transform *= mat4::rotation(std::numbers::pi, float3{1, 0, 0}); - entity_transform *= - mat4::scaling(float3{kArrowHeadSize, kArrowHeadSize, 1.0f}); - } else if (j == kArrow2TopConeDisk) { - entity_transform *= mat4::translation(float3{0, 0, size.z}); - entity_transform *= mat4::rotation(std::numbers::pi, float3{1, 0, 0}); - entity_transform *= - mat4::scaling(float3{kArrowHeadSize, kArrowHeadSize, 1.0f}); - } else if (j == kArrow2BottomConeDisk) { - entity_transform *= mat4::translation(float3{0, 0, -size.z}); - entity_transform *= - mat4::scaling(float3{kArrowHeadSize, kArrowHeadSize, 1.0f}); - } - } - if (geom.type == mjGEOM_PLANE) { - const bool is_infinite = !(size.x > 0 && size.y > 0); - if (is_infinite) { - // Infinite planes are scaled to match the tile size used by - // re-centering in engine_vis_visualize.c. - const float plane_scale = static_cast(mjMAXPLANEGRID) / 2.0f; - entity_transform *= - mat4::scaling(float3{plane_scale, plane_scale, 1.0f}); - } else { - // Regular planes are scaled by geom.size. - entity_transform *= mat4::scaling(float3{size.x, size.y, 1.0f}); - } - } else if (geom.type != mjGEOM_MESH && geom.type != mjGEOM_HFIELD) { - entity_transform *= mat4::scaling(size); - } - tm.setTransform(tm.getInstance(entity), entity_transform); - } + renderable.SetMeshes(meshes, get_transforms); + renderable.SetTransform(trs); } static void UpdateGeomMaterial(Renderable& renderable, const mjvGeom& geom, @@ -338,10 +365,12 @@ static void UpdateGeomMaterial(Renderable& renderable, const mjvGeom& geom, enable_reflection && geom.reflectance > 0 && params.color.a == 1.0f; } } - renderable.SetWireframe(scene->flags[mjRND_WIREFRAME]); + renderable.SetLayerMask(geom.category); if (geom.category == mjCAT_DECOR) { renderable.SetCastShadows(false); renderable.SetReceiveShadows(false); + } else { + renderable.SetWireframe(scene->flags[mjRND_WIREFRAME]); } MaterialTextures textures; @@ -463,19 +492,19 @@ static void UpdateGeomMaterial(Renderable& renderable, const mjvGeom& geom, std::unique_ptr CreateGeomRenderable( const mjvGeom& geom, const mjvScene* scene, ObjectManager* object_mgr, ModelObjects* model_objs, const float headpos[3]) { - Renderable::Usage usage = Renderable::Usage::SceneObject; + ShadingModel shading_model = ShadingModel::SceneObject; if (geom.type == mjGEOM_LINE || geom.type == mjGEOM_LINEBOX) { - usage = Renderable::Usage::DecorLines; + shading_model = ShadingModel::DecorLines; } else if (geom.category == mjCAT_DECOR) { - usage = Renderable::Usage::Decor; + shading_model = ShadingModel::Decor; } - auto renderable = std::make_unique(usage, object_mgr); + RenderableParams config; + DefaultRenderableParams(&config); + config.shading_model = shading_model; + auto renderable = std::make_unique(object_mgr, config); - // The order of these calls is important. e.g. We need to create the filament - // renderable entities before we can set their transform. PrepareGeomMeshes(*renderable, geom, scene, model_objs); - SetGeomTransform(*renderable, geom); UpdateGeomMaterial(*renderable, geom, scene, model_objs, object_mgr, headpos); return renderable; diff --git a/src/experimental/filament/filament/scene_view.cc b/src/experimental/filament/filament/scene_view.cc index d6832f6f..efcbd76f 100644 --- a/src/experimental/filament/filament/scene_view.cc +++ b/src/experimental/filament/filament/scene_view.cc @@ -277,13 +277,11 @@ void SceneView::Render(filament::Renderer* renderer, // Render reflection passes. if (request.draw_mode == DrawMode::Color) { - filament::TransformManager& tm = engine_->getTransformManager(); for (size_t i = 0; i < reflectives_.size(); ++i) { Renderable* renderable = reflectives_[i]; // We assume the 0th entity is the reflective entity. - const utils::Entity entity = (*renderable)[0]; - const mat4 transform(tm.getTransform(tm.getInstance(entity))); + mat4 transform(renderable->GetTransform()); SetupReflectionCamera(transform, camera_, reflect_camera_); // Hide reflective surface from its own reflection pass. @@ -305,8 +303,7 @@ void SceneView::Render(filament::Renderer* renderer, if (request.enable_ux) { ux_camera_->setProjection(filament::Camera::Projection::ORTHO, 0.0f, - viewport.width / request.gui_scale, - viewport.height / request.gui_scale, 0.0f, 0.0f, + viewport.width, viewport.height, 0.0f, 0.0f, 1.0f); ux_view_->setRenderTarget(render_target); renderer->render(ux_view_); @@ -325,9 +322,12 @@ void SceneView::AddReflectiveRenderable(Renderable* renderable) { // Ensure we have the same number of render targets as we do reflective // renderables. while (reflect_targets_.size() < reflectives_.size()) { - reflect_targets_.push_back(std::make_unique( - engine_, RenderTargetTextureType::kReflectionColor, - RenderTargetTextureType::kDepth)); + RenderTargetConfig config; + DefaultRenderTargetConfig(&config); + + config.color_format = mjPIXEL_FORMAT_RGBA8; + config.depth_format = mjPIXEL_FORMAT_DEPTH32F; + reflect_targets_.push_back(std::make_unique(engine_, config)); } // Prepare a render target for the reflective renderable. diff --git a/src/experimental/filament/filament/scene_view.h b/src/experimental/filament/filament/scene_view.h index f50a86fa..a9a2874b 100644 --- a/src/experimental/filament/filament/scene_view.h +++ b/src/experimental/filament/filament/scene_view.h @@ -69,8 +69,6 @@ class SceneView { RenderTarget* target = nullptr; // Whether or not to render the UX as a separate pass. bool enable_ux = false; - // The scale factor to use for UX rendering. - float gui_scale = 1.0f; }; // Renders the scene. diff --git a/src/experimental/filament/filament/texture.cc b/src/experimental/filament/filament/texture.cc index 1ce62774..1277ec44 100644 --- a/src/experimental/filament/filament/texture.cc +++ b/src/experimental/filament/filament/texture.cc @@ -99,6 +99,10 @@ static filament::Texture::InternalFormat GetTextureInternalFormat( return filament::Texture::InternalFormat::RGB8; case mjPIXEL_FORMAT_RGBA8: return filament::Texture::InternalFormat::RGBA8; + case mjPIXEL_FORMAT_R32F: + return filament::Texture::InternalFormat::R32F; + case mjPIXEL_FORMAT_DEPTH32F: + return filament::Texture::InternalFormat::DEPTH32F; default: mju_error("Unsupported format: %d", (int)config.format); return filament::Texture::InternalFormat::UNUSED; @@ -114,7 +118,8 @@ void DefaultTextureConfig(TextureConfig* config) { std::memset(config, 0, sizeof(TextureConfig)); } -Texture::Texture(filament::Engine* engine, const TextureConfig& config) +Texture::Texture(filament::Engine* engine, const TextureConfig& config, + InternalFlags flags) : engine_(engine), config_(config) { if (IsCompressed(config_)) { // We defer creation of compressed textures until Upload() is called. In @@ -139,45 +144,19 @@ Texture::Texture(filament::Engine* engine, const TextureConfig& config) builder.sampler(filament::Texture::Sampler::SAMPLER_2D); } - if (config_.color_space != mjCOLORSPACE_SRGB) { - builder.usage(filament::Texture::Usage::GEN_MIPMAPPABLE | - filament::Texture::Usage::SAMPLEABLE | - filament::Texture::Usage::UPLOADABLE); + filament::Texture::Usage usage = filament::Texture::Usage::DEFAULT; + if (flags.color_attachment) { + usage |= filament::Texture::Usage::COLOR_ATTACHMENT; + usage |= filament::Texture::Usage::BLIT_SRC; + } else if (flags.depth_attachment) { + usage |= filament::Texture::Usage::DEPTH_ATTACHMENT; + usage |= filament::Texture::Usage::BLIT_SRC; + } else if (config_.color_space != mjCOLORSPACE_SRGB) { + usage |= filament::Texture::Usage::GEN_MIPMAPPABLE; } - texture_ = builder.build(*engine_); -} + builder.usage(usage); -Texture::Texture(filament::Engine* engine, RenderTargetTextureType type, - int width, int height) : engine_(engine) { - filament::Texture::Builder builder; - builder.width(width); - builder.height(height); - switch (type) { - case RenderTargetTextureType::kColor: - builder.usage(filament::Texture::Usage::COLOR_ATTACHMENT | - filament::Texture::Usage::BLIT_SRC); - builder.format(filament::Texture::InternalFormat::RGB8); - break; - case RenderTargetTextureType::kDepth: - builder.usage(filament::Texture::Usage::DEPTH_ATTACHMENT | - filament::Texture::Usage::SAMPLEABLE); - builder.format(filament::Texture::InternalFormat::DEPTH32F); - break; - case RenderTargetTextureType::kDepthColor: - builder.usage(filament::Texture::Usage::COLOR_ATTACHMENT | - filament::Texture::Usage::BLIT_SRC); - builder.format(filament::Texture::InternalFormat::R32F); - break; - case RenderTargetTextureType::kReflectionColor: - builder.usage(filament::Texture::Usage::COLOR_ATTACHMENT | - filament::Texture::Usage::BLIT_SRC | - filament::Texture::Usage::SAMPLEABLE); - builder.format(filament::Texture::InternalFormat::RGBA8); - break; - default: - mju_error("Unknown type: %d", static_cast(type)); - } - texture_ = builder.build(*engine); + texture_ = builder.build(*engine_); } Texture::~Texture() { diff --git a/src/experimental/filament/filament/texture.h b/src/experimental/filament/filament/texture.h index 44b0ea31..b1493470 100644 --- a/src/experimental/filament/filament/texture.h +++ b/src/experimental/filament/filament/texture.h @@ -25,29 +25,13 @@ // Functions for creating filament textures. namespace mujoco { -// The types of textures we can create. For internal use only. -enum class TextureTarget { - // A standard 2D image with a width and a height. - kNormal2d, - // A 2D texture split up into the 6 faces of a cube. - kCube, -}; - -// The different types of textures we can create for a render target. -// For internal use only. -enum class RenderTargetTextureType { - kColor, - kDepth, - kDepthColor, - kReflectionColor, -}; - // Pixel formats for textures. typedef enum mjtPixelFormat_ { mjPIXEL_FORMAT_UNKNOWN = 0, mjPIXEL_FORMAT_R8, mjPIXEL_FORMAT_RGB8, mjPIXEL_FORMAT_RGBA8, + mjPIXEL_FORMAT_R32F, mjPIXEL_FORMAT_DEPTH32F, mjPIXEL_FORMAT_KTX, } mjtPixelFormat; @@ -98,12 +82,16 @@ void DefaultTextureConfig(TextureConfig* config); // Wrapper around a filament::Texture. class Texture { public: - // Creates a texture with the given data. - Texture(filament::Engine* engine, const TextureConfig& config); + // Flags for internal use. + struct InternalFlags { + InternalFlags() : color_attachment(false), depth_attachment(false) {} + bool color_attachment; + bool depth_attachment; + }; - // Creates a texture for use with a render target, for internal use. - Texture(filament::Engine* engine, RenderTargetTextureType type, int width, - int height); + // Creates a texture with the given data. + Texture(filament::Engine* engine, const TextureConfig& config, + InternalFlags flags = InternalFlags()); ~Texture(); diff --git a/src/experimental/platform/helpers.cc b/src/experimental/platform/helpers.cc index add021f9..e10d8e8d 100644 --- a/src/experimental/platform/helpers.cc +++ b/src/experimental/platform/helpers.cc @@ -14,6 +14,7 @@ #include "experimental/platform/helpers.h" +#include #include #include #include @@ -23,6 +24,7 @@ #include #include #include +#include #include "webp/encode.h" #include "webp/types.h" @@ -77,8 +79,13 @@ std::string ResolveFile(const std::string& filename, return resolved; } + std::vector entries; for (const auto& it : std::filesystem::recursive_directory_iterator(path)) { - resolved = CheckPathForFile(it.path(), filename); + entries.push_back(it.path()); + } + std::sort(entries.begin(), entries.end()); + for (const auto& entry : entries) { + resolved = CheckPathForFile(entry, filename); if (!resolved.empty()) { return resolved; } diff --git a/src/experimental/platform/ux/imgui_widgets.cc b/src/experimental/platform/ux/imgui_widgets.cc index 9ffbbe3e..61d2989a 100644 --- a/src/experimental/platform/ux/imgui_widgets.cc +++ b/src/experimental/platform/ux/imgui_widgets.cc @@ -321,7 +321,7 @@ bool ImGui_Slider(const char* name, mjtNum* value, mjtNum min, mjtNum max) { float f = *value; const bool res = ImGui::SliderFloat(name, &f, min, max); if (res) { - *value = f; + *value = mju_clip(f, min, max); } return res; } diff --git a/src/experimental/usd/plugins/mjcf/mujoco_to_usd.cc b/src/experimental/usd/plugins/mjcf/mujoco_to_usd.cc index 2820a3aa..ccc4158c 100644 --- a/src/experimental/usd/plugins/mjcf/mujoco_to_usd.cc +++ b/src/experimental/usd/plugins/mjcf/mujoco_to_usd.cc @@ -649,7 +649,6 @@ class ModelWriter { }; const std::vector> enable_flags = { - {MjcPhysicsTokens->mjcFlagMulticcd, mjENBL_MULTICCD}, {MjcPhysicsTokens->mjcFlagFwdinv, mjENBL_FWDINV}, {MjcPhysicsTokens->mjcFlagEnergy, mjENBL_ENERGY}, {MjcPhysicsTokens->mjcFlagOverride, mjENBL_OVERRIDE}, @@ -677,7 +676,8 @@ class ModelWriter { {MjcPhysicsTokens->mjcFlagEulerdamp, mjDSBL_EULERDAMP}, {MjcPhysicsTokens->mjcFlagAutoreset, mjDSBL_AUTORESET}, {MjcPhysicsTokens->mjcFlagNativeccd, mjDSBL_NATIVECCD}, - {MjcPhysicsTokens->mjcFlagIsland, mjDSBL_ISLAND}}; + {MjcPhysicsTokens->mjcFlagIsland, mjDSBL_ISLAND}, + {MjcPhysicsTokens->mjcFlagMulticcd, mjDSBL_MULTICCD}}; for (const auto &[token, flag] : disable_flags) { create_flag_attr(token, flag, false); } diff --git a/src/ui/ui_main.c b/src/ui/ui_main.c index ffc2fca2..14f588f1 100644 --- a/src/ui/ui_main.c +++ b/src/ui/ui_main.c @@ -849,8 +849,9 @@ static void setslider(mjuiItem* it, mjUI* ui, rx = mju_round(rx * it->slider.divisions) / mjMAX(1, it->slider.divisions); rx = mjMAX(0, mjMIN(1, rx)); - // compute value - mjtNum val = (mjtNum)(it->slider.range[0]*(1-rx) + it->slider.range[1]*rx); + // compute value, clamp to range + mjtNum val = mju_clip(it->slider.range[0]*(1-rx) + it->slider.range[1]*rx, + it->slider.range[0], it->slider.range[1]); // set slider position if (it->type == mjITEM_SLIDERINT) { diff --git a/src/user/user_flexcomp.cc b/src/user/user_flexcomp.cc index 4035a176..081781ac 100644 --- a/src/user/user_flexcomp.cc +++ b/src/user/user_flexcomp.cc @@ -13,12 +13,14 @@ // limitations under the License. #include +#include #include #include #include #include #include #include +#include #include #include #include @@ -98,6 +100,156 @@ mjCFlexcomp::mjCFlexcomp(void) { } +// identify empty cells and pin nodes exclusively in empty cells +void mjCFlexcomp::MarkEmptyCells(mjCFlex* flex, const double* points, + int npnt, const double minmax[6], + int nx, int ny, int nz) { + int cx = flex->spec.cellcount[0]; + int cy = flex->spec.cellcount[1]; + int cz = flex->spec.cellcount[2]; + int ncells = cx * cy * cz; + int order = flex->spec.order; + + // determine which cells contain mesh elements (not just vertices) + // for each element, compute its AABB and mark all overlapping cells + std::vector has_element(ncells, false); + + double dx = minmax[3] - minmax[0]; + double dy = minmax[4] - minmax[1]; + double dz = minmax[5] - minmax[2]; + + // vertices per element: dim+1 (edges=2, triangles=3, tets=4) + int nvpe = flex->spec.dim + 1; + + if (nvpe > 0 && !element.empty()) { + int nelem = element.size() / nvpe; + for (int e = 0; e < nelem; e++) { + // compute element AABB + double elo[3] = {1e30, 1e30, 1e30}; + double ehi[3] = {-1e30, -1e30, -1e30}; + for (int v = 0; v < nvpe; v++) { + int vid = element[nvpe * e + v]; + for (int j = 0; j < 3; j++) { + elo[j] = std::min(elo[j], points[3 * vid + j]); + ehi[j] = std::max(ehi[j], points[3 * vid + j]); + } + } + + // map element AABB to cell range + auto cellIdx = [](double coord, double lo, double d, int nc) { + if (d <= 0) return 0; + int c = (int)((coord - lo) / d * nc); + return std::max(0, std::min(nc - 1, c)); + }; + + int ci0 = cellIdx(elo[0], minmax[0], dx, cx); + int ci1 = cellIdx(ehi[0], minmax[0], dx, cx); + int cj0 = cellIdx(elo[1], minmax[1], dy, cy); + int cj1 = cellIdx(ehi[1], minmax[1], dy, cy); + int ck0 = cellIdx(elo[2], minmax[2], dz, cz); + int ck1 = cellIdx(ehi[2], minmax[2], dz, cz); + + // mark all overlapping cells as containing elements + for (int ci = ci0; ci <= ci1; ci++) { + for (int cj = cj0; cj <= cj1; cj++) { + for (int ck = ck0; ck <= ck1; ck++) { + has_element[ci * cy * cz + cj * cz + ck] = true; + } + } + } + } + } + + // default: all cells non-empty (only exterior cells will be empty) + flex->cell_empty.assign(ncells, false); + + // for dim=2 (surface mesh): check watertightness and flood-fill + if (flex->spec.dim == 2 && nvpe == 3 && !element.empty()) { + // flood-fill from grid boundary to find exterior cells + // cells reachable from the boundary through non-element cells + // are outside the mesh volume; cells NOT reachable are interior + std::vector visited(ncells, false); + std::queue> bfs; + + // seed BFS from boundary cells that have no elements + for (int ci = 0; ci < cx; ci++) { + for (int cj = 0; cj < cy; cj++) { + for (int ck = 0; ck < cz; ck++) { + if (ci == 0 || ci == cx - 1 || + cj == 0 || cj == cy - 1 || + ck == 0 || ck == cz - 1) { + int idx = ci * cy * cz + cj * cz + ck; + if (!has_element[idx] && !visited[idx]) { + visited[idx] = true; + flex->cell_empty[idx] = true; + bfs.push({ci, cj, ck}); + } + } + } + } + } + + // BFS: spread through non-element cells + const int dirs[6][3] = { + {-1, 0, 0}, {1, 0, 0}, {0, -1, 0}, + {0, 1, 0}, {0, 0, -1}, {0, 0, 1}}; + while (!bfs.empty()) { + auto [ci, cj, ck] = bfs.front(); + bfs.pop(); + for (auto& d : dirs) { + int ni = ci + d[0], nj = cj + d[1], nk = ck + d[2]; + if (ni < 0 || ni >= cx || + nj < 0 || nj >= cy || + nk < 0 || nk >= cz) { + continue; + } + int nidx = ni * cy * cz + nj * cz + nk; + if (!visited[nidx] && !has_element[nidx]) { + visited[nidx] = true; + flex->cell_empty[nidx] = true; + bfs.push({ni, nj, nk}); + } + } + } + } else { + // dim!=2 (e.g., tet mesh): cells without element overlap are empty + for (int c = 0; c < ncells; c++) { + flex->cell_empty[c] = !has_element[c]; + } + } + + // pin nodes that belong exclusively to empty cells + for (int gi = 0; gi < nx; gi++) { + for (int gj = 0; gj < ny; gj++) { + for (int gk = 0; gk < nz; gk++) { + // find all cells that reference this node + bool all_empty = true; + int ci_min = std::max(0, gi == 0 ? 0 : (gi - 1) / order); + int ci_max = std::min(cx - 1, gi / order); + int cj_min = std::max(0, gj == 0 ? 0 : (gj - 1) / order); + int cj_max = std::min(cy - 1, gj / order); + int ck_min = std::max(0, gk == 0 ? 0 : (gk - 1) / order); + int ck_max = std::min(cz - 1, gk / order); + + for (int ci = ci_min; ci <= ci_max && all_empty; ci++) { + for (int cj = cj_min; cj <= cj_max && all_empty; cj++) { + for (int ck = ck_min; ck <= ck_max && all_empty; ck++) { + if (!flex->cell_empty[ci * cy * cz + cj * cz + ck]) { + all_empty = false; + } + } + } + } + + if (all_empty) { + int idx = gi * ny * nz + gj * nz + gk; + pinned[idx] = true; + } + } + } + } +} + // make flexcomp object bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz, const mjVFS* vfs) { @@ -548,6 +700,17 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz, const mjVFS* vf } } + // add two orthogonal sliders (x and y only) + else if (doftype == mjFCOMPDOF_2D) { + for (int j=0; j < 2; j++) { + mjsJoint* jnt = mjs_addJoint(pb, 0); + jnt->type = mjJNT_SLIDE; + mjuu_setvec(jnt->pos, 0, 0, 0); + mjuu_setvec(jnt->axis, 0, 0, 0); + jnt->axis[j] = 1; + } + } + // construct body name, add to vertbody char txt[100]; mju::sprintf_arr(txt, "%s_%d", name.c_str(), i); @@ -588,15 +751,30 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz, const mjVFS* vf int nz = flex->spec.cellcount[2] * flex->spec.order + 1; int nnode = nx * ny * nz; + // mark empty cells and pin nodes exclusively in empty cells + MarkEmptyCells(flex, point.data(), npnt, minmax, nx, ny, nz); + + // if MarkEmptyCells pinned any nodes, force centered=false + // so that pf->node (local positions) is saved to the model + if (centered) { + for (int i = 0; i < nnode; i++) { + if (pinned[i]) { + centered = false; + break; + } + } + } + std::vector node(3 * nnode, 0); int idx = 0; // Simpson's rule weights for quadratic mass distribution double massP2[3] = {1. / 6., 2. / 3., 1. / 6.}; - // compute per-node mass for trilinear: - // mass / nnode (uniform), or use Simpson for quadratic - double node_mass_uniform = mass / nnode; + + + // collect created bodies for mass normalization + std::vector node_bodies; for (int gi = 0; gi < nx; gi++) { for (int gj = 0; gj < ny; gj++) { @@ -629,7 +807,7 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz, const mjVFS* vf // mass distribution if (doftype == mjFCOMPDOF_TRILINEAR) { - pb->mass = node_mass_uniform; + pb->mass = 1.0; } else { // local index within the cell for mass computation int li = gi % flex->spec.order; @@ -639,14 +817,15 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz, const mjVFS* vf int ncells_i = (gi > 0 && gi < nx-1 && li == 0) ? 2 : 1; int ncells_j = (gj > 0 && gj < ny-1 && lj == 0) ? 2 : 1; int ncells_k = (gk > 0 && gk < nz-1 && lk == 0) ? 2 : 1; - // use Simpson weights scaled by cell count + // use Simpson weights double wi = massP2[li == 0 ? 0 : li]; double wj = massP2[lj == 0 ? 0 : lj]; double wk = massP2[lk == 0 ? 0 : lk]; - pb->mass = mass * wi * wj * wk * ncells_i * ncells_j * ncells_k - / (flex->spec.cellcount[0] * flex->spec.cellcount[1] * flex->spec.cellcount[2]); + pb->mass = wi * wj * wk * ncells_i * ncells_j * ncells_k; } + node_bodies.push_back(pb); + pb->inertia[0] = pb->mass*(2.0*inertiabox*inertiabox)/3.0; pb->inertia[1] = pb->mass*(2.0*inertiabox*inertiabox)/3.0; pb->inertia[2] = pb->mass*(2.0*inertiabox*inertiabox)/3.0; @@ -671,6 +850,21 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz, const mjVFS* vf } } + // normalize masses so total equals prescribed mass + double total_mass = 0; + for (mjsBody* pb : node_bodies) { + total_mass += pb->mass; + } + if (total_mass > 0) { + double scale = mass / total_mass; + for (mjsBody* pb : node_bodies) { + pb->mass *= scale; + pb->inertia[0] *= scale; + pb->inertia[1] *= scale; + pb->inertia[2] *= scale; + } + } + if (!centered) { mjs_setDouble(pf->node, node.data(), node.size()); } @@ -680,20 +874,41 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz, const mjVFS* vf mjs_setDouble(pf->vert, point.data(), point.size()); } - // create edge equality constraint + // create equality constraints if (equality) { - mjsEquality* pe = mjs_addEquality(&model->spec, &def.spec); - mjs_setDefault(pe->element, &model->Default()->spec); // equality 1=edge(mjEQ_FLEX), 2=vert(mjEQ_FLEXVERT), 3=strain(mjEQ_FLEXSTRAIN) - if (equality == 1) { - pe->type = mjEQ_FLEX; - } else if (equality == 2) { - pe->type = mjEQ_FLEXVERT; + if (equality == 1 || equality == 2) { + mjsEquality* pe = mjs_addEquality(&model->spec, &def.spec); + mjs_setDefault(pe->element, &model->Default()->spec); + pe->type = (equality == 1) ? mjEQ_FLEX : mjEQ_FLEXVERT; + pe->active = true; + mjs_setString(pe->name1, name.c_str()); } else if (equality == 3) { - pe->type = mjEQ_FLEXSTRAIN; + // create one strain constraint per cell, storing cell index in eq_data + flex->has_strain_eq = true; + int cell_cx = flex->spec.cellcount[0]; + int cell_cy = flex->spec.cellcount[1]; + int cell_cz = flex->spec.cellcount[2]; + for (int ci = 0; ci < cell_cx; ci++) { + for (int cj = 0; cj < cell_cy; cj++) { + for (int ck = 0; ck < cell_cz; ck++) { + // skip empty cells + if (!flex->cell_empty.empty() && + flex->cell_empty[ci * cell_cy * cell_cz + cj * cell_cz + ck]) { + continue; + } + mjsEquality* pe = mjs_addEquality(&model->spec, &def.spec); + mjs_setDefault(pe->element, &model->Default()->spec); + pe->type = mjEQ_FLEXSTRAIN; + pe->active = true; + mjs_setString(pe->name1, name.c_str()); + pe->data[0] = ci; + pe->data[1] = cj; + pe->data[2] = ck; + } + } + } } - pe->active = true; - mjs_setString(pe->name1, name.c_str()); } return true; diff --git a/src/user/user_flexcomp.h b/src/user/user_flexcomp.h index 8624be7b..782a7915 100644 --- a/src/user/user_flexcomp.h +++ b/src/user/user_flexcomp.h @@ -45,6 +45,7 @@ typedef enum _mjtDof { mjFCOMPDOF_RADIAL, mjFCOMPDOF_TRILINEAR, mjFCOMPDOF_QUADRATIC, + mjFCOMPDOF_2D, mjNFCOMPDOFS } mjtDof; @@ -116,6 +117,11 @@ class mjCFlexcomp { std::string plugin_name; std::string plugin_instance_name; mjsPlugin plugin; + + private: + // identify empty cells and pin nodes exclusively in empty cells + void MarkEmptyCells(mjCFlex* flex, const double* points, int npnt, + const double minmax[6], int nx, int ny, int nz); }; #endif // MUJOCO_SRC_USER_USER_FLEXCOMP_H_ diff --git a/src/user/user_mesh.cc b/src/user/user_mesh.cc index 244a48f2..3389a71e 100644 --- a/src/user/user_mesh.cc +++ b/src/user/user_mesh.cc @@ -300,9 +300,6 @@ void mjCMesh::NameSpace(const mjCModel* m) { name = mjuu_stripext(stripped); } mjCBase::NameSpace(m); - if (modelfiledir_.empty()) { - modelfiledir_ = FilePath(m->spec_modelfiledir_); - } if (!plugin_instance_name.empty()) { plugin_instance_name = m->prefix + plugin_instance_name + m->suffix; } @@ -712,17 +709,14 @@ void mjCMesh::TryCompile(const mjVFS* vfs) { mujoco::user::FilePath meshdir_; meshdir_ = FilePath(mjs_getString(compiler->meshdir)); - if (modelfiledir_.empty()) { - modelfiledir_ = FilePath(model->modelfiledir_); - } - // remove path from file if necessary if (model->strippath) { file_ = mjuu_strippath(file_); } + mjSpec* owning_spec = model->FindSpec(compiler); FilePath filename = meshdir_ + FilePath(file_); - resource_ = LoadResource(modelfiledir_.Str(), filename.Str(), vfs); + resource_ = LoadResource(owning_spec->modelfiledir->c_str(), filename.Str(), vfs); // try loading from cache if (cache != nullptr && LoadCachedMesh(cache, resource_)) { @@ -2957,9 +2951,6 @@ void mjCSkin::NameSpace(const mjCModel* m) { for (auto& name : spec_bodyname_) { name = m->prefix + name + m->suffix; } - if (modelfiledir_.empty()) { - modelfiledir_ = FilePath(m->spec_modelfiledir_); - } } @@ -3046,15 +3037,12 @@ void mjCSkin::Compile(const mjVFS* vfs) { throw mjCError(this, "Unknown skin file type: %s", file_.c_str()); } - // copy paths from model if not already defined - if (modelfiledir_.empty()) { - modelfiledir_ = FilePath(model->modelfiledir_); - } mujoco::user::FilePath meshdir_; meshdir_ = FilePath(mjs_getString(compiler->meshdir)); FilePath filename = meshdir_ + FilePath(file_); - mjResource* resource = LoadResource(modelfiledir_.Str(), filename.Str(), vfs); + mjSpec* owning_spec = model->FindSpec(compiler); + mjResource* resource = LoadResource(owning_spec->modelfiledir->c_str(), filename.Str(), vfs); try { LoadSKN(resource); @@ -3818,6 +3806,48 @@ void inline ComputeLinearStiffness(std::vector& K, } } + +// Eigendecompose cell stiffness matrix and store scaled eigenvectors. +// K_cell is n×n stored (negative convention: K_stored = -K_physical). +// Output layout in `out`: +// [0]: neig (as double) +// [1 .. neig*n]: sqrt(λ_phys_i) * v_i, row-major +// Returns number of retained eigenmodes. +static int EigendecomposeStiffness(const double* K_cell_data, + double* out, int ndof) { + // copy K_cell for in-place decomposition + std::vector mat(K_cell_data, K_cell_data + ndof * ndof); + std::vector eigval(ndof); + std::vector eigvec(ndof * ndof); + + mjuu_eigendecompose(mat.data(), eigval.data(), eigvec.data(), ndof); + + // K_stored = -K_physical, so physical eigenvalue = -eigval[i] + // retain modes where physical eigenvalue > threshold + double max_eigval = 0; + for (int i = 0; i < ndof; i++) { + max_eigval = std::max(max_eigval, std::abs(eigval[i])); + } + double threshold = max_eigval * 1e-8; + + int neig = 0; + for (int i = 0; i < ndof; i++) { + double lambda_phys = -eigval[i]; // negate to get physical eigenvalue + if (lambda_phys > threshold) { + // store sqrt(λ) * eigenvector (column i of eigvec matrix) + double scale = std::sqrt(lambda_phys); + for (int j = 0; j < ndof; j++) { + out[1 + neig * ndof + j] = scale * eigvec[j * ndof + i]; + } + neig++; + } + } + + out[0] = static_cast(neig); + return neig; +} + + //------------------ class mjCFlex implementation -------------------------------------------------- // constructor @@ -4070,6 +4100,19 @@ void mjCFlex::Compile(const mjVFS* vfs) { } nelem = (int)elem_.size()/(dim+1); + // elastic2d checks + if (elastic2d) { + if (thickness <= 0) { + throw mjCError(this, "2d elasticity requires positive thickness"); + } + if (interpolated) { + throw mjCError(this, "interpolated flex does not yet support 2d elasticity"); + } + if (dim != 2 && !interpolated) { + throw mjCError(this, "2d elasticity requires 2d flex"); + } + } + // set nvert, rigid, centered; check size if (vert_.empty()) { centered = true; @@ -4221,6 +4264,9 @@ void mjCFlex::Compile(const mjVFS* vfs) { } } + // compute unrotated node positions for stiffness computation + std::vector nodexpos_local = ComputeUnrotatedNodePositions(nodexpos); + // reorder tetrahedra so right-handed face orientation is outside // faces are (0,1,2); (0,2,3); (0,3,1); (1,3,2) if (dim == 3) { @@ -4309,9 +4355,6 @@ void mjCFlex::Compile(const mjVFS* vfs) { // bending stiffness (2D only) if (dim == 2 && (elastic2d == 1 || elastic2d == 3)) { - if (thickness < 0) { - throw mjCError(this, "thickness must be positive for bending stiffness"); - } bending.assign(nedge*17, 0); for (unsigned int e = 0; e < nedge; e++) { @@ -4344,7 +4387,11 @@ void mjCFlex::Compile(const mjVFS* vfs) { stiffness_cached = LoadCachedStiffness(); } - if (!stiffness_cached && young > 0 && interpolated) { + if (!stiffness_cached && interpolated && (young > 0 || has_strain_eq)) { + // use young=1 for strain constraints (eigenvectors are geometry-only) + double K_young = has_strain_eq ? 1e1 : young; + double K_poisson = has_strain_eq ? 0.3 : poisson; + int npc = pow(spec.order + 1, 3); // nodes per cell int ndof_cell = 3 * npc; int cx = spec.cellcount[0], cy = spec.cellcount[1], cz = spec.cellcount[2]; @@ -4361,6 +4408,11 @@ void mjCFlex::Compile(const mjVFS* vfs) { for (int ck = 0; ck < cz; ck++) { int cell_idx = ci * cy * cz + cj * cz + ck; + // skip stiffness computation for empty cells (no mesh content) + if (!cell_empty.empty() && cell_empty[cell_idx]) { + continue; + } + // gather cell's local node positions std::vector cell_pos(3 * npc); int local = 0; @@ -4371,7 +4423,7 @@ void mjCFlex::Compile(const mjVFS* vfs) { int gj = cj * spec.order + lj; int gk = ck * spec.order + lk; int global = gi * ny_global * nz_global + gj * nz_global + gk; - mjuu_copyvec(cell_pos.data() + 3*local, nodexpos.data() + 3*global, 3); + mjuu_copyvec(cell_pos.data() + 3*local, nodexpos_local.data() + 3*global, 3); local++; } } @@ -4379,11 +4431,17 @@ void mjCFlex::Compile(const mjVFS* vfs) { // compute per-cell stiffness std::vector K_cell(ndof_cell * ndof_cell, 0); - ComputeLinearStiffness(K_cell, cell_pos.data(), young, poisson, spec.order); + ComputeLinearStiffness(K_cell, cell_pos.data(), K_young, K_poisson, spec.order); + double* out = stiffness.data() + cell_idx * ndof_cell * ndof_cell; - // copy into global stiffness array - mjuu_copyvec(stiffness.data() + cell_idx * ndof_cell * ndof_cell, - K_cell.data(), ndof_cell * ndof_cell); + if (has_strain_eq) { + // eigendecompose: store [neig, sqrt(λ)*v_1, sqrt(λ)*v_2, ...] + std::fill(out, out + ndof_cell * ndof_cell, 0.0); + EigendecomposeStiffness(K_cell.data(), out, ndof_cell); + } else { + // store raw K for passive forces + std::copy(K_cell.begin(), K_cell.end(), out); + } } } } @@ -4408,14 +4466,88 @@ void mjCFlex::Compile(const mjVFS* vfs) { } } - // store node cartesian positions + // store node positions in unrotated (body-local) frame + // this ensures the runtime displacement refpos - R^{-1}*x is zero at rest node0_.assign(3*nnode, 0); for (int i=0; i < nnode; i++) { - mjuu_copyvec(node0_.data()+3*i, nodexpos.data()+3*i, 3); + mjuu_copyvec(node0_.data()+3*i, nodexpos_local.data()+3*i, 3); } } +// compute unrotated node positions for stiffness computation and node0_ +// +// the runtime corotational code extracts rotation R from the deformation +// gradient and computes displacement as R^{-1}*x - refpos; at rest R = R0 +// (the total grid rotation), so refpos must equal R0^{-1}*nodexpos to get +// zero displacement at rest; additionally, the stiffness eigenvectors must +// be computed from axis-aligned positions to preserve the diagonal Jacobian +// assumption in ComputeLinearStiffness. +std::vector mjCFlex::ComputeUnrotatedNodePositions( + const std::vector& nodexpos) const { + std::vector nodexpos_local(3*nnode); + if (interpolated && nnode > 0) { + int ny_global = spec.cellcount[1] * spec.order + 1; + int nz_global = spec.cellcount[2] * spec.order + 1; + + // find first non-empty cell + int cx = spec.cellcount[0], cy = spec.cellcount[1], cz = spec.cellcount[2]; + int ref_ci = 0, ref_cj = 0, ref_ck = 0; + bool found = false; + for (int ci = 0; ci < cx && !found; ci++) { + for (int cj = 0; cj < cy && !found; cj++) { + for (int ck = 0; ck < cz && !found; ck++) { + int cell_idx = ci * cy * cz + cj * cz + ck; + if (cell_empty.empty() || !cell_empty[cell_idx]) { + ref_ci = ci; ref_cj = cj; ref_ck = ck; + found = true; + } + } + } + } + + // corner indices of the reference cell (order=1 corners at local 0,0,0 + // and at offsets along each parametric axis) + int g000 = (ref_ci * spec.order) * ny_global * nz_global + + (ref_cj * spec.order) * nz_global + + (ref_ck * spec.order); + int g100 = ((ref_ci * spec.order) + spec.order) * ny_global * nz_global + + (ref_cj * spec.order) * nz_global + + (ref_ck * spec.order); + int g010 = (ref_ci * spec.order) * ny_global * nz_global + + ((ref_cj * spec.order) + spec.order) * nz_global + + (ref_ck * spec.order); + int g001 = (ref_ci * spec.order) * ny_global * nz_global + + (ref_cj * spec.order) * nz_global + + ((ref_ck * spec.order) + spec.order); + + // edge vectors (columns of the deformation gradient F = R * S) + // we store them as rows in R0 to use mjuu_mulvecmat for applying R0^{-1} + double R0[9]; + for (int d = 0; d < 3; d++) { + R0[0+d] = nodexpos[3*g100 + d] - nodexpos[3*g000 + d]; + R0[3+d] = nodexpos[3*g010 + d] - nodexpos[3*g000 + d]; + R0[6+d] = nodexpos[3*g001 + d] - nodexpos[3*g000 + d]; + } + + // normalize to get rotation matrix columns (valid for regular grids) + double li = mjuu_normvec(R0+0, 3); + double lj = mjuu_normvec(R0+3, 3); + double lk = mjuu_normvec(R0+6, 3); + (void)li; (void)lj; (void)lk; + + // apply inverse rotation to each nodexpos to get local-frame positions + for (int i = 0; i < nnode; i++) { + const double* p = nodexpos.data() + 3*i; + double* q = nodexpos_local.data() + 3*i; + mjuu_mulvecmat(q, p, R0); + } + } else { + nodexpos_local = nodexpos; + } + return nodexpos_local; +} + // create flex BVH void mjCFlex::CreateBVH() { diff --git a/src/user/user_model.cc b/src/user/user_model.cc index 85245020..19672803 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -3657,6 +3657,8 @@ void mjCModel::CopyObjects(mjModel* m) { int b1 = pfl->vertbodyid[pfl->edge[k].first]; int b2 = pfl->vertbodyid[pfl->edge[k].second]; m->flexedge_rigid[edge_adr+k] = (bodies_[b1]->weldid == bodies_[b2]->weldid); + } else { + m->flexedge_rigid[edge_adr+k] = 0; } } diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index f83bd387..75a7e679 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -4668,9 +4668,6 @@ void mjCHField::NameSpace(const mjCModel* m) { name = mjuu_stripext(stripped); } mjCBase::NameSpace(m); - if (modelfiledir_.empty()) { - modelfiledir_ = FilePath(m->spec_modelfiledir_); - } } @@ -4798,15 +4795,12 @@ void mjCHField::Compile(const mjVFS* vfs) { throw mjCError(this, "unsupported content type: '%s'", asset_type.c_str()); } - // copy paths from model if not already defined - if (modelfiledir_.empty()) { - modelfiledir_ = FilePath(model->modelfiledir_); - } mujoco::user::FilePath meshdir_; meshdir_ = FilePath(mjs_getString(compiler->meshdir)); FilePath filename = meshdir_ + FilePath(file_); - mjResource* resource = LoadResource(modelfiledir_.Str(), filename.Str(), vfs); + mjSpec* owning_spec = model->FindSpec(compiler); + mjResource* resource = LoadResource(owning_spec->modelfiledir->c_str(), filename.Str(), vfs); struct CachedHField { int nrow, ncol; @@ -4965,9 +4959,6 @@ void mjCTexture::NameSpace(const mjCModel* m) { name = mjuu_stripext(stripped); } mjCBase::NameSpace(m); - if (modelfiledir_.empty()) { - modelfiledir_ = FilePath(m->spec_modelfiledir_); - } } @@ -5388,7 +5379,8 @@ void mjCTexture::LoadFlip(std::string filename, const mjVFS* vfs, } // try loading from cache - mjResource* resource = LoadResource(modelfiledir_.Str(), filename, vfs); + mjSpec* owning_spec = model->FindSpec(compiler); + mjResource* resource = LoadResource(owning_spec->modelfiledir->c_str(), filename, vfs); if (cache && cache->PopulateData(GetCacheId(resource, asset_type), resource, callback)) { mju_closeResource(resource); return; @@ -5640,10 +5632,6 @@ void mjCTexture::LoadCubeSeparate(const mjVFS* vfs) { void mjCTexture::Compile(const mjVFS* vfs) { CopyFromSpec(); - // copy paths from model if not already defined - if (modelfiledir_.empty()) { - modelfiledir_ = FilePath(model->modelfiledir_); - } mujoco::user::FilePath texturedir_; texturedir_ = FilePath(mjs_getString(compiler->texturedir)); diff --git a/src/user/user_objects.h b/src/user/user_objects.h index 8ca60124..bb3bbed5 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -983,6 +983,8 @@ class mjCFlex_ : public mjCBase { std::vector edgeidx_; // element edge ids std::vector stiffness; // elasticity stiffness matrix std::vector bending; // bending stiffness matrix + bool has_strain_eq = false; // true if strain constraints reference this flex + std::vector cell_empty; // true if cell contains no mesh geometry // variable-size data std::vector vertbody_; // vertex body names @@ -1052,7 +1054,8 @@ class mjCFlex: public mjCFlex_, private mjsFlex { std::vector vert0_; // vertex positions in [0, 1]^d in the bounding box std::vector node0_; // node Cartesian positions - + // compute unrotated node positions for stiffness computation + std::vector ComputeUnrotatedNodePositions(const std::vector& nodexpos) const; // stiffness caching std::string ComputeStiffnessCacheKey() const; @@ -1120,9 +1123,6 @@ class mjCMesh_ : public mjCBase { // octree mjCOctree octree_; // octree of the mesh - - // paths stored during model attachment - mujoco::user::FilePath modelfiledir_; }; class mjCMesh: public mjCMesh_, private mjsMesh { @@ -1334,9 +1334,6 @@ class mjCSkin_ : public mjCBase { int matid; // material id std::vector bodyid; // body ids - - // paths stored during model attachment - mujoco::user::FilePath modelfiledir_; }; class mjCSkin: public mjCSkin_, private mjsSkin { @@ -1389,9 +1386,6 @@ class mjCHField_ : public mjCBase { std::string spec_file_; std::string spec_content_type_; std::vector spec_userdata_; - - // paths stored during model attachment - mujoco::user::FilePath modelfiledir_; }; class mjCHField : public mjCHField_, private mjsHField { @@ -1440,9 +1434,6 @@ class mjCTexture_ : public mjCBase { std::string spec_file_; std::string spec_content_type_; std::vector spec_cubefiles_; - - // paths stored during model attachment - mujoco::user::FilePath modelfiledir_; }; class mjCTexture : public mjCTexture_, private mjsTexture { diff --git a/src/user/user_util.cc b/src/user/user_util.cc index d9ad6cb6..6f5bc1a4 100644 --- a/src/user/user_util.cc +++ b/src/user/user_util.cc @@ -754,6 +754,81 @@ int mjuu_eig3(double eigval[3], double eigvec[9], double quat[4], const double m return iter; } + +// Jacobi eigenvalue decomposition of symmetric n×n matrix. +// On output, eigenvalues are in eigval and eigenvectors are columns of eigvec. +// Both arrays must be pre-allocated: eigval[n], eigvec[n*n]. +// The input matrix mat is destroyed. +int mjuu_eigendecompose(double* mat, double* eigval, double* eigvec, int n) { + // initialize eigvec to identity + std::fill(eigvec, eigvec + n*n, 0.0); + for (int i = 0; i < n; i++) { + eigvec[i*n + i] = 1.0; + } + + const int max_sweeps = 200; + const double tol = 1e-12; + + int sweep; + for (sweep = 0; sweep < max_sweeps; sweep++) { + // check convergence: sum of squared off-diagonal elements + double off_diag = 0; + for (int i = 0; i < n; i++) { + for (int j = i+1; j < n; j++) { + off_diag += mat[i*n + j] * mat[i*n + j]; + } + } + if (off_diag < tol * tol) break; + + // sweep over all off-diagonal pairs + for (int p = 0; p < n; p++) { + for (int q = p+1; q < n; q++) { + double apq = mat[p*n + q]; + if (std::abs(apq) < tol * 1e-3) continue; + + // compute rotation angle + double app = mat[p*n + p]; + double aqq = mat[q*n + q]; + double tau = (aqq - app) / (2.0 * apq); + double t = (tau >= 0 ? 1.0 : -1.0) / + (std::abs(tau) + std::sqrt(1.0 + tau*tau)); + double c = 1.0 / std::sqrt(1.0 + t*t); + double s = t * c; + + // update matrix (Jacobi rotation) + mat[p*n + p] -= t * apq; + mat[q*n + q] += t * apq; + mat[p*n + q] = 0; + mat[q*n + p] = 0; + + for (int r = 0; r < n; r++) { + if (r == p || r == q) continue; + double mrp = mat[r*n + p]; + double mrq = mat[r*n + q]; + mat[r*n + p] = mat[p*n + r] = c*mrp - s*mrq; + mat[r*n + q] = mat[q*n + r] = s*mrp + c*mrq; + } + + // accumulate eigenvectors + for (int r = 0; r < n; r++) { + double vrp = eigvec[r*n + p]; + double vrq = eigvec[r*n + q]; + eigvec[r*n + p] = c*vrp - s*vrq; + eigvec[r*n + q] = s*vrp + c*vrq; + } + } + } + } + + // extract eigenvalues from diagonal + for (int i = 0; i < n; i++) { + eigval[i] = mat[i*n + i]; + } + + return sweep; +} + + // transform vector by pose void mjuu_trnVecPose(double res[3], const double pos[3], const double quat[4], const double vec[3]) { @@ -1189,10 +1264,10 @@ template std::string VectorToString(const std::vector& v) { return s; } -template std::string VectorToString(const std::vector& v); -template std::string VectorToString(const std::vector& v); -template std::string VectorToString(const std::vector& v); -template std::string VectorToString(const std::vector& v); +template MJAPI std::string VectorToString(const std::vector& v); +template MJAPI std::string VectorToString(const std::vector& v); +template MJAPI std::string VectorToString(const std::vector& v); +template MJAPI std::string VectorToString(const std::vector& v); namespace { @@ -1258,7 +1333,11 @@ template std::vector StringToVector(char* cs) { return v; } -template<> std::vector StringToVector(const std::string& s) { +template<> MJAPI std::vector StringToVector(char* cs) { + return StringToVector(std::string(cs)); +} + +template<> MJAPI std::vector StringToVector(const std::string& s) { std::vector v; std::stringstream ss(s); std::string word; @@ -1268,17 +1347,18 @@ template<> std::vector StringToVector(const std::string& s) { return v; } -template std::vector StringToVector(char* cs); -template std::vector StringToVector(char* cs); -template std::vector StringToVector(char* cs); +template MJAPI std::vector StringToVector(char* cs); +template MJAPI std::vector StringToVector(char* cs); +template MJAPI std::vector StringToVector(char* cs); +template MJAPI std::vector StringToVector(char* cs); template std::vector StringToVector(const std::string& s) { return StringToVector(const_cast(s.c_str())); } -template std::vector StringToVector(const std::string& s); -template std::vector StringToVector(const std::string& s); -template std::vector StringToVector(const std::string& s); -template std::vector StringToVector(const std::string& s); +template MJAPI std::vector StringToVector(const std::string& s); +template MJAPI std::vector StringToVector(const std::string& s); +template MJAPI std::vector StringToVector(const std::string& s); +template MJAPI std::vector StringToVector(const std::string& s); } // namespace mujoco::user diff --git a/src/user/user_util.h b/src/user/user_util.h index 2eeb3a0f..65a21a7b 100644 --- a/src/user/user_util.h +++ b/src/user/user_util.h @@ -26,6 +26,8 @@ #include #include +#include + const double mjEPS = 1E-14; // minimum value in various calculations const double mjMINMASS = 1E-6; // minimum mass allowed @@ -157,6 +159,12 @@ double mjuu_updateFrame(double quat[4], double normal[3], const double edge[3], // eigenvalue decomposition of symmetric 3x3 matrix int mjuu_eig3(double eigval[3], double eigvec[9], double quat[4], const double mat[9]); +// Jacobi eigenvalue decomposition of symmetric n×n matrix +// eigval[n]: output eigenvalues, eigvec[n*n]: output eigenvectors (columns) +// mat[n*n]: input matrix (destroyed on output) +// returns number of sweeps used +MJAPI int mjuu_eigendecompose(double* mat, double* eigval, double* eigvec, int n); + // transform vector by pose void mjuu_trnVecPose(double res[3], const double pos[3], const double quat[4], const double vec[3]); @@ -166,7 +174,7 @@ const char* mjuu_fullInertia(double quat[4], double inertia[3], const double ful namespace mujoco::user { // utility class for handling file paths -class FilePath { +class MJAPI FilePath { public: FilePath() = default; explicit FilePath(const std::string& str) : path_(PathReduce(str)) {} @@ -251,11 +259,13 @@ struct Cleanup { std::vector FileToMemory(const char* filename); // convert vector to string separating elements by whitespace -template std::string VectorToString(const std::vector& v); +template MJAPI std::string VectorToString(const std::vector& v); // convert string to vector -template std::vector StringToVector(char *cs); -template std::vector StringToVector(const std::string& s); +template MJAPI std::vector StringToVector(char *cs); +template MJAPI std::vector StringToVector(const std::string& s); +template<> MJAPI std::vector StringToVector(char* cs); +template<> MJAPI std::vector StringToVector(const std::string& s); } // namespace mujoco::user diff --git a/src/user/user_vfs.cc b/src/user/user_vfs.cc index 11d35c71..60c5c945 100644 --- a/src/user/user_vfs.cc +++ b/src/user/user_vfs.cc @@ -236,6 +236,24 @@ VFS::Status VFS::Unmount(const FilePath& path) { return kInvalidResourceProvider; } +bool VFS::ContainsBuffer(const char* name) { + if (name == nullptr) { + return false; + } + std::lock_guard lock(mutex_); + return mounts_.contains(name); +} + +bool VFS::ContainsFile(const char* directory, const char* filename) { + if (filename == nullptr) { + return false; + } + mujoco::user::FilePath path(directory ? directory : "", filename); + std::string key = path.StripPath().Lower().Str(); + std::lock_guard lock(mutex_); + return mounts_.contains(key); +} + int VFS::Read(mjResource* resource, const void** buffer) { if (resource && resource->provider && resource->provider->read) { return resource->provider->read(resource, buffer); @@ -498,3 +516,22 @@ int mj_deleteFileVFS(mjVFS* vfs, const char* filename) { } return mujoco::user::VFS::kSuccess; } + +int mj_containsBufferVFS(mjVFS* vfs, const char* name) { + mujoco::user::VFS* impl = mujoco::user::VFS::Upcast(vfs); + if (impl == nullptr) { + mju_error("mjVFS is null."); + return -1; + } + return impl->ContainsBuffer(name); +} + +int mj_containsFileVFS(mjVFS* vfs, const char* directory, const char* filename) { + mujoco::user::VFS* impl = mujoco::user::VFS::Upcast(vfs); + if (impl == nullptr) { + mju_error("mjVFS is null."); + return -1; + } + return impl->ContainsFile(directory, filename); +} + diff --git a/src/user/user_vfs.h b/src/user/user_vfs.h index cfa2694c..6bef85c2 100644 --- a/src/user/user_vfs.h +++ b/src/user/user_vfs.h @@ -90,6 +90,12 @@ class VFS { // Unmounts the ResourceProvider from the given path. Status Unmount(const FilePath& path); + // Returns true if the VFS contains a buffer with the given name. + bool ContainsBuffer(const char* name); + + // Returns true if the VFS contains a file with the given name. + bool ContainsFile(const char* directory, const char* filename); + // Sets a destructor to be called when the VFS has no more open resources. // Assumes that `destructor` will delete `this`. // diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index d38bde20..af291787 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -371,7 +371,7 @@ std::vector MJCF[nMJCF] = { "active", "solref", "solimp"}, {"flexvert", "*", "name", "class", "flex", "active", "solref", "solimp"}, - {"flexstrain", "*", "name", "class", "flex", + {"flexstrain", "*", "name", "class", "flex", "cell", "active", "solref", "solimp"}, {">"}, @@ -932,7 +932,8 @@ const mjMap fdof_map[mjNFCOMPDOFS] = { {"full", mjFCOMPDOF_FULL}, {"radial", mjFCOMPDOF_RADIAL}, {"trilinear", mjFCOMPDOF_TRILINEAR}, - {"quadratic", mjFCOMPDOF_QUADRATIC} + {"quadratic", mjFCOMPDOF_QUADRATIC}, + {"2d", mjFCOMPDOF_2D} }; @@ -1283,6 +1284,7 @@ void mjXReader::Option(XMLElement* section, mjOption* opt) { READDSBL("autoreset", mjDSBL_AUTORESET) READDSBL("nativeccd", mjDSBL_NATIVECCD) READDSBL("island", mjDSBL_ISLAND) + READDSBL("multiccd", mjDSBL_MULTICCD) #undef READDSBL #define READENBL(NAME, MASK) \ @@ -1294,7 +1296,6 @@ void mjXReader::Option(XMLElement* section, mjOption* opt) { READENBL("energy", mjENBL_ENERGY) READENBL("fwdinv", mjENBL_FWDINV) READENBL("invdiscrete", mjENBL_INVDISCRETE) - READENBL("multiccd", mjENBL_MULTICCD) READENBL("sleep", mjENBL_SLEEP) #undef READENBL } @@ -2245,8 +2246,12 @@ void mjXReader::OneEquality(XMLElement* elem, mjsEquality* equality) { case mjEQ_FLEX: case mjEQ_FLEXVERT: + ReadAttrTxt(elem, "flex", name1, true); + break; + case mjEQ_FLEXSTRAIN: ReadAttrTxt(elem, "flex", name1, true); + ReadAttr(elem, "cell", 3, equality->data, text); break; case mjEQ_DISTANCE: diff --git a/src/xml/xml_native_writer.cc b/src/xml/xml_native_writer.cc index 007a76a3..0bc4c07a 100644 --- a/src/xml/xml_native_writer.cc +++ b/src/xml/xml_native_writer.cc @@ -728,8 +728,12 @@ void mjXWriter::OneEquality(XMLElement* elem, const mjCEquality* equality, mjCDe case mjEQ_FLEX: case mjEQ_FLEXVERT: + WriteAttrTxt(elem, "flex", mjs_getString(equality->name1)); + break; + case mjEQ_FLEXSTRAIN: WriteAttrTxt(elem, "flex", mjs_getString(equality->name1)); + WriteAttr(elem, "cell", 3, equality->data); break; default: @@ -1089,7 +1093,7 @@ void mjXWriter::Option(XMLElement* root) { XMLElement* sub = InsertEnd(section, "flag"); #define WRITEDSBL(NAME, MASK) \ - if( model->option.disableflags & MASK ) \ + if (model->option.disableflags & MASK) \ WriteAttrKey(sub, NAME, enable_map, 2, 0); WRITEDSBL("constraint", mjDSBL_CONSTRAINT) WRITEDSBL("equality", mjDSBL_EQUALITY) @@ -1110,16 +1114,16 @@ void mjXWriter::Option(XMLElement* root) { WRITEDSBL("autoreset", mjDSBL_AUTORESET) WRITEDSBL("nativeccd", mjDSBL_NATIVECCD) WRITEDSBL("island", mjDSBL_ISLAND) + WRITEDSBL("multiccd", mjDSBL_MULTICCD) #undef WRITEDSBL #define WRITEENBL(NAME, MASK) \ - if( model->option.enableflags & MASK ) \ + if (model->option.enableflags & MASK) \ WriteAttrKey(sub, NAME, enable_map, 2, 1); WRITEENBL("override", mjENBL_OVERRIDE) WRITEENBL("energy", mjENBL_ENERGY) WRITEENBL("fwdinv", mjENBL_FWDINV) WRITEENBL("invdiscrete", mjENBL_INVDISCRETE) - WRITEENBL("multiccd", mjENBL_MULTICCD) WRITEENBL("sleep", mjENBL_SLEEP) #undef WRITEENBL } diff --git a/test/benchmark/chol_benchmark_test.cc b/test/benchmark/chol_benchmark_test.cc index 004e0c2a..ba8d94bf 100644 --- a/test/benchmark/chol_benchmark_test.cc +++ b/test/benchmark/chol_benchmark_test.cc @@ -22,7 +22,6 @@ #include #include #include -#include "src/engine/engine_memory.h" #include "src/engine/engine_support.h" #include "src/engine/engine_util_solve.h" #include "src/engine/engine_util_sparse.h" @@ -352,10 +351,6 @@ constexpr int kNumUpdateVectors = 25; int ABSL_ATTRIBUTE_NOINLINE mju_cholUpdateSparse_old( mjtNum* mat, mjtNum* x, int n, int flg_plus, const int* rownnz, const int* rowadr, const int* colind, int x_nnz, int* x_ind, mjData* d) { - mj_markStack(d); - int* buf_ind = mjSTACKALLOC(d, n, int); - mjtNum* sparse_buf = mjSTACKALLOC(d, n, mjtNum); - int rank = n, i = x_nnz - 1; while (i >= 0) { int nnz = rownnz[x_ind[i]], adr = rowadr[x_ind[i]]; @@ -372,10 +367,9 @@ int ABSL_ATTRIBUTE_NOINLINE mju_cholUpdateSparse_old( mju_combineSparseInc(mat + adr, x, n, 1 / c, (flg_plus ? s / c : -s / c), nnz - 1, i, colind + adr, x_ind); int new_x_nnz = mju_combineSparse(x, mat + adr, c, -s, i, nnz - 1, x_ind, - colind + adr, sparse_buf, buf_ind); + colind + adr); i = i - 1 + (new_x_nnz - i); } - mj_freeStack(d); return rank; } diff --git a/test/benchmark/engine_util_sparse_benchmark_test.cc b/test/benchmark/engine_util_sparse_benchmark_test.cc index a82be98e..81609beb 100644 --- a/test/benchmark/engine_util_sparse_benchmark_test.cc +++ b/test/benchmark/engine_util_sparse_benchmark_test.cc @@ -14,7 +14,6 @@ // A benchmark for comparing different implementations of mj_solveLD. -#include #include #include @@ -31,14 +30,179 @@ namespace { using CombineFuncPtr = decltype(&mju_combineSparse); using TransposeFuncPtr = decltype(&mju_transposeSparse); -using SqrMatTDFuncPtr = decltype(&mju_sqrMatTDSparse); -// number of steps to roll out before benchmarking -static const int kNumWarmupSteps = 500; +// ================================ Cached Data ================================ -// ----------------------------- old functions -------------------------------- +// ---- MatVecSparse data ---- +struct MatVecData { + int nv; + int nefc; + int nJ; + std::vector efc_J; + std::vector efc_J_rownnz, efc_J_rowadr, efc_J_colind, efc_J_rowsuper; + std::vector vec; +}; +MatVecData& GetMatVecData() { + static MatVecData data = [] { + MatVecData d; + mjModel* m = LoadModelFromPath("flex/flag.xml"); + mjData* dat = mj_makeData(m); + for (int i = 0; i < 500; i++) { + mj_step(m, dat); + } + + d.nv = m->nv; + d.nefc = dat->nefc; + d.nJ = dat->nJ; + d.efc_J.assign(dat->efc_J, dat->efc_J + d.nJ); + d.efc_J_rownnz.assign(dat->efc_J_rownnz, dat->efc_J_rownnz + d.nefc); + d.efc_J_rowadr.assign(dat->efc_J_rowadr, dat->efc_J_rowadr + d.nefc); + d.efc_J_colind.assign(dat->efc_J_colind, dat->efc_J_colind + d.nJ); + d.efc_J_rowsuper.assign(dat->efc_J_rowsuper, dat->efc_J_rowsuper + d.nefc); + + // compute direction: vec = -M^{-1} * (Ma - qfrc_smooth - qfrc_constraint) + mj_markStack(dat); + mjtNum* Ma = mj_stackAllocNum(dat, m->nv); + mjtNum* grad = mj_stackAllocNum(dat, m->nv); + mjtNum* Mgrad = mj_stackAllocNum(dat, m->nv); + mj_mulM(m, dat, Ma, dat->qacc); + for (int i = 0; i < m->nv; i++) { + grad[i] = Ma[i] - dat->qfrc_smooth[i] - dat->qfrc_constraint[i]; + } + mj_solveM(m, dat, Mgrad, grad, 1); + d.vec.resize(m->nv); + mju_scl(d.vec.data(), Mgrad, -1, m->nv); + mj_freeStack(dat); + + mj_deleteData(dat); + mj_deleteModel(m); + return d; + }(); + return data; +} + +// ---- CombineSparse data ---- +struct CombineData { + int nv; + std::vector H; + std::vector rownnz, rowadr, colind; +}; + +CombineData& GetCombineData() { + static CombineData data = [] { + CombineData cd; + mjModel* m = LoadModelFromPath("humanoid/humanoid.xml"); + m->opt.jacobian = mjJAC_SPARSE; + mjData* d = mj_makeData(m); + + for (int i = 0; i < 500; i++) { + mj_step(m, d); + } + + cd.nv = m->nv; + mj_markStack(d); + mjtNum* H = mj_stackAllocNum(d, m->nv*m->nv); + int* rownnz = mj_stackAllocInt(d, m->nv); + int* rowadr = mj_stackAllocInt(d, m->nv); + int* colind = mj_stackAllocInt(d, m->nv*m->nv); + int* diagind = mj_stackAllocInt(d, m->nv); + + mjtNum* D = mj_stackAllocNum(d, d->nefc); + for (int i = 0; i < d->nefc; i++) { + if (d->efc_state[i] == mjCNSTRSTATE_QUADRATIC) { + D[i] = d->efc_D[i]; + } else { + D[i] = 0; + } + } + + int* JT_rownnz = mj_stackAllocInt(d, m->nv); + int* JT_rowadr = mj_stackAllocInt(d, m->nv); + int* JT_rowsuper = mj_stackAllocInt(d, m->nv); + int* JT_colind = mj_stackAllocInt(d, d->nJ); + mjtNum* JT = mj_stackAllocNum(d, d->nJ); + mju_transposeSparse(JT, d->efc_J, d->nefc, m->nv, + JT_rownnz, JT_rowadr, JT_colind, JT_rowsuper, + d->efc_J_rownnz, d->efc_J_rowadr, d->efc_J_colind); + + // compute H = J'*D*J, uncompressed layout + mju_sqrMatTDUncompressedInit(rowadr, m->nv); + mju_sqrMatTDSparse(H, d->efc_J, JT, D, d->nefc, m->nv, + rownnz, rowadr, colind, + d->efc_J_rownnz, d->efc_J_rowadr, + d->efc_J_colind, d->efc_J_rowsuper, + JT_rownnz, JT_rowadr, + JT_colind, JT_rowsuper, d, + diagind); + + // compute H = M + J'*D*J + mj_addM(m, d, H, rownnz, rowadr, colind); + + // copy to persistent storage + int nH = rowadr[m->nv-1] + m->nv; // uncompressed: rowadr[r] = r*nv + cd.H.assign(H, H + nH); + cd.rownnz.assign(rownnz, rownnz + m->nv); + cd.rowadr.assign(rowadr, rowadr + m->nv); + cd.colind.assign(colind, colind + nH); + + mj_freeStack(d); + mj_deleteData(d); + mj_deleteModel(m); + return cd; + }(); + return data; +} + +// ---- TransposeSparse data ---- +struct TransposeData { + int nv; + int nefc; + int nJ; + std::vector efc_J; + std::vector efc_J_rownnz, efc_J_rowadr, efc_J_colind; +}; + +enum class Size { H2_100, H100 }; + +template +const char* ModelPath() { + if constexpr (S == Size::H2_100) { + return "../test/benchmark/testdata/2humanoid100_chol.xml"; + } else { + return "../test/benchmark/testdata/100_humanoids_chol.xml"; + } +} + +template +TransposeData& GetTransposeData() { + static TransposeData data = [] { + TransposeData td; + mjModel* m = LoadModelFromPath(ModelPath()); + m->opt.jacobian = mjJAC_SPARSE; + mjData* d = mj_makeData(m); + + while (d->time < 2) { + mj_step(m, d); + } + + td.nv = m->nv; + td.nefc = d->nefc; + td.nJ = d->nJ; + td.efc_J.assign(d->efc_J, d->efc_J + d->nJ); + td.efc_J_rownnz.assign(d->efc_J_rownnz, d->efc_J_rownnz + d->nefc); + td.efc_J_rowadr.assign(d->efc_J_rowadr, d->efc_J_rowadr + d->nefc); + td.efc_J_colind.assign(d->efc_J_colind, d->efc_J_colind + d->nJ); + + mj_deleteData(d); + mj_deleteModel(m); + return td; + }(); + return data; +} + +// ================================ old functions ============================== // transpose sparse matrix (uncompressed) void ABSL_ATTRIBUTE_NOINLINE transposeSparse_baseline( @@ -99,8 +263,7 @@ int ABSL_ATTRIBUTE_NOINLINE combineSparse_baseline(mjtNum* dst, mjtNum a, mjtNum b, int dst_nnz, int src_nnz, int* dst_ind, - const int* src_ind, - mjtNum* buf, int* buf_ind) { + const int* src_ind) { // check for identical pattern if (compare_baseline(dst_ind, src_ind, dst_nnz)) { // combine mjtNum data directly @@ -116,8 +279,7 @@ int ABSL_ATTRIBUTE_NOINLINE combineSparse_new(mjtNum* dst, mjtNum a, mjtNum b, int dst_nnz, int src_nnz, int* dst_ind, - const int* src_ind, - mjtNum* buf, int* buf_ind) { + const int* src_ind) { // check for identical pattern if (compare_memcmp(dst_ind, src_ind, dst_nnz)) { // combine mjtNum data directly @@ -231,61 +393,31 @@ void ABSL_ATTRIBUTE_NOINLINE mulMatVecSparse_8(mjtNum* res, } } -// ----------------------------- benchmark ------------------------------------ +// ----------------------------- benchmark ------------------------------------- static void BM_MatVecSparse(benchmark::State& state, int unroll) { - static mjModel* m = LoadModelFromPath("flex/flag.xml"); - mjData* d = mj_makeData(m); + MatVecData& data = GetMatVecData(); + std::vector res(data.nefc); - // warm-up rollout to get a typical state - for (int i=0; i < kNumWarmupSteps; i++) { - mj_step(m, d); - } - - // allocate gradient - mj_markStack(d); - mjtNum *Ma = mj_stackAllocNum(d, m->nv); - mjtNum *vec = mj_stackAllocNum(d, m->nv); - mjtNum *res = mj_stackAllocNum(d, d->nefc); - mjtNum *grad = mj_stackAllocNum(d, m->nv); - mjtNum *Mgrad = mj_stackAllocNum(d, m->nv); - - // compute gradient - mj_mulM(m, d, Ma, d->qacc); - for (int i=0; i < m->nv; i++) { - grad[i] = Ma[i] - d->qfrc_smooth[i] - d->qfrc_constraint[i]; - } - - // compute search direction - mj_solveM(m, d, Mgrad, grad, 1); - mju_scl(vec, Mgrad, -1, m->nv); - - // save state - std::vector qpos = AsVector(d->qpos, m->nq); - std::vector qvel = AsVector(d->qvel, m->nv); - std::vector act = AsVector(d->act, m->na); - std::vector warmstart = AsVector(d->qacc_warmstart, m->nv); - - // time benchmark for (auto s : state) { if (unroll == 4) { - mju_mulMatVecSparse(res, d->efc_J, vec, d->nefc, - d->efc_J_rownnz, d->efc_J_rowadr, - d->efc_J_colind, d->efc_J_rowsuper); + mju_mulMatVecSparse(res.data(), data.efc_J.data(), data.vec.data(), + data.nefc, data.efc_J_rownnz.data(), + data.efc_J_rowadr.data(), data.efc_J_colind.data(), + data.efc_J_rowsuper.data()); } else if (unroll == 1) { - mulMatVecSparse_1(res, d->efc_J, vec, d->nefc, - d->efc_J_rownnz, d->efc_J_rowadr, - d->efc_J_colind, d->efc_J_rowsuper); + mulMatVecSparse_1(res.data(), data.efc_J.data(), data.vec.data(), + data.nefc, data.efc_J_rownnz.data(), + data.efc_J_rowadr.data(), data.efc_J_colind.data(), + data.efc_J_rowsuper.data()); } else if (unroll == 8) { - mulMatVecSparse_8(res, d->efc_J, vec, d->nefc, - d->efc_J_rownnz, d->efc_J_rowadr, - d->efc_J_colind, d->efc_J_rowsuper); + mulMatVecSparse_8(res.data(), data.efc_J.data(), data.vec.data(), + data.nefc, data.efc_J_rownnz.data(), + data.efc_J_rowadr.data(), data.efc_J_colind.data(), + data.efc_J_rowsuper.data()); } } - // finalize - mj_freeStack(d); - mj_deleteData(d); state.SetItemsProcessed(state.iterations()); } @@ -311,75 +443,30 @@ void ABSL_ATTRIBUTE_NO_TAIL_CALL BM_MatVecSparse_1( BENCHMARK(BM_MatVecSparse_1); static void BM_combineSparse(benchmark::State& state, CombineFuncPtr func) { - static mjModel* m = LoadModelFromPath("humanoid/humanoid.xml"); - m->opt.jacobian = mjJAC_SPARSE; + CombineData& data = GetCombineData(); - mjData* d = mj_makeData(m); - - // warm-up rollout to get a typical state - for (int i=0; i < kNumWarmupSteps; i++) { - mj_step(m, d); - } - - // allocate - mj_markStack(d); - mjtNum* H = mj_stackAllocNum(d, m->nv*m->nv); - int* rownnz = mj_stackAllocInt(d, m->nv); - int* rowadr = mj_stackAllocInt(d, m->nv); - int* colind = mj_stackAllocInt(d, m->nv*m->nv); - int* diagind = mj_stackAllocInt(d, m->nv); - - // compute D corresponding to quad states - mjtNum* D = mj_stackAllocNum(d, d->nefc); - for (int i = 0; i < d->nefc; i++) { - if (d->efc_state[i] == mjCNSTRSTATE_QUADRATIC) { - D[i] = d->efc_D[i]; - } else { - D[i] = 0; - } - } - - int* JT_rownnz = mj_stackAllocInt(d, m->nv); - int* JT_rowadr = mj_stackAllocInt(d, m->nv); - int* JT_rowsuper = mj_stackAllocInt(d, m->nv); - int* JT_colind = mj_stackAllocInt(d, d->nJ); - mjtNum* JT = mj_stackAllocNum(d, d->nJ); - mju_transposeSparse(JT, d->efc_J, d->nefc, m->nv, - JT_rownnz, JT_rowadr, JT_colind, JT_rowsuper, - d->efc_J_rownnz, d->efc_J_rowadr, d->efc_J_colind); - - // compute H = J'*D*J, uncompressed layout - mju_sqrMatTDUncompressedInit(rowadr, m->nv); - mju_sqrMatTDSparse(H, d->efc_J, JT, D, d->nefc, m->nv, - rownnz, rowadr, colind, - d->efc_J_rownnz, d->efc_J_rowadr, - d->efc_J_colind, d->efc_J_rowsuper, - JT_rownnz, JT_rowadr, - JT_colind, JT_rowsuper, d, - diagind); - - // compute H = M + J'*D*J - mj_addM(m, d, H, rownnz, rowadr, colind); + // make working copies that get modified each iteration + std::vector H = data.H; + std::vector rownnz = data.rownnz; + std::vector rowadr = data.rowadr; + std::vector colind = data.colind; // time benchmark for (auto s : state) { - for (int r = m->nv-1; r >= 0; r--) { + for (int r = data.nv-1; r >= 0; r--) { for (int i = 0; i < rownnz[r]-1; i++) { int adr = rowadr[r]; int c = colind[adr+i]; // true arguments should be i+1 and colind+rowadr[r] // but instead we repeat rownnz[c] and colind+rowadr[c] // in order to trigger all if's in combineSparse - func(H+rowadr[c], H+rowadr[r], 1, -H[adr+i], + func(H.data()+rowadr[c], H.data()+rowadr[r], 1, -H[adr+i], rownnz[c], rownnz[c], - colind+rowadr[c], colind+rowadr[c], NULL, NULL); + colind.data()+rowadr[c], colind.data()+rowadr[c]); } } } - // finalize - mj_freeStack(d); - mj_deleteData(d); state.SetItemsProcessed(state.iterations()); } @@ -397,17 +484,6 @@ void ABSL_ATTRIBUTE_NO_TAIL_CALL BM_combineSparse_old( } BENCHMARK(BM_combineSparse_old); -enum class Size { H2_100, H100 }; - -template -const char* ModelPath() { - if constexpr (S == Size::H2_100) { - return "../test/benchmark/testdata/2humanoid100_chol.xml"; - } else { - return "../test/benchmark/testdata/100_humanoids_chol.xml"; - } -} - enum class Supernode { None, PostProcess, @@ -417,44 +493,33 @@ enum class Supernode { template static void BM_transposeSparse(benchmark::State& state, TransposeFuncPtr func, Supernode super) { - static mjModel* m = LoadModelFromPath(ModelPath()); + TransposeData& data = GetTransposeData(); - // force use of sparse matrices - m->opt.jacobian = mjJAC_SPARSE; - - mjData* d = mj_makeData(m); - - // warm-up rollout to get a typical state - while (d->time < 2) { - mj_step(m, d); - } - - mj_markStack(d); - - // need uncompressed layout - mjtNum* res = mj_stackAllocNum(d, m->nv * d->nefc); - int* res_rownnz = mj_stackAllocInt(d, m->nv); - int* res_rowadr = mj_stackAllocInt(d, m->nv); - int* res_rowsuper = mj_stackAllocInt(d, m->nv); - int* res_colind = mj_stackAllocInt(d, m->nv * d->nefc); + // allocate output buffers (uncompressed layout) + std::vector res(data.nv * data.nefc); + std::vector res_rownnz(data.nv); + std::vector res_rowadr(data.nv); + std::vector res_rowsuper(data.nv); + std::vector res_colind(data.nv * data.nefc); // time benchmark for (auto s : state) { - int* rowsuper = (super == Supernode::Inline) ? res_rowsuper : nullptr; - func(res, d->efc_J, d->nefc, m->nv, - res_rownnz, res_rowadr, res_colind, rowsuper, - d->efc_J_rownnz, d->efc_J_rowadr, d->efc_J_colind); + int* rowsuper = + (super == Supernode::Inline) ? res_rowsuper.data() : nullptr; + func(res.data(), data.efc_J.data(), data.nefc, data.nv, + res_rownnz.data(), res_rowadr.data(), res_colind.data(), rowsuper, + data.efc_J_rownnz.data(), data.efc_J_rowadr.data(), + data.efc_J_colind.data()); if (super == Supernode::PostProcess) { - mju_superSparse(m->nv, res_rowsuper, - res_rownnz, res_rowadr, res_colind); + mju_superSparse(data.nv, res_rowsuper.data(), + res_rownnz.data(), res_rowadr.data(), res_colind.data()); } } - mj_freeStack(d); - mj_deleteData(d); state.SetItemsProcessed(state.iterations()); } + void ABSL_ATTRIBUTE_NO_TAIL_CALL BM_transposeSparse_2H100_old(benchmark::State& state) { MujocoErrorTestGuard guard; diff --git a/test/engine/engine_collision_convex_test.cc b/test/engine/engine_collision_convex_test.cc index 9615b542..7d4346e5 100644 --- a/test/engine/engine_collision_convex_test.cc +++ b/test/engine/engine_collision_convex_test.cc @@ -68,7 +68,7 @@ TEST_F(MjcConvexTest, CylinderBox) { EXPECT_EQ(data->ncon, 5); // with multiCCD disabled, should find 1 contact - model->opt.enableflags &= ~mjENBL_MULTICCD; + model->opt.disableflags |= mjDSBL_MULTICCD; mj_forward(model, data); EXPECT_EQ(data->ncon, 1); diff --git a/test/engine/engine_collision_driver_test.cc b/test/engine/engine_collision_driver_test.cc index 0db2556d..aef1d9fe 100644 --- a/test/engine/engine_collision_driver_test.cc +++ b/test/engine/engine_collision_driver_test.cc @@ -390,5 +390,73 @@ TEST_F(MjCollisionTest, MarginSumming) { mj_deleteModel(m); } +TEST_F(MjCollisionTest, MaxContact) { + constexpr char xml[] = R"( + + + + + + + + + + + + + + + + )"; + char error[1024]; + mjModel* m = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(m, NotNull()) << error; + mjData* d = mj_makeData(m); + ASSERT_THAT(d, NotNull()); + + int mesh = mj_name2id(m, mjOBJ_GEOM, "mesh"); + int box = mj_name2id(m, mjOBJ_GEOM, "box"); + int plane = mj_name2id(m, mjOBJ_GEOM, "plane"); + int sphere = mj_name2id(m, mjOBJ_GEOM, "sphere"); + int capsule = mj_name2id(m, mjOBJ_GEOM, "capsule"); + int ellipsoid = mj_name2id(m, mjOBJ_GEOM, "ellipsoid"); + int cylinder = mj_name2id(m, mjOBJ_GEOM, "cylinder"); + + EXPECT_EQ(mj_maxContact(m, mesh, box, -1), 4); + EXPECT_EQ(mj_maxContact(m, mesh, plane, -1), 3); + EXPECT_EQ(mj_maxContact(m, box, plane, -1), 4); + EXPECT_EQ(mj_maxContact(m, mesh, mesh, -1), 4); + EXPECT_EQ(mj_maxContact(m, box, box, -1), 8); + EXPECT_EQ(mj_maxContact(m, capsule, capsule, -1), 2); + EXPECT_EQ(mj_maxContact(m, capsule, box, -1), 4); + EXPECT_EQ(mj_maxContact(m, capsule, plane, -1), 2); + EXPECT_EQ(mj_maxContact(m, cylinder, plane, -1), 4); + EXPECT_EQ(mj_maxContact(m, sphere, sphere, -1), 1); + EXPECT_EQ(mj_maxContact(m, sphere, capsule, -1), 1); + EXPECT_EQ(mj_maxContact(m, sphere, box, -1), 1); + EXPECT_EQ(mj_maxContact(m, sphere, mesh, -1), 1); + EXPECT_EQ(mj_maxContact(m, sphere, plane, -1), 1); + EXPECT_EQ(mj_maxContact(m, sphere, cylinder, -1), 1); + EXPECT_EQ(mj_maxContact(m, ellipsoid, ellipsoid, -1), 1); + EXPECT_EQ(mj_maxContact(m, ellipsoid, box, -1), 1); + EXPECT_EQ(mj_maxContact(m, ellipsoid, mesh, -1), 1); + EXPECT_EQ(mj_maxContact(m, ellipsoid, plane, -1), 1); + EXPECT_EQ(mj_maxContact(m, ellipsoid, cylinder, -1), 1); + EXPECT_EQ(mj_maxContact(m, ellipsoid, capsule, -1), 1); + EXPECT_EQ(mj_maxContact(m, capsule, cylinder, -1), 5); + EXPECT_EQ(mj_maxContact(m, capsule, mesh, -1), 5); + EXPECT_EQ(mj_maxContact(m, cylinder, cylinder, -1), 5); + EXPECT_EQ(mj_maxContact(m, cylinder, box, -1), 5); + EXPECT_EQ(mj_maxContact(m, cylinder, mesh, -1), 5); + + mj_deleteData(d); + mj_deleteModel(m); +} + } // namespace } // namespace mujoco diff --git a/test/engine/engine_collision_gjk_test.cc b/test/engine/engine_collision_gjk_test.cc index 653abbb4..4e4f9f10 100644 --- a/test/engine/engine_collision_gjk_test.cc +++ b/test/engine/engine_collision_gjk_test.cc @@ -1998,7 +1998,7 @@ TEST_F(MjGjkTest, CylinderBoxMargin) { diff --git a/test/engine/engine_core_constraint_test.cc b/test/engine/engine_core_constraint_test.cc index d795a404..07204df8 100644 --- a/test/engine/engine_core_constraint_test.cc +++ b/test/engine/engine_core_constraint_test.cc @@ -15,8 +15,6 @@ // Tests for engine/engine_core_constraint.c. #include -#include -#include #include #include @@ -622,6 +620,149 @@ TEST_F(CoreConstraintTest, StrainConstraintNoPinning) { mj_deleteModel(m); } +// Test flex strain constraint with quadratic interpolation +TEST_F(CoreConstraintTest, StrainConstraintQuadratic) { + static constexpr char xml[] = R"( + + + )"; + std::array error; + mjModel* m = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(m, NotNull()) << error.data(); + mjData* d = mj_makeData(m); + + mj_resetData(m, d); + mj_forward(m, d); + + // Check constraints generated + EXPECT_GT(d->ne, 0) << "Expected strain constraints"; + + // Check that initial strain is ~0 + mjtNum max_pos = 0; + for (int i = 0; i < d->ne; i++) { + if (mju_abs(d->efc_pos[i]) > max_pos) { + max_pos = mju_abs(d->efc_pos[i]); + } + } + EXPECT_LT(max_pos, 1e-6) << "Initial strain should be ~0"; + + // Check Jacobian for NaN + int nv = m->nv; + bool has_bad_jacobian = false; + for (int i = 0; i < d->ne; i++) { + for (int j = 0; j < nv; j++) { + if (mju_isBad(d->efc_J[i*nv + j])) { + has_bad_jacobian = true; + } + } + } + EXPECT_FALSE(has_bad_jacobian) << "Jacobian has NaN"; + + // Run simulation for a few steps + for (int i = 0; i < 100; i++) { + mj_step(m, d); + ASSERT_FALSE(mju_isBad(d->qpos[0])) + << "Simulation unstable at step " << i; + } + + mj_deleteData(d); + mj_deleteModel(m); +} + +// Test quadratic passive forces (no constraints) for stability +TEST_F(CoreConstraintTest, QuadraticPassiveForceStability) { + static constexpr char xml[] = R"( + + + )"; + std::array error; + mjModel* m = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(m, NotNull()) << error.data(); + mjData* d = mj_makeData(m); + + // Run for 500 steps — should stay stable + for (int i = 0; i < 500; i++) { + mj_step(m, d); + ASSERT_FALSE(mju_isBad(d->qpos[0])) + << "Passive quadratic unstable at step " << i; + for (int j = 0; j < m->nv; j++) { + ASSERT_LT(mju_abs(d->qvel[j]), 1000.0) + << "Velocity exploded at step " << i; + } + } + + mj_deleteData(d); + mj_deleteModel(m); +} + +// Test quadratic with anisotropic cells (like what mesh bounding box creates) +TEST_F(CoreConstraintTest, QuadraticAnisotropicStrain) { + static constexpr char xml[] = R"( + + + )"; + std::array error; + mjModel* m = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(m, NotNull()) << error.data(); + mjData* d = mj_makeData(m); + + mj_forward(m, d); + EXPECT_GT(d->ne, 0) << "Expected strain constraints"; + + // Run for 200 steps with gravity + contact + for (int i = 0; i < 200; i++) { + mj_step(m, d); + ASSERT_FALSE(mju_isBad(d->qpos[0])) + << "Anisotropic quadratic unstable at step " << i; + for (int j = 0; j < m->nv; j++) { + ASSERT_LT(mju_abs(d->qvel[j]), 1000.0) + << "Velocity exploded at step " << i + << ", qvel[" << j << "]=" << d->qvel[j]; + } + } + + mj_deleteData(d); + mj_deleteModel(m); +} + TEST_F(CoreConstraintTest, ContactSharedDofJacobian) { constexpr char xml[] = R"( @@ -771,5 +912,143 @@ TEST_F(CoreConstraintTest, JdotvFwdInvIdentity) { } } +// --------------------------- strain constraint rotated parent ---------------- + +struct StrainConstraintTestCase { + std::string test_name; + std::string body_pos; + std::string body_quat; + std::string flex_spacing; + std::string flex_xyaxes; +}; + +class StrainConstraintRotatedTest : public CoreConstraintTest, + public ::testing::WithParamInterface< + StrainConstraintTestCase> { +}; + +TEST_P(StrainConstraintRotatedTest, ResidualIsZero) { + auto param = GetParam(); + std::string xml = R"( + + + )"; + + std::array error; + mjModel* m = LoadModelFromString(xml.c_str(), error.data(), error.size()); + ASSERT_THAT(m, NotNull()) << error.data(); + mjData* d = mj_makeData(m); + + mj_forward(m, d); + + // Check we have strain constraints + EXPECT_GT(d->ne, 0) << "Expected strain constraints"; + + // The critical check: constraint residuals must be ~0 at the initial + // (undeformed) configuration, even though the body is rotated. + mjtNum max_pos = 0; + for (int i = 0; i < d->ne; i++) { + max_pos = mju_max(max_pos, mju_abs(d->efc_pos[i])); + } + EXPECT_LT(max_pos, 1e-6) + << "Strain constraint residual should be ~0" + << " (max_pos=" << max_pos << ")"; + + // Verify stability + for (int i = 0; i < 200; i++) { + mj_step(m, d); + ASSERT_FALSE(mju_isBad(d->qpos[0])) + << "Simulation unstable at step " << i; + for (int j = 0; j < m->nv; j++) { + ASSERT_LT(mju_abs(d->qvel[j]), 1000.0) + << "Velocity exploded at step " << i + << ", qvel[" << j << "]=" << d->qvel[j]; + } + } + + mj_deleteData(d); + mj_deleteModel(m); +} + +INSTANTIATE_TEST_SUITE_P( + StrainConstraintRotatedTests, StrainConstraintRotatedTest, + testing::ValuesIn({ + // Test strain constraint with a rotated parent body. + // The flexcomp is placed inside a parent body that has a non-identity + // initial rotation. This reproduces the "grocery scene" bug where the + // stiffness matrix eigenvectors and reference positions were computed + // in world frame instead of the unrotated local frame, causing + // spurious constraint forces. + { + "RotatedParent", + "1 2 3", + "0.707107 0 0.707107 0", + ".1 .1 .1", + "" + }, + // Same test with an anisotropic box (different spacing per axis) and + // arbitrary rotation (combined 45-deg Y + 30-deg X). + { + "RotatedParentAnisotropic", + "0.5 -1 2", + "0.8924 0.2392 0.3696 -0.0990", + ".15 .08 .05", + "" + }, + // Test strain constraint with flexcomp-level xyaxes rotation. + // This is the "grocery scene" pattern where the flexcomp grid itself is + // rotated via xyaxes="0 1 0 0 0 1" (X->Y, Y->Z). + { + "FlexcompXyaxes", + "", + "", + ".1 .02 .1", + "0 1 0 0 0 1" + }, + // Test combining parent body rotation with flexcomp xyaxes rotation. + // The total rotation is the composition of both. + { + "RotatedParentPlusXyaxes", + "1 2 3", + "0.707107 0 0.707107 0", + ".15 .08 .05", + "0 1 0 0 0 1" + } + }), + [](const testing::TestParamInfo< + StrainConstraintRotatedTest::ParamType>& info) { + return info.param.test_name; + } +); + } // namespace } // namespace mujoco diff --git a/test/engine/engine_solver_test.cc b/test/engine/engine_solver_test.cc index 523dfd33..8aaf1d9d 100644 --- a/test/engine/engine_solver_test.cc +++ b/test/engine/engine_solver_test.cc @@ -46,6 +46,7 @@ TEST_F(SolverTest, IslandsEquivalent) { model->opt.tolerance = 0; // set tolerance to 0 model->opt.ls_tolerance = 0; // set ls_tolerance to 0 model->opt.ccd_tolerance = 0; // set ccd_tolerance to 0 + model->opt.disableflags |= mjDSBL_MULTICCD; // disable multiccd int nv = model->nv; diff --git a/test/experimental/usd/mjcPhysics/mjc_physics_scene_test.cc b/test/experimental/usd/mjcPhysics/mjc_physics_scene_test.cc index 30cb4d70..d7f106d4 100644 --- a/test/experimental/usd/mjcPhysics/mjc_physics_scene_test.cc +++ b/test/experimental/usd/mjcPhysics/mjc_physics_scene_test.cc @@ -179,8 +179,6 @@ TEST_F(MjcPhysicsSceneTest, TestDefaults) { EXPECT_ENABLE_FLAG_USD_FALLBACK_EQ_MODEL_DEFAULT(FwdinvFlag, mjENBL_FWDINV); EXPECT_ENABLE_FLAG_USD_FALLBACK_EQ_MODEL_DEFAULT(InvDiscreteFlag, mjENBL_INVDISCRETE); - EXPECT_ENABLE_FLAG_USD_FALLBACK_EQ_MODEL_DEFAULT(MultiCCDFlag, - mjENBL_MULTICCD); mj_deleteModel(default_model); mj_deleteSpec(empty_spec); diff --git a/test/user/CMakeLists.txt b/test/user/CMakeLists.txt index 912a26c0..c2d3b69d 100644 --- a/test/user/CMakeLists.txt +++ b/test/user/CMakeLists.txt @@ -40,3 +40,5 @@ mujoco_test(user_composite_test) mujoco_test(user_resource_test) mujoco_test(user_vfs_test) + +mujoco_test(user_util_test) diff --git a/test/user/user_api_test.cc b/test/user/user_api_test.cc index 3c7af426..57e889bb 100644 --- a/test/user/user_api_test.cc +++ b/test/user/user_api_test.cc @@ -19,6 +19,7 @@ #include #include #include +#include #include // NOLINT #include #include @@ -32,6 +33,7 @@ #include "src/cc/array_safety.h" #include #include +#include #include "src/xml/xml_api.h" #include "src/xml/xml_numeric_format.h" #include "test/fixture.h" @@ -203,6 +205,103 @@ TEST_F(MujocoTest, AttachAndChildDeletion) { mj_deleteSpec(parent_spec); } +int open_mock(mjResource* resource) { + static const char parent_xml[] = R"( + + + + + + )"; + resource->data = mju_malloc(sizeof(parent_xml)); + std::strcpy((char*)resource->data, parent_xml); + return 1; +} + +int read_mock(mjResource* resource, const void** buffer) { + *buffer = resource->data; + return std::strlen((const char*)resource->data); +} + +void close_mock(mjResource* resource) { + mju_free(resource->data); + resource->data = nullptr; +} + +TEST_F(MujocoTest, AttachedSpecDoesNotInheritURI) { + // This test checks that when we attach a child spec to a parent spec that was + // loaded from a resource provider, the child spec does not inherit the + // resource URI from the parent. This allows the child spec to specify assets + // relative to its model file or in the VFS. + mjpResourceProvider provider = { + .prefix = "fakeprovider", + .open = open_mock, + .read = read_mock, + .close = close_mock, + }; + + mjp_registerResourceProvider(&provider); + + std::array err; + mjSpec* parent_spec = + mj_parseXML("fakeprovider:parent.xml", nullptr, err.data(), err.size()); + mjs_setString(parent_spec->modelname, "parent"); + ASSERT_THAT(parent_spec, NotNull()) << err.data(); + + // Create child spec + static constexpr char child_xml[] = R"( + + + + + + + + + + + )"; + + // Setup VFS with asset + mjVFS vfs; + mj_defaultVFS(&vfs); + static constexpr char asset_data[] = R"( + v 0 0 0 + v 1 0 0 + v 0 1 0 + v 0 0 1 + f 1 2 3 + f 1 2 4 + f 2 3 4 + f 3 1 4 + )"; + mj_addBufferVFS(&vfs, "asset.obj", asset_data, sizeof(asset_data)); + + mjSpec* child_spec = + mj_parseXMLString(child_xml, &vfs, err.data(), err.size()); + mjs_setString(child_spec->modelname, "child"); + ASSERT_THAT(child_spec, NotNull()) << err.data(); + + // Attach child spec to parent spec's world body + mjsBody* world = mjs_findBody(parent_spec, "world"); + ASSERT_THAT(world, NotNull()); + + mjsElement* attached = + mjs_attach(world->element, child_spec->element, "", ""); + ASSERT_THAT(attached, NotNull()); + + mjModel* model = mj_compile(parent_spec, &vfs); + mj_deleteVFS(&vfs); + + EXPECT_THAT(model, NotNull()) << mjs_getError(parent_spec); + + if (model) { + mj_deleteModel(model); + } + mj_deleteSpec(parent_spec); + mj_deleteSpec(child_spec); +} + TEST_F(MujocoTest, ActivatePlugin) { mjSpec* spec = mj_makeSpec(); mjs_activatePlugin(spec, "mujoco.elasticity.cable"); diff --git a/test/user/user_flex_test.cc b/test/user/user_flex_test.cc index 5482568d..c4d10e10 100644 --- a/test/user/user_flex_test.cc +++ b/test/user/user_flex_test.cc @@ -1031,5 +1031,325 @@ TEST_F(UserFlexTest, FlexNoConstraintsWarning) { mj_deleteModel(m); } +TEST_F(UserFlexTest, EmptyCellNodePinning) { + // A 2x2x2 grid with a box mesh that fills all cells. + // No nodes should be pinned. + static constexpr char xml[] = R"( + + + + + + + + + )"; + std::array error; + mjModel* m = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(m, NotNull()) << error.data(); + + // A 2x2x2 grid with trilinear order has (2+1)^3 = 27 node positions. + int nadr = m->flex_nodeadr[0]; + int nnode = m->flex_nodenum[0]; + EXPECT_EQ(nnode, 27); + + // All cells are occupied by the box, so no node should be pinned. + int pinned = 0; + for (int n = nadr; n < nadr + nnode; n++) { + int bid = m->flex_nodebodyid[n]; + if (m->body_jntnum[bid] == 0) { + pinned++; + } + } + EXPECT_EQ(pinned, 0); + + // Verify simulation works + mjData* d = mj_makeData(m); + for (int i = 0; i < 10; i++) { + mj_step(m, d); + } + + mj_deleteData(d); + mj_deleteModel(m); +} + +TEST_F(UserFlexTest, EmptyCellNodePinningMesh) { + // Load bunny_multicell.xml which has a 3x3x3 grid. + // The bunny mesh only occupies some cells, so many nodes should be pinned. + const std::string xml_path = + GetModelPath("flex/bunny_multicell.xml"); + std::array error; + mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size()); + ASSERT_THAT(m, NotNull()) << error.data(); + + // 3x3x3 grid, order=1: (3+1)^3 = 64 node positions + int nadr = m->flex_nodeadr[0]; + int nnode = m->flex_nodenum[0]; + EXPECT_EQ(nnode, 64); + + // Count pinned nodes (no joints) + int pinned = 0; + int free_nodes = 0; + for (int n = nadr; n < nadr + nnode; n++) { + int bid = m->flex_nodebodyid[n]; + if (m->body_jntnum[bid] == 0) { + pinned++; + } else { + free_nodes++; + } + } + + // At least some nodes should be pinned since the bunny doesn't fill all cells + EXPECT_GT(pinned, 0) << "Expected some nodes to be pinned from empty cells"; + EXPECT_GT(free_nodes, 0) << "Expected some nodes to remain free"; + EXPECT_EQ(pinned + free_nodes, nnode); + + // Verify the model can simulate + mjData* d = mj_makeData(m); + mj_forward(m, d); + for (int i = 0; i < 10; i++) { + mj_step(m, d); + } + + mj_deleteData(d); + mj_deleteModel(m); +} + +TEST_F(UserFlexTest, EmptyCellNodePinningQuadratic) { + // Regression test for ci_min calculation with order=2. + // A 2x1x1 quadratic grid has nodes at gi=0..4 (5 nodes per axis). + // We place mesh vertices only in cell 0 (x in [0, 0.5]), so cell 1 is empty. + // + // Node gi=3 belongs only to cell 1 (1*2 <= 3 <= 2*2). + // With the old formula (gi-order)/order = (3-2)/2 = 0, it would also check + // cell 0 (non-empty), incorrectly marking gi=3 as non-pinned. + // Single hex element at x=[0,0.3], well inside cell 0 of a 3x1x1 grid. + // Anchor vertex at x=1.0 extends the bounding box to [0,1]^3. + // The 3x1x1 quadratic grid splits at x=0.33, 0.67. + // Cell 0 has vertices, cells 1 and 2 are empty. + // Interior nodes for cells 1,2 should be pinned to the parent body. + static constexpr char xml[] = R"( + + + + + + + + + + + + + )"; + std::array error; + mjModel* m = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(m, NotNull()) << error.data(); + + // 3x1x1 quadratic grid: (3*2+1) * (1*2+1) * (1*2+1) = 7*3*3 = 63 nodes + int nadr = m->flex_nodeadr[0]; + int nnode = m->flex_nodenum[0]; + EXPECT_EQ(nnode, 63); + + // Count pinned nodes: pinned nodes are assigned to the parent body. + int parent_bid = mj_name2id(m, mjOBJ_BODY, "parent"); + ASSERT_GT(parent_bid, 0); + int pinned = 0; + for (int n = nadr; n < nadr + nnode; n++) { + if (m->flex_nodebodyid[n] == parent_bid) { + pinned++; + } + } + + // Cells 1 and 2 are empty, so nodes exclusively in those cells are pinned. + // Nodes at gi=3..6 (with any gj, gk) are only in cells 1 and/or 2. + // That's 4 * 3 * 3 = 36 nodes. + EXPECT_EQ(pinned, 36); + + mj_deleteData(mj_makeData(m)); + mj_deleteModel(m); +} + +TEST_F(UserFlexTest, EmptyCellDetectsElements) { + // A cube surface mesh (dim=2, 12 triangles) spanning [0,1]^3. + // With cellcount="6 6 6" (216 cells), only 8 corner cells contain + // mesh vertices. + // + // Bug: MarkEmptyCells only checked vertices, so 208/216 cells are + // marked empty, causing most interior nodes to be incorrectly pinned. + // Fix: check element AABBs to correctly identify occupied cells. + static constexpr char xml[] = R"( + + + + + + + + + + + + )"; + std::array error; + mjModel* m = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(m, NotNull()) << error.data(); + + // 6x6x6 trilinear grid: (6+1)^3 = 343 nodes + int nadr = m->flex_nodeadr[0]; + int nnode = m->flex_nodenum[0]; + ASSERT_EQ(nnode, 343); + + // Count pinned nodes: those assigned to the parent body. + int parent_bid = mj_name2id(m, mjOBJ_BODY, "parent"); + ASSERT_GT(parent_bid, 0); + int pinned = 0; + for (int n = nadr; n < nadr + nnode; n++) { + if (m->flex_nodebodyid[n] == parent_bid) { + pinned++; + } + } + + // The cube surface fills the entire bounding box. The element-AABB + // marks all boundary cells as surface cells (152/216). The interior + // flood-fill finds no exterior seeds (all boundary cells are surface), + // so the remaining 64 cells are classified as interior (non-empty). + // No cells are empty → 0 nodes pinned. + EXPECT_EQ(pinned, 0); + + mj_deleteData(mj_makeData(m)); + mj_deleteModel(m); +} + +TEST_F(UserFlexTest, TotalMassTrilinear) { + static constexpr char xml[] = R"( + + + + + + + + )"; + std::array error; + mjModel* m = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(m, NotNull()) << error.data(); + + double total_mass = 0; + for (int i = 1; i < m->nbody; ++i) { + total_mass += m->body_mass[i]; + } + + EXPECT_NEAR(total_mass, 1.5, 1e-5); + mj_deleteModel(m); +} + +TEST_F(UserFlexTest, TotalMassQuadratic) { + static constexpr char xml[] = R"( + + + + + + + + )"; + std::array error; + mjModel* m = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(m, NotNull()) << error.data(); + + double total_mass = 0; + for (int i = 1; i < m->nbody; ++i) { + total_mass += m->body_mass[i]; + } + + EXPECT_NEAR(total_mass, 2.0, 1e-5); + mj_deleteModel(m); +} + +TEST_F(UserFlexTest, Dof2d) { + // 3x3 grid with dof="2d": 9 vertices, 2 DOFs each -> nv = 18 + static constexpr char xml_2d[] = R"( + + + + + + + + )"; + + // same model with dof="full" for comparison: 9 vertices, 3 DOFs each -> nv = 27 + static constexpr char xml_full[] = R"( + + + + + + + + )"; + + std::array error; + + // load 2d model + mjModel* m_2d = LoadModelFromString(xml_2d, error.data(), error.size()); + ASSERT_THAT(m_2d, NotNull()) << error.data(); + mjData* d_2d = mj_makeData(m_2d); + + // load full model + mjModel* m_full = LoadModelFromString(xml_full, error.data(), error.size()); + ASSERT_THAT(m_full, NotNull()) << error.data(); + mjData* d_full = mj_makeData(m_full); + + // verify DOF counts + EXPECT_EQ(m_2d->nv, 18); // 9 vertices * 2 DOFs + EXPECT_EQ(m_full->nv, 27); // 9 vertices * 3 DOFs + + // same number of vertices and elements + EXPECT_EQ(m_2d->nflexvert, m_full->nflexvert); + EXPECT_EQ(m_2d->nflexelem, m_full->nflexelem); + + // each body has 2 DOFs in 2d mode, 3 in full mode + for (int i = 1; i < m_2d->nbody; i++) { + EXPECT_EQ(m_2d->body_dofnum[i], 2) << "body " << i; + } + for (int i = 1; i < m_full->nbody; i++) { + EXPECT_EQ(m_full->body_dofnum[i], 3) << "body " << i; + } + + // simulate a few steps to make sure nothing crashes + for (int i = 0; i < 10; i++) { + mj_step(m_2d, d_2d); + mj_step(m_full, d_full); + } + + mj_deleteModel(m_2d); + mj_deleteModel(m_full); + mj_deleteData(d_2d); + mj_deleteData(d_full); +} + } // namespace } // namespace mujoco + diff --git a/test/user/user_mesh_test.cc b/test/user/user_mesh_test.cc index 98944622..59833aef 100644 --- a/test/user/user_mesh_test.cc +++ b/test/user/user_mesh_test.cc @@ -819,6 +819,58 @@ TEST_F(MjCMeshTest, VolumeSmallAllowedShell) { mj_deleteModel(model); } +TEST_F(MjCMeshTest, Flex2DElasticityRequiresPositiveThickness) { + static constexpr char xml[] = R"( + + + + + + + + )"; + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(model, testing::IsNull()); + EXPECT_THAT(error.data(), + HasSubstr("2d elasticity requires positive thickness")); +} + +TEST_F(MjCMeshTest, InterpolatedFlexDoesNotSupport2DElasticity) { + static constexpr char xml[] = R"( + + + + + + + + + )"; + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(model, testing::IsNull()); + EXPECT_THAT( + error.data(), + HasSubstr("interpolated flex does not yet support 2d elasticity")); +} + +TEST_F(MjCMeshTest, Flex2DElasticityRequires2DFlex) { + static constexpr char xml[] = R"( + + + + + + + + )"; + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(model, testing::IsNull()); + EXPECT_THAT(error.data(), HasSubstr("2d elasticity requires 2d flex")); +} + TEST_F(MjCMeshTest, VolumeNegativeThrowsError) { static constexpr char xml[] = R"( diff --git a/test/user/user_util_test.cc b/test/user/user_util_test.cc index 9873c74b..67c91e2b 100644 --- a/test/user/user_util_test.cc +++ b/test/user/user_util_test.cc @@ -17,6 +17,8 @@ #include "src/user/user_util.h" #include +#include +#include #include #include @@ -180,5 +182,142 @@ TEST_F(UserUtilTest, VectorToStringEmpty) { EXPECT_EQ(VectorToString(v), ""); } +// utility: modified Gram-Schmidt to orthogonalize columns of Q (n x n) +static void gramSchmidt(double* Q, int n) { + for (int j = 0; j < n; j++) { + // subtract projections onto previous columns + for (int k = 0; k < j; k++) { + double dot = 0; + for (int i = 0; i < n; i++) { + dot += Q[i * n + j] * Q[i * n + k]; + } + for (int i = 0; i < n; i++) { + Q[i * n + j] -= dot * Q[i * n + k]; + } + } + // normalize + double norm = 0; + for (int i = 0; i < n; i++) { + norm += Q[i * n + j] * Q[i * n + j]; + } + norm = std::sqrt(norm); + for (int i = 0; i < n; i++) { + Q[i * n + j] /= norm; + } + } +} + +// utility: compose SPD matrix A = Q * diag(eigvals) * Q^T +static void composeMatrix(double* A, const double* Q, + const double* eigvals, int n) { + for (int i = 0; i < n; i++) { + for (int j = 0; j <= i; j++) { + double sum = 0; + for (int k = 0; k < n; k++) { + sum += Q[i * n + k] * eigvals[k] * Q[j * n + k]; + } + A[i * n + j] = sum; + A[j * n + i] = sum; + } + } +} + +TEST_F(UserUtilTest, EigendecomposeConvergence) { + // seeded RNG for reproducibility + std::mt19937_64 rng; + rng.seed(42); + std::normal_distribution dist(0, 1); + + // sweep over matrix sizes used by flex stiffness + // order=1: 8 nodes * 3 dof = 24 + // order=2: 27 nodes * 3 dof = 81 + for (int n : {24, 81}) { + int total_sweeps = 0; + int max_sweeps = 0; + int count = 0; + + // generate random orthogonal matrix Q via Gram-Schmidt + std::vector Q(n * n); + for (int i = 0; i < n * n; i++) { + Q[i] = dist(rng); + } + gramSchmidt(Q.data(), n); + + // sweep eigenvalue spectra of varying difficulty + // well-separated, clustered, wide condition number + for (double condition : {1e1, 1e3, 1e6}) { + for (double cluster : {0.0, 0.5, 0.9}) { + // construct eigenvalues + std::vector eigvals(n); + for (int i = 0; i < n; i++) { + // base: logarithmically spaced from 1 to condition + double t = (double)i / (n - 1); + double base = std::exp(t * std::log(condition)); + + // cluster: push eigenvalues toward geometric mean + double mean = std::sqrt(condition); + eigvals[i] = (1 - cluster) * base + cluster * mean; + } + + // compose A = Q * diag(eigvals) * Q^T + std::vector A(n * n); + composeMatrix(A.data(), Q.data(), eigvals.data(), n); + + // save copy for verification + std::vector A_copy(A); + + // decompose + std::vector found_eigval(n); + std::vector found_eigvec(n * n); + int sweeps = mjuu_eigendecompose( + A.data(), found_eigval.data(), + found_eigvec.data(), n); + + total_sweeps += sweeps; + if (sweeps > max_sweeps) max_sweeps = sweeps; + count++; + + // verify convergence + EXPECT_LT(sweeps, 200) + << "n=" << n + << " condition=" << condition + << " cluster=" << cluster; + + // verify A*v = lambda*v for each eigenpair + for (int i = 0; i < n; i++) { + for (int r = 0; r < n; r++) { + double Av = 0; + for (int c = 0; c < n; c++) { + Av += A_copy[r * n + c] * found_eigvec[c * n + i]; + } + double lv = found_eigval[i] * found_eigvec[r * n + i]; + EXPECT_NEAR(Av, lv, + 1e-6 * std::abs(found_eigval[i])) + << "n=" << n << " condition=" << condition + << " cluster=" << cluster + << " eigpair=" << i << " row=" << r; + } + } + + // verify all eigenvalues are positive + for (int i = 0; i < n; i++) { + EXPECT_GT(found_eigval[i], 0) + << "n=" << n << " eigenvalue " << i; + } + } + } + + double mean_sweeps = (double)total_sweeps / count; + + // assert reasonable average convergence + EXPECT_LE(mean_sweeps, 20.0) + << "n=" << n << ": mean sweeps too high"; + + // assert max sweeps within budget + EXPECT_LT(max_sweeps, 200) + << "n=" << n << ": max sweeps exceeded 200"; + } +} + } // namespace } // namespace mujoco diff --git a/test/user/user_vfs_test.cc b/test/user/user_vfs_test.cc index db5a22b5..16a62da2 100644 --- a/test/user/user_vfs_test.cc +++ b/test/user/user_vfs_test.cc @@ -224,6 +224,38 @@ TEST_F(UserVfsTest, DeleteFileRepeat) { mj_deleteVFS(&vfs); } +TEST_F(UserVfsTest, ContainsBuffer) { + mjVFS vfs; + mj_defaultVFS(&vfs); + std::string buffer = ""; + const void* ptr = static_cast(buffer.c_str()); + mj_addBufferVFS(&vfs, "model", ptr, buffer.size()); + + EXPECT_TRUE(mj_containsBufferVFS(&vfs, "model")); + EXPECT_FALSE(mj_containsBufferVFS(&vfs, "nonexistent")); + EXPECT_FALSE(mj_containsBufferVFS(&vfs, "Model")); + + mj_deleteVFS(&vfs); +} + +TEST_F(UserVfsTest, ContainsFile) { + mjVFS vfs; + mj_defaultVFS(&vfs); + + constexpr char path[] = "engine/testdata/actuation/"; + const std::string dir = GetTestDataFilePath(path); + std::string file = "activation.xml"; + mj_addFileVFS(&vfs, dir.c_str(), file.c_str()); + + EXPECT_TRUE(mj_containsFileVFS(&vfs, dir.c_str(), file.c_str())); + EXPECT_TRUE(mj_containsFileVFS(&vfs, nullptr, (dir + file).c_str())); + EXPECT_TRUE(mj_containsFileVFS(&vfs, nullptr, "Activation.xml")); + EXPECT_TRUE(mj_containsFileVFS(&vfs, "some/dir/", "activation.xml")); + EXPECT_FALSE(mj_containsFileVFS(&vfs, nullptr, "nonexistent.xml")); + + mj_deleteVFS(&vfs); +} + TEST_F(UserVfsTest, AddBuffer) { mjVFS vfs; diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index 1673c4d0..e3bd9fe9 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -137,16 +137,16 @@ public enum mjtDisableBit : int{ mjDSBL_AUTORESET = 65536, mjDSBL_NATIVECCD = 131072, mjDSBL_ISLAND = 262144, - mjNDISABLE = 19, + mjDSBL_MULTICCD = 524288, + mjNDISABLE = 20, } public enum mjtEnableBit : int{ mjENBL_OVERRIDE = 1, mjENBL_ENERGY = 2, mjENBL_FWDINV = 4, mjENBL_INVDISCRETE = 8, - mjENBL_MULTICCD = 16, - mjENBL_SLEEP = 32, - mjNENABLE = 6, + mjENBL_SLEEP = 16, + mjNENABLE = 5, } public enum mjtJoint : int{ mjJNT_FREE = 0, @@ -6684,6 +6684,12 @@ public static unsafe extern int mj_addBufferVFS(void* vfs, [MarshalAs(UnmanagedT [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] public static unsafe extern int mj_deleteFileVFS(void* vfs, [MarshalAs(UnmanagedType.LPStr)]string filename); +[DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] +public static unsafe extern int mj_containsBufferVFS(void* vfs, [MarshalAs(UnmanagedType.LPStr)]string name); + +[DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] +public static unsafe extern int mj_containsFileVFS(void* vfs, [MarshalAs(UnmanagedType.LPStr)]string directory, [MarshalAs(UnmanagedType.LPStr)]string filename); + [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] public static unsafe extern void mj_deleteVFS(void* vfs); @@ -6951,6 +6957,9 @@ public static unsafe extern void mj_rne(mjModel_* m, mjData_* d, int flg_acc, do [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] public static unsafe extern void mj_rnePostConstraint(mjModel_* m, mjData_* d); +[DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] +public static unsafe extern int mj_maxContact(mjModel_* m, int g1, int g2, int has_margin); + [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] public static unsafe extern void mj_collision(mjModel_* m, mjData_* d); diff --git a/wasm/codegen/generated/bindings.cc b/wasm/codegen/generated/bindings.cc index 185f5bb7..ae618fd8 100644 --- a/wasm/codegen/generated/bindings.cc +++ b/wasm/codegen/generated/bindings.cc @@ -8845,6 +8845,10 @@ void mj_makeM_wrapper(const MjModel& m, MjData& d) { mj_makeM(m.get(), d.get()); } +int mj_maxContact_wrapper(const MjModel& m, int g1, int g2, int has_margin) { + return mj_maxContact(m.get(), g1, g2, has_margin); +} + void mj_mulJacTVec_wrapper(const MjModel& m, const MjData& d, const val& res, const NumberArray& vec) { UNPACK_VALUE(mjtNum, res); UNPACK_ARRAY(mjtNum, vec); @@ -10942,6 +10946,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) { .value("mjDSBL_AUTORESET", mjDSBL_AUTORESET) .value("mjDSBL_NATIVECCD", mjDSBL_NATIVECCD) .value("mjDSBL_ISLAND", mjDSBL_ISLAND) + .value("mjDSBL_MULTICCD", mjDSBL_MULTICCD) .value("mjNDISABLE", mjNDISABLE); enum_("mjtDyn") .value("mjDYN_NONE", mjDYN_NONE) @@ -10956,7 +10961,6 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) { .value("mjENBL_ENERGY", mjENBL_ENERGY) .value("mjENBL_FWDINV", mjENBL_FWDINV) .value("mjENBL_INVDISCRETE", mjENBL_INVDISCRETE) - .value("mjENBL_MULTICCD", mjENBL_MULTICCD) .value("mjENBL_SLEEP", mjENBL_SLEEP) .value("mjNENABLE", mjNENABLE); enum_("mjtEq") @@ -13173,6 +13177,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) { function("mj_local2Global", &mj_local2Global_wrapper); function("mj_makeConstraint", &mj_makeConstraint_wrapper); function("mj_makeM", &mj_makeM_wrapper); + function("mj_maxContact", &mj_maxContact_wrapper); function("mj_mulJacTVec", &mj_mulJacTVec_wrapper); function("mj_mulJacVec", &mj_mulJacVec_wrapper); function("mj_mulM", &mj_mulM_wrapper); diff --git a/wasm/codegen/generators/constants.py b/wasm/codegen/generators/constants.py index 1a8aaeee..111c9997 100644 --- a/wasm/codegen/generators/constants.py +++ b/wasm/codegen/generators/constants.py @@ -124,6 +124,8 @@ _SKIPPED_ASSET_CACHE_FUNCTIONS: tuple[str, ...] = ( _SKIPPED_VFS_FUNCTIONS: tuple[str, ...] = ( # go/keep-sorted start "mj_addFileVFS", + "mj_containsBufferVFS", + "mj_containsFileVFS", "mj_mountVFS", "mj_unmountVFS", # go/keep-sorted end diff --git a/wasm/tests/bindings_test.ts b/wasm/tests/bindings_test.ts index 516cf507..a58fd087 100644 --- a/wasm/tests/bindings_test.ts +++ b/wasm/tests/bindings_test.ts @@ -684,10 +684,10 @@ describe('MuJoCo WASM Bindings', () => { it('should check constants values', () => { expect(mujoco.mjNEQDATA).toBe(11); expect(mujoco.mjDISABLESTRING).toEqual([ - 'Constraint', 'Equality', 'Frictionloss', 'Limit', 'Contact', 'Spring', - 'Damper', 'Gravity', 'Clampctrl', 'Warmstart', 'Filterparent', - 'Actuation', 'Refsafe', 'Sensor', 'Midphase', 'Eulerdamp', 'AutoReset', - 'NativeCCD', 'Island' + 'Constraint', 'Equality', 'Frictionloss', 'Limit', 'Contact', + 'Spring', 'Damper', 'Gravity', 'Clampctrl', 'Warmstart', + 'Filterparent', 'Actuation', 'Refsafe', 'Sensor', 'Midphase', + 'Eulerdamp', 'AutoReset', 'NativeCCD', 'Island', 'MultiCCD', ]); expect(mujoco.mjRNDSTRING).toEqual([ ['Shadow', '1', 'S'], ['Wireframe', '0', 'W'], ['Reflection', '1', 'R'],