Add Signed Distance Field to collision geometries.
PiperOrigin-RevId: 557507088 Change-Id: I358a642407aee1ba8dfc9d405eb9a4f609435fe9
This commit is contained in:
Alessio Quaglino
committed by
Copybara-Service
Copybara-Service
parent
6245edae28
commit
fdb041580c
+2
-1
@@ -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()
|
||||
|
||||
@@ -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(
|
||||
|
||||
+51
-1
@@ -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<exPlugin>`. 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 <asset-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 <mesh-plugin>` attribute. See the
|
||||
`model/plugin/sdf/ <https://github.com/deepmind/mujoco/tree/main/model/plugin/sdf>`__ directory for example models
|
||||
with SDF geometries. For more details regarding SDF plugins, see the :ref:`Extensions chapter<exWriting>`.
|
||||
|
||||
.. _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<exPlugin>`. 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:
|
||||
|
||||
|
||||
+29
-1
@@ -167,6 +167,20 @@
|
||||
| | | | :ref:`refpos<asset-mesh-refpos>` | :ref:`refquat<asset-mesh-refquat>` | :ref:`scale<asset-mesh-scale>` | :ref:`smoothnormal<asset-mesh-smoothnormal>` | |
|
||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
|
||||
| |_2| mesh |br| |_2| |L| | | .. table:: |
|
||||
| :ref:`plugin | \* | :class: mjcf-attributes |
|
||||
| <mesh-plugin>` | | |
|
||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||
| | | | :ref:`plugin<mesh-plugin-plugin>` | :ref:`instance<mesh-plugin-instance>` | | | |
|
||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
|
||||
| |_3| plugin |br| |_3| |L| | | .. table:: |
|
||||
| :ref:`config | \* | :class: mjcf-attributes |
|
||||
| <plugin-config>` | | |
|
||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||
| | | | :ref:`key<plugin-config-key>` | :ref:`value<plugin-config-value>` | | | |
|
||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
|
||||
| |_| asset |br| |_| |L| | | .. table:: |
|
||||
| :ref:`skin | \* | :class: mjcf-attributes |
|
||||
| <asset-skin>` | | |
|
||||
@@ -212,7 +226,7 @@
|
||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||
| | | | :ref:`cone<option-cone>` | :ref:`jacobian<option-jacobian>` | :ref:`solver<option-solver>` | :ref:`iterations<option-iterations>` | |
|
||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||
| | | | :ref:`noslip_iterations<option-noslip_iterations>` | :ref:`mpr_iterations<option-mpr_iterations>` | | | |
|
||||
| | | | :ref:`noslip_iterations<option-noslip_iterations>` | :ref:`mpr_iterations<option-mpr_iterations>` | :ref:`sdf_iterations<option-sdf_iterations>` | :ref:`sdf_initpoints<option-sdf_initpoints>` | |
|
||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
|
||||
| |_| option |br| |_| |L| | | .. table:: |
|
||||
@@ -313,6 +327,20 @@
|
||||
| | | | :ref:`user<body-geom-user>` | | | | |
|
||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
|
||||
| |_2| geom |br| |_2| |L| | | .. table:: |
|
||||
| :ref:`plugin | \* | :class: mjcf-attributes |
|
||||
| <geom-plugin>` | | |
|
||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||
| | | | :ref:`plugin<geom-plugin-plugin>` | :ref:`instance<geom-plugin-instance>` | | | |
|
||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
|
||||
| |_3| plugin |br| |_3| |L| | | .. table:: |
|
||||
| :ref:`config | \* | :class: mjcf-attributes |
|
||||
| <plugin-config>` | | |
|
||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||
| | | | :ref:`key<plugin-config-key>` | :ref:`value<plugin-config-value>` | | | |
|
||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
|
||||
| |_| body |br| |_| |L| | | .. table:: |
|
||||
| :ref:`site | \* | :class: mjcf-attributes |
|
||||
| <body-site>` | | |
|
||||
|
||||
+14
-3
@@ -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<exWriting>` 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<option-flag-island>`
|
||||
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<actuator-general-dyntype>`, ``filterexact``, which updates first-order filter states with
|
||||
the exact formula rather than with Euler integration.
|
||||
#. Added an actuator attribute, :ref:`actearly<actuator-general-actearly>`, which uses semi-implicit integration for
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -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 <https://github.com/deepmind/mujoco/blob/main/test/engine/engine_plugin_test.cc>`_ and the first-party
|
||||
plugins in the `first-party plugin directory <https://github.com/deepmind/mujoco/tree/main/plugin>`_.
|
||||
@@ -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/ <https://github.com/deepmind/mujoco/tree/main/plugin/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 <https://en.wikipedia.org/wiki/Hyperelastic_material#Saint_Venant%E2%80%93Kirchhoff_model>`__
|
||||
model discretized with piecewise linear finite elements, which is suitable for large deformations with small strains.
|
||||
* **sensor:** The plugins in the `sensor/ <https://github.com/deepmind/mujoco/tree/main/plugin/sensor>`__
|
||||
directory implement custom sensors. Currently the sole sensor plugin is the touch grid sensor, see the
|
||||
`README <https://github.com/deepmind/mujoco/blob/main/plugin/sensor/README.md>`__ for details.
|
||||
* **sdf:** The plugins in the `sdf/ <https://github.com/deepmind/mujoco/tree/main/plugin/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 <https://github.com/deepmind/mujoco/blob/main/src/engine/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<option-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<option-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 <https://github.com/aparis69/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
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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;
|
||||
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -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 ) \
|
||||
|
||||
+5
-2
@@ -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',
|
||||
|
||||
+18
-1
@@ -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)',
|
||||
),
|
||||
|
||||
@@ -0,0 +1,50 @@
|
||||
|
||||
<mujoco>
|
||||
<extension>
|
||||
<plugin plugin="mujoco.sdf.bowl">
|
||||
<instance name="bowl">
|
||||
<config key="height" value="0.4"/>
|
||||
<config key="radius" value="1.0"/>
|
||||
<config key="thickness" value="0.02"/>
|
||||
</instance>
|
||||
</plugin>
|
||||
</extension>
|
||||
|
||||
<asset>
|
||||
<mesh name="bowl">
|
||||
<plugin instance="bowl"/>
|
||||
</mesh>
|
||||
</asset>
|
||||
|
||||
<option sdf_iterations="5" sdf_initpoints="20"/>
|
||||
|
||||
<default>
|
||||
<geom solref="0.01 1" solimp=".95 .99 .0001" condim="1"/>
|
||||
</default>
|
||||
|
||||
<statistic meansize=".1"/>
|
||||
|
||||
<include file="scene.xml"/>
|
||||
|
||||
<worldbody>
|
||||
<light name="left" pos="0 0 4"/>
|
||||
<light name="right" pos="-1 -1 2" dir="1 1 -2"/>
|
||||
<body euler="45 -15 0" pos="0 0 1">
|
||||
<geom type="sdf" name="bowl" mesh="bowl" rgba=".2 .8 .8 1">
|
||||
<plugin instance="bowl"/>
|
||||
</geom>
|
||||
</body>
|
||||
<body pos=".6 -.4 2.5">
|
||||
<freejoint/>
|
||||
<geom type="sphere" size=".15" rgba=".8 .17 .15 1"/>
|
||||
</body>
|
||||
<body pos="0 -.4 2.5">
|
||||
<freejoint/>
|
||||
<geom type="sphere" size=".15" rgba=".8 .17 .15 1"/>
|
||||
</body>
|
||||
<body pos=".3 -.4 2.5">
|
||||
<freejoint/>
|
||||
<geom type="sphere" size=".15" rgba=".8 .17 .15 1"/>
|
||||
</body>
|
||||
</worldbody>
|
||||
</mujoco>
|
||||
@@ -0,0 +1,55 @@
|
||||
|
||||
<mujoco>
|
||||
<extension>
|
||||
<plugin plugin="mujoco.sdf.gear">
|
||||
<instance name="gear1">
|
||||
<config key="alpha" value="0"/>
|
||||
</instance>
|
||||
</plugin>
|
||||
<plugin plugin="mujoco.sdf.gear">
|
||||
<instance name="gear2">
|
||||
<config key="alpha" value="15"/>
|
||||
</instance>
|
||||
</plugin>
|
||||
</extension>
|
||||
|
||||
<asset>
|
||||
<mesh name="gear1">
|
||||
<plugin instance="gear1"/>
|
||||
</mesh>
|
||||
<mesh name="gear2">
|
||||
<plugin instance="gear2"/>
|
||||
</mesh>
|
||||
</asset>
|
||||
|
||||
<option sdf_iterations="5" sdf_initpoints="20"/>
|
||||
|
||||
<default>
|
||||
<geom solref="0.01 1" solimp=".95 .99 .0001" friction="0.2"/>
|
||||
</default>
|
||||
|
||||
<statistic meansize=".1"/>
|
||||
|
||||
<include file="scene.xml"/>
|
||||
|
||||
<worldbody>
|
||||
<body pos="0 0 .2">
|
||||
<joint name="freewheel" type="hinge" damping="1000" axis="0 0 1"/>
|
||||
<geom type="sdf" name="gear1" mesh="gear1" rgba="0.4 0.4 0.4 1">
|
||||
<plugin instance="gear1"/>
|
||||
</geom>
|
||||
</body>
|
||||
<body pos="2.85 0 .2">
|
||||
<joint name="drive" type="hinge" damping=".1" axis="0 0 1"/>
|
||||
<geom type="sdf" name="gear2" mesh="gear2" rgba="0.7 0.7 0.7 1">
|
||||
<plugin instance="gear2"/>
|
||||
</geom>
|
||||
</body>
|
||||
<light name="left" pos="0 0 1"/>
|
||||
<light name="right" pos="1 0 1"/>
|
||||
</worldbody>
|
||||
|
||||
<actuator>
|
||||
<motor name="drive" joint="drive" ctrlrange="-1 1" gear="1500" ctrllimited="true"/>
|
||||
</actuator>
|
||||
</mujoco>
|
||||
@@ -0,0 +1,35 @@
|
||||
|
||||
<mujoco>
|
||||
<extension>
|
||||
<plugin plugin="mujoco.sdf.torus">
|
||||
<instance name="torus">
|
||||
<config key="radius1" value="0.35"/>
|
||||
<config key="radius2" value="0.15"/>
|
||||
</instance>
|
||||
</plugin>
|
||||
</extension>
|
||||
|
||||
<option gravity="0 0 -9.81"/>
|
||||
<asset>
|
||||
<mesh name="torus">
|
||||
<plugin instance="torus"/>
|
||||
</mesh>
|
||||
<mesh file="../../mug/mug.obj" scale=".2 .2 .2"/>
|
||||
</asset>
|
||||
|
||||
<include file="scene.xml"/>
|
||||
|
||||
<worldbody>
|
||||
<body pos="0 .05 2.5" euler="90 0 0">
|
||||
<freejoint/>
|
||||
<geom type="sdf" mesh="torus" rgba=".8 .17 .15 1" group="1">
|
||||
<plugin instance="torus"/>
|
||||
</geom>
|
||||
</body>
|
||||
<body>
|
||||
<geom type="mesh" mesh="mug" euler="90 0 0" rgba="0 0 1 .2"/>
|
||||
</body>
|
||||
<light pos="1 0 7" dir="0 0 -1" castshadow="false"/>
|
||||
<light pos="-1 0 7" dir="0 0 -1" castshadow="false"/>
|
||||
</worldbody>
|
||||
</mujoco>
|
||||
@@ -0,0 +1,56 @@
|
||||
|
||||
<mujoco>
|
||||
<extension>
|
||||
<plugin plugin="mujoco.sdf.nut">
|
||||
<instance name="nut">
|
||||
<config key="radius" value="0.26"/>
|
||||
</instance>
|
||||
</plugin>
|
||||
<plugin plugin="mujoco.sdf.bolt">
|
||||
<instance name="bolt">
|
||||
<config key="radius" value="0.26"/>
|
||||
</instance>
|
||||
</plugin>
|
||||
</extension>
|
||||
|
||||
<compiler autolimits="true"/>
|
||||
|
||||
<include file="scene.xml"/>
|
||||
|
||||
<visual>
|
||||
<map force="0.05"/>
|
||||
</visual>
|
||||
|
||||
<asset>
|
||||
<mesh name="nut">
|
||||
<plugin instance="nut"/>
|
||||
</mesh>
|
||||
<mesh name="bolt">
|
||||
<plugin instance="bolt"/>
|
||||
</mesh>
|
||||
</asset>
|
||||
|
||||
<option sdf_iterations="15" sdf_initpoints="60"/>
|
||||
|
||||
<default>
|
||||
<geom solref="0.01 1" solimp=".95 .99 .0001" friction="0.01"/>
|
||||
</default>
|
||||
|
||||
<statistic meansize=".1"/>
|
||||
|
||||
<worldbody>
|
||||
<body pos="-0.0012496 0.00329058 0.830362" quat="-0.000212626 0.999996 -0.00200453 0.00185878">
|
||||
<joint type="free" damping="30"/>
|
||||
<geom type="sdf" name="nut" mesh="nut" rgba="0.83 0.68 0.4 1">
|
||||
<plugin instance="nut"/>
|
||||
</geom>
|
||||
</body>
|
||||
<body euler="180 0 0">
|
||||
<geom type="sdf" name="bolt" mesh="bolt" rgba="0.7 0.7 0.7 1">
|
||||
<plugin instance="bolt"/>
|
||||
</geom>
|
||||
</body>
|
||||
<light name="left" pos="-1 0 2" cutoff="80"/>
|
||||
<light name="right" pos="1 0 2" cutoff="80"/>
|
||||
</worldbody>
|
||||
</mujoco>
|
||||
@@ -0,0 +1,68 @@
|
||||
|
||||
<mujoco>
|
||||
<extension>
|
||||
<plugin plugin="mujoco.sdf.torus">
|
||||
<instance name="torus">
|
||||
<config key="radius1" value="0.35"/>
|
||||
<config key="radius2" value="0.15"/>
|
||||
</instance>
|
||||
</plugin>
|
||||
</extension>
|
||||
|
||||
<asset>
|
||||
<mesh name="torus">
|
||||
<plugin instance="torus"/>
|
||||
</mesh>
|
||||
</asset>
|
||||
|
||||
<option sdf_iterations="10" sdf_initpoints="20"/>
|
||||
|
||||
<include file="scene.xml"/>
|
||||
|
||||
<default>
|
||||
<geom solref="0.01 1" solimp=".95 .99 .0001"/>
|
||||
</default>
|
||||
|
||||
<worldbody>
|
||||
<body pos="0 0 .175">
|
||||
<freejoint/>
|
||||
<geom type="sdf" mesh="torus" rgba=".8 .2 .2 1">
|
||||
<plugin instance="torus"/>
|
||||
</geom>
|
||||
</body>
|
||||
<body pos=".1 .1 1.2">
|
||||
<freejoint/>
|
||||
<geom type="sdf" mesh="torus" rgba=".8 .2 .2 1">
|
||||
<plugin instance="torus"/>
|
||||
</geom>
|
||||
</body>
|
||||
<body pos=".1 .1 2.6">
|
||||
<freejoint/>
|
||||
<geom type="sdf" mesh="torus" rgba=".8 .2 .2 1">
|
||||
<plugin instance="torus"/>
|
||||
</geom>
|
||||
</body>
|
||||
<body pos="-.1 .2 2">
|
||||
<freejoint/>
|
||||
<geom type="sphere" size="0.3"/>
|
||||
</body>
|
||||
<body pos=".3 -.1 2" euler="0 0 45">
|
||||
<freejoint/>
|
||||
<geom type="ellipsoid" size=".4 .2 .2"/>
|
||||
</body>
|
||||
<body pos="-.1 .1 .5">
|
||||
<freejoint/>
|
||||
<geom type="capsule" size=".1" fromto="-.2 -.2 .1 .2 .2 .1"/>
|
||||
</body>
|
||||
<body pos=".2 -.2 .3">
|
||||
<freejoint/>
|
||||
<geom type="cylinder" size=".1" fromto="-.2 -.2 .1 .2 .2 .1"/>
|
||||
</body>
|
||||
<body pos=".1 .1 3.5">
|
||||
<freejoint/>
|
||||
<geom type="box" size=".2 .2 .2"/>
|
||||
</body>
|
||||
<light name="left" pos="0 0 1"/>
|
||||
<light name="right" pos="1 0 1"/>
|
||||
</worldbody>
|
||||
</mujoco>
|
||||
@@ -0,0 +1,38 @@
|
||||
<!-- 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.
|
||||
-->
|
||||
|
||||
<mujoco>
|
||||
<compiler texturedir="asset"/>
|
||||
|
||||
<statistic meansize=".05"/>
|
||||
|
||||
<visual>
|
||||
<rgba haze="0.15 0.25 0.35 1"/>
|
||||
<map stiffness="700" shadowscale="0.5" fogstart="1" fogend="15" zfar="40" haze="1" shadowclip="3"/>
|
||||
</visual>
|
||||
|
||||
<asset>
|
||||
<texture type="skybox" builtin="gradient" rgb1="0.3 0.5 0.7" rgb2="0 0 0" width="512" height="512"/>
|
||||
<texture name="texplane" type="2d" builtin="checker" rgb1=".2 .3 .4" rgb2=".1 0.15 0.2"
|
||||
width="512" height="512" mark="cross" markrgb=".8 .8 .8"/>
|
||||
|
||||
<material name="matplane" reflectance="0.3" texture="texplane" texrepeat="1 1" texuniform="true"/>
|
||||
</asset>
|
||||
|
||||
<worldbody>
|
||||
<light directional="true" diffuse=".8 .8 .8" specular="0.2 0.2 0.2" pos="0 0 4" dir="0 0 -1"/>
|
||||
<geom name="ground" type="plane" pos="0 0 -.02" size="10 10 .01" material="matplane" condim="1"/>
|
||||
</worldbody>
|
||||
</mujoco>
|
||||
@@ -0,0 +1,54 @@
|
||||
|
||||
<mujoco>
|
||||
<extension>
|
||||
<plugin plugin="mujoco.sdf.torus">
|
||||
<instance name="torus">
|
||||
<config key="radius1" value="0.35"/>
|
||||
<config key="radius2" value="0.15"/>
|
||||
</instance>
|
||||
</plugin>
|
||||
</extension>
|
||||
|
||||
<asset>
|
||||
<mesh name="torus">
|
||||
<plugin instance="torus"/>
|
||||
</mesh>
|
||||
</asset>
|
||||
|
||||
<option sdf_iterations="10" sdf_initpoints="40"/>
|
||||
|
||||
<include file="scene.xml"/>
|
||||
|
||||
<default>
|
||||
<geom solref="0.01 1" solimp=".95 .99 .0001" friction="0.1"/>
|
||||
</default>
|
||||
|
||||
<worldbody>
|
||||
<body pos="-1 0 3.8">
|
||||
<freejoint/>
|
||||
<geom type="sdf" mesh="torus" rgba=".2 .2 .8 1">
|
||||
<plugin instance="torus"/>
|
||||
</geom>
|
||||
</body>
|
||||
<body pos="-1 0 3.4">
|
||||
<freejoint/>
|
||||
<geom type="sdf" mesh="torus" rgba=".2 .8 .2 1">
|
||||
<plugin instance="torus"/>
|
||||
</geom>
|
||||
</body>
|
||||
<body pos="-1 0 3">
|
||||
<freejoint/>
|
||||
<geom type="sdf" mesh="torus" rgba=".8 .2 .2 1">
|
||||
<plugin instance="torus"/>
|
||||
</geom>
|
||||
</body>
|
||||
<body pos="0 0 2">
|
||||
<geom type="cylinder" size=".1" fromto="1 0 -1 -1 0 1"/>
|
||||
</body>
|
||||
<body pos="0 0 2">
|
||||
<geom type="cylinder" size=".5" fromto="1.2 0 -1.2 1 0 -1"/>
|
||||
</body>
|
||||
<light name="left" pos="-2 0 7" cutoff="80"/>
|
||||
<light name="right" pos="2 0 7" cutoff="80"/>
|
||||
</worldbody>
|
||||
</mujoco>
|
||||
@@ -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}
|
||||
)
|
||||
@@ -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 <cmath>
|
||||
#include <cstdlib>
|
||||
#include <optional>
|
||||
|
||||
#include <mujoco/mjplugin.h>
|
||||
#include <mujoco/mjtnum.h>
|
||||
#include <mujoco/mujoco.h>
|
||||
#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> 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<uintptr_t>(
|
||||
new Bolt(std::move(*sdf_or_null)));
|
||||
return 0;
|
||||
};
|
||||
plugin.destroy = +[](mjData* d, int instance) {
|
||||
delete reinterpret_cast<Bolt*>(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<Bolt*>(plugin_data);
|
||||
sdf->Reset();
|
||||
};
|
||||
plugin.visualize = +[](const mjModel* m, mjData* d, const mjvOption* opt,
|
||||
mjvScene* scn, int instance) {
|
||||
auto* sdf = reinterpret_cast<Bolt*>(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<Bolt*>(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<Bolt*>(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<Bolt*>(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
|
||||
@@ -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 <optional>
|
||||
|
||||
#include <mujoco/mjdata.h>
|
||||
#include <mujoco/mjmodel.h>
|
||||
#include <mujoco/mjtnum.h>
|
||||
#include <mujoco/mjvisualize.h>
|
||||
#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<Bolt> 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_
|
||||
@@ -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 <sstream>
|
||||
#include <optional>
|
||||
|
||||
#include <mujoco/mjplugin.h>
|
||||
#include <mujoco/mjtnum.h>
|
||||
#include <mujoco/mujoco.h>
|
||||
#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> 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<uintptr_t>(
|
||||
new Bowl(std::move(*sdf_or_null)));
|
||||
return 0;
|
||||
};
|
||||
plugin.destroy = +[](mjData* d, int instance) {
|
||||
delete reinterpret_cast<Bowl*>(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<Bowl*>(plugin_data);
|
||||
sdf->Reset();
|
||||
};
|
||||
plugin.visualize = +[](const mjModel* m, mjData* d, const mjvOption* opt,
|
||||
mjvScene* scn, int instance) {
|
||||
auto* sdf = reinterpret_cast<Bowl*>(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<Bowl*>(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<Bowl*>(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<Bowl*>(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
|
||||
@@ -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 <optional>
|
||||
|
||||
#include <mujoco/mjdata.h>
|
||||
#include <mujoco/mjmodel.h>
|
||||
#include <mujoco/mjtnum.h>
|
||||
#include <mujoco/mjvisualize.h>
|
||||
#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<Bowl> 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_
|
||||
@@ -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 <cstdio>
|
||||
#include <sstream>
|
||||
#include <optional>
|
||||
|
||||
#include <mujoco/mjplugin.h>
|
||||
#include <mujoco/mjtnum.h>
|
||||
#include <mujoco/mjvisualize.h>
|
||||
#include <mujoco/mujoco.h>
|
||||
#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> 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<uintptr_t>(
|
||||
new Gear(std::move(*sdf_or_null)));
|
||||
return 0;
|
||||
};
|
||||
plugin.destroy = +[](mjData* d, int instance) {
|
||||
delete reinterpret_cast<Gear*>(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<Gear*>(plugin_data);
|
||||
sdf->Reset();
|
||||
};
|
||||
plugin.visualize = +[](const mjModel* m, mjData* d, const mjvOption* opt,
|
||||
mjvScene* scn, int instance) {
|
||||
auto* sdf = reinterpret_cast<Gear*>(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<Gear*>(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<Gear*>(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<Gear*>(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
|
||||
@@ -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 <optional>
|
||||
#include <vector>
|
||||
|
||||
#include <mujoco/mjdata.h>
|
||||
#include <mujoco/mjmodel.h>
|
||||
#include <mujoco/mjtnum.h>
|
||||
#include <mujoco/mjvisualize.h>
|
||||
#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<Gear> 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_
|
||||
@@ -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 <cmath>
|
||||
#include <sstream>
|
||||
#include <optional>
|
||||
|
||||
#include <mujoco/mjplugin.h>
|
||||
#include <mujoco/mjtnum.h>
|
||||
#include <mujoco/mujoco.h>
|
||||
#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> 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<uintptr_t>(
|
||||
new Nut(std::move(*sdf_or_null)));
|
||||
return 0;
|
||||
};
|
||||
plugin.destroy = +[](mjData* d, int instance) {
|
||||
delete reinterpret_cast<Nut*>(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<Nut*>(plugin_data);
|
||||
sdf->Reset();
|
||||
};
|
||||
plugin.visualize = +[](const mjModel* m, mjData* d, const mjvOption* opt,
|
||||
mjvScene* scn, int instance) {
|
||||
auto* sdf = reinterpret_cast<Nut*>(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<Nut*>(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<Nut*>(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<Nut*>(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
|
||||
@@ -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 <optional>
|
||||
|
||||
#include <mujoco/mjdata.h>
|
||||
#include <mujoco/mjmodel.h>
|
||||
#include <mujoco/mjtnum.h>
|
||||
#include <mujoco/mjvisualize.h>
|
||||
#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<Nut> 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_
|
||||
@@ -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
|
||||
@@ -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 <cstddef>
|
||||
#include <string>
|
||||
|
||||
#include <mujoco/mjplugin.h>
|
||||
#include <mujoco/mujoco.h>
|
||||
#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<float>(j > 0), 0,
|
||||
static_cast<float>(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
|
||||
@@ -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 <vector>
|
||||
|
||||
#include <mujoco/mujoco.h>
|
||||
|
||||
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<mjtNum> points_; // query points
|
||||
std::vector<int> npoints_; // number of iterations from the starting point
|
||||
};
|
||||
|
||||
} // namespace mujoco::plugin::sdf
|
||||
|
||||
#endif // MUJOCO_PLUGIN_SDF_SDF_H_
|
||||
@@ -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 <sstream>
|
||||
#include <optional>
|
||||
|
||||
#include <mujoco/mjplugin.h>
|
||||
#include <mujoco/mjtnum.h>
|
||||
#include <mujoco/mujoco.h>
|
||||
#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> 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<uintptr_t>(
|
||||
new Torus(std::move(*sdf_or_null)));
|
||||
return 0;
|
||||
};
|
||||
plugin.destroy = +[](mjData* d, int instance) {
|
||||
delete reinterpret_cast<Torus*>(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<Torus*>(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<Torus*>(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
|
||||
@@ -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 <optional>
|
||||
|
||||
#include <mujoco/mjdata.h>
|
||||
#include <mujoco/mjmodel.h>
|
||||
#include <mujoco/mjtnum.h>
|
||||
#include "sdf.h"
|
||||
|
||||
namespace mujoco::plugin::sdf {
|
||||
|
||||
class Torus {
|
||||
public:
|
||||
// Creates a new Torus instance or returns null on failure.
|
||||
static std::optional<Torus> 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_
|
||||
@@ -27,6 +27,7 @@
|
||||
|
||||
#include "lodepng.h"
|
||||
#include <mujoco/mjdata.h>
|
||||
#include <mujoco/mjui.h>
|
||||
#include <mujoco/mjvisualize.h>
|
||||
#include <mujoco/mjxmacro.h>
|
||||
#include <mujoco/mujoco.h>
|
||||
@@ -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
|
||||
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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}
|
||||
};
|
||||
|
||||
|
||||
|
||||
@@ -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 <stdio.h>
|
||||
|
||||
#include <mujoco/mjdata.h>
|
||||
#include <mujoco/mjmodel.h>
|
||||
#include <mujoco/mjtnum.h>
|
||||
#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; step<m->opt.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; i<ncorners; i++) {
|
||||
// compute sdf
|
||||
fun = mju_dot3(corners + 3*i, grad);
|
||||
|
||||
// save argmin
|
||||
if (fun<best) {
|
||||
best = fun;
|
||||
mju_copy3(s, corners + 3*i);
|
||||
}
|
||||
}
|
||||
|
||||
// update collision point
|
||||
mju_subFrom3(s, x);
|
||||
mju_addToScl3(x, s, 2. / (step+2.));
|
||||
}
|
||||
|
||||
// compute distance
|
||||
return mjc_distance(m, d, sdf, x);
|
||||
}
|
||||
|
||||
// finds minimum using gradient descent
|
||||
static mjtNum stepGradient(mjtNum x[3], const mjModel* m, const mjSDF* s,
|
||||
mjData* d) {
|
||||
mjtNum alpha = 0.2; // step along the gradient direction
|
||||
|
||||
for (int step=0; step<m->opt.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)<r;
|
||||
}
|
||||
|
||||
// intersect with circumsphere of bounding box
|
||||
static int boxIntersect(const mjtNum bvh[6], const mjtNum offset[3],
|
||||
const mjtNum rotation[9], const mjModel* m,
|
||||
const mjSDF* s, mjData* d) {
|
||||
mjtNum candidate[3];
|
||||
mjtNum r = mju_norm3(bvh+3);
|
||||
|
||||
mju_rotVecMat(candidate, bvh, rotation);
|
||||
mju_addTo3(candidate, offset);
|
||||
|
||||
// check if inside the bounding box
|
||||
return mjc_distance(m, d, s, candidate)<r;
|
||||
}
|
||||
|
||||
//---------------------------- mesh vs sdf broad phase --------------------------------------------
|
||||
|
||||
// tree vs sdf binary search
|
||||
static void collideBVH(const mjModel* m, mjData* d, int g,
|
||||
const mjtNum offset[3], const mjtNum rotation[9],
|
||||
int* faces, int* npoints, int* n0,
|
||||
const mjSDF* sdf) {
|
||||
const int bvhadr = m->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; i<npoints; i++) {
|
||||
int face = faceadr + faces[i];
|
||||
for (int v=0; v<3; v++) {
|
||||
mjtNum vec[3] = {
|
||||
m->mesh_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]<aabb[0] || aabb[4]<aabb[1] || aabb[5]<aabb[2]) {
|
||||
return cnt;
|
||||
}
|
||||
|
||||
// create sdf pointers
|
||||
int instance[2];
|
||||
const mjpPlugin* sdf_ptr[2];
|
||||
mjtGeom geomtypes[2] = {m->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 (i<m->opt.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;
|
||||
}
|
||||
|
||||
@@ -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 <mujoco/mjdata.h>
|
||||
#include <mujoco/mjexport.h>
|
||||
#include <mujoco/mjmodel.h>
|
||||
#include <mujoco/mjplugin.h>
|
||||
|
||||
#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_
|
||||
@@ -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.";
|
||||
}
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -22,6 +22,7 @@
|
||||
#include <mujoco/mjmodel.h>
|
||||
#include <mujoco/mjvisualize.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"
|
||||
@@ -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
|
||||
|
||||
@@ -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", ""}
|
||||
};
|
||||
|
||||
|
||||
|
||||
@@ -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])) {
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
|
||||
+78
-1
@@ -27,7 +27,7 @@
|
||||
#define TINYOBJLOADER_IMPLEMENTATION
|
||||
#endif
|
||||
|
||||
#include <mujoco/mjmacro.h>
|
||||
#include <MC.h>
|
||||
#include <mujoco/mjmodel.h>
|
||||
#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<float> uservert;
|
||||
std::vector<float> usernormal;
|
||||
std::vector<int> 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
|
||||
|
||||
@@ -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<std::vector<int>> plugin_to_sensors(nplugin);
|
||||
for (int i = 0; i < nsensor; ++i) {
|
||||
if (sensors[i]->type == mjSENS_PLUGIN) {
|
||||
|
||||
+34
-11
@@ -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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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<double, 4>(elem, "refquat"));
|
||||
pmesh->set_scale(ReadAttrArr<double, 3>(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);
|
||||
}
|
||||
|
||||
|
||||
|
||||
@@ -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; i<nmesh; i++) {
|
||||
// create element and write
|
||||
mjCMesh* pmesh = (mjCMesh*)model->GetObject(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
|
||||
|
||||
@@ -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_FILE_DIR:elasticity>"
|
||||
)
|
||||
target_link_libraries(
|
||||
engine_plugin_test
|
||||
fixture
|
||||
|
||||
@@ -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 <cstdio>
|
||||
#include <gmock/gmock.h>
|
||||
#include <gtest/gtest.h>
|
||||
#include <mujoco/mjdata.h>
|
||||
#include <mujoco/mjmodel.h>
|
||||
#include <mujoco/mjtnum.h>
|
||||
#include <mujoco/mujoco.h>
|
||||
#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"(
|
||||
<mujoco>
|
||||
<worldbody>
|
||||
<geom type="plane" size="5 5 .1" pos="0 0 -1"/>
|
||||
<body pos="-.1 .2 2" euler="0 0 45">
|
||||
<geom type="sphere" size="1"/>
|
||||
<geom type="capsule" size=".1" fromto="-2 -2 1 2 2 1"/>
|
||||
<geom type="cylinder" size="1" fromto="0 0 0 2 0 0"/>
|
||||
<geom type="box" size="1 1 1"/>
|
||||
</body>
|
||||
</worldbody>
|
||||
</mujoco>
|
||||
)";
|
||||
|
||||
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
|
||||
@@ -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"(
|
||||
</mujoco>
|
||||
)";
|
||||
|
||||
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);
|
||||
|
||||
+8
-17
@@ -66,27 +66,18 @@ const std::string SaveAndReadXml(const mjModel* model);
|
||||
std::vector<mjtNum> 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);
|
||||
}
|
||||
});
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
@@ -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<plugin::elasticity::Solid*>(d->plugin_data[0]);
|
||||
|
||||
// check that if the entire geometry is rescaled by a factor "scale", then
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user