[multibody topology] Modify MultibodyPlant to use new topology

[sherm1]

This is the final PR in the #20225 train. All the new topology functionality is already in master as of #21818. This PR just switches MultibodyPlant & MultibodyTree to make use of the new stuff and removes the old topology code. (First check box in MultibodyPlant performance epic #18442.)

The goal here is just to make the switchover while preserving the existing functionality, passing all the same unit tests. Taking advantage of newly-enabled functionality like combining welded-together bodies and breaking loops is still to come. Upcoming restructuring for a high-speed kernel is facilitated here by making the (BodyNode, Mobilizer) pair equivalent to a SpanningForest Mobod (previously Mobilizers had a separate numbering scheme from BodyNodes.)

Many functions that were computed on the fly before (e.g. IsBodyAnchored()) are replaced with fast precalculated functions supplied by SpanningForest.

All the changes here are in internal code so no release notes are needed.

For reviewers: what changes are here

• SpanningForest is built at the start of Finalize and defines the mobilized bodies, tree assignments, depth-first numbering, correspondence to the input rigid bodies ("links") and joints, assignments of q's and v's, etc. All of that is available now before we create the first BodyNode & Mobilizer.
• There is no longer a MobilizerIndex. Instead MobodIndex now serves as the index for Mobods, BodyNodes, and Mobilizers -- they represent the same concept identically now.
• The computational data structures are now built by running through all the Mobods in their already-constructed depth-first order, and generating the corresponding (BodyNode, Mobilizer) pair in that order.
• MakeImplementationBlueprint() for each joint type is now handed the Mobod that models it. That is then used to construct the mobilizer which is just an implementation of the Mobod, not a separately numbered element. The Mobod says whether the mobilizer should be reversed from the original Joint, in which case the parent & child frames swap positions. Actually reversing the generalized coordinates is left as a TODO for now.
• There is now a dummy mobilizer 0 for the World Mobod to harmonize the numbering. It doesn't do anything.
• The new graph & forest are copyable so transmogrifying is easier.
• There are a few name changes and cleanups for clarity.
• multibody_tree_topology.{h,cc} are mostly gutted since everything needed there is precalculated now. Most of the data structures remain but are much easier to populate now.
• There are some minor changes to the test cases but they are mostly unchanged.

---

This change is 

[sherm1]

+@amcastro-tri for feature review, please (or LMK if you don't have time before your vacation)

Reviewable status: LGTM missing from assignee amcastro-tri, needs platform reviewer assigned, needs at least two assigned reviewers, labeled "do not merge", missing label for release notes (waiting on @sherm1)

[amcastro-tri]

Lots of hard work here, and all unit tests pass!
Just minor comments.

+@EricCousineau-TRI for platform review please.

Reviewed 75 of 78 files at r1.
Reviewable status: 10 unresolved discussions, LGTM missing from assignee EricCousineau-TRI(platform)

a discussion (no related file):
I imagine you considered this so I'll ask, why to keep MultibodyTreeTopology around? is that to simplefy the refactoring? is the plan to eventually retire it?

---

---

multibody/benchmarks/free_body/test/multibody_plant_free_body_test.cc line 21 at r1 (raw file):

#include "drake/multibody/tree/mobilizer.h"
#include "drake/multibody/tree/multibody_tree.h"
#include "drake/multibody/tree/rigid_body.h"

are these needed?

---

multibody/plant/test/multibody_plant_test.cc line 1730 at r1 (raw file):

      "extra", default_model_instance(), SpatialInertia<double>::NaN());
  plant.WeldFrames(extra.body_frame(), plant.world_frame());
  EXPECT_NO_THROW(plant.Finalize());

could you place a note here of when/where-from a message would be thrown?
Do we need to unit test that instead?

---

multibody/tree/ball_rpy_joint.cc line 71 at r1 (raw file):

  auto blue_print = std::make_unique<typename Joint<T>::BluePrint>();
  const auto [inboard_frame, outboard_frame] =
      this->tree_frames(mobod.is_reversed());

since this is not implemented, consider placing a DRAKE_DEMAND(!mobod.is_reveresed())?
Here or wherever else it'd be appropriate.
Or simply on each MakeImplementationBlueprint().

---

