chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions 2 Packages Projects Releases Wiki Activity

Remove sdflib plugin and move interpolation to engine_collision_sdf.

Browse Source 
Before:
```
 Simulation time      : 1.71 s
 Steps per second     : 5854
 Realtime factor      : 11.71 x
 Time per step        : 170.8 µs

 Newton iters / step  : 2.19
 Contacts / step      : 3.45
 Constraints / step   : 13.79
 Degrees of freedom   : 12
 Dynamic memory usage : 0.2% of 14M
```

After:
```
 Simulation time      : 1.41 s
 Steps per second     : 7093
 Realtime factor      : 14.19 x
 Time per step        : 141.0 µs

 Newton iters / step  : 2.19
 Contacts / step      : 3.45
 Constraints / step   : 13.79
 Degrees of freedom   : 12
 Dynamic memory usage : 0.2% of 14M
```

PiperOrigin-RevId: 781087896
Change-Id: Iaf69dccc5e95af5bac862344683c74f962162186
This commit is contained in:
Alessio Quaglino
2025-07-09 09:28:15 -07:00
committed by Copybara-Service
parent 96118da08b
commit cef02fabea
9 changed files with 189 additions and 374 deletions
Download Patch File Download Diff File Expand all files Collapse all files
model/plugin/sdf/cow.xml
+3 -14
View File
@@ -8,19 +8,12 @@
<config key="radius2" value="0.05"/>
</instance>
</plugin>
<plugin plugin="mujoco.sdf.sdflib">
<instance name="sdf">
<config key="aabb" value="0"/>
</instance>
</plugin>
</extension>
<asset>
<texture name="texspot" type="2d" file="spot.png"/>
<material name="matspot" texture="texspot"/>
<mesh name="spot" file="asset/spot.obj">
<plugin instance="sdf"/>
</mesh>
<mesh name="spot" file="asset/spot.obj"/>
<mesh name="torus">
<plugin instance="torus"/>
</mesh>
@@ -48,15 +41,11 @@
</geom>
</body>
<body euler="90 0 0" pos="0 0 .7">
<geom type="sdf" name="cow1" mesh="spot" material="matspot">
<plugin instance="sdf"/>
</geom>
<geom type="sdf" name="cow1" mesh="spot" material="matspot"/>
</body>
<body pos="0.05 .25 2.2">
<freejoint/>
<geom type="sdf" name="cow2" mesh="spot" material="matspot">
<plugin instance="sdf"/>
</geom>
<geom type="sdf" name="cow2" mesh="spot" material="matspot"/>
</body>
<light name="left" pos="0 0 1"/>
<light name="right" pos="1 0 1"/>
plugin/sdf/CMakeLists.txt
-2
View File
@@ -28,8 +28,6 @@ set(MUJOCO_SDF_SRCS
register.cc
nut.cc
nut.h
sdflib.cc
sdflib.h
torus.cc
torus.h
)
plugin/sdf/README.md
-10
View File
@@ -62,16 +62,6 @@ Parameters:
- `radius1` [m]: major radius (default `0.35`).
- `radius1` [m]: minor radius (default `0.15`).
### SdfLib
Implemented in [sdflib.cc](sdflib.cc). Example usage in [cow.xml](../../model/plugin/sdf/cow.xml).
This plugin uses the library [TriangleMeshDistance](https://github.com/InteractiveComputerGraphics/TriangleMeshDistance)
to compute a voxel-based approximation of a user-specified mesh. The mesh can be arbitrary and not necessarily convex.
This offers an alternative to convex-decomposed meshes. The performance is likely to be slower than that of analytic
SDFs, since a cubic approximation has to be evaluated on the convex grid. However, the SDF generation is done
automatically, simplifying the task of creating an SDF, which can be difficult for complex shapes.
### 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
plugin/sdf/register.cc
-2
View File
@@ -17,7 +17,6 @@
#include "gear.h"
#include "nut.h"
#include "torus.h"
#include "sdflib.h"
namespace mujoco::plugin::sdf {
@@ -27,7 +26,6 @@ mjPLUGIN_LIB_INIT {
Gear::RegisterPlugin();
Nut::RegisterPlugin();
Torus::RegisterPlugin();
SdfLib::RegisterPlugin();
}
} // namespace mujoco::plugin::sdf
plugin/sdf/sdflib.cc
-279
View File
@@ -1,279 +0,0 @@
// Copyright 2022 DeepMind Technologies Limited
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <cstdint>
#include <cstring>
#include <optional>
#include <utility>
#include <vector>
#include <mujoco/mjplugin.h>
#include <mujoco/mujoco.h>
#include "sdf.h"
#include "sdflib.h"
namespace mujoco::plugin::sdf {
namespace {
mjtNum boxProjection(mjtNum point[3], const mjtNum box[6]) {
mjtNum r[3] = {point[0] - box[0], point[1] - box[1], point[2] - box[2]};
mjtNum q[3] = {mju_abs(r[0]) - box[3], mju_abs(r[1]) - box[4],
mju_abs(r[2]) - box[5]};
mjtNum dist_sqr = 0;
mjtNum eps = 1e-6;
// skip the projection if inside
if (q[0] <= 0 && q[1] <= 0 && q[2] <= 0) {
return mju_max(q[0], mju_max(q[1], q[2]));
}
// in-place projection inside the box if outside
if ( q[0] >= 0 ) {
dist_sqr += q[0] * q[0];
point[0] -= r[0] > 0 ? (q[0]+eps) : -(q[0]+eps);
}
if ( q[1] >= 0 ) {
dist_sqr += q[1] * q[1];
point[1] -= r[1] > 0 ? (q[1]+eps) : -(q[1]+eps);
}
if ( q[2] >= 0 ) {
dist_sqr += q[2] * q[2];
point[2] -= r[2] > 0 ? (q[2]+eps) : -(q[2]+eps);
}
return mju_sqrt(dist_sqr);
}
// find the octree leaf containing the point p, return the index of the leaf and
// populate the weights of the interpolated function (if w is not null) and of
// its gradient (if dw is not null) using the vertices as degrees of freedom for
// trilinear interpolation.
static int findOct(mjtNum w[8], mjtNum dw[8][3], const mjtNum* oct_aabb,
const int* oct_child, const mjtNum p[3]) {
std::vector<int> stack = {0};
mjtNum eps = 1e-8;
while (!stack.empty()) {
int node = stack.back();
stack.pop_back();
mjtNum vmin[3], vmax[3];
if (node == -1) { // SHOULD NOT OCCUR
mju_error("Invalid node number");
return -1;
}
for (int j = 0; j < 3; j++) {
vmin[j] = oct_aabb[6*node+j] - oct_aabb[6*node+3+j];
vmax[j] = oct_aabb[6*node+j] + oct_aabb[6*node+3+j];
}
// check if the point is inside the aabb of the octree node
if (p[0] + eps < vmin[0] || p[0] - eps > vmax[0] ||
p[1] + eps < vmin[1] || p[1] - eps > vmax[1] ||
p[2] + eps < vmin[2] || p[2] - eps > vmax[2]) {
continue;
}
mjtNum coord[3] = {(p[0] - vmin[0]) / (vmax[0] - vmin[0]),
(p[1] - vmin[1]) / (vmax[1] - vmin[1]),
(p[2] - vmin[2]) / (vmax[2] - vmin[2])};
// check if the node is a leaf
if (oct_child[8*node+0] == -1 && oct_child[8*node+1] == -1 &&
oct_child[8*node+2] == -1 && oct_child[8*node+3] == -1 &&
oct_child[8*node+4] == -1 && oct_child[8*node+5] == -1 &&
oct_child[8*node+6] == -1 && oct_child[8*node+7] == -1) {
for (int j = 0; j < 8; j++) {
if (w) {
w[j] = (j & 1 ? coord[0] : 1 - coord[0]) *
(j & 2 ? coord[1] : 1 - coord[1]) *
(j & 4 ? coord[2] : 1 - coord[2]);
}
if (dw) {
dw[j][0] = (j & 1 ? 1 : -1) *
(j & 2 ? coord[1] : 1 - coord[1]) *
(j & 4 ? coord[2] : 1 - coord[2]);
dw[j][1] = (j & 1 ? coord[0] : 1 - coord[0]) *
(j & 2 ? 1 : -1) *
(j & 4 ? coord[2] : 1 - coord[2]);
dw[j][2] = (j & 1 ? coord[0] : 1 - coord[0]) *
(j & 2 ? coord[1] : 1 - coord[1]) *
(j & 4 ? 1 : -1);
}
}
return node;
}
// compute which of 8 children to visit next
int x = coord[0] < .5 ? 1 : 0;
int y = coord[1] < .5 ? 1 : 0;
int z = coord[2] < .5 ? 1 : 0;
stack.push_back(oct_child[8*node + 4*z + 2*y + x]);
}
mju_error("Node not found"); // SHOULD NOT OCCUR
return -1;
}
} // namespace
// factory function
std::optional<SdfLib> SdfLib::Create(const mjModel* m, mjData* d,
int instance) {
int geomid = 0;
for (int i = 0; i < m->ngeom; ++i) {
if (m->geom_plugin[i] == instance) {
geomid = i;
break;
}
}
return SdfLib(m, m->geom_dataid[geomid]);
}
// plugin constructor
SdfLib::SdfLib(const mjModel* m, int meshid) {
int octadr = m->mesh_octadr[meshid];
int octnum = m->mesh_octnum[meshid];
oct_aabb_.assign(m->oct_aabb + 6*octadr,
m->oct_aabb + 6*octadr + 6*octnum);
oct_child_.assign(m->oct_child + 8 * octadr,
m->oct_child + 8 * octadr + 8 * octnum);
sdf_coeff_.assign(8 * octnum, 0);
memcpy(sdf_coeff_.data(), m->oct_coeff + 8*octadr, 8*octnum*sizeof(mjtNum));
mju_copy(box_, m->oct_aabb + 6*octadr, 6);
}
// plugin computation
void SdfLib::Compute(const mjModel* m, mjData* d, int instance) {
visualizer_.Next();
}
// plugin reset
void SdfLib::Reset() {
visualizer_.Reset();
}
// plugin visualization
void SdfLib::Visualize(const mjModel* m, mjData* d, const mjvOption* opt,
mjvScene* scn, int instance) {
visualizer_.Visualize(m, d, opt, scn, instance);
}
// sdf
mjtNum SdfLib::Distance(const mjtNum p[3]) const {
mjtNum w[8];
mjtNum sdf = 0;
mjtNum point[3] = {p[0], p[1], p[2]};
mjtNum boxDist = boxProjection(point, box_);
if (boxDist > 0) {
return boxDist;
}
int node = findOct(w, nullptr, oct_aabb_.data(), oct_child_.data(), point);
for (int i = 0; i < 8; ++i) {
sdf += w[i] * sdf_coeff_[8*node + i];
}
return sdf;
}
// gradient of sdf
void SdfLib::Gradient(mjtNum grad[3], const mjtNum point[3]) const {
mjtNum p[3] = {point[0], point[1], point[2]};
// analytic in the interior
if (boxProjection(p, box_) <= 0) {
mjtNum dw[8][3];
mju_zero3(grad);
int node = findOct(nullptr, dw, oct_aabb_.data(), oct_child_.data(), p);
for (int i = 0; i < 8; ++i) {
grad[0] += dw[i][0] * sdf_coeff_[8*node + i];
grad[1] += dw[i][1] * sdf_coeff_[8*node + i];
grad[2] += dw[i][2] * sdf_coeff_[8*node + i];
}
return;
}
// finite difference in the exterior
mjtNum eps = 1e-8;
mjtNum dist0 = Distance(point);
mjtNum pointX[3] = {point[0]+eps, point[1], point[2]};
mjtNum distX = Distance(pointX);
mjtNum pointY[3] = {point[0], point[1]+eps, point[2]};
mjtNum distY = Distance(pointY);
mjtNum pointZ[3] = {point[0], point[1], point[2]+eps};
mjtNum distZ = Distance(pointZ);
grad[0] = (distX - dist0) / eps;
grad[1] = (distY - dist0) / eps;
grad[2] = (distZ - dist0) / eps;
}
// plugin registration
void SdfLib::RegisterPlugin() {
mjpPlugin plugin;
mjp_defaultPlugin(&plugin);
plugin.name = "mujoco.sdf.sdflib";
plugin.capabilityflags |= mjPLUGIN_SDF;
const char* attributes[] = {"aabb"};
plugin.nattribute = sizeof(attributes) / sizeof(attributes[0]);
plugin.attributes = attributes;
plugin.nstate = +[](const mjModel* m, int instance) { return 0; };
plugin.init = +[](const mjModel* m, mjData* d, int instance) {
auto sdf_or_null = SdfLib::Create(m, d, instance);
if (!sdf_or_null.has_value()) {
return -1;
}
d->plugin_data[instance] = reinterpret_cast<uintptr_t>(
new SdfLib(std::move(*sdf_or_null)));
return 0;
};
plugin.destroy = +[](mjData* d, int instance) {
delete reinterpret_cast<SdfLib*>(d->plugin_data[instance]);
d->plugin_data[instance] = 0;
};
plugin.reset = +[](const mjModel* m, mjtNum* plugin_state, void* plugin_data,
int instance) {
auto sdf = reinterpret_cast<SdfLib*>(plugin_data);
sdf->Reset();
};
plugin.visualize = +[](const mjModel* m, mjData* d, const mjvOption* opt,
mjvScene* scn, int instance) {
auto* sdf = reinterpret_cast<SdfLib*>(d->plugin_data[instance]);
sdf->Visualize(m, d, opt, scn, instance);
};
plugin.compute =
+[](const mjModel* m, mjData* d, int instance, int capability_bit) {
auto* sdf = reinterpret_cast<SdfLib*>(d->plugin_data[instance]);
sdf->Compute(m, d, instance);
};
plugin.sdf_distance =
+[](const mjtNum point[3], const mjData* d, int instance) {
auto* sdf = reinterpret_cast<SdfLib*>(d->plugin_data[instance]);
return sdf->Distance(point);
};
plugin.sdf_gradient = +[](mjtNum gradient[3], const mjtNum point[3],
const mjData* d, int instance) {
auto* sdf = reinterpret_cast<SdfLib*>(d->plugin_data[instance]);
sdf->visualizer_.AddPoint(point);
sdf->Gradient(gradient, point);
};
mjp_registerPlugin(&plugin);
}
} // namespace mujoco::plugin::sdf
plugin/sdf/sdflib.h
-57
View File
@@ -1,57 +0,0 @@
// Copyright 2022 DeepMind Technologies Limited
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef MUJOCO_PLUGIN_SDF_SDFLIB_H_
#define MUJOCO_PLUGIN_SDF_SDFLIB_H_
#include <optional>
#include <vector>
#include <mujoco/mjdata.h>
#include <mujoco/mjmodel.h>
#include <mujoco/mjtnum.h>
#include <mujoco/mjvisualize.h>
#include "sdf.h"
namespace mujoco::plugin::sdf {
class SdfLib {
public:
// Creates a new SdfLib instance or returns null on failure.
static std::optional<SdfLib> Create(const mjModel* m, mjData* d,
int instance);
SdfLib(SdfLib&&) = default;
~SdfLib() = default;
void Reset();
void Visualize(const mjModel* m, mjData* d, const mjvOption* opt,
mjvScene* scn, int instance);
void Compute(const mjModel* m, mjData* d, int instance);
mjtNum Distance(const mjtNum point[3]) const;
void Gradient(mjtNum grad[3], const mjtNum point[3]) const;
static void RegisterPlugin();
private:
SdfLib(const mjModel* m, int meshid);
SdfVisualizer visualizer_;
std::vector<double> sdf_coeff_;
mjtNum box_[6];
std::vector<mjtNum> oct_aabb_;
std::vector<int> oct_child_;
};
} // namespace mujoco::plugin::sdf
#endif // MUJOCO_PLUGIN_SDF_SDFLIB_H_
src/engine/engine_collision_sdf.c
+184 -7
View File
@@ -34,6 +34,168 @@
#define MAXMESHPNT 500
//---------------------------- interpolated sdf -------------------------------------------
mjtNum boxProjection(mjtNum point[3], const mjtNum box[6]) {
mjtNum r[3] = {point[0] - box[0], point[1] - box[1], point[2] - box[2]};
mjtNum q[3] = {mju_abs(r[0]) - box[3], mju_abs(r[1]) - box[4],
mju_abs(r[2]) - box[5]};
mjtNum dist_sqr = 0;
mjtNum eps = 1e-6;
// skip the projection if inside
if (q[0] <= 0 && q[1] <= 0 && q[2] <= 0) {
return mju_max(q[0], mju_max(q[1], q[2]));
}
// in-place projection inside the box if outside
if ( q[0] >= 0 ) {
dist_sqr += q[0] * q[0];
point[0] -= r[0] > 0 ? (q[0]+eps) : -(q[0]+eps);
}
if ( q[1] >= 0 ) {
dist_sqr += q[1] * q[1];
point[1] -= r[1] > 0 ? (q[1]+eps) : -(q[1]+eps);
}
if ( q[2] >= 0 ) {
dist_sqr += q[2] * q[2];
point[2] -= r[2] > 0 ? (q[2]+eps) : -(q[2]+eps);
}
return mju_sqrt(dist_sqr);
}
// find the octree leaf containing the point p, return the index of the leaf and
// populate the weights of the interpolated function (if w is not null) and of
// its gradient (if dw is not null) using the vertices as degrees of freedom for
// trilinear interpolation.
static int findOct(mjtNum w[8], mjtNum dw[8][3], const mjtNum* oct_aabb,
const int* oct_child, const mjtNum p[3]) {
int stack = 0;
mjtNum eps = 1e-8;
int niter = 100;
while (niter-- > 0) {
int node = stack;
mjtNum vmin[3], vmax[3];
if (node == -1) { // SHOULD NOT OCCUR
mju_error("Invalid node number");
return -1;
}
for (int j = 0; j < 3; j++) {
vmin[j] = oct_aabb[6*node+j] - oct_aabb[6*node+3+j];
vmax[j] = oct_aabb[6*node+j] + oct_aabb[6*node+3+j];
}
// check if the point is inside the aabb of the octree node
if (p[0] + eps < vmin[0] || p[0] - eps > vmax[0] ||
p[1] + eps < vmin[1] || p[1] - eps > vmax[1] ||
p[2] + eps < vmin[2] || p[2] - eps > vmax[2]) {
continue;
}
mjtNum coord[3] = {(p[0] - vmin[0]) / (vmax[0] - vmin[0]),
(p[1] - vmin[1]) / (vmax[1] - vmin[1]),
(p[2] - vmin[2]) / (vmax[2] - vmin[2])};
// check if the node is a leaf
if (oct_child[8*node+0] == -1 && oct_child[8*node+1] == -1 &&
oct_child[8*node+2] == -1 && oct_child[8*node+3] == -1 &&
oct_child[8*node+4] == -1 && oct_child[8*node+5] == -1 &&
oct_child[8*node+6] == -1 && oct_child[8*node+7] == -1) {
for (int j = 0; j < 8; j++) {
if (w) {
w[j] = (j & 1 ? coord[0] : 1 - coord[0]) *
(j & 2 ? coord[1] : 1 - coord[1]) *
(j & 4 ? coord[2] : 1 - coord[2]);
}
if (dw) {
dw[j][0] = (j & 1 ? 1 : -1) *
(j & 2 ? coord[1] : 1 - coord[1]) *
(j & 4 ? coord[2] : 1 - coord[2]);
dw[j][1] = (j & 1 ? coord[0] : 1 - coord[0]) *
(j & 2 ? 1 : -1) *
(j & 4 ? coord[2] : 1 - coord[2]);
dw[j][2] = (j & 1 ? coord[0] : 1 - coord[0]) *
(j & 2 ? coord[1] : 1 - coord[1]) *
(j & 4 ? 1 : -1);
}
}
return node;
}
// compute which of 8 children to visit next
int x = coord[0] < .5 ? 1 : 0;
int y = coord[1] < .5 ? 1 : 0;
int z = coord[2] < .5 ? 1 : 0;
stack = oct_child[8 * node + 4*z + 2*y + x];
}
mju_error("Node not found"); // SHOULD NOT OCCUR
return -1;
}
// sdf
mjtNum oct_distance(const mjModel* m, const mjtNum p[3], int meshid) {
int octadr = m->mesh_octadr[meshid];
int* oct_child = m->oct_child + 8*octadr;
mjtNum* oct_aabb = m->oct_aabb + 6*octadr;
mjtNum* oct_coeff = m->oct_coeff + 8*octadr;
mjtNum w[8];
mjtNum sdf = 0;
mjtNum point[3] = {p[0], p[1], p[2]};
mjtNum boxDist = boxProjection(point, oct_aabb);
if (boxDist > 0) {
return boxDist;
}
int node = findOct(w, NULL, oct_aabb, oct_child, point);
for (int i = 0; i < 8; ++i) {
sdf += w[i] * oct_coeff[8*node + i];
}
return sdf;
}
// gradient of sdf
void oct_gradient(const mjModel* m, mjtNum grad[3], const mjtNum point[3], int meshid) {
mju_zero3(grad);
mjtNum p[3] = {point[0], point[1], point[2]};
int octadr = m->mesh_octadr[meshid];
int* oct_child = m->oct_child + 8*octadr;
mjtNum* oct_aabb = m->oct_aabb + 6*octadr;
mjtNum* oct_coeff = m->oct_coeff + 8*octadr;
// analytic in the interior
if (boxProjection(p, oct_aabb) <= 0) {
mjtNum dw[8][3];
int node = findOct(NULL, dw, oct_aabb, oct_child, p);
for (int i = 0; i < 8; ++i) {
grad[0] += dw[i][0] * oct_coeff[8*node + i];
grad[1] += dw[i][1] * oct_coeff[8*node + i];
grad[2] += dw[i][2] * oct_coeff[8*node + i];
}
return;
}
// finite difference in the exterior
mjtNum eps = 1e-8;
mjtNum dist0 = oct_distance(m, point, meshid);
mjtNum pointX[3] = {point[0]+eps, point[1], point[2]};
mjtNum distX = oct_distance(m, pointX, meshid);
mjtNum pointY[3] = {point[0], point[1]+eps, point[2]};
mjtNum distY = oct_distance(m, pointY, meshid);
mjtNum pointZ[3] = {point[0], point[1], point[2]+eps};
mjtNum distZ = oct_distance(m, pointZ, meshid);
grad[0] = (distX - dist0) / eps;
grad[1] = (distY - dist0) / eps;
grad[2] = (distZ - dist0) / eps;
}
//---------------------------- primitives sdf ---------------------------------------------
static void radialField3d(mjtNum field[3], const mjtNum a[3], const mjtNum x[3],
@@ -100,7 +262,11 @@ static mjtNum geomDistance(const mjModel* m, const mjData* d, const mjpPlugin* p
b[1] = mju_max(a[1], 0);
return mju_min(mju_max(a[0], a[1]), 0) + mju_norm(b, 2);
case mjGEOM_SDF:
return p->sdf_distance(x, d, i);
if (p) {
return p->sdf_distance(x, d, i);
} else {
return oct_distance(m, x, i);
}
default:
mjERROR("sdf collisions not available for geom type %d", type);
return 0;
@@ -199,7 +365,11 @@ static void geomGradient(mjtNum gradient[3], const mjModel* m, const mjData* d,
}
break;
case mjGEOM_SDF:
p->sdf_gradient(gradient, x, d, i);
if (p) {
p->sdf_gradient(gradient, x, d, i);
} else {
oct_gradient(m, gradient, x, i);
}
break;
default:
mjERROR("sdf collisions not available for geom type %d", type);
@@ -608,7 +778,8 @@ int mjc_MeshSDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int g
// get sdf plugin
int instance = m->geom_plugin[g2];
const mjpPlugin* sdf_ptr = mjc_getSDF(m, g2);
const mjpPlugin* sdf_ptr = instance == -1 ? NULL : mjc_getSDF(m, g2);
instance = instance == -1 ? m->geom_dataid[g2] : instance;
mjtGeom geomtype = mjGEOM_SDF;
// copy into data
@@ -750,22 +921,26 @@ int mjc_SDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2, m
mjtGeom geomtypes[2] = {m->geom_type[g2], m->geom_type[g1]};
instance[0] = m->geom_plugin[g2];
sdf_ptr[0] = mjc_getSDF(m, g2);
sdf_ptr[0] = instance[0] == -1 ? NULL : mjc_getSDF(m, g2);
// get sdf plugins
if (m->geom_type[g1] == mjGEOM_SDF) {
instance[1] = m->geom_plugin[g1];
sdf_ptr[1] = mjc_getSDF(m, g1);
sdf_ptr[1] = instance[1] == -1 ? NULL : mjc_getSDF(m, g1);
} else {
instance[1] = g1;
sdf_ptr[1] = NULL;
}
// reset visualization count
sdf_ptr[0]->reset(m, NULL, (void*)(d->plugin_data[instance[0]]), instance[0]);
if (sdf_ptr[0]) {
sdf_ptr[0]->reset(m, NULL, (void*)(d->plugin_data[instance[0]]), instance[0]);
}
// copy into sdf
mjSDF sdf;
instance[0] = instance[0] == -1 ? m->geom_dataid[g2] : instance[0];
instance[1] = instance[1] == -1 ? m->geom_dataid[g1] : instance[1];
sdf.id = instance;
sdf.relpos = offset21;
sdf.relmat = rotation21;
@@ -794,7 +969,9 @@ int mjc_SDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2, m
i++;
// start counters
sdf_ptr[0]->compute(m, (mjData*)d, instance[0], mjPLUGIN_SDF);
if (sdf_ptr[0]) {
sdf_ptr[0]->compute(m, (mjData*)d, instance[0], mjPLUGIN_SDF);
}
// gradient descent - we use a special function of the two SDF as objective
sdf.type = mjSDFTYPE_COLLISION;
src/user/user_mesh.cc
+1 -2
View File
@@ -1551,8 +1551,7 @@ void mjCMesh::Process() {
octree_.CreateOctree(aamm);
// compute sdf coefficients
// TODO: only check !plugin.active once sdflib is removed
if (plugin.active && *plugin.name == "sdf") {
if (!plugin.active) {
tmd::TriangleMeshDistance sdf(vert_.data(), nvert(), face_.data(), nface());
// TODO: do not evaluate the SDF multiple times at the same vertex
test/engine/engine_plugin_test.cc
+1 -1
View File
@@ -37,7 +37,7 @@ using ::testing::DoubleNear;
using ::testing::HasSubstr;
using ::testing::NotNull;
constexpr int kNumTruePlugins = 10;
constexpr int kNumTruePlugins = 9;
constexpr int kNumFakePlugins = 30;
constexpr int kNumTestPlugins = 4;