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:
committed by
Copybara-Service
parent
c921c3956e
commit
50843cad30
@@ -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);
|
||||
};
|
||||
|
||||
@@ -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.
|
||||
|
||||
@@ -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,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
|
||||
|
||||
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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"
|
||||
|
||||
|
||||
@@ -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
@@ -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
@@ -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);
|
||||
|
||||
@@ -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};
|
||||
|
||||
Reference in New Issue
Block a user