multibody/tree/mobilizer_impl.h line 200 at r1 (raw file):

      const systems::Context<T>& context) const {
    return this->get_parent_tree().template get_state_segment<kNv>(context,
        num_qs_in_state() + this->velocity_start_in_v());

nit, here and in other places, maybe not a bad idea to have the mobod store the index within a full state vector? as before?

---

multibody/tree/multibody_tree_topology.cc line 285 at r1 (raw file):

    if (node_index != 0) {         // Skip if World.
      parent_node = rigid_bodies_[parent_body_index].mobod_index;
      body_nodes_[parent_node].child_nodes.push_back(node_index);

nit, consider adding NB stating this is true given forest.mobds() are in depth-first order

---

multibody/tree/revolute_spring.cc line 6 at r1 (raw file):

#include "drake/multibody/tree/multibody_tree.h"
#include "drake/multibody/tree/rigid_body.h"

is this intentional?

---

multibody/tree/test/body_node_test.cc line 84 at r1 (raw file):

  one_by_one(0, 0) = -1;  // This should *definitely* fail.

  const SpanningForest::Mobod* dummy_mobod{};

nit, itnialize to nullptr? or create an actual dummy object?

---

multibody/tree/test/multibody_tree_creation_test.cc line 253 at r1 (raw file):

// is created before node (2) because joint j2 was added before j4. Similarly,
// node (7) is created before (8) because joint j1 was added before j6. Notice
// that bodies 8 and 9 are anchored to World. We have not selected the option to

nit,

Suggestion:

 that links 8 and 9 are a

---

multibody/tree/test/screw_mobilizer_test.cc line 57 at r1 (raw file):

TEST_F(ScrewMobilizerTest, ExceptionRaisingWhenZeroPitch) {
  const double zero_screw_pitch{0};
  const SpanningForest::Mobod* dummy_mobod{};

ditto, initialize to nullptr or instantiate a dummy Mobod.

[EricCousineau-TRI]

platform! thanks for the changelist summary!

Reviewed 78 of 78 files at r1, all commit messages.
Reviewable status: 10 unresolved discussions

---

multibody/topology/BUILD.bazel line 13 at r1 (raw file):

)

drake_cc_library(

BTW Due to visibility, downstream users could have been consuming this target.
However, I don't see obvious usages in Anzu, so probably no need to deprecate the target.

[amcastro-tri]

Reviewed 3 of 78 files at r1.
Reviewable status: 13 unresolved discussions

---

multibody/tree/multibody_tree.h line 486 at r1 (raw file):

  // Registers a joint in the graph if it isn't already there.
  void RegisterJointInGraph(const Joint<T>& joint);

nit, consider something like MaybeRegisterJointInGraph() or similar.

---

multibody/tree/multibody_tree.h line 966 at r1 (raw file):

  [[nodiscard]] const SpanningForest& forest() const {
    DRAKE_ASSERT(graph().forest_is_valid());

shouldn't this be DRAKE_DEMAND?

---

multibody/tree/multibody_tree.cc line 95 at r1 (raw file):

// Registers a joint in the graph if it isn't already there.
template <typename T>
void MultibodyTree<T>::RegisterJointInGraph(const Joint<T>& joint) {

ah, maybe consider naming something like AddAndMaybeRegisterJoint()?

[sherm1]

Thanks, @amcastro-tri and @EricCousineau-TRI! All review comments addressed, PTAL.

Reviewable status: 8 unresolved discussions

a discussion (no related file):

Previously, amcastro-tri (Alejandro Castro) wrote…

I imagine you considered this so I'll ask, why to keep MultibodyTreeTopology around? is that to simplefy the refactoring? is the plan to eventually retire it?

Yes exactly. It was too hard to get rid of MultibodyTreeTopology entirely this time around - it would have required many ugly-but-trivial changes throughout the code that would have disguised the actual substantive changes. It should be a lot easier to do that with all the new functionality in place. We can do it all at once or a bit at a time with some easy-to-review PRs.

---

---

multibody/plant/test/multibody_plant_test.cc line 1730 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

could you place a note here of when/where-from a message would be thrown?
Do we need to unit test that instead?

Done. Per f2f reversed joints are now rejected with a suitable error message as tested here.

---

multibody/topology/BUILD.bazel line 13 at r1 (raw file):

Previously, EricCousineau-TRI (Eric Cousineau) wrote…

BTW Due to visibility, downstream users could have been consuming this target.
However, I don't see obvious usages in Anzu, so probably no need to deprecate the target.

Good, thanks. Also this was always in ::internal (as is the new stuff).

---

multibody/tree/ball_rpy_joint.cc line 71 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

since this is not implemented, consider placing a DRAKE_DEMAND(!mobod.is_reveresed())?
Here or wherever else it'd be appropriate.
Or simply on each MakeImplementationBlueprint().

Done. Catching at the top in MultibodyTree::CreateJointImplementations(), unit tested in multibody_plant_test.cc.

---

multibody/tree/mobilizer_impl.h line 200 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

nit, here and in other places, maybe not a bad idea to have the mobod store the index within a full state vector? as before?

That doesn't fit with the longer term plan that we should be separating q's from v's (at least internally) so we can better manage the cache dependencies. Also it was very difficult in the former code to figure out when a velocity index included an offset for the q's and when it did not (and there wasn't consistency). Better to have it clear IMO than to attempt a dubious pre-optimization. The kernel shouldn't care either way -- it won't be digging around in the context. I'm going to leave it like this for now.

---

multibody/tree/multibody_tree.h line 486 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

nit, consider something like MaybeRegisterJointInGraph() or similar.

Done

---

multibody/tree/multibody_tree.h line 966 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

shouldn't this be DRAKE_DEMAND?

OK. This is internal and gets called a lot. It is checked at finalize with a DRAKE_DEMAND already (multibody_tree.cc:848). I didn't want to re-check it every time we need topology info.

---

multibody/tree/multibody_tree.cc line 95 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

ah, maybe consider naming something like AddAndMaybeRegisterJoint()?

Done. Settled on RegisterJointAndMaybeJointTypeInGraph()

---

multibody/tree/multibody_tree_topology.cc line 285 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

nit, consider adding NB stating this is true given forest.mobds() are in depth-first order

Done (here and in the comment at the start of the loop also)

---

multibody/tree/test/body_node_test.cc line 84 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

nit, itnialize to nullptr? or create an actual dummy object?

Done. Much better with a dummy object, thanks.

---

multibody/tree/test/multibody_tree_creation_test.cc line 253 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

nit,

Done

---

multibody/tree/test/screw_mobilizer_test.cc line 57 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

ditto, initialize to nullptr or instantiate a dummy Mobod.

Done

---

multibody/benchmarks/free_body/test/multibody_plant_free_body_test.cc line 21 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

are these needed?

Nope -- good eye! Done.

---

multibody/tree/revolute_spring.cc line 6 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

is this intentional?

Nope, thanks.

[amcastro-tri]

Looks great! just one minor nit

Reviewed 10 of 10 files at r2, all commit messages.
Reviewable status: 1 unresolved discussion

a discussion (no related file):

Previously, sherm1 (Michael Sherman) wrote…

Yes exactly. It was too hard to get rid of MultibodyTreeTopology entirely this time around - it would have required many ugly-but-trivial changes throughout the code that would have disguised the actual substantive changes. It should be a lot easier to do that with all the new functionality in place. We can do it all at once or a bit at a time with some easy-to-review PRs.

Sounds good, thanks

---

---

multibody/tree/mobilizer_impl.h line 200 at r1 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

That doesn't fit with the longer term plan that we should be separating q's from v's (at least internally) so we can better manage the cache dependencies. Also it was very difficult in the former code to figure out when a velocity index included an offset for the q's and when it did not (and there wasn't consistency). Better to have it clear IMO than to attempt a dubious pre-optimization. The kernel shouldn't care either way -- it won't be digging around in the context. I'm going to leave it like this for now.

SGTM

---

multibody/tree/multibody_tree.cc line 791 at r2 (raw file):

                      "ordering in your joint definition so that all "
                      "parent/child directions form a tree structure.",
                      joint.name(), joint.model_instance()));

nit, consider

Suggestion:

GetModelInstanceName(joint.model_instance())))

[sherm1]

Reviewable status: complete! all discussions resolved, LGTM from assignees amcastro-tri,EricCousineau-TRI(platform)

---

multibody/tree/multibody_tree.cc line 791 at r2 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

nit, consider

Nice, thanks! Done

[RussTedrake]

Should we expect any performance changes associated with this PR?

[sherm1]

Nothing significant yet. Still to come.

[RussTedrake]

Is there any risk of performance degradation? Should we alert our most relevant folks to watch for it? (I assume we ran the benchmarks?)

[sherm1]

Running our go-to MbP benchmark (cassie) before & after are comparable (kinematics & dynamics). The other major change was in the setup of MbT internals at the start (building the computational data structures from the input links & joints). It should be somewhat faster but I don't have a good way to measure it. And hopefully that's an insignificant part of any important sim. So I think the overall performance risk of this change is low -- certainly worth noting that there was a big change though in case I'm wrong!

The upcoming changes to MbT should start showing some performance wins.

[RussTedrake]

Great. Thanks!