contact normal changes randomly between two opposite directions

[ClementRolinat]

hello,

I am using DART as physics engine for Gazebo 9, and DART is using FCL as collision detector.
(this issue is also referenced on DART here: issue 1425

I found this bug when attaching a contact sensor to a model.

When I display the gazebo topic associated with this sensor, I can see that the normals of the contacts are not constant, even if the model is lying on the ground and is at rest (only gravity force is applied on it). More precisely, the directions of the normals oscillate between approximately (0, 0, 1), and (-0, -0, -1).

The bug is very easy to reproduce in Gazebo (with DART physics engine enabled), you just need to attach a contact sensor to a model, and display the associated gazebo topic.

The FCL version used is the one available on apt repository, libfcl0.5

[SeanCurtis-TRI]

@ClementRolinat have you seen issue #368? It sounds like this may be affecting you. If it oscillates with each query (even if nothing is moving), that sounds like it's a victim of the implicit BVH changes.

[ClementRolinat]

I am not sure to understand the issue. But as far as I understood, my issue could be a direct consequence of this, if the normal direction returned by FCL depend directly on the order of the two colliding objets...

Anyway, I confirm that the direction oscillate approximately at the update rate of the simulator (so at each query to FCL)

[SeanCurtis-TRI]

Exactly right. The broadphase mutates itself (even if nothing moves), so that the ordering of the two objects to the narrowphase algorithm alternates: (A, B), (B, A), (A, B), etc. The narrowphase algorithm could be consistent (and probably is) in how it reports the normals, but it lacks the greater context of knowing which is A and which is B.

This burned us as well (it's horrible for physics if you need to know where and how to apply a contact force). We resolved it outside of FCL by associating our own ids so that we could guarantee the interpretation of the normal.

Fixing the BVH to be stable in its update method is a much bigger headache we haven't felt like tackling. Until that's fixed, you might want to post an issue to Dart, referencing the issue in question, and see if they can accommodate this quirk like we did.

[ClementRolinat]

There is something I still don't understand.
If the order of the two objects alternates, it should still be visible in the contact info, as a contact know the order of its colliding objects (the o1 and o2 variables in the contact object).
So if I have two object A and B, I should expect to have o1=A, o2=B, and after a call to update(), I can have o1=B, o2=A, ect... Am I correct ?

This info is retrieved by DART as well (at least, the contact object on DART side also have two variables to store colliding objects for this contact). But here, the returned objects doesn't switch place...

[SeanCurtis-TRI]

A fine point. I'm not familiar with the DART code. I went looking and my limited scanning didn't detect anything obvious. In translating from fcl results to dart results, they are storing pointers to the fcl collision objects, so there should be sufficient data to handle the order flipping. But I didn't happen across the resolution the stored contact results (via dart::Contact) to body forces.

Finally, going back to your original stated issue. Does the contact sensor merely report on the geometric contact being measured and not the resultant contact force? Here's a possibly hypothesis: It's displaying an oscillatory collision normal. However, before that's converted to a force, it's properly reconciled so that there is always a correct repulsive force applied to the bodies in contact. If it weren't doing that, one would assume that the object would not be "at rest" on the ground, because it would be getting alternating attractive and repulsive forces w.r.t. ground. If that's true, this wouldn't be a bug per se. Just an annoyance.

In the Dart code, I did find where the dart::Contact object is instantiated from the fcl results. That data structure is definitely going to be subject to swapping order of o1 and o2.

So, what quantity is your sensor actually measuring?

[ClementRolinat]

my sensor report for a given object every collision in which it is involved. Each collision consist of several contacts. For each contacts, the position, penetration depth, normal, force, torque and colliding objects are reported. On these reported values, the normal and force are oscillating, whereas the two colliding objects stay in the same order.

However, the physics behavior is correct. So, as you said, it is just an annoyance, but for my application it is really bad because I need to make some computation involving contact normal...

EDIT:
more precisely I am using Gazebo ContactSensor, which read contact information provided by ContactManager, which simply aggregate contacts information provided by the underlying physics engine, here DART.
So there is a lot of interface layers, but I already looked into it quite deeply, and it seem that the normal and the colliding objects order are not post processed after it is returned by fcl... Or at least I could find where and how !

[SeanCurtis-TRI]

I would assume then, that the pointers to the two objects likewise "oscillate" (swap back and forth). So, you'll probably have to do the same thing as dart -- devise some canonical ordering for the given pair and reconcile it.

One thing you might do if you feel confident there's no destruction/construction of objects. Compare the pointers of the reported objects. If o1 < o2, leave the normal, otherwise flip it. That should normalize your directions.

[ClementRolinat]

oh, thanks for the pointer trick, I will definitely try this out ! I will let you know if it works.

[SeanCurtis-TRI]

Did it work? Or did it open up a wormhole into the dungeon dimensions and release all of the demons of C++ on you?

[ClementRolinat]

I didn't try it yet (but I expect some C++ black magic will occur of course).
However, I digged into the data structure you found, which convert fcl contact to dart contact. There is something odd here line 1582 and 1584. The colliding objects put in the dart contact (o1 and o2) are not the ones from the fcl contact, but they are function parameters.
This function (convertContact) is called in postProcessFCL and in postProcessDART. Both are called in collisionCallback. This callback is used here by FCLCollisionManager.
So maybe I am missing something, but from what I see, there is no guarantee that the object order is the same in dart contact as in fcl contact ? Maybe you have a better understanding of what is happening ?

edit:
anyway I can still implement your pointer trick directly in convertContact function, as it is the last spot where i'm sure that o1 and o2 from fcl contact is available. I would have preferred to do this on a higher level, but it should works.

[SeanCurtis-TRI]

I saw the same code you pointed out.

I agree the dart::Contact object ordering is going to be subject to the ordering provided by FCL as a result of its broadphase. However, given that the physics seems to work, that suggests the conversion from geometric contact information to contact forces is doing an appropriate mapping. (I.e., it assumes the normal always points from o2 -> o1, and figures out who o1 and o2 are in terms of bodies). Doesn't help you in putting a monitor on the contact info, though.

If my guess is right, doing the ordering in convertContact seems relatively harmless with two caveats:

1. Make sure you reverse the assignments of contact.triID1 and contact.triID2.
2. Confirm that the position and normal directions are defined in some frame that is independent of the ordering of the objects (i.e., expressed in the world frame and not the frame of the first object.)

[ClementRolinat]

Ok, for the fix implementation details, I will see with DART maintainers, hopefully they will know better where and what to do.
Thank you for your help, I let you choose to close or keep this issue open.

[SeanCurtis-TRI]

I'm going to close it in favor of the more general issue #368. If this proves to be something more than that issue, we can re-open it.