Changes to MbP and MbT

Passes all tests now (except space_xyz_floating_mobilizer is disabled)

Author: sherm1

multibody/plant/compliant_contact_manager.cc

@@ -40,9 +40,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,7 +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/spanning_forest_model.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"
@@ -992,7 +992,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.");
   }
@@ -1016,17 +1016,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];
   }
 }
 
@@ -1097,65 +1097,13 @@ void MultibodyPlant<T>::RenameModelInstance(ModelInstanceIndex model_instance,
 
 template<typename T>
 void MultibodyPlant<T>::Finalize() {
-  /* Given the user-defined directed graph of Links and Joints, decide how we're
-  going to model this using a spanning forest comprised of
-    - bodies and their mobilizers, paired as "mobilized bodies" (mobods) and
-      directed by inboard/outboard edges, and
-    - added constraints where needed to close kinematic loops in the graph.
-  The modeler sorts the mobilized bodies into depth-first order.
-
-  Every Link will be modeled with one "primary" body and possibly several
-  "shadow" bodies. Every Joint will be modeled with a mobilizer, with the
-  Joint's parent/child connections mapped to the mobilizer's inboard/outboard
-  connection or to the reverse, as necessary for the mobilizers to form a
-  properly-directed tree. Every body in the tree must have a path in the
-  inboard direction connecting it to World. If necessary, additional "floating"
-  (6 dof) or "weld" (0 dof) mobilizers are added to make the final connection
-  to World.
-
-  During the modeling process, the LinkJointGraph is augmented to have
-  additional elements to provide an interface to the additional elements that
-  were required to build the model. Below, we will augment the MultibodyPlant
-  elements to match, so that advanced users can use the familiar Plant API to
-  access and control these elements. */
-  this->mutable_tree().BuildSpanningForest();
-
-  /* Add Links, Joints, and Constraints that were created during the modeling
-  process. */
-
-  const internal::LinkJointGraph& graph = internal_tree().link_joint_graph();
-  unused(graph);
-
-  /* TODO(sherm1) why not use MbP as the LinkJointGraph and do away with
-      the extra class? Then we wouldn't have to repeat these additions.
-  // Added floating mobods are in the order the new joints were added so
-  // the index we get in the plant should match the one stored with the mobod.
-  for (auto index : graph.added_floating_mobods()) {
-      const internal::SpanningForestModel::Mobod& floating_mobod =
-              model.mobods()[index];
-      const JointIndex floating = AddFloatingJointToWorld(floating_mobod);
-      DRAKE_DEMAND(floating == floating_mobod.joint_index);
-  }
-
-  // Shadow mobods are in the order the new bodies were added so the body
-  // index should match.
-  for (auto index : model.shadow_mobods()) {
-    const internal::SpanningForestModel::Mobod shadow_mobod = model.mobods()[index];
-    const BodyIndex shadow = AddShadowBody(shadow_mobod);
-    DRAKE_DEMAND(shadow == shadow_mobod.link_index_);
-  }
-
-  // The modeler only knows about constraints it adds so there is no
-  // indexing correspondence.
-  for (auto& loop_constraint : model.added_constraints()) {
-    const ConstraintIndex constraint =
-        AddLoopClosingConstraint(loop_constraint);
-  }
-  */
+  // After finalizing the base class, tree is read-only.
+  internal::MultibodyTreeSystem<T>::Finalize();
 
   if (geometry_source_is_registered()) {
     ApplyDefaultCollisionFilters();
   }
+
   FinalizePlantOnly();
 
   // Make the manager of discrete updates.
@@ -1173,12 +1121,6 @@ void MultibodyPlant<T>::Finalize() {
         "only supported for discrete models. Refer to MultibodyPlant's "
         "documentation for further details.");
   }
-
-  /* The Plant is complete now. Next, build an efficient computational
-  representation structured in accordance with `model`. */
-
-  // After finalizing the base class, tree is read-only.
-  internal::MultibodyTreeSystem<T>::Finalize();
 }
 
 template<typename T>

