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

Improved SDF objective function.

Browse Source 
For two colliding SDFs `A` and `B`, the function is `(A+B)+abs(max(A,B))`. This can also be written as `clearance + abs(intersection)`. This function has the following properties:

- On the penetrating surface, it is equal to `A+B`, i.e. the clearance field which is the penetration depth.
- The object boundary is a set of local minima in 3D space, since `abs(max(A, B))>0` away from the surface.
- Along the penetrating surface, the maximum penetration is a local minimum of the clearance since this field is orthogonal to the midsurface field `A-B`, which acts as a support plane for the contact.

[Level sets of the improved function for two colliding circles](https://www.wolframalpha.com/input?i=plot+sqrt%28x%5E2%2By%5E2%29-1+%2B+sqrt%28%28x-1%29%5E2%2B%28y-1%29%5E2%29-1+%2B+max%28max%28sqrt%28x%5E2%2By%5E2%29-1%2C+sqrt%28%28x-1%29%5E2%2B%28y-1%29%5E2%29-1%29%2C+0%29+-+min%28max%28sqrt%28x%5E2%2By%5E2%29-1%2C+sqrt%28%28x-1%29%5E2%2B%28y-1%29%5E2%29-1%29%2C+0%29+).

PiperOrigin-RevId: 585106580
Change-Id: I5b18a1ef262ceb0a1ab8abcad8acfd6dc1992e2c
This commit is contained in:
Alessio Quaglino
2023-11-24 09:06:19 -08:00
committed by Copybara-Service
parent 6a83b86553
commit d070b1893e
3 changed files with 22 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/programming/extension.rst
+5 -5
View File
@@ -288,11 +288,11 @@ Currently, there are three directories of first-party plugins:
<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 a quadratic form 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.
The number of starting points is set using :ref:`sdf_initpoints<option-sdf_initpoints>`, and are
initialized using the Halton sequence inside the intersection of the axis-aligned bounding boxes.
The number of gradient descent iterations is set using :ref:`sdf_iterations<option-sdf_iterations>`.
Collision points are found by minimizing the function A + B + abs(max(A, B)), where A and B are 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. The number of starting points is set using :ref:`sdf_initpoints<option-sdf_initpoints>`, and
are initialized using the Halton sequence inside the intersection of the axis-aligned bounding boxes. The number of
gradient descent iterations is set using :ref:`sdf_iterations<option-sdf_iterations>`.
While *exact* SDFs---encoding the precise signed distance to the surface---are preferred, collisions are possible with
any function whose value vanishes at the surface and grows monotonically away from it, with a negative sign in the
src/engine/engine_collision_sdf.c
+16 -17
View File
@@ -191,13 +191,12 @@ mjtNum mjc_distance(const mjModel* m, const mjData* d, const mjSDF* s, const mjt
mju_addTo3(y, s->relpos);
return geomDistance(m, d, s->plugin[0], s->id[0], x, s->geomtype[0]) -
geomDistance(m, d, s->plugin[1], s->id[1], y, s->geomtype[1]);
case mjSDFTYPE_QUADRATIC:
case mjSDFTYPE_COLLISION:
mju_rotVecMat(y, x, s->relmat);
mju_addTo3(y, s->relpos);
mjtNum A = geomDistance(m, d, s->plugin[0], s->id[0], x, s->geomtype[0]);
mjtNum B = geomDistance(m, d, s->plugin[1], s->id[1], y, s->geomtype[1]);
return .5 * mju_max(A, 0) * mju_max(A, 0) +
.5 * mju_max(B, 0) * mju_max(B, 0) - mju_min(A, 0) * mju_min(B, 0);
return A + B + mju_abs(mju_max(A, B));
default:
mjERROR("SDF type not available");
return 0;
@@ -233,7 +232,7 @@ void mjc_gradient(const mjModel* m, const mjData* d, const mjSDF* s,
mju_sub3(gradient, grad1, grad2);
mju_normalize3(gradient);
break;
case mjSDFTYPE_QUADRATIC:
case mjSDFTYPE_COLLISION:
mju_rotVecMat(y, x, s->relmat);
mju_addTo3(y, s->relpos);
mjtNum A = geomDistance(m, d, s->plugin[0], s->id[0], x, s->geomtype[0]);
@@ -241,14 +240,10 @@ void mjc_gradient(const mjModel* m, const mjData* d, const mjSDF* s,
geomGradient(grad1, m, d, s->plugin[0], s->id[0], x, s->geomtype[0]);
geomGradient(grad2, m, d, s->plugin[1], s->id[1], y, s->geomtype[1]);
mju_rotVecMatT(grad2, grad2, s->relmat);
gradient[0] = grad1[0] * mju_max(A, 0) + grad2[0] * mju_max(B, 0);
gradient[1] = grad1[1] * mju_max(A, 0) + grad2[1] * mju_max(B, 0);
gradient[2] = grad1[2] * mju_max(A, 0) + grad2[2] * mju_max(B, 0);
if (A < 0 && B < 0) {
gradient[0] = - grad1[0] * B - grad2[0] * A;
gradient[1] = - grad1[1] * B - grad2[1] * A;
gradient[2] = - grad1[2] * B - grad2[2] * A;
}
gradient[0] = grad1[0] + grad2[0];
gradient[1] = grad1[1] + grad2[1];
gradient[2] = grad1[2] + grad2[2];
mju_addToScl3(gradient, A > B ? grad1 : grad2, mju_max(A, B) > 0 ? 1 : -1);
break;
case mjSDFTYPE_SINGLE:
geomGradient(gradient, m, d, s->plugin[0], s->id[0], point[0], s->geomtype[0]);
@@ -386,13 +381,13 @@ static mjtNum stepFrankWolfe(mjtNum x[3], const mjtNum* corners, int ncorners,
// finds minimum using gradient descent
static mjtNum stepGradient(mjtNum x[3], const mjModel* m, const mjSDF* s,
mjData* d) {
mjData* d, int niter) {
const mjtNum c = .1; // reduction factor for the target decrease in the objective function
const mjtNum rho = .5; // reduction factor for the gradient scaling (alpha)
const mjtNum amin = 1e-4; // minimum value for alpha
mjtNum dist = mjMAXVAL;
for (int step=0; step < m->opt.sdf_iterations; step++) {
for (int step=0; step < niter; step++) {
mjtNum grad[3];
mjtNum alpha = 2.; // initial line search factor scaling the gradient
// the units of the gradient depend on s->type
@@ -767,9 +762,13 @@ int mjc_SDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2, m
// start counters
sdf_ptr[0]->compute(m, (mjData*)d, instance[0], mjPLUGIN_SDF);
// gradient descent - we use a quadratic form of the two SDF as objective
sdf.type = mjSDFTYPE_QUADRATIC;
dist = stepGradient(x, m, &sdf, (mjData*)d);
// gradient descent - we use a special function of the two SDF as objective
sdf.type = mjSDFTYPE_COLLISION;
dist = stepGradient(x, m, &sdf, (mjData*)d, m->opt.sdf_iterations);
// inexact SDFs can yield spurious collisions, filter them by projecting on the midsurface
sdf.type = mjSDFTYPE_INTERSECTION;
dist = stepGradient(x, m, &sdf, (mjData*)d, 1);
// contact point and normal - we use the midsurface where SDF1=SDF2 as zero level set
sdf.type = mjSDFTYPE_MIDSURFACE;
src/engine/engine_collision_sdf.h
+1 -1
View File
@@ -28,7 +28,7 @@ typedef enum mjtSDFType_ { // signed distance function (SDF) type
mjSDFTYPE_SINGLE = 0, // single SDF
mjSDFTYPE_INTERSECTION, // max(A, B)
mjSDFTYPE_MIDSURFACE, // A - B
mjSDFTYPE_QUADRATIC, // max(A, 0)^2/2 + max(B, 0)^2/2 + min(A, 0)*min(B, 0)
mjSDFTYPE_COLLISION, // A + B + abs(max(A, B))
} mjtSDFType;
struct mjSDF_ {