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

New SDF objective function.

Browse Source 
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
This commit is contained in:
Alessio Quaglino
2023-11-20 05:05:42 -08:00
committed by Copybara-Service
parent 98082ae7f9
commit dbf44fe2b4
4 changed files with 44 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/XMLreference.rst
+1 -1
View File
@@ -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
+1 -1
View File
@@ -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
+37 -5
View File
@@ -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
sdf.type = mjSDFTYPE_INTERSECTION;
// gradient descent - we use a quadratic form of the two SDF as objective
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
+5 -4
View File
@@ -24,10 +24,11 @@
extern "C" {
#endif
typedef enum mjtSDFType_ {
mjSDFTYPE_SINGLE = 0,
mjSDFTYPE_INTERSECTION,
mjSDFTYPE_AVERAGE,
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)
} mjtSDFType;
struct mjSDF_ {