multibody/plant/multibody_plant.h

@@ -4348,9 +4348,10 @@ class MultibodyPlant : public internal::MultibodyTreeSystem<T> {
   /// Returns the number of bodies in the model, including the "world" body,
   /// 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
@@ -4364,7 +4365,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.

multibody/plant/test/multibody_plant_test.cc

@@ -1703,15 +1703,16 @@ GTEST_TEST(MultibodyPlantTest, ReversedWeldError) {
       "extra", default_model_instance(), SpatialInertia<double>());
   plant.WeldFrames(extra.body_frame(), plant.world_frame());
 
+  /* TODO(sherm1) This shouldn't throw at all. */
+
   // 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(),
+                              ".*Link.*not assigned a mobilizer.*");
 }
 
 // Utility to verify the subset of output ports we expect to be direct a
@@ -3508,7 +3509,7 @@ GTEST_TEST(SetRandomTest, SetDefaultWhenNoDistributionSpecified) {
   // explicitly added).
   const Body<double>& body1 =
       plant.AddRigidBody("free body 1", SpatialInertia<double>::MakeUnitary());
-  plant.AddJoint<QuaternionFloatingJoint>("" + body1.name(),
+  plant.AddJoint<QuaternionFloatingJoint>(body1.name(),
                                           plant.world_body(), {}, body1, {});
   const std::string acrobot_file_name =
       "drake/multibody/benchmarks/acrobot/acrobot.sdf";
@@ -4421,16 +4422,16 @@ GTEST_TEST(MultibodyPlantTest, GetNames) {
   names = plant->GetPositionNames(false /* add_model_instance_prefix */,
                                   false /* always_add_suffix */);
   EXPECT_THAT(names, testing::ElementsAreArray(
-                         {"iiwa_joint_1",          "iiwa_joint_2",
-                          "iiwa_joint_3",          "iiwa_joint_4",
-                          "iiwa_joint_5",          "iiwa_joint_6",
-                          "iiwa_joint_7",          "iiwa_link_0_qw",
+                         {"iiwa_joint_1",   "iiwa_joint_2",
+                          "iiwa_joint_3",   "iiwa_joint_4",
+                          "iiwa_joint_5",   "iiwa_joint_6",
+                          "iiwa_joint_7",   "iiwa_link_0_qw",
                           "iiwa_link_0_qx", "iiwa_link_0_qy",
                           "iiwa_link_0_qz", "iiwa_link_0_x",
                           "iiwa_link_0_y",  "iiwa_link_0_z",
-                          "iiwa_joint_1",          "iiwa_joint_2",
-                          "iiwa_joint_3",          "iiwa_joint_4",
-                          "iiwa_joint_5",          "iiwa_joint_6",
+                          "iiwa_joint_1",   "iiwa_joint_2",
+                          "iiwa_joint_3",   "iiwa_joint_4",
+                          "iiwa_joint_5",   "iiwa_joint_6",
                           "iiwa_joint_7"}));
 
   names = plant->GetVelocityNames(iiwa0_instance);
@@ -4511,16 +4512,16 @@ GTEST_TEST(MultibodyPlantTest, GetNames) {
   names = plant->GetVelocityNames(false /* add_model_instance_prefix */,
                                   false /* always_add_suffix */);
   EXPECT_THAT(names, testing::ElementsAreArray(
-                         {"iiwa_joint_1",          "iiwa_joint_2",
-                          "iiwa_joint_3",          "iiwa_joint_4",
-                          "iiwa_joint_5",          "iiwa_joint_6",
-                          "iiwa_joint_7",          "iiwa_link_0_wx",
+                         {"iiwa_joint_1",   "iiwa_joint_2",
+                          "iiwa_joint_3",   "iiwa_joint_4",
+                          "iiwa_joint_5",   "iiwa_joint_6",
+                          "iiwa_joint_7",   "iiwa_link_0_wx",
                           "iiwa_link_0_wy", "iiwa_link_0_wz",
                           "iiwa_link_0_vx", "iiwa_link_0_vy",
                           "iiwa_link_0_vz", "iiwa_joint_1",
-                          "iiwa_joint_2",          "iiwa_joint_3",
-                          "iiwa_joint_4",          "iiwa_joint_5",
-                          "iiwa_joint_6",          "iiwa_joint_7"}));
+                          "iiwa_joint_2",   "iiwa_joint_3",
+                          "iiwa_joint_4",   "iiwa_joint_5",
+                          "iiwa_joint_6",   "iiwa_joint_7"}));
 
   names = plant->GetStateNames(iiwa0_instance);
   EXPECT_THAT(names, testing::ElementsAreArray(
@@ -4590,26 +4591,26 @@ GTEST_TEST(MultibodyPlantTest, GetNames) {
   names =
       plant->GetStateNames(false /* add_model_instance_prefix */);
   EXPECT_THAT(names, testing::ElementsAreArray(
-                         {"iiwa_joint_1_q",        "iiwa_joint_2_q",
-                          "iiwa_joint_3_q",        "iiwa_joint_4_q",
-                          "iiwa_joint_5_q",        "iiwa_joint_6_q",
-                          "iiwa_joint_7_q",        "iiwa_link_0_qw",
+                         {"iiwa_joint_1_q", "iiwa_joint_2_q",
+                          "iiwa_joint_3_q", "iiwa_joint_4_q",
+                          "iiwa_joint_5_q", "iiwa_joint_6_q",
+                          "iiwa_joint_7_q", "iiwa_link_0_qw",
                           "iiwa_link_0_qx", "iiwa_link_0_qy",
                           "iiwa_link_0_qz", "iiwa_link_0_x",
                           "iiwa_link_0_y",  "iiwa_link_0_z",
-                          "iiwa_joint_1_q",        "iiwa_joint_2_q",
-                          "iiwa_joint_3_q",        "iiwa_joint_4_q",
-                          "iiwa_joint_5_q",        "iiwa_joint_6_q",
-                          "iiwa_joint_7_q",        "iiwa_joint_1_w",
-                          "iiwa_joint_2_w",        "iiwa_joint_3_w",
-                          "iiwa_joint_4_w",        "iiwa_joint_5_w",
-                          "iiwa_joint_6_w",        "iiwa_joint_7_w",
+                          "iiwa_joint_1_q", "iiwa_joint_2_q",
+                          "iiwa_joint_3_q", "iiwa_joint_4_q",
+                          "iiwa_joint_5_q", "iiwa_joint_6_q",
+                          "iiwa_joint_7_q", "iiwa_joint_1_w",
+                          "iiwa_joint_2_w", "iiwa_joint_3_w",
+                          "iiwa_joint_4_w", "iiwa_joint_5_w",
+                          "iiwa_joint_6_w", "iiwa_joint_7_w",
                           "iiwa_link_0_wx", "iiwa_link_0_wy",
                           "iiwa_link_0_wz", "iiwa_link_0_vx",
                           "iiwa_link_0_vy", "iiwa_link_0_vz",
-                          "iiwa_joint_1_w",        "iiwa_joint_2_w",
-                          "iiwa_joint_3_w",        "iiwa_joint_4_w",
-                          "iiwa_joint_5_w",        "iiwa_joint_6_w",
+                          "iiwa_joint_1_w", "iiwa_joint_2_w",
+                          "iiwa_joint_3_w", "iiwa_joint_4_w",
+                          "iiwa_joint_5_w", "iiwa_joint_6_w",
                           "iiwa_joint_7_w"}));
 
   names = plant->GetActuatorNames(iiwa0_instance);

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;
   }
 }