Self-collisions not working?

[costashatz]

We are simulating some simple robots (hexapod) in very simple environments (just floor) and using code from your examples (spd controller). Everything works very well until the moment we enable self-collisions. Then we get the following errors.

Either this:

CullVisitor::apply(Geode&) detected NaN,
    depth=nan, center=(0 0 0),
    matrix={
    -nan -nan -nan -nan 
    -nan -nan -nan -nan 
    -nan -nan -nan -nan 
    -nan -nan -nan -nan 
}

Or:

LCP internal error, s <= 0

Here's a picture of our setup:

Is there anything special we need to do for safely enabling self collisions?

Notes:

• We are using the branch release-6.1.
• We tried enabling self collisions in your BipedStand example and we got similar errors:

ODE INTERNAL ERROR 1: assertion "d[i] != dReal(0.0)" failed in _dLDLTRemove()

[jslee02]

Could you give it a try with disabling adjacent body checking (Skeleton::disableAdjacentBodyCheck)? It seems the case that the contact constraint is not feasible to me.

[costashatz]

Could you give it a try with disabling adjacent body checking (Skeleton::disableAdjacentBodyCheck)? It seems the case that the contact constraint is not feasible to me.

Still getting errors. We tried with several collision detectors (DART, FCL, Bullet) and several examples and it still doesn't work.. Any other ideas? Do you have a working example with self collisions?

[bpapaspyros]

Any news on that ?

[mxgrey]

One possible explanation is that disabling collisions between "adjacent bodies" as defined by DART is not always sufficient. DART considers adjacent bodies to be bodies which have a direct parent-child relationship. This means that a grandparent-grandchild pair will not be considered "adjacent" by DART. In some kinematic models, it may be necessary for a grandparent and grandchild body to be considered adjacent in order to get sensible results. This happens especially often when multiple sequential joints are closely intersecting each other.

When I've run into that in the past, my procedure has been to take the Skeleton out of simulation and just perform a collision check against itself when it's in a self-collision-free configuration. If the problem is indeed that some grandparent-grandchild pairs should be adjacent, then the collision check will report false positives. Those false positive readings will tell you which additional bodies need to be considered adjacent (and therefore need to be ignored by the collision detector).

Currently the only way I can think of to ignore custom pairs is to create your own collision filter by extending the dart::collision::BodyNodeCollisionFilter class. You can give your custom class a map of string pairs (or index pairs) which need to be ignored. Then you can override the needCollision function such that if the pair of objects that get passed in belong to one of the body pairs in your map, they get ignored. Be sure to call BodyNodeCollisionFilter::needCollision(o1, o2) for cases where the pairs should not be ignored. Here's a gist which shows what the class might look like. You may need to tweak it if there are bodies which need to ignore collisions with multiple other bodies.

[mklingen]

Btw that solution sounds similar to what was done for the MoveIt skeleton configuration wizard. We just sampled a few thousand random configurations of the robot and declared links that were in collision >90% of cases to be "adjacent" and ignored their collisions.

[aclegg3]

Collision pair filtering is now included in the DART master branch.

[stale[bot]]

This issue has been automatically marked as stale because it has not had recent activity. It will be closed in 7 days if no further activity occurs. Thank you for your contributions.