Adds SpanningForest builder & modify MbP to use it.

Passes all tests (except space_xyz_floating_mobilizer is disabled)

Author: sherm1

multibody/plant/BUILD.bazel

@@ -149,6 +149,7 @@ drake_cc_library(
         "//multibody/contact_solvers/sap",
         "//multibody/fem",
         "//multibody/hydroelastics:hydroelastic_engine",
+        "//multibody/topology:multibody_topology",
         "//multibody/tree",
         "//systems/framework:diagram_builder",
         "//systems/framework:leaf_system",

multibody/plant/compliant_contact_manager.cc

@@ -39,9 +39,9 @@ template <typename T>
 AccelerationsDueNonConstraintForcesCache<
     T>::AccelerationsDueNonConstraintForcesCache(const MultibodyTreeTopology&
                                                      topology)
-    : forces(topology.num_bodies(), topology.num_velocities()),
+    : forces(topology.num_links(), topology.num_velocities()),
       abic(topology),
-      Zb_Bo_W(topology.num_bodies()),
+      Zb_Bo_W(topology.num_body_nodes()),
       aba_forces(topology),
       ac(topology) {}
 

multibody/plant/multibody_plant.cc

@@ -25,6 +25,7 @@
 #include "drake/multibody/plant/hydroelastic_traction_calculator.h"
 #include "drake/multibody/plant/make_discrete_update_manager.h"
 #include "drake/multibody/plant/slicing_and_indexing.h"
+#include "drake/multibody/topology/link_joint_graph.h"
 #include "drake/multibody/tree/prismatic_joint.h"
 #include "drake/multibody/tree/quaternion_floating_joint.h"
 #include "drake/multibody/tree/revolute_joint.h"
@@ -312,7 +313,6 @@ MultibodyPlant<T>::MultibodyPlant(
   // it less brittle.
   visual_geometries_.emplace_back();  // Entries for the "world" body.
   collision_geometries_.emplace_back();
-
   DeclareSceneGraphPorts();
 }
 
@@ -867,7 +867,7 @@ template <typename T>
 std::vector<const Body<T>*> MultibodyPlant<T>::GetBodiesWeldedTo(
     const Body<T>& body) const {
   const std::set<BodyIndex> island =
-      internal_tree().multibody_graph().FindBodiesWeldedTo(body.index());
+      internal_tree().link_joint_graph().FindLinksWeldedTo(body.index());
   // Map body indices to pointers.
   std::vector<const Body<T>*> sub_graph_bodies;
   for (BodyIndex body_index : island) {
@@ -967,7 +967,7 @@ void MultibodyPlant<T>::SetFreeBodyPoseInAnchoredFrame(
   DRAKE_MBP_THROW_IF_NOT_FINALIZED();
   this->ValidateContext(context);
 
-  if (!internal_tree().get_topology().IsBodyAnchored(frame_F.body().index())) {
+  if (!internal_tree().get_topology().IsLinkAnchored(frame_F.body().index())) {
     throw std::logic_error("Frame '" + frame_F.name() +
                            "' must be anchored to the world frame.");
   }
@@ -991,17 +991,17 @@ void MultibodyPlant<T>::CalcSpatialAccelerationsFromVdot(
   internal_tree().CalcSpatialAccelerationsFromVdot(
       context, internal_tree().EvalPositionKinematics(context),
       internal_tree().EvalVelocityKinematics(context), known_vdot, A_WB_array);
-  // Permute BodyNodeIndex -> BodyIndex.
+  // Permute BodyNodeIndex -> LinkIndex.
   // TODO(eric.cousineau): Remove dynamic allocations. Making this in-place
   // still required dynamic allocation for recording permutation indices.
   // Can change implementation once MultibodyTree becomes fully internal.
   std::vector<SpatialAcceleration<T>> A_WB_array_node = *A_WB_array;
   const internal::MultibodyTreeTopology& topology =
       internal_tree().get_topology();
   for (internal::BodyNodeIndex node_index(1);
-       node_index < topology.get_num_body_nodes(); ++node_index) {
-    const BodyIndex body_index = topology.get_body_node(node_index).body;
-    (*A_WB_array)[body_index] = A_WB_array_node[node_index];
+       node_index < topology.num_body_nodes(); ++node_index) {
+    const LinkIndex link_index = topology.get_body_node(node_index).link;
+    (*A_WB_array)[link_index] = A_WB_array_node[node_index];
   }
 }
 
@@ -1078,6 +1078,7 @@ void MultibodyPlant<T>::Finalize() {
   if (geometry_source_is_registered()) {
     ApplyDefaultCollisionFilters();
   }
+
   FinalizePlantOnly();
 
   // Make the manager of discrete updates.
@@ -1316,7 +1317,7 @@ void MultibodyPlant<T>::ApplyDefaultCollisionFilters() {
   }
   // We explicitly exclude collisions within welded subgraphs.
   std::vector<std::set<BodyIndex>> subgraphs =
-      internal_tree().multibody_graph().FindSubgraphsOfWeldedBodies();
+      internal_tree().link_joint_graph().FindSubgraphsOfWeldedLinks();
   for (const auto& subgraph : subgraphs) {
     // Only operate on non-trivial weld subgraphs.
     if (subgraph.size() <= 1) {
@@ -3494,7 +3495,7 @@ void MultibodyPlant<T>::RemoveUnsupportedScalars(
 template <typename T>
 std::vector<std::set<BodyIndex>>
 MultibodyPlant<T>::FindSubgraphsOfWeldedBodies() const {
-  return internal_tree().multibody_graph().FindSubgraphsOfWeldedBodies();
+  return internal_tree().link_joint_graph().FindSubgraphsOfWeldedLinks();
 }
 
 template <typename T>

multibody/plant/multibody_plant.h

@@ -26,7 +26,8 @@
 #include "drake/multibody/plant/discrete_update_manager.h"
 #include "drake/multibody/plant/multibody_plant_config.h"
 #include "drake/multibody/plant/physical_model.h"
-#include "drake/multibody/topology/multibody_graph.h"
+#include "drake/multibody/topology/link_joint_graph.h"
+#include "drake/multibody/topology/spanning_forest.h"
 #include "drake/multibody/tree/force_element.h"
 #include "drake/multibody/tree/multibody_tree-inl.h"
 #include "drake/multibody/tree/multibody_tree_system.h"
@@ -4345,10 +4346,13 @@ class MultibodyPlant : public internal::MultibodyTreeSystem<T> {
     return internal_tree().world_frame();
   }
 
-  /// Returns the number of bodies in the model, including the "world" body,
+  /// Returns the number of Links in the model, including the "world" Link,
   /// which is always part of the model.
   /// @see AddRigidBody().
-  int num_bodies() const { return internal_tree().num_bodies(); }
+  int num_links() const { return internal_tree().num_links(); }
+
+  /// Alternate spelling for num_links(). Prefer num_links().
+  int num_bodies() const { return num_links(); }
 
   /// Returns a constant reference to the body with unique index `body_index`.
   /// @throws std::exception if `body_index` does not correspond to a body in
@@ -4362,7 +4366,7 @@ class MultibodyPlant : public internal::MultibodyTreeSystem<T> {
   /// @throws std::exception if called pre-finalize.
   bool IsAnchored(const Body<T>& body) const {
     DRAKE_MBP_THROW_IF_NOT_FINALIZED();
-    return internal_tree().get_topology().IsBodyAnchored(body.index());
+    return internal_tree().get_topology().IsLinkAnchored(body.index());
   }
 
   /// @returns `true` if a body named `name` was added to the %MultibodyPlant.
@@ -4966,7 +4970,7 @@ class MultibodyPlant : public internal::MultibodyTreeSystem<T> {
   /// @} <!-- Introspection -->
 
 #ifndef DRAKE_DOXYGEN_CXX
-  // Internal-only access to MultibodyGraph::FindSubgraphsOfWeldedBodies();
+  // Internal-only access to LinkJointGraph::FindSubgraphsOfWeldedBodies();
   // TODO(calderpg-tri) Properly expose this method (docs/tests/bindings).
   std::vector<std::set<BodyIndex>> FindSubgraphsOfWeldedBodies() const;
 #endif

multibody/plant/test/multibody_plant_test.cc

@@ -1689,8 +1689,11 @@ GTEST_TEST(MultibodyPlantTest, GetBodiesKinematicallyAffectedBy) {
                               ".*No joint with index.*registered.");
 }
 
-// Regression test for unhelpful error message -- see #14641.
-GTEST_TEST(MultibodyPlantTest, ReversedWeldError) {
+// Weld a body to World but with the body as the parent and World as the
+// child. This is fine but must be implemented with a reversed Weld
+// mobilizer that specifies World as the inboard body. We're just verifying
+// here that we can create the plant and finalize it with no trouble.
+GTEST_TEST(MultibodyPlantTest, ReversedWeldJoint) {
   // This test expects that the following model has a world body and a pair of
   // welded-together bodies.
   const std::string sdf_file =
@@ -1702,16 +1705,7 @@ GTEST_TEST(MultibodyPlantTest, ReversedWeldError) {
   const Body<double>& extra = plant.AddRigidBody(
       "extra", default_model_instance(), SpatialInertia<double>());
   plant.WeldFrames(extra.body_frame(), plant.world_frame());
-
-  // The important property of this message is that it reports some identifier
-  // for the involved objects, so at least the developer can map those back to
-  // objects and deduce what API call was in error. If the details of the
-  // message change, update this check to match. If in future the error can be
-  // caught at the WeldFrames() step, so much the better. Modify this test to
-  // reflect that.
-  DRAKE_EXPECT_THROWS_MESSAGE(
-      plant.Finalize(),
-      ".*already has a joint.*extra_welds_to_world.*joint.*not allowed.*");
+  EXPECT_NO_THROW(plant.Finalize());
 }
 
 // Utility to verify the subset of output ports we expect to be direct a
@@ -3504,8 +3498,8 @@ GTEST_TEST(SetRandomTest, SetDefaultWhenNoDistributionSpecified) {
   // explicitly added).
   const Body<double>& body1 =
       plant.AddRigidBody("free body 1", SpatialInertia<double>::MakeUnitary());
-  plant.AddJoint<QuaternionFloatingJoint>("" + body1.name(), plant.world_body(),
-                                          {}, body1, {});
+  plant.AddJoint<QuaternionFloatingJoint>(body1.name(), plant.world_body(), {},
+                                          body1, {});
   const std::string acrobot_file_name =
       "drake/multibody/benchmarks/acrobot/acrobot.sdf";
   const ModelInstanceIndex acrobot =

multibody/plant/test/sap_driver_multidof_joints_test.cc

@@ -85,14 +85,17 @@ class MultiDofJointWithLimits final : public Joint<T> {
   int do_get_velocity_start() const override { return 0; }
   int do_get_position_start() const override { return 0; }
 
-  std::unique_ptr<typename Joint<T>::BluePrint> MakeImplementationBlueprint()
-      const override {
+  std::unique_ptr<typename Joint<T>::BluePrint> MakeImplementationBlueprint(
+      bool use_reversed_mobilizer) const override {
     auto blue_print = std::make_unique<typename Joint<T>::BluePrint>();
+    const auto [inboard_frame, outboard_frame] =
+        this->tree_frames(use_reversed_mobilizer);
+    // TODO(sherm1) The mobilizer needs to be reversed, not just the frames.
     // The only restriction here relevant for these tests is that we provide a
     // mobilizer with kNumDofs positions and velocities, so that indexes are
     // consistent during MultibodyPlant::Finalize().
     auto revolute_mobilizer = std::make_unique<internal::SpaceXYZMobilizer<T>>(
-        this->frame_on_parent(), this->frame_on_child());
+        *inboard_frame, *outboard_frame);
     blue_print->mobilizer = std::move(revolute_mobilizer);
     return blue_print;
   }

multibody/rational/rational_forward_kinematics.cc

@@ -66,12 +66,12 @@ RationalForwardKinematics::RationalForwardKinematics(
   // Initialize map_mobilizer_to_s_index_ to -1, where -1 indicates "no s
   // variable".
   map_mobilizer_to_s_index_ = std::vector<int>(tree.num_mobilizers(), -1);
-  for (BodyIndex body_index(1); body_index < plant_.num_bodies();
-       ++body_index) {
-    const internal::BodyTopology& body_topology =
-        tree.get_topology().get_body(body_index);
+  for (LinkIndex link_index(1); link_index < plant_.num_bodies();
+       ++link_index) {
+    const internal::LinkTopology& link_topology =
+        tree.get_topology().get_link(link_index);
     const internal::Mobilizer<double>* mobilizer =
-        &(tree.get_mobilizer(body_topology.inboard_mobilizer));
+        &(tree.get_mobilizer(link_topology.inboard_mobilizer));
     if (IsRevolute(*mobilizer)) {
       const symbolic::Variable s_angle(fmt::format("s[{}]", s_.size()));
       s_.push_back(s_angle);
@@ -230,18 +230,18 @@ RationalForwardKinematics::CalcChildBodyPoseAsMultilinearPolynomial(
   // X_F'M' = X_FM.inverse()
   // X_M'C' = X_PF.inverse()
   const internal::MultibodyTree<double>& tree = GetInternalTree(plant_);
-  const internal::BodyTopology& parent_topology =
-      tree.get_topology().get_body(parent);
-  const internal::BodyTopology& child_topology =
-      tree.get_topology().get_body(child);
+  const internal::LinkTopology& parent_topology =
+      tree.get_topology().get_link(parent);
+  const internal::LinkTopology& child_topology =
+      tree.get_topology().get_link(child);
   internal::MobilizerIndex mobilizer_index;
   bool is_order_reversed;
-  if (parent_topology.parent_body.is_valid() &&
-      parent_topology.parent_body == child) {
+  if (parent_topology.parent_link.is_valid() &&
+      parent_topology.parent_link == child) {
     is_order_reversed = true;
     mobilizer_index = parent_topology.inboard_mobilizer;
-  } else if (child_topology.parent_body.is_valid() &&
-             child_topology.parent_body == parent) {
+  } else if (child_topology.parent_link.is_valid() &&
+             child_topology.parent_link == parent) {
     is_order_reversed = false;
     mobilizer_index = child_topology.inboard_mobilizer;
   } else {

multibody/rational/rational_forward_kinematics_internal.cc

@@ -35,24 +35,24 @@ std::vector<BodyIndex> FindPath(const MultibodyPlant<double>& plant,
     }
   };
   while (!worklist.empty()) {
-    BodyIndex current = worklist.front();
+    LinkIndex current = worklist.front();
     worklist.pop();
     if (current == end) {
       break;
     }
-    const BodyTopology& current_node = topology.get_body(current);
+    const LinkTopology& current_node = topology.get_link(current);
     if (current != world_index()) {
-      const BodyIndex parent = current_node.parent_body;
+      const LinkIndex parent = current_node.parent_link;
       visit_edge(current, parent);
     }
-    for (BodyIndex child : current_node.child_bodies) {
+    for (LinkIndex child : current_node.child_links) {
       visit_edge(current, child);
     }
   }
 
   // Retrieve the path in reverse order.
-  std::vector<BodyIndex> path;
-  for (BodyIndex current = end; ; current = ancestors.at(current)) {
+  std::vector<LinkIndex> path;
+  for (LinkIndex current = end; ; current = ancestors.at(current)) {
     path.push_back(current);
     if (current == start) {
       break;
@@ -71,16 +71,16 @@ std::vector<MobilizerIndex> FindMobilizersOnPath(
   mobilizers_on_path.reserve(path.size() - 1);
   const MultibodyTree<double>& tree = GetInternalTree(plant);
   for (int i = 0; i < static_cast<int>(path.size()) - 1; ++i) {
-    const BodyTopology& body_topology = tree.get_topology().get_body(path[i]);
-    if (path[i] != world_index() && body_topology.parent_body == path[i + 1]) {
+    const LinkTopology& link_topology = tree.get_topology().get_link(path[i]);
+    if (path[i] != world_index() && link_topology.parent_link == path[i + 1]) {
       // path[i] is the child of path[i+1] in MultibodyTreeTopology, they are
       // connected by path[i]'s inboard mobilizer.
-      mobilizers_on_path.push_back(body_topology.inboard_mobilizer);
+      mobilizers_on_path.push_back(link_topology.inboard_mobilizer);
     } else {
       // path[i] is the parent of path[i+1] in MultibodyTreeTopology, they are
       // connected by path[i+1]'s inboard mobilizer.
       mobilizers_on_path.push_back(
-          tree.get_topology().get_body(path[i + 1]).inboard_mobilizer);
+          tree.get_topology().get_link(path[i + 1]).inboard_mobilizer);
     }
   }
   return mobilizers_on_path;

multibody/rational/test/rational_forward_kinematics_test_utilities.cc

@@ -82,7 +82,7 @@ IiwaTest::IiwaTest()
     iiwa_link_[i] =
         iiwa_->GetBodyByName("iiwa_link_" + std::to_string(i)).index();
     iiwa_joint_[i] =
-        iiwa_tree_.get_topology().get_body(iiwa_link_[i]).inboard_mobilizer;
+        iiwa_tree_.get_topology().get_link(iiwa_link_[i]).inboard_mobilizer;
   }
 }
 
@@ -94,7 +94,7 @@ FinalizedIiwaTest::FinalizedIiwaTest()
     iiwa_link_[i] =
         iiwa_->GetBodyByName("iiwa_link_" + std::to_string(i)).index();
     iiwa_joint_[i] =
-        iiwa_tree_.get_topology().get_body(iiwa_link_[i]).inboard_mobilizer;
+        iiwa_tree_.get_topology().get_link(iiwa_link_[i]).inboard_mobilizer;
   }
 }
 

multibody/topology/BUILD.bazel

@@ -11,23 +11,26 @@ package(
 )
 
 drake_cc_library(
-    name = "multibody_graph",
+    name = "multibody_topology",
     srcs = [
-        "multibody_graph.cc",
+        "link_joint_graph.cc",
+        "spanning_forest.cc",
     ],
     hdrs = [
-        "multibody_graph.h",
+        "link_joint_graph.h",
+        "spanning_forest.h",
     ],
     deps = [
+        "//common:copyable_unique_ptr",
         "//common:sorted_pair",
         "//multibody/tree:multibody_tree_indexes",
     ],
 )
 
 drake_cc_googletest(
-    name = "multibody_graph_test",
+    name = "link_joint_graph_test",
     deps = [
-        ":multibody_graph",
+        ":multibody_topology",
         "//common/test_utilities:expect_throws_message",
     ],
 )