[multibody] Combine BodyNode and Mobilizer into MobilizedBody

geometry/optimization/dev/cspace_free_polytope.cc

@@ -33,7 +33,7 @@ namespace {
 [[nodiscard]] bool ChainIsWeld(const multibody::MultibodyPlant<double>& plant,
                                multibody::BodyIndex start,
                                multibody::BodyIndex end) {
-  const std::vector<multibody::internal::MobilizerIndex> mobilizers =
+  const std::vector<multibody::internal::MobilizedBodyIndex> mobilizers =
       multibody::internal::FindMobilizersOnPath(plant, start, end);
   if (mobilizers.size() == 0) {
     return true;
@@ -124,7 +124,8 @@ void FindMonomialBasisArray(
     std::array<VectorX<symbolic::Monomial>, 4>* monomial_basis_array) {
   auto it = map_body_to_monomial_basis_array->find(body_pair);
   if (it == map_body_to_monomial_basis_array->end()) {
-    const std::vector<multibody::internal::MobilizerIndex> mobilizer_indices =
+    const std::vector<multibody::internal::MobilizedBodyIndex>
+        mobilizer_indices =
         multibody::internal::FindMobilizersOnPath(rational_forward_kin.plant(),
                                                   body_pair.first(),
                                                   body_pair.second());

multibody/benchmarks/free_body/test/multibody_plant_free_body_test.cc

@@ -17,7 +17,7 @@
 #include "drake/multibody/benchmarks/free_body/free_body.h"
 #include "drake/multibody/test_utilities/floating_body_plant.h"
 #include "drake/multibody/tree/body.h"
-#include "drake/multibody/tree/mobilizer.h"
+#include "drake/multibody/tree/mobilized_body.h"
 #include "drake/multibody/tree/multibody_tree.h"
 #include "drake/systems/analysis/runge_kutta3_integrator.h"
 #include "drake/systems/analysis/simulator.h"

multibody/plant/multibody_plant.cc

@@ -2988,9 +2988,9 @@ void MultibodyPlant<T>::CalcReactionForces(
   // necessary frame conversions.
   for (JointIndex joint_index(0); joint_index < num_joints(); ++joint_index) {
     const Joint<T>& joint = get_joint(joint_index);
-    const internal::MobilizerIndex mobilizer_index =
+    const internal::MobilizedBodyIndex mobilizer_index =
         internal_tree().get_joint_mobilizer(joint_index);
-    const internal::Mobilizer<T>& mobilizer =
+    const internal::MobilizedBody<T>& mobilizer =
         internal_tree().get_mobilizer(mobilizer_index);
     const internal::BodyNodeIndex body_node_index =
         mobilizer.get_topology().body_node;

multibody/plant/multibody_plant.h

@@ -2456,8 +2456,8 @@ class MultibodyPlant : public internal::MultibodyTreeSystem<T> {
   /// A %MultibodyPlant user adds sets of Body and Joint objects to `this` plant
   /// to build a physical representation of a mechanical model.
   /// At Finalize(), %MultibodyPlant builds a mathematical representation of
-  /// such system, consisting of a tree representation. In this
-  /// representation each body is assigned a Mobilizer, which grants a certain
+  /// such system, consisting of a tree representation. In this representation
+  /// each body is assigned a MobilizedBody, which grants a certain
   /// number of degrees of freedom in accordance to the physical specification.
   /// In this regard, the modeling representation can be seen as a forest of
   /// tree structures each of which contains a single body at the root of the
@@ -3199,7 +3199,7 @@ class MultibodyPlant : public internal::MultibodyTreeSystem<T> {
   ///   or if it is not of size num_positions().
   ///
   /// @see MapQDotToVelocity()
-  /// @see Mobilizer::MapVelocityToQDot()
+  /// @see MobilizedBody::MapVelocityToQDot()
   void MapVelocityToQDot(
       const systems::Context<T>& context,
       const Eigen::Ref<const VectorX<T>>& v,
@@ -3233,7 +3233,7 @@ class MultibodyPlant : public internal::MultibodyTreeSystem<T> {
   ///   is not of size num_velocities().
   ///
   /// @see MapVelocityToQDot()
-  /// @see Mobilizer::MapQDotToVelocity()
+  /// @see MobilizedBody::MapQDotToVelocity()
   void MapQDotToVelocity(
       const systems::Context<T>& context,
       const Eigen::Ref<const VectorX<T>>& qdot,

multibody/rational/rational_forward_kinematics.cc

@@ -69,8 +69,8 @@ RationalForwardKinematics::RationalForwardKinematics(
        ++body_index) {
     const internal::BodyTopology& body_topology =
         tree.get_topology().get_body(body_index);
-    const internal::Mobilizer<double>* mobilizer =
-        &(tree.get_mobilizer(body_topology.inboard_mobilizer));
+    const internal::MobilizedBody<double>* mobilizer =
+        &(tree.get_mobilizer(body_topology.mobilized_body));
     if (IsRevolute(*mobilizer)) {
       const symbolic::Variable s_angle(fmt::format("s[{}]", s_.size()));
       s_.push_back(s_angle);
@@ -235,23 +235,23 @@ RationalForwardKinematics::CalcChildBodyPoseAsMultilinearPolynomial(
       tree.get_topology().get_body(parent);
   const internal::BodyTopology& child_topology =
       tree.get_topology().get_body(child);
-  internal::MobilizerIndex mobilizer_index;
+  internal::MobilizedBodyIndex mobilizer_index;
   bool is_order_reversed;
-  if (parent_topology.parent_body.is_valid() &&
-      parent_topology.parent_body == child) {
+  if (parent_topology.inboard_body.is_valid() &&
+      parent_topology.inboard_body == child) {
     is_order_reversed = true;
-    mobilizer_index = parent_topology.inboard_mobilizer;
-  } else if (child_topology.parent_body.is_valid() &&
-             child_topology.parent_body == parent) {
+    mobilizer_index = parent_topology.mobilized_body;
+  } else if (child_topology.inboard_body.is_valid() &&
+             child_topology.inboard_body == parent) {
     is_order_reversed = false;
-    mobilizer_index = child_topology.inboard_mobilizer;
+    mobilizer_index = child_topology.mobilized_body;
   } else {
     throw std::invalid_argument(fmt::format(
         "CalcChildBodyPoseAsMultilinearPolynomial: the pair of body indices "
         "({}, {}) do not have a parent-child relationship.",
         parent, child));
   }
-  const internal::Mobilizer<double>* mobilizer =
+  const internal::MobilizedBody<double>* mobilizer =
       &(tree.get_mobilizer(mobilizer_index));
   math::RigidTransformd X_PF, X_MC;
   if (!is_order_reversed) {
@@ -310,7 +310,7 @@ RationalForwardKinematics::CalcChildBodyPoseAsMultilinearPolynomial(
 
 // TODO(hongkai.dai): determine the joint type through a Reifier.
 bool RationalForwardKinematics::IsRevolute(
-    const internal::Mobilizer<double>& mobilizer) const {
+    const internal::MobilizedBody<double>& mobilizer) const {
   const bool is_revolute =
       (mobilizer.num_positions() == 1 && mobilizer.num_velocities() == 1 &&
        mobilizer.can_rotate() && !mobilizer.can_translate());
@@ -323,7 +323,7 @@ bool RationalForwardKinematics::IsRevolute(
 
 // TODO(hongkai.dai): determine the joint type through a Reifier.
 bool RationalForwardKinematics::IsWeld(
-    const internal::Mobilizer<double>& mobilizer) const {
+    const internal::MobilizedBody<double>& mobilizer) const {
   const bool is_weld =
       (mobilizer.num_positions() == 0 && mobilizer.num_velocities() == 0 &&
        !mobilizer.can_rotate() && !mobilizer.can_translate());
@@ -336,7 +336,7 @@ bool RationalForwardKinematics::IsWeld(
 
 // TODO(hongkai.dai): determine the joint type through a Reifier.
 bool RationalForwardKinematics::IsPrismatic(
-    const internal::Mobilizer<double>& mobilizer) const {
+    const internal::MobilizedBody<double>& mobilizer) const {
   const bool is_prismatic =
       (mobilizer.num_positions() == 1 && mobilizer.num_velocities() == 1 &&
        !mobilizer.can_rotate() && mobilizer.can_translate());

multibody/rational/rational_forward_kinematics.h

@@ -8,7 +8,7 @@
 #include "drake/common/symbolic/rational_function.h"
 #include "drake/common/symbolic/trigonometric_polynomial.h"
 #include "drake/multibody/plant/multibody_plant.h"
-#include "drake/multibody/tree/mobilizer.h"
+#include "drake/multibody/tree/mobilized_body.h"
 #include "drake/multibody/tree/multibody_tree.h"
 
 namespace drake {
@@ -114,7 +114,7 @@ class RationalForwardKinematics {
                 const Eigen::Ref<const Eigen::VectorXd>& q_star_val) const {
     VectorX<typename Derived::Scalar> s_val(s_.size());
     for (int i = 0; i < s_val.size(); ++i) {
-      const internal::Mobilizer<double>& mobilizer =
+      const internal::MobilizedBody<double>& mobilizer =
           GetInternalTree(plant_).get_mobilizer(
               map_s_to_mobilizer_.at(s_[i].get_id()));
       // the mobilizer cannot be a weld joint since weld joint doesn't introduce
@@ -145,7 +145,7 @@ class RationalForwardKinematics {
                 const Eigen::Ref<const Eigen::VectorXd>& q_star_val) const {
     VectorX<typename Derived::Scalar> q_val(s_.size());
     for (int i = 0; i < s_val.size(); ++i) {
-      const internal::Mobilizer<double>& mobilizer =
+      const internal::MobilizedBody<double>& mobilizer =
           GetInternalTree(plant_).get_mobilizer(
               map_s_to_mobilizer_.at(s_[i].get_id()));
       // the mobilizer cannot be a weld joint since weld joint doesn't introduce
@@ -255,20 +255,20 @@ class RationalForwardKinematics {
       BodyIndex child, const Pose<symbolic::Polynomial>& X_AP) const;
 
   /* Determines whether the current joint is revolute. */
-  bool IsRevolute(const internal::Mobilizer<double>& mobilizer) const;
+  bool IsRevolute(const internal::MobilizedBody<double>& mobilizer) const;
 
   /* Determines whether the current joint is a weld. */
-  bool IsWeld(const internal::Mobilizer<double>& mobilizer) const;
+  bool IsWeld(const internal::MobilizedBody<double>& mobilizer) const;
 
   /* Determines whether the current joint is prismatic. */
-  bool IsPrismatic(const internal::Mobilizer<double>& mobilizer) const;
+  bool IsPrismatic(const internal::MobilizedBody<double>& mobilizer) const;
 
   const MultibodyPlant<double>& plant_;
   // The variables used in computing the pose as rational functions. s_ are the
   // 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::MobilizedBodyIndex>
       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

@@ -42,7 +42,7 @@ std::vector<BodyIndex> FindPath(const MultibodyPlant<double>& plant,
     }
     const BodyTopology& current_node = topology.get_body(current);
     if (current != world_index()) {
-      const BodyIndex parent = current_node.parent_body;
+      const BodyIndex parent = current_node.inboard_body;
       visit_edge(current, parent);
     }
     for (BodyIndex child : current_node.child_bodies) {
@@ -64,23 +64,23 @@ std::vector<BodyIndex> FindPath(const MultibodyPlant<double>& plant,
   return path;
 }
 
-std::vector<MobilizerIndex> FindMobilizersOnPath(
+std::vector<MobilizedBodyIndex> 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<MobilizedBodyIndex> 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 BodyTopology& body_topology = tree.get_topology().get_body(path[i]);
-    if (path[i] != world_index() && body_topology.parent_body == path[i + 1]) {
+    if (path[i] != world_index() && body_topology.inboard_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(body_topology.mobilized_body);
     } 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_body(path[i + 1]).mobilized_body);
     }
   }
   return mobilizers_on_path;
@@ -96,11 +96,12 @@ 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<MobilizedBodyIndex> 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 Mobilizer<double>& mobilizer = tree.get_mobilizer(mobilizer_index);
+    const MobilizedBodyIndex mobilizer_index = mobilizer_indices[i];
+    const MobilizedBody<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::MobilizedBodyIndex> FindMobilizersOnPath(
     const MultibodyPlant<double>& plant, BodyIndex start, BodyIndex end);
 
 /*

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_body(iiwa_link_[i]).mobilized_body;
   }
 }
 
@@ -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_body(iiwa_link_[i]).mobilized_body;
   }
 }
 

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::MobilizedBodyIndex, 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::MobilizedBodyIndex, 8> iiwa_joint_;
 };
 
 /*

multibody/tree/BUILD.bazel

@@ -112,7 +112,7 @@ drake_cc_library(
         "joint_actuator.cc",
         "linear_bushing_roll_pitch_yaw.cc",
         "linear_spring_damper.cc",
-        "mobilizer_impl.cc",
+        "mobilized_body_impl.cc",
         "model_instance.cc",
         "multibody_element.cc",
         "multibody_forces.cc",
@@ -154,8 +154,8 @@ drake_cc_library(
         "joint_actuator.h",
         "linear_bushing_roll_pitch_yaw.h",
         "linear_spring_damper.h",
-        "mobilizer.h",
-        "mobilizer_impl.h",
+        "mobilized_body.h",
+        "mobilized_body_impl.h",
         "model_instance.h",
         "multibody_element.h",
         "multibody_forces.h",

multibody/tree/ball_rpy_joint.cc

@@ -56,9 +56,6 @@ std::unique_ptr<Joint<symbolic::Expression>> BallRpyJoint<T>::DoCloneToScalar(
   return TemplatedDoCloneToScalar(tree_clone);
 }
 
-// N.B. Due to esoteric linking errors on Mac (see #9345) involving
-// `MobilizerImpl`, we must place this implementation in the source file, not
-// in the header file.
 template <typename T>
 std::unique_ptr<typename Joint<T>::BluePrint>
 BallRpyJoint<T>::MakeImplementationBlueprint() const {

multibody/tree/body.h

@@ -220,7 +220,7 @@ class Body : public MultibodyElement<T> {
           "Attempted to call Lock() on non-floating body {}", name()));
     }
     this->get_parent_tree()
-        .get_mobilizer(topology_.inboard_mobilizer)
+        .get_mobilizer(topology_.mobilized_body)
         .Lock(context);
   }
 
@@ -239,7 +239,7 @@ class Body : public MultibodyElement<T> {
           "Attempted to call Unlock() on non-floating body {}", name()));
     }
     this->get_parent_tree()
-        .get_mobilizer(topology_.inboard_mobilizer)
+        .get_mobilizer(topology_.mobilized_body)
         .Unlock(context);
   }
 
@@ -249,7 +249,7 @@ class Body : public MultibodyElement<T> {
   /// @returns true if the body is locked, false otherwise.
   bool is_locked(const systems::Context<T>& context) const {
     return this->get_parent_tree()
-        .get_mobilizer(topology_.inboard_mobilizer)
+        .get_mobilizer(topology_.mobilized_body)
         .is_locked(context);
   }
 
@@ -327,7 +327,7 @@ class Body : public MultibodyElement<T> {
       DRAKE_DEMAND(0 <= position_index_in_body && position_index_in_body < 6);
     }
     return this->get_parent_tree().get_mobilizer(
-      topology_.inboard_mobilizer).position_suffix(position_index_in_body);
+      topology_.mobilized_body).position_suffix(position_index_in_body);
   }
 
   /// Returns a string suffix (e.g. to be appended to the name()) to identify
@@ -341,7 +341,7 @@ class Body : public MultibodyElement<T> {
     DRAKE_DEMAND(is_floating());
     DRAKE_DEMAND(0 <= velocity_index_in_body && velocity_index_in_body < 6);
     return this->get_parent_tree().get_mobilizer(
-      topology_.inboard_mobilizer).velocity_suffix(velocity_index_in_body);
+      topology_.mobilized_body).velocity_suffix(velocity_index_in_body);
   }
 
   /// Returns the default mass (not Context dependent) for `this` body.

multibody/tree/body_node.cc

@@ -50,7 +50,7 @@ void BodyNode<T>::CalcArticulatedBodyHingeInertiaMatrixFactorization(
   //  positive definite, we _know_ D_B is near singular.
   if (llt_D_B->eigen_linear_solver().info() != Eigen::Success) {
     // Create a meaningful message that helps the user as much as possible.
-    const Mobilizer<T>& mobilizer = get_mobilizer();
+    const MobilizedBody<T>& mobilizer = get_mobilizer();
     const Body<T>& inboard_body = mobilizer.inboard_body();
     const Body<T>& outboard_body = mobilizer.outboard_body();
     const std::string& inboard_body_name = inboard_body.name();