Collision Detection

[basicNew]

• Milestone 3 Planning
• Design Doc

Goals

Delphyne can identify and notify when collisions occur, but is not responsible for the physics of collision behaviours. i.e. just report out when a collision occurs.

Work Items

• [x] Research on existing ways of doing collision detection and prepare a first demo (PR #449).
• [x] Add on collision callbacks and expose in python.
• [x] A toggle for enabling/disabling the collision subsystem.
• [x] A demo, delphyne-crash with two agents setup for a crash.
• [x] Identify / answer open questions, sketch out the plan in the design doc
• [x] User & Developer Documentation in the Delphyne Guide -> Collision detection

Success Criteria

• [x] delphyne-crash detects the collision and responds 'appropriately' (to be determined)

Descoped from Milestone 3

• [ ] Define a design that scales across cars and that can be simply evolved from bounding boxes to more complex shapes. Most likely something similar to CarViz, but for geometries (see design doc on arch). Opened #567 to be tackled outside M3.
• [ ] Add more info about the collision (points, depth, etc) and expose in python (#479, #460). Opened #568 to be tackled outside M3.

[hidmic]

Alright, I did some research through Drake's code looking for collision detection support.

In one hand, there's RigidBodyTree. Collision detection is built into the tree, that owns and delegates geometrical computations to an internal collision model. There're two implementations for this model: one based on fcl, the other on bullet. By default, if available, RigidBodyTree will use the bullet-based one.

On the other hand, there's SceneGraph. And not MultibodyTree as you might have guessed. With the multibody namespace revamp, the geometry of a body is now decoupled from its physical properties. This allows systems to interact directly with the topology of their world (through the SceneGraph). This includes collision detection, that currently relies on fcl for the computations and is made available through the QueryObject output of the SceneGraph.

The apparent functionality duplication is due to the fact that, although still around for the time being, RigidBodyTree will be replaced by MultibodyTree in the near future. However, both approaches come with certain caveats. RigidBodyTree does not expose the collision model it builds, making it harder for us to use. SceneGraph is still under active development and thus some parts are still TBD, e.g. it currently lacks support to detect collisions between mesh geometries.

What follows are two potential roadmaps to vehicle collision detection, one for each approach.

Using RigidBodyTree

• [ ] Adapt the Agent::Configure() API to accept a RigidBodyTree and add a body with collision geometries to it using floating joints (e.g. RollPitchYawFloatingJoint).
• [ ] Make the SimulationRunner aggregate all cars' poses into a single generalized position vector q suitable for the RigidBodyTree in order to call RigidBodyTree::doKinematics() followed by RigidBodyTree::updateDynamicCollisionElements() followed by RigidBodyTree::allCollisions().
• [ ] Pass collision information through step callbacks (we can start out with a boolean to state whether there was any collision or not).

Using SceneGraph

• [ ] Instantiate a global SceneGraph within the AutomotiveSimulator and expose it.
• [ ] Adapt car systems (e.g. SimpleCarSystem) to register geometry on a given SceneGraph and update its kinematics accordingly (class documentation is very thorough).
• [ ] Adapt Agent::Configure() API to accept a SceneGraph and perform the necessary wiring within.
• [ ] Make the SimulationRunner retrieve the QueryObject output from the SceneGraph of the simulator and query collision information on every step.
• [ ] Pass collision information through step callbacks (we can start out with a boolean to state whether there was any collision or not).

Note that neither roadmap requires providing physical entity to the cars. And it's important to understand what that means: we will be able to tell if there was a collision between vehicles, but the simulation will not model the collision itself (i.e. no cars crashing or flying around).

[stonier]

I've been considering steps 1. 2. and 3. of the SceneGraph with the consideration that I know nothing about it's state of readiness right now and it might be a potential rabbit hole that we need to back out of.

Nonetheless, it's worth enduring a bit of pain in that direction just to get us off a path that seems to be finally deprecating.

Your final caveat is spot on. Of all the driving simulations we do here at TRI, collision modelling is never considered. We'll be getting value out of just detecting an imminent collision point for a long time to come.

[hidmic]

Alright. Moving forward with the SceneGraph approach.

[stonier]

@basicNew @hidmic I updated this one with the bullets from our planning doc and primed it for the collision detection story rather than create a new one and lose the thread here.

[hidmic]

Based on the latest additions to the Design Doc, and in line with the goals mentioned in this issue, we're:

• Moving away from collision polling and towards an event-based approach (#478).
• Augmenting collision information, returning geometric details along with the agents themselves (#479).
• Enabling/disable collision detection on demand (#480).

Regarding geometries, we'll be sticking to the Prius' one for the time being.

[basicNew]

@stonier @clalancette Added docs and created new issues for the two remaining items. Are we good on closing this one?

[clalancette]

It looks like it to me. I'll close it out, @stonier can reopen if there is something else to do here.