Rename spaceXYZ mobilizers to more closely match joint terminology (R…

…obotLocomotion#20827)

Also added missing coordinate suffixes to rpy_floating mobilizer

multibody/plant/test/compliant_contact_manager_test.cc

@@ -1,7 +1,6 @@
 #include "drake/multibody/plant/compliant_contact_manager.h"
 
 #include <algorithm>
-#include <memory>
 
 #include <gmock/gmock.h>
 #include <gtest/gtest.h>
@@ -16,17 +15,11 @@
 #include "drake/multibody/contact_solvers/sap/sap_contact_problem.h"
 #include "drake/multibody/contact_solvers/sap/sap_solver.h"
 #include "drake/multibody/contact_solvers/sap/sap_solver_results.h"
-#include "drake/multibody/parsing/parser.h"
 #include "drake/multibody/plant/multibody_plant.h"
-#include "drake/multibody/plant/multibody_plant_config_functions.h"
 #include "drake/multibody/plant/sap_driver.h"
 #include "drake/multibody/plant/test/compliant_contact_manager_tester.h"
 #include "drake/multibody/plant/test/spheres_stack.h"
 #include "drake/multibody/plant/test_utilities/rigid_body_on_compliant_ground.h"
-#include "drake/multibody/tree/joint_actuator.h"
-#include "drake/multibody/tree/prismatic_joint.h"
-#include "drake/multibody/tree/revolute_joint.h"
-#include "drake/multibody/tree/space_xyz_mobilizer.h"
 
 using drake::geometry::GeometryId;
 using drake::geometry::GeometryInstance;

multibody/plant/test/sap_driver_multidof_joints_test.cc

@@ -7,7 +7,7 @@
 #include "drake/multibody/plant/multibody_plant.h"
 #include "drake/multibody/plant/sap_driver.h"
 #include "drake/multibody/plant/test/compliant_contact_manager_tester.h"
-#include "drake/multibody/tree/space_xyz_mobilizer.h"
+#include "drake/multibody/tree/rpy_ball_mobilizer.h"
 
 /* @file This file provides testing for the SapDriver's limited support for
 joint limits on joints with multiple degrees of freedom.
@@ -97,7 +97,7 @@ class MultiDofJointWithLimits final : public Joint<T> {
     // 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>>(
+    auto revolute_mobilizer = std::make_unique<internal::RpyBallMobilizer<T>>(
         this->frame_on_parent(), this->frame_on_child());
     blue_print->mobilizer = std::move(revolute_mobilizer);
     return blue_print;

multibody/rational/rational_forward_kinematics.cc

@@ -1,14 +1,12 @@
 #include "drake/multibody/rational/rational_forward_kinematics.h"
 
-#include <limits>
 #include <utility>
 
 #include "drake/common/drake_assert.h"
 #include "drake/multibody/rational/rational_forward_kinematics_internal.h"
 #include "drake/multibody/tree/multibody_tree_topology.h"
 #include "drake/multibody/tree/prismatic_mobilizer.h"
 #include "drake/multibody/tree/revolute_mobilizer.h"
-#include "drake/multibody/tree/space_xyz_mobilizer.h"
 #include "drake/multibody/tree/weld_mobilizer.h"
 
 namespace drake {

multibody/tree/BUILD.bazel

@@ -116,10 +116,10 @@ drake_cc_library(
         "revolute_mobilizer.cc",
         "revolute_spring.cc",
         "rigid_body.cc",
+        "rpy_ball_mobilizer.cc",
+        "rpy_floating_mobilizer.cc",
         "screw_joint.cc",
         "screw_mobilizer.cc",
-        "space_xyz_floating_mobilizer.cc",
-        "space_xyz_mobilizer.cc",
         "uniform_gravity_field_element.cc",
         "universal_joint.cc",
         "universal_mobilizer.cc",
@@ -162,10 +162,10 @@ drake_cc_library(
         "revolute_mobilizer.h",
         "revolute_spring.h",
         "rigid_body.h",
+        "rpy_ball_mobilizer.h",
+        "rpy_floating_mobilizer.h",
         "screw_joint.h",
         "screw_mobilizer.h",
-        "space_xyz_floating_mobilizer.h",
-        "space_xyz_mobilizer.h",
         "uniform_gravity_field_element.h",
         "universal_joint.h",
         "universal_mobilizer.h",
@@ -499,7 +499,7 @@ drake_cc_googletest(
 )
 
 drake_cc_googletest(
-    name = "space_xyz_mobilizer_test",
+    name = "rpy_ball_mobilizer_test",
     deps = [
         ":mobilizer_tester",
         ":tree",
@@ -509,7 +509,7 @@ drake_cc_googletest(
 )
 
 drake_cc_googletest(
-    name = "space_xyz_floating_mobilizer_test",
+    name = "rpy_floating_mobilizer_test",
     deps = [
         ":mobilizer_tester",
         ":tree",

multibody/tree/ball_rpy_joint.cc

@@ -63,7 +63,7 @@ template <typename T>
 std::unique_ptr<typename Joint<T>::BluePrint>
 BallRpyJoint<T>::MakeImplementationBlueprint() const {
   auto blue_print = std::make_unique<typename Joint<T>::BluePrint>();
-  auto ballrpy_mobilizer = std::make_unique<internal::SpaceXYZMobilizer<T>>(
+  auto ballrpy_mobilizer = std::make_unique<internal::RpyBallMobilizer<T>>(
       this->frame_on_parent(), this->frame_on_child());
   ballrpy_mobilizer->set_default_position(this->default_positions());
   blue_print->mobilizer = std::move(ballrpy_mobilizer);

multibody/tree/ball_rpy_joint.h

@@ -9,7 +9,7 @@
 #include "drake/common/drake_copyable.h"
 #include "drake/multibody/tree/joint.h"
 #include "drake/multibody/tree/multibody_forces.h"
-#include "drake/multibody/tree/space_xyz_mobilizer.h"
+#include "drake/multibody/tree/rpy_ball_mobilizer.h"
 
 namespace drake {
 namespace multibody {
@@ -258,18 +258,18 @@ class BallRpyJoint final : public Joint<T> {
   // Returns the mobilizer implementing this joint.
   // The internal implementation of this joint could change in a future version.
   // However its public API should remain intact.
-  const internal::SpaceXYZMobilizer<T>* get_mobilizer() const {
+  const internal::RpyBallMobilizer<T>* get_mobilizer() const {
     DRAKE_DEMAND(this->get_implementation().has_mobilizer());
-    const internal::SpaceXYZMobilizer<T>* mobilizer =
-        dynamic_cast<const internal::SpaceXYZMobilizer<T>*>(
+    const internal::RpyBallMobilizer<T>* mobilizer =
+        dynamic_cast<const internal::RpyBallMobilizer<T>*>(
             this->get_implementation().mobilizer);
     DRAKE_DEMAND(mobilizer != nullptr);
     return mobilizer;
   }
 
-  internal::SpaceXYZMobilizer<T>* get_mutable_mobilizer() {
+  internal::RpyBallMobilizer<T>* get_mutable_mobilizer() {
     DRAKE_DEMAND(this->get_implementation().has_mobilizer());
-    auto* mobilizer = dynamic_cast<internal::SpaceXYZMobilizer<T>*>(
+    auto* mobilizer = dynamic_cast<internal::RpyBallMobilizer<T>*>(
         this->get_implementation().mobilizer);
     DRAKE_DEMAND(mobilizer != nullptr);
     return mobilizer;

multibody/tree/quaternion_floating_mobilizer.h

@@ -132,12 +132,6 @@ class QuaternionFloatingMobilizer final : public MobilizerImpl<T, 7, 6> {
   // @param[in] R_FM
   //   The rotation matrix relating the orientation of frame F and frame M.
   // @returns a constant reference to `this` mobilizer.
-  // @note: To create a RotationMatrix R_FM (which is inherently orthonormal)
-  // from a non-orthonormal Matrix3<T> m (e.g., m is approximate data), use
-  // R_FM = math::RotationMatrix<T>::ProjectToRotationMatrix( m ).
-  // Alternatively, set this mobilizer's orientation with the two statements:
-  // const Eigen::Quaternion<T> q_FM = RotationMatrix<T>::ToQuaternion( m );
-  // set_quaternion(context, q_FM);
   const QuaternionFloatingMobilizer<T>& SetFromRotationMatrix(
       systems::Context<T>* context, const math::RotationMatrix<T>& R_FM) const {
     const Eigen::Quaternion<T> q_FM = R_FM.ToQuaternion();

multibody/tree/rigid_body.h

@@ -288,8 +288,9 @@ class RigidBody : public MultibodyElement<T> {
   /// (Advanced) Returns `true` if this body is granted 6-dofs by a Mobilizer
   /// and the parent body of this body's associated 6-dof joint is `world`.
   /// @note A floating body is not necessarily modeled with a quaternion
-  /// mobilizer, see has_quaternion_dofs(). Alternative options include a space
-  /// XYZ parametrization of rotations, see SpaceXYZMobilizer.
+  /// mobilizer, see has_quaternion_dofs(). Alternative options include a
+  /// roll-pitch-yaw (rpy) parametrization of rotations, see
+  /// RpyFloatingMobilizer.
   /// @throws std::exception if called pre-finalize,
   /// @see MultibodyPlant::Finalize()
   bool is_floating() const {

multibody/tree/space_xyz_mobilizer.cc → multibody/tree/rpy_ball_mobilizer.cc

@@ -1,4 +1,4 @@
-#include "drake/multibody/tree/space_xyz_mobilizer.h"
+#include "drake/multibody/tree/rpy_ball_mobilizer.h"
 
 #include <memory>
 #include <stdexcept>
@@ -15,7 +15,7 @@ namespace multibody {
 namespace internal {
 
 template <typename T>
-std::string SpaceXYZMobilizer<T>::position_suffix(
+std::string RpyBallMobilizer<T>::position_suffix(
   int position_index_in_mobilizer) const {
   switch (position_index_in_mobilizer) {
     case 0:
@@ -25,11 +25,11 @@ std::string SpaceXYZMobilizer<T>::position_suffix(
     case 2:
       return "qz";
   }
-  throw std::runtime_error("SpaceXYZMobilizer has only 3 positions.");
+  throw std::runtime_error("RpyBallMobilizer has only 3 positions.");
 }
 
 template <typename T>
-std::string SpaceXYZMobilizer<T>::velocity_suffix(
+std::string RpyBallMobilizer<T>::velocity_suffix(
   int velocity_index_in_mobilizer) const {
   switch (velocity_index_in_mobilizer) {
     case 0:
@@ -40,25 +40,25 @@ std::string SpaceXYZMobilizer<T>::velocity_suffix(
       return "wz";
   }
   throw std::runtime_error(
-    "SpaceXYZMobilizer has only 3 velocities.");
+    "RpyBallMobilizer has only 3 velocities.");
 }
 
 template <typename T>
-Vector3<T> SpaceXYZMobilizer<T>::get_angles(
+Vector3<T> RpyBallMobilizer<T>::get_angles(
     const systems::Context<T>& context) const {
   return this->get_positions(context);
 }
 
 template <typename T>
-const SpaceXYZMobilizer<T>& SpaceXYZMobilizer<T>::set_angles(
+const RpyBallMobilizer<T>& RpyBallMobilizer<T>::set_angles(
     systems::Context<T>* context, const Vector3<T>& angles) const {
   auto q = this->GetMutablePositions(context);
   q = angles;
   return *this;
 }
 
 template <typename T>
-const SpaceXYZMobilizer<T>& SpaceXYZMobilizer<T>::SetFromRotationMatrix(
+const RpyBallMobilizer<T>& RpyBallMobilizer<T>::SetFromRotationMatrix(
     systems::Context<T>* context, const math::RotationMatrix<T>& R_FM) const {
   auto q = this->GetMutablePositions(context);
   DRAKE_ASSERT(q.size() == kNq);
@@ -67,19 +67,19 @@ const SpaceXYZMobilizer<T>& SpaceXYZMobilizer<T>::SetFromRotationMatrix(
 }
 
 template <typename T>
-Vector3<T> SpaceXYZMobilizer<T>::get_angular_velocity(
+Vector3<T> RpyBallMobilizer<T>::get_angular_velocity(
     const systems::Context<T>& context) const {
   return this->get_velocities(context);
 }
 
 template <typename T>
-const SpaceXYZMobilizer<T>& SpaceXYZMobilizer<T>::set_angular_velocity(
+const RpyBallMobilizer<T>& RpyBallMobilizer<T>::set_angular_velocity(
     systems::Context<T>* context, const Vector3<T>& w_FM) const {
   return set_angular_velocity(*context, w_FM, &context->get_mutable_state());
 }
 
 template <typename T>
-const SpaceXYZMobilizer<T>& SpaceXYZMobilizer<T>::set_angular_velocity(
+const RpyBallMobilizer<T>& RpyBallMobilizer<T>::set_angular_velocity(
     const systems::Context<T>&, const Vector3<T>& w_FM,
     systems::State<T>* state) const {
   auto v = this->get_mutable_velocities(state);
@@ -89,7 +89,7 @@ const SpaceXYZMobilizer<T>& SpaceXYZMobilizer<T>::set_angular_velocity(
 }
 
 template <typename T>
-math::RigidTransform<T> SpaceXYZMobilizer<T>::CalcAcrossMobilizerTransform(
+math::RigidTransform<T> RpyBallMobilizer<T>::CalcAcrossMobilizerTransform(
     const systems::Context<T>& context) const {
   const Eigen::Matrix<T, 3, 1>& rpy = this->get_positions(context);
   DRAKE_ASSERT(rpy.size() == kNq);
@@ -99,7 +99,7 @@ math::RigidTransform<T> SpaceXYZMobilizer<T>::CalcAcrossMobilizerTransform(
 }
 
 template <typename T>
-SpatialVelocity<T> SpaceXYZMobilizer<T>::CalcAcrossMobilizerSpatialVelocity(
+SpatialVelocity<T> RpyBallMobilizer<T>::CalcAcrossMobilizerSpatialVelocity(
     const systems::Context<T>&,
     const Eigen::Ref<const VectorX<T>>& v) const {
   DRAKE_ASSERT(v.size() == kNv);
@@ -108,15 +108,15 @@ SpatialVelocity<T> SpaceXYZMobilizer<T>::CalcAcrossMobilizerSpatialVelocity(
 
 template <typename T>
 SpatialAcceleration<T>
-SpaceXYZMobilizer<T>::CalcAcrossMobilizerSpatialAcceleration(
+RpyBallMobilizer<T>::CalcAcrossMobilizerSpatialAcceleration(
     const systems::Context<T>&,
     const Eigen::Ref<const VectorX<T>>& vdot) const {
   DRAKE_ASSERT(vdot.size() == kNv);
   return SpatialAcceleration<T>(vdot, Vector3<T>::Zero());
 }
 
 template <typename T>
-void SpaceXYZMobilizer<T>::ProjectSpatialForce(
+void RpyBallMobilizer<T>::ProjectSpatialForce(
     const systems::Context<T>&,
     const SpatialForce<T>& F_Mo_F,
     Eigen::Ref<VectorX<T>> tau) const {
@@ -125,7 +125,7 @@ void SpaceXYZMobilizer<T>::ProjectSpatialForce(
 }
 
 template <typename T>
-void SpaceXYZMobilizer<T>::DoCalcNMatrix(
+void RpyBallMobilizer<T>::DoCalcNMatrix(
     const systems::Context<T>& context, EigenPtr<MatrixX<T>> N) const {
   using std::sin;
   using std::cos;
@@ -151,7 +151,7 @@ void SpaceXYZMobilizer<T>::DoCalcNMatrix(
   // singular.
   if (abs(cp) < 1.0e-3) {
     throw std::runtime_error(fmt::format(
-        "The SpaceXYZMobilizer (likely associated with a BallRpyJoint) between "
+        "The RpyBallMobilizer (implementing a BallRpyJoint) between "
         "body {} and body {} has reached a singularity. This occurs when the "
         "pitch angle takes values near π/2 + kπ, ∀ k ∈ ℤ. At the current "
         "configuration, we have pitch = {}. Drake does not yet support a "
@@ -179,7 +179,7 @@ void SpaceXYZMobilizer<T>::DoCalcNMatrix(
 }
 
 template <typename T>
-void SpaceXYZMobilizer<T>::DoCalcNplusMatrix(
+void RpyBallMobilizer<T>::DoCalcNplusMatrix(
     const systems::Context<T>& context, EigenPtr<MatrixX<T>> Nplus) const {
   // The linear map between q̇ and v is given by matrix E_F(q) defined by:
   //          [ cos(y) * cos(p), -sin(y), 0]
@@ -208,7 +208,7 @@ void SpaceXYZMobilizer<T>::DoCalcNplusMatrix(
 }
 
 template <typename T>
-void SpaceXYZMobilizer<T>::MapVelocityToQDot(
+void RpyBallMobilizer<T>::MapVelocityToQDot(
     const systems::Context<T>& context,
     const Eigen::Ref<const VectorX<T>>& v,
     EigenPtr<VectorX<T>> qdot) const {
@@ -271,7 +271,7 @@ void SpaceXYZMobilizer<T>::MapVelocityToQDot(
   const T cp = cos(angles[1]);
   if (abs(cp) < 1.0e-3) {
     throw std::runtime_error(fmt::format(
-        "The SpaceXYZMobilizer (likely associated with a BallRpyJoint) between "
+        "The RpyBallMobilizer (implementing a BallRpyJoint) between "
         "body {} and body {} has reached a singularity. This occurs when the "
         "pitch angle takes values near π/2 + kπ, ∀ k ∈ ℤ. At the current "
         "configuration, we have pitch = {}. Drake does not yet support a "
@@ -303,7 +303,7 @@ void SpaceXYZMobilizer<T>::MapVelocityToQDot(
 }
 
 template <typename T>
-void SpaceXYZMobilizer<T>::MapQDotToVelocity(
+void RpyBallMobilizer<T>::MapQDotToVelocity(
     const systems::Context<T>& context,
     const Eigen::Ref<const VectorX<T>>& qdot,
     EigenPtr<VectorX<T>> v) const {
@@ -370,32 +370,32 @@ void SpaceXYZMobilizer<T>::MapQDotToVelocity(
 template <typename T>
 template <typename ToScalar>
 std::unique_ptr<Mobilizer<ToScalar>>
-SpaceXYZMobilizer<T>::TemplatedDoCloneToScalar(
+RpyBallMobilizer<T>::TemplatedDoCloneToScalar(
     const MultibodyTree<ToScalar>& tree_clone) const {
   const Frame<ToScalar>& inboard_frame_clone =
       tree_clone.get_variant(this->inboard_frame());
   const Frame<ToScalar>& outboard_frame_clone =
       tree_clone.get_variant(this->outboard_frame());
-  return std::make_unique<SpaceXYZMobilizer<ToScalar>>(
+  return std::make_unique<RpyBallMobilizer<ToScalar>>(
       inboard_frame_clone, outboard_frame_clone);
 }
 
 template <typename T>
-std::unique_ptr<Mobilizer<double>> SpaceXYZMobilizer<T>::DoCloneToScalar(
+std::unique_ptr<Mobilizer<double>> RpyBallMobilizer<T>::DoCloneToScalar(
     const MultibodyTree<double>& tree_clone) const {
   return TemplatedDoCloneToScalar(tree_clone);
 }
 
 template <typename T>
 std::unique_ptr<Mobilizer<AutoDiffXd>>
-SpaceXYZMobilizer<T>::DoCloneToScalar(
+RpyBallMobilizer<T>::DoCloneToScalar(
     const MultibodyTree<AutoDiffXd>& tree_clone) const {
   return TemplatedDoCloneToScalar(tree_clone);
 }
 
 template <typename T>
 std::unique_ptr<Mobilizer<symbolic::Expression>>
-SpaceXYZMobilizer<T>::DoCloneToScalar(
+RpyBallMobilizer<T>::DoCloneToScalar(
     const MultibodyTree<symbolic::Expression>& tree_clone) const {
   return TemplatedDoCloneToScalar(tree_clone);
 }
@@ -405,4 +405,4 @@ SpaceXYZMobilizer<T>::DoCloneToScalar(
 }  // namespace drake
 
 DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS(
-    class ::drake::multibody::internal::SpaceXYZMobilizer)
+    class ::drake::multibody::internal::RpyBallMobilizer)