Compute signed collision distance of concave shapes

[joao-pm-santos96]

Hello! First of all, thank you for your amazing work!

One question: Is there any way that we can compute the signed collision distance between two meshes that may or may not be concave?

[jcarpent]

It might be possible depending on your objects. Could you provide a typical example?

PS: we will soon add the decomposition of non-convex shapes into a collection of convex shapes (see https://github.com/humanoid-path-planner/hpp-fcl/issues/428)

[joao-pm-santos96]

I'll try to give you an example as soon as I'm able. Just to add a little more detail, I'm using pin.computeDistances method to obtain the min_distance. This seems to work as I need when at least one of the shapes is a hppfcl.Sphere or hppfcl.Box. But when the shapes are obtained from hppfcl.MeshLoader then the min_distance is always zero when in collision, and grows as expected when they are not in collision.

[joao-pm-santos96]

PS: we will soon add the decomposition of non-convex shapes into a collection of convex shapes (see humanoid-path-planner/hpp-fcl#428)

won't this decrease the performance?

[jcarpent]

won't this decrease the performance?

Yes, but it will be the exact quantity you are looking for ;)

[joao-pm-santos96]

In the following example b.min_distance should be negative but is, actually, 0.0:

import pinocchio as pin
import hppfcl as fcl
import numpy as np
import time
import os

from pinocchio.visualize import MeshcatVisualizer

q = np.array([])

pinocchio_model_dir = join(dirname(dirname(str(abspath(__file__)))),"models")
model_path = join(pinocchio_model_dir,"example-robot-data/robots/ur_description/meshes/ur10/collision")

mesh_1_path = os.path.join(model_path, "base.stl")
mesh_2_path =  os.path.join(model_path, "shoulder.stl")

mesh_loader = fcl.MeshLoader()
mesh_1 = mesh_loader.load(mesh_1_path, np.ones(3))
mesh_2 = mesh_loader.load(mesh_2_path, np.ones(3))

model = pin.Model()
geom_model = pin.GeometryModel()

geometries = [
    mesh_1,
    mesh_2,
    # fcl.Sphere(0.5),
    # fcl.Box(1, 1, 1),
]

placement = pin.SE3(np.eye(3), np.array([0, 0, 0]))
geom_obj = pin.GeometryObject("obj{}".format(0), 0, geometries[0], placement)
color = np.random.uniform(0, 1, 4)
color[3] = 1
geom_obj.meshColor = color
geom_model.addGeometryObject(geom_obj)

placement = pin.SE3(np.eye(3), np.array([0, 0, 0,]))
geom_obj = pin.GeometryObject("obj{}".format(1), 0, geometries[1], placement)
color = np.random.uniform(0, 1, 4)
color[3] = 1
geom_obj.meshColor = color
geom_model.addGeometryObject(geom_obj)

collision_pair = pin.CollisionPair(0, 1)
geom_model.addCollisionPair(collision_pair)

data = model.createData()
geom_data = geom_model.createData()

viz = MeshcatVisualizer(model=model, collision_model=geom_model, visual_model=geom_model, data=data, collision_data=geom_data, visual_data=geom_data)
viz.initViewer(open=False)
viz.loadViewerModel()
viz.display()
viz.displayCollisions(True)
viz.displayVisuals(True)

pin.computeCollisions(model,data,geom_model,geom_data,q,False)

a = pin.computeDistances(model,data,geom_model,geom_data,q)
b = geom_data.distanceResults[0]

[jcarpent]

@lmontaut Could you have a look at this issue?

[joao-pm-santos96]

May it be related with https://github.com/flexible-collision-library/fcl/issues/221?

[joao-pm-santos96]

If instead we do

mesh_2.buildConvexRepresentation(True)

geometries = [
    mesh_1,
    mesh_2.convex,
]

then we can actually compute the signed distance, even negative ones. This suggests that only one of the shapes needs to be convex.

Does this make sense? Is this the expected behaviour?

[jcarpent]

I do understand now. This is not the same algo which is called.

mesh_1 and mesh_2 are BVH data structures (surfaces without interior), while when calling mesh_1.convex or mesh_2.convex, built after a call to the method buildConvexRepresentation, you in fact deal with the same objects, but considered as convex shape, and then GJK/EPA are called to infer the signed distance between the objects.

Is it clear?

[joao-pm-santos96]

Hi, thank you for the detailed explanation.

Yes, it is clear. I just don't understand I only one of them needs to be convex, and not both or neither... but it certainly is a lack in my knowledge of GJK/EPA.

But, then, it is the expected behavior, right?

[jcarpent]

Yes, it is clear. I just don't understand I only one of them needs to be convex, and not both or neither... but it certainly is a lack in my knowledge of GJK/EPA.

I can only suggest reading our latest article on the topic: GJK++: Leveraging Acceleration Methods for Faster Collision Detection, Montaut et al (available at https://hal.science/hal-04070039)

But, then, it is the expected behavior, right?

Yes, this is expected

[jcarpent]

I will now close this issue. Fell free @joao-pm-santos96 to open it again if needed.