New SDF objective function.

This solves the jittery behavior observed with the gear example in the case of small applied torques (~0.5).

The new quadratic option has the form max(A, 0)^2/2 + max(B, 0)^2/2 - min(A, 0)*min(B, 0). This function has a minimum in the intersections of two SDFs A and B, while avoiding the flat areas which would be generated if only the clearance field A+B were employed. See for example [the function resulting from two colliding circles](https://www.wolframalpha.com/input?i=minimize+max%28sqrt%28x%5E2%2By%5E2%29-1%2C0%29%5E2+%2B+max%28sqrt%28%28x-1%29%5E2%2B%28y-1%29%5E2%29-1%2C0%29%5E2+-+2*min%28sqrt%28x%5E2%2By%5E2%29-1%2C0%29*min%28sqrt%28%28x-1%29%5E2%2B%28y-1%29%5E2%29-1%2C0%29)

PiperOrigin-RevId: 583992855
Change-Id: I135a1b5931cd136d7d33cc275f8d361a8b7e290c

doc/XMLreference.rst

@@ -1877,7 +1877,7 @@ adjust it properly through the XML.
 .. _option-sdf_initpoints:
 
 :at:`sdf_initpoints`: :at-val:`int, "40"`
-   Number of starting points used for fining contacts with Signed Distance Field collisions.
+   Number of starting points used for finding contacts with Signed Distance Field collisions.
 
 .. _option-actuatorgroupdisable:
 

doc/programming/extension.rst

@@ -269,7 +269,7 @@ 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 the maximum of the two colliding SDFs via gradient descent.
+  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.

src/engine/engine_collision_sdf.c

@@ -186,6 +186,18 @@ mjtNum mjc_distance(const mjModel* m, const mjData* d, const mjSDF* s, const mjt
     mju_addTo3(y, s->relpos);
     return mju_max(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_MIDSURFACE:
+    mju_rotVecMat(y, x, s->relmat);
+    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:
+    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);
   default:
     mjERROR("SDF type not available");
     return 0;
@@ -197,6 +209,7 @@ void mjc_gradient(const mjModel* m, const mjData* d, const mjSDF* s,
                   mjtNum gradient[3], const mjtNum x[3]) {
   mjtNum y[3];
   const mjtNum* point[2] = {x, y};
+  mjtNum grad1[3], grad2[3];
 
   switch (s->type) {
   case mjSDFTYPE_INTERSECTION:
@@ -209,10 +222,9 @@ void mjc_gradient(const mjModel* m, const mjData* d, const mjSDF* s,
       mju_rotVecMatT(gradient, gradient, s->relmat);
     }
     break;
-  case mjSDFTYPE_AVERAGE:
+  case mjSDFTYPE_MIDSURFACE:
     mju_rotVecMat(y, x, s->relmat);
     mju_addTo3(y, s->relpos);
-    mjtNum grad1[3], grad2[3];
     geomGradient(grad1, m, d, s->plugin[0], s->id[0], x, s->geomtype[0]);
     mju_normalize3(grad1);
     geomGradient(grad2, m, d, s->plugin[1], s->id[1], y, s->geomtype[1]);
@@ -221,6 +233,24 @@ void mjc_gradient(const mjModel* m, const mjData* d, const mjSDF* s,
     mju_sub3(gradient, grad1, grad2);
     mju_normalize3(gradient);
     break;
+  case mjSDFTYPE_QUADRATIC:
+    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]);
+    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;
+    }
+    mju_normalize3(gradient);
+    break;
   case mjSDFTYPE_SINGLE:
     geomGradient(gradient, m, d, s->plugin[0], s->id[0], point[0], s->geomtype[0]);
     break;
@@ -716,10 +746,12 @@ 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
+    // gradient descent - we use a quadratic form of the two SDF as objective
-    sdf.type = mjSDFTYPE_INTERSECTION;
+    sdf.type = mjSDFTYPE_QUADRATIC;
     dist = stepGradient(x, m, &sdf, (mjData*)d);
-    sdf.type = mjSDFTYPE_AVERAGE;
+
+    // contact point and normal - we use the midsurface where SDF1=SDF2 as zero level set
+    sdf.type = mjSDFTYPE_MIDSURFACE;
     cnt = addContact(contacts, con, x, pos2true, squat2, dist, cnt, m, &sdf, (mjData*)d);
 
     // SHOULD NOT OCCUR

src/engine/engine_collision_sdf.h

@@ -24,10 +24,11 @@
 extern "C" {
 #endif
 
-typedef enum mjtSDFType_ {
+typedef enum mjtSDFType_ {        // signed distance function (SDF) type
-  mjSDFTYPE_SINGLE     = 0,
+  mjSDFTYPE_SINGLE     = 0,       // single SDF
-  mjSDFTYPE_INTERSECTION,
+  mjSDFTYPE_INTERSECTION,         // max(A, B)
-  mjSDFTYPE_AVERAGE,
+  mjSDFTYPE_MIDSURFACE,           // A - B
+  mjSDFTYPE_QUADRATIC,            // max(A, 0)^2/2 + max(B, 0)^2/2 + min(A, 0)*min(B, 0)
 } mjtSDFType;
 
 struct mjSDF_ {