Architectural Improvements & Bugfixes

[stonier]

Resources

• Milestone 3

Goals

Keep tabs on the architecture and the complexity. This list will likely evolve as flags get raised while we work on items.

Front Load

• A benchmark scenario running 20+ of our most expensive agents (currently MOBIL cars) ⇨ Diagnose CPU costs #455
• Splitter that sits after the vectorised SimpleCarState_V publisher ⇨ Add splitter that sits after the vectorised SimpleCarState_V publisher #504 (PR Add SimpleCarState Splitter #511)
• Rename and improve the SimpleCarState message ⇨ Rename/Improve SimpleCarState message. #505 (PR Improved SimpleCarState: AgentState #517)
• Move simulator-specific wiring out of the individual agents ⇨ (PR Simulator agnostic agents #481)
• Diagnostics report on where unnecessary CPU costs are coming from ⇨ Diagnose CPU costs #455
• Simple checklist list of items that caused problems (check off what has been resolved) ⇨ Diagnose CPU costs #455
• Decide if we implement any of these remaining jobs in M3
• CMake Cleanup ⇨ CMake Cleanup Part 2 #472
• Migrate drake automotive systems (brute force move) ⇨ Migrating drake automotive systems #498

Stretch Bugs

• MOBIL car in gazoo - totally loses it in the corner ⇨ MOBIL car overshoots corners #573
• Car Pileup in gazoo ⇨ Gazoo Pileup #545

Success Criteria

• Issues for any remaining (hopefully non-architectural) performance problems that are killing us, but not to be resolved in M3.
• Agents do not depend on (even indirectly) the simulator
• Can run 20+ agents (any of the current) @ 100Hz realtime.

[apojomovsky]

@stonier , FYI.
Just added two items to the Work Items list to track the completion of the items discussed here.

[hidmic]

@basicNew @stonier @clalancette @apojomovsky On RigidBodyTree (aka RBT) to MultibodyTree (aka MBT) migration. All RBT uses within Drake's automotive namespace, and to some extent within Delphyne's backend as well, reduce to car and road visual geometry loading (e.g. see PriusVis and AutomotiveSimulator classes).

Now, before delving into details, it's important to understand the differences between RBT and MBT. As I understand it, RBT was designed to provide a self-contained representation of a rigid kinematic chain (e.g. a robotic arm) and thus geometrical (visuals, collisions) and physical (inertias, forces) descriptions are coupled. MBT, however, only provides a representation for a not-strictly rigid multibody physical system in order to solve for its dynamics, while geometry is delegated to the SceneGraph (aka SG) [1]. Coupled with these trees, both RigidBodyPlant (aka RBP) and MultibodyPlant (aka MBP) provide System interfaces for them to be used in simulation context. This is particularly relevant for the latter case, considering SG was designed to work with systems that register geometry and update/query it through ports.

In migrating from one to the other, a couple difficulties have been identified:

• There're no parsers available to load geometry directly into an SG. There is, however, a parsing API to load an SDF into an MBP (composing an MBT) that, as a side effect, registers geometry if an SG is given. There's no explicit support for URDFs (which we use for roads), but given that sdformat is used behind the scenes and that it'd seem that said library handles both formats seamlessly, we'd be OK. We must validate this hypothesis.
• Although an SG provides an LCM load message for visualization setup, currently we cannot grab it: it is dispatched to Drake's visualizer directly. It's trivial to extend though.
• Although an SG provides a PoseBundle for visualization update, because of its port-based API, it's harder to use outside a simulation diagram. Moreover, it is my understanding that SG was designed to be just another system in a diagram; all other systems may register geometry and hook up a port or two to the (global?) SG to update their own geometry and/or act upon current scene state (e.g. to model contact forces).

The last bullet is the most crucial, as it'll shape how we move forward with this:

• We could comply with the SG design and have systems register their visual and/or collision geometries with a global SG (like the one we have). This would eliminate the need for a CarVisApplicator system -visual registering would be handled by car systems themselves-, as well as the need for a PoseAgreggator system -since we could get the same information from the SG-. It would simplify the architecture and provide a full description of the world (collision geometries included). But that's not for free: we would have to significantly change automotive code base and push agents' SPC-G-V modularization (or any of its variations) down to systems. If we were to use SDF/URDF models, we'd still need a temporary MBP to load them into that SG. Good thing is that we can steal its SourceId, which is necessary to update SG poses later on. Bad thing is that knowing what to update requires a bit of reverse engineering if we drop that MBP.
• We could (and I want to stress that this is purely hypotethical and needs validation as it falls outside current intended use) replace RBTs with an (MBP, SG) pair, updating MBP's internal MBT manually and evaluating SG's PoseBundle output port manually.

Tangentially, we should also decide if we'll be doing the migration within Drake or if we'll be moving away from it, in the spirit of #498.

[1] It was stated that, because of this, for our particular use case we may not even need MBT (given we care about visual geometries only).

[basicNew]

@hidmic thanks for the research and the summary! Just to see if I got it right, the core issues are:

• No support for loading URDF / SDF into SG directly.
• If we go in the pure / correct way, we should delegate all to SG. However that means a major rewrite of the automotive package (e.g. removing things like CarVisApplicator and PoseAgreggator).
• As an alternative we could do a hack and use an (MBP, SG) pair to act as an RBT. In this case we would be deprecating the RBT use, but we wouldn't using MBP correctly.
• Finally we should also decide if we want to take automotive out of Drake. If so, this would be a good time to do it.

Am I missing anything?

[hidmic]

@basicNew That we need to make a small modification here if we want to get the LCM load message (i.e. the one we query from RBT here for example) out from the SG directly.

Other than that, it's a great TL;DR.

[hidmic]

One more detail that I think is worth mentioning: initially, @SeanCurtis-TRI was straightforward about SG not being ready for automotive. Thus SceneGraph's API may still be subject to changes in a not so distant future, hopefully simplifying our use cases.

[clalancette]

I'm going to close this one out now, since everything in here is either complete or has a separate issue. Feel free to reopen if you think we should continue to use this tracking bug instead.