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
|
// called during compilation for marching cubes
|
||||||
mjtNum (*sdf_staticdistance)(const mjtNum point[3], const mjtNum* attributes);
|
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
|
// bounding box of implicit surface
|
||||||
void (*sdf_aabb)(mjtNum aabb[6], const mjtNum* attributes);
|
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:
|
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>`__
|
* **elasticity:** The plugins in the `elasticity/
|
||||||
directory are passive forces based on continuum mechanics for 1-dimensional and 3-dimensional bodies. The
|
<https://github.com/google-deepmind/mujoco/tree/main/plugin/elasticity>`__ directory are passive forces based on
|
||||||
1D model is invariant under rotations and captures the large deformation of elastic cables, decoupling twisting and
|
continuum mechanics for 1-dimensional and 3-dimensional bodies. The 1D model is invariant under rotations and captures
|
||||||
bending strains. The 3D solid is a
|
the large deformation of elastic cables, decoupling twisting and bending strains. The 3D solid is a `Saint
|
||||||
`Saint Venant-Kirchhoff <https://en.wikipedia.org/wiki/Hyperelastic_material#Saint_Venant%E2%80%93Kirchhoff_model>`__
|
Venant-Kirchhoff <https://en.wikipedia.org/wiki/Hyperelastic_material#Saint_Venant%E2%80%93Kirchhoff_model>`__ model
|
||||||
model discretized with piecewise linear finite elements, which is suitable for large deformations with small strains.
|
discretized with piecewise linear finite elements, which is suitable for large deformations with small strains. See
|
||||||
See also :ref:`composite <CComposite>` and :ref:`deformable <CDeformable>` objects.
|
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>`__
|
* **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
|
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.
|
`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>`__
|
* **sdf:** The plugins in the `sdf/ <https://github.com/google-deepmind/mujoco/tree/main/plugin/sdf>`__ directory
|
||||||
directory specify custom shapes in a mesh-free manner, by defining methods computing a signed distance field and its
|
specify custom shapes in a mesh-free manner, by defining methods computing a signed distance field and its gradient at
|
||||||
gradient at query points. This shape then acts as a new geom type in the collision table at the top of
|
query points. This shape then acts as a new geom type in the collision table at the top of `engine_collision_driver.c
|
||||||
`engine_collision_driver.c <https://github.com/google-deepmind/mujoco/blob/main/src/engine/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.
|
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.
|
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/mjdata.h>
|
||||||
#include <mujoco/mjmodel.h>
|
#include <mujoco/mjmodel.h>
|
||||||
|
#include <mujoco/mjtnum.h>
|
||||||
#include <mujoco/mjvisualize.h>
|
#include <mujoco/mjvisualize.h>
|
||||||
|
|
||||||
|
|
||||||
@@ -117,6 +118,9 @@ struct mjpPlugin_ {
|
|||||||
// called during compilation for marching cubes
|
// called during compilation for marching cubes
|
||||||
mjtNum (*sdf_staticdistance)(const mjtNum point[3], const mjtNum* attributes);
|
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
|
// bounding box of implicit surface
|
||||||
void (*sdf_aabb)(mjtNum aabb[6], const mjtNum* attributes);
|
void (*sdf_aabb)(mjtNum aabb[6], const mjtNum* attributes);
|
||||||
};
|
};
|
||||||
|
|||||||
@@ -1,5 +1,5 @@
|
|||||||
<h1>
|
<h1>
|
||||||
<a href="#"><img alt="MuJoCo" src="banner.png" width="100%"/></a>
|
<a href="#"><img alt="MuJoCo" src="../../banner.png" width="100%"/></a>
|
||||||
</h1>
|
</h1>
|
||||||
|
|
||||||
## Elasticity plugins
|
## 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.
|
// limitations under the License.
|
||||||
|
|
||||||
#include <cmath>
|
#include <cmath>
|
||||||
|
#include <cstdint>
|
||||||
#include <cstdlib>
|
#include <cstdlib>
|
||||||
#include <optional>
|
#include <optional>
|
||||||
|
#include <utility>
|
||||||
|
|
||||||
#include <mujoco/mjplugin.h>
|
#include <mujoco/mjplugin.h>
|
||||||
#include <mujoco/mjtnum.h>
|
#include <mujoco/mjtnum.h>
|
||||||
@@ -75,7 +77,13 @@ std::optional<Bolt> Bolt::Create(
|
|||||||
|
|
||||||
// plugin constructor
|
// plugin constructor
|
||||||
Bolt::Bolt(const mjModel* m, mjData* d, int instance) {
|
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
|
// 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
|
// sdf
|
||||||
mjtNum Bolt::Distance(const mjtNum point[3]) const {
|
mjtNum Bolt::Distance(const mjtNum point[3]) const {
|
||||||
return distance(point, &radius);
|
return distance(point, attribute);
|
||||||
}
|
}
|
||||||
|
|
||||||
// gradient of sdf
|
// gradient of sdf
|
||||||
void Bolt::Gradient(mjtNum grad[3], const mjtNum point[3]) const {
|
void Bolt::Gradient(mjtNum grad[3], const mjtNum point[3]) const {
|
||||||
mjtNum eps = 1e-8;
|
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 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 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 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[0] = (distX - dist0) / eps;
|
||||||
grad[1] = (distY - dist0) / eps;
|
grad[1] = (distY - dist0) / eps;
|
||||||
@@ -123,9 +131,8 @@ void Bolt::RegisterPlugin() {
|
|||||||
plugin.name = "mujoco.sdf.bolt";
|
plugin.name = "mujoco.sdf.bolt";
|
||||||
plugin.capabilityflags |= mjPLUGIN_SDF;
|
plugin.capabilityflags |= mjPLUGIN_SDF;
|
||||||
|
|
||||||
const char* attributes[] = {"radius"};
|
plugin.nattribute = BoltAttribute::nattribute;
|
||||||
plugin.nattribute = sizeof(attributes) / sizeof(attributes[0]);
|
plugin.attributes = BoltAttribute::names;
|
||||||
plugin.attributes = attributes;
|
|
||||||
plugin.nstate = +[](const mjModel* m, int instance) { return 0; };
|
plugin.nstate = +[](const mjModel* m, int instance) { return 0; };
|
||||||
|
|
||||||
plugin.init = +[](const mjModel* m, mjData* d, int instance) {
|
plugin.init = +[](const mjModel* m, mjData* d, int instance) {
|
||||||
@@ -177,6 +184,11 @@ void Bolt::RegisterPlugin() {
|
|||||||
aabb[3] = aabb[4] = .6;
|
aabb[3] = aabb[4] = .6;
|
||||||
aabb[5] = 1;
|
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);
|
mjp_registerPlugin(&plugin);
|
||||||
}
|
}
|
||||||
|
|||||||
+7
-1
@@ -28,6 +28,12 @@ namespace mujoco::plugin::sdf {
|
|||||||
// this plugin implements a modification of the signed distance function
|
// this plugin implements a modification of the signed distance function
|
||||||
// from https://www.shadertoy.com/view/XtffzX of a bolt with a hexagonal head
|
// 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 {
|
class Bolt {
|
||||||
public:
|
public:
|
||||||
// Creates a new Bolt instance (allocated with `new`) or
|
// Creates a new Bolt instance (allocated with `new`) or
|
||||||
@@ -45,7 +51,7 @@ class Bolt {
|
|||||||
|
|
||||||
static void RegisterPlugin();
|
static void RegisterPlugin();
|
||||||
|
|
||||||
mjtNum radius;
|
mjtNum attribute[BoltAttribute::nattribute];
|
||||||
|
|
||||||
private:
|
private:
|
||||||
Bolt(const mjModel* m, mjData* d, int instance);
|
Bolt(const mjModel* m, mjData* d, int instance);
|
||||||
|
|||||||
+25
-14
@@ -12,12 +12,14 @@
|
|||||||
// See the License for the specific language governing permissions and
|
// See the License for the specific language governing permissions and
|
||||||
// limitations under the License.
|
// limitations under the License.
|
||||||
|
|
||||||
#include <sstream>
|
#include <cstdint>
|
||||||
#include <optional>
|
#include <optional>
|
||||||
|
#include <utility>
|
||||||
|
|
||||||
#include <mujoco/mjplugin.h>
|
#include <mujoco/mjplugin.h>
|
||||||
#include <mujoco/mjtnum.h>
|
#include <mujoco/mjtnum.h>
|
||||||
#include <mujoco/mujoco.h>
|
#include <mujoco/mujoco.h>
|
||||||
|
#include "sdf.h"
|
||||||
#include "bowl.h"
|
#include "bowl.h"
|
||||||
|
|
||||||
namespace mujoco::plugin::sdf {
|
namespace mujoco::plugin::sdf {
|
||||||
@@ -51,9 +53,16 @@ std::optional<Bowl> Bowl::Create(
|
|||||||
|
|
||||||
// plugin constructor
|
// plugin constructor
|
||||||
Bowl::Bowl(const mjModel* m, mjData* d, int instance) {
|
Bowl::Bowl(const mjModel* m, mjData* d, int instance) {
|
||||||
radius = strtod(mj_getPluginConfig(m, instance, "radius"), nullptr);
|
SdfDefault<BowlAttribute> defattribute;
|
||||||
height = strtod(mj_getPluginConfig(m, instance, "height"), nullptr);
|
|
||||||
thick = strtod(mj_getPluginConfig(m, instance, "thickness"), nullptr);
|
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);
|
width = mju_sqrt(radius*radius - height*height);
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -75,8 +84,7 @@ void Bowl::Visualize(const mjModel* m, mjData* d, const mjvOption* opt, mjvScene
|
|||||||
|
|
||||||
// sdf
|
// sdf
|
||||||
mjtNum Bowl::Distance(const mjtNum point[3]) const {
|
mjtNum Bowl::Distance(const mjtNum point[3]) const {
|
||||||
mjtNum attributes[3]= {height, radius, thick};
|
return distance(point, attribute);
|
||||||
return distance(point, attributes);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// gradient of sdf
|
// 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[1] = - grad_dist * point[1] / pnorm;
|
||||||
// grad[2] = - grad_dist * point[2] / pnorm;
|
// grad[2] = - grad_dist * point[2] / pnorm;
|
||||||
// }
|
// }
|
||||||
mjtNum attributes[3]= {height, radius, thick};
|
|
||||||
mjtNum eps = 1e-8;
|
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 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 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 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[0] = (distX - dist0) / eps;
|
||||||
grad[1] = (distY - dist0) / eps;
|
grad[1] = (distY - dist0) / eps;
|
||||||
@@ -122,9 +129,8 @@ void Bowl::RegisterPlugin() {
|
|||||||
plugin.name = "mujoco.sdf.bowl";
|
plugin.name = "mujoco.sdf.bowl";
|
||||||
plugin.capabilityflags |= mjPLUGIN_SDF;
|
plugin.capabilityflags |= mjPLUGIN_SDF;
|
||||||
|
|
||||||
const char* attributes[] = {"radius", "height", "thickness"};
|
plugin.nattribute = BowlAttribute::nattribute;
|
||||||
plugin.nattribute = sizeof(attributes) / sizeof(attributes[0]);
|
plugin.attributes = BowlAttribute::names;
|
||||||
plugin.attributes = attributes;
|
|
||||||
plugin.nstate = +[](const mjModel* m, int instance) { return 0; };
|
plugin.nstate = +[](const mjModel* m, int instance) { return 0; };
|
||||||
|
|
||||||
plugin.init = +[](const mjModel* m, mjData* d, int instance) {
|
plugin.init = +[](const mjModel* m, mjData* d, int instance) {
|
||||||
@@ -177,6 +183,11 @@ void Bowl::RegisterPlugin() {
|
|||||||
aabb[0] = aabb[1] = aabb[2] = 0;
|
aabb[0] = aabb[1] = aabb[2] = 0;
|
||||||
aabb[3] = aabb[4] = aabb[5] = radius + thick;
|
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);
|
mjp_registerPlugin(&plugin);
|
||||||
}
|
}
|
||||||
|
|||||||
+8
-3
@@ -25,6 +25,13 @@
|
|||||||
|
|
||||||
namespace mujoco::plugin::sdf {
|
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 {
|
class Bowl {
|
||||||
public:
|
public:
|
||||||
// Creates a new Bowl instance (allocated with `new`) or
|
// Creates a new Bowl instance (allocated with `new`) or
|
||||||
@@ -42,9 +49,7 @@ class Bowl {
|
|||||||
|
|
||||||
static void RegisterPlugin();
|
static void RegisterPlugin();
|
||||||
|
|
||||||
mjtNum radius;
|
mjtNum attribute[BowlAttribute::nattribute];
|
||||||
mjtNum height;
|
|
||||||
mjtNum thick;
|
|
||||||
mjtNum width;
|
mjtNum width;
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
|||||||
+40
-26
@@ -12,14 +12,16 @@
|
|||||||
// See the License for the specific language governing permissions and
|
// See the License for the specific language governing permissions and
|
||||||
// limitations under the License.
|
// limitations under the License.
|
||||||
|
|
||||||
#include <cstdio>
|
#include <cstdint>
|
||||||
#include <sstream>
|
#include <cstdlib>
|
||||||
#include <optional>
|
#include <optional>
|
||||||
|
#include <utility>
|
||||||
|
|
||||||
#include <mujoco/mjplugin.h>
|
#include <mujoco/mjplugin.h>
|
||||||
#include <mujoco/mjtnum.h>
|
#include <mujoco/mjtnum.h>
|
||||||
#include <mujoco/mjvisualize.h>
|
#include <mujoco/mjvisualize.h>
|
||||||
#include <mujoco/mujoco.h>
|
#include <mujoco/mujoco.h>
|
||||||
|
#include "sdf.h"
|
||||||
#include "gear.h"
|
#include "gear.h"
|
||||||
|
|
||||||
namespace mujoco::plugin::sdf {
|
namespace mujoco::plugin::sdf {
|
||||||
@@ -50,10 +52,11 @@ static mjtNum mod(mjtNum x, mjtNum y) {
|
|||||||
return x - y * floor(x/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
|
// see https://www.shadertoy.com/view/3lG3WR
|
||||||
mjtNum D = 2.8; // should be an attribute
|
mjtNum D = attributes[1];
|
||||||
mjtNum N = 25; // should be an attribute
|
mjtNum N = attributes[2];
|
||||||
mjtNum psi = 3.096e-5 * N * N -6.557e-3 * N + 0.551; // pressure angle
|
mjtNum psi = 3.096e-5 * N * N -6.557e-3 * N + 0.551; // pressure angle
|
||||||
mjtNum alpha = attributes[0];
|
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 h = 2.2 / Pd;
|
||||||
|
|
||||||
mjtNum innerR = Ro - h - 0.4;
|
mjtNum innerR = Ro - h - 0.14*D;
|
||||||
|
|
||||||
// Early exit
|
// Early exit
|
||||||
if (innerR - rho > 0.0)
|
if (innerR - rho > 0.0)
|
||||||
@@ -126,15 +129,16 @@ static mjtNum distance2D(const mjtNum p[3], const mjtNum attributes[3]) {
|
|||||||
fib - (alphaStride - shift));
|
fib - (alphaStride - shift));
|
||||||
|
|
||||||
cogs = Intersection(baseWalls, cogs);
|
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 = smoothUnion(gearLowBase, cogs, Rb - Ro + h);
|
||||||
cogs = Subtraction(cogs, crownBase);
|
cogs = Subtraction(cogs, crownBase);
|
||||||
|
|
||||||
return extrusion(p, cogs, .1);
|
return cogs;
|
||||||
}
|
}
|
||||||
|
|
||||||
static mjtNum distance(const mjtNum p[3], const mjtNum attributes[3]) {
|
static mjtNum distance(const mjtNum p[3],
|
||||||
return extrusion(p, distance2D(p, attributes), .1) - .005;
|
const mjtNum attributes[GearAttribute::nattribute]) {
|
||||||
|
return extrusion(p, distance2D(p, attributes), attributes[3]/2.);
|
||||||
}
|
}
|
||||||
|
|
||||||
} // namespace
|
} // namespace
|
||||||
@@ -142,17 +146,24 @@ static mjtNum distance(const mjtNum p[3], const mjtNum attributes[3]) {
|
|||||||
// factory function
|
// factory function
|
||||||
std::optional<Gear> Gear::Create(
|
std::optional<Gear> Gear::Create(
|
||||||
const mjModel* m, mjData* d, int instance) {
|
const mjModel* m, mjData* d, int instance) {
|
||||||
if (CheckAttr("alpha", m, instance)) {
|
if (CheckAttr("alpha", m, instance) && CheckAttr("diameter", m, instance) &&
|
||||||
return Gear(m, d, instance);
|
CheckAttr("teeth", m, instance)) {
|
||||||
|
return Gear(m, d, instance);
|
||||||
} else {
|
} else {
|
||||||
mju_warning("Invalid parameter specification in Gear plugin");
|
mju_warning("Invalid parameter specification in Gear plugin");
|
||||||
return std::nullopt;
|
return std::nullopt;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// plugin constructor
|
// plugin constructor
|
||||||
Gear::Gear(const mjModel* m, mjData* d, int instance) {
|
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
|
// plugin computation
|
||||||
@@ -173,21 +184,20 @@ void Gear::Visualize(const mjModel* m, mjData* d, const mjvOption* opt,
|
|||||||
|
|
||||||
// sdf
|
// sdf
|
||||||
mjtNum Gear::Distance(const mjtNum point[3]) const {
|
mjtNum Gear::Distance(const mjtNum point[3]) const {
|
||||||
return distance(point, &alpha);
|
return distance(point, attribute);
|
||||||
}
|
}
|
||||||
|
|
||||||
// gradient of sdf
|
// gradient of sdf
|
||||||
void Gear::Gradient(mjtNum grad[3], const mjtNum point[3]) const {
|
void Gear::Gradient(mjtNum grad[3], const mjtNum point[3]) const {
|
||||||
mjtNum attributes[1]= {alpha};
|
|
||||||
mjtNum eps = 1e-8;
|
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 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 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 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[0] = (distX - dist0) / eps;
|
||||||
grad[1] = (distY - dist0) / eps;
|
grad[1] = (distY - dist0) / eps;
|
||||||
@@ -202,9 +212,8 @@ void Gear::RegisterPlugin() {
|
|||||||
plugin.name = "mujoco.sdf.gear";
|
plugin.name = "mujoco.sdf.gear";
|
||||||
plugin.capabilityflags |= mjPLUGIN_SDF;
|
plugin.capabilityflags |= mjPLUGIN_SDF;
|
||||||
|
|
||||||
const char* attributes[] = {"alpha"};
|
plugin.nattribute = GearAttribute::nattribute;
|
||||||
plugin.nattribute = sizeof(attributes) / sizeof(attributes[0]);
|
plugin.attributes = GearAttribute::names;
|
||||||
plugin.attributes = attributes;
|
|
||||||
plugin.nstate = +[](const mjModel* m, int instance) { return 0; };
|
plugin.nstate = +[](const mjModel* m, int instance) { return 0; };
|
||||||
|
|
||||||
plugin.init = +[](const mjModel* m, mjData* d, int instance) {
|
plugin.init = +[](const mjModel* m, mjData* d, int instance) {
|
||||||
@@ -253,8 +262,13 @@ void Gear::RegisterPlugin() {
|
|||||||
plugin.sdf_aabb =
|
plugin.sdf_aabb =
|
||||||
+[](mjtNum aabb[6], const mjtNum* attributes) {
|
+[](mjtNum aabb[6], const mjtNum* attributes) {
|
||||||
aabb[0] = aabb[1] = aabb[2] = 0;
|
aabb[0] = aabb[1] = aabb[2] = 0;
|
||||||
aabb[3] = aabb[4] = 1.7;
|
aabb[3] = aabb[4] = attributes[1] / 2. * 1.25;
|
||||||
aabb[5] = .11;
|
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);
|
mjp_registerPlugin(&plugin);
|
||||||
|
|||||||
+8
-1
@@ -26,6 +26,13 @@
|
|||||||
|
|
||||||
namespace mujoco::plugin::sdf {
|
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 {
|
class Gear {
|
||||||
public:
|
public:
|
||||||
// Creates a new Gear instance (allocated with `new`) or
|
// Creates a new Gear instance (allocated with `new`) or
|
||||||
@@ -43,7 +50,7 @@ class Gear {
|
|||||||
|
|
||||||
static void RegisterPlugin();
|
static void RegisterPlugin();
|
||||||
|
|
||||||
mjtNum alpha;
|
mjtNum attribute[GearAttribute::nattribute];
|
||||||
|
|
||||||
private:
|
private:
|
||||||
Gear(const mjModel* m, mjData* d, int instance);
|
Gear(const mjModel* m, mjData* d, int instance);
|
||||||
|
|||||||
+23
-10
@@ -13,12 +13,15 @@
|
|||||||
// limitations under the License.
|
// limitations under the License.
|
||||||
|
|
||||||
#include <cmath>
|
#include <cmath>
|
||||||
#include <sstream>
|
#include <cstdint>
|
||||||
|
#include <cstdlib>
|
||||||
#include <optional>
|
#include <optional>
|
||||||
|
#include <utility>
|
||||||
|
|
||||||
#include <mujoco/mjplugin.h>
|
#include <mujoco/mjplugin.h>
|
||||||
#include <mujoco/mjtnum.h>
|
#include <mujoco/mjtnum.h>
|
||||||
#include <mujoco/mujoco.h>
|
#include <mujoco/mujoco.h>
|
||||||
|
#include "sdf.h"
|
||||||
#include "nut.h"
|
#include "nut.h"
|
||||||
|
|
||||||
namespace mujoco::plugin::sdf {
|
namespace mujoco::plugin::sdf {
|
||||||
@@ -74,7 +77,13 @@ std::optional<Nut> Nut::Create(
|
|||||||
|
|
||||||
// plugin constructor
|
// plugin constructor
|
||||||
Nut::Nut(const mjModel* m, mjData* d, int instance) {
|
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
|
// plugin computation
|
||||||
@@ -95,19 +104,19 @@ void Nut::Visualize(const mjModel* m, mjData* d, const mjvOption* opt,
|
|||||||
|
|
||||||
// sdf
|
// sdf
|
||||||
mjtNum Nut::Distance(const mjtNum point[3]) const {
|
mjtNum Nut::Distance(const mjtNum point[3]) const {
|
||||||
return distance(point, &radius);
|
return distance(point, attribute);
|
||||||
}
|
}
|
||||||
|
|
||||||
// gradient of sdf
|
// gradient of sdf
|
||||||
void Nut::Gradient(mjtNum grad[3], const mjtNum point[3]) const {
|
void Nut::Gradient(mjtNum grad[3], const mjtNum point[3]) const {
|
||||||
mjtNum eps = 1e-8;
|
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 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 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 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[0] = (distX - dist0) / eps;
|
||||||
grad[1] = (distY - dist0) / eps;
|
grad[1] = (distY - dist0) / eps;
|
||||||
@@ -122,9 +131,8 @@ void Nut::RegisterPlugin() {
|
|||||||
plugin.name = "mujoco.sdf.nut";
|
plugin.name = "mujoco.sdf.nut";
|
||||||
plugin.capabilityflags |= mjPLUGIN_SDF;
|
plugin.capabilityflags |= mjPLUGIN_SDF;
|
||||||
|
|
||||||
const char* attributes[] = {"radius"};
|
plugin.nattribute = NutAttribute::nattribute;
|
||||||
plugin.nattribute = sizeof(attributes) / sizeof(attributes[0]);
|
plugin.attributes = NutAttribute::names;
|
||||||
plugin.attributes = attributes;
|
|
||||||
plugin.nstate = +[](const mjModel* m, int instance) { return 0; };
|
plugin.nstate = +[](const mjModel* m, int instance) { return 0; };
|
||||||
|
|
||||||
plugin.init = +[](const mjModel* m, mjData* d, int instance) {
|
plugin.init = +[](const mjModel* m, mjData* d, int instance) {
|
||||||
@@ -176,6 +184,11 @@ void Nut::RegisterPlugin() {
|
|||||||
aabb[3] = aabb[4] = .6;
|
aabb[3] = aabb[4] = .6;
|
||||||
aabb[5] = 1;
|
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);
|
mjp_registerPlugin(&plugin);
|
||||||
}
|
}
|
||||||
|
|||||||
+7
-1
@@ -28,6 +28,12 @@ namespace mujoco::plugin::sdf {
|
|||||||
// this plugin implements a modification of the signed distance function
|
// this plugin implements a modification of the signed distance function
|
||||||
// from https://www.shadertoy.com/view/XtffzX of hexagonal nut
|
// 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 {
|
class Nut {
|
||||||
public:
|
public:
|
||||||
// Creates a new Nut instance (allocated with `new`) or
|
// Creates a new Nut instance (allocated with `new`) or
|
||||||
@@ -45,7 +51,7 @@ class Nut {
|
|||||||
|
|
||||||
static void RegisterPlugin();
|
static void RegisterPlugin();
|
||||||
|
|
||||||
mjtNum radius;
|
mjtNum attribute[NutAttribute::nattribute];
|
||||||
|
|
||||||
private:
|
private:
|
||||||
Nut(const mjModel* m, mjData* d, int instance);
|
Nut(const mjModel* m, mjData* d, int instance);
|
||||||
|
|||||||
+3
-1
@@ -13,10 +13,12 @@
|
|||||||
// limitations under the License.
|
// limitations under the License.
|
||||||
|
|
||||||
#include <algorithm>
|
#include <algorithm>
|
||||||
|
#include <cctype>
|
||||||
#include <cstddef>
|
#include <cstddef>
|
||||||
|
#include <cstdio>
|
||||||
|
#include <cstdlib>
|
||||||
#include <string>
|
#include <string>
|
||||||
|
|
||||||
#include <mujoco/mjplugin.h>
|
|
||||||
#include <mujoco/mujoco.h>
|
#include <mujoco/mujoco.h>
|
||||||
#include "sdf.h"
|
#include "sdf.h"
|
||||||
|
|
||||||
|
|||||||
@@ -15,6 +15,9 @@
|
|||||||
#ifndef MUJOCO_PLUGIN_SDF_SDF_H_
|
#ifndef MUJOCO_PLUGIN_SDF_SDF_H_
|
||||||
#define MUJOCO_PLUGIN_SDF_SDF_H_
|
#define MUJOCO_PLUGIN_SDF_SDF_H_
|
||||||
|
|
||||||
|
#include <map>
|
||||||
|
#include <stdexcept>
|
||||||
|
#include <string>
|
||||||
#include <vector>
|
#include <vector>
|
||||||
|
|
||||||
#include <mujoco/mujoco.h>
|
#include <mujoco/mujoco.h>
|
||||||
@@ -40,6 +43,43 @@ inline mjtNum Fract(mjtNum x) {
|
|||||||
// reads numeric attributes
|
// reads numeric attributes
|
||||||
bool CheckAttr(const char* name, const mjModel* m, int instance);
|
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 {
|
class SdfVisualizer {
|
||||||
public:
|
public:
|
||||||
SdfVisualizer();
|
SdfVisualizer();
|
||||||
|
|||||||
+20
-9
@@ -12,12 +12,14 @@
|
|||||||
// See the License for the specific language governing permissions and
|
// See the License for the specific language governing permissions and
|
||||||
// limitations under the License.
|
// limitations under the License.
|
||||||
|
|
||||||
#include <sstream>
|
#include <cstdint>
|
||||||
#include <optional>
|
#include <optional>
|
||||||
|
#include <utility>
|
||||||
|
|
||||||
#include <mujoco/mjplugin.h>
|
#include <mujoco/mjplugin.h>
|
||||||
#include <mujoco/mjtnum.h>
|
#include <mujoco/mjtnum.h>
|
||||||
#include <mujoco/mujoco.h>
|
#include <mujoco/mujoco.h>
|
||||||
|
#include "sdf.h"
|
||||||
#include "torus.h"
|
#include "torus.h"
|
||||||
|
|
||||||
namespace mujoco::plugin::sdf {
|
namespace mujoco::plugin::sdf {
|
||||||
@@ -43,19 +45,24 @@ std::optional<Torus> Torus::Create(
|
|||||||
|
|
||||||
// plugin constructor
|
// plugin constructor
|
||||||
Torus::Torus(const mjModel* m, mjData* d, int instance) {
|
Torus::Torus(const mjModel* m, mjData* d, int instance) {
|
||||||
radius[0] = strtod(mj_getPluginConfig(m, instance, "radius1"), nullptr);
|
SdfDefault<TorusAttribute> defattribute;
|
||||||
radius[1] = strtod(mj_getPluginConfig(m, instance, "radius2"), nullptr);
|
|
||||||
|
for (int i=0; i < TorusAttribute::nattribute; i++) {
|
||||||
|
attribute[i] = defattribute.GetDefault(
|
||||||
|
TorusAttribute::names[i],
|
||||||
|
mj_getPluginConfig(m, instance, TorusAttribute::names[i]));
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// sdf
|
// sdf
|
||||||
mjtNum Torus::Distance(const mjtNum point[3]) const {
|
mjtNum Torus::Distance(const mjtNum point[3]) const {
|
||||||
return distance(point, radius);
|
return distance(point, attribute);
|
||||||
}
|
}
|
||||||
|
|
||||||
// gradient of sdf
|
// gradient of sdf
|
||||||
void Torus::Gradient(mjtNum grad[3], const mjtNum p[3]) const {
|
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 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 grad_q[2] = { p[0] / len_xy, p[1] / len_xy };
|
||||||
mjtNum len_qz = mju_sqrt(q*q + p[2]*p[2]);
|
mjtNum len_qz = mju_sqrt(q*q + p[2]*p[2]);
|
||||||
grad[0] = q*grad_q[0] / mjMAX(len_qz, mjMINVAL);
|
grad[0] = q*grad_q[0] / mjMAX(len_qz, mjMINVAL);
|
||||||
@@ -71,9 +78,8 @@ void Torus::RegisterPlugin() {
|
|||||||
plugin.name = "mujoco.sdf.torus";
|
plugin.name = "mujoco.sdf.torus";
|
||||||
plugin.capabilityflags |= mjPLUGIN_SDF;
|
plugin.capabilityflags |= mjPLUGIN_SDF;
|
||||||
|
|
||||||
const char* attributes[] = {"radius1", "radius2", "axis"};
|
plugin.nattribute = TorusAttribute::nattribute;
|
||||||
plugin.nattribute = sizeof(attributes) / sizeof(attributes[0]);
|
plugin.attributes = TorusAttribute::names;
|
||||||
plugin.attributes = attributes;
|
|
||||||
plugin.nstate = +[](const mjModel* m, int instance) { return 0; };
|
plugin.nstate = +[](const mjModel* m, int instance) { return 0; };
|
||||||
|
|
||||||
plugin.init = +[](const mjModel* m, mjData* d, int instance) {
|
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[3] = aabb[4] = attributes[0] + attributes[1];
|
||||||
aabb[5] = 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
|
} // namespace mujoco::plugin::sdf
|
||||||
|
|||||||
+7
-1
@@ -24,6 +24,12 @@
|
|||||||
|
|
||||||
namespace mujoco::plugin::sdf {
|
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 {
|
class Torus {
|
||||||
public:
|
public:
|
||||||
// Creates a new Torus instance or returns null on failure.
|
// Creates a new Torus instance or returns null on failure.
|
||||||
@@ -36,7 +42,7 @@ class Torus {
|
|||||||
|
|
||||||
static void RegisterPlugin();
|
static void RegisterPlugin();
|
||||||
|
|
||||||
mjtNum radius[2];
|
mjtNum attribute[TorusAttribute::nattribute];
|
||||||
|
|
||||||
private:
|
private:
|
||||||
Torus(const mjModel* m, mjData* d, int instance);
|
Torus(const mjModel* m, mjData* d, int instance);
|
||||||
|
|||||||
@@ -21,7 +21,6 @@
|
|||||||
#include <functional>
|
#include <functional>
|
||||||
#include <memory>
|
#include <memory>
|
||||||
#include <optional>
|
#include <optional>
|
||||||
#include <stdexcept>
|
|
||||||
#include <string>
|
#include <string>
|
||||||
#include <unordered_map>
|
#include <unordered_map>
|
||||||
#include <utility>
|
#include <utility>
|
||||||
@@ -303,15 +302,16 @@ void mjCMesh::LoadSDF() {
|
|||||||
throw mjCError(this, "plugin '%s' does not support signed distance fields", plugin->name);
|
throw mjCError(this, "plugin '%s' does not support signed distance fields", plugin->name);
|
||||||
}
|
}
|
||||||
|
|
||||||
int i=0;
|
|
||||||
std::vector<mjtNum> attributes(plugin->nattribute, 0);
|
std::vector<mjtNum> attributes(plugin->nattribute, 0);
|
||||||
for (auto const& pair : plugin_instance->config_attribs) {
|
std::vector<const char*> names(plugin->nattribute, 0);
|
||||||
try {
|
std::vector<const char*> values(plugin->nattribute, 0);
|
||||||
attributes[i++] = std::stod(pair.second);
|
for (int i=0; i < plugin->nattribute; i++) {
|
||||||
} catch (const std::invalid_argument& e) {
|
names[i] = plugin->attributes[i];
|
||||||
throw mjCError(this, "invalid attribute value for '%s'",
|
values[i] = plugin_instance->config_attribs[names[i]].c_str();
|
||||||
pair.first.c_str());
|
}
|
||||||
}
|
|
||||||
|
if (plugin->sdf_attribute) {
|
||||||
|
plugin->sdf_attribute(attributes.data(), names.data(), values.data());
|
||||||
}
|
}
|
||||||
|
|
||||||
mjtNum aabb[6] = {0};
|
mjtNum aabb[6] = {0};
|
||||||
|
|||||||
Reference in New Issue
Block a user