Interior SDF seems to be wrong

[JonathanKuelz]

1. cuRobo installation mode: python
2. python version: 3.12

---

Issue: CuRobo SDFs seem to be incorrect in the interior of "merged" objects.

Background: Boolean operations on SDFs are not trivial - given two SDFs A and B, the commonly performed approximation $sdf(A \cup B) = \max sd(A), sd(B)$ is correct outside the SDF, but a lower bound on the inside only. [more]

Details: It seems as if the SDF computation in CuRobo is following this approach, but I couldn't find any documentation regarding this. This "problem" would arise whenever multiple objects (meshes, primitives, ...) overlap and the signed distance is computed with a max operator. If this is the case, is there a way to get the correct interior SDF with curobo?

This might be beneficial for:

• Computing the 'severity' of a collision
• Moving out of a collision by following the gradients of the SDF

Example:

I wrote this small example to check my assumption. It creates a grid of overlapping spheres and then computes the signed distance between the grid and their center points. If the SDF was correct in its interior, the signed distances would differ. However, they do not:

from curobo.geom.sdf.world import CollisionCheckerType, CollisionQueryBuffer, WorldCollisionConfig
from curobo.geom.sdf.world_mesh import WorldMeshCollision
from curobo.geom.types import Mesh, Sphere, WorldConfig
from curobo.types.base import TensorDeviceType
import torch

tdt = TensorDeviceType()
n = 5
x = torch.linspace(0, 1, n)
grid = torch.stack(torch.meshgrid(x, x, x)).transpose(0, 3).reshape(-1, 3)
poses = [p.detach().cpu().numpy().tolist() + [1., 0., 0., 0.] for p in grid]

radius = 1 / n  # Spheres are overlapping
tm = Sphere(radius=radius, pose=[0, 0, 0, 1, 0, 0, 0], name='sphere').get_trimesh_mesh()

cfg = WorldConfig(mesh=[Mesh(vertices=tm.vertices.tolist(), faces=tm.faces.tolist(), name=f'm_{i}', pose=p)
                        for i, p in enumerate(poses)])
wcc = WorldCollisionConfig(
    max_distance=1.,
    tensor_args=tdt,
    world_model=cfg,
    checker_type=CollisionCheckerType.MESH,
)
world_collision = WorldMeshCollision(wcc)

query_spheres = torch.concat([grid, torch.zeros(grid.shape[0], 1)], dim=-1).view(1, 1, -1, 4).to(tdt.device)
query_buffer = CollisionQueryBuffer.initialize_from_shape(
    query_spheres.shape, TensorDeviceType(),
    {'mesh': True}
    )
d = world_collision.get_sphere_distance(
    query_spheres, query_buffer, weight=torch.tensor([1.], device=tdt.device),
    activation_distance=torch.tensor([0.], device=tdt.device),
    compute_esdf=True
)
print(d)

tensor([[[0.1991, 0.1991, 0.1991, 0.1991, 0.1991, 0.1991, 0.1991, 0.1991,
          0.1991, 0.1991, 0.1991, 0.1991, 0.1991, 0.1991, 0.1991, 0.1991,
          0.1991, 0.1991, 0.1991, 0.1991, 0.1991, 0.1991, 0.1991, 0.1991,
          0.1991, 0.1991, 0.1991, 0.1991, 0.1991, 0.1991, 0.1991, 0.1991,
          0.1991, 0.1991, 0.1991, 0.1991, 0.1991, 0.1991, 0.1991, 0.1991,
          0.1991, 0.1991, 0.1991, 0.1991, 0.1991, 0.1991, 0.1991, 0.1991,
          0.1991, 0.1991, 0.1991, 0.1991, 0.1991, 0.1991, 0.1991, 0.1991,
          0.1991, 0.1991, 0.1991, 0.1991, 0.1991, 0.1991, 0.1991, 0.1991,
          0.1991, 0.1991, 0.1991, 0.1991, 0.1991, 0.1991, 0.1991, 0.1991,
          0.1991, 0.1991, 0.1991, 0.1991, 0.1991, 0.1991, 0.1991, 0.1991,
          0.1991, 0.1991, 0.1991, 0.1991, 0.1991, 0.1991, 0.1991, 0.1991,
          0.1991, 0.1991, 0.1991, 0.1991, 0.1991, 0.1991, 0.1991, 0.1991,
          0.1991, 0.1991, 0.1991, 0.1991, 0.1991, 0.1991, 0.1991, 0.1991,
          0.1991, 0.1991, 0.1991, 0.1991, 0.1991, 0.1991, 0.1991, 0.1991,
          0.1991, 0.1991, 0.1991, 0.1991, 0.1991, 0.1991, 0.1991, 0.1991,
          0.1991, 0.1991, 0.1991, 0.1991, 0.1991]]], device='cuda:0')

[balakumar-s]

cuRobo is not doing anything extra to handle SDF values in the interior. Here is the code:

curobo/src/curobo/curobolib/cpp/sphere_obb_kernel.cu

Line 1231 in 2fbffc3

max_dist = loc_grad.w;

cuRobo uses finite difference (central difference) to compute gradient given sdf:

curobo/src/curobo/curobolib/cpp/sphere_obb_kernel.cu

Line 787 in 2fbffc3

compute_voxel_fd_gradient(

Your link shows ways to improve interior SDF, have you used a particular one in the past? I still have to read them in detail to see which one would be easy to implement.

One workaround is to merge all your spheres into a single mesh before creating the SDF. You can do that with:

cfg = WorldConfig(mesh=[Mesh(vertices=tm.vertices.tolist(), faces=tm.faces.tolist(), name=f'm_{i}', pose=p)
                        for i, p in enumerate(poses)])
cfg = cfg.get_mesh_world(merge_meshes=True)

[JonathanKuelz]

Thanks for your fast response!

Browsing the code, it looks to me as if the SDF is indeed computed as the maximum over all surfaces, which gives a lower bound on the signed distance instead of the signed distance in the interior. If I am not mistaken, this should also result in the gradients being invalid in the interior of "overlapping" objects (it is easy to construct a case where this gradient points in the opposite direction of the actual gradient).

I tried to merge the meshes, but it seems that the problem remains. The merging doesn't seem to compute a new mesh surface but -- instead it keeps all the "interior" faces as well, so the SDF computation remains the same as before merging.

Unfortunately, I don't know what would be the best strategy to repair the interior SDF -- I don't think the methods mentioned can be applied to arbitrary 3D meshes or even compositions of primitives. My best guess is that the easiest way to repair the SDF would be to reconstruct the actual surface (excluding faces that are now in the interior) of the merged mesh and then recompute the SDF for the interior.

Here's a screenshot of the mesh I was generating with my code: The signed distance will always be computed with respect the next sphere surface, not the surface of the composed object.