From e97e5d31d0e5c3205467cc495aa128dd707f520b Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Fri, 27 Jun 2025 07:20:43 -0700 Subject: [PATCH] Replace box SDF with a smooth approximation. This approximation is equivalent to the exact SDF on the surface and externally, but it is smooth on the inside, rotating the gradient from radial at the box center to normal to the faces at the surface, and interpolated linearly (using two Euler angles) in between. This enables stable contact gradients for deeper penetrations. PiperOrigin-RevId: 776574022 Change-Id: I562976e9f0535b0067aa12b2bc9d48eccb6472e9 --- src/engine/engine_collision_sdf.c | 33 ++++++++++++++++++++---- test/engine/engine_collision_sdf_test.cc | 4 +-- 2 files changed, 30 insertions(+), 7 deletions(-) diff --git a/src/engine/engine_collision_sdf.c b/src/engine/engine_collision_sdf.c index 591c96b8..b1942970 100644 --- a/src/engine/engine_collision_sdf.c +++ b/src/engine/engine_collision_sdf.c @@ -36,6 +36,19 @@ //---------------------------- primitives sdf --------------------------------------------- +static void radialField3d(mjtNum field[3], const mjtNum a[3], const mjtNum x[3], + const mjtNum size[3]) { + field[0] = -size[0] / a[0]; + field[1] = -size[1] / a[1]; + field[2] = -size[2] / a[2]; + mju_normalize3(field); + + // flip sign if necessary + if (x[0] < 0) field[0] = -field[0]; + if (x[1] < 0) field[1] = -field[1]; + if (x[2] < 0) field[2] = -field[2]; +} + static mjtNum geomDistance(const mjModel* m, const mjData* d, const mjpPlugin* p, int i, const mjtNum x[3], mjtGeom type) { mjtNum a[3], b[3]; @@ -48,13 +61,23 @@ static mjtNum geomDistance(const mjModel* m, const mjData* d, const mjpPlugin* p case mjGEOM_SPHERE: return mju_norm3(x) - size[0]; case mjGEOM_BOX: + // compute shortest distance to box surface if outside, otherwise + // intersect with a unit gradient that linearly rotates from radial to the face normals a[0] = mju_abs(x[0]) - size[0]; a[1] = mju_abs(x[1]) - size[1]; a[2] = mju_abs(x[2]) - size[2]; - b[0] = mju_max(a[0], 0); - b[1] = mju_max(a[1], 0); - b[2] = mju_max(a[2], 0); - return mju_norm3(b) + mju_min(mju_max(a[0], mju_max(a[1], a[2])), 0); + if (a[0] >= 0 || a[1] >= 0 || a[2] >= 0) { + b[0] = mju_max(a[0], 0); + b[1] = mju_max(a[1], 0); + b[2] = mju_max(a[2], 0); + return mju_norm3(b) + mju_min(mju_max(a[0], mju_max(a[1], a[2])), 0); + } + radialField3d(b, a, x, size); + mjtNum t[3]; + t[0] = -a[0] / mju_abs(b[0]); + t[1] = -a[1] / mju_abs(b[1]); + t[2] = -a[2] / mju_abs(b[2]); + return -mju_min(t[0], mju_min(t[1], t[2])) * mju_norm3(b); case mjGEOM_CAPSULE: a[0] = x[0]; a[1] = x[1]; @@ -111,7 +134,7 @@ static void geomGradient(mjtNum gradient[3], const mjModel* m, const mjData* d, int k = a[0] > a[1] ? 0 : 1; int l = a[2] > a[k] ? 2 : k; if (a[l] < 0) { - gradient[l] = x[l] / mju_abs(x[l]); + radialField3d(gradient, a, x, size); } else { b[0] = mju_max(a[0], 0); b[1] = mju_max(a[1], 0); diff --git a/test/engine/engine_collision_sdf_test.cc b/test/engine/engine_collision_sdf_test.cc index e20d2fbe..5d38c627 100644 --- a/test/engine/engine_collision_sdf_test.cc +++ b/test/engine/engine_collision_sdf_test.cc @@ -60,7 +60,7 @@ TEST_F(SdfTest, SdfPrimitive) { {-1, 0, 0, mju_sqrt(2)-1, mju_sqrt(2)-1, mju_sqrt(3)-1}, // sphere {-.1, .9, .9, mju_sqrt(2)-.1, .9, mju_sqrt(2)-.1}, // capsule {-1, 0, 0, mju_sqrt(2)-1, 0, mju_sqrt(2)-1}, // cylinder - {-1, 0, 0, 0, 0, 0}, // box + {-mju_sqrt(3), 0, 0, 0, 0, 0}, // box }; mjtNum points[kpoints][3] = {{0, 0, 0}, {1, 0, 0}, {0, 1, 0}, {1, 1, 0}, {0, 1, 1}, {1, 1, 1}}; @@ -71,7 +71,7 @@ TEST_F(SdfTest, SdfPrimitive) { sdf.type = mjSDFTYPE_SINGLE; sdf.geomtype = (mjtGeom*)(model->geom_type+i); for (int j = 0; j < kpoints; j++) { - ASSERT_THAT(mjc_distance(model, data, &sdf, points[j]), dist[i][j]); + EXPECT_NEAR(mjc_distance(model, data, &sdf, points[j]), dist[i][j], 1e-9); mjc_gradient(model, data, &sdf, gradient, points[j]); } }