Nearest points inconsistent when switching a collision pair

[Gregwar]

Hello, I am trying to compute the nearest points between two parts (in this example, the tibia and food of a robot). I am using Pinocchio to invoke HPP FCL.

Here is what I obtain:

https://user-images.githubusercontent.com/367022/223107983-69ca6660-2360-48cf-b33d-29e9df4b21a3.mp4

---

However, if I swap the indices in the collision pair, I got different result:

https://user-images.githubusercontent.com/367022/223108108-55ab234a-21f8-44a6-8d7b-b1654d16b889.mp4

I double checked and I don't think there is something wrong since it is pretty straigthforward, I just draw the nearest points returned (the break_distance here is always set to 1m to be sure nearest points are computed)

Am I missing something ?

[Gregwar]

Note: if I change the securityMargin, it appear to work as expected and returns the proper nearest points, but the objects are in that case always considered as in collision

[lmontaut]

Hi @Gregwar, Are you using pinocchio's computeDistances function to get the nearest points? If you are using computeCollisions and the shapes are not in collision, hppfcl stops as soon as it has found a separating hyperplane between the shapes (it also takes into account the security margin). It is only when the shapes are in collision that computeCollisions always returns the true nearest points.

The securityMargin parameter in a collisionRequest allows to set a margin around the shape such that collisions are detected even though 2 objects don't overlap, i.e if their distance is > 0 but < securityMargin. So if you set this parameter to be very high, the shapes will always be considered in collision, although they do not graphically overlap. Finally break_distance is related to BVH break down; it's not related to nearest points.

[Gregwar]

Hello, thanks, indeed, computeDistances is the way to go to achieve that!

[Gregwar]

Another question:

/// In case both objects are in collision, store the normal
  Vec3f normal;

1. Does it means that the normal is not available if there is no collision ?
2. What about the normal orientation when there is a penetration ?

I would expect the normal to be a unit vector going from the first point to the second:

But when there is collision, I would also like the normal to be "outward":

So that I could use it for instance to build inequality constraints

[lmontaut]

It depends if you use computeDistance or computeCollision.

When using computeCollision, if no collision is encountered, then no normal will be returned. When using computeDistance, a normal will always be returned.

In hppfcl3x, we fixed an issue where the normals were defined differently depending on the pair of objects. To have normal always pointing from o1 to o2, you can switch to the hppfcl3x branch.

Otherwise you can recompute the normal as you which (according you whatever definition suits you best) by using the nearest points. In the hppfcl3x branch for example, the normal is always defined as normal = sign(dist) * (p2 - p1) / (p2 - p1).norm() where dist is negative if the objects are in collision, positive otherwise.

Regardless of the hppfcl branch used, the nearest points p1 and p2 (belonging to o1 and o2 respectively) can be obtained in the distance result after calling pinocchio's computeDistance.

[Gregwar]

Ok, actually I think in my case the best is to avoid using normal and use min_distance instead; it is apparently a signed value that is giving me the information I want (in case of overlap, it becomes slightly negative, thus indicating I should switch the orientation of the normal I want so that it stays in the "release" direction)