Port collision removal

[ggould-tri]

TRI has a deterministic, heuristic-based mechanism to adjust a multibody plant context until an attached scenegraph no longer reports collisions. This is an important companion to stochastic scenarios (#13251 #13295 ... ), since for many interesting plants there is no easy way to write a schema for stochasticity that doesn't include some colliding configurations (either because the valid configuration space is not convex or because it would be a burdensome chain of coordinate transforms to express).

A strawman PR of this functionality exists at #12824 but a proper port will require adding (fairly complex) unit tests and resolving the design question of whether/how to feed information between the three heuristic approaches attempted in that code.

[sherm1]

Wishlist item: When we allow users to add or activate new free-floating bodies in a running simulation they will also need a way to make sure the new bodies are equilibrated before resuming simulation. Ideally no existing bodies should move during that process since the simulation is supposed to be continuing. It would be handy if this collision removal feature could be limited to particular bodies.

[ggould-tri]

Indeed! So useful it already does it!

It turns out that decolliding all the joints/bodies at once is computationally prohibitive anyway so you always want to decollide a complex scene a few joints/bodies at a time. So the API lets you say "decollide this list of bodies by moving this list of joints/freebodies only"

[jwnimmer-tri]

Just to help calibrate here, in #13291 (one of the motivating examples, I think) it says "... Such switching should be very fast and not involve rebuilding large structures from scratch ...". The collision remover is great, but it is far from fast, cheap, or heapless. The use case so far is setting the stage at t=0 of a simulation. I would not expect it to be performant as part of a control system.

[ggould-tri]

Good point. It is possible that the bisect heuristic could be made comparatively cheap, though I doubt fully heapless.

Possibly an argument for exposing each heuristic individually as a public method (my first implementation did this).

[EricCousineau-TRI]

Possibly distracting, but would love to have this in Drake too when possible.
See StackOverflow question: https://stackoverflow.com/q/61841013/7829525