From 50843cad300f2069053859139c9b699c6c3c40f0 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Mon, 30 Oct 2023 03:08:41 -0700 Subject: [PATCH] Add defaults to SDF plugin attributes. Expose diameter and number of teeth for the gear SDF. PiperOrigin-RevId: 577779297 Change-Id: Iec5890ab7f39080b395cf06a76f9c2216f4f39ed --- doc/includes/references.h | 3 + doc/programming/extension.rst | 26 +++++---- include/mujoco/mjplugin.h | 4 ++ plugin/elasticity/README.md | 2 +- plugin/sdf/README.md | 100 ++++++++++++++++++++++++++++++++++ plugin/sdf/bolt.cc | 30 +++++++--- plugin/sdf/bolt.h | 8 ++- plugin/sdf/bowl.cc | 39 ++++++++----- plugin/sdf/bowl.h | 11 +++- plugin/sdf/gear.cc | 66 +++++++++++++--------- plugin/sdf/gear.h | 9 ++- plugin/sdf/nut.cc | 33 +++++++---- plugin/sdf/nut.h | 8 ++- plugin/sdf/sdf.cc | 4 +- plugin/sdf/sdf.h | 40 ++++++++++++++ plugin/sdf/torus.cc | 29 +++++++--- plugin/sdf/torus.h | 8 ++- src/user/user_mesh.cc | 18 +++--- 18 files changed, 341 insertions(+), 97 deletions(-) create mode 100644 plugin/sdf/README.md diff --git a/doc/includes/references.h b/doc/includes/references.h index 3284ff9f..79f5438e 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -1432,6 +1432,9 @@ struct mjpPlugin_ { // called during compilation for marching cubes mjtNum (*sdf_staticdistance)(const mjtNum point[3], const mjtNum* attributes); + // convert attributes and provide defaults if not present + void (*sdf_attribute)(mjtNum attribute[], const char* name[], const char* value[]); + // bounding box of implicit surface void (*sdf_aabb)(mjtNum aabb[6], const mjtNum* attributes); }; diff --git a/doc/programming/extension.rst b/doc/programming/extension.rst index 5d8167e3..f41c9629 100644 --- a/doc/programming/extension.rst +++ b/doc/programming/extension.rst @@ -250,20 +250,24 @@ A future version of this section will include: Currently, there are three directories of first-party plugins: -* **elasticity:** The plugins in the `elasticity/ `__ - directory are passive forces based on continuum mechanics for 1-dimensional and 3-dimensional bodies. The - 1D model is invariant under rotations and captures the large deformation of elastic cables, decoupling twisting and - bending strains. The 3D solid is a - `Saint Venant-Kirchhoff `__ - model discretized with piecewise linear finite elements, which is suitable for large deformations with small strains. - See also :ref:`composite ` and :ref:`deformable ` objects. +* **elasticity:** The plugins in the `elasticity/ + `__ directory are passive forces based on + continuum mechanics for 1-dimensional and 3-dimensional bodies. The 1D model is invariant under rotations and captures + the large deformation of elastic cables, decoupling twisting and bending strains. The 3D solid is a `Saint + Venant-Kirchhoff `__ model + discretized with piecewise linear finite elements, which is suitable for large deformations with small strains. See + also :ref:`composite ` and :ref:`deformable ` objects. For more information, please see the + `README `__. * **sensor:** The plugins in the `sensor/ `__ directory implement custom sensors. Currently the sole sensor plugin is the touch grid sensor, see the `README `__ for details. -* **sdf:** The plugins in the `sdf/ `__ - directory specify custom shapes in a mesh-free manner, by defining methods computing a signed distance field and its - gradient at query points. This shape then acts as a new geom type in the collision table at the top of - `engine_collision_driver.c `__. +* **sdf:** The plugins in the `sdf/ `__ directory + specify custom shapes in a mesh-free manner, by defining methods computing a signed distance field and its gradient at + query points. This shape then acts as a new geom type in the collision table at the top of `engine_collision_driver.c + `__. For more information + concerning the available SDFs and how to write your own implicit geometry, please see the `README + `__. The rest of this section will give more + detail concerning the collision algorithm and the plugin engine interface. 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. diff --git a/include/mujoco/mjplugin.h b/include/mujoco/mjplugin.h index 2c071195..6f583c32 100644 --- a/include/mujoco/mjplugin.h +++ b/include/mujoco/mjplugin.h @@ -17,6 +17,7 @@ #include #include +#include #include @@ -117,6 +118,9 @@ struct mjpPlugin_ { // called during compilation for marching cubes mjtNum (*sdf_staticdistance)(const mjtNum point[3], const mjtNum* attributes); + // convert attributes and provide defaults if not present + void (*sdf_attribute)(mjtNum attribute[], const char* name[], const char* value[]); + // bounding box of implicit surface void (*sdf_aabb)(mjtNum aabb[6], const mjtNum* attributes); }; diff --git a/plugin/elasticity/README.md b/plugin/elasticity/README.md index eba2440b..ade722a6 100644 --- a/plugin/elasticity/README.md +++ b/plugin/elasticity/README.md @@ -1,5 +1,5 @@

- MuJoCo + MuJoCo

## Elasticity plugins diff --git a/plugin/sdf/README.md b/plugin/sdf/README.md new file mode 100644 index 00000000..a51d84bb --- /dev/null +++ b/plugin/sdf/README.md @@ -0,0 +1,100 @@ +

+ MuJoCo +

+ +## Signed distance function (SDF) plugins + +These are first-party plugins that implement implicit geometries using SDFs. They can be applied to **geoms** and +**meshes** (in the **asset** section). Sample models can be found in [this folder](../../model/plugin/sdf/). + +### Bolt + +Implemented in [bolt.cc](bolt.cc). Example usage in [nutbolt.xml](../../model/plugin/sdf/nutbolt.xml). + +This plugin implements a bolt with a hexagonal head, similar to https://www.shadertoy.com/view/XtffzX. + +Parameters: + + - `radius` [m]: bolt radius (default `0.26`). + +### Bowl + +Implemented in [bowl.cc](bowl.cc). Example usage in [bowl.xml](../../model/plugin/sdf/bowl.xml). + +The plugin implements a cut hollow sphere from https://www.shadertoy.com/view/7tVXRt. + +Parameters: + + - `height` [m]: location of the cut plane (default `0.4`). + - `radius` [m]: radius of the sphere (default `1`). + - `thickness` [m]: thickness of the bowl (default `0.02`). + +### Gear + +Implemented in [gear.cc](gear.cc). Example usage in [gear.xml](../../model/plugin/sdf/gear.xml). + +The plugin implements a 3D extrusion of the 2D gear geometry from https://www.shadertoy.com/view/3lG3WR. + +Parameters: + + - `alpha` [m]: initial angle of rotation of the gear (default `0`). + - `diameter` [m]: gear diameter (default `2.8`). + - `teeth` []: number of teeth (default `25`). + +### Nut + +Implemented in [nut.cc](nut.cc). Example usage in [nutbolt.xml](../../model/plugin/sdf/nutbolt.xml). + +This plugin implements a hexagonal nut identical to the bolt head from https://www.shadertoy.com/view/XtffzX. + +Parameters: + + - `radius` [m]: nut radius (default `0.26`). + +### Torus + +Implemented in [torus.cc](torus.cc). Example usage in [torus.xml](../../model/plugin/sdf/torus.xml). + +This plugin implements a torus. + +Parameters: + + - `radius1` [m]: major radius (default `0.35`). + - `radius1` [m]: minor radius (default `0.15`). + +### How to make your own SDF + +Create your `MySDF.h` and `MySDF.cc` files in the SDF folder, where this README is located. Implement your SDF using the +following interface: + +``` +struct MySDFAttribute { + static constexpr int nattribute = + /* insert the number of attributes */; + static constexpr char const* names[nattribute] = + /* an array of attributes with the same order as the attribute array in your SDF class */; + static constexpr mjtNum defaults[nattribute] = + /* an array of default values for your attributes */; +}; + +class MySDF { + public: + // creates a new MySDF instance or returns null on failure. + static std::optional Create(const mjModel* m, mjData* d, int instance); + MySDF(MySDF&&) = default; + ~MySDF() = default; + + // functions that return the SDF and its gradient at a query point + mjtNum Distance(const mjtNum point[3]) const; + void Gradient(mjtNum grad[3], const mjtNum point[3]) const; + + // a call to this needs to be added to register.cc + static void RegisterPlugin(); + + // an array of attributes with the same order as in the struct above + mjtNum attribute[MySDFAttribute::nattribute]; + + private: + Torus(const mjModel* m, mjData* d, int instance); +}; +``` diff --git a/plugin/sdf/bolt.cc b/plugin/sdf/bolt.cc index 39cf9ec0..99264d23 100644 --- a/plugin/sdf/bolt.cc +++ b/plugin/sdf/bolt.cc @@ -13,8 +13,10 @@ // limitations under the License. #include +#include #include #include +#include #include #include @@ -75,7 +77,13 @@ std::optional Bolt::Create( // plugin constructor Bolt::Bolt(const mjModel* m, mjData* d, int instance) { - radius = strtod(mj_getPluginConfig(m, instance, "radius"), nullptr); + SdfDefault defattribute; + + for (int i=0; i < BoltAttribute::nattribute; i++) { + attribute[i] = defattribute.GetDefault( + BoltAttribute::names[i], + mj_getPluginConfig(m, instance, BoltAttribute::names[i])); + } } // add new element in the vector storing iteration counts @@ -96,19 +104,19 @@ void Bolt::Visualize(const mjModel* m, mjData* d, const mjvOption* opt, // sdf mjtNum Bolt::Distance(const mjtNum point[3]) const { - return distance(point, &radius); + return distance(point, attribute); } // gradient of sdf void Bolt::Gradient(mjtNum grad[3], const mjtNum point[3]) const { mjtNum eps = 1e-8; - mjtNum dist0 = distance(point, &radius); + mjtNum dist0 = distance(point, attribute); mjtNum pointX[3] = {point[0]+eps, point[1], point[2]}; - mjtNum distX = distance(pointX, &radius); + mjtNum distX = distance(pointX, attribute); mjtNum pointY[3] = {point[0], point[1]+eps, point[2]}; - mjtNum distY = distance(pointY, &radius); + mjtNum distY = distance(pointY, attribute); mjtNum pointZ[3] = {point[0], point[1], point[2]+eps}; - mjtNum distZ = distance(pointZ, &radius); + mjtNum distZ = distance(pointZ, attribute); grad[0] = (distX - dist0) / eps; grad[1] = (distY - dist0) / eps; @@ -123,9 +131,8 @@ void Bolt::RegisterPlugin() { plugin.name = "mujoco.sdf.bolt"; plugin.capabilityflags |= mjPLUGIN_SDF; - const char* attributes[] = {"radius"}; - plugin.nattribute = sizeof(attributes) / sizeof(attributes[0]); - plugin.attributes = attributes; + plugin.nattribute = BoltAttribute::nattribute; + plugin.attributes = BoltAttribute::names; plugin.nstate = +[](const mjModel* m, int instance) { return 0; }; plugin.init = +[](const mjModel* m, mjData* d, int instance) { @@ -177,6 +184,11 @@ void Bolt::RegisterPlugin() { aabb[3] = aabb[4] = .6; aabb[5] = 1; }; + plugin.sdf_attribute = + +[](mjtNum attribute[], const char* name[], const char* value[]) { + SdfDefault defattribute; + defattribute.GetDefaults(attribute, name, value); + }; mjp_registerPlugin(&plugin); } diff --git a/plugin/sdf/bolt.h b/plugin/sdf/bolt.h index 37a5fa5b..af04ba9a 100644 --- a/plugin/sdf/bolt.h +++ b/plugin/sdf/bolt.h @@ -28,6 +28,12 @@ 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 +struct BoltAttribute { + static constexpr int nattribute = 1; + static constexpr char const* names[nattribute] = {"radius"}; + static constexpr mjtNum defaults[nattribute] = {0.26}; +}; + class Bolt { public: // Creates a new Bolt instance (allocated with `new`) or @@ -45,7 +51,7 @@ class Bolt { static void RegisterPlugin(); - mjtNum radius; + mjtNum attribute[BoltAttribute::nattribute]; private: Bolt(const mjModel* m, mjData* d, int instance); diff --git a/plugin/sdf/bowl.cc b/plugin/sdf/bowl.cc index f5130527..451b2b4c 100644 --- a/plugin/sdf/bowl.cc +++ b/plugin/sdf/bowl.cc @@ -12,12 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include +#include #include #include #include +#include "sdf.h" #include "bowl.h" namespace mujoco::plugin::sdf { @@ -51,9 +53,16 @@ std::optional Bowl::Create( // 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); + SdfDefault defattribute; + + for (int i=0; i < BowlAttribute::nattribute; i++) { + attribute[i] = defattribute.GetDefault( + BowlAttribute::names[i], + mj_getPluginConfig(m, instance, BowlAttribute::names[i])); + } + + mjtNum height = attribute[0]; + mjtNum radius = attribute[1]; width = mju_sqrt(radius*radius - height*height); } @@ -75,8 +84,7 @@ void Bowl::Visualize(const mjModel* m, mjData* d, const mjvOption* opt, mjvScene // sdf mjtNum Bowl::Distance(const mjtNum point[3]) const { - mjtNum attributes[3]= {height, radius, thick}; - return distance(point, attributes); + return distance(point, attribute); } // gradient of sdf @@ -98,16 +106,15 @@ void Bowl::Gradient(mjtNum grad[3], const mjtNum point[3]) const { // 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 dist0 = distance(point, attribute); mjtNum pointX[3] = {point[0]+eps, point[1], point[2]}; - mjtNum distX = distance(pointX, attributes); + mjtNum distX = distance(pointX, attribute); mjtNum pointY[3] = {point[0], point[1]+eps, point[2]}; - mjtNum distY = distance(pointY, attributes); + mjtNum distY = distance(pointY, attribute); mjtNum pointZ[3] = {point[0], point[1], point[2]+eps}; - mjtNum distZ = distance(pointZ, attributes); + mjtNum distZ = distance(pointZ, attribute); grad[0] = (distX - dist0) / eps; grad[1] = (distY - dist0) / eps; @@ -122,9 +129,8 @@ void Bowl::RegisterPlugin() { 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.nattribute = BowlAttribute::nattribute; + plugin.attributes = BowlAttribute::names; plugin.nstate = +[](const mjModel* m, int instance) { return 0; }; plugin.init = +[](const mjModel* m, mjData* d, int instance) { @@ -177,6 +183,11 @@ void Bowl::RegisterPlugin() { aabb[0] = aabb[1] = aabb[2] = 0; aabb[3] = aabb[4] = aabb[5] = radius + thick; }; + plugin.sdf_attribute = + +[](mjtNum attribute[], const char* name[], const char* value[]) { + SdfDefault defattribute; + defattribute.GetDefaults(attribute, name, value); + }; mjp_registerPlugin(&plugin); } diff --git a/plugin/sdf/bowl.h b/plugin/sdf/bowl.h index 66210885..471f685c 100644 --- a/plugin/sdf/bowl.h +++ b/plugin/sdf/bowl.h @@ -25,6 +25,13 @@ namespace mujoco::plugin::sdf { +struct BowlAttribute { + static constexpr int nattribute = 3; + static constexpr char const* names[nattribute] = {"height", "radius", + "thickness"}; + static constexpr mjtNum defaults[nattribute] = {.4, 1., .02}; +}; + class Bowl { public: // Creates a new Bowl instance (allocated with `new`) or @@ -42,9 +49,7 @@ class Bowl { static void RegisterPlugin(); - mjtNum radius; - mjtNum height; - mjtNum thick; + mjtNum attribute[BowlAttribute::nattribute]; mjtNum width; private: diff --git a/plugin/sdf/gear.cc b/plugin/sdf/gear.cc index b9f42b83..0546b1d0 100644 --- a/plugin/sdf/gear.cc +++ b/plugin/sdf/gear.cc @@ -12,14 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include +#include +#include #include +#include #include #include #include #include +#include "sdf.h" #include "gear.h" namespace mujoco::plugin::sdf { @@ -50,10 +52,11 @@ static mjtNum mod(mjtNum x, mjtNum y) { return x - y * floor(x/y); } -static mjtNum distance2D(const mjtNum p[3], const mjtNum attributes[3]) { +static mjtNum distance2D(const mjtNum p[3], + const mjtNum attributes[GearAttribute::nattribute]) { // see https://www.shadertoy.com/view/3lG3WR - mjtNum D = 2.8; // should be an attribute - mjtNum N = 25; // should be an attribute + mjtNum D = attributes[1]; + mjtNum N = attributes[2]; mjtNum psi = 3.096e-5 * N * N -6.557e-3 * N + 0.551; // pressure angle mjtNum alpha = attributes[0]; @@ -78,7 +81,7 @@ static mjtNum distance2D(const mjtNum p[3], const mjtNum attributes[3]) { mjtNum h = 2.2 / Pd; - mjtNum innerR = Ro - h - 0.4; + mjtNum innerR = Ro - h - 0.14*D; // Early exit if (innerR - rho > 0.0) @@ -126,15 +129,16 @@ static mjtNum distance2D(const mjtNum p[3], const mjtNum attributes[3]) { fib - (alphaStride - shift)); cogs = Intersection(baseWalls, cogs); - cogs = smoothIntersection(gearOuter, cogs, 0.01); + cogs = smoothIntersection(gearOuter, cogs, 0.0035*D); cogs = smoothUnion(gearLowBase, cogs, Rb - Ro + h); cogs = Subtraction(cogs, crownBase); - return extrusion(p, cogs, .1); + return cogs; } -static mjtNum distance(const mjtNum p[3], const mjtNum attributes[3]) { - return extrusion(p, distance2D(p, attributes), .1) - .005; +static mjtNum distance(const mjtNum p[3], + const mjtNum attributes[GearAttribute::nattribute]) { + return extrusion(p, distance2D(p, attributes), attributes[3]/2.); } } // namespace @@ -142,17 +146,24 @@ static mjtNum distance(const mjtNum p[3], const mjtNum attributes[3]) { // factory function std::optional Gear::Create( const mjModel* m, mjData* d, int instance) { - if (CheckAttr("alpha", m, instance)) { - return Gear(m, d, instance); + if (CheckAttr("alpha", m, instance) && CheckAttr("diameter", m, instance) && + CheckAttr("teeth", m, instance)) { + return Gear(m, d, instance); } else { - mju_warning("Invalid parameter specification in Gear plugin"); - return std::nullopt; + 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); + SdfDefault defattribute; + + for (int i=0; i < GearAttribute::nattribute; i++) { + attribute[i] = defattribute.GetDefault( + GearAttribute::names[i], + mj_getPluginConfig(m, instance, GearAttribute::names[i])); + } } // plugin computation @@ -173,21 +184,20 @@ void Gear::Visualize(const mjModel* m, mjData* d, const mjvOption* opt, // sdf mjtNum Gear::Distance(const mjtNum point[3]) const { - return distance(point, &alpha); + return distance(point, attribute); } // 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 dist0 = distance(point, attribute); mjtNum pointX[3] = {point[0]+eps, point[1], point[2]}; - mjtNum distX = distance(pointX, attributes); + mjtNum distX = distance(pointX, attribute); mjtNum pointY[3] = {point[0], point[1]+eps, point[2]}; - mjtNum distY = distance(pointY, attributes); + mjtNum distY = distance(pointY, attribute); mjtNum pointZ[3] = {point[0], point[1], point[2]+eps}; - mjtNum distZ = distance(pointZ, attributes); + mjtNum distZ = distance(pointZ, attribute); grad[0] = (distX - dist0) / eps; grad[1] = (distY - dist0) / eps; @@ -202,9 +212,8 @@ void Gear::RegisterPlugin() { plugin.name = "mujoco.sdf.gear"; plugin.capabilityflags |= mjPLUGIN_SDF; - const char* attributes[] = {"alpha"}; - plugin.nattribute = sizeof(attributes) / sizeof(attributes[0]); - plugin.attributes = attributes; + plugin.nattribute = GearAttribute::nattribute; + plugin.attributes = GearAttribute::names; plugin.nstate = +[](const mjModel* m, int instance) { return 0; }; plugin.init = +[](const mjModel* m, mjData* d, int instance) { @@ -253,8 +262,13 @@ void Gear::RegisterPlugin() { 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; + aabb[3] = aabb[4] = attributes[1] / 2. * 1.25; + aabb[5] = attributes[3] / 2. * 1.1; + }; + plugin.sdf_attribute = + +[](mjtNum attribute[], const char* name[], const char* value[]) { + SdfDefault defattribute; + defattribute.GetDefaults(attribute, name, value); }; mjp_registerPlugin(&plugin); diff --git a/plugin/sdf/gear.h b/plugin/sdf/gear.h index 400c8d6e..f6e49ebc 100644 --- a/plugin/sdf/gear.h +++ b/plugin/sdf/gear.h @@ -26,6 +26,13 @@ namespace mujoco::plugin::sdf { +struct GearAttribute { + static constexpr int nattribute = 4; + static constexpr char const* names[nattribute] = {"alpha", "diameter", + "teeth", "thickness"}; + static constexpr mjtNum defaults[nattribute] = { 0, 2.8, 25, .2 }; +}; + class Gear { public: // Creates a new Gear instance (allocated with `new`) or @@ -43,7 +50,7 @@ class Gear { static void RegisterPlugin(); - mjtNum alpha; + mjtNum attribute[GearAttribute::nattribute]; private: Gear(const mjModel* m, mjData* d, int instance); diff --git a/plugin/sdf/nut.cc b/plugin/sdf/nut.cc index fe753a55..9e0fb53b 100644 --- a/plugin/sdf/nut.cc +++ b/plugin/sdf/nut.cc @@ -13,12 +13,15 @@ // limitations under the License. #include -#include +#include +#include #include +#include #include #include #include +#include "sdf.h" #include "nut.h" namespace mujoco::plugin::sdf { @@ -74,7 +77,13 @@ std::optional Nut::Create( // plugin constructor Nut::Nut(const mjModel* m, mjData* d, int instance) { - radius = strtod(mj_getPluginConfig(m, instance, "radius"), nullptr); + SdfDefault defattribute; + + for (int i=0; i < NutAttribute::nattribute; i++) { + attribute[i] = defattribute.GetDefault( + NutAttribute::names[i], + mj_getPluginConfig(m, instance, NutAttribute::names[i])); + } } // plugin computation @@ -95,19 +104,19 @@ void Nut::Visualize(const mjModel* m, mjData* d, const mjvOption* opt, // sdf mjtNum Nut::Distance(const mjtNum point[3]) const { - return distance(point, &radius); + return distance(point, attribute); } // gradient of sdf void Nut::Gradient(mjtNum grad[3], const mjtNum point[3]) const { mjtNum eps = 1e-8; - mjtNum dist0 = distance(point, &radius); + mjtNum dist0 = distance(point, attribute); mjtNum pointX[3] = {point[0]+eps, point[1], point[2]}; - mjtNum distX = distance(pointX, &radius); + mjtNum distX = distance(pointX, attribute); mjtNum pointY[3] = {point[0], point[1]+eps, point[2]}; - mjtNum distY = distance(pointY, &radius); + mjtNum distY = distance(pointY, attribute); mjtNum pointZ[3] = {point[0], point[1], point[2]+eps}; - mjtNum distZ = distance(pointZ, &radius); + mjtNum distZ = distance(pointZ, attribute); grad[0] = (distX - dist0) / eps; grad[1] = (distY - dist0) / eps; @@ -122,9 +131,8 @@ void Nut::RegisterPlugin() { plugin.name = "mujoco.sdf.nut"; plugin.capabilityflags |= mjPLUGIN_SDF; - const char* attributes[] = {"radius"}; - plugin.nattribute = sizeof(attributes) / sizeof(attributes[0]); - plugin.attributes = attributes; + plugin.nattribute = NutAttribute::nattribute; + plugin.attributes = NutAttribute::names; plugin.nstate = +[](const mjModel* m, int instance) { return 0; }; plugin.init = +[](const mjModel* m, mjData* d, int instance) { @@ -176,6 +184,11 @@ void Nut::RegisterPlugin() { aabb[3] = aabb[4] = .6; aabb[5] = 1; }; + plugin.sdf_attribute = + +[](mjtNum attribute[], const char* name[], const char* value[]) { + SdfDefault defattribute; + defattribute.GetDefaults(attribute, name, value); + }; mjp_registerPlugin(&plugin); } diff --git a/plugin/sdf/nut.h b/plugin/sdf/nut.h index e9da6e34..9d7377f1 100644 --- a/plugin/sdf/nut.h +++ b/plugin/sdf/nut.h @@ -28,6 +28,12 @@ namespace mujoco::plugin::sdf { // this plugin implements a modification of the signed distance function // from https://www.shadertoy.com/view/XtffzX of hexagonal nut +struct NutAttribute { + static constexpr int nattribute = 1; + static constexpr char const* names[nattribute] = {"radius"}; + static constexpr mjtNum defaults[nattribute] = {0.26}; +}; + class Nut { public: // Creates a new Nut instance (allocated with `new`) or @@ -45,7 +51,7 @@ class Nut { static void RegisterPlugin(); - mjtNum radius; + mjtNum attribute[NutAttribute::nattribute]; private: Nut(const mjModel* m, mjData* d, int instance); diff --git a/plugin/sdf/sdf.cc b/plugin/sdf/sdf.cc index 2f3d565c..31599794 100644 --- a/plugin/sdf/sdf.cc +++ b/plugin/sdf/sdf.cc @@ -13,10 +13,12 @@ // limitations under the License. #include +#include #include +#include +#include #include -#include #include #include "sdf.h" diff --git a/plugin/sdf/sdf.h b/plugin/sdf/sdf.h index bdefd436..6c7a139c 100644 --- a/plugin/sdf/sdf.h +++ b/plugin/sdf/sdf.h @@ -15,6 +15,9 @@ #ifndef MUJOCO_PLUGIN_SDF_SDF_H_ #define MUJOCO_PLUGIN_SDF_SDF_H_ +#include +#include +#include #include #include @@ -40,6 +43,43 @@ inline mjtNum Fract(mjtNum x) { // reads numeric attributes bool CheckAttr(const char* name, const mjModel* m, int instance); +// converts attributes to numeric or returns default if not present +template +class SdfDefault { + public: + SdfDefault() { + for (int i = 0; i < T::nattribute; i++) { + default_[T::names[i]] = T::defaults[i]; + } + } + + // get a single default value + mjtNum GetDefault(const char* name, const char* value) { + if (std::string(value).empty()) { + return default_[name]; + } + try { + mjtNum num = std::stod(value); + return num; + } catch (const std::invalid_argument& e) { + mju_error("invalid attribute value for '%s'", name); + return 0; + } + } + + // populate attribute array + void GetDefaults(mjtNum* attribute, const char* names[], + const char* values[]) { + for (int i = 0; i < default_.size(); i++) { + attribute[i] = GetDefault(names[i], values[i]); + } + } + + private: + std::map default_; +}; + +// stores the history of gradient descent iterations class SdfVisualizer { public: SdfVisualizer(); diff --git a/plugin/sdf/torus.cc b/plugin/sdf/torus.cc index 48af63dd..a3c6195c 100644 --- a/plugin/sdf/torus.cc +++ b/plugin/sdf/torus.cc @@ -12,12 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include +#include #include #include #include +#include "sdf.h" #include "torus.h" namespace mujoco::plugin::sdf { @@ -43,19 +45,24 @@ std::optional Torus::Create( // 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); + SdfDefault defattribute; + + for (int i=0; i < TorusAttribute::nattribute; i++) { + attribute[i] = defattribute.GetDefault( + TorusAttribute::names[i], + mj_getPluginConfig(m, instance, TorusAttribute::names[i])); + } } // sdf mjtNum Torus::Distance(const mjtNum point[3]) const { - return distance(point, radius); + return distance(point, attribute); } // 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 q = len_xy - attribute[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); @@ -71,9 +78,8 @@ void Torus::RegisterPlugin() { 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.nattribute = TorusAttribute::nattribute; + plugin.attributes = TorusAttribute::names; plugin.nstate = +[](const mjModel* m, int instance) { return 0; }; plugin.init = +[](const mjModel* m, mjData* d, int instance) { @@ -117,8 +123,13 @@ void Torus::RegisterPlugin() { aabb[3] = aabb[4] = attributes[0] + attributes[1]; aabb[5] = attributes[1]; }; + plugin.sdf_attribute = + +[](mjtNum attribute[], const char* name[], const char* value[]) { + SdfDefault defattribute; + defattribute.GetDefaults(attribute, name, value); + }; - mjp_registerPlugin(&plugin); + mjp_registerPlugin(&plugin); } } // namespace mujoco::plugin::sdf diff --git a/plugin/sdf/torus.h b/plugin/sdf/torus.h index 0b1b3cb5..8966d984 100644 --- a/plugin/sdf/torus.h +++ b/plugin/sdf/torus.h @@ -24,6 +24,12 @@ namespace mujoco::plugin::sdf { +struct TorusAttribute { + static constexpr int nattribute = 2; + static constexpr char const* names[nattribute] = {"radius1", "radius2"}; + static constexpr mjtNum defaults[nattribute] = { .35, .15 }; +}; + class Torus { public: // Creates a new Torus instance or returns null on failure. @@ -36,7 +42,7 @@ class Torus { static void RegisterPlugin(); - mjtNum radius[2]; + mjtNum attribute[TorusAttribute::nattribute]; private: Torus(const mjModel* m, mjData* d, int instance); diff --git a/src/user/user_mesh.cc b/src/user/user_mesh.cc index 0c70f03a..cc2536bf 100644 --- a/src/user/user_mesh.cc +++ b/src/user/user_mesh.cc @@ -21,7 +21,6 @@ #include #include #include -#include #include #include #include @@ -303,15 +302,16 @@ void mjCMesh::LoadSDF() { throw mjCError(this, "plugin '%s' does not support signed distance fields", plugin->name); } - int i=0; std::vector attributes(plugin->nattribute, 0); - for (auto const& pair : plugin_instance->config_attribs) { - try { - attributes[i++] = std::stod(pair.second); - } catch (const std::invalid_argument& e) { - throw mjCError(this, "invalid attribute value for '%s'", - pair.first.c_str()); - } + std::vector names(plugin->nattribute, 0); + std::vector values(plugin->nattribute, 0); + for (int i=0; i < plugin->nattribute; i++) { + names[i] = plugin->attributes[i]; + values[i] = plugin_instance->config_attribs[names[i]].c_str(); + } + + if (plugin->sdf_attribute) { + plugin->sdf_attribute(attributes.data(), names.data(), values.data()); } mjtNum aabb[6] = {0};