Drake: multibody: Make API distinctions clearer between MBT and MPB

[EricCousineau-TRI]

At present, there are significant bits of the MultibodyTree API that are manually forwarded into the MultibodyPlant API as sugar. However, this introduces coupling and some confusion as to what really belongs where, and what the concrete responsibilities are for each API. (Is MBT really just for kinematics and dynamics? What if an external contributor decides to PR unique functionality to MBP?)

Per f2f with @amcastro-tri, this coupling was initially offered as sugar methods; however, as I've been binding stuff in Python, it's been a bit confusing to know what duplicates I should bind and test.

While it's true that copy and pasting APIs and docstrings are kind of low-effort, it makes testing (for proper code coverage, which we really should go for?), maintenance, and in general high-level explanations much more annoying and boilerplate-y.

My vote is to deprecate all of MultibodyPlants forwarding of the MultibodyTree API, and simplify things once and for all.

\cc @amcastro-tri @sherm1 @RussTedrake @sammy-tri @jwnimmer-tri

---

Given my current travel, I'd like to suggest these updated steps / timelines:

• [x] (Dec. ~9~ ~11~ 13) Forward relevant methods (w/ Python bindings) *
  
  
  
  • [x] #10128
  
  
  • [x] #10197
  
  
  • [x] #10212 CalcInverseDynamics, other bits
  
  
• [x] #10199 (Dec. ~12~ 13) Move MultibodyTree to internal::MultibodyTree, make alias
• [x] #10252 (Dec. ~14~ 17) Move other classes that do not need to be public into internal:
  
  
  
  • MultibodyTreeSystem, all cache objects, BodyNodeIndex, Mobilizer stuff, MultibodyTreeTopology, MultibodyTreeContext
  
  
• [x] (~Dec. 20~ Jan. 5?) Deprecate aliases.
• ~#10380 (Jan. 10) Shuffle headers to internal/~

* We have a TRI-only document (search for "Drake Issue 9366") for methods that we want to migrate; I can share this publicly if anyone outside of TRI requests it.

[sherm1]

We had been heading in the direction of supporting only MBPlant as the user-facing API because MBTree necessarily requires a Context for efficiency, and MBPlant provides user-friendly concepts like "Joints" rather than dynamicist-friendly concepts like "Mobilizers" (which are subtly different). That would suggest wrapping _only_ the MBPlant methods. @amcastro-tri, are you still moving that way?

[amcastro-tri]

Not quite, since it depends on the use case.
This is what I had in mind:

• Plant related queries live in the plant. Sizes, input/output ports, context creation, transmogrification.
• Tree related queries do live, publicly, in MBT. So that people can ask thinks like CalcMassMatrix or CalcAnalyticalJacobian() etc. Examples of this are #9372

For the second case, you might argue that all multibody queries, including things like Jacobians, should go through ports.

[EricCousineau-TRI]

It would be nice to draw a little finer line.

If there is any redundancy, I'd prefer that we have that functionality live in one and only one place, minimizing any sugary interface forwarding; we should avoid sugary forwarding as it introduces needless coupling, that makes it more annoying for bindings and other things, which should ultimately be tested in as concise a fashion as possible.

Can I ask if you'd be OK with moving tree-specific queries (such as sizes) to the tree? e.g. things like num_positions(), etc., would live in the tree, and only be accessible from the plant via something like plant.tree().num_positions().
For mutation, this does mean that we should expose a mutable_tree() from the MultibodyPlant for adding things like frames, and/or permit constructing a MultibodyPlant using a MultibodyTree (which is currently not possible). We should also make it clear when something like MultibodyPlant::AddBody is needed vs. MultibodyTree::AddBody (e.g. when there is plant-specific code that is needed for MultibodyTree).

