From 5d46c39529819d1b31249e249ca399f306a108ac Mon Sep 17 00:00:00 2001
From: Alessio Quaglino <quaglino@google.com>
Date: Fri, 12 Jan 2024 07:53:09 -0800
Subject: [PATCH] Increased SdfLib bounding box by 10% to prevent querying
 outside it during collision detection.

In case a query outside is performed, add SDF gradient computation using finite differences.

For SDFs generated from meshes, when the query point lies outside of the bounding box, the gradient provided by SdfLib ignores the SDF and is equivalent to the gradient of the distance from the bounding box, causing local minima in the contact point optimization. This CL increased the size of the bounding boxes and replaces the SdfLib gradient with a finite difference approximation of the gradient if a point lies outside it.

PiperOrigin-RevId: 597839637
Change-Id: I79dfddbe25288eca7eb235db51781fc2bb2c3c5f
---
 plugin/sdf/sdflib.cc | 33 ++++++++++++++++++++++++++++-----
 1 file changed, 28 insertions(+), 5 deletions(-)

diff --git a/plugin/sdf/sdflib.cc b/plugin/sdf/sdflib.cc
index a1405eb7..3985ca5b 100644
--- a/plugin/sdf/sdflib.cc
+++ b/plugin/sdf/sdflib.cc
@@ -93,8 +93,12 @@ std::optional<SdfLib> SdfLib::Create(const mjModel* m, mjData* d,
 
 // plugin constructor
 SdfLib::SdfLib(sdflib::Mesh&& mesh) {
+  sdflib::BoundingBox box = mesh.getBoundingBox();
+  const glm::vec3 modelBBsize = box.getSize();
+  box.addMargin(
+      0.1f * glm::max(glm::max(modelBBsize.x, modelBBsize.y), modelBBsize.z));
   sdf_func_ =
-      sdflib::OctreeSdf(mesh, mesh.getBoundingBox(), 8, 3, 1e-3,
+      sdflib::OctreeSdf(mesh, box, 8, 3, 1e-3,
                         sdflib::OctreeSdf::InitAlgorithm::CONTINUITY, 1);
 }
 
@@ -125,10 +129,29 @@ mjtNum SdfLib::Distance(const mjtNum p[3]) const {
 void SdfLib::Gradient(mjtNum grad[3], const mjtNum point[3]) const {
   glm::vec3 gradient;
   glm::vec3 p(point[0], point[1], point[2]);
-  sdf_func_.getDistance(p, gradient);
-  grad[0] = gradient[0];
-  grad[1] = gradient[1];
-  grad[2] = gradient[2];
+
+  // analytic in the interior
+  if (boxProjection(p, sdf_func_.getGridBoundingBox()) <= 0) {
+    sdf_func_.getDistance(p, gradient);
+    grad[0] = gradient[0];
+    grad[1] = gradient[1];
+    grad[2] = gradient[2];
+    return;
+  }
+
+  // finite difference in the exterior
+  mjtNum eps = 1e-8;
+  mjtNum dist0 = Distance(point);
+  mjtNum pointX[3] = {point[0]+eps, point[1], point[2]};
+  mjtNum distX = Distance(pointX);
+  mjtNum pointY[3] = {point[0], point[1]+eps, point[2]};
+  mjtNum distY = Distance(pointY);
+  mjtNum pointZ[3] = {point[0], point[1], point[2]+eps};
+  mjtNum distZ = Distance(pointZ);
+
+  grad[0] = (distX - dist0) / eps;
+  grad[1] = (distY - dist0) / eps;
+  grad[2] = (distZ - dist0) / eps;
 }
 
 // plugin registration