Improved SDF objective function.

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

doc/programming/extension.rst

@@ -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.
+  Collision points are found by minimizing the function A + B + abs(max(A, B)), where A and B are the two colliding
-  Because SDFs are non-convex, multiple starting points are required in order to converge to multiple local minima.
+  SDFs, via gradient descent. Because SDFs are non-convex, multiple starting points are required in order to converge to
-  The number of starting points is set using :ref:`sdf_initpoints<option-sdf_initpoints>`, and are
+  multiple local minima. The number of starting points is set using :ref:`sdf_initpoints<option-sdf_initpoints>`, and
-  initialized using the Halton sequence inside the intersection of the axis-aligned bounding boxes.
+  are initialized using the Halton sequence inside the intersection of the axis-aligned bounding boxes. The number of
-  The number of gradient descent iterations is set using :ref:`sdf_iterations<option-sdf_iterations>`.
+  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

@@ -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) +
+    return A + B + mju_abs(mju_max(A, B));
-           .5 * mju_max(B, 0) * mju_max(B, 0) - mju_min(A, 0) * mju_min(B, 0);
   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[0] = grad1[0] + grad2[0];
-    gradient[1] = grad1[1] * mju_max(A, 0) + grad2[1] * mju_max(B, 0);
+    gradient[1] = grad1[1] + grad2[1];
-    gradient[2] = grad1[2] * mju_max(A, 0) + grad2[2] * mju_max(B, 0);
+    gradient[2] = grad1[2] + grad2[2];
-    if (A < 0 && B < 0) {
+    mju_addToScl3(gradient, A > B ? grad1 : grad2, mju_max(A, B) > 0 ? 1 : -1);
-      gradient[0] = - grad1[0] * B - grad2[0] * A;
-      gradient[1] = - grad1[1] * B - grad2[1] * A;
-      gradient[2] = - grad1[2] * B - grad2[2] * A;
-    }
     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
+    // gradient descent - we use a special function of the two SDF as objective
-    sdf.type = mjSDFTYPE_QUADRATIC;
+    sdf.type = mjSDFTYPE_COLLISION;
-    dist = stepGradient(x, m, &sdf, (mjData*)d);
+    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

@@ -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_ {