Regarding having everything route through the ports, it would be nice to defer that until we can figure out a generic to make that less painful in the Systems framework when you start to have multiple small call sites (e.g. hiding the deal of allocating contexts / ports if / when you don't care about it, but avoid needing to write a whole bunch of slow wrapping code).

To make things more concrete, we should make a MBT vs. MBP matrix for methods for things like:

• Adding / querying elements
  
  
  
  • Kinematic: frames, mobillizers, joints
  
  
  • Inertial: bodies
  
  
  • Collision
  
  
  • Visualization
  
  
• Kinematic queries: state (num positions, num velocities), forward kinematics, state mutation (SetFreeBodyPose)
• Dynamic queries: input (num actuators, etc.)
• Collision queries: ?

That's all I can think of at the moment; will see if I can draft up a quick table.

[EricCousineau-TRI]

Here's a draft spreadsheet

[amcastro-tri]

Thanks for doing this @EricCousineau-TRI.

The reason I introduced the sugary forwarding methods is because then they'd allow you to do:

const auto& link1 = plant.AddRigidBody("link1", M_B);  // Someone defined M_B for link1.
plant.RegisterCollisionGeometry(
  link1, X_BG /* geometry frame G in the body frame B*/,
  geometry::Box(sizes), "link1_geometry", friction_coefficients, &scene_graph);

all with the plant's provided API. How would this look if we move AddRigidBody() out from the plant into MBT?

[amcastro-tri]

let me add that @SeanCurtis-TRI is thinking (correct me if I'm wrong) to provide a way for the SceneGraph to book-keep thinks like friction coefficients (and visual properties for visual geometry). I wonder if that would change the way we could do things. But for now RegisterCollisionGeometry lives in MBP for us to book keep properties for that geometry. The suggar is there to keep things more or less homogeneuos. I'm super open to ideas though

[EricCousineau-TRI]

Welcome! For the above code, why would you have aversion to this version if we mutate MutlibodyTree?

auto& tree = plant.mutable_tree();
const auto& link1 = tree.AddRigidBody("link1", M_B);  // Someone defined M_B for link1.
plant.RegisterCollisionGeometry(...);

I can understand why MBP has its own version of AddRigidBody, since it wants its num_collision and num_visuals to grow in lock-step with its bodies (but only the rigid ones???).
However, that can be resolved with other things:

• Rather than have *_geometries_ be a vector, just make it be a map<BodyIndex, Ptr>, then you don't have to worry about synchronization. If performance is a worry, you could always move this data to the finalized vector which Finalize() is called.
• Add an OnBodyAdded callback, which can handle internal queries to enable consistency.

Either one of these may lend themselves to enabling people to construct MultibodyTrees outside of MultibodyPlant, which would permit the development of APIs to use only MultibodyTree (which seems ideal if you don't need too much fanciness, as is what we're kind of doing with MBT in Anzu at present?).

[SeanCurtis-TRI]

@amcastro-tri is right. It is my intent to revisit how much of this works. In general, MBP shouldn't be saving geometry ids -- only frame ids. However, that's largely dependent on a general parsing interface. However, in the spirit of expediency, @amcastro-tri has made this API that effectively gets the job done (taking some of the pressure off of me....yay!) But it's around the corner.

[SeanCurtis-TRI]

The idea of using MBT without MBP is a bit of an historical anachronism. People could do that in the past because it was a one-stop shop. More particularly, someone could use the MBT to pose and test for collisions. In MBP that's no longer possible. You need an SG to do geometry queries. And SG updates its internals based on what's coming in on its input ports. The simplest way to not reproduce the MBP functionality that feeds to SG is to just use an MBP.

[EricCousineau-TRI]

@SeanCurtis-TRI From what you've written, I'm assuming that you're advocating that MBT shouldn't really be used at all, in which case we should have MBP cover all the bases?
~If this is the case, should we make a concrete decision soon in light of other features, such as @hongkai-dai's #9372? Might EOD tomorrow be feasible?~
If that's not the case, can I ask what your recommendation is in terms of delineating the APIs? (specifically guidelines that we write for users and developers about what goes where)

@hongkai-dai Having worked with the current API, and considering what'd be necessary for incorporating collision-based constraints in IK, can I ask if you have any insight into what would be optimal for features like what you're developing?
EDIT: Just read the PR and saw that you switched over to MultibodyPlant. Still, I'd like to ask, do you see value in (a) keeping the APIs separate, (b) just using MultibodyPlant, ~(c) keeping things as-is,~ or something else?

[sherm1]

My preference: do as much as possible through MBPlant. Provide read-only MBPlant::tree() method for querying as-built information about how we took the original (possibly cyclic) specifications and modeled them using a spanning tree and constraints.

[EricCousineau-TRI]

My concern is that this problem is still present: there's no hard distinction between the two, so it's ambiguous to know what should be used / developed upon /etc., and there's two ways to do the exact same thing.

To resolve this issue, I would really like there to be two choices that minimize ambiguity during development and consumption:
Either MBT is a public API (and we should decouple things as much as possible) or it's not (and we should deprecate its public API and promote everything to MultibodyPlant and never suggest that anyone use MultibodyTree outside of internal Drake, period).
The timeline can be relatively flexible (e.g. how it affects the current binding development, etc.), but I'd like to know what direction to go towards so that way we can write succinct tests, examples, tutorials, etc.

If there's no value to keeping MultibodyTree as a separate API (e.g. we always parse SDFs into MBP, all IK and other usable interfaces, etc., all go through MBP), then we should make it private, so that there's a consistent story.

[SeanCurtis-TRI]

I see what you mean. SG has something akin to MBT, but it's, by design, completely internal and all queries on that data come through the QueryObject (for queries that depend on input state) and SceneGraphInspector (for queries about the contents and topology of the scene graph). It's probably worth sitting down and making sure if this direction will satisfy use cases.

[sherm1]

If we can make MBTree private and satisfy all uses through MBPlant, I'd be very happy with that.

[amcastro-tri]

We had a little f2f with @EricCousineau-TRI last night. IMO we should do is to make sure something like IK works with whatever scheme we decide (hide/not-hide MBT, add MBP ports, have a bigger class wrapping MBP+SG... blah...). If we can successfully go through that exercise I believe we solved the problem (and assuming all caching magic can happen of course).
The reason I say this is because I don't feel comfortable just yet hiding/not-hiding APIs until we are absolutely sure we can get our basics to work.

We could prototype IK (starting off from @hongkai-dai's work in #9372) with only two constraints: 1) position constraint, 2) collision constraints. The prototype should successfully show how we wire all this together.

[SeanCurtis-TRI]

I like it. Proof of concept.

[EricCousineau-TRI]

Sounds good to me!

[EricCousineau-TRI]

@sherm1 @amcastro-tri The story of the separation between MBT and MBP, to me at least, seems to be getting murkier with the merge of #9489. The requirement of MultibodyTreeSystem for using MultibodyTree is only documented in MBT (which admittedly can be fixed), but seems to further indicate that MBT functionality should really only be accessible through MBP?

Might y'all have time at the on-site for us to make a concrete action item on this?

[sherm1]

Thanks, @EricCousineau-TRI -- this is a great topic to discuss f2f at the offsite. Let's do that.

[EricCousineau-TRI]

From meeting, we've deemed that we should be able to subsume the public MultibodyTree API into MultibodyPlant, and move MBT to be a private implementation detail.

Will post more details about plan of change for (a) public user interface (e.g. deprecation timeline) and (b) ensuring that things are convenient for developers.

[EricCousineau-TRI]

Suggested plan:

• Make an alias, internal::MultibodyTree, and have MultibodyPlant use this. Target date: Nov. 10
• Forward all useful and unique methods to MultibodyPlant (Calc* methods, SetFreeBody* methods, indexing, etc.). Target date: Nov. 15
  
  
  
  • The doc will be moved from MBT to MBP, and a placehold will be put in MBT referring to the MBP function.
  
  
  • Ideally, the only unittest additions would be a simple (and hopefully single) call to the forwarded method.
  
  
• Initiate deprecation in pydrake after the conclusion of 6.881; implement it by putting the definition of MBT in internal, make the public symbol an alias, and deprecate the alias. Target date: Dec. 20

Any objections?

[amcastro-tri]

This is great @EricCousineau-TRI. Are you thinking to do this migration yourself (I can help of course)? or who are you thinking the owner would be?

[EricCousineau-TRI]

Yuppers. (Also, updated the above plan to include your question about docs.)

[amcastro-tri]

Thanks @EricCousineau-TRI! this is awesome.

[sherm1]

@EricCousineau-TRI, before the new MBPlant API is set in stone I'd like to discuss the abstractions that we want it to support. Here is an outline of what is really happening (most users won't need to understand all of this):

• The urdf/sdf abstraction is the top level. That is, there is a collection of "models" we instantiate into "model instances". Models are labeling for human convenience; they do not have a physical meaning. Within the models are a set of "links" (roughly, frames that carry mass and geometry). Joints and other constraints are defined to interconnect the links, forming a (possibly cyclic) graph with directed edges. Such constraints may interconnect links from different models. Links without constraints are free to move with respect to World, unless marked "static" in which case they are welded to World.
• There is nothing in the sdf-level abstraction that says how the motion of the system should be represented by state variables; that is a modeling choice and there are many ways to do it. Different choices make different operations easy or hard. For example, positioning links is easiest with maximal coordinates; forming useful Jacobians is easiest in internal coordinates.
• When finalized, Drake will choose a representation. That will consist of a set of "multibodies" and "constraints". Each multibody is a tree with "bodies" as nodes and "mobilizers" as edges. (These correspond exactly to links and joints when the input graph has no cycles, but are subtly different in general.) The root of a multibody tree is called its "base body" and always has a mobilizer connecting it to World (often that is a Free mobilizer providing the base body with six dofs). When the base body dofs are moved, the entire multibody moves with it.
• (The collection of multibodies is exactly the abstraction that MultibodyTree implements; altogether it is a forest of multibody trees with World as their common root.)
• For robotics users, the chosen representation is highly relevant, since the multibodies are very compact and provide useful things like mass matrices and Jacobians and can be positioned in space via their base bodies. For example, see issue #9747 for a typical user request -- Russ wants to find the base body associated with a model instance he added, so that he can freely position it. Another example -- in #9740 you recently needed to identify subtrees of welded-together bodies.
• (The relevant topology is what the MultibodyTreeTopology class represents.)
• The assignment of kinematic state variables q & v is determined by the partitioning of the model collection into a spanning set of multibodies. Information about the as-built state assignments is needed to work with the system in matrix form, or in other numerically-convenient ways.
• Articulated q's and v's arise from the mobilizers. Flexible bodies will also introduce q's and v's that are internal to a body (not implemented yet).

I'm mentioning these various abstractions because as we consolidate the API into MBPlant, users will still need to deal with it at the levels of abstraction best-suited to the work to be done. Using different classes for that is one way to keep the API size manageable; if we don't do that we'll need to provide the same capabilities, perhaps in doxygen groupings in the flattened API.

[tri-ltyyu]

Per issue scrubbing with Sherm, since this affect user experience quite a bit, we're marking it high priority. Next step would be for Eric and Sherm to have a conversation to discuss API design

[EricCousineau-TRI]

For upcoming meeting, just did a brief pass of capturing the difference in APIs between MBP and MBT, just local to those classes:
https://gist.github.com/EricCousineau-TRI/1183ece65e199958f2f2967a0eea758a#file-diff-patch

[EricCousineau-TRI]

EDIT: Moved items up to top-level for checkbox-ness.

[EricCousineau-TRI]

Per discussion in #10380, resolved!