No way to set a safety margin/padding?

[pbeeson]

We have used FCL in robotics applications for a while, and it seems like there is no way to add a "margin" (in Bullet parlance) or "padding" when creating a Collision Object. We have been artificially doing this by messing with meshes, but that presumes mesh normals are well formed, etc. The correct way (and the way Bullet does it) is to handle this at the triangle/primitive comparison level.

So 1) Is it really correct that I can't add a "padding" to an object? I don't see this documented or in the source code.

2) I realize that there is a distance() function, but in our benchmarking it is 1000x slower than collide(), which is unacceptable for our applications.

3) If I were to add this as a Pull Request, my initial thinking is that I 1) add the padding to the OBB for meshes so that the OBB is bigger, 2) save the padding for primitive shapes to be used in those checks too, 3) when a OBB is overlapped, then essentially use this with the distance() computations to check is the padding threshold is breached. In this way, I'm only calling the expensive distance() functions when a padded-OBB is overlapped. Does this seem reasonable?

[pbeeson]

I'm willing to try to spend some of my own time/money to work on this, but it would be nice if some FCL developer/maintainer at least gave me some response to the questions above before I set out on this journey.

[wxmerkt]

@pbeeson We used the geometric_shapes scale and padding functions to achieve the same purpose before constructing the OBB model.

[pbeeson]

@wxmerkt This doesn't really work properly though. The geometric_shapes package "pads" from the origin of a mesh. If you have, let's say a bin, it will move the outer walls of the bin away from the center of the bin, but it also moves the inner walls away from the bin. What you want is something more like a "dilate" function. https://github.com/ros-planning/geometric_shapes/issues/76

The other issue is that any "padding" or "dilate" function for a mesh is going to assume your normals are correct, but often times we get meshes (STLs or DAE) than have been converted from various CAD models by customers, and the triangle normals simply either don't exist or are inverted (you can see this in Blender for complicated meshes). In these cases, when the mesh is overly complex, no automated tools can figure out the normals properly (e.g., suppose you have a white scan model that's an STL of nearby points). In these scenarios, padding using geometric_shapes (even extensions to this as per the link above) move the verticies in the wrong direction.

By handling this in FCL, you can simply say if the min distance between two objects is below the padding, then "Collision". It doesn't matter in this case what the normals are.

[wxmerkt]

I see - thank you very much for the detailed explanation :-).

[pbeeson]

No problem. For simple, well formed convex meshes, your approach would work well.

[SeanCurtis-TRI]

Even for well-formed convex meshes, it wouldn't really work. Vertices that are more distant from the origin would get moved more than vertices close to the origin. It would only really work if you were "padding" a tesselated sphere. Everything else will be wrong (to varying degrees).

Ultimately, you want to perform queries on an offset surface. Essentially, the Minkowski sum of the original geometry with a sphere. I've played with Bullet's margins and they're a tetchy thing. Architecturally, they're built into the system from the ground up -- at first glance, I'm not convinced that they can simply be layered on top of things. Also, the author has suggested that the margin should never be set to zero, which suggests some undesirable entanglement.

I'd be perfectly happy to discuss your plan for introducing margins and give you thoughts/feedback, but I don't have many cycles to more overtly contribute.

[Levi-Armstrong]

I believe @pbeeson mention expanding just the AABB to reduce unnecessary check as alternative compromise. In bullet they do this through setting the contact process threshold and it should not be to hard to accomplish something very similar in FCL. One could add a new method setContactThreshold(S expand) to the CollisionObject class. This constant would be used in the computAABB which would expand the AABB. @SeanCurtis-TRI would this be an acceptable enhancement to FCL?

[SeanCurtis-TRI]

Vague brain dump as I head to bed...

Simply expanding the AABB nodes would cause leaves of the BVTT to be considered that might not be considered without the padding, however, once the broadphase creates a candidate pair, you'll still be colliding against non-expanded geometry and the straight-forward query would end up saying they don't collide.

One simple option is to always just perform a signed-distance query. If the objects are separated (i.e., positive distance) one can subtract the padding from distance and if that becomes negative, than the expanded geometries intersect. That, in conjunction with padded AABBs, could work. Although, that feels like a hacky overlay.

Alternatively a distance offset could be provided to the collision query. Currently broadphase and collision use a distance > 0 as a threshold for terminating consideration. If you provided a positive tolerance, you would only dismiss things for distance > tolerance.

This would require exposing this parameter to both the broad-phase and narrow-phase code (a lot of code). This approach has the most value if you plan on having a single geometry have different padding w.r.t. different objects. If it has consistent padding than a quasi-reasonable alternative would be simply to inflate the original geometry when registering it with FCL. (Although, simply moving all vertices a fixed distance along their normals is not generally the same as an offset surface.)

[pbeeson]

We are back to looking at this, and I have one of our best researchers/programmers on it.

One high level question is why is the public collision call between two collision objects is so much faster than the distance call? If, as you imply above, collision is ultimately grounded in a signed distance function, then shouldn't a dist call (at least for narrow phase) be as fast as a collide call? We've done tests where we used the dist call instead of collide and it was 1-2 orders of magnitude slower. For actual objects that were in collision, not just for far away objects. Because if requesting the distance between two objects was as fast a just querying collision (I realize this might not be the case for far away objects) then we don't need any changes for margins in FCL.

[SeanCurtis-TRI]

So, it's worth noting that my implication that everything is ultimately "grounded in a signed distance function", is only a conceptual statement. One can look at FCL's public API and it's basically just syntactic sugar on the signed distance function. From an implementation perspective, there are quite arbitrarily independent code paths.

~When you say that one query is faster than another (collide faster than distance), what is the configuration of the objects in question? If they are separated, then I wouldn't be surprised. The collision query doesn't do any work once it has sufficient indication that objects are separated. However, the distance function has to keep doing work in order to characterize the distance.~ Sorry, I hadn't read your comment in its entirety before writing.

So, instead, I'll ask these questions:

• what are the geometries that are colliding?
• What gjk solver are you using? libccd? Indep? (I assume libccd.)

I don't suppose you have your profiling code in some nice, standalone program, do you? Ultimately, these issues of performance are near and dear to our hearts and after we release the next version, that'll be one of the things we're tackling. Obviously, we'd appreciate all help in that regard.

[pbeeson]

Clsoing as I think the original question was answered in this thread.