Modified MultibodyTree to replace its internal topology with SpanningForest. (#21820)

Harmonized BodyNode & Mobilizer with Mobilized Body numbering.

Author: sherm1

geometry/optimization/cspace_free_internal.cc

@@ -38,7 +38,7 @@ struct BodyPairHash {
 [[nodiscard]] bool ChainIsWelded(const multibody::MultibodyPlant<double>& plant,
                                  multibody::BodyIndex start,
                                  multibody::BodyIndex end) {
-  const std::vector<multibody::internal::MobilizerIndex> mobilizers =
+  const std::vector<multibody::internal::MobodIndex> mobilizers =
       multibody::internal::FindMobilizersOnPath(plant, start, end);
   if (mobilizers.size() == 0) {
     return true;

geometry/optimization/cspace_free_polytope_base.cc

@@ -208,7 +208,7 @@ void CspaceFreePolytopeBase::CalcSBoundsPolynomial(
 void CspaceFreePolytopeBase::SetIndicesOfSOnChainForBodyPair(
     const SortedPair<multibody::BodyIndex>& body_pair) {
   if (!map_body_pair_to_s_on_chain_.contains(body_pair)) {
-    const std::vector<multibody::internal::MobilizerIndex> mobilizer_indices =
+    const std::vector<multibody::internal::MobodIndex> mobilizer_indices =
         multibody::internal::FindMobilizersOnPath(rational_forward_kin_.plant(),
                                                   body_pair.first(),
                                                   body_pair.second());

multibody/parsing/test/detail_dmd_parser_test.cc

@@ -304,7 +304,7 @@ TEST_F(DmdParserTest, DefaultFreeBodyPoseAddsDuplicateJoints) {
 
   DRAKE_EXPECT_THROWS_MESSAGE(
       ParseModelDirectives(directives),
-      ".*already has a joint .* connecting 'dummy' to 'ball'.*");
+      ".*already has.*joint.*connecting.*dummy.*ball.*");
 }
 
 }  // namespace

multibody/plant/BUILD.bazel

@@ -147,6 +147,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

@@ -40,7 +40,7 @@ AccelerationsDueNonConstraintForcesCache<
                                                      topology)
     : forces(topology.num_rigid_bodies(), topology.num_velocities()),
       abic(topology),
-      Zb_Bo_W(topology.num_rigid_bodies()),
+      Zb_Bo_W(topology.num_mobods()),
       aba_forces(topology),
       ac(topology) {}
 

multibody/plant/multibody_plant.cc

@@ -27,6 +27,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/graph.h"
 #include "drake/multibody/tree/door_hinge.h"
 #include "drake/multibody/tree/prismatic_joint.h"
 #include "drake/multibody/tree/prismatic_spring.h"
@@ -1053,8 +1054,10 @@ const SceneGraphInspector<T>& MultibodyPlant<T>::EvalSceneGraphInspector(
 template <typename T>
 std::vector<const RigidBody<T>*> MultibodyPlant<T>::GetBodiesWeldedTo(
     const RigidBody<T>& body) const {
+  const internal::LinkJointGraph& graph = internal_tree().graph();
   const std::set<BodyIndex> island =
-      internal_tree().multibody_graph().FindBodiesWeldedTo(body.index());
+      graph.forest_is_valid() ? graph.GetLinksWeldedTo(body.index())
+                              : graph.CalcLinksWeldedTo(body.index());
   // Map body indices to pointers.
   std::vector<const RigidBody<T>*> sub_graph_bodies;
   for (BodyIndex body_index : island) {
@@ -1079,7 +1082,10 @@ std::vector<BodyIndex> MultibodyPlant<T>::GetBodiesKinematicallyAffectedBy(
           fmt::format("{}: joint with index {} is welded.", __func__, joint));
     }
   }
-  return internal_tree().GetBodiesKinematicallyAffectedBy(joint_indexes);
+  const std::set<BodyIndex> links =
+      internal_tree().GetBodiesKinematicallyAffectedBy(joint_indexes);
+  // TODO(sherm1) Change the return type to set to avoid this copy.
+  return std::vector<BodyIndex>(links.cbegin(), links.cend());
 }
 
 template <typename T>
@@ -1157,7 +1163,10 @@ 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()
+           .graph()
+           .link_by_index(frame_F.body().index())
+           .is_anchored()) {
     throw std::logic_error("Frame '" + frame_F.name() +
                            "' must be anchored to the world frame.");
   }
@@ -1527,7 +1536,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().graph().GetSubgraphsOfWeldedLinks();
   for (const auto& subgraph : subgraphs) {
     // Only operate on non-trivial weld subgraphs.
     if (subgraph.size() <= 1) {
@@ -3911,12 +3920,11 @@ void MultibodyPlant<T>::CalcReactionForces(
   // necessary frame conversions.
   for (JointIndex joint_index : GetJointIndices()) {
     const Joint<T>& joint = get_joint(joint_index);
-    const internal::MobilizerIndex mobilizer_index =
+    const internal::MobodIndex mobilizer_index =
         internal_tree().get_joint_mobilizer(joint_index);
     const internal::Mobilizer<T>& mobilizer =
         internal_tree().get_mobilizer(mobilizer_index);
-    const internal::MobodIndex mobod_index =
-        mobilizer.get_topology().mobod_index;
+    const internal::MobodIndex mobod_index = mobilizer.mobod().index();
 
     // Force on mobilized body B at mobilized frame's origin Mo, expressed in
     // world frame.
@@ -4051,7 +4059,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().graph().GetSubgraphsOfWeldedLinks();
 }
 
 template <typename T>

multibody/plant/multibody_plant.h

@@ -27,6 +27,7 @@
 #include "drake/multibody/plant/dummy_physical_model.h"
 #include "drake/multibody/plant/multibody_plant_config.h"
 #include "drake/multibody/plant/physical_model_collection.h"
+#include "drake/multibody/topology/graph.h"
 #include "drake/multibody/tree/force_element.h"
 #include "drake/multibody/tree/frame.h"
 #include "drake/multibody/tree/joint.h"
@@ -4640,8 +4641,8 @@ class MultibodyPlant : public internal::MultibodyTreeSystem<T> {
     return internal_tree().world_frame();
   }
 
-  /// Returns the number of bodies in the model, including the "world" body,
-  /// which is always part of the model.
+  /// Returns the number of RigidBody elements in the model, including the
+  /// "world" RigidBody, which is always part of the model.
   /// @see AddRigidBody().
   int num_bodies() const { return internal_tree().num_bodies(); }
 
@@ -4657,7 +4658,7 @@ class MultibodyPlant : public internal::MultibodyTreeSystem<T> {
   /// @throws std::exception if called pre-finalize.
   bool IsAnchored(const RigidBody<T>& body) const {
     DRAKE_MBP_THROW_IF_NOT_FINALIZED();
-    return internal_tree().get_topology().IsBodyAnchored(body.index());
+    return internal_tree().graph().link_by_index(body.index()).is_anchored();
   }
 
   /// @returns `true` if a body named `name` was added to the %MultibodyPlant.
@@ -4771,7 +4772,7 @@ class MultibodyPlant : public internal::MultibodyTreeSystem<T> {
       const RigidBody<T>& body) const;
 
   /// Returns all bodies whose kinematics are transitively affected by the given
-  /// vector of joints. The affected bodies are returned in increasing order of
+  /// vector of Joints. The affected bodies are returned in increasing order of
   /// body indexes. Note that this is a kinematic relationship rather than a
   /// dynamic one. For example, if one of the inboard joints is a free (6dof)
   /// joint, the kinematic influence is still felt even though dynamically
@@ -5289,7 +5290,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

@@ -1711,8 +1711,12 @@ GTEST_TEST(MultibodyPlantTest, GetBodiesKinematicallyAffectedBy) {
                               ".*No joint with index.*registered.*removed.");
 }
 
-// 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. Drake doesn't support
+// that yet.
+// TODO(sherm1) Remove this restriction and fix the test.
+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_url =
@@ -1725,15 +1729,10 @@ GTEST_TEST(MultibodyPlantTest, ReversedWeldError) {
       "extra", default_model_instance(), SpatialInertia<double>::NaN());
   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.*");
+  DRAKE_EXPECT_THROWS_MESSAGE(plant.Finalize(),
+                              ".*Finalize.*: parent/child ordering.*"
+                              "Joint extra_welds_to_world.*WorldModelInstance.*"
+                              "would have to be reversed.*");
 }
 
 // Verifies exact set of output ports we expect to be a direct feedthrough of
@@ -3848,8 +3847,8 @@ GTEST_TEST(SetRandomTest, SetDefaultWhenNoDistributionSpecified) {
   // explicitly added).
   const RigidBody<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_url =
       "package://drake/multibody/benchmarks/acrobot/acrobot.sdf";
   const ModelInstanceIndex acrobot =

multibody/plant/test/sap_driver_multidof_joints_test.cc

@@ -91,14 +91,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(
+      const SpanningForest::Mobod& mobod) const override {
     auto blue_print = std::make_unique<typename Joint<T>::BluePrint>();
+    const auto [inboard_frame, outboard_frame] =
+        this->tree_frames(mobod.is_reversed());
+    // 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::RpyBallMobilizer<T>>(
-        this->frame_on_parent(), this->frame_on_child());
+        mobod, *inboard_frame, *outboard_frame);
     blue_print->mobilizer = std::move(revolute_mobilizer);
     return blue_print;
   }

multibody/rational/rational_forward_kinematics.cc

@@ -66,10 +66,10 @@ RationalForwardKinematics::RationalForwardKinematics(
   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::RigidBodyTopology& body_topology =
+    const internal::RigidBodyTopology& rigid_body_topology =
         tree.get_topology().get_rigid_body(body_index);
     const internal::Mobilizer<double>* mobilizer =
-        &(tree.get_mobilizer(body_topology.inboard_mobilizer));
+        &(tree.get_mobilizer(rigid_body_topology.inboard_mobilizer));
     if (IsRevolute(*mobilizer)) {
       const symbolic::Variable s_angle(fmt::format("s[{}]", s_.size()));
       s_.push_back(s_angle);
@@ -229,8 +229,8 @@ RationalForwardKinematics::CalcChildBodyPoseAsMultilinearPolynomial(
       tree.get_topology().get_rigid_body(parent);
   const internal::RigidBodyTopology& child_topology =
       tree.get_topology().get_rigid_body(child);
-  internal::MobilizerIndex mobilizer_index;
-  bool is_order_reversed;
+  internal::MobodIndex mobilizer_index;
+  bool is_order_reversed{};
   if (parent_topology.parent_body.is_valid() &&
       parent_topology.parent_body == child) {
     is_order_reversed = true;

multibody/rational/rational_forward_kinematics.h

@@ -285,7 +285,7 @@ class RationalForwardKinematics {
   // indeterminates in the rational functions.
   std::vector<symbolic::Variable> s_;
   // Each s(i) is associated with a mobilizer.
-  std::unordered_map<symbolic::Variable::Id, internal::MobilizerIndex>
+  std::unordered_map<symbolic::Variable::Id, internal::MobodIndex>
       map_s_to_mobilizer_;
   // map_mobilizer_to_s_index_[mobilizer_index] returns the starting index of
   // the mobilizer's variable in s_ (the variable will be contiguous in s for

multibody/rational/rational_forward_kinematics_internal.cc

@@ -64,19 +64,20 @@ std::vector<BodyIndex> FindPath(const MultibodyPlant<double>& plant,
   return path;
 }
 
-std::vector<MobilizerIndex> FindMobilizersOnPath(
+std::vector<MobodIndex> FindMobilizersOnPath(
     const MultibodyPlant<double>& plant, BodyIndex start, BodyIndex end) {
   const std::vector<BodyIndex> path = FindPath(plant, start, end);
-  std::vector<MobilizerIndex> mobilizers_on_path;
+  std::vector<MobodIndex> mobilizers_on_path;
   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 RigidBodyTopology& body_topology =
+    const RigidBodyTopology& rigid_body_topology =
         tree.get_topology().get_rigid_body(path[i]);
-    if (path[i] != world_index() && body_topology.parent_body == path[i + 1]) {
+    if (path[i] != world_index() &&
+        rigid_body_topology.parent_body == 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(rigid_body_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.
@@ -97,10 +98,10 @@ BodyIndex FindBodyInTheMiddleOfChain(const MultibodyPlant<double>& plant,
   path_not_weld.reserve(path.size());
   path_not_weld.push_back(start);
   const MultibodyTree<double>& tree = GetInternalTree(plant);
-  const std::vector<MobilizerIndex> mobilizer_indices =
+  const std::vector<MobodIndex> mobilizer_indices =
       FindMobilizersOnPath(plant, path[0], path.back());
   for (int i = 0; i < static_cast<int>(mobilizer_indices.size()); ++i) {
-    const MobilizerIndex mobilizer_index = mobilizer_indices[i];
+    const MobodIndex mobilizer_index = mobilizer_indices[i];
     const Mobilizer<double>& mobilizer = tree.get_mobilizer(mobilizer_index);
     if (mobilizer.num_positions() != 0) {
       path_not_weld.push_back(path[i + 1]);

multibody/rational/rational_forward_kinematics_internal.h

@@ -30,7 +30,7 @@ std::vector<BodyIndex> FindPath(const MultibodyPlant<double>& plant,
 /*
  * Finds all the mobilizer on the path from start to the end.
  */
-std::vector<internal::MobilizerIndex> FindMobilizersOnPath(
+std::vector<internal::MobodIndex> FindMobilizersOnPath(
     const MultibodyPlant<double>& plant, BodyIndex start, BodyIndex end);
 
 /*

multibody/rational/test/rational_forward_kinematics_test_utilities.h

@@ -51,7 +51,7 @@ class IiwaTest : public ::testing::Test {
   const internal::MultibodyTree<double>& iiwa_tree_;
   const BodyIndex world_;
   std::array<BodyIndex, 8> iiwa_link_;
-  std::array<internal::MobilizerIndex, 8> iiwa_joint_;
+  std::array<internal::MobodIndex, 8> iiwa_joint_;
 };
 
 /*
@@ -66,7 +66,7 @@ class FinalizedIiwaTest : public ::testing::Test {
   const internal::MultibodyTree<double>& iiwa_tree_;
   const BodyIndex world_;
   std::array<BodyIndex, 8> iiwa_link_;
-  std::array<internal::MobilizerIndex, 8> iiwa_joint_;
+  std::array<internal::MobodIndex, 8> iiwa_joint_;
 };
 
 /*

multibody/topology/BUILD.bazel

@@ -10,28 +10,6 @@ package(
     default_visibility = ["//visibility:public"],
 )
 
-drake_cc_library(
-    name = "multibody_graph",
-    srcs = [
-        "multibody_graph.cc",
-    ],
-    hdrs = [
-        "multibody_graph.h",
-    ],
-    deps = [
-        "//common:sorted_pair",
-        "//multibody/tree:multibody_tree_indexes",
-    ],
-)
-
-drake_cc_googletest(
-    name = "multibody_graph_test",
-    deps = [
-        ":multibody_graph",
-        "//common/test_utilities:expect_throws_message",
-    ],
-)
-
 drake_cc_library(
     name = "multibody_topology",
     srcs = [

multibody/topology/graph.h

@@ -5,8 +5,6 @@ This is the file to #include to use LinkJointGraph. It includes
 subsidiary headers for nested class definitions to keep file sizes
 reasonable. */
 
-// TODO(sherm1) Not used yet by MultibodyPlant; see PR #20225.
-
 #define DRAKE_MULTIBODY_TOPOLOGY_GRAPH_INCLUDED
 
 // Don't alpha-sort these internal includes; the order matters.