Collision filtering

[m-decoster]

Adds examples to the collision checking notebook of:

• How to detect which bodies are colliding
• How to filter collisions with the SceneGraphCollisionChecker (SGCC)

In this current version, these methods are incompatible. When filtering with the SGCC, we will still get contacts when using the plant's contact output port. If we use an alternative method of filtering, see here, using the scene graph's collision filter manager, then we get the opposite: the SGCC will report that the configuration is not collision free, but there will be zero contacts reported from the plant's contact output port.

I will investigate further, this link will probably be useful. Most likely, this is due to these two operating on different contexts.

[m-decoster]

From the Drake C++ API documentation (source):

The CollisionChecker's definition of filtered body pairs is initially drawn from the underlying model's configuration (see GetNominalFilteredCollisionMatrix()). However, the CollisionChecker's definition of the set of collision filters is independent of the model after construction and can be freely modified to allow for greater freedom in determining which bodies can affect each other.

GetNominalFilteredCollisionMatrix():

The nominal matrix is initialized at construction time and represents the configuration of the model's plant and scene graph. It serves as a reference point to assess any changes to collision filters beyond this checker's intrinsic model.

Hence, if we update the collision filter manager of the scene graph after construction of the SceneGraphCollisionChecker, the latter will not reflect these changes.

For our use cases, we will mostly want to use the SceneGraphCollisionChecker for now, to check if states are valid when motion planning with airo-planner. My proposal is to only implement collision filtering and checking with SceneGraphCollisionChecker for now. See the commit after this comment.

When use cases arise where we want to do more in simulation or model certain constraints, we will want to add functionality that works with the SceneGraph's CollisionFilterManager.

[Victorlouisdg]

PR looks great, these are very useful features for airo-drake! Feel free to merge and release a new version of airo-drake.

[Victorlouisdg]

Is this asterisk * an accident?