Avoid NaN values caused by zero-length normals in ContactConstraint

[JenniferBuehler]

When testing the integration of DART 6 into Gazebo (see PR 2547) one issue was found with the iRobot Hand model: Some contacts with a zero-length normal were generated, which lead to NaN values, which in turn lead to a crash.

This PR fixes the issue, though it is only a suggestion on how to handle this. We could also prevent zero-length normals from counting as valid contact altogether (e.g. in ConstraintSolver.cpp), and not insert them to the list of contacts. That would prevent the issue as well.

[mxgrey]

That's a really interesting problem. In my mind, that raises some pretty fundamental questions: What is the actual meaning of a zero-length normal? I would be inclined to interpret that as a bug in the collision detector, unless the collision detection library assigns some special semantic meaning to a normal vector of zero length (perhaps it means the direction of the normal is ambiguous or indeterminable?). Furthermore, are non-unit-length normal vectors something that should be expected in general? Is there a special meaning to these lengths?

If it's nothing more than a bug, then I might propose discarding that specific contact point (because other aspects of its contact info might also be bugged) and maybe printing a warning for the user. But if the vector length has some deeper meaning, then it might be a good idea to work with it rather than against it.

[JenniferBuehler]

You are right, the interpretation of the contact normal may vary from collision detector to collision detector. In FCL it is the "Contact normal, pointing from o1 to o2" - a bit of a vague definition in the documentation (would need to dig up the code which computes it to know more). The penetration depth in FCL is a separate field, which makes me think the length will always be 1. I'm not sure how this normal is to be interpreted, but my guess of what would make most sense: if the objects are moved along this normal for a length equalling the penetration depth, they are not touching any more - and the normal points along the minimum penetration depth between the objects. But that's just a guess, I'd have to dig into the collision detectors more deeply to look up how they all exactly calculate this.

A more general interpretation of the contact normal would be that it is the force component perpendicular to the surface of contact (the "normal force"). In this case the length could be the actual force. Some collision detectors (maybe even future ones) may actually use this definition.

So bottom line IMO: at least in principle, the definition can vary between collision detectors.

Therefore I would probably try to allow zero normals in dart, and leave the catching of this error to the collision detector: if it regards a zero-length normal as an error, it should not return a contact in the first place. If there is a bug in the collision detector, and zero length normals are returned anyway, DART should at least not crash.

As to the length of the normal: this can probably be similarly ambiguous between collision detectors. The world "normal" alone suggests unit length - otherwise it would (or should) be a "direction". However when thinking of "normal force", the length of the normal can be the actual force exerted in normal direction. Unless the normal length is actually used as a force in DART (which I don't think it is, it's only interpreted as direction?), I think it should always be normalized just in case, to be sure it doesn't cause issues.

[jslee02]

Thanks for catching this issue.

However, I wonder if the zero-length normal was generated because CollisionRequest::enable_contact was set to false. If so, that's a bug of the caller (I need to check if somewhere of DART does this 😅).

Even if that's not the case, I still believe zero-length contact normal is just a bug of the collision detector. AFAIK, at least FCL doesn't intend to return zero-length normal (or even non-unit vector) in any case.

A more general interpretation of the contact normal would be that it is the force component perpendicular to the surface of contact (the "normal force"). In this case the length could be the actual force. Some collision detectors (maybe even future ones) may actually use this definition.

This is possible in dynamics simulation, but I don't believe collision detectors use "normal" field for that purpose.

If there is a bug in the collision detector, and zero length normals are returned anyway, DART should at least not crash.

👍

Here is what I would like to propose instead:

• We discard the contact if the normal is zero length even though we requested contact normal computation, which is a bug of the collision detector. The contact rejection might need to be done in each collision detector adaptors (i.e., dart::collision::XXXCollisionDetector).
• We don't generate a contact constraint if the contact has invalid contact information (e.g., zero-length normal). This is not a bug but we might want to print a warning for this case.

[JenniferBuehler]

Sounds good to me. Will extend this PR to include the proposed changes. Will probably do this within the next days, gotta finish something else first ;)

[JenniferBuehler]

I've done the suggested changes for FCL... before I do the same for bullet, let me know if this is what you had in mind, of if you had thought of it differently.

So the error happens when option.enableContact is set to true, because the case in line 1375 FCLCollisionDetector.cpp is caught.
If I don't misunderstand things, then not adding this contact at all would automatically not generate a contact constraint? At least I don't get the case in line 814 ContactConstraint.cpp when skipping the zero-length normals in FCLCollisionDetector.

[jslee02]

If I don't misunderstand things, then not adding this contact at all would automatically not generate a contact constraint?

Yes. That's what I intended.

Additionally, I would like to handle the second bullet points in my previous comment, but it is not necessary to be addressed in this PR though. 😄

[JenniferBuehler]

I thought that was already addressing the first and 2nd bullet point together, so I must be misunderstanding this ;) Anyway, if that's the kind of fix you had in mind, I'll do the same for bullet, and then we can merge. Coming up soon :)

[JenniferBuehler]

Sorry for taking so long. I have added the changes for bullet too - should there ever be the case that it produces zero length normals at all.

I also added another case where zero length normals could sneak in, in postProcessFCL() (FclCollisionDetector.cpp). It just does the same as the test which I already added before (where it caused the segfault. However we should do it consistently everywhere before adding any contact with CollisionResult::addContact()).

Which way would you suggest is the best to test this with bullet? I see data/skel/test/test_shapes.skel is the only example using bullet?

[jslee02]

Sorry, this PR was accidentally closed as the target branch is deleted. Reopening.

[jslee02]

Sorry for late response. I made some changes to resolve conflictions with the target branch (release-6.4).

I think we can merge this unless receiving more feedback soon. Thank you for your contribution!

Which way would you suggest is the best to test this with bullet?

We could test Bullet (and other collision detectors) for the same test of FCL by making the collision detector as a parameter to the test like this test for example.