Add defaults to SDF plugin attributes.

Expose diameter and number of teeth for the gear SDF.

PiperOrigin-RevId: 577779297
Change-Id: Iec5890ab7f39080b395cf06a76f9c2216f4f39ed
This commit is contained in:
Alessio Quaglino
2023-10-30 03:08:41 -07:00
committed by Copybara-Service
parent c921c3956e
commit 50843cad30
18 changed files with 341 additions and 97 deletions
+3
View File
@@ -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);
};
+15 -11
View File
@@ -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/ <https://github.com/google-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.
See also :ref:`composite <CComposite>` and :ref:`deformable <CDeformable>` objects.
* **elasticity:** The plugins in the `elasticity/
<https://github.com/google-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. See
also :ref:`composite <CComposite>` and :ref:`deformable <CDeformable>` objects. For more information, please see the
`README <https://github.com/google-deepmind/mujoco/blob/main/plugin/elasticity/README.md>`__.
* **sensor:** The plugins in the `sensor/ <https://github.com/google-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/google-deepmind/mujoco/blob/main/plugin/sensor/README.md>`__ for details.
* **sdf:** The plugins in the `sdf/ <https://github.com/google-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/google-deepmind/mujoco/blob/main/src/engine/engine_collision_driver.c>`__.
* **sdf:** The plugins in the `sdf/ <https://github.com/google-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/google-deepmind/mujoco/blob/main/src/engine/engine_collision_driver.c>`__. For more information
concerning the available SDFs and how to write your own implicit geometry, please see the `README
<https://github.com/google-deepmind/mujoco/blob/main/plugin/sdf/README.md>`__. 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.
+4
View File
@@ -17,6 +17,7 @@
#include <mujoco/mjdata.h>
#include <mujoco/mjmodel.h>
#include <mujoco/mjtnum.h>
#include <mujoco/mjvisualize.h>
@@ -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);
};
+1 -1
View File
@@ -1,5 +1,5 @@
<h1>
<a href="#"><img alt="MuJoCo" src="banner.png" width="100%"/></a>
<a href="#"><img alt="MuJoCo" src="../../banner.png" width="100%"/></a>
</h1>
## Elasticity plugins
+100
View File
@@ -0,0 +1,100 @@
<h1>
<a href="#"><img alt="MuJoCo" src="../../banner.png" width="100%"/></a>
</h1>
## 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<MySDF> 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);
};
```
+21 -9
View File
@@ -13,8 +13,10 @@
// limitations under the License.
#include <cmath>
#include <cstdint>
#include <cstdlib>
#include <optional>
#include <utility>
#include <mujoco/mjplugin.h>
#include <mujoco/mjtnum.h>
@@ -75,7 +77,13 @@ std::optional<Bolt> Bolt::Create(
// plugin constructor
Bolt::Bolt(const mjModel* m, mjData* d, int instance) {
radius = strtod(mj_getPluginConfig(m, instance, "radius"), nullptr);
SdfDefault<BoltAttribute> 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<BoltAttribute> defattribute;
defattribute.GetDefaults(attribute, name, value);
};
mjp_registerPlugin(&plugin);
}
+7 -1
View File
@@ -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);
+25 -14
View File
@@ -12,12 +12,14 @@
// See the License for the specific language governing permissions and
// limitations under the License.
#include <sstream>
#include <cstdint>
#include <optional>
#include <utility>
#include <mujoco/mjplugin.h>
#include <mujoco/mjtnum.h>
#include <mujoco/mujoco.h>
#include "sdf.h"
#include "bowl.h"
namespace mujoco::plugin::sdf {
@@ -51,9 +53,16 @@ std::optional<Bowl> 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<BowlAttribute> 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<BowlAttribute> defattribute;
defattribute.GetDefaults(attribute, name, value);
};
mjp_registerPlugin(&plugin);
}
+8 -3
View File
@@ -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:
+40 -26
View File
@@ -12,14 +12,16 @@
// See the License for the specific language governing permissions and
// limitations under the License.
#include <cstdio>
#include <sstream>
#include <cstdint>
#include <cstdlib>
#include <optional>
#include <utility>
#include <mujoco/mjplugin.h>
#include <mujoco/mjtnum.h>
#include <mujoco/mjvisualize.h>
#include <mujoco/mujoco.h>
#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> 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<GearAttribute> 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<GearAttribute> defattribute;
defattribute.GetDefaults(attribute, name, value);
};
mjp_registerPlugin(&plugin);
+8 -1
View File
@@ -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);
+23 -10
View File
@@ -13,12 +13,15 @@
// limitations under the License.
#include <cmath>
#include <sstream>
#include <cstdint>
#include <cstdlib>
#include <optional>
#include <utility>
#include <mujoco/mjplugin.h>
#include <mujoco/mjtnum.h>
#include <mujoco/mujoco.h>
#include "sdf.h"
#include "nut.h"
namespace mujoco::plugin::sdf {
@@ -74,7 +77,13 @@ std::optional<Nut> Nut::Create(
// plugin constructor
Nut::Nut(const mjModel* m, mjData* d, int instance) {
radius = strtod(mj_getPluginConfig(m, instance, "radius"), nullptr);
SdfDefault<NutAttribute> 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<NutAttribute> defattribute;
defattribute.GetDefaults(attribute, name, value);
};
mjp_registerPlugin(&plugin);
}
+7 -1
View File
@@ -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);
+3 -1
View File
@@ -13,10 +13,12 @@
// limitations under the License.
#include <algorithm>
#include <cctype>
#include <cstddef>
#include <cstdio>
#include <cstdlib>
#include <string>
#include <mujoco/mjplugin.h>
#include <mujoco/mujoco.h>
#include "sdf.h"
+40
View File
@@ -15,6 +15,9 @@
#ifndef MUJOCO_PLUGIN_SDF_SDF_H_
#define MUJOCO_PLUGIN_SDF_SDF_H_
#include <map>
#include <stdexcept>
#include <string>
#include <vector>
#include <mujoco/mujoco.h>
@@ -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 <typename T>
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<std::string, mjtNum> default_;
};
// stores the history of gradient descent iterations
class SdfVisualizer {
public:
SdfVisualizer();
+20 -9
View File
@@ -12,12 +12,14 @@
// See the License for the specific language governing permissions and
// limitations under the License.
#include <sstream>
#include <cstdint>
#include <optional>
#include <utility>
#include <mujoco/mjplugin.h>
#include <mujoco/mjtnum.h>
#include <mujoco/mujoco.h>
#include "sdf.h"
#include "torus.h"
namespace mujoco::plugin::sdf {
@@ -43,19 +45,24 @@ std::optional<Torus> 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<TorusAttribute> 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<TorusAttribute> defattribute;
defattribute.GetDefaults(attribute, name, value);
};
mjp_registerPlugin(&plugin);
mjp_registerPlugin(&plugin);
}
} // namespace mujoco::plugin::sdf
+7 -1
View File
@@ -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);
+9 -9
View File
@@ -21,7 +21,6 @@
#include <functional>
#include <memory>
#include <optional>
#include <stdexcept>
#include <string>
#include <unordered_map>
#include <utility>
@@ -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<mjtNum> 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<const char*> names(plugin->nattribute, 0);
std::vector<const char*> 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};