From d070b1893e271c105cee635ba70a26c6458de161 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Fri, 24 Nov 2023 09:06:19 -0800 Subject: [PATCH] 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 | 10 +++++----- src/engine/engine_collision_sdf.c | 33 +++++++++++++++---------------- src/engine/engine_collision_sdf.h | 2 +- 3 files changed, 22 insertions(+), 23 deletions(-) diff --git a/doc/programming/extension.rst b/doc/programming/extension.rst index 1e594380..9a69a702 100644 --- a/doc/programming/extension.rst +++ b/doc/programming/extension.rst @@ -288,11 +288,11 @@ Currently, there are three directories of first-party plugins: `__. 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`, 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`. + 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`, 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`. 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 diff --git a/src/engine/engine_collision_sdf.c b/src/engine/engine_collision_sdf.c index ec46002a..82237438 100644 --- a/src/engine/engine_collision_sdf.c +++ b/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) + - .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; diff --git a/src/engine/engine_collision_sdf.h b/src/engine/engine_collision_sdf.h index 00ee5f07..c6acbe1c 100644 --- a/src/engine/engine_collision_sdf.h +++ b/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_ {