diff --git a/CMakeLists.txt b/CMakeLists.txt index bb607afd..1caaa05c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -83,13 +83,14 @@ target_include_directories( add_subdirectory(plugin/elasticity) add_subdirectory(plugin/sensor) +add_subdirectory(plugin/sdf) add_subdirectory(src/engine) add_subdirectory(src/user) add_subdirectory(src/xml) add_subdirectory(src/render) add_subdirectory(src/ui) -target_compile_definitions(mujoco PRIVATE _GNU_SOURCE CCD_STATIC_DEFINE MUJOCO_DLL_EXPORTS) +target_compile_definitions(mujoco PRIVATE _GNU_SOURCE CCD_STATIC_DEFINE MUJOCO_DLL_EXPORTS -DMC_IMPLEM_ENABLE) if(MUJOCO_ENABLE_AVX_INTRINSICS) target_compile_definitions(mujoco PUBLIC mjUSEPLATFORMSIMD) endif() diff --git a/cmake/MujocoDependencies.cmake b/cmake/MujocoDependencies.cmake index 5b44318c..3e322ea4 100644 --- a/cmake/MujocoDependencies.cmake +++ b/cmake/MujocoDependencies.cmake @@ -26,6 +26,10 @@ set(MUJOCO_DEP_VERSION_tinyobjloader 1421a10d6ed9742f5b2c1766d22faa6cfbc56248 CACHE STRING "Version of `tinyobjloader` to be fetched." ) +set(MUJOCO_DEP_VERSION_MarchingCubeCpp + 5b79e5d6bded086a0abe276a4b5a69fc17ae9bf1 + CACHE STRING "Version of `MarchingCubeCpp` to be fetched." +) set(MUJOCO_DEP_VERSION_ccd 7931e764a19ef6b21b443376c699bbc9c6d4fba8 # v2.1 CACHE STRING "Version of `ccd` to be fetched." @@ -55,6 +59,7 @@ set(MUJOCO_DEP_VERSION_benchmark ) mark_as_advanced(MUJOCO_DEP_VERSION_lodepng) +mark_as_advanced(MUJOCO_DEP_VERSION_MarchingCubeCpp) mark_as_advanced(MUJOCO_DEP_VERSION_tinyxml2) mark_as_advanced(MUJOCO_DEP_VERSION_tinyobjloader) mark_as_advanced(MUJOCO_DEP_VERSION_ccd) @@ -99,6 +104,20 @@ if(NOT TARGET lodepng) endif() endif() +if(NOT TARGET marchingcubecpp) + FetchContent_Declare( + marchingcubecpp + GIT_REPOSITORY https://github.com/aparis69/MarchingCubeCpp.git + GIT_TAG ${MUJOCO_DEP_VERSION_MarchingCubeCpp} + ) + + FetchContent_GetProperties(marchingcubecpp) + if(NOT marchingcubecpp_POPULATED) + FetchContent_Populate(marchingcubecpp) + include_directories(${marchingcubecpp_SOURCE_DIR}) + endif() +endif() + set(QHULL_ENABLE_TESTING OFF) findorfetch( diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index ee9fba7a..98f241e9 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -1558,6 +1558,24 @@ The full list of processing steps applied by the compiler to each mesh is as fol Reference orientation relative to which the 3D vertex coordinates and normals are defined. The conjugate of this quaternion is used to rotate the positions and normals. The model compiler normalizes the quaternion automatically. +.. _mesh-plugin: + +:el-prefix:`mesh/` |-| **plugin** (?) +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Associate this mesh with an :ref:`engine plugin`. Either :at:`plugin` or :at:`instance` are required. + +.. _mesh-plugin-plugin: + +:at:`plugin`: :at-val:`string, optional` + Plugin identifier, used for implicit plugin instantiation. + +.. _mesh-plugin-instance: + +:at:`instance`: :at-val:`string, optional` + Instance name, used for explicit plugin instantiation. + + .. _asset-skin: @@ -1977,6 +1995,16 @@ adjust it properly through the XML. :at:`mpr_tolerance`: :at-val:`real, "1e-6"` Tolerance threshold used for early termination of the MPR algorithm. +.. _option-sdf_iterations: + +:at:`sdf_iterations`: :at-val:`int, "10"` + Number of iterations used for Signed Distance Field collisions (per initial point). + +.. _option-sdf_initpoints: + +:at:`sdf_initpoints`: :at-val:`int, "40"` + Number of starting points used for fining contacts with Signed Distance Field collisions. + .. _option-flag: @@ -2513,7 +2541,7 @@ helps clarify the role of bodies and geoms in MuJoCo. .. _body-geom-type: -:at:`type`: :at-val:`[plane, hfield, sphere, capsule, ellipsoid, cylinder, box, mesh], "sphere"` +:at:`type`: :at-val:`[plane, hfield, sphere, capsule, ellipsoid, cylinder, box, mesh, sdf], "sphere"` Type of geometric shape. The keywords have the following meaning: The **plane** type defines a plane which is infinite for collision detection purposes. It can only be attached to the world body or static children of the world. The plane passes through a point specified via the pos attribute. It is normal to the Z axis of the geom's local @@ -2576,6 +2604,11 @@ helps clarify the role of bodies and geoms in MuJoCo. they are offset by the translation and rotation that were needed to center and align the mesh asset in its own coordinate frame. Recall the discussion of centering and alignment in the :ref:`mesh ` element. + The **sdf** type defines a signed distance field (SDF, also referred to as signed distance function). In order to + visualize the SDF, a custom mesh must be specified using the :ref:`mesh/plugin ` attribute. See the + `model/plugin/sdf/ `__ directory for example models + with SDF geometries. For more details regarding SDF plugins, see the :ref:`Extensions chapter`. + .. _body-geom-contype: :at:`contype`: :at-val:`int, "1"` @@ -2832,6 +2865,23 @@ helps clarify the role of bodies and geoms in MuJoCo. :at:`user`: :at-val:`real(nuser_geom), "0 0 ..."` See :ref:`CUser`. +.. _geom-plugin: + +:el-prefix:`geom/` |-| **plugin** (?) +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Associate this geom with an :ref:`engine plugin`. Either :at:`plugin` or :at:`instance` are required. + +.. _geom-plugin-plugin: + +:at:`plugin`: :at-val:`string, optional` + Plugin identifier, used for implicit plugin instantiation. + +.. _geom-plugin-instance: + +:at:`instance`: :at-val:`string, optional` + Instance name, used for explicit plugin instantiation. + .. _body-site: diff --git a/doc/XMLschema.rst b/doc/XMLschema.rst index e681670f..7f045807 100644 --- a/doc/XMLschema.rst +++ b/doc/XMLschema.rst @@ -167,6 +167,20 @@ | | | | :ref:`refpos` | :ref:`refquat` | :ref:`scale` | :ref:`smoothnormal` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ +| |_2| mesh |br| |_2| |L| | | .. table:: | +| :ref:`plugin | \* | :class: mjcf-attributes | +| ` | | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +| | | | :ref:`plugin` | :ref:`instance` | | | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | ++------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ +| |_3| plugin |br| |_3| |L| | | .. table:: | +| :ref:`config | \* | :class: mjcf-attributes | +| ` | | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +| | | | :ref:`key` | :ref:`value` | | | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | ++------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ | |_| asset |br| |_| |L| | | .. table:: | | :ref:`skin | \* | :class: mjcf-attributes | | ` | | | @@ -212,7 +226,7 @@ | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | | | | | :ref:`cone` | :ref:`jacobian` | :ref:`solver` | :ref:`iterations` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`noslip_iterations` | :ref:`mpr_iterations` | | | | +| | | | :ref:`noslip_iterations` | :ref:`mpr_iterations` | :ref:`sdf_iterations` | :ref:`sdf_initpoints` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ | |_| option |br| |_| |L| | | .. table:: | @@ -313,6 +327,20 @@ | | | | :ref:`user` | | | | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ +| |_2| geom |br| |_2| |L| | | .. table:: | +| :ref:`plugin | \* | :class: mjcf-attributes | +| ` | | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +| | | | :ref:`plugin` | :ref:`instance` | | | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | ++------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ +| |_3| plugin |br| |_3| |L| | | .. table:: | +| :ref:`config | \* | :class: mjcf-attributes | +| ` | | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +| | | | :ref:`key` | :ref:`value` | | | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | ++------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ | |_| body |br| |_| |L| | | .. table:: | | :ref:`site | \* | :class: mjcf-attributes | | ` | | | diff --git a/doc/changelog.rst b/doc/changelog.rst index fc2df359..27eac1f0 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -8,15 +8,26 @@ Upcoming version (not yet released) General ^^^^^^^ -.. youtube:: Vc1tq0fFvQA +.. youtube:: QewlEqIZi1o :align: right :width: 240px -1. Added constraint island discovery in :ref:`mj_island`. Constraint islands are disjoint sets of constraints +1. Added new signed distance field (SDF) collision primitive. SDFs can take any shape and are not constrained to be + convex. Collision points are found by minimizing the maximum of the two colliding SDFs via gradient descent. + + - Added new SDF plugin for defining implicit geometries. The plugin must define methods computing an SDF and its + gradient at query points See the :ref:`documentation` for more details. + - Increased ``mjMAXCONPAIR`` to 100. + + .. youtube:: Vc1tq0fFvQA + :align: right + :width: 240px + +#. Added constraint island discovery in :ref:`mj_island`. Constraint islands are disjoint sets of constraints and degrees-of-freedom that do not interact. In a future release the constraint solver will be refactored to exploit the disjoint structure. Island discovery can be activated using a new :ref:`enable flag` which will be removed after the refactor. If island discovery is enabled, geoms, contacts and - tendons will be colored according to the corresponding island, see video: + tendons will be colored according to the corresponding island, see video. #. Added a new :ref:`dyntype`, ``filterexact``, which updates first-order filter states with the exact formula rather than with Euler integration. #. Added an actuator attribute, :ref:`actearly`, which uses semi-implicit integration for diff --git a/doc/includes/references.h b/doc/includes/references.h index 6c70afe9..4cb9404e 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -416,6 +416,7 @@ typedef enum mjtGeom_ { // type of geometric shape mjGEOM_CYLINDER, // cylinder mjGEOM_BOX, // box mjGEOM_MESH, // mesh + mjGEOM_SDF, // signed distance field mjNGEOMTYPES, // number of regular geom types @@ -703,6 +704,10 @@ struct mjOption_ { // physics options int mpr_iterations; // maximum number of MPR solver iterations int disableflags; // bit flags for disabling standard features int enableflags; // bit flags for enabling optional features + + // sdf collision settings + int sdf_initpoints; // number of starting points for gradient descent + int sdf_iterations; // max number of iterations for gradient descent }; typedef struct mjOption_ mjOption; struct mjVisual_ { // visualization options @@ -969,6 +974,7 @@ struct mjModel_ { int* geom_matid; // material id for rendering; -1: none (ngeom x 1) int* geom_group; // group for visibility (ngeom x 1) int* geom_priority; // geom contact priority (ngeom x 1) + int* geom_plugin; // plugin instance id; -1: not in use (ngeom x 1) mjtByte* geom_sameframe; // same as body frame (1) or iframe (2) (ngeom x 1) mjtNum* geom_solmix; // mixing coef for solref/imp in geom pair (ngeom x 1) mjtNum* geom_solref; // constraint solver reference: contact (ngeom x mjNREF) @@ -1262,6 +1268,7 @@ typedef enum mjtPluginCapabilityBit_ { mjPLUGIN_ACTUATOR = 1<<0, // actuator forces mjPLUGIN_SENSOR = 1<<1, // sensor measurements mjPLUGIN_PASSIVE = 1<<2, // passive forces + mjPLUGIN_SDF = 1<<3, // signed distance fields } mjtPluginCapabilityBit; struct mjpPlugin_ { const char* name; // globally unique name identifying the plugin @@ -1298,6 +1305,20 @@ struct mjpPlugin_ { // called by mjv_updateScene (optional) void (*visualize)(const mjModel*m, mjData* d, const mjvOption* opt, mjvScene* scn, int instance); + + // methods specific to signed distance fields (optional) + + // signed distance from the surface + mjtNum (*sdf_distance)(const mjtNum point[3], const mjData* d, int instance); + + // gradient of distance with respect to local coordinates + void (*sdf_gradient)(mjtNum gradient[3], const mjtNum point[3], const mjData* d, int instance); + + // called during compilation for marching cubes + mjtNum (*sdf_staticdistance)(const mjtNum point[3], const mjtNum* attributes); + + // bounding box of implicit surface + void (*sdf_aabb)(mjtNum aabb[6], const mjtNum* attributes); }; typedef struct mjpPlugin_ mjpPlugin; typedef enum mjtGridPos_ { // grid position for overlay @@ -1719,6 +1740,7 @@ typedef enum mjtVisFlag_ { // flags enabling model element visualization mjVIS_SKIN, // skin mjVIS_MIDPHASE, // mid-phase bounding volume hierarchy mjVIS_MESHBVH, // mesh bounding volume hierarchy + mjVIS_SDFITER, // iterations of SDF gradient descent mjNVISFLAG // number of visualization flags } mjtVisFlag; diff --git a/doc/programming/extension.rst b/doc/programming/extension.rst index 043bfab5..e5e22846 100644 --- a/doc/programming/extension.rst +++ b/doc/programming/extension.rst @@ -84,6 +84,7 @@ supported plugin capabilities are: * Actuator plugin * Sensor plugin * Passive force plugin +* Signed distance field plugin Additional capabilities will be added in the future as required. @@ -234,7 +235,7 @@ provided for this scan-and-load use case. Writing plugins ^^^^^^^^^^^^^^^ -This section, targeted at developers, is not yet written. We encourage people who wish to write their own plugins +This section, targeted at developers, is incomplete. We encourage people who wish to write their own plugins to contact the MuJoCo development team for help. A good starting point for experienced developers is the `associated tests `_ and the first-party plugins in the `first-party plugin directory `_. @@ -247,6 +248,55 @@ A future version of this section will include: * Things that developers need to keep in mind in order to ensure that plugins function correctly when :ref:`mjData` is copied, stepped, or reset. +Currently, there are three directories of first-party plugins: + +* **elasticity:** The plugins in the `elasticity/ `__ + directory are passive forces based on continuum mechanics for 1-dimensional and 3-dimensional bodies. The + 1D model is invariant under rotations and captures the large deformation of elastic cables, decoupling twisting and + bending strains. The 3D solid is a + `Saint Venant-Kirchhoff `__ + model discretized with piecewise linear finite elements, which is suitable for large deformations with small strains. +* **sensor:** The plugins in the `sensor/ `__ + directory implement custom sensors. Currently the sole sensor plugin is the touch grid sensor, see the + `README `__ for details. +* **sdf:** The plugins in the `sdf/ `__ + directory specify custom shapes in a mesh-free manner, by defining methods computing a signed distance field and its + gradient at query points. This shape then acts as a new geom type in the collision table at the top of + `engine_collision_driver.c `__. + + Collision points are found by minimizing the maximum of the two colliding SDFs via gradient descent. + Because SDFs are non-convex, multiple starting points are required in order to converge to multiple local minima. + The number of starting points is set using :ref:`sdf_initpoints`, and are + initialized using the Halton sequence inside the intersection of the axis-aligned bounding boxes. + The number of gradient descent iterations is set using :ref:`sdf_iterations`. + + While *exact* SDFs---encoding the precise signed distance to the surface---are preferred, collisions are possible with + any function whose value vanishes at the surface and grows monotonically away from it, with a negative sign in the + interior. For such functions, it is still possible to find collisons, albeit with a possibly + increased number of starting points. + + The ``sdf_distance`` method is called by the compiler to produce a visual mesh for rendering using the marching cubes + algorithm implemented by `MarchingCubeCpp `__. + + Future improvement to the gradient descent algorithm, such as a line search which takes advantage of the properties of + SDFs, might reduce the number of iterations and/or starting points. + +For the sdf plugin, the following methods need to be specified + +``sdf_distance``: + Returns the signed distance of the query point given in local coordinates. + +``sdf_staticdistance``: + This is the static version of the previous function, taking config attributes as additional inputs. This function is + required because mesh creation occurs during model compilation before the plugin object has been instantiated. + +``sdf_gradient``: + Computes the gradient in local coodinates of the SDF at the query point. + +``sdf_aabb``: + Computes the axis-aligned bounding box in local coordinates. This volume is voxelized uniformly before the call to + the marching cubes algorithm. + .. _exProvider: Resource providers diff --git a/include/mujoco/mjmodel.h b/include/mujoco/mjmodel.h index 6298edf6..a9bd828c 100644 --- a/include/mujoco/mjmodel.h +++ b/include/mujoco/mjmodel.h @@ -23,7 +23,7 @@ #define mjMINMU 1E-5 // minimum friction coefficient #define mjMINIMP 0.0001 // minimum constraint impedance #define mjMAXIMP 0.9999 // maximum constraint impedance -#define mjMAXCONPAIR 50 // maximum number of contacts per geom pair +#define mjMAXCONPAIR 100 // maximum number of contacts per geom pair #define mjMAXTREEDEPTH 50 // maximum bounding volume hierarchy depth #define mjMAXVFS 2000 // maximum number of files in virtual file system #define mjMAXVFSNAME 1000 // maximum filename size in virtual file system @@ -94,6 +94,7 @@ typedef enum mjtGeom_ { // type of geometric shape mjGEOM_CYLINDER, // cylinder mjGEOM_BOX, // box mjGEOM_MESH, // mesh + mjGEOM_SDF, // signed distance field mjNGEOMTYPES, // number of regular geom types @@ -437,6 +438,10 @@ struct mjOption_ { // physics options int mpr_iterations; // maximum number of MPR solver iterations int disableflags; // bit flags for disabling standard features int enableflags; // bit flags for enabling optional features + + // sdf collision settings + int sdf_initpoints; // number of starting points for gradient descent + int sdf_iterations; // max number of iterations for gradient descent }; typedef struct mjOption_ mjOption; @@ -715,6 +720,7 @@ struct mjModel_ { int* geom_matid; // material id for rendering; -1: none (ngeom x 1) int* geom_group; // group for visibility (ngeom x 1) int* geom_priority; // geom contact priority (ngeom x 1) + int* geom_plugin; // plugin instance id; -1: not in use (ngeom x 1) mjtByte* geom_sameframe; // same as body frame (1) or iframe (2) (ngeom x 1) mjtNum* geom_solmix; // mixing coef for solref/imp in geom pair (ngeom x 1) mjtNum* geom_solref; // constraint solver reference: contact (ngeom x mjNREF) diff --git a/include/mujoco/mjplugin.h b/include/mujoco/mjplugin.h index 11acf84f..a6936e20 100644 --- a/include/mujoco/mjplugin.h +++ b/include/mujoco/mjplugin.h @@ -56,6 +56,7 @@ typedef enum mjtPluginCapabilityBit_ { mjPLUGIN_ACTUATOR = 1<<0, // actuator forces mjPLUGIN_SENSOR = 1<<1, // sensor measurements mjPLUGIN_PASSIVE = 1<<2, // passive forces + mjPLUGIN_SDF = 1<<3, // signed distance fields } mjtPluginCapabilityBit; struct mjpPlugin_ { @@ -93,6 +94,20 @@ struct mjpPlugin_ { // called by mjv_updateScene (optional) void (*visualize)(const mjModel*m, mjData* d, const mjvOption* opt, mjvScene* scn, int instance); + + // methods specific to signed distance fields (optional) + + // signed distance from the surface + mjtNum (*sdf_distance)(const mjtNum point[3], const mjData* d, int instance); + + // gradient of distance with respect to local coordinates + void (*sdf_gradient)(mjtNum gradient[3], const mjtNum point[3], const mjData* d, int instance); + + // called during compilation for marching cubes + mjtNum (*sdf_staticdistance)(const mjtNum point[3], const mjtNum* attributes); + + // bounding box of implicit surface + void (*sdf_aabb)(mjtNum aabb[6], const mjtNum* attributes); }; typedef struct mjpPlugin_ mjpPlugin; diff --git a/include/mujoco/mjvisualize.h b/include/mujoco/mjvisualize.h index e4e7f5c8..f4aee119 100644 --- a/include/mujoco/mjvisualize.h +++ b/include/mujoco/mjvisualize.h @@ -126,6 +126,7 @@ typedef enum mjtVisFlag_ { // flags enabling model element visualization mjVIS_SKIN, // skin mjVIS_MIDPHASE, // mid-phase bounding volume hierarchy mjVIS_MESHBVH, // mesh bounding volume hierarchy + mjVIS_SDFITER, // iterations of SDF gradient descent mjNVISFLAG // number of visualization flags } mjtVisFlag; diff --git a/include/mujoco/mjxmacro.h b/include/mujoco/mjxmacro.h index 4d622ada..32eebff4 100644 --- a/include/mujoco/mjxmacro.h +++ b/include/mujoco/mjxmacro.h @@ -41,7 +41,9 @@ X( int, noslip_iterations ) \ X( int, mpr_iterations ) \ X( int, disableflags ) \ - X( int, enableflags ) + X( int, enableflags ) \ + X( int, sdf_initpoints ) \ + X( int, sdf_iterations ) #define MJOPTION_SCALARS \ @@ -225,6 +227,7 @@ XMJV( int, geom_matid, ngeom, 1 ) \ XMJV( int, geom_group, ngeom, 1 ) \ X ( int, geom_priority, ngeom, 1 ) \ + X ( int, geom_plugin, ngeom, 1 ) \ X ( mjtByte, geom_sameframe, ngeom, 1 ) \ X ( mjtNum, geom_solmix, ngeom, 1 ) \ X ( mjtNum, geom_solref, ngeom, mjNREF ) \ diff --git a/introspect/enums.py b/introspect/enums.py index f2d99d5d..2042d428 100644 --- a/introspect/enums.py +++ b/introspect/enums.py @@ -82,7 +82,8 @@ ENUMS: Mapping[str, EnumDecl] = dict([ ('mjGEOM_CYLINDER', 5), ('mjGEOM_BOX', 6), ('mjGEOM_MESH', 7), - ('mjNGEOMTYPES', 8), + ('mjGEOM_SDF', 8), + ('mjNGEOMTYPES', 9), ('mjGEOM_ARROW', 100), ('mjGEOM_ARROW1', 101), ('mjGEOM_ARROW2', 102), @@ -553,7 +554,8 @@ ENUMS: Mapping[str, EnumDecl] = dict([ ('mjVIS_SKIN', 23), ('mjVIS_MIDPHASE', 24), ('mjVIS_MESHBVH', 25), - ('mjNVISFLAG', 26), + ('mjVIS_SDFITER', 26), + ('mjNVISFLAG', 27), ]), )), ('mjtRndFlag', @@ -592,6 +594,7 @@ ENUMS: Mapping[str, EnumDecl] = dict([ ('mjPLUGIN_ACTUATOR', 1), ('mjPLUGIN_SENSOR', 2), ('mjPLUGIN_PASSIVE', 4), + ('mjPLUGIN_SDF', 8), ]), )), ('mjtGridPos', diff --git a/introspect/structs.py b/introspect/structs.py index 60ad5f1c..24a6ead9 100644 --- a/introspect/structs.py +++ b/introspect/structs.py @@ -263,6 +263,16 @@ STRUCTS: Mapping[str, StructDecl] = dict([ type=ValueType(name='int'), doc='bit flags for enabling optional features', ), + StructFieldDecl( + name='sdf_initpoints', + type=ValueType(name='int'), + doc='number of starting points for gradient descent', + ), + StructFieldDecl( + name='sdf_iterations', + type=ValueType(name='int'), + doc='max number of iterations for gradient descent', + ), ), )), ('mjVisual', @@ -1627,6 +1637,13 @@ STRUCTS: Mapping[str, StructDecl] = dict([ ), doc='geom contact priority (ngeom x 1)', ), + StructFieldDecl( + name='geom_plugin', + type=PointerType( + inner_type=ValueType(name='int'), + ), + doc='plugin instance id; -1: not in use (ngeom x 1)', + ), StructFieldDecl( name='geom_sameframe', type=PointerType( @@ -4874,7 +4891,7 @@ STRUCTS: Mapping[str, StructDecl] = dict([ name='flags', type=ArrayType( inner_type=ValueType(name='mjtByte'), - extents=(26,), + extents=(27,), ), doc='visualization flags (indexed by mjtVisFlag)', ), diff --git a/model/plugin/sdf/bowl.xml b/model/plugin/sdf/bowl.xml new file mode 100644 index 00000000..a50a3c3e --- /dev/null +++ b/model/plugin/sdf/bowl.xml @@ -0,0 +1,50 @@ + + + + + + + + + + + + + + + + + + + diff --git a/model/plugin/sdf/gear.xml b/model/plugin/sdf/gear.xml new file mode 100644 index 00000000..e5e220d9 --- /dev/null +++ b/model/plugin/sdf/gear.xml @@ -0,0 +1,55 @@ + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/model/plugin/sdf/mug.xml b/model/plugin/sdf/mug.xml new file mode 100644 index 00000000..11fcc891 --- /dev/null +++ b/model/plugin/sdf/mug.xml @@ -0,0 +1,35 @@ + + + + + + + + + + + + diff --git a/model/plugin/sdf/nutbolt.xml b/model/plugin/sdf/nutbolt.xml new file mode 100644 index 00000000..d3d80ff7 --- /dev/null +++ b/model/plugin/sdf/nutbolt.xml @@ -0,0 +1,56 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/model/plugin/sdf/primitives.xml b/model/plugin/sdf/primitives.xml new file mode 100644 index 00000000..f7baf63a --- /dev/null +++ b/model/plugin/sdf/primitives.xml @@ -0,0 +1,68 @@ + + + + + + + + + + + + + + + + + + diff --git a/model/plugin/sdf/scene.xml b/model/plugin/sdf/scene.xml new file mode 100644 index 00000000..1591519e --- /dev/null +++ b/model/plugin/sdf/scene.xml @@ -0,0 +1,38 @@ + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/model/plugin/sdf/torus.xml b/model/plugin/sdf/torus.xml new file mode 100644 index 00000000..e482e434 --- /dev/null +++ b/model/plugin/sdf/torus.xml @@ -0,0 +1,54 @@ + + + + + + + + + + + + + + + + + + diff --git a/plugin/sdf/CMakeLists.txt b/plugin/sdf/CMakeLists.txt new file mode 100644 index 00000000..a87cd067 --- /dev/null +++ b/plugin/sdf/CMakeLists.txt @@ -0,0 +1,51 @@ +# Copyright 2022 DeepMind Technologies Limited +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# https://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +set(MUJOCO_SDF_INCLUDE ${CMAKE_CURRENT_SOURCE_DIR}/../.. + ${CMAKE_CURRENT_SOURCE_DIR}/../../src +) + +set(MUJOCO_SDF_SRCS + sdf.cc + sdf.h + bolt.cc + bolt.h + bowl.cc + bowl.h + gear.cc + gear.h + register.cc + nut.cc + nut.h + torus.cc + torus.h +) + +add_library(sdf SHARED) +target_sources(sdf PRIVATE ${MUJOCO_SDF_SRCS}) +target_include_directories(sdf PRIVATE ${MUJOCO_SDF_INCLUDE}) +target_link_libraries(sdf PRIVATE mujoco) +target_compile_options( + sdf + PRIVATE ${AVX_COMPILE_OPTIONS} + ${MUJOCO_MACOS_COMPILE_OPTIONS} + ${EXTRA_COMPILE_OPTIONS} + ${MUJOCO_CXX_FLAGS} +) +target_link_options( + sdf + PRIVATE + ${MUJOCO_MACOS_LINK_OPTIONS} + ${EXTRA_LINK_OPTIONS} +) diff --git a/plugin/sdf/bolt.cc b/plugin/sdf/bolt.cc new file mode 100644 index 00000000..39cf9ec0 --- /dev/null +++ b/plugin/sdf/bolt.cc @@ -0,0 +1,184 @@ +// Copyright 2022 DeepMind Technologies Limited +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include + +#include +#include +#include +#include "sdf.h" +#include "bolt.h" + +namespace mujoco::plugin::sdf { +namespace { + +static mjtNum distance(const mjtNum p[3], const mjtNum attributes[1]) { + // see https://www.shadertoy.com/view/XtffzX + mjtNum screw = 12; + mjtNum radius = mju_sqrt(p[0]*p[0]+p[1]*p[1]) - attributes[0]; + mjtNum sqrt12 = mju_sqrt(2.)/2.; + + // a triangle wave spun around Oy, offset by the angle between x and z + mjtNum azimuth = mju_atan2(p[1], p[0]); + mjtNum triangle = abs(Fract(p[2] * screw - azimuth / mjPI / 2.) - .5); + mjtNum thread = (radius - triangle / screw) * sqrt12; + + // clip the top and bottom + mjtNum bolt = Subtraction(thread, .5 - abs(p[2] + .5)); + mjtNum cone = (p[2] - radius) * sqrt12; + + // add a diagonal clipping for more realism + bolt = Subtraction(bolt, cone + 1. * sqrt12); + + // create the hexagonal geometry for the head + mjtNum point2D[2] = {p[0], p[1]}; + mjtNum res[2]; + mjtNum k = 6. / mjPI / 2.; + mjtNum angle = -floor((mju_atan2(point2D[1], point2D[0])) * k + .5) / k; + mjtNum s[2] = {mju_sin(angle), mju_sin(angle + mjPI * .5)}; + mjtNum mat[4] = {s[1], -s[0], s[0], s[1]}; + mju_mulMatVec(res, mat, point2D, 2, 2); + mjtNum point3D[3] = {res[0], res[1], p[2]}; + mjtNum head = point3D[0] - .5; + + // the top is also rounded down with a cone + head = Intersection(head, abs(point3D[2] + .25) - .25); + head = Intersection(head, (point3D[2] + radius - .22) * sqrt12); + return Union(bolt, head); +} + +} // namespace + +// factory function +std::optional Bolt::Create( + const mjModel* m, mjData* d, int instance) { + if (CheckAttr("radius", m, instance)) { + return Bolt(m, d, instance); + } else { + mju_warning("Invalid parameter specification in Bolt plugin"); + return std::nullopt; + } +} + +// plugin constructor +Bolt::Bolt(const mjModel* m, mjData* d, int instance) { + radius = strtod(mj_getPluginConfig(m, instance, "radius"), nullptr); +} + +// add new element in the vector storing iteration counts +void Bolt::Compute(const mjModel* m, mjData* d, int instance) { + visualizer_.Next(); +} + +// reset visualization counter +void Bolt::Reset() { + visualizer_.Reset(); +} + +// plugin visualization +void Bolt::Visualize(const mjModel* m, mjData* d, const mjvOption* opt, + mjvScene* scn, int instance) { + visualizer_.Visualize(m, d, opt, scn, instance); +} + +// sdf +mjtNum Bolt::Distance(const mjtNum point[3]) const { + return distance(point, &radius); +} + +// gradient of sdf +void Bolt::Gradient(mjtNum grad[3], const mjtNum point[3]) const { + mjtNum eps = 1e-8; + mjtNum dist0 = distance(point, &radius); + mjtNum pointX[3] = {point[0]+eps, point[1], point[2]}; + mjtNum distX = distance(pointX, &radius); + mjtNum pointY[3] = {point[0], point[1]+eps, point[2]}; + mjtNum distY = distance(pointY, &radius); + mjtNum pointZ[3] = {point[0], point[1], point[2]+eps}; + mjtNum distZ = distance(pointZ, &radius); + + grad[0] = (distX - dist0) / eps; + grad[1] = (distY - dist0) / eps; + grad[2] = (distZ - dist0) / eps; +} + +// plugin registration +void Bolt::RegisterPlugin() { + mjpPlugin plugin; + mjp_defaultPlugin(&plugin); + + plugin.name = "mujoco.sdf.bolt"; + plugin.capabilityflags |= mjPLUGIN_SDF; + + const char* attributes[] = {"radius"}; + plugin.nattribute = sizeof(attributes) / sizeof(attributes[0]); + plugin.attributes = attributes; + plugin.nstate = +[](const mjModel* m, int instance) { return 0; }; + + plugin.init = +[](const mjModel* m, mjData* d, int instance) { + auto sdf_or_null = Bolt::Create(m, d, instance); + if (!sdf_or_null.has_value()) { + return -1; + } + d->plugin_data[instance] = reinterpret_cast( + new Bolt(std::move(*sdf_or_null))); + return 0; + }; + plugin.destroy = +[](mjData* d, int instance) { + delete reinterpret_cast(d->plugin_data[instance]); + d->plugin_data[instance] = 0; + }; + plugin.reset = +[](const mjModel* m, double* plugin_state, void* plugin_data, + int instance) { + auto sdf = reinterpret_cast(plugin_data); + sdf->Reset(); + }; + plugin.visualize = +[](const mjModel* m, mjData* d, const mjvOption* opt, + mjvScene* scn, int instance) { + auto* sdf = reinterpret_cast(d->plugin_data[instance]); + sdf->Visualize(m, d, opt, scn, instance); + }; + plugin.compute = + +[](const mjModel* m, mjData* d, int instance, int capability_bit) { + auto* sdf = reinterpret_cast(d->plugin_data[instance]); + sdf->Compute(m, d, instance); + }; + plugin.sdf_distance = + +[](const mjtNum point[3], const mjData* d, int instance) { + auto* sdf = reinterpret_cast(d->plugin_data[instance]); + sdf->visualizer_.AddPoint(point); + return sdf->Distance(point); + }; + plugin.sdf_gradient = +[](mjtNum gradient[3], const mjtNum point[3], + const mjData* d, int instance) { + auto* sdf = reinterpret_cast(d->plugin_data[instance]); + sdf->Gradient(gradient, point); + }; + plugin.sdf_staticdistance = + +[](const mjtNum point[3], const mjtNum* attributes) { + return distance(point, attributes); + }; + plugin.sdf_aabb = + +[](mjtNum aabb[6], const mjtNum* attributes) { + aabb[0] = aabb[1] = aabb[2] = 0; + aabb[3] = aabb[4] = .6; + aabb[5] = 1; + }; + + mjp_registerPlugin(&plugin); +} + +} // namespace mujoco::plugin::sdf diff --git a/plugin/sdf/bolt.h b/plugin/sdf/bolt.h new file mode 100644 index 00000000..37a5fa5b --- /dev/null +++ b/plugin/sdf/bolt.h @@ -0,0 +1,58 @@ +// Copyright 2022 DeepMind Technologies Limited +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef MUJOCO_PLUGIN_SDF_BOLT_H_ +#define MUJOCO_PLUGIN_SDF_BOLT_H_ + +#include + +#include +#include +#include +#include +#include "sdf.h" + +namespace mujoco::plugin::sdf { + +// this plugin implements a modification of the signed distance function +// from https://www.shadertoy.com/view/XtffzX of a bolt with a hexagonal head + +class Bolt { + public: + // Creates a new Bolt instance (allocated with `new`) or + // returns null on failure. + static std::optional Create(const mjModel* m, mjData* d, int instance); + Bolt(Bolt&&) = default; + ~Bolt() = default; + + void Reset(); + void Visualize(const mjModel* m, mjData* d, const mjvOption* opt, + mjvScene* scn, int instance); + void Compute(const mjModel* m, mjData* d, int instance); + mjtNum Distance(const mjtNum point[3]) const; + void Gradient(mjtNum grad[3], const mjtNum point[3]) const; + + static void RegisterPlugin(); + + mjtNum radius; + + private: + Bolt(const mjModel* m, mjData* d, int instance); + + SdfVisualizer visualizer_; +}; + +} // namespace mujoco::plugin::sdf + +#endif // MUJOCO_PLUGIN_SDF_BOLT_H_ diff --git a/plugin/sdf/bowl.cc b/plugin/sdf/bowl.cc new file mode 100644 index 00000000..f5130527 --- /dev/null +++ b/plugin/sdf/bowl.cc @@ -0,0 +1,184 @@ +// Copyright 2022 DeepMind Technologies Limited +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include +#include +#include +#include "bowl.h" + +namespace mujoco::plugin::sdf { +namespace { + +static mjtNum distance(const mjtNum point[3], const mjtNum attributes[3]) { + mjtNum height = attributes[0]; + mjtNum radius = attributes[1]; + mjtNum thick = attributes[2]; + mjtNum width = mju_sqrt(radius*radius - height*height); + // see https://iquilezles.org/articles/distfunctions/ + mjtNum q[2] = { mju_norm(point, 2), point[2] }; + mjtNum qdiff[2] = { q[0] - width, q[1] - height }; + return ((height*q[0] < width*q[1]) ? mju_norm(qdiff, 2) + : mju_abs(mju_norm(q, 2)-radius))-thick; +} + +} // namespace + +// factory function +std::optional Bowl::Create( + const mjModel* m, mjData* d, int instance) { + if (CheckAttr("radius", m, instance) && CheckAttr("height", m, instance) && + CheckAttr("thickness", m, instance)) { + return Bowl(m, d, instance); + } else { + mju_warning("Invalid parameter specification in Bowl plugin"); + return std::nullopt; + } +} + +// plugin constructor +Bowl::Bowl(const mjModel* m, mjData* d, int instance) { + radius = strtod(mj_getPluginConfig(m, instance, "radius"), nullptr); + height = strtod(mj_getPluginConfig(m, instance, "height"), nullptr); + thick = strtod(mj_getPluginConfig(m, instance, "thickness"), nullptr); + width = mju_sqrt(radius*radius - height*height); +} + +// add new element in the vector storing iteration counts +void Bowl::Compute(const mjModel* m, mjData* d, int instance) { + visualizer_.Next(); +} + +// reset visualization counter +void Bowl::Reset() { + visualizer_.Reset(); +} + +// plugin visualization +void Bowl::Visualize(const mjModel* m, mjData* d, const mjvOption* opt, mjvScene* scn, + int instance) { + visualizer_.Visualize(m, d, opt, scn, instance); +} + +// sdf +mjtNum Bowl::Distance(const mjtNum point[3]) const { + mjtNum attributes[3]= {height, radius, thick}; + return distance(point, attributes); +} + +// gradient of sdf +void Bowl::Gradient(mjtNum grad[3], const mjtNum point[3]) const { + // mjtNum q[2] = { mju_norm(point, 2), point[2] }; + // if (height*q[0] < width*q[1]) { + // mjtNum qdiff[2] = { q[0] - width, q[1] - height }; + // mjtNum qdiffnorm = mju_norm(qdiff, 2); + // mjtNum grad_qdiff[3] = {qdiff[0] * point[0] / q[0], + // qdiff[0] * point[1] / q[0], + // qdiff[1]}; + // grad[0] = - grad_qdiff[0] / qdiffnorm; + // grad[1] = - grad_qdiff[1] / qdiffnorm; + // grad[2] = - grad_qdiff[2] / qdiffnorm; + // } else { + // mjtNum pnorm = mju_norm3(point); + // mjtNum grad_dist = (pnorm - radius) / mju_abs(pnorm - radius); + // grad[0] = - grad_dist * point[0] / pnorm; + // grad[1] = - grad_dist * point[1] / pnorm; + // grad[2] = - grad_dist * point[2] / pnorm; + // } + mjtNum attributes[3]= {height, radius, thick}; + mjtNum eps = 1e-8; + mjtNum dist0 = distance(point, attributes); + + mjtNum pointX[3] = {point[0]+eps, point[1], point[2]}; + mjtNum distX = distance(pointX, attributes); + mjtNum pointY[3] = {point[0], point[1]+eps, point[2]}; + mjtNum distY = distance(pointY, attributes); + mjtNum pointZ[3] = {point[0], point[1], point[2]+eps}; + mjtNum distZ = distance(pointZ, attributes); + + grad[0] = (distX - dist0) / eps; + grad[1] = (distY - dist0) / eps; + grad[2] = (distZ - dist0) / eps; +} + +// plugin registration +void Bowl::RegisterPlugin() { + mjpPlugin plugin; + mjp_defaultPlugin(&plugin); + + plugin.name = "mujoco.sdf.bowl"; + plugin.capabilityflags |= mjPLUGIN_SDF; + + const char* attributes[] = {"radius", "height", "thickness"}; + plugin.nattribute = sizeof(attributes) / sizeof(attributes[0]); + plugin.attributes = attributes; + plugin.nstate = +[](const mjModel* m, int instance) { return 0; }; + + plugin.init = +[](const mjModel* m, mjData* d, int instance) { + auto sdf_or_null = Bowl::Create(m, d, instance); + if (!sdf_or_null.has_value()) { + return -1; + } + d->plugin_data[instance] = reinterpret_cast( + new Bowl(std::move(*sdf_or_null))); + return 0; + }; + plugin.destroy = +[](mjData* d, int instance) { + delete reinterpret_cast(d->plugin_data[instance]); + d->plugin_data[instance] = 0; + }; + plugin.reset = +[](const mjModel* m, double* plugin_state, void* plugin_data, + int instance) { + auto sdf = reinterpret_cast(plugin_data); + sdf->Reset(); + }; + plugin.visualize = +[](const mjModel* m, mjData* d, const mjvOption* opt, + mjvScene* scn, int instance) { + auto* sdf = reinterpret_cast(d->plugin_data[instance]); + sdf->Visualize(m, d, opt, scn, instance); + }; + plugin.compute = + +[](const mjModel* m, mjData* d, int instance, int capability_bit) { + auto* sdf = reinterpret_cast(d->plugin_data[instance]); + sdf->Compute(m, d, instance); + }; + plugin.sdf_distance = + +[](const mjtNum point[3], const mjData* d, int instance) { + auto* sdf = reinterpret_cast(d->plugin_data[instance]); + sdf->visualizer_.AddPoint(point); + return sdf->Distance(point); + }; + plugin.sdf_gradient = +[](mjtNum gradient[3], const mjtNum point[3], + const mjData* d, int instance) { + auto* sdf = reinterpret_cast(d->plugin_data[instance]); + sdf->Gradient(gradient, point); + }; + plugin.sdf_staticdistance = + +[](const mjtNum point[3], const mjtNum* attributes) { + return distance(point, attributes); + }; + plugin.sdf_aabb = + +[](mjtNum aabb[6], const mjtNum* attributes) { + mjtNum radius = attributes[1]; + mjtNum thick = attributes[2]; + aabb[0] = aabb[1] = aabb[2] = 0; + aabb[3] = aabb[4] = aabb[5] = radius + thick; + }; + + mjp_registerPlugin(&plugin); +} + +} // namespace mujoco::plugin::sdf diff --git a/plugin/sdf/bowl.h b/plugin/sdf/bowl.h new file mode 100644 index 00000000..66210885 --- /dev/null +++ b/plugin/sdf/bowl.h @@ -0,0 +1,58 @@ +// Copyright 2022 DeepMind Technologies Limited +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef MUJOCO_PLUGIN_SDF_BOWL_H_ +#define MUJOCO_PLUGIN_SDF_BOWL_H_ + +#include + +#include +#include +#include +#include +#include "sdf.h" + +namespace mujoco::plugin::sdf { + +class Bowl { + public: + // Creates a new Bowl instance (allocated with `new`) or + // returns null on failure. + static std::optional Create(const mjModel* m, mjData* d, int instance); + Bowl(Bowl&&) = default; + ~Bowl() = default; + + void Reset(); + void Visualize(const mjModel* m, mjData* d, const mjvOption* opt, + mjvScene* scn, int instance); + void Compute(const mjModel* m, mjData* d, int instance); + mjtNum Distance(const mjtNum point[3]) const; + void Gradient(mjtNum grad[3], const mjtNum point[3]) const; + + static void RegisterPlugin(); + + mjtNum radius; + mjtNum height; + mjtNum thick; + mjtNum width; + + private: + Bowl(const mjModel* m, mjData* d, int instance); + + SdfVisualizer visualizer_; +}; + +} // namespace mujoco::plugin::sdf + +#endif // MUJOCO_PLUGIN_SDF_BOWL_H_ diff --git a/plugin/sdf/gear.cc b/plugin/sdf/gear.cc new file mode 100644 index 00000000..b9f42b83 --- /dev/null +++ b/plugin/sdf/gear.cc @@ -0,0 +1,263 @@ +// Copyright 2022 DeepMind Technologies Limited +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include + +#include +#include +#include +#include +#include "gear.h" + +namespace mujoco::plugin::sdf { +namespace { + +static mjtNum circle(mjtNum rho, mjtNum r) { + return rho - r; +} + +static mjtNum smoothUnion(mjtNum a, mjtNum b, mjtNum k) { + mjtNum h = mju_clip(0.5 + 0.5*(b - a) / k, 0.0, 1.0); + return b * (1. - h) + a * h - k * h * (1. - h); +} + +static mjtNum smoothIntersection(mjtNum a, mjtNum b, mjtNum k) { + return Subtraction( + Intersection(a, b), + smoothUnion(Subtraction(a, b), Subtraction(b, a), k)); +} + +static mjtNum extrusion(const mjtNum p[3], mjtNum sdf_2d, mjtNum h) { + mjtNum w[2] = { sdf_2d, abs(p[2]) - h }; + mjtNum w_abs[2] = { mju_max(w[0], 0), mju_max(w[1], 0) }; + return mju_min(mju_max(w[0], w[1]), 0.) + mju_norm(w_abs, 2); +} + +static mjtNum mod(mjtNum x, mjtNum y) { + return x - y * floor(x/y); +} + +static mjtNum distance2D(const mjtNum p[3], const mjtNum attributes[3]) { + // see https://www.shadertoy.com/view/3lG3WR + mjtNum D = 2.8; // should be an attribute + mjtNum N = 25; // should be an attribute + mjtNum psi = 3.096e-5 * N * N -6.557e-3 * N + 0.551; // pressure angle + mjtNum alpha = attributes[0]; + + mjtNum R = D / 2.0; + /* The Pitch Circle Diameter is the diameter of a circle which by a pure + * rolling action would transmit the same motion as the actual gear wheel. It + * should be noted that in the case of wheels which connect non-parallel + * shafts, the pitch circle diameter is different for each cross section of + * the wheel normal to the axis of rotation. + */ + + mjtNum rho = mju_norm(p, 2); + mjtNum Pd = N / D; // Diametral Pitch: teeth per unit length of diameter + mjtNum P = + mjPI / Pd; // Circular Pitch: the length of arc round the pitch circle + // between corresponding points on adjacent teeth. + mjtNum a = 1.0 / Pd; // Addendum: radial length of a tooth from the pitch + // circle to the tip of the tooth. + + mjtNum Do = D + 2.0 * a; // Outside Diameter + mjtNum Ro = Do / 2.0; + + mjtNum h = 2.2 / Pd; + + mjtNum innerR = Ro - h - 0.4; + + // Early exit + if (innerR - rho > 0.0) + return innerR - rho; + + // Early exit + if (Ro - rho < -0.2) + return rho - Ro; + + mjtNum Db = D * mju_cos(psi); // Base Diameter + mjtNum Rb = Db / 2.0; + + mjtNum fi = mju_atan2(p[1], p[0]) + alpha; + mjtNum alphaStride = P / R; + + mjtNum invAlpha = mju_acos(Rb / R); + mjtNum invPhi = mju_tan(invAlpha) - invAlpha; + + mjtNum shift = alphaStride / 2.0 - 2.0 * invPhi; + + mjtNum fia = mod(fi + shift / 2.0, alphaStride) - shift / 2.0; + mjtNum fib = mod(-fi - shift + shift / 2.0, alphaStride) - shift / 2.0; + + mjtNum dista = -1.0e6; + mjtNum distb = -1.0e6; + + if (Rb < rho) { + mjtNum acos_rbRho = mju_acos(Rb/rho); + + mjtNum thetaa = fia + acos_rbRho; + mjtNum thetab = fib + acos_rbRho; + + mjtNum ta = mju_sqrt(rho * rho - Rb * Rb); + + // https://math.stackexchange.com/questions/1266689/distance-from-a-point-to-the-involute-of-a-circle + dista = ta - Rb * thetaa; + distb = ta - Rb * thetab; + } + + mjtNum gearOuter = circle(rho, Ro); + mjtNum gearLowBase = circle(rho, Ro - h); + mjtNum crownBase = circle(rho, innerR); + mjtNum cogs = Intersection(dista, distb); + mjtNum baseWalls = Intersection(fia - (alphaStride - shift), + fib - (alphaStride - shift)); + + cogs = Intersection(baseWalls, cogs); + cogs = smoothIntersection(gearOuter, cogs, 0.01); + cogs = smoothUnion(gearLowBase, cogs, Rb - Ro + h); + cogs = Subtraction(cogs, crownBase); + + return extrusion(p, cogs, .1); +} + +static mjtNum distance(const mjtNum p[3], const mjtNum attributes[3]) { + return extrusion(p, distance2D(p, attributes), .1) - .005; +} + +} // namespace + +// factory function +std::optional Gear::Create( + const mjModel* m, mjData* d, int instance) { + if (CheckAttr("alpha", m, instance)) { + return Gear(m, d, instance); + } else { + mju_warning("Invalid parameter specification in Gear plugin"); + return std::nullopt; + } +} + +// plugin constructor +Gear::Gear(const mjModel* m, mjData* d, int instance) { + alpha = strtod(mj_getPluginConfig(m, instance, "alpha"), nullptr); +} + +// plugin computation +void Gear::Compute(const mjModel* m, mjData* d, int instance) { + visualizer_.Next(); +} + +// plugin reset +void Gear::Reset() { + visualizer_.Reset(); +} + +// plugin visualization +void Gear::Visualize(const mjModel* m, mjData* d, const mjvOption* opt, + mjvScene* scn, int instance) { + visualizer_.Visualize(m, d, opt, scn, instance); +} + +// sdf +mjtNum Gear::Distance(const mjtNum point[3]) const { + return distance(point, &alpha); +} + +// gradient of sdf +void Gear::Gradient(mjtNum grad[3], const mjtNum point[3]) const { + mjtNum attributes[1]= {alpha}; + mjtNum eps = 1e-8; + mjtNum dist0 = distance(point, attributes); + + mjtNum pointX[3] = {point[0]+eps, point[1], point[2]}; + mjtNum distX = distance(pointX, attributes); + mjtNum pointY[3] = {point[0], point[1]+eps, point[2]}; + mjtNum distY = distance(pointY, attributes); + mjtNum pointZ[3] = {point[0], point[1], point[2]+eps}; + mjtNum distZ = distance(pointZ, attributes); + + grad[0] = (distX - dist0) / eps; + grad[1] = (distY - dist0) / eps; + grad[2] = (distZ - dist0) / eps; +} + +// plugin registration +void Gear::RegisterPlugin() { + mjpPlugin plugin; + mjp_defaultPlugin(&plugin); + + plugin.name = "mujoco.sdf.gear"; + plugin.capabilityflags |= mjPLUGIN_SDF; + + const char* attributes[] = {"alpha"}; + plugin.nattribute = sizeof(attributes) / sizeof(attributes[0]); + plugin.attributes = attributes; + plugin.nstate = +[](const mjModel* m, int instance) { return 0; }; + + plugin.init = +[](const mjModel* m, mjData* d, int instance) { + auto sdf_or_null = Gear::Create(m, d, instance); + if (!sdf_or_null.has_value()) { + return -1; + } + d->plugin_data[instance] = reinterpret_cast( + new Gear(std::move(*sdf_or_null))); + return 0; + }; + plugin.destroy = +[](mjData* d, int instance) { + delete reinterpret_cast(d->plugin_data[instance]); + d->plugin_data[instance] = 0; + }; + plugin.reset = +[](const mjModel* m, double* plugin_state, void* plugin_data, + int instance) { + auto sdf = reinterpret_cast(plugin_data); + sdf->Reset(); + }; + plugin.visualize = +[](const mjModel* m, mjData* d, const mjvOption* opt, + mjvScene* scn, int instance) { + auto* sdf = reinterpret_cast(d->plugin_data[instance]); + sdf->Visualize(m, d, opt, scn, instance); + }; + plugin.compute = + +[](const mjModel* m, mjData* d, int instance, int capability_bit) { + auto* sdf = reinterpret_cast(d->plugin_data[instance]); + sdf->Compute(m, d, instance); + }; + plugin.sdf_distance = + +[](const mjtNum point[3], const mjData* d, int instance) { + auto* sdf = reinterpret_cast(d->plugin_data[instance]); + sdf->visualizer_.AddPoint(point); + return sdf->Distance(point); + }; + plugin.sdf_gradient = +[](mjtNum gradient[3], const mjtNum point[3], + const mjData* d, int instance) { + auto* sdf = reinterpret_cast(d->plugin_data[instance]); + sdf->Gradient(gradient, point); + }; + plugin.sdf_staticdistance = + +[](const mjtNum point[3], const mjtNum* attributes) { + return distance(point, attributes); + }; + plugin.sdf_aabb = + +[](mjtNum aabb[6], const mjtNum* attributes) { + aabb[0] = aabb[1] = aabb[2] = 0; + aabb[3] = aabb[4] = 1.7; + aabb[5] = .11; + }; + + mjp_registerPlugin(&plugin); +} + +} // namespace mujoco::plugin::sdf diff --git a/plugin/sdf/gear.h b/plugin/sdf/gear.h new file mode 100644 index 00000000..400c8d6e --- /dev/null +++ b/plugin/sdf/gear.h @@ -0,0 +1,56 @@ +// Copyright 2022 DeepMind Technologies Limited +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef MUJOCO_PLUGIN_SDF_GEAR_H_ +#define MUJOCO_PLUGIN_SDF_GEAR_H_ + +#include +#include + +#include +#include +#include +#include +#include "sdf.h" + +namespace mujoco::plugin::sdf { + +class Gear { + public: + // Creates a new Gear instance (allocated with `new`) or + // returns null on failure. + static std::optional Create(const mjModel* m, mjData* d, int instance); + Gear(Gear&&) = default; + ~Gear() = default; + + void Reset(); + void Visualize(const mjModel* m, mjData* d, const mjvOption* opt, + mjvScene* scn, int instance); + void Compute(const mjModel* m, mjData* d, int instance); + mjtNum Distance(const mjtNum point[3]) const; + void Gradient(mjtNum grad[3], const mjtNum point[3]) const; + + static void RegisterPlugin(); + + mjtNum alpha; + + private: + Gear(const mjModel* m, mjData* d, int instance); + + SdfVisualizer visualizer_; +}; + +} // namespace mujoco::plugin::sdf + +#endif // MUJOCO_PLUGIN_SDF_GEAR_H_ diff --git a/plugin/sdf/nut.cc b/plugin/sdf/nut.cc new file mode 100644 index 00000000..fe753a55 --- /dev/null +++ b/plugin/sdf/nut.cc @@ -0,0 +1,183 @@ +// Copyright 2022 DeepMind Technologies Limited +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include + +#include +#include +#include +#include "nut.h" + +namespace mujoco::plugin::sdf { +namespace { + +static mjtNum distance(const mjtNum p[3], const mjtNum attributes[1]) { + // see https://www.shadertoy.com/view/XtffzX + mjtNum screw = 12; + mjtNum radius2 = mju_sqrt(p[0]*p[0]+p[1]*p[1]) - attributes[0]; + mjtNum sqrt12 = mju_sqrt(2.)/2.; + + // a triangle wave spun around Oy, offset by the angle between x and z + mjtNum azimuth = mju_atan2(p[1], p[0]); + mjtNum triangle = abs(Fract(p[2] * screw - azimuth / mjPI / 2.) - .5); + mjtNum thread2 = (radius2 - triangle / screw) * sqrt12; + + // clip the top + mjtNum cone2 = (p[2] - radius2) * sqrt12; + + // the hole is the same thing, but substracted from the whole thing + mjtNum hole = Subtraction(thread2, cone2 + .5 * sqrt12); + hole = Union(hole, -cone2 - .05 * sqrt12); + + // create the hexagonal geometry for the head + mjtNum point2D[2] = {p[0], p[1]}; + mjtNum res[2]; + mjtNum k = 6. / mjPI / 2.; + mjtNum angle = -floor((mju_atan2(point2D[1], point2D[0])) * k + .5) / k; + mjtNum s[2] = {mju_sin(angle), mju_sin(angle + mjPI * .5)}; + mjtNum mat[4] = {s[1], -s[0], s[0], s[1]}; + mju_mulMatVec(res, mat, point2D, 2, 2); + mjtNum point3D[3] = {res[0], res[1], p[2]}; + mjtNum head = point3D[0] - .5; + + // the top is also rounded down with a cone + head = Intersection(head, abs(point3D[2] + .25) - .25); + head = Intersection(head, (point3D[2] + radius2 - .22) * sqrt12); + return Subtraction(head, hole); +} + +} // namespace + +// factory function +std::optional Nut::Create( + const mjModel* m, mjData* d, int instance) { + if (CheckAttr("radius", m, instance)) { + return Nut(m, d, instance); + } else { + mju_warning("Invalid parameter specification in Nut plugin"); + return std::nullopt; + } +} + +// plugin constructor +Nut::Nut(const mjModel* m, mjData* d, int instance) { + radius = strtod(mj_getPluginConfig(m, instance, "radius"), nullptr); +} + +// plugin computation +void Nut::Compute(const mjModel* m, mjData* d, int instance) { + visualizer_.Next(); +} + +// plugin reset +void Nut::Reset() { + visualizer_.Reset(); +} + +// plugin visualization +void Nut::Visualize(const mjModel* m, mjData* d, const mjvOption* opt, + mjvScene* scn, int instance) { + visualizer_.Visualize(m, d, opt, scn, instance); +} + +// sdf +mjtNum Nut::Distance(const mjtNum point[3]) const { + return distance(point, &radius); +} + +// gradient of sdf +void Nut::Gradient(mjtNum grad[3], const mjtNum point[3]) const { + mjtNum eps = 1e-8; + mjtNum dist0 = distance(point, &radius); + mjtNum pointX[3] = {point[0]+eps, point[1], point[2]}; + mjtNum distX = distance(pointX, &radius); + mjtNum pointY[3] = {point[0], point[1]+eps, point[2]}; + mjtNum distY = distance(pointY, &radius); + mjtNum pointZ[3] = {point[0], point[1], point[2]+eps}; + mjtNum distZ = distance(pointZ, &radius); + + grad[0] = (distX - dist0) / eps; + grad[1] = (distY - dist0) / eps; + grad[2] = (distZ - dist0) / eps; +} + +// plugin registration +void Nut::RegisterPlugin() { + mjpPlugin plugin; + mjp_defaultPlugin(&plugin); + + plugin.name = "mujoco.sdf.nut"; + plugin.capabilityflags |= mjPLUGIN_SDF; + + const char* attributes[] = {"radius"}; + plugin.nattribute = sizeof(attributes) / sizeof(attributes[0]); + plugin.attributes = attributes; + plugin.nstate = +[](const mjModel* m, int instance) { return 0; }; + + plugin.init = +[](const mjModel* m, mjData* d, int instance) { + auto sdf_or_null = Nut::Create(m, d, instance); + if (!sdf_or_null.has_value()) { + return -1; + } + d->plugin_data[instance] = reinterpret_cast( + new Nut(std::move(*sdf_or_null))); + return 0; + }; + plugin.destroy = +[](mjData* d, int instance) { + delete reinterpret_cast(d->plugin_data[instance]); + d->plugin_data[instance] = 0; + }; + plugin.reset = +[](const mjModel* m, double* plugin_state, void* plugin_data, + int instance) { + auto sdf = reinterpret_cast(plugin_data); + sdf->Reset(); + }; + plugin.visualize = +[](const mjModel* m, mjData* d, const mjvOption* opt, + mjvScene* scn, int instance) { + auto* sdf = reinterpret_cast(d->plugin_data[instance]); + sdf->Visualize(m, d, opt, scn, instance); + }; + plugin.compute = + +[](const mjModel* m, mjData* d, int instance, int capability_bit) { + auto* sdf = reinterpret_cast(d->plugin_data[instance]); + sdf->Compute(m, d, instance); + }; + plugin.sdf_distance = + +[](const mjtNum point[3], const mjData* d, int instance) { + auto* sdf = reinterpret_cast(d->plugin_data[instance]); + sdf->visualizer_.AddPoint(point); + return sdf->Distance(point); + }; + plugin.sdf_gradient = +[](mjtNum gradient[3], const mjtNum point[3], + const mjData* d, int instance) { + auto* sdf = reinterpret_cast(d->plugin_data[instance]); + sdf->Gradient(gradient, point); + }; + plugin.sdf_staticdistance = + +[](const mjtNum point[3], const mjtNum* attributes) { + return distance(point, attributes); + }; + plugin.sdf_aabb = + +[](mjtNum aabb[6], const mjtNum* attributes) { + aabb[0] = aabb[1] = aabb[2] = 0; + aabb[3] = aabb[4] = .6; + aabb[5] = 1; + }; + + mjp_registerPlugin(&plugin); +} + +} // namespace mujoco::plugin::sdf diff --git a/plugin/sdf/nut.h b/plugin/sdf/nut.h new file mode 100644 index 00000000..e9da6e34 --- /dev/null +++ b/plugin/sdf/nut.h @@ -0,0 +1,58 @@ +// Copyright 2022 DeepMind Technologies Limited +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef MUJOCO_PLUGIN_SDF_NUT_H_ +#define MUJOCO_PLUGIN_SDF_NUT_H_ + +#include + +#include +#include +#include +#include +#include "sdf.h" + +namespace mujoco::plugin::sdf { + +// this plugin implements a modification of the signed distance function +// from https://www.shadertoy.com/view/XtffzX of hexagonal nut + +class Nut { + public: + // Creates a new Nut instance (allocated with `new`) or + // returns null on failure. + static std::optional Create(const mjModel* m, mjData* d, int instance); + Nut(Nut&&) = default; + ~Nut() = default; + + void Reset(); + void Visualize(const mjModel* m, mjData* d, const mjvOption* opt, + mjvScene* scn, int instance); + void Compute(const mjModel* m, mjData* d, int instance); + mjtNum Distance(const mjtNum point[3]) const; + void Gradient(mjtNum grad[3], const mjtNum point[3]) const; + + static void RegisterPlugin(); + + mjtNum radius; + + private: + Nut(const mjModel* m, mjData* d, int instance); + + SdfVisualizer visualizer_; +}; + +} // namespace mujoco::plugin::sdf + +#endif // MUJOCO_PLUGIN_SDF_NUT_H_ diff --git a/plugin/sdf/register.cc b/plugin/sdf/register.cc new file mode 100644 index 00000000..4e727e38 --- /dev/null +++ b/plugin/sdf/register.cc @@ -0,0 +1,31 @@ +// Copyright 2022 DeepMind Technologies Limited +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "bolt.h" +#include "bowl.h" +#include "gear.h" +#include "nut.h" +#include "torus.h" + +namespace mujoco::plugin::sdf { + +mjPLUGIN_LIB_INIT { + Bolt::RegisterPlugin(); + Bowl::RegisterPlugin(); + Gear::RegisterPlugin(); + Nut::RegisterPlugin(); + Torus::RegisterPlugin(); +} + +} // namespace mujoco::plugin::sdf diff --git a/plugin/sdf/sdf.cc b/plugin/sdf/sdf.cc new file mode 100644 index 00000000..87375ba3 --- /dev/null +++ b/plugin/sdf/sdf.cc @@ -0,0 +1,128 @@ +// Copyright 2022 DeepMind Technologies Limited +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include +#include +#include "sdf.h" + +namespace mujoco::plugin::sdf { + +bool CheckAttr(const char* name, const mjModel* m, int instance) { + char *end; + std::string value = mj_getPluginConfig(m, instance, name); + value.erase(std::remove_if(value.begin(), value.end(), isspace), value.end()); + strtod(value.c_str(), &end); + return end == value.data() + value.size(); +} + +SdfVisualizer::SdfVisualizer() { + points_.assign(10*(mjMAXCONPAIR+1)*(mjMAXCONPAIR+1)*3, 0); + npoints_.clear(); +} + +void SdfVisualizer::AddPoint(const mjtNum point[3]) { + if (!npoints_.empty()) { + points_[3*npoints_.back()+0] = point[0]; + points_[3*npoints_.back()+1] = point[1]; + points_[3*npoints_.back()+2] = point[2]; + npoints_.back()++; + } +} + +void SdfVisualizer::Next() { + npoints_.push_back(npoints_.empty() ? 0 : npoints_.back()); +} + +void SdfVisualizer::Reset() { + npoints_.clear(); +} + +void SdfVisualizer::Visualize(const mjModel* m, const mjData* d, + const mjvOption* opt, mjvScene* scn, + int instance) { + if (!opt->flags[mjVIS_SDFITER]) { + return; + } + if (npoints_.empty()) { + return; + } + int tot = 0, n = 0, g = 0; + for (int i = 0; i < m->ngeom; i++) { + if (m->geom_plugin[i] == instance) { + g = i; + break; + } + } + mjtNum* points = points_.data(); + int* npoints = npoints_.data(); + int niter = npoints_.size(); + mjtNum geom_mat[9], offset[3], rotation[9], from[3], to[3]; + mjtNum* geom_xpos = d->geom_xpos + 3*g; + mjtNum* geom_xmat = d->geom_xmat + 9*g; + mjtNum* geom_pos = m->geom_pos + 3*g; + mjtNum* geom_quat = m->geom_quat + 4*g; + mju_quat2Mat(geom_mat, geom_quat); + mju_mulMatMatT(rotation, geom_xmat, geom_mat, 3, 3, 3); + mju_rotVecMat(offset, geom_pos, rotation); + mju_sub3(offset, geom_xpos, offset); + + for (int i = 0; i < niter; i++) { + n = npoints[i] - tot; + if (!n) { + continue; + } + for (int k = 0; k < 2; k++) { + for (int j = 0; j < (k == 0 ? 2 : n-1); j++) { + if (scn->ngeom >= scn->maxgeom) { + mj_warning((mjData*)d, mjWARN_VGEOMFULL, scn->maxgeom); + return; + } + mjvGeom* thisgeom = scn->geoms + scn->ngeom; + mjtNum* p1 = points + (tot + (k == 0 ? (n-1) * j : j))*3; + mjtNum* p2 = points + (tot + j + 1)*3; + mju_rotVecMat(from, p1, rotation); + mju_addTo3(from, offset); + mju_rotVecMat(to, p2, rotation); + mju_addTo3(to, offset); + if (k == 0) { + float rgba[4] = {static_cast(j > 0), 0, + static_cast(j == 0), 1}; + mjtNum size[] = {.2*m->stat.meansize}; + mjv_initGeom(thisgeom, mjGEOM_SPHERE, size, from, geom_xmat, rgba); + } else { + mjv_initGeom(thisgeom, mjGEOM_NONE, NULL, NULL, NULL, NULL); + thisgeom->objtype = mjOBJ_UNKNOWN; + thisgeom->objid = i; + thisgeom->category = mjCAT_DECOR; + thisgeom->segid = scn->ngeom; + to[0] = from[0] + .95*(to[0]-from[0]); + to[1] = from[1] + .95*(to[1]-from[1]); + to[2] = from[2] + .95*(to[2]-from[2]); + mjv_connector(thisgeom, mjGEOM_LINE, 2, from, to); + thisgeom->rgba[0] = (j+1.)/(n-1.); + thisgeom->rgba[1] = 0; + thisgeom->rgba[2] = 1 - (j+1.)/(n-1.); + thisgeom->rgba[3] = 1; + } + scn->ngeom++; + } + } + tot += n; + } +} + +} // namespace mujoco::plugin::sdf diff --git a/plugin/sdf/sdf.h b/plugin/sdf/sdf.h new file mode 100644 index 00000000..bdefd436 --- /dev/null +++ b/plugin/sdf/sdf.h @@ -0,0 +1,61 @@ +// Copyright 2022 DeepMind Technologies Limited +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef MUJOCO_PLUGIN_SDF_SDF_H_ +#define MUJOCO_PLUGIN_SDF_SDF_H_ + +#include + +#include + +namespace mujoco::plugin::sdf { + +inline mjtNum Union(mjtNum a, mjtNum b) { + return mju_min(a, b); +} + +inline mjtNum Intersection(mjtNum a, mjtNum b) { + return mju_max(a, b); +} + +inline mjtNum Subtraction(mjtNum a, mjtNum b) { + return mju_max(a, -b); +} + +inline mjtNum Fract(mjtNum x) { + return x - floor(x); +} + +// reads numeric attributes +bool CheckAttr(const char* name, const mjModel* m, int instance); + +class SdfVisualizer { + public: + SdfVisualizer(); + + void Visualize(const mjModel* m, const mjData* d, const mjvOption* opt, + mjvScene* scn, int instance); + + void AddPoint(const mjtNum point[3]); + void Reset(); + void Next(); // adds a new gradient descent trajectory to be visualized + + private: + std::vector points_; // query points + std::vector npoints_; // number of iterations from the starting point +}; + +} // namespace mujoco::plugin::sdf + +#endif // MUJOCO_PLUGIN_SDF_SDF_H_ diff --git a/plugin/sdf/torus.cc b/plugin/sdf/torus.cc new file mode 100644 index 00000000..48af63dd --- /dev/null +++ b/plugin/sdf/torus.cc @@ -0,0 +1,124 @@ +// Copyright 2022 DeepMind Technologies Limited +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include +#include +#include +#include "torus.h" + +namespace mujoco::plugin::sdf { +namespace { + +static mjtNum distance(const mjtNum p[3], const mjtNum radius[2]) { + mjtNum q = mju_sqrt(p[0]*p[0] + p[1]*p[1]) - radius[0]; + return mju_sqrt(q*q + p[2]*p[2]) - radius[1]; +} + +} // namespace + +// factory function +std::optional Torus::Create( + const mjModel* m, mjData* d, int instance) { + if (CheckAttr("radius1", m, instance) && CheckAttr("radius2", m, instance)) { + return Torus(m, d, instance); + } else { + mju_warning("Invalid radius1 or radius2 parameters in Torus plugin"); + return std::nullopt; + } +} + +// plugin constructor +Torus::Torus(const mjModel* m, mjData* d, int instance) { + radius[0] = strtod(mj_getPluginConfig(m, instance, "radius1"), nullptr); + radius[1] = strtod(mj_getPluginConfig(m, instance, "radius2"), nullptr); +} + +// sdf +mjtNum Torus::Distance(const mjtNum point[3]) const { + return distance(point, radius); +} + +// gradient of sdf +void Torus::Gradient(mjtNum grad[3], const mjtNum p[3]) const { + mjtNum len_xy = mju_sqrt(p[0]*p[0] + p[1]*p[1]); + mjtNum q = len_xy - radius[0]; + mjtNum grad_q[2] = { p[0] / len_xy, p[1] / len_xy }; + mjtNum len_qz = mju_sqrt(q*q + p[2]*p[2]); + grad[0] = q*grad_q[0] / mjMAX(len_qz, mjMINVAL); + grad[1] = q*grad_q[1] / mjMAX(len_qz, mjMINVAL); + grad[2] = p[2] / mjMAX(len_qz, mjMINVAL); +} + +// plugin registration +void Torus::RegisterPlugin() { + mjpPlugin plugin; + mjp_defaultPlugin(&plugin); + + plugin.name = "mujoco.sdf.torus"; + plugin.capabilityflags |= mjPLUGIN_SDF; + + const char* attributes[] = {"radius1", "radius2", "axis"}; + plugin.nattribute = sizeof(attributes) / sizeof(attributes[0]); + plugin.attributes = attributes; + plugin.nstate = +[](const mjModel* m, int instance) { return 0; }; + + plugin.init = +[](const mjModel* m, mjData* d, int instance) { + auto sdf_or_null = Torus::Create(m, d, instance); + if (!sdf_or_null.has_value()) { + return -1; + } + d->plugin_data[instance] = reinterpret_cast( + new Torus(std::move(*sdf_or_null))); + return 0; + }; + plugin.destroy = +[](mjData* d, int instance) { + delete reinterpret_cast(d->plugin_data[instance]); + d->plugin_data[instance] = 0; + }; + plugin.reset = +[](const mjModel* m, double* plugin_state, void* plugin_data, + int instance) { + // do nothing + }; + plugin.compute = + +[](const mjModel* m, mjData* d, int instance, int capability_bit) { + // do nothing; + }; + plugin.sdf_distance = + +[](const mjtNum point[3], const mjData* d, int instance) { + auto* sdf = reinterpret_cast(d->plugin_data[instance]); + return sdf->Distance(point); + }; + plugin.sdf_gradient = +[](mjtNum gradient[3], const mjtNum point[3], + const mjData* d, int instance) { + auto* sdf = reinterpret_cast(d->plugin_data[instance]); + sdf->Gradient(gradient, point); + }; + plugin.sdf_staticdistance = + +[](const mjtNum point[3], const mjtNum* attributes) { + return distance(point, attributes); + }; + plugin.sdf_aabb = + +[](mjtNum aabb[6], const mjtNum* attributes) { + aabb[0] = aabb[1] = aabb[2] = 0; + aabb[3] = aabb[4] = attributes[0] + attributes[1]; + aabb[5] = attributes[1]; + }; + + mjp_registerPlugin(&plugin); +} + +} // namespace mujoco::plugin::sdf diff --git a/plugin/sdf/torus.h b/plugin/sdf/torus.h new file mode 100644 index 00000000..0b1b3cb5 --- /dev/null +++ b/plugin/sdf/torus.h @@ -0,0 +1,47 @@ +// Copyright 2022 DeepMind Technologies Limited +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef MUJOCO_PLUGIN_SDF_TORUS_H_ +#define MUJOCO_PLUGIN_SDF_TORUS_H_ + +#include + +#include +#include +#include +#include "sdf.h" + +namespace mujoco::plugin::sdf { + +class Torus { + public: + // Creates a new Torus instance or returns null on failure. + static std::optional Create(const mjModel* m, mjData* d, int instance); + Torus(Torus&&) = default; + ~Torus() = default; + + mjtNum Distance(const mjtNum point[3]) const; + void Gradient(mjtNum grad[3], const mjtNum point[3]) const; + + static void RegisterPlugin(); + + mjtNum radius[2]; + + private: + Torus(const mjModel* m, mjData* d, int instance); +}; + +} // namespace mujoco::plugin::sdf + +#endif // MUJOCO_PLUGIN_SDF_TORUS_H_ diff --git a/simulate/simulate.cc b/simulate/simulate.cc index 1f6b9a60..12f19de5 100644 --- a/simulate/simulate.cc +++ b/simulate/simulate.cc @@ -27,6 +27,7 @@ #include "lodepng.h" #include +#include #include #include #include @@ -605,6 +606,8 @@ void MakePhysicsSection(mj::Simulate* sim, int oldstate) { {mjITEM_EDITINT, "MRR Iter", 2, &(opt->mpr_iterations), "1 0 1000"}, {mjITEM_EDITNUM, "MPR Tol", 2, &(opt->mpr_tolerance), "1 0 1"}, {mjITEM_EDITNUM, "API Rate", 2, &(opt->apirate), "1 0 1000"}, + {mjITEM_EDITINT, "SDF Iter", 2, &(opt->sdf_iterations), "1 1 20"}, + {mjITEM_EDITINT, "SDF Init", 2, &(opt->sdf_initpoints), "1 1 100"}, {mjITEM_SEPARATOR, "Physical Parameters", 1}, {mjITEM_EDITNUM, "Gravity", 2, opt->gravity, "3"}, {mjITEM_EDITNUM, "Wind", 2, opt->wind, "3"}, @@ -756,7 +759,7 @@ void MakeRenderingSection(mj::Simulate* sim, const mjModel* m, int oldstate) { // create tree slider mjuiDef defTree[] = { - {mjITEM_SLIDERINT, "Tree depth", 2, &sim->opt.bvh_depth, "0 15"}, + {mjITEM_SLIDERINT, "Tree depth", 2, &sim->opt.bvh_depth, "0 20"}, {mjITEM_END} }; mjui_add(&sim->ui0, defTree); @@ -1713,6 +1716,8 @@ void Simulate::Sync() { X(mpr_iterations); X(disableflags); X(enableflags); + X(sdf_initpoints); + X(sdf_iterations); #undef X diff --git a/src/engine/CMakeLists.txt b/src/engine/CMakeLists.txt index 029b6d23..b5cd890f 100644 --- a/src/engine/CMakeLists.txt +++ b/src/engine/CMakeLists.txt @@ -23,6 +23,8 @@ set(MUJOCO_ENGINE_SRCS engine_collision_driver.h engine_collision_primitive.c engine_collision_primitive.h + engine_collision_sdf.c + engine_collision_sdf.h engine_core_constraint.c engine_core_constraint.h engine_core_smooth.c diff --git a/src/engine/engine_collision_driver.c b/src/engine/engine_collision_driver.c index 1dc70ecd..5674b8dc 100644 --- a/src/engine/engine_collision_driver.c +++ b/src/engine/engine_collision_driver.c @@ -23,6 +23,7 @@ #include "engine/engine_callback.h" #include "engine/engine_collision_convex.h" #include "engine/engine_collision_primitive.h" +#include "engine/engine_collision_sdf.h" #include "engine/engine_core_constraint.h" #include "engine/engine_crossplatform.h" #include "engine/engine_io.h" @@ -34,15 +35,16 @@ // table of pair-wise collision functions mjfCollision mjCOLLISIONFUNC[mjNGEOMTYPES][mjNGEOMTYPES] = { - /* PLANE HFIELD SPHERE CAPSULE ELLIPSOID CYLINDER BOX MESH */ - /*PLANE */ {0, 0, mjc_PlaneSphere, mjc_PlaneCapsule, mjc_PlaneConvex, mjc_PlaneCylinder, mjc_PlaneBox, mjc_PlaneConvex}, - /*HFIELD */ {0, 0, mjc_ConvexHField, mjc_ConvexHField, mjc_ConvexHField, mjc_ConvexHField, mjc_ConvexHField, mjc_ConvexHField}, - /*SHPERE */ {0, 0, mjc_SphereSphere, mjc_SphereCapsule, mjc_Convex, mjc_SphereCylinder, mjc_SphereBox, mjc_Convex}, - /*CAPSULE */ {0, 0, 0, mjc_CapsuleCapsule, mjc_Convex, mjc_Convex, mjc_CapsuleBox, mjc_Convex}, - /*ELLIPSOID */ {0, 0, 0, 0, mjc_Convex, mjc_Convex, mjc_Convex, mjc_Convex}, - /*CYLINDER */ {0, 0, 0, 0, 0, mjc_Convex, mjc_Convex, mjc_Convex}, - /*BOX */ {0, 0, 0, 0, 0, 0, mjc_BoxBox, mjc_Convex}, - /*MESH */ {0, 0, 0, 0, 0, 0, 0, mjc_Convex} + /* PLANE HFIELD SPHERE CAPSULE ELLIPSOID CYLINDER BOX MESH SDF */ + /*PLANE */ {0, 0, mjc_PlaneSphere, mjc_PlaneCapsule, mjc_PlaneConvex, mjc_PlaneCylinder, mjc_PlaneBox, mjc_PlaneConvex, mjc_SDF}, + /*HFIELD */ {0, 0, mjc_ConvexHField, mjc_ConvexHField, mjc_ConvexHField, mjc_ConvexHField, mjc_ConvexHField, mjc_ConvexHField, mjc_HFieldSDF}, + /*SPHERE */ {0, 0, mjc_SphereSphere, mjc_SphereCapsule, mjc_Convex, mjc_SphereCylinder, mjc_SphereBox, mjc_Convex, mjc_SDF}, + /*CAPSULE */ {0, 0, 0, mjc_CapsuleCapsule, mjc_Convex, mjc_Convex, mjc_CapsuleBox, mjc_Convex, mjc_SDF}, + /*ELLIPSOID */ {0, 0, 0, 0, mjc_Convex, mjc_Convex, mjc_Convex, mjc_Convex, mjc_SDF}, + /*CYLINDER */ {0, 0, 0, 0, 0, mjc_Convex, mjc_Convex, mjc_Convex, mjc_SDF}, + /*BOX */ {0, 0, 0, 0, 0, 0, mjc_BoxBox, mjc_Convex, mjc_SDF}, + /*MESH */ {0, 0, 0, 0, 0, 0, 0, mjc_Convex, mjc_MeshSDF}, + /*SDF */ {0, 0, 0, 0, 0, 0, 0, 0, mjc_SDF} }; diff --git a/src/engine/engine_collision_sdf.c b/src/engine/engine_collision_sdf.c new file mode 100644 index 00000000..fb261fd5 --- /dev/null +++ b/src/engine/engine_collision_sdf.c @@ -0,0 +1,695 @@ +// Copyright 2021 DeepMind Technologies Limited +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "engine/engine_collision_sdf.h" + +#include + +#include +#include +#include +#include "engine/engine_collision_primitive.h" +#include "engine/engine_io.h" +#include "engine/engine_plugin.h" +#include "engine/engine_util_blas.h" +#include "engine/engine_util_errmem.h" +#include "engine/engine_util_misc.h" +#include "engine/engine_util_spatial.h" + + +#define MAXSDFFACE 1300 + + + +//---------------------------- primitives sdf --------------------------------------------- + +static mjtNum geomDistance(const mjModel* m, const mjData* d, const mjpPlugin* p, + int i, const mjtNum x[3], mjtGeom type) { + mjtNum a[3], b[3]; + const mjtNum* size = m->geom_size+3*i; + + // see https://iquilezles.org/articles/distfunctions/ + switch (type) { + case mjGEOM_PLANE: + return x[2]; + case mjGEOM_SPHERE: + return mju_norm3(x) - size[0]; + case mjGEOM_BOX: + a[0] = mju_abs(x[0]) - size[0]; + a[1] = mju_abs(x[1]) - size[1]; + a[2] = mju_abs(x[2]) - size[2]; + b[0] = mju_max(a[0], 0); + b[1] = mju_max(a[1], 0); + b[2] = mju_max(a[2], 0); + return mju_norm3(b) + mju_min(mju_max(a[0], mju_max(a[1], a[2])), 0); + case mjGEOM_CAPSULE: + a[0] = x[0]; + a[1] = x[1]; + a[2] = x[2] - mju_clip(x[2], -size[1], size[1]); + return mju_norm3(a) - size[0]; + case mjGEOM_ELLIPSOID: + a[0] = x[0] / size[0]; + a[1] = x[1] / size[1]; + a[2] = x[2] / size[2]; + b[0] = a[0] / size[0]; + b[1] = a[1] / size[1]; + b[2] = a[2] / size[2]; + mjtNum k0 = mju_norm3(a); + mjtNum k1 = mju_norm3(b); + return k0 * (k0 - 1.0) / k1; + case mjGEOM_CYLINDER: + a[0] = mju_sqrt(x[0]*x[0]+x[1]*x[1]) - size[0]; + a[1] = mju_abs(x[2]) - size[1]; + b[0] = mju_max(a[0], 0); + b[1] = mju_max(a[1], 0); + return mju_min(mju_max(a[0], a[1]), 0) + mju_norm(b, 2); + case mjGEOM_SDF: + return p->sdf_distance(x, d, i); + default: + mju_error("sdf collisions not available for geom type %d", type); + return 0; + } +} + +static void geomGradient(mjtNum gradient[3], const mjModel* m, const mjData* d, + const mjpPlugin* p, int i, const mjtNum x[3], + mjtGeom type) { + mjtNum a[3], b[3], c, e; + const mjtNum* size = m->geom_size+3*i; + + // see https://iquilezles.org/articles/distfunctions/ + switch (type) { + case mjGEOM_PLANE: + mju_zero3(gradient); + gradient[2] = 1; + break; + case mjGEOM_SPHERE: + mju_copy3(gradient, x); + c = mju_norm3(x); + gradient[0] *= 1. / c; + gradient[1] *= 1. / c; + gradient[2] *= 1. / c; + break; + case mjGEOM_BOX: + mju_zero3(gradient); + a[0] = mju_abs(x[0]) - size[0]; + a[1] = mju_abs(x[1]) - size[1]; + a[2] = mju_abs(x[2]) - size[2]; + int k = a[0] > a[1] ? 0 : 1; + int l = a[2] > a[k] ? 2 : k; + if (a[l]<0) { + gradient[l] = x[l] / mju_abs(x[l]); + } else { + b[0] = mju_max(a[0], 0); + b[1] = mju_max(a[1], 0); + b[2] = mju_max(a[2], 0); + c = mju_norm3(b); + gradient[0] = a[0]>0 ? b[0] / c * x[0] / mju_abs(x[0]) : 0; + gradient[1] = a[1]>0 ? b[1] / c * x[1] / mju_abs(x[1]) : 0; + gradient[2] = a[2]>0 ? b[2] / c * x[2] / mju_abs(x[2]) : 0; + } + break; + case mjGEOM_CAPSULE: + a[0] = x[0]; + a[1] = x[1]; + a[2] = x[2] - mju_clip(x[2], -size[1], size[1]); + c = mju_norm3(a); + gradient[0] = a[0] / c; + gradient[1] = a[1] / c; + gradient[2] = a[2] / c; + break; + case mjGEOM_ELLIPSOID: + a[0] = x[0] / size[0]; + a[1] = x[1] / size[1]; + a[2] = x[2] / size[2]; + b[0] = a[0] / size[0]; + b[1] = a[1] / size[1]; + b[2] = a[2] / size[2]; + mjtNum k0 = mju_norm3(a); + mjtNum k1 = mju_norm3(b); + gradient[0] = a[0]*(2.*k0 - 1.) / k1 + k0*(k0 - 1.) * b[0]/(k1*k1); + gradient[1] = a[1]*(2.*k0 - 1.) / k1 + k0*(k0 - 1.) * b[1]/(k1*k1); + gradient[2] = a[2]*(2.*k0 - 1.) / k1 + k0*(k0 - 1.) * b[2]/(k1*k1); + break; + case mjGEOM_CYLINDER: + c = mju_sqrt(x[0]*x[0]+x[1]*x[1]); + e = mju_abs(x[2]); + a[0] = c - size[0]; + a[1] = e - size[1]; + mjtNum grada[3] = {x[0] / c, x[1] / c, x[2] / e}; + int j = a[0] > a[1] ? 0 : 1; + if (a[j] < 0) { + gradient[0] = j==0 ? grada[0] : 0; + gradient[1] = j==0 ? grada[1] : 0; + gradient[2] = j==1 ? grada[2] : 0; + } else { + b[0] = mju_max(a[0], 0); + b[1] = mju_max(a[1], 0); + mjtNum bnorm = mju_norm(b, 2); + gradient[0] = grada[0] * b[0] / bnorm; + gradient[1] = grada[1] * b[0] / bnorm; + gradient[2] = grada[2] * b[1] / bnorm; + } + break; + case mjGEOM_SDF: + p->sdf_gradient(gradient, x, d, i); + break; + default: + mju_error("sdf collisions not available for geom type %d", type); + } +} + +//---------------------------- helper functions ------------------------------------------- + +// signed distance function +mjtNum mjc_distance(const mjModel* m, const mjData* d, const mjSDF* s, const mjtNum x[3]) { + mjtNum y[3]; + + switch (s->type) { + case mjSDFTYPE_SINGLE: + return geomDistance(m, d, s->plugin[0], s->id[0], x, s->geomtype[0]); + case mjSDFTYPE_INTERSECTION: + mju_rotVecMat(y, x, s->relmat); + mju_addTo3(y, s->relpos); + return mju_max(geomDistance(m, d, s->plugin[0], s->id[0], x, s->geomtype[0]), + geomDistance(m, d, s->plugin[1], s->id[1], y, s->geomtype[1])); + default: + mju_error("SDF type not available"); + return 0; + } +} + +// gradient of sdf +void mjc_gradient(const mjModel* m, const mjData* d, const mjSDF* s, + mjtNum gradient[3], const mjtNum x[3]) { + mjtNum y[3]; + const mjtNum* point[2] = {x, y}; + + switch (s->type) { + case mjSDFTYPE_INTERSECTION: + mju_rotVecMat(y, x, s->relmat); + mju_addTo3(y, s->relpos); + int i = geomDistance(m, d, s->plugin[0], s->id[0], x, s->geomtype[0]) > + geomDistance(m, d, s->plugin[1], s->id[1], y, s->geomtype[1]) ? 0 : 1; + geomGradient(gradient, m, d, s->plugin[i], s->id[i], point[i], s->geomtype[i]); + if (i==1) { + mju_rotVecMatT(gradient, gradient, s->relmat); + } + break; + case mjSDFTYPE_AVERAGE: + mju_rotVecMat(y, x, s->relmat); + mju_addTo3(y, s->relpos); + mjtNum grad1[3], grad2[3]; + geomGradient(grad1, m, d, s->plugin[0], s->id[0], x, s->geomtype[0]); + mju_normalize3(grad1); + geomGradient(grad2, m, d, s->plugin[1], s->id[1], y, s->geomtype[1]); + mju_rotVecMatT(grad2, grad2, s->relmat); + mju_normalize3(grad2); + mju_sub3(gradient, grad1, grad2); + mju_normalize3(gradient); + break; + case mjSDFTYPE_SINGLE: + geomGradient(gradient, m, d, s->plugin[0], s->id[0], point[0], s->geomtype[0]); + break; + default: + mju_error("SDF type not available"); + } +} + +// get sdf from geom id +static const mjpPlugin* getSDF(const mjModel* m, int id) { + int instance = m->geom_plugin[id]; + const int nslot = mjp_pluginCount(); + const int slot = m->plugin[instance]; + const mjpPlugin* sdf = mjp_getPluginAtSlotUnsafe(slot, nslot); + if (!sdf) mju_error("invalid plugin slot: %d", slot); + if (!(sdf->capabilityflags & mjPLUGIN_SDF)) { + mju_error("Plugin is not a signed distance field at slot %d", slot); + } + return sdf; +} + +// map (pos12, mat12) as (xpos2, xmat2)^-1 \circ (xpos1, xmat1) +static void mapPose(const mjtNum xpos1[3], const mjtNum xquat1[4], + const mjtNum xpos2[3], const mjtNum xquat2[4], + mjtNum pos12[3], mjtNum mat12[9]) { + mjtNum negpos[3], negquat[4], quat12[4]; + mju_negPose(negpos, negquat, xpos2, xquat2); + mju_mulPose(pos12, quat12, negpos, negquat, xpos1, xquat1); + mju_quat2Mat(mat12, quat12); +} + +// subtract mesh position from sdf transformation +static void undoTransformation(const mjModel* m, const mjData* d, int g, + mjtNum sdf_xpos[3], mjtNum sdf_quat[4]) { + mjtNum* xpos = d->geom_xpos + 3 * g; + mjtNum* xmat = d->geom_xmat + 9 * g; + if (m->geom_type[g]==mjGEOM_MESH || m->geom_type[g]==mjGEOM_SDF) { + mjtNum negpos[3], negquat[4], xquat[4]; + mjtNum* pos = m->geom_pos + 3 * g; + mjtNum* quat = m->geom_quat + 4 * g; + mju_mat2Quat(xquat, xmat); + mju_negPose(negpos, negquat, pos, quat); + mju_mulPose(sdf_xpos, sdf_quat, xpos, xquat, negpos, negquat); + } else { + mju_copy3(sdf_xpos, xpos); + mju_mat2Quat(sdf_quat, xmat); + } +} + +//---------------------------- narrow phase ----------------------------------------------- + +// check if the collision point already exists +static int isknown(const mjtNum* points, const mjtNum x[3], int cnt) { + for (int i = 0; i < cnt; i++) { + if (mju_dist3(x, points + 3*i) < mjMINVAL) { + return 1; + } + } + return 0; +} + +// adds candidate point to result +static int addContact(mjtNum* points, mjContact* con, const mjtNum x[3], + const mjtNum pos2[3], const mjtNum quat2[4], mjtNum dist, + int cnt, const mjModel* m, const mjSDF* s, mjData* d) { + // check if there is a collision + if (dist > 0 || isknown(points, x, cnt)) { + return cnt; + } else { + mju_copy3(points+3*cnt, x); + } + + // compute normal in local coordinates + mjtNum norm[3], vec[3]; + mjc_gradient(m, d, s, norm, x); + mju_scl3(norm, norm, -1); + + // construct contact + con[cnt].dist = dist; + mju_rotVecQuat(con[cnt].frame, norm, quat2); + mju_zero3(con[cnt].frame+3); + mju_makeFrame(con[cnt].frame); + mju_scl3(vec, con[cnt].frame, -con[cnt].dist/2); + mju_rotVecQuat(con[cnt].pos, x, quat2); + mju_addTo3(con[cnt].pos, pos2); + mju_addTo3(con[cnt].pos, vec); + + return cnt+1; +} + +// finds minimum of Frank-Wolfe objective +static mjtNum stepFrankWolfe(mjtNum x[3], const mjtNum* corners, int ncorners, + const mjModel* m, const mjSDF* sdf, mjData* d) { + for (int step=0; stepopt.sdf_iterations; step++) { + mjtNum best = 1e10, fun, s[3], grad[3]; + + // evaluate gradient + mjc_gradient(m, d, sdf, grad, x); + + // evaluate all corners + for (int i=0; iopt.sdf_iterations; step++) { + mjtNum grad[3]; + + // evaluate gradient + mjc_gradient(m, d, s, grad, x); + + // sanity check + if (isnan(grad[0]) || isnan(grad[1]) || isnan(grad[2])) { + return mjMAXVAL; + } + + // update solution + mju_addToScl3(x, grad, -alpha/(mjtNum)(step+1)); + } + + // compute distance + return mjc_distance(m, d, s, x); +} + +//---------------------------- bounding box vs sdf ------------------------------------------------- + +// stricter triangle collision +static int triangleIntersect(const mjtNum triangle[9], const mjModel* m, + const mjSDF* sdf, mjData* d) { + mjtNum edges[6]; + mjtNum normal[3], center[3]; + mjtNum v[9], cross[9], p[3]; + mjtNum kDistanceScl = 10.; + + // triangle normal + mju_sub3(edges+0, triangle+3, triangle); + mju_sub3(edges+3, triangle+6, triangle); + mju_cross(normal, edges, edges+3); + mju_normalize3(normal); + + // fourth point + mju_scl3(p, triangle, 1./3.); + mju_addToScl3(p, triangle+3, 1./3.); + mju_addToScl3(p, triangle+6, 1./3.); + mjtNum h = -mjc_distance(m, d, sdf, p)/kDistanceScl; + mju_addToScl3(p, normal, -h); + + // circumsphere center + mju_sub3(v+0, triangle+0, p); + mju_sub3(v+3, triangle+3, p); + mju_sub3(v+6, triangle+6, p); + mju_cross(cross+0, v+3, v+6); + mju_cross(cross+3, v+6, v+0); + mju_cross(cross+6, v+0, v+3); + mju_scl3(center, cross, mju_dot3(v, v)); + mju_addToScl3(center, cross+3, mju_dot3(v+3, v+3)); + mju_addToScl3(center, cross+6, mju_dot3(v+6, v+6)); + mju_scl3(center, center, 1./(2.*mju_dot3(v, cross))); + + // circumsphere radius + mjtNum r = mju_sqrt(mju_dot3(center, center)); + + // coordinate change + mju_addTo3(center, p); + + return mjc_distance(m, d, sdf, center)mesh_bvhadr[m->geom_dataid[g]]; + const int* faceid = m->bvh_geomid + bvhadr; + const mjtNum* bvh = m->bvh_aabb + 6*bvhadr; + const int* child = m->bvh_child + 2*bvhadr; + mjtByte* visited = d->bvh_active + bvhadr; + + mjMARKSTACK; + // TODO(quaglino): Store bvh max depths to make this bound tighter. + int max_stack = m->mesh_bvhnum[m->geom_dataid[g]]; + struct CollideTreeArgs_ { + int node; + }; + typedef struct CollideTreeArgs_ CollideTreeArgs; + CollideTreeArgs* stack = (CollideTreeArgs*)mj_stackAlloc( + d, max_stack * sizeof(CollideTreeArgs*) / sizeof(mjtNum)); + + int nstack = 0; + stack[nstack].node = 0; + nstack++; + + while (nstack) { + (*n0)++; + + // pop from stack + nstack--; + int node = stack[nstack].node; + + // node1 is a leaf + if (faceid[node] != -1) { + if (visited[node]) { + continue; + } + if (boxIntersect(bvh+6*node, offset, rotation, m, sdf, d)) { + faces[*npoints] = faceid[node]; + if (++(*npoints)==MAXSDFFACE) { + mju_warning("mjc_MeshSDF: too many bounding volumes, some contacts may be missed"); + mjFREESTACK; + return; + } + visited[node] = 1; + } + continue; + } + + // if no intersection at intermediate levels, stop + if (!boxIntersect(bvh+6*node, offset, rotation, m, sdf, d)) { + continue; + } + + visited[node] = 1; + + // recursive call + for (int i=0; i<2; i++) { + if (child[2*node+i] != -1) { + if (nstack >= max_stack) mju_error("BVH stack depth exceeded."); + stack[nstack].node = child[2*node+i]; + nstack++; + } + } + } + + mjFREESTACK; +} + +//------------------------------ collision functions ----------------------------------------------- + +// collision between a height field and a signed distance field +int mjc_HFieldSDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2, mjtNum margin) { + mju_warning("HField vs SDF collision not yet supported!"); + return 0; +} + +// collision between a mesh and a signed distance field +int mjc_MeshSDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2, mjtNum margin) { + mjtNum* pos1 = d->geom_xpos + 3 * g1; + mjtNum* mat1 = d->geom_xmat + 9 * g1; + + mjtNum offset[3], rotation[9]; + mjtNum corners[9], points[3*MAXSDFFACE], x[3], dist; + int vertadr = m->mesh_vertadr[m->geom_dataid[g1]]; + int faceadr = m->mesh_faceadr[m->geom_dataid[g1]]; + int cnt=0, npoints=0, n0=0, faces[MAXSDFFACE]={-1}; + + // get sdf plugin + int instance = m->geom_plugin[g2]; + const mjpPlugin* sdf_ptr = getSDF(m, g2); + mjtGeom geomtype = mjGEOM_SDF; + + // copy into data + mjSDF sdf; + sdf.id = &instance; + sdf.type = mjSDFTYPE_SINGLE; + sdf.plugin = &sdf_ptr; + sdf.geomtype = &geomtype; + + // compute transformation from g1 to g2 + mjtNum pos2true[3], sdf_quat[4], quat1[4]; + mju_mat2Quat(quat1, mat1); + undoTransformation(m, d, g2, pos2true, sdf_quat); + mapPose(pos1, quat1, pos2true, sdf_quat, offset, rotation); + + // binary tree search + collideBVH(m, (mjData*)d, g1, offset, rotation, faces, &npoints, &n0, &sdf); + + // Frank-Wolfe algorithm + for (int i=0; imesh_vert[3*(vertadr+m->mesh_face[3*face+v])+0], + m->mesh_vert[3*(vertadr+m->mesh_face[3*face+v])+1], + m->mesh_vert[3*(vertadr+m->mesh_face[3*face+v])+2], + }; + + // transform local 1 (mesh) to local 2 (sdf) + mju_rotVecMat(corners+3*v, vec, rotation); + mju_addTo3(corners+3*v, offset); + } + + // stricter culling + if (!triangleIntersect(corners, m, &sdf, (mjData*)d)) { + continue; + } + + // starting point + x[0] = (corners[0]+corners[3]+corners[6])/3; + x[1] = (corners[1]+corners[4]+corners[7])/3; + x[2] = (corners[2]+corners[5]+corners[8])/3; + + // SHOULD NOT OCCUR + if (cnt==mjMAXCONPAIR) mju_error("mjc_MeshSDF: too many contact points"); + + // Frank-Wolfe + dist = stepFrankWolfe(x, corners, 3, m, &sdf, (mjData*)d); + cnt = addContact(points, con, x, pos2true, sdf_quat, dist, cnt, m, &sdf, (mjData*)d); + } + + return cnt; +} + +// collision between two SDFs +int mjc_SDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2, mjtNum margin) { + mjGETINFO; + size1 = m->geom_aabb + 6*g1; + size2 = m->geom_aabb + 6*g2; + + int cnt = 0; + mjtNum x[3], y[3], dist, vec1[3], vec2[3]; + mjtNum aabb1[6] = {mjMAXVAL, mjMAXVAL, mjMAXVAL, -mjMAXVAL, -mjMAXVAL, -mjMAXVAL}; + mjtNum aabb2[6] = {mjMAXVAL, mjMAXVAL, mjMAXVAL, -mjMAXVAL, -mjMAXVAL, -mjMAXVAL}; + mjtNum aabb[6] = {mjMAXVAL, mjMAXVAL, mjMAXVAL, -mjMAXVAL, -mjMAXVAL, -mjMAXVAL}; + + // second geom must be an SDF + if (m->geom_type[g2] != mjGEOM_SDF) { + mju_error("geom is not an SDF"); + } + + // compute transformations from/to g1 to/from g2 + mjtNum quat1[4], quat2[4]; + mjtNum pos1true[3], offset21[3], rotation21[9], rotation12[9]; + mjtNum pos2true[3], offset1[3], rotation1[9], offset12[3]; + mjtNum offset2[3], rotation2[9], squat1[4], squat2[4]; + undoTransformation(m, d, g1, pos1true, squat1); + undoTransformation(m, d, g2, pos2true, squat2); + mju_mat2Quat(quat1, mat1); + mju_mat2Quat(quat2, mat2); + mapPose(pos2, quat2, pos1, quat1, offset1, rotation1); + mapPose(pos1, quat1, pos1true, squat1, offset2, rotation2); + mapPose(pos2true, squat2, pos1true, squat1, offset21, rotation21); + mapPose(pos1true, squat1, pos2true, squat2, offset12, rotation12); + + // axis-aligned bounding boxes in g1 frame + for (int i=0; i<8; i++) { + vec1[0] = (i&1 ? size1[0]+size1[3] : size1[0]-size1[3]); + vec1[1] = (i&2 ? size1[1]+size1[4] : size1[1]-size1[4]); + vec1[2] = (i&4 ? size1[2]+size1[5] : size1[2]-size1[5]); + + vec2[0] = (i&1 ? size2[0]+size2[3] : size2[0]-size2[3]); + vec2[1] = (i&2 ? size2[1]+size2[4] : size2[1]-size2[4]); + vec2[2] = (i&4 ? size2[2]+size2[5] : size2[2]-size2[5]); + + mju_rotVecMat(vec2, vec2, rotation1); + mju_addTo3(vec2, offset1); + + for (int k=0; k<3; k++) { + aabb1[0+k] = mju_min(aabb1[0+k], vec1[k]); + aabb1[3+k] = mju_max(aabb1[3+k], vec1[k]); + aabb2[0+k] = mju_min(aabb2[0+k], vec2[k]); + aabb2[3+k] = mju_max(aabb2[3+k], vec2[k]); + } + } + + // intersection of aabbs + for (int k=0; k<3; k++) { + aabb[0+k] = mju_max(aabb1[0+k], aabb2[0+k]); + aabb[3+k] = mju_min(aabb1[3+k], aabb2[3+k]); + } + + // no intersection if max < min + if (aabb[3]geom_type[g2], m->geom_type[g1]}; + + instance[0] = m->geom_plugin[g2]; + sdf_ptr[0] = getSDF(m, g2); + + // get sdf plugins + if (m->geom_type[g1] == mjGEOM_SDF) { + instance[1] = m->geom_plugin[g1]; + sdf_ptr[1] = getSDF(m, g1); + } else { + instance[1] = g1; + sdf_ptr[1] = NULL; + } + + // reset visualization count + sdf_ptr[0]->reset(m, NULL, (void*)(d->plugin_data[instance[0]]), instance[0]); + + // copy into sdf + mjSDF sdf; + sdf.id = instance; + sdf.relpos = offset21; + sdf.relmat = rotation21; + sdf.plugin = sdf_ptr; + sdf.geomtype = geomtypes; + + // minimize sdf intersection + mjtNum contacts[3*mjMAXCONPAIR]; + + int i = 0, j = 0; + while (iopt.sdf_initpoints) { + x[0] = aabb[0] + (aabb[3]-aabb[0]) * mju_Halton(j, 2); + x[1] = aabb[1] + (aabb[4]-aabb[1]) * mju_Halton(j, 3); + x[2] = aabb[2] + (aabb[5]-aabb[2]) * mju_Halton(j, 5); + + mju_rotVecMat(y, x, rotation2); + mju_addTo3(y, offset2); + + mju_rotVecMat(x, y, rotation12); + mju_addTo3(x, offset12); + + j++; + + // here a criterion for rejecting points could be inserted + + i++; + + // start counters + sdf_ptr[0]->compute(m, (mjData*)d, instance[0], mjPLUGIN_SDF); + + // gradient descent + sdf.type = mjSDFTYPE_INTERSECTION; + dist = stepGradient(x, m, &sdf, (mjData*)d); + sdf.type = mjSDFTYPE_AVERAGE; + cnt = addContact(contacts, con, x, pos2true, squat2, dist, cnt, m, &sdf, (mjData*)d); + } + + return cnt; +} + diff --git a/src/engine/engine_collision_sdf.h b/src/engine/engine_collision_sdf.h new file mode 100644 index 00000000..ef9f3f95 --- /dev/null +++ b/src/engine/engine_collision_sdf.h @@ -0,0 +1,62 @@ +// Copyright 2021 DeepMind Technologies Limited +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef MUJOCO_SRC_ENGINE_ENGINE_COLLISION_SDF_H_ +#define MUJOCO_SRC_ENGINE_ENGINE_COLLISION_SDF_H_ + +#include +#include +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +typedef enum mjtSDFType_ { + mjSDFTYPE_SINGLE = 0, + mjSDFTYPE_INTERSECTION, + mjSDFTYPE_AVERAGE, +} mjtSDFType; + +struct mjSDF_ { + const mjpPlugin** plugin; + int* id; + mjtSDFType type; + mjtNum* relpos; + mjtNum* relmat; + mjtGeom* geomtype; +}; +typedef struct mjSDF_ mjSDF; + +// signed distance function +MJAPI mjtNum mjc_distance(const mjModel* m, const mjData* d, const mjSDF* s, const mjtNum x[3]); + +// gradient of sdf +MJAPI void mjc_gradient(const mjModel* m, const mjData* d, const mjSDF* s, mjtNum gradient[3], + const mjtNum x[3]); + +// collision between a height field and a signed distance field +int mjc_HFieldSDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2, mjtNum margin); + +// collision between a mesh and a signed distance field +int mjc_MeshSDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2, mjtNum margin); + +// collision between two signed distance fields +int mjc_SDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2, mjtNum margin); + +#ifdef __cplusplus +} +#endif +#endif // MUJOCO_SRC_ENGINE_ENGINE_COLLISION_SDF_H_ diff --git a/src/engine/engine_io.c b/src/engine/engine_io.c index a5673dbf..a9d64d21 100644 --- a/src/engine/engine_io.c +++ b/src/engine/engine_io.c @@ -123,6 +123,10 @@ void mj_defaultOption(mjOption* opt) { opt->mpr_iterations = 50; opt->disableflags = 0; opt->enableflags = 0; + + // sdf collisions + opt->sdf_initpoints = 40; + opt->sdf_iterations = 10; } @@ -1716,7 +1720,7 @@ const char* mj_validateReferences(const mjModel* m) { if (m->geom_dataid[i] >= m->nhfield || m->geom_dataid[i] < -1) { return "Invalid model: geom_dataid out of bounds."; } - } else if (m->geom_type[i] == mjGEOM_MESH) { + } else if ((m->geom_type[i]==mjGEOM_MESH) || (m->geom_type[i]==mjGEOM_SDF)) { if (m->geom_dataid[i] >= m->nmesh || m->geom_dataid[i] < -1) { return "Invalid model: geom_dataid out of bounds."; } diff --git a/src/engine/engine_plugin.cc b/src/engine/engine_plugin.cc index 075aacca..1b314b5d 100644 --- a/src/engine/engine_plugin.cc +++ b/src/engine/engine_plugin.cc @@ -55,7 +55,7 @@ namespace { constexpr int kMaxNameLength = 1024; constexpr int kMaxAttributes = 255; -constexpr int kCacheLine = 64; +constexpr int kCacheLine = 256; // vfs prefix constexpr const char* kVfsPrefix = mjVFS_PREFIX; diff --git a/src/engine/engine_ray.c b/src/engine/engine_ray.c index c6f8695d..5363d68c 100644 --- a/src/engine/engine_ray.c +++ b/src/engine/engine_ray.c @@ -22,6 +22,7 @@ #include #include #include "engine/engine_io.h" +#include "engine/engine_plugin.h" #include "engine/engine_util_blas.h" #include "engine/engine_util_errmem.h" #include "engine/engine_util_misc.h" @@ -721,6 +722,67 @@ mjtNum mju_rayTree(const mjModel* m, const mjData* d, int id, const mjtNum* pnt, return x; } + + +// intersect ray with signed distance field +mjtNum ray_sdf(const mjModel* m, const mjData* d, int g, + const mjtNum* pnt, const mjtNum* vec) { + mjtNum distance_total = 0; + mjtNum p[3]; + + // exclude using bounding box + if (ray_box(d->geom_xpos+3*g, d->geom_xmat+9*g, m->geom_size+3*g, pnt, vec, NULL) < 0) { + return -1; + } + + // get sdf + int instance = m->geom_plugin[g]; + const int nslot = mjp_pluginCount(); + const int slot = m->plugin[instance]; + const mjpPlugin* sdf = mjp_getPluginAtSlotUnsafe(slot, nslot); + if (!sdf) mju_error("invalid plugin slot: %d", slot); + if (!(sdf->capabilityflags & mjPLUGIN_SDF)) { + mju_error("Plugin is not a sign distance field at slot %d", slot); + } + + // reset counter + sdf->reset(m, NULL, (void*)(d->plugin_data[instance]), instance); + + // compute transformation + mjtNum sdf_quat[4], sdf_xmat[9], sdf_xpos[9]; + mjtNum negpos[3], negquat[4], xquat[4]; + mjtNum* xpos = d->geom_xpos + 3*g; + mjtNum* pos = m->geom_pos + 3*g; + mjtNum* quat = m->geom_quat + 4*g; + mju_mat2Quat(xquat, d->geom_xmat + 9*g); + mju_negPose(negpos, negquat, pos, quat); + mju_mulPose(sdf_xpos, sdf_quat, xpos, xquat, negpos, negquat); + mju_quat2Mat(sdf_xmat, sdf_quat); + + // unit direction + mjtNum dir[3] = {vec[0], vec[1], vec[2]}; + mju_normalize3(dir); + + // ray marching + for (int i=0; i < 40; i++) { + mju_addScl3(p, pnt, dir, distance_total); + mju_subFrom3(p, sdf_xpos); + mju_rotVecMatT(p, p, sdf_xmat); + mjtNum distance = sdf->sdf_distance(p, (mjData*)d, instance); + distance_total += distance; + if (distance < 1e-8) { + return distance_total; + } + } + + // reset counter + sdf->reset(m, NULL, (void*)(d->plugin_data[instance]), instance); + + return -1; +} + + + // intersect ray with mesh mjtNum mj_rayMesh(const mjModel* m, const mjData* d, int id, const mjtNum* pnt, const mjtNum* vec) { @@ -911,6 +973,8 @@ mjtNum mj_ray(const mjModel* m, const mjData* d, const mjtNum* pnt, const mjtNum newdist = mj_rayMesh(m, d, i, pnt, vec); } else if (m->geom_type[i] == mjGEOM_HFIELD) { newdist = mj_rayHfield(m, d, i, pnt, vec); + } else if (m->geom_type[i] == mjGEOM_SDF) { + newdist = ray_sdf(m, d, i, pnt, vec); } // otherwise general dispatch @@ -1068,6 +1132,8 @@ static mjtNum mju_singleRay(const mjModel* m, mjData* d, const mjtNum pnt[3], co newdist = mj_rayMesh(m, d, i, pnt, vec); } else if (m->geom_type[i] == mjGEOM_HFIELD) { newdist = mj_rayHfield(m, d, i, pnt, vec); + } else if (m->geom_type[i] == mjGEOM_SDF) { + newdist = ray_sdf(m, d, i, pnt, vec); } // otherwise general dispatch diff --git a/src/engine/engine_vis_init.c b/src/engine/engine_vis_init.c index e0d4cdd9..b207727b 100644 --- a/src/engine/engine_vis_init.c +++ b/src/engine/engine_vis_init.c @@ -92,7 +92,8 @@ const char* mjVISSTRING[mjNVISFLAG][3] = { {"Static Bo&dy", "1", "D"}, {"Skin", "1", ";"}, {"Body Tree", "0", "`"}, - {"Mesh Tree", "0", ""} + {"Mesh Tree", "0", ""}, + {"SDF iters", "0", ""} }; diff --git a/src/engine/engine_vis_visualize.c b/src/engine/engine_vis_visualize.c index ef45a723..ccc357ca 100644 --- a/src/engine/engine_vis_visualize.c +++ b/src/engine/engine_vis_visualize.c @@ -647,6 +647,10 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, const mjtNum* xpos = d->geom_xpos + 3 * geomid; const mjtNum* xmat = d->geom_xmat + 9 * geomid; + if (!d->bvh_active[i]) { + continue; + } + rgba[0] = d->bvh_active[i] ? 1 : 0; rgba[1] = d->bvh_active[i] ? 0 : 1; @@ -1053,7 +1057,8 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, for (int k=geomadr; k < geomadr+geomnum; k++) { int geomtype = m->geom_type[k]; // add inflated geom if it is a regular primitive - if (geomtype != mjGEOM_PLANE && geomtype != mjGEOM_HFIELD && geomtype != mjGEOM_MESH) { + if (geomtype != mjGEOM_PLANE && geomtype != mjGEOM_HFIELD && + geomtype != mjGEOM_MESH && geomtype != mjGEOM_SDF) { START // inflate sizes by 5% mju_scl3(sz, m->geom_size+3*k, 1.05); @@ -1183,7 +1188,7 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, setMaterial(m, thisgeom, geom_matid, rgba, vopt->flags); // set texcoord - if (m->geom_type[i] == mjGEOM_MESH && + if ((m->geom_type[i] == mjGEOM_MESH || m->geom_type[i] == mjGEOM_SDF) && m->geom_dataid[i] >= 0 && m->mesh_texcoordadr[m->geom_dataid[i]] >= 0) { thisgeom->texcoord = 1; @@ -1205,7 +1210,7 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, } // mesh: 2*i is original, 2*i+1 is convex hull - if (m->geom_type[i] == mjGEOM_MESH) { + if (m->geom_type[i] == mjGEOM_MESH || m->geom_type[i] == mjGEOM_SDF) { thisgeom->dataid *= 2; if (m->mesh_graphadr[m->geom_dataid[i]] >= 0 && vopt->flags[mjVIS_CONVEXHULL] && (m->geom_contype[i] || m->geom_conaffinity[i])) { diff --git a/src/render/render_gl3.c b/src/render/render_gl3.c index 69a20f23..74b83c48 100644 --- a/src/render/render_gl3.c +++ b/src/render/render_gl3.c @@ -356,6 +356,7 @@ static void renderGeom(const mjvGeom* geom, int mode, const float* headpos, break; case mjGEOM_MESH: // mesh + case mjGEOM_SDF: if (geom->dataid >= 0) { glCallList(con->baseMesh + geom->dataid); } diff --git a/src/user/user_mesh.cc b/src/user/user_mesh.cc index ff36557d..6a04f4b3 100644 --- a/src/user/user_mesh.cc +++ b/src/user/user_mesh.cc @@ -27,7 +27,7 @@ #define TINYOBJLOADER_IMPLEMENTATION #endif -#include +#include #include #include "cc/array_safety.h" #include "engine/engine_crossplatform.h" @@ -256,6 +256,78 @@ void mjCMesh::set_needhull(bool needhull) { +// generate mesh using marching cubes +void mjCMesh::LoadSDF() { + if (plugin_name.empty() && plugin_instance_name.empty()) { + throw mjCError( + this, "neither 'plugin' nor 'instance' is specified for mesh '%s', (id = %d)", + name.c_str(), id); + } + + model->ResolvePlugin(this, plugin_name, plugin_instance_name, &plugin_instance); + const mjpPlugin* plugin = mjp_getPluginAtSlot(plugin_instance->plugin_slot); + if (!(plugin->capabilityflags & mjPLUGIN_SDF)) { + throw mjCError(this, "plugin '%s' does not support signed distance fields", plugin->name); + } + + int i=0; + mjtNum attributes[10] = {0}; + for (auto const& pair : plugin_instance->config_attribs) { + attributes[i++] = std::stod(pair.second); + } + + mjtNum aabb[6] = {0}; + plugin->sdf_aabb(aabb, attributes); + mjtNum total = aabb[3] + aabb[4] + aabb[5]; + + const mjtNum n = 300; + int nx, ny, nz; + nx = floor(n / total * aabb[3]) + 1; + ny = floor(n / total * aabb[4]) + 1; + nz = floor(n / total * aabb[5]) + 1; + MC::MC_FLOAT* field = new MC::MC_FLOAT[nx * ny * nz]; + + for (int i = 0; i < nx; i++) { + for (int j = 0; j < ny; j++) { + for (int k = 0; k < nz; k++) { + mjtNum point[] = {aabb[0]-aabb[3] + 2 * aabb[3] * i / (nx-1), + aabb[1]-aabb[4] + 2 * aabb[4] * j / (ny-1), + aabb[2]-aabb[5] + 2 * aabb[5] * k / (nz-1)}; + field[(k * ny + j) * nx + i] = plugin->sdf_staticdistance(point, attributes); + } + } + } + + MC::mcMesh mesh; + MC::marching_cube(field, nx, ny, nz, mesh); + std::vector uservert; + std::vector usernormal; + std::vector userface; + + for (size_t i = 0; i < mesh.vertices.size(); i++) { + uservert.push_back(2*aabb[3]*mesh.vertices.at(i).x/(nx-1) + aabb[0]-aabb[3]); + uservert.push_back(2*aabb[4]*mesh.vertices.at(i).y/(ny-1) + aabb[1]-aabb[4]); + uservert.push_back(2*aabb[5]*mesh.vertices.at(i).z/(nz-1) + aabb[2]-aabb[5]); + } + + for (size_t i = 0; i < mesh.normals.size(); i++) { + usernormal.push_back(mesh.normals.at(i).x); + usernormal.push_back(mesh.normals.at(i).y); + usernormal.push_back(mesh.normals.at(i).z); + } + + for (size_t i = 0; i < mesh.indices.size(); i++) { + userface.push_back(mesh.indices.at(i)); + } + + set_uservert(uservert); + set_usernormal(usernormal); + set_userface(userface); + delete[] field; +} + + + // compiler void mjCMesh::Compile(int vfs_provider) { // load file @@ -293,6 +365,11 @@ void mjCMesh::Compile(int vfs_provider) { } } + // create using marching cubes + if (is_plugin) { + LoadSDF(); + } + // copy user vertex if (!uservert_.empty()) { // check repeated diff --git a/src/user/user_model.cc b/src/user/user_model.cc index 269e1dc3..9a44a635 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -2743,6 +2743,14 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, int vfs_provider) { } } + for (int i = 0; i < ngeom; ++i) { + if (geoms[i]->is_plugin) { + m->geom_plugin[i] = geoms[i]->plugin_instance->id; + } else { + m->geom_plugin[i] = -1; + } + } + std::vector> plugin_to_sensors(nplugin); for (int i = 0; i < nsensor; ++i) { if (sensors[i]->type == mjSENS_PLUGIN) { diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index b55f6553..853d5a70 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -484,6 +484,12 @@ mjCBase::mjCBase() { xmlpos[0] = xmlpos[1] = -1; model = 0; def = 0; + + // plugin variables + is_plugin = false; + plugin_instance = nullptr; + plugin_name = ""; + plugin_instance_name = ""; } @@ -555,12 +561,6 @@ mjCBody::mjCBody(mjCModel* _model) { gravcomp = 0; userdata.clear(); - // plugin variables - is_plugin = false; - plugin_instance = nullptr; - plugin_name = ""; - plugin_instance_name = ""; - // clear object lists bodies.clear(); geoms.clear(); @@ -1284,6 +1284,12 @@ mjCGeom::mjCGeom(mjCModel* _model, mjCDef* _def) { // set model, def model = _model; def = (_def ? _def : (_model ? _model->defaults[0] : 0)); + + // plugin variables + is_plugin = false; + plugin_instance = nullptr; + plugin_name = ""; + plugin_instance_name = ""; } @@ -1293,7 +1299,7 @@ double mjCGeom::GetVolume(void) { double height; // get from mesh - if (type==mjGEOM_MESH) { + if (type==mjGEOM_MESH || type==mjGEOM_SDF) { if (meshid<0 || meshid>=(int)model->meshes.size()) { throw mjCError(this, "invalid meshid in mesh geom '%s' (id = %d)", name.c_str(), id); } @@ -1353,7 +1359,7 @@ void mjCGeom::SetInertia(void) { double height; // get from mesh - if (type==mjGEOM_MESH) { + if (type==mjGEOM_MESH || type==mjGEOM_SDF) { if (meshid<0 || meshid>=(int)model->meshes.size()) { throw mjCError(this, "invalid meshid in mesh geom '%s' (id = %d)", name.c_str(), id); } @@ -1440,6 +1446,7 @@ double mjCGeom::GetRBound(void) { return sqrt(size[0]*size[0]+size[1]*size[1]+size[2]*size[2]); case mjGEOM_MESH: + case mjGEOM_SDF: aabb = model->meshes[meshid]->aabb(); haabb[0] = mjMAX(fabs(aabb[0]), fabs(aabb[3])); haabb[1] = mjMAX(fabs(aabb[1]), fabs(aabb[4])); @@ -1586,6 +1593,7 @@ void mjCGeom::ComputeAABB() { break; case mjGEOM_MESH: + case mjGEOM_SDF: mjuu_copyvec(aabb, model->meshes[meshid]->aabb(), 6); break; @@ -1647,7 +1655,7 @@ void mjCGeom::Compile(void) { } // check mesh - if (type==mjGEOM_MESH && meshid<0) { + if ((type==mjGEOM_MESH || type==mjGEOM_SDF) && meshid<0) { throw mjCError(this, "mesh geom '%s' (id = %d) must have valid meshid", name.c_str(), id); } @@ -1730,7 +1738,7 @@ void mjCGeom::Compile(void) { // fit geom if type is not mjGEOM_MESH double meshpos[3]; - if (type!=mjGEOM_MESH) { + if (type!=mjGEOM_MESH && type!=mjGEOM_SDF) { pmesh->FitGeom(this, meshpos); // remove reference to mesh @@ -1752,7 +1760,7 @@ void mjCGeom::Compile(void) { size[0] = model->hfields[hfieldid]->size[0]; size[1] = model->hfields[hfieldid]->size[1]; size[2] = 0.5*(model->hfields[hfieldid]->size[2]+model->hfields[hfieldid]->size[3]); - } else if (type==mjGEOM_MESH) { + } else if (type==mjGEOM_MESH || type==mjGEOM_SDF) { const double* aabb = model->meshes[meshid]->aabb(); size[0] = mjMAX(fabs(aabb[0]), fabs(aabb[3])); size[1] = mjMAX(fabs(aabb[1]), fabs(aabb[4])); @@ -1793,6 +1801,21 @@ void mjCGeom::Compile(void) { if (fluid_switch > 0) { SetFluidCoefs(); } + + // plugin + if (is_plugin) { + if (plugin_name.empty() && plugin_instance_name.empty()) { + throw mjCError( + this, "neither 'plugin' nor 'instance' is specified for geom '%s', (id = %d)", + name.c_str(), id); + } + + model->ResolvePlugin(this, plugin_name, plugin_instance_name, &plugin_instance); + const mjpPlugin* plugin = mjp_getPluginAtSlot(plugin_instance->plugin_slot); + if (!(plugin->capabilityflags & mjPLUGIN_SDF)) { + throw mjCError(this, "plugin '%s' does not support sign distance fields", plugin->name); + } + } } diff --git a/src/user/user_objects.h b/src/user/user_objects.h index c1fcf9cb..e5bbd58a 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -610,6 +610,7 @@ class mjCMesh: public mjCBase { void LoadOBJ(mjResource* resource); // load mesh in wavefront OBJ format void LoadSTL(mjResource* resource); // load mesh in STL BIN format void LoadMSH(mjResource* resource); // load mesh in MSH BIN format + void LoadSDF(); // generate mesh using marching cubes void MakeGraph(void); // make graph of convex hull void CopyGraph(void); // copy graph into face data void MakeNormal(void); // compute vertex normals diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index b3dcd469..0d622c63 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -80,7 +80,7 @@ void ReadPluginConfigs(tinyxml2::XMLElement* elem, mjCPlugin* pp) { //---------------------------------- MJCF schema --------------------------------------------------- -static const int nMJCF = 191; +static const int nMJCF = 203; static const char* MJCF[nMJCF][mjXATTRNUM] = { {"mujoco", "!", "1", "model"}, {"<"}, @@ -95,12 +95,13 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = { "inttotal", "interval", "tolrange"}, {">"}, - {"option", "*", "22", + {"option", "*", "24", "timestep", "apirate", "impratio", "tolerance", "noslip_tolerance", "mpr_tolerance", "gravity", "wind", "magnetic", "density", "viscosity", "o_margin", "o_solref", "o_solimp", "integrator", "collision", "cone", "jacobian", - "solver", "iterations", "noslip_iterations", "mpr_iterations"}, + "solver", "iterations", "noslip_iterations", "mpr_iterations", + "sdf_iterations", "sdf_initpoints"}, {"<"}, {"flag", "?", "19", "constraint", "equality", "frictionloss", "limit", "contact", "passive", "gravity", "clampctrl", "warmstart", @@ -220,6 +221,12 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = { {"hfield", "*", "6", "name", "content_type", "file", "nrow", "ncol", "size"}, {"mesh", "*", "12", "name", "class", "content_type", "file", "vertex", "normal", "texcoord", "face", "refpos", "refquat", "scale", "smoothnormal"}, + {"<"}, + {"plugin", "*", "2", "plugin", "instance"}, + {"<"}, + {"config", "*", "2", "key", "value"}, + {">"}, + {">"}, {"skin", "*", "9", "name", "file", "material", "rgba", "inflate", "vertex", "texcoord", "face", "group"}, {"<"}, @@ -249,6 +256,12 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = { "shellinertia", "solmix", "solref", "solimp", "margin", "gap", "fromto", "pos", "quat", "axisangle", "xyaxes", "zaxis", "euler", "hfield", "mesh", "fitscale", "rgba", "fluidshape", "fluidcoef", "user"}, + {"<"}, + {"plugin", "*", "2", "plugin", "instance"}, + {"<"}, + {"config", "*", "2", "key", "value"}, + {">"}, + {">"}, {"site", "*", "15", "name", "class", "type", "group", "pos", "quat", "material", "size", "fromto", "axisangle", "xyaxes", "zaxis", "euler", "rgba", "user"}, {"camera", "*", "13", "name", "class", "fovy", "ipd", @@ -497,7 +510,8 @@ const mjMap geom_map[mjNGEOMTYPES] = { {"ellipsoid", mjGEOM_ELLIPSOID}, {"cylinder", mjGEOM_CYLINDER}, {"box", mjGEOM_BOX}, - {"mesh", mjGEOM_MESH} + {"mesh", mjGEOM_MESH}, + {"sdf", mjGEOM_SDF} }; @@ -964,6 +978,8 @@ void mjXReader::Option(XMLElement* section, mjOption* opt) { ReadAttrInt(section, "iterations", &opt->iterations); ReadAttrInt(section, "noslip_iterations", &opt->noslip_iterations); ReadAttrInt(section, "mpr_iterations", &opt->mpr_iterations); + ReadAttrInt(section, "sdf_iterations", &opt->sdf_iterations); + ReadAttrInt(section, "sdf_initpoints", &opt->sdf_initpoints); // read disable sub-element XMLElement* elem = FindSubElem(section, "flag"); @@ -1196,6 +1212,11 @@ void mjXReader::OneMesh(XMLElement* elem, mjCMesh* pmesh) { pmesh->set_refquat(ReadAttrArr(elem, "refquat")); pmesh->set_scale(ReadAttrArr(elem, "scale")); + XMLElement* eplugin = elem->FirstChildElement("plugin"); + if (eplugin) { + OnePlugin(eplugin, pmesh); + } + if (MapValue(elem, "smoothnormal", &n, bool_map, 2)) { pmesh->set_smoothnormal((n==1)); } @@ -1382,6 +1403,12 @@ void mjXReader::OneGeom(XMLElement* elem, mjCGeom* pgeom) { // read userdata ReadVector(elem, "user", pgeom->userdata, text); + // plugin sub-element + XMLElement* eplugin = elem->FirstChildElement("plugin"); + if (eplugin) { + OnePlugin(eplugin, pgeom); + } + // remaining attributes ReadAttr(elem, "mass", 1, &pgeom->_mass, text); ReadAttr(elem, "density", 1, &pgeom->density, text); @@ -2101,10 +2128,10 @@ void mjXReader::OnePlugin(XMLElement* elem, mjCBase* object) { ReadAttrTxt(elem, "instance", object->plugin_instance_name); if (object->plugin_instance_name.empty()) { object->plugin_instance = model->AddPlugin(); + ReadPluginConfigs(elem, object->plugin_instance); } else { model->hasImplicitPluginElem = true; } - ReadPluginConfigs(elem, object->plugin_instance); } diff --git a/src/xml/xml_native_writer.cc b/src/xml/xml_native_writer.cc index 46d95280..1491ef8e 100644 --- a/src/xml/xml_native_writer.cc +++ b/src/xml/xml_native_writer.cc @@ -278,7 +278,7 @@ void mjXWriter::OneGeom(XMLElement* elem, mjCGeom* pgeom, mjCDef* def) { } // mesh geom - if (pgeom->type==mjGEOM_MESH) { + if (pgeom->type==mjGEOM_MESH || pgeom->type==mjGEOM_SDF) { mjCMesh* pmesh = model->meshes[pgeom->meshid]; // write pos/quat if there is a difference @@ -337,7 +337,7 @@ void mjXWriter::OneGeom(XMLElement* elem, mjCGeom* pgeom, mjCDef* def) { if (pgeom->type==mjGEOM_HFIELD) { WriteAttrTxt(elem, "hfield", pgeom->hfield); } - if (pgeom->type==mjGEOM_MESH) { + if (pgeom->type==mjGEOM_MESH || pgeom->type==mjGEOM_SDF) { WriteAttrTxt(elem, "mesh", pgeom->mesh); } @@ -347,6 +347,11 @@ void mjXWriter::OneGeom(XMLElement* elem, mjCGeom* pgeom, mjCDef* def) { } else { WriteVector(elem, "user", pgeom->userdata, def->geom.userdata); } + + // write plugin + if (pgeom->is_plugin) { + OnePlugin(InsertEnd(elem, "plugin"), pgeom); + } } @@ -807,6 +812,8 @@ void mjXWriter::Option(XMLElement* root) { WriteAttrInt(section, "iterations", model->option.iterations, opt.iterations); WriteAttrInt(section, "noslip_iterations", model->option.noslip_iterations, opt.noslip_iterations); WriteAttrInt(section, "mpr_iterations", model->option.mpr_iterations, opt.mpr_iterations); + WriteAttrInt(section, "sdf_iterations", model->option.sdf_iterations, opt.sdf_iterations); + WriteAttrInt(section, "sdf_initpoints", model->option.sdf_initpoints, opt.sdf_initpoints); // write disable/enable flags if any of them are set; invert while writing if (model->option.disableflags || model->option.enableflags) { @@ -1307,8 +1314,14 @@ void mjXWriter::Asset(XMLElement* root) { for (int i=0; iGetObject(mjOBJ_MESH, i); - elem = InsertEnd(section, "mesh"); - OneMesh(elem, pmesh, pmesh->def); + if (pmesh->is_plugin) { + elem = InsertEnd(section, "mesh"); + WriteAttrTxt(elem, "name", pmesh->name); + OnePlugin(InsertEnd(elem, "plugin"), pmesh); + } else{ + elem = InsertEnd(section, "mesh"); + OneMesh(elem, pmesh, pmesh->def); + } } // write skins diff --git a/test/engine/CMakeLists.txt b/test/engine/CMakeLists.txt index 8678d4a1..d4dff98b 100644 --- a/test/engine/CMakeLists.txt +++ b/test/engine/CMakeLists.txt @@ -41,7 +41,12 @@ target_link_libraries( absl::str_format ) -mujoco_test(engine_plugin_test) +mujoco_test( + engine_plugin_test + PROPERTIES + ENVIRONMENT + "MUJOCO_PLUGIN_DIR=$" +) target_link_libraries( engine_plugin_test fixture diff --git a/test/engine/engine_collision_sdf_test.cc b/test/engine/engine_collision_sdf_test.cc new file mode 100644 index 00000000..e20d2fbe --- /dev/null +++ b/test/engine/engine_collision_sdf_test.cc @@ -0,0 +1,84 @@ +// Copyright 2022 DeepMind Technologies Limited +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// Tests for sdf collisions. + +#include +#include +#include +#include +#include +#include +#include +#include "src/engine/engine_collision_sdf.h" +#include "test/fixture.h" + +namespace mujoco { +namespace { + +using ::testing::NotNull; +using SdfTest = MujocoTest; + +static constexpr int kpoints = 6; +static constexpr int kgeoms = 5; +static constexpr char kSdfModel[] = R"( + + + + + + + + + + + +)"; + +TEST_F(SdfTest, SdfPrimitive) { + mjModel* model = LoadModelFromString(kSdfModel); + ASSERT_THAT(model, NotNull()); + mjData* data = mj_makeData(model); + ASSERT_THAT(data, NotNull()); + ASSERT_THAT(model->ngeom, kgeoms); + + mjSDF sdf; + const mjpPlugin* null_plugin = NULL; + mjtNum gradient[3], dist[kgeoms][kpoints] = { + {0, 0, 0, 0, 1, 1}, // plane + {-1, 0, 0, mju_sqrt(2)-1, mju_sqrt(2)-1, mju_sqrt(3)-1}, // sphere + {-.1, .9, .9, mju_sqrt(2)-.1, .9, mju_sqrt(2)-.1}, // capsule + {-1, 0, 0, mju_sqrt(2)-1, 0, mju_sqrt(2)-1}, // cylinder + {-1, 0, 0, 0, 0, 0}, // box + }; + mjtNum points[kpoints][3] = {{0, 0, 0}, {1, 0, 0}, {0, 1, 0}, + {1, 1, 0}, {0, 1, 1}, {1, 1, 1}}; + + for (int i = 0; i < kgeoms; i++) { + sdf.plugin = &null_plugin; + sdf.id = &i; + sdf.type = mjSDFTYPE_SINGLE; + sdf.geomtype = (mjtGeom*)(model->geom_type+i); + for (int j = 0; j < kpoints; j++) { + ASSERT_THAT(mjc_distance(model, data, &sdf, points[j]), dist[i][j]); + mjc_gradient(model, data, &sdf, gradient, points[j]); + } + } + + mj_deleteData(data); + mj_deleteModel(model); +} + +} // namespace +} // namespace mujoco diff --git a/test/engine/engine_plugin_test.cc b/test/engine/engine_plugin_test.cc index 73f17247..9966715d 100644 --- a/test/engine/engine_plugin_test.cc +++ b/test/engine/engine_plugin_test.cc @@ -32,9 +32,9 @@ namespace { using ::testing::HasSubstr; using ::testing::NotNull; +constexpr int kNumTruePlugins = 8; constexpr int kNumFakePlugins = 30; constexpr int kNumTestPlugins = 3; -const int kNumTruePlugins = mjp_pluginCount(); class BaseTestPlugin { public: @@ -319,10 +319,10 @@ int RegisterPassivePlugin() { return mjp_registerPlugin(&plugin); } -class PluginTest : public MujocoTest { +class EnginePluginTest : public PluginTest { public: // register all plugins - PluginTest() : MujocoTest() { + EnginePluginTest() : PluginTest() { RegisterSensorPlugin(); for (int i = 1; i <= kNumFakePlugins; ++i) { @@ -388,7 +388,11 @@ constexpr char xml[] = R"( )"; -TEST_F(PluginTest, MultiplePluginTableBlocks) { +TEST_F(PluginTest, FirstPartyPlugins) { + EXPECT_THAT(mjp_pluginCount(), kNumTruePlugins); +} + +TEST_F(EnginePluginTest, MultiplePluginTableBlocks) { EXPECT_EQ(mjp_pluginCount(), kNumTruePlugins + kNumFakePlugins + kNumTestPlugins); const mjpPlugin* last_plugin = nullptr; @@ -412,14 +416,14 @@ TEST_F(PluginTest, MultiplePluginTableBlocks) { EXPECT_LT(table_count, kNumFakePlugins); } -TEST_F(PluginTest, RegisterIdenticalPlugin) { +TEST_F(EnginePluginTest, RegisterIdenticalPlugin) { EXPECT_EQ(RegisterSensorPlugin(), kNumTruePlugins); EXPECT_EQ(RegisterActuatorPlugin(), kNumTruePlugins + kNumFakePlugins + 1); EXPECT_EQ(RegisterPassivePlugin(), kNumTruePlugins + kNumFakePlugins + 2); EXPECT_EQ(mjp_pluginCount(), kNumTruePlugins + kNumFakePlugins + kNumTestPlugins); } -TEST_F(PluginTest, SaveXml) { +TEST_F(EnginePluginTest, SaveXml) { char error[1024] = {0}; mjModel* m = LoadModelFromString(xml, error, sizeof(error)); @@ -471,7 +475,7 @@ TEST_F(PluginTest, SaveXml) { mj_deleteModel(m2); } -TEST_F(PluginTest, SensorPlugin) { +TEST_F(EnginePluginTest, SensorPlugin) { int expected_init_count = TestSensor::InitCount(); int expected_destroy_count = TestSensor::DestroyCount(); EXPECT_EQ(expected_init_count, expected_destroy_count); @@ -529,7 +533,7 @@ TEST_F(PluginTest, SensorPlugin) { EXPECT_EQ(TestSensor::DestroyCount(), expected_destroy_count); } -TEST_F(PluginTest, ActuatorPlugin) { +TEST_F(EnginePluginTest, ActuatorPlugin) { int expected_init_count = TestActuator::InitCount(); int expected_destroy_count = TestActuator::DestroyCount(); EXPECT_EQ(expected_init_count, expected_destroy_count); diff --git a/test/fixture.h b/test/fixture.h index 35cc0449..83111f1c 100644 --- a/test/fixture.h +++ b/test/fixture.h @@ -66,27 +66,18 @@ const std::string SaveAndReadXml(const mjModel* model); std::vector GetCtrlNoise(const mjModel* m, int nsteps, mjtNum ctrlnoise = 0.01); -// Installs elasticity plugins -// TODO(quaglino): load all plugins with a macro +// Installs all plugins class PluginTest : public MujocoTest { public: // load plugin library PluginTest() : MujocoTest() { - #if defined(_WIN32) || defined(__CYGWIN__) - mj_loadPluginLibrary(( - std::string(std::getenv("MUJOCO_PLUGIN_DIR")) + - std::string("\\elasticity.dll")).c_str()); - #else - #if defined(__APPLE__) - mj_loadPluginLibrary(( - std::string(std::getenv("MUJOCO_PLUGIN_DIR")) + - std::string("/libelasticity.dylib")).c_str()); - #else - mj_loadPluginLibrary(( - std::string(std::getenv("MUJOCO_PLUGIN_DIR")) + - std::string("/libelasticity.so")).c_str()); - #endif - #endif + mj_loadAllPluginLibraries( + std::string(std::getenv("MUJOCO_PLUGIN_DIR")).c_str(), +[](const char* filename, int first, int count) { + std::printf("Plugins registered by library '%s':\n", filename); + for (int i = first; i < first + count; ++i) { + std::printf(" %s\n", mjp_getPluginAtSlot(i)->name); + } + }); } }; diff --git a/test/plugin/elasticity/elasticity_test.cc b/test/plugin/elasticity/elasticity_test.cc index 2c6d8858..d6a03d2e 100644 --- a/test/plugin/elasticity/elasticity_test.cc +++ b/test/plugin/elasticity/elasticity_test.cc @@ -56,8 +56,6 @@ TEST_F(PluginTest, ElasticEnergy) { mjModel* m = LoadModelFromString(cantilever_xml, error, sizeof(error)); ASSERT_THAT(m, testing::NotNull()) << error; mjData* d = mj_makeData(m); - - EXPECT_THAT(mjp_pluginCount(), 2); auto* solid = reinterpret_cast(d->plugin_data[0]); // check that if the entire geometry is rescaled by a factor "scale", then diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index 6e0e8066..d4dce44d 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -39,7 +39,7 @@ public const double mjMAXVAL = 10000000000.0; public const double mjMINMU = 1e-05; public const double mjMINIMP = 0.0001; public const double mjMAXIMP = 0.9999; -public const int mjMAXCONPAIR = 50; +public const int mjMAXCONPAIR = 100; public const int mjMAXTREEDEPTH = 50; public const int mjMAXVFS = 2000; public const int mjMAXVFSNAME = 1000; @@ -177,7 +177,8 @@ public enum mjtGeom : int{ mjGEOM_CYLINDER = 5, mjGEOM_BOX = 6, mjGEOM_MESH = 7, - mjNGEOMTYPES = 8, + mjGEOM_SDF = 8, + mjNGEOMTYPES = 9, mjGEOM_ARROW = 100, mjGEOM_ARROW1 = 101, mjGEOM_ARROW2 = 102, @@ -374,6 +375,7 @@ public enum mjtPluginCapabilityBit : int{ mjPLUGIN_ACTUATOR = 1, mjPLUGIN_SENSOR = 2, mjPLUGIN_PASSIVE = 4, + mjPLUGIN_SDF = 8, } public enum mjtGridPos : int{ mjGRID_TOPLEFT = 0, @@ -497,7 +499,8 @@ public enum mjtVisFlag : int{ mjVIS_SKIN = 23, mjVIS_MIDPHASE = 24, mjVIS_MESHBVH = 25, - mjNVISFLAG = 26, + mjVIS_SDFITER = 26, + mjNVISFLAG = 27, } public enum mjtRndFlag : int{ mjRND_SHADOW = 0, @@ -1776,6 +1779,8 @@ public unsafe struct mjOption_ { public int mpr_iterations; public int disableflags; public int enableflags; + public int sdf_initpoints; + public int sdf_iterations; } [StructLayout(LayoutKind.Sequential)] @@ -2033,6 +2038,7 @@ public unsafe struct mjModel_ { public int* geom_matid; public int* geom_group; public int* geom_priority; + public int* geom_plugin; public byte* geom_sameframe; public double* geom_solmix; public double* geom_solref; @@ -2601,7 +2607,7 @@ public unsafe struct mjvOption_ { public fixed byte tendongroup[6]; public fixed byte actuatorgroup[6]; public fixed byte skingroup[6]; - public fixed byte flags[26]; + public fixed byte flags[27]; public int bvh_depth; }