Drake: Ability to modify MBP and SceneGraph at any time

[edrumwri]

We anticipate wanting to benefit from the ability to do real-time system identification for model-based control: we would take observations from controlling our robot and use those observations to update the MBP+SG that models our robot and our world, in order to better control our robot and its interactions with the environment.

I don't think this is currently possible with MBP (it looks at least partially supported in SceneGraph†), since the MBP must be finalized before use. It would be nice to, at minimum, be able to change the inertial properties of objects being manipulated as new sensory data arrives and, e.g., change the frictional properties of our robot as it moves. It would really be great to be able to add bodies to MBP+SG as they are identified from sensory observations and also to update geometry (mesh or, e.g., cylinder length) as perceptions become refined through new data.

The only way that I know at present to "update" an MBP+SG in the manner I want is to build it anew, which is infeasible in a real-time control loop. Rebuilding MBP+SG to account for even small changes, like altering the estimated inertia of an object in the environment or the estimated friction at the joints, seems like overkill.

† At least partial support in SceneGraph is indicated by issue #9909, which describes two PRs that allow geometries to be added/removed from SceneGraph

cc @RussTedrake (especially since this ask seems in line with his original vision for Drake) and @siyuanfeng-tri (since I know he has made similar requests previously) for their thoughts.

[sherm1]

One relatively easy thing to do would be to allow parameterization of existing MBP bodies -- then the SysID updates would just be Context variable changes. That's obviously limited to objects that already exist in the model, although a "pool" could probably be maintained to allow some flexibility. Would that work in your application?

Another thought is that rebuilding MBP+SG _might_ actually be a quick operation -- we've never measured it. It seems complicated from a human's point of view but is likely a lot fewer operations than a runtime computation. Might be worth measuring before rejecting that idea.

[RussTedrake]

The "Finalize workflow" in MBP has a major pain point forever. It's not only about runtime efficiency, but also has huge consequences for the way people can write code. ManipulationStation is four times as long as it should be (it should really be in python). To add an object to the plant (must be pre-finalize) and then set its initial conditions (must be post-finalize), we end up doing things in completely different methods.

Many people have been rebuilding MBP+SG in the loop to get around this. Presumably there is some experience out there about the performance.

[edrumwri]

a "pool" could probably be maintained to allow some flexibility.

It wouldn't be elegant, but that might work.

Another thought is that rebuilding MBP+SG _might_ actually be a quick operation -- we've never measured it. It seems complicated from a human's point of view but is likely a lot fewer operations than a runtime computation. Might be worth measuring before rejecting that idea.

We're just starting to measure performance on this. Overall performance has not been good, but it would be premature to blame the construction process. I'll report back with actual data within the next few weeks.

[siyuanfeng-tri]

This is a really essential feature for robot in the wild applications... I definitely prefer mutating MBP / SG over maintaining a pool of models. It is painful to do book keeping, especially because you need to also maintain the associated contexts. This will be extra painful when you start doing computations in parallel.
I can image adding / removing bodies or geometries is hard post Finalize. But I don't really know why it's hard to support constant (like body inertia / friction constant) changing?

[edrumwri]

Update after analyzing MBP+SG construction performance as I promised.

First, the setup:

I timed 100 calls to a function (built in release mode, of course) that constructed an MBP+SG consisting of:

• 3 unconstrained ("floating") boxes, added to the MBP+SG programmatically, i.e., without parsing.
• 1 constrained (i.e., welded to the ground) box, also added programmatically.
• A 10 DoF robot consisting of the following visual geometries: 6 boxes, 4 cylinders, 2 cones. No collision geometries were used.

Building this MBP+SG required 13.191s.

Profiling indicates that most (100%, according to perf) of the computation is spent in parsing the 18.6k of SDF files (about 600 lines of text), in AddModelFromFile(.). Of that 100%, 55% is spent in sdf::Load(), 30% is spent in AddModelFromSpecification(.), and 15% is spent in the sdf::Root destructor (!).

I want to highlight two points:

• 132ms mean-per-plant-construction for such a simple model indicates that the reconstruct-on-the-fly paradigm is not a great option at the present time. When we added a few more geometries- 100 spheres for contact purposes- to the sdf, mean parsing time went to 370ms per construction. I can easily see plant construction time rising to a few seconds for a MBP+SG containing our model of the robot and our perceived model of the world.
• As noted above, performance timing does not reflect any meshes being parsed, but I added a 74 vertex/144 face obj file and a 224k vertex/231k face obj file in a separate experiment and _observed overall construction time go down_ (i.e., adding some meshes is still within the noise of the timings). This indicates to me that sdf::Load() is a dog, comparatively.

Anything I miss or other information that might be helpful that you would like to see?

[SeanCurtis-TRI]

Thanks for the measurements. I hadn't realized the sdf parser was so heavy weight.

Quick question: with what frequency would this reconstruction be happening? I.e., 132 ms every other time step would be catastrophic, but every 10 minutes it would meaningless. Or, put another way, how does 132 ms of CPU time compare with the amount of work done between constructions?

[siyuanfeng-tri]

I am guessing Evan can survive eating up any kind of performance hits when just simulating, but if he's controlling a real robot and has to remake his control models on the fly, that's likely a non starter.

[SeanCurtis-TRI]

Excellent point.

[EricCousineau-TRI]

When writing #13073, it was unclear what the scope of this issue is / could be.

Is it resolve by mitigations (adjusting parameters), or by full-blown topology mutations as well? (add/remove joints?)
Really, the question is: Do we need a separate issue for topology mutations?

Related Slack discussion: https://drakedevelopers.slack.com/archives/C2WBPQDB7/p1587067